[發明專利]一種運載火箭加泄連接器自動對接控制系統

有效

• 申請號: 201710464553.6
• 申請日: 2017-06-19
• 公開（公告）號: CN107284699B
• 公開（公告）日: 2019-12-20
• 發明（設計）人: 鄭國昆;李道平;王小軍;劉毅;白文龍;楊之江
• 申請（專利權）人: 北京航天發射技術研究所;中國運載火箭技術研究院
• 主分類號: B64G5/00
• 分類號: B64G5/00
• 代理公司: 11009 中國航天科技專利中心
• 代理人: 楊春穎

1.一種運載火箭加泄連接器自動對接控制系統，其特征在于：包括：監控單元、運動控制單元、伺服控制器、力檢測單元、鎖緊機構，六自由度并聯機構、視覺監測單元；

六自由度并聯機構，包括上表面和下表面，上表面的面積小于下表面的面積，上表面和下表面之間包括六個電動缸，六個電動缸分別為第一電動缸、第二電動缸、第三電動缸、第四電動缸、第五電動缸、第六電動缸；

第一電動缸、第二電動缸、第三電動缸、第四電動缸、第五電動缸、第六電動缸的缸桶一端均通過球形鉸鏈連接在下表面的一側，第一電動缸、第二電動缸、第三電動缸、第四電動缸、第五電動缸、第六電動缸的活塞桿一端通過球形鉸鏈連接在上表面的一側；

設定上表面上的六個電動缸活塞桿一端的球形鉸鏈與上表面的連接點所形成的圓為上鉸鏈分布圓，上鉸鏈分布圓的半徑為R_a；

設定下表面上的六個電動缸缸桶一端的球形鉸鏈與下表面的連接點所形成的圓為下鉸鏈分布圓，下鉸鏈分布圓的半徑為R_b；

第一電動缸、第三電動缸、第五電動缸活塞桿一端的球形鉸鏈在上表面均勻分布在半徑為R_a的圓的邊緣，且第一電動缸、第三電動缸、第五電動缸活塞桿一端的球形鉸鏈分別與上表面連接形成的三個連接點，連線后呈正三角形；

第二電動缸、第四電動缸、第六電動缸缸桶一端的球形鉸鏈在下表面均勻分布在半徑為R_a的圓的邊緣，且第二電動缸、第四電動缸、第六電動缸缸桶一端的球形鉸鏈分別與下表面連接形成的三個連接點，連線后呈正三角形；

第一電動缸活塞桿一端的球形鉸鏈與上表面連接點與半徑為R_a的圓的圓心的連線和第二電動缸活塞桿一端的球形鉸鏈與上表面連接點與半徑為R_a的圓的圓心的連線的夾角小于60°，設定第一電動缸活塞桿一端的球形鉸鏈與上表面連接點與第二電動缸活塞桿一端的球形鉸鏈與上表面連接點的距離為上鉸鏈短邊間距L_a；

第一電動缸、第三電動缸、第五電動缸缸桶一端的球形鉸鏈在下表面均勻分布在半徑為R_b的圓的邊緣，且第一電動缸、第三電動缸、第五電動缸缸桶一端的球形鉸鏈分別與下表面連接形成的三個連接點，連線后呈正三角形；

第二電動缸、第四電動缸、第六電動缸缸桶一端的球形鉸鏈在下表面均勻分布在半徑為R_b的圓的邊緣，且第二電動缸、第四電動缸、第六電動缸缸桶一端的球形鉸鏈分別與下表面連接形成的三個連接點，連線后呈正三角形；

第一電動缸的球形鉸鏈與下表面連接點與半徑為R_b的圓的圓心的連線和第二電動缸的球形鉸鏈與下表面連接點與半徑為R_b的圓的圓心的連線夾角小于60°，設定第一電動缸的球形鉸鏈與下表面連接點與第二電動缸的球形鉸鏈與下表面連接點的距離為下鉸鏈短邊間距L_b；

設定六自由度并聯機構處于中位時，六個電動缸的活塞桿處于伸縮范圍的中間位置；設定每個電動缸與上表面連接的球形鉸鏈為上鉸鏈，每個電動缸與下表面連接的球形鉸鏈為下鉸鏈，每個電動缸的上鉸鏈中心與下鉸鏈中心的距離L₂；

力檢測單元的一側安裝在上表面的另一側，且力檢測單元的中心與上鉸鏈分布圓的圓心重合；鎖緊機構安裝在力檢測單元的另一側；鎖緊機構與上鉸鏈分布圓的圓心重合；視覺監測單元固定在上表面的另一側的邊緣；

視覺監測單元安裝在力檢測單元旁，并與六自由度并聯機構上表面固定連接；視覺監測單元，實時獲取箭上的推進劑加注口的靶標圖像，根據靶標圖像獲得視覺監測單元和靶標圖像之間的位置偏差和姿態偏差，將該位置偏差和姿態偏差傳送至運動控制單元，運動控制單元根據該位置偏差和姿態偏差，得到六自由度并聯機構上的鎖緊機構的當前位置到箭上的推進劑加注口位置需六個電動缸的活塞桿運動的位移，將該位移傳送至伺服控制器，伺服控制器根據該位移驅動六個電動缸運動，使鎖緊機構和箭上的推進劑加注口對接，當力檢測單元檢測到鎖緊機構和箭上的推進劑加注口有力產生后，鎖緊機構執行鎖緊動作，即將鎖緊機構和箭上的推進劑加注口鎖緊；

執行鎖緊動作后，力檢測單元檢測實時檢測鎖緊機構和箭上的推進劑加注口對接面上各個方向的力，形成力信號，將該力信號傳送至運動控制單元，運動控制單元根據該力信號，得出使鎖緊機構和箭上的推進劑加注口對接面上各個方向的力為零時六個電動缸的活塞桿需運動的位移，將該位移傳送至伺服控制器，伺服控制器根據該位移驅動六個電動缸運動，完成鎖緊機構和箭上的推進劑加注口的對接；

所述運動控制單元根據該力信號，得出使鎖緊機構和箭上的推進劑加注口對接面上各個方向的力為零時六個電動缸的活塞桿需運動的位移，步驟如下：

(1)建立三位坐標系，在上鉸鏈分布圓的圓心處建立慣性坐標系O_a-X_aY_aZ_a，上鉸鏈分布圓的圓心為原點O_a，在下鉸鏈分布圓的圓心處建立慣性坐標系O_b-X_bY_bZ_b，下鉸鏈分布圓的圓心為原點O_b，即靜坐標系，設定第一電動缸、第二電動缸、第三電動缸、第四電動缸、第五電動缸、第六電動缸的球形鉸鏈分別與下表面連接形成的六連接點分別為B₁、B₂、B₃、B₄、B₅、B₆；

設定第一電動缸、第二電動缸、第三電動缸、第四電動缸、第五電動缸、第六電動缸的球形鉸鏈分別與上表面連接形成的六連接點分別為A₁、A₂、A₃、A₄、A₅、A₆；

X_a軸的正方向為原點O_a指向A₁與A₆的中點，即X_a軸穿過A₁與A₆的中點，Y_a軸在上表面上超前X_a軸90°，即將X_a軸的正方向在上鉸鏈分布圓的平面上順時針轉動90°為Y_a軸的正方向，根據右手定則確定Z_a軸的正方向；

X_b軸的正方向為原點O_b指向B₁與B₆的中點，即X_b軸穿過B₁與B₆的中點，Y_b軸在下表面上超前X_b軸90°，即將X_b軸的正方向在下鉸鏈分布圓的平面上順時針轉動90°為Y_b軸的正方向，根據右手定則確定Z_b軸的正方向；

(2)執行鎖緊動作后，力檢測單元實時檢測鎖緊機構和箭上的推進劑加注口對接面上各個方向的力F_x,F_y,F_z分別表示鎖緊機構和箭上的推進劑加注口對接面在X_a、Y_a、Z_a軸的受力，T_θ,T_φ為鎖緊機構和箭上的推進劑加注口對接面在X_a、Y_a、Z_a軸的力矩，即扭矩；

根據剛體的剛度計算原理得到F＝M·Δq，Δq＝M^-1·F，式中，M為六自由度并聯機構的剛度矩陣；為得到六自由度并聯機構上表面的位置和姿態，用廣義坐標向量來表示，x,y,z分別表示上鉸鏈分布圓的圓心在O_b-X_bY_bZ_b坐標系下的三維坐標，令t＝[x,y,z]^T；為上鉸鏈分布圓的圓心在O_b-X_bY_bZ_b中的姿態角，即歐拉角，采用Z_b-Y_b-X_b軸的依次旋轉次序，即在坐標系O_a-X_aY_aZ_a與坐標系O_b-X_bY_bZ_b重合的初始條件下，φ表示先將O_a-X_aY_aZ_a坐標系先繞Z_b軸旋轉的角度，θ表示再將O_a-X_aY_aZ_a繞Y_b軸旋轉的角度，表示最后將O_a-X_aY_aZ_a繞X_b軸旋轉的角度；

根據姿態角采用Z_b-Y_b-X_b的旋轉次序，即將O_a-X_aY_aZ_a坐標系先繞Z_b軸旋轉角度φ，再繞Y_b軸旋轉角度θ，最后再繞X_b軸旋轉角度形成旋轉矩陣R為：

(3)設定上鉸鏈A_i的中心在O_a-X_aY_aZ_a坐標系中的三維坐標為下鉸鏈B_i的中心在O_b-X_bY_bZ_b坐標系中的三維坐標為b_i＝[b_ix,b_iy,b_iz]^T，i＝1,2,…,6；當上鉸鏈A_i隨著上表面一起運動，上鉸鏈中心在O_a-X_aY_aZ_a坐標系中的坐標不變；

由二維空間的旋轉原理，可得到上鉸鏈A_i的中心在O_a-X_aY_aZ_a坐標系中坐標的矩陣A，公式如下：

式中，H_a＝0，

同理，能夠得到下鉸鏈B_i的中心在O_b-X_bY_bZ_b坐標系下的坐標矩陣B，公式如下，

式中，H_b＝0，

(4)隨著六自由度并聯機構的運動，O_a-X_aY_aZ_a坐標系相對于O_b-X_bY_bZ_b坐標系的位置和姿態不斷變化，上鉸鏈A_i的中心在O_b-X_bY_bZ_b坐標系中的坐標不斷變化，由空間機構學的原理，上鉸鏈A_i的中心在O_b-X_bY_bZ_b坐標系中的坐標a_i為：

(5)將步驟(4)的公式等號兩邊同時減去b_i，即得到第i個電動缸的長度矢量l_i，即

(6)根據步驟(5)得到的六個電動缸的長度矢量l_i，確定第i個電動缸的期望長度L'_i，公式如下

(7)根據步驟(6)確定第i個電動缸的長度L'_i，分別減去第i個電動缸的實際長度，得到第i個電動缸的位移,最終得到全部六個電動缸的位移。

2.根據權利要求1所述的一種運載火箭加泄連接器自動對接控制系統，其特征在于：所述運動控制單元根據該位置偏差和姿態偏差，得到六自由度并聯機構上的鎖緊機構的當前位置到箭上的推進劑加注口位置需六個電動缸的活塞桿運動的位移，步驟如下：

(1)建立三維正交坐標系，即在上鉸鏈分布圓的圓心處建立慣性坐標系O_a-X_aY_aZ_a，上鉸鏈分布圓的圓心為原點O_a，在下鉸鏈分布圓的圓心處建立慣性坐標系O_b-X_bY_bZ_b，下鉸鏈分布圓的圓心為原點O_b，即靜坐標系，設定第一電動缸、第二電動缸、第三電動缸、第四電動缸、第五電動缸、第六電動缸缸筒一端的球形鉸鏈分別與下表面連接形成的六連接點分別為B₁、B₂、B₃、B₄、B₅、B₆；

設定第一電動缸、第二電動缸、第三電動缸、第四電動缸、第五電動缸、第六電動缸活塞桿一端的球形鉸鏈分別與上表面連接形成的六連接點分別為A₁、A₂、A₃、A₄、A₅、A₆；

X_a軸的正方向為原點O_a指向A₁與A₆的中點，即X_a軸穿過A₁與A₆的中點，Y_a軸在上表面上超前X_a軸90°，即將X_a軸的正方向在上鉸鏈分布圓的平面上順時針轉動90°為Y_a軸的正方向，根據右手定則確定Z_a軸的正方向；

X_b軸的正方向為原點O_b指向B₁與B₆的中點，即X_b軸穿過B₁與B₆的中點，Y_b軸在下表面上超前X_b軸90°，即將X_b軸的正方向在下鉸鏈分布圓的平面上順時針轉動90°為Y_b軸的正方向，根據右手定則確定Z_b軸的正方向；

(2)在步驟(1)確定了坐標系的前提下，上表面的位置和姿態，用廣義坐標向量q來表示，其中式中x,y,z分別表示上鉸鏈分布圓的圓心在O_b-X_bY_bZ_b坐標系下的三維坐標，令t＝[x,y,z]^T為位置向量；為慣性坐標系O_a-X_aY_aZ_a在O_b-X_bY_bZ_b中的姿態角，采用Z_b-Y_b-X_b軸的依次旋轉次序，即在坐標系O_a-X_aY_aZ_a與坐標系O_b-X_bY_bZ_b重合的初始條件下，φ表示先將O_a-X_aY_aZ_a坐標系先繞Z_b軸旋轉的角度，θ表示再將O_a-X_aY_aZ_a繞Y_b軸旋轉的角度，表示最后將O_a-X_aY_aZ_a繞X_b軸旋轉的角度；

根據姿態角采用Z_b-Y_b-X_b軸的依次旋轉次序，即將O_a-X_aY_aZ_a坐標系先繞Z_b軸旋轉角度φ，再繞Y_b軸旋轉角度θ，最后再繞X_b軸旋轉角度形成旋轉矩陣R為：

(3)設定第i電動缸活塞桿一端的球形鉸鏈與上表面連接點即上鉸鏈A_i的中心在O_a-X_aY_aZ_a坐標系中的三維坐標為設定第i電動缸缸筒一端的球形鉸鏈與下表面連接點即下鉸鏈B_i的中心在O_b-X_bY_bZ_b坐標系中的三維坐標為當上鉸鏈A_i隨著六自由度并聯機構上表面一起運動，上鉸鏈中心在O_a-X_aY_aZ_a坐標系中的坐標不變；

由二維空間的旋轉原理，得到上鉸鏈A_i的中心在O_a-X_aY_aZ_a坐標系中坐標，所有上鉸鏈A_i的坐標按照A₁-A₆的順序組成的矩陣A，公式如下：

式中，H_a＝0，A中第i列表示上鉸鏈A_i在O_a-X_aY_aZ_a中的坐標；

同理，能夠得到下鉸鏈B_i的中心在O_b-X_bY_bZ_b坐標系下的坐標，所有下鉸鏈B_i的坐標按照B₁-B₆的順序組成矩陣B，公式如下，

式中，H_b＝0，B中第i列表示下鉸鏈B_i在O_b-X_bY_bZ_b中的坐標；

(4)隨著六自由度并聯機構的運動，O_a-X_aY_aZ_a坐標系相對于O_b-X_bY_bZ_b坐標系的位置和姿態不斷變化，上鉸鏈A_i的中心在O_b-X_bY_bZ_b坐標系中的坐標不斷變化，由空間機構學的原理，上鉸鏈A_i的中心在O_b-X_bY_bZ_b坐標系中的坐標a_i為：

式中，為矩陣A的第i列，即A_i在O_a-X_aY_aZ_a中的坐標；

(5)將步驟(4)的公式等號兩邊同時減去b_i，即得到第i個電動缸的長度矢量l_i，即

(6)根據步驟(5)得到的六個電動缸的長度矢量l_i，確定第i個電動缸的期望長度L'_i，公式如下

(7)根據步驟(6)確定第i個電動缸的長度L'_i，分別減去第i個電動缸的實際長度，得到第i個電動缸的位移，最終得到全部六個電動缸的位移。