本發(fā)明涉及一種運(yùn)載火箭加泄連接器自動(dòng)對(duì)接控制系統(tǒng)，屬于運(yùn)載火箭地面設(shè)備加注技術(shù)領(lǐng)域。

背景技術(shù)：

運(yùn)載火箭或?qū)椗c地面設(shè)備之間的氣、液接口設(shè)備一般稱為連接器。在發(fā)射之前，運(yùn)載火箭一直通過(guò)箭上的接口與連接器連接在一起，完成運(yùn)載火箭的各項(xiàng)測(cè)試。

目前我國(guó)運(yùn)載火箭的連接器與箭上的接口對(duì)接全部依靠人工完成，尤其是推進(jìn)劑加注和瀉出連接器(簡(jiǎn)稱“加泄連接器”)，由操作人員采取手動(dòng)直接操作方式進(jìn)行加泄連接器與箭體接口之間的連接，而運(yùn)載火箭的液態(tài)推進(jìn)劑具有易燃、易爆、易揮發(fā)及強(qiáng)腐蝕性的顯著特性，少量吸入或接觸即可能導(dǎo)致人員中毒，出現(xiàn)傷亡事故。

隨著我國(guó)新一代重型運(yùn)載火箭發(fā)展，加泄連接器的質(zhì)量也在不斷增大，正在逐步超出人力的能力范圍；而由于技術(shù)的發(fā)展，對(duì)發(fā)射過(guò)程提出了快速、安全、可靠等要求，人工對(duì)接的方式已經(jīng)不能滿足相應(yīng)要求。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明解決的技術(shù)問(wèn)題為：克服現(xiàn)有技術(shù)不足，提供一種運(yùn)載火箭加泄連接器自動(dòng)對(duì)接控制系統(tǒng)，根據(jù)視覺(jué)和力檢測(cè)箭上接口的位置和姿態(tài)，驅(qū)動(dòng)六自由度并聯(lián)機(jī)構(gòu)完成對(duì)接、鎖緊，并在加注過(guò)程中能夠?qū)崟r(shí)調(diào)整以適應(yīng)箭體的晃動(dòng)，加注完畢后控制加泄連接器與箭體活門解鎖、分離并可靠回收到安全位置。解決了我國(guó)運(yùn)載火箭連接依靠人工對(duì)接所帶來(lái)的問(wèn)題。

本發(fā)明解決的技術(shù)方案為：一種運(yùn)載火箭加泄連接器自動(dòng)對(duì)接控制系統(tǒng)，包括：監(jiān)控單元、運(yùn)動(dòng)控制單元、伺服控制器、力檢測(cè)單元、鎖緊機(jī)構(gòu)，六自由度并聯(lián)機(jī)構(gòu)、視覺(jué)監(jiān)測(cè)單元；

六自由度并聯(lián)機(jī)構(gòu)，包括上表面和下表面，上表面的面積小于下表面的面積，上表面和下表面之間包括六個(gè)電動(dòng)缸，六個(gè)電動(dòng)缸分別為第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸。

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

設(shè)定上表面上的六個(gè)電動(dòng)缸活塞桿一端的球形鉸鏈與上表面的連接點(diǎn)所形成的圓為上鉸鏈分布圓，上鉸鏈分布圓的半徑為ra；

設(shè)定下表面上的六個(gè)電動(dòng)缸缸桶一端的球形鉸鏈與下表面的連接點(diǎn)所形成的圓為下鉸鏈分布圓，下鉸鏈分布圓的半徑為rb；

第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸活塞桿一端的球形鉸鏈在上表面均勻分布在半徑為ra的圓的邊緣，且第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸活塞桿一端的球形鉸鏈分別與上表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈在上表面均勻分布在半徑為的ra圓的邊緣，且第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈分別與上表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第一電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)與半徑為ra的圓的圓心的連線和第二電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)與半徑為ra的圓的圓心的連線的夾角小于60°，設(shè)定第一電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)與第二電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)的距離為上鉸鏈短邊間距l(xiāng)a；

第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸缸桶一端的球形鉸鏈在下表面均勻分布在半徑為rb的圓的邊緣，且第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸缸桶一端的球形鉸鏈分別與下表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈在下表面均勻分布在半徑為rb的圓的邊緣，且第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈分別與下表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第一電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)與半徑為rb的圓的圓心的連線和第二電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)與半徑為rb的圓的圓心的連線夾角小于60°，設(shè)定第一電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)與第二電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)的距離為下鉸鏈短邊間距l(xiāng)b；

設(shè)定六自由度并聯(lián)機(jī)構(gòu)處于中位時(shí)，六個(gè)電動(dòng)缸的活塞桿處于伸縮范圍的中間位置；設(shè)定每個(gè)電動(dòng)缸與上表面連接的球形鉸鏈為上鉸鏈，每個(gè)電動(dòng)缸與下表面連接的球形鉸鏈為下鉸鏈，每個(gè)電動(dòng)缸的上鉸鏈中心與下鉸鏈中心的距離l2；

力檢測(cè)單元的一側(cè)安裝在上表面的另一側(cè)，且力檢測(cè)單元的中心與上鉸鏈分布圓的圓心重合；鎖緊機(jī)構(gòu)安裝在力檢測(cè)單元的另一側(cè)；鎖緊機(jī)構(gòu)與上鉸鏈分布圓的圓心重合；視覺(jué)監(jiān)測(cè)單元固定在上表面的另一側(cè)的邊緣；

視覺(jué)監(jiān)測(cè)單元安裝在力檢測(cè)單元旁，并與六自由度并聯(lián)機(jī)構(gòu)上表面固定連接；視覺(jué)監(jiān)測(cè)單元，實(shí)時(shí)獲取箭上的推進(jìn)劑加注口的靶標(biāo)圖像，根據(jù)靶標(biāo)圖像獲得視覺(jué)監(jiān)測(cè)單元和靶標(biāo)圖像之間的位置偏差和姿態(tài)偏差，將該位置偏差和姿態(tài)偏差傳送至運(yùn)動(dòng)控制單元，運(yùn)動(dòng)控制單元根據(jù)該位置偏差和姿態(tài)偏差，得到六自由度并聯(lián)機(jī)構(gòu)上的鎖緊機(jī)構(gòu)的當(dāng)前位置到箭上的推進(jìn)劑加注口位置需六個(gè)電動(dòng)缸的活塞桿運(yùn)動(dòng)的位移，將該位移傳送至伺服控制器，伺服控制器根據(jù)該位移驅(qū)動(dòng)六個(gè)電動(dòng)缸運(yùn)動(dòng)，使鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接，當(dāng)力檢測(cè)單元檢測(cè)到鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口有力產(chǎn)生后，鎖緊機(jī)構(gòu)執(zhí)行鎖緊動(dòng)作，即將鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口鎖緊；(箭上的推進(jìn)劑加注口設(shè)置了靶標(biāo)，靶標(biāo)為幾個(gè)正圓和定位點(diǎn)，使視覺(jué)監(jiān)測(cè)單元能夠通過(guò)靶標(biāo)檢測(cè)到箭上的推進(jìn)劑加注口的圖像)

執(zhí)行鎖緊動(dòng)作后，力檢測(cè)單元檢測(cè)實(shí)時(shí)檢測(cè)鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面上各個(gè)方向的力，形成力信號(hào)，將該力信號(hào)傳送至運(yùn)動(dòng)控制單元，運(yùn)動(dòng)控制單元根據(jù)該力信號(hào)，得出使鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面上各個(gè)方向的力為零時(shí)六個(gè)電動(dòng)缸的活塞桿需運(yùn)動(dòng)的位移，將該位移傳送至伺服控制器，伺服控制器根據(jù)該位移驅(qū)動(dòng)六個(gè)電動(dòng)缸運(yùn)動(dòng)，完成鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口的對(duì)接。(對(duì)接完成后，與鎖緊機(jī)構(gòu)一體成型連接的加泄連接器能夠?qū)⒌孛娴娜剂瞎拗械娜剂霞幼⒌郊系耐七M(jìn)劑加注口或?qū)⒓系耐七M(jìn)劑加注口內(nèi)(即火箭內(nèi))的燃料泄回至地面的燃料罐中。加注或泄回過(guò)程在火箭起飛點(diǎn)火前完成。)

所述運(yùn)動(dòng)控制單元根據(jù)該位置偏差和姿態(tài)偏差，得到六自由度并聯(lián)機(jī)構(gòu)上的鎖緊機(jī)構(gòu)的當(dāng)前位置到箭上的推進(jìn)劑加注口位置需六個(gè)電動(dòng)缸的活塞桿運(yùn)動(dòng)的位移，步驟如下：

(1)建立三維正交坐標(biāo)系，即在上鉸鏈分布圓的圓心處建立慣性坐標(biāo)系oa-xayaza，上鉸鏈分布圓的圓心為原點(diǎn)oa，在下鉸鏈分布圓的圓心處建立慣性坐標(biāo)系ob-xbybzb，下鉸鏈分布圓的圓心為原點(diǎn)ob，即靜坐標(biāo)系，設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸缸筒一端的球形鉸鏈分別與下表面連接形成的六連接點(diǎn)分別為b1、b2、b3、b4、b5、b6；

設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸活塞桿一端的球形鉸鏈分別與上表面連接形成的六連接點(diǎn)分別為a1、a2、a3、a4、a5、a6；

xa軸的正方向?yàn)樵c(diǎn)oa指向a1與a6的中點(diǎn)，即xa軸穿過(guò)a1與a6的中點(diǎn)，ya軸在上表面上超前xa軸90°，即將xa軸的正方向在上鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為ya軸的正方向，根據(jù)右手定則確定za軸的正方向；

xb軸的正方向?yàn)樵c(diǎn)ob指向b1與b6的中點(diǎn)，即xb軸穿過(guò)b1與b6的中點(diǎn)，yb軸在下表面上超前xb軸90°，即將xb軸的正方向在下鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為yb軸的正方向，根據(jù)右手定則確定zb軸的正方向；

(2)在步驟(1)確定了坐標(biāo)系的前提下，上表面的位置和姿態(tài)，用廣義坐標(biāo)向量q來(lái)表示，其中式中x,y,z分別表示上鉸鏈分布圓的圓心在ob-xbybzb坐標(biāo)系下的三維坐標(biāo)，令t＝[x,y,z]^t為位置向量；為慣性坐標(biāo)系oa-xayaza在ob-xbybzb中的姿態(tài)角，采用zb-yb-xb軸的依次旋轉(zhuǎn)次序，即在坐標(biāo)系oa-xayaza與坐標(biāo)系ob-xbybzb重合的初始條件下，φ表示先將oa-xayaza坐標(biāo)系先繞zb軸旋轉(zhuǎn)的角度，θ表示再將oa-xayaza繞yb軸旋轉(zhuǎn)的角度，表示最后將oa-xayaza繞xb軸旋轉(zhuǎn)得角度。

根據(jù)姿態(tài)角采用zb-yb-xb軸的依次旋轉(zhuǎn)次序，即將oa-xayaza坐標(biāo)系先繞zb軸旋轉(zhuǎn)角度φ，再繞yb軸旋轉(zhuǎn)角度θ，最后再繞xb軸旋轉(zhuǎn)角度形成旋轉(zhuǎn)矩陣r為：

(3)設(shè)定第i電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)即上鉸鏈ai的中心在oa-xayaza坐標(biāo)系中的三維坐標(biāo)為設(shè)定第i電動(dòng)缸缸筒一端的球形鉸鏈與下表面連接點(diǎn)即下鉸鏈bi的中心在ob-xbybzb坐標(biāo)系中的三維坐標(biāo)為bi＝[bix,biy,biz]^t，i＝1,2,…,6；當(dāng)上鉸鏈ai隨著六自由度并聯(lián)機(jī)構(gòu)上表面一起運(yùn)動(dòng)，上鉸鏈中心在oa-xayaza坐標(biāo)系中的坐標(biāo)不變；

由二維空間的旋轉(zhuǎn)原理，得到上鉸鏈ai的中心在oa-xayaza坐標(biāo)系中坐標(biāo)，所有上鉸鏈ai的坐標(biāo)按照a1-a6的順序組成的矩陣a，公式如下：

式中，ha＝0，a中第i列表示上鉸鏈ai在oa-xayaza中的坐標(biāo)；

同理，能夠得到下鉸鏈bi的中心在ob-xbybzb坐標(biāo)系下的坐標(biāo)，所有下鉸鏈bi的坐標(biāo)按照b1-b6的順序組成矩陣b，公式如下，

式中，hb＝0，b中第i列表示下鉸鏈bi在ob-xbybzb中的坐標(biāo)；

(4)隨著六自由度并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)，oa-xayaza坐標(biāo)系相對(duì)于ob-xbybzb坐標(biāo)系的位置和姿態(tài)不斷變化，上鉸鏈ai的中心在ob-xbybzb坐標(biāo)系中的坐標(biāo)不斷變化，由空間機(jī)構(gòu)學(xué)的原理，上鉸鏈ai的中心在ob-xbybzb坐標(biāo)系中的坐標(biāo)ai為：

式中，為矩陣a的第i列，即ai在oa-xayaza中的坐標(biāo)；

(5)將步驟(4)的公式等號(hào)兩邊同時(shí)減去bi，即得到第i個(gè)電動(dòng)缸的長(zhǎng)度矢量li，即

(6)根據(jù)步驟(5)得到的六個(gè)電動(dòng)缸的長(zhǎng)度矢量li，確定第i個(gè)電動(dòng)缸的期望長(zhǎng)度li，公式如下

(7)根據(jù)步驟(6)確定第i個(gè)電動(dòng)缸的長(zhǎng)度li，分別減去第i個(gè)電動(dòng)缸的實(shí)際長(zhǎng)度，得到第i個(gè)電動(dòng)缸的位移，最終得到全部六個(gè)電動(dòng)缸的位移。

所述運(yùn)動(dòng)控制單元根據(jù)該力信號(hào)，得出使鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面上各個(gè)方向的力為零時(shí)六個(gè)電動(dòng)缸的活塞桿需運(yùn)動(dòng)的位移，步驟如下：

(1)建立三位坐標(biāo)系，在上鉸鏈分布圓的圓心處建立慣性坐標(biāo)系oa-xayaza，上鉸鏈分布圓的圓心為原點(diǎn)oa，在下鉸鏈分布圓的圓心處建立慣性坐標(biāo)系ob-xbybzb，下鉸鏈分布圓的圓心為原點(diǎn)ob，即靜坐標(biāo)系，設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸的球形鉸鏈分別與下表面連接形成的六連接點(diǎn)分別為b1、b2、b3、b4、b5、b6；

設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸的球形鉸鏈分別與上表面連接形成的六連接點(diǎn)分別為a1、a2、a3、a4、a5、a6；

xa軸的正方向?yàn)樵c(diǎn)oa指向a1與a6的中點(diǎn)，即xa軸穿過(guò)a1與a6的中點(diǎn)，ya軸在上表面上超前xa軸90°，即將xa軸的正方向在上鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為ya軸的正方向，根據(jù)右手定則確定za軸的正方向；

xb軸的正方向?yàn)樵c(diǎn)ob指向b1與b6的中點(diǎn)，即xb軸穿過(guò)b1與b6的中點(diǎn)，yb軸在下表面上超前xb軸90°，即將xb軸的正方向在下鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為yb軸的正方向，根據(jù)右手定則確定zb軸的正方向；

(2)執(zhí)行鎖緊動(dòng)作后，力檢測(cè)單元實(shí)時(shí)檢測(cè)鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面上各個(gè)方向的力fx,fy,fz分別表示鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面在xa、ya、za軸的受力，tθ,tφ為鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面在xa、ya、za軸的力矩，即扭矩；

根據(jù)剛體的剛度計(jì)算原理得到f＝m·δq，δq＝m^-1·f，式中，m為六自由度并聯(lián)機(jī)構(gòu)的剛度矩陣；為得到六自由度并聯(lián)機(jī)構(gòu)上表面的位置和姿態(tài)，用廣義坐標(biāo)向量來(lái)表示，x,y,z分別表示上鉸鏈分布圓的圓心在ob-xbybzb坐標(biāo)系下的三維坐標(biāo)，令t＝[x,y,z]^t；為上鉸鏈分布圓的圓心在ob-xbybzb中的姿態(tài)角，即歐拉角，采用zb-yb-xb軸的依次旋轉(zhuǎn)次序，即在坐標(biāo)系oa-xayaza與坐標(biāo)系ob-xbybzb重合的初始條件下，φ表示先將oa-xayaza坐標(biāo)系先繞zb軸旋轉(zhuǎn)的角度，θ表示再將oa-xayaza繞yb軸旋轉(zhuǎn)的角度，表示最后將oa-xayaza繞xb軸旋轉(zhuǎn)得角度。

根據(jù)姿態(tài)角采用zb-yb-xb的旋轉(zhuǎn)次序，即將oa-xayaza坐標(biāo)系先繞zb軸旋轉(zhuǎn)角度φ，再繞yb軸旋轉(zhuǎn)角度θ，最后再繞xb軸旋轉(zhuǎn)角度形成旋轉(zhuǎn)矩陣r為：

(3)設(shè)定上鉸鏈ai的中心在oa-xayaza坐標(biāo)系中的三維坐標(biāo)為下鉸鏈bi的中心在ob-xbybzb坐標(biāo)系中的三維坐標(biāo)為bi＝[bix,biy,biz]^t，i＝1,2,…,6；當(dāng)上鉸鏈ai隨著上表面一起運(yùn)動(dòng)，上鉸鏈中心在oa-xayaza坐標(biāo)系中的坐標(biāo)不變；

由二維空間的旋轉(zhuǎn)原理，可得到上鉸鏈ai的中心在oa-xayaza坐標(biāo)系中坐標(biāo)的矩陣a，公式如下：

式中，ha＝0，

同理，能夠得到下鉸鏈bi的中心在ob-xbybzb坐標(biāo)系下的坐標(biāo)矩陣b，公式如下，

式中，hb＝0，

(4)隨著六自由度并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)，oa-xayaza坐標(biāo)系相對(duì)于ob-xbybzb坐標(biāo)系的位置和姿態(tài)不斷變化，上鉸鏈ai的中心在ob-xbybzb坐標(biāo)系中的坐標(biāo)不斷變化，由空間機(jī)構(gòu)學(xué)的原理，上鉸鏈ai的中心在ob-xbybzb坐標(biāo)系中的坐標(biāo)ai為：

(5)將步驟(4)的公式等號(hào)兩邊同時(shí)減去bi，即得到第i個(gè)電動(dòng)缸的長(zhǎng)度矢量li，即

(6)根據(jù)步驟(5)得到的六個(gè)電動(dòng)缸的長(zhǎng)度矢量li，確定第i個(gè)電動(dòng)缸的期望長(zhǎng)度li，公式如下

(7)根據(jù)步驟(6)確定第i個(gè)電動(dòng)缸的長(zhǎng)度li，分別減去第i個(gè)電動(dòng)缸的實(shí)際長(zhǎng)度，得到第i個(gè)電動(dòng)缸的位移,最終得到全部六個(gè)電動(dòng)缸的位移。

本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：

(1)本發(fā)明提供了運(yùn)載火箭加泄連接器自動(dòng)對(duì)接控制的技術(shù)方案，依據(jù)此方案可實(shí)現(xiàn)運(yùn)載火箭加泄連接器的自動(dòng)對(duì)接；

(2)本發(fā)明提出的視覺(jué)監(jiān)測(cè)單元，在不改動(dòng)運(yùn)載火箭箭體結(jié)構(gòu)的情況下，完成箭上接口的位置和姿態(tài)的檢測(cè)，因此也可應(yīng)用在現(xiàn)有的運(yùn)載火箭型號(hào)上，提升了通用性；

(3)本發(fā)明提出的力監(jiān)測(cè)單元，在對(duì)接鎖緊后，通過(guò)測(cè)量對(duì)接面的力和力矩，并轉(zhuǎn)換為位置和姿態(tài)的運(yùn)動(dòng)量，實(shí)時(shí)調(diào)整鎖緊機(jī)構(gòu)的位置和姿態(tài)，有效保護(hù)了箭體接口不被損壞；

(4)本發(fā)明提出的機(jī)械結(jié)構(gòu)方案為六自由度并聯(lián)機(jī)構(gòu)，相比于一般的串聯(lián)式機(jī)械臂，具有精度高、承受載荷大的優(yōu)點(diǎn)、更適用于運(yùn)載火箭加泄連接器自動(dòng)對(duì)接；

(5)本發(fā)明提出視覺(jué)監(jiān)測(cè)單元和力監(jiān)測(cè)單元的計(jì)算方法，具有很大的靈活性，既適合電動(dòng)缸驅(qū)動(dòng)的六自由度并聯(lián)機(jī)構(gòu)，也可以根據(jù)具體情況更改為液壓等其他方案，且計(jì)算方法不變。

附圖說(shuō)明

圖1為本發(fā)明的系統(tǒng)示意圖；

圖2為本發(fā)明的控制信號(hào)流程圖；

圖3為坐標(biāo)系建立示意圖

圖4為六自由度并聯(lián)機(jī)構(gòu)三維示意圖

具體實(shí)施方式

本發(fā)明的基本思路為：一種運(yùn)載火箭加泄連接器自動(dòng)對(duì)接控制系統(tǒng)包括：監(jiān)控單元、運(yùn)動(dòng)控制單元、伺服控制器、力檢測(cè)單元、鎖緊機(jī)構(gòu)，六自由度并聯(lián)機(jī)構(gòu)、視覺(jué)監(jiān)測(cè)單元；視覺(jué)監(jiān)測(cè)單元利用攝像頭對(duì)運(yùn)載火箭箭上接口位置和姿態(tài)進(jìn)行檢測(cè)；力監(jiān)測(cè)單元利用力和力矩傳感器對(duì)接后產(chǎn)生的力和力矩進(jìn)行檢測(cè)；鎖緊機(jī)構(gòu)完成加泄連接器與箭上接口的鎖緊；運(yùn)動(dòng)控制單元利用視覺(jué)監(jiān)測(cè)單元和力檢測(cè)單元對(duì)六自由并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)進(jìn)行控制；伺服驅(qū)動(dòng)器完成對(duì)電動(dòng)機(jī)的驅(qū)動(dòng)。本發(fā)明實(shí)現(xiàn)在不對(duì)箭體進(jìn)行改動(dòng)的條件下完成加泄連接器與箭上接口的自動(dòng)對(duì)接與鎖緊，并跟隨箭上接口的運(yùn)動(dòng)，相比于現(xiàn)在人工對(duì)接的方法，節(jié)省人力，保障人員安全。

下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說(shuō)明。

一種運(yùn)載火箭加泄連接器自動(dòng)對(duì)接控制系統(tǒng)，包括：監(jiān)控單元、運(yùn)動(dòng)控制單元、伺服控制器、力檢測(cè)單元、鎖緊機(jī)構(gòu)，六自由度并聯(lián)機(jī)構(gòu)、視覺(jué)監(jiān)測(cè)單元；

如圖4所示，六自由度并聯(lián)機(jī)構(gòu)可實(shí)現(xiàn)三維空間中六個(gè)自由度的運(yùn)動(dòng)，以適應(yīng)火箭在風(fēng)載荷作用下的無(wú)規(guī)律晃動(dòng)，包括上表面和下表面，上表面的面積小于下表面的面積，上表面和下表面之間包括六個(gè)電動(dòng)缸，六個(gè)電動(dòng)缸分別為第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸。

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

設(shè)定上表面上的六個(gè)電動(dòng)缸活塞桿一端的球形鉸鏈與上表面的連接點(diǎn)所形成的圓為上鉸鏈分布圓，上鉸鏈分布圓的半徑為ra；

設(shè)定下表面上的六個(gè)電動(dòng)缸缸桶一端的球形鉸鏈與下表面的連接點(diǎn)所形成的圓為下鉸鏈分布圓，下鉸鏈分布圓的半徑為rb；

第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸活塞桿一端的球形鉸鏈在上表面均勻分布在半徑為ra的圓的邊緣，且第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸活塞桿一端的球形鉸鏈分別與上表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈在上表面均勻分布在半徑為的ra圓的邊緣，且第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈分別與上表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第一電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)與半徑為ra的圓的圓心的連線和第二電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)與半徑為ra的圓的圓心的連線的夾角小于60°，設(shè)定第一電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)與第二電動(dòng)缸活塞桿一端的球形鉸鏈與上表面連接點(diǎn)的距離為上鉸鏈短邊間距l(xiāng)a；

第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸缸桶一端的球形鉸鏈在下表面均勻分布在半徑為rb的圓的邊緣，且第一電動(dòng)缸、第三電動(dòng)缸、第五電動(dòng)缸缸桶一端的球形鉸鏈分別與下表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈在下表面均勻分布在半徑為rb的圓的邊緣，且第二電動(dòng)缸、第四電動(dòng)缸、第六電動(dòng)缸缸桶一端的球形鉸鏈分別與下表面連接形成的三個(gè)連接點(diǎn)，連線后呈正三角形；

第一電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)與半徑為rb的圓的圓心的連線和第二電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)與半徑為rb的圓的圓心的連線夾角小于60°，設(shè)定第一電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)與第二電動(dòng)缸的球形鉸鏈與下表面連接點(diǎn)的距離為下鉸鏈短邊間距l(xiāng)b；

設(shè)定六個(gè)電動(dòng)缸的活塞桿全部縮回至最短長(zhǎng)度時(shí)為零位，六個(gè)電動(dòng)缸活塞桿全部伸出至最大長(zhǎng)度時(shí)為極限位置，零位與極限位置的中間長(zhǎng)度為中位，即處于伸縮范圍的中間位置；六自由度并聯(lián)機(jī)構(gòu)處于中位時(shí)，設(shè)定每個(gè)電動(dòng)缸與上表面連接的球形鉸鏈為上鉸鏈，每個(gè)電動(dòng)缸與下表面連接的球形鉸鏈為下鉸鏈，每個(gè)電動(dòng)缸的上鉸鏈中心與下鉸鏈中心的距離l2；

力檢測(cè)單元的一側(cè)安裝在上表面的另一側(cè)，且力檢測(cè)單元的中心與上鉸鏈分布圓的圓心重合；鎖緊機(jī)構(gòu)安裝在力檢測(cè)單元的另一側(cè)；鎖緊機(jī)構(gòu)與上鉸鏈分布圓的圓心重合；視覺(jué)監(jiān)測(cè)單元固定在上表面的另一側(cè)的邊緣；

建立三維正交坐標(biāo)系，即在上鉸鏈分布圓的圓心處建立慣性坐標(biāo)系oa-xayaza，上鉸鏈分布圓的圓心為原點(diǎn)oa，在下鉸鏈分布圓的圓心處建立慣性坐標(biāo)系ob-xbybzb，下鉸鏈分布圓的圓心為原點(diǎn)ob，即靜坐標(biāo)系，設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸缸筒一端的球形鉸鏈分別與下表面連接形成的六連接點(diǎn)分別為b1、b2、b3、b4、b5、b6；

設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸活塞桿一端的球形鉸鏈分別與上表面連接形成的六連接點(diǎn)分別為a1、a2、a3、a4、a5、a6；

xa軸的正方向?yàn)樵c(diǎn)oa指向a1與a6的中點(diǎn)，即xa軸穿過(guò)a1與a6的中點(diǎn)，ya軸在上表面上超前xa軸90°，即將xa軸的正方向在上鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為ya軸的正方向，根據(jù)右手定則確定za軸的正方向；

xb軸的正方向?yàn)樵c(diǎn)ob指向b1與b6的中點(diǎn)，即xb軸穿過(guò)b1與b6的中點(diǎn)，yb軸在下表面上超前xb軸90°，即將xb軸的正方向在下鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為yb軸的正方向，根據(jù)右手定則確定zb軸的正方向；

建立三維正交坐標(biāo)系，即在上鉸鏈分布圓的圓心處建立慣性坐標(biāo)系oa-xayaza，上鉸鏈分布圓的圓心為原點(diǎn)oa，在下鉸鏈分布圓的圓心處建立慣性坐標(biāo)系ob-xbybzb，下鉸鏈分布圓的圓心為原點(diǎn)ob，即靜坐標(biāo)系，設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸缸筒一端的球形鉸鏈分別與下表面連接形成的六連接點(diǎn)分別為b1、b2、b3、b4、b5、b6；

如圖3所示，設(shè)定第一電動(dòng)缸、第二電動(dòng)缸、第三電動(dòng)缸、第四電動(dòng)缸、第五電動(dòng)缸、第六電動(dòng)缸活塞桿一端的球形鉸鏈分別與上表面連接形成的六連接點(diǎn)分別為a1、a2、a3、a4、a5、a6；

xa軸的正方向?yàn)樵c(diǎn)oa指向a1與a6的中點(diǎn)，即xa軸穿過(guò)a1與a6的中點(diǎn)，ya軸在上表面上超前xa軸90°，即將xa軸的正方向在上鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為ya軸的正方向，根據(jù)右手定則確定za軸的正方向；

xb軸的正方向?yàn)樵c(diǎn)ob指向b1與b6的中點(diǎn)，即xb軸穿過(guò)b1與b6的中點(diǎn)，yb軸在下表面上超前xb軸90°，即將xb軸的正方向在下鉸鏈分布圓的平面上順時(shí)針轉(zhuǎn)動(dòng)90°為yb軸的正方向，根據(jù)右手定則確定zb軸的正方向；

視覺(jué)監(jiān)測(cè)單元安裝在力檢測(cè)單元旁，并與六自由度并聯(lián)機(jī)構(gòu)上表面固定連接，由高速攝像頭等設(shè)備組成，可以實(shí)現(xiàn)箭上推進(jìn)劑加注口的無(wú)接觸式檢測(cè)，防止接觸式測(cè)量對(duì)火箭的影響，有效減輕火箭載荷，同時(shí)保護(hù)火箭的箭體結(jié)構(gòu)；同時(shí)在箭上推進(jìn)劑加注口附近設(shè)置靶標(biāo)，用于視覺(jué)監(jiān)測(cè)單元檢測(cè)使用。視覺(jué)監(jiān)測(cè)單元實(shí)時(shí)獲取箭上的推進(jìn)劑加注口的靶標(biāo)圖像，根據(jù)靶標(biāo)圖像獲得視覺(jué)監(jiān)測(cè)單元和靶標(biāo)圖像之間的位置偏差和姿態(tài)偏差，利用位置偏差和姿態(tài)偏差可以得到六自由度并聯(lián)機(jī)構(gòu)上的鎖緊機(jī)構(gòu)的當(dāng)前位置到箭上的推進(jìn)劑加注口位置的位置和姿態(tài)，將該位置偏差和姿態(tài)偏差傳送至運(yùn)動(dòng)控制單元，運(yùn)動(dòng)控制單元根據(jù)該位置偏差和姿態(tài)偏差，得到六自由度并聯(lián)機(jī)構(gòu)上的鎖緊機(jī)構(gòu)的當(dāng)前位置到箭上的推進(jìn)劑加注口位置的位置和姿態(tài)，再進(jìn)一步得到六自由度并聯(lián)機(jī)構(gòu)上的鎖緊機(jī)構(gòu)的當(dāng)前位置到箭上的推進(jìn)劑加注口位置需六個(gè)電動(dòng)缸的活塞桿運(yùn)動(dòng)的位移，將該位移傳送至伺服控制器，伺服控制器根據(jù)該位移驅(qū)動(dòng)六個(gè)電動(dòng)缸運(yùn)動(dòng)，使鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接，當(dāng)力檢測(cè)單元檢測(cè)到鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口有力產(chǎn)生后，鎖緊機(jī)構(gòu)執(zhí)行鎖緊動(dòng)作，即將鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口鎖緊；

執(zhí)行鎖緊動(dòng)作后，力檢測(cè)單元檢測(cè)實(shí)時(shí)檢測(cè)鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面上各個(gè)方向的力，fx,fy,fz分別表示鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面在xa、ya、za軸的受力，tθ,tφ為鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面在xa、ya、za軸的力矩，即扭矩；將該力信號(hào)傳送至運(yùn)動(dòng)控制單元，運(yùn)動(dòng)控制單元根據(jù)該力信號(hào)，根據(jù)剛體的剛度計(jì)算原理，得出使鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口對(duì)接面上各個(gè)方向的力為零時(shí)六個(gè)電動(dòng)缸的活塞桿需運(yùn)動(dòng)的位移，將該位移傳送至伺服控制器，伺服控制器根據(jù)該位移驅(qū)動(dòng)六個(gè)電動(dòng)缸運(yùn)動(dòng)，完成鎖緊機(jī)構(gòu)和箭上的推進(jìn)劑加注口的對(duì)接。至此，與鎖緊機(jī)構(gòu)固定連接的加泄連接器對(duì)接完成，加泄連接器能夠?qū)⒌孛娴娜剂瞎拗械娜剂霞幼⒌郊系耐七M(jìn)劑加注口或?qū)⒓系耐七M(jìn)劑加注口內(nèi)(即火箭內(nèi))的燃料泄回至地面的燃料罐中。加注或泄回過(guò)程在火箭起飛點(diǎn)火前完成。

自動(dòng)對(duì)接完成后，控制系統(tǒng)持續(xù)檢測(cè)對(duì)接面的力，并實(shí)時(shí)調(diào)整六自由度并聯(lián)機(jī)構(gòu)的位置和姿態(tài)，以減小箭上推進(jìn)劑加注口的受力，起到保護(hù)火箭結(jié)構(gòu)的作用。

如圖1所示，為本發(fā)明的一種具體實(shí)現(xiàn)方案，其中，監(jiān)控單元選用pc機(jī)、運(yùn)動(dòng)控制單元選用pxi實(shí)時(shí)控制計(jì)算機(jī)，為系統(tǒng)的核心單元，完成箭上推進(jìn)劑加注口的位置姿態(tài)的采集、對(duì)接面受力和力矩參數(shù)的采集，并通過(guò)伺服控制算法，計(jì)算出六個(gè)電動(dòng)缸的位移量，通過(guò)伺服控制器來(lái)驅(qū)動(dòng)電動(dòng)缸進(jìn)行移動(dòng)，達(dá)到自動(dòng)對(duì)接的工作；視覺(jué)檢測(cè)單元主要通過(guò)視覺(jué)檢測(cè)的方式測(cè)量待測(cè)物體的位置和姿態(tài)，力檢測(cè)單元主要在對(duì)接鎖緊后實(shí)時(shí)檢測(cè)對(duì)接面受力，為六自由度機(jī)構(gòu)的位置姿態(tài)調(diào)整提供依據(jù)。

根據(jù)實(shí)際的六自由度并聯(lián)機(jī)構(gòu)結(jié)構(gòu)，建模中加入上表面的質(zhì)心與上鉸鏈分布圓圓心的距離ha、下表面的質(zhì)心與下鉸鏈分布圓中心的距離hb這兩個(gè)參數(shù)，提高系統(tǒng)控制精度和伺服控制性能。

圖2顯示了本發(fā)明中的控制算法及控制信號(hào)流程、用于指導(dǎo)控制算法的工程實(shí)現(xiàn)。對(duì)接控制流程為：在六自由度并聯(lián)機(jī)構(gòu)初始中位條件下，先控制上表面運(yùn)動(dòng)至箭上推進(jìn)劑加注口晃動(dòng)包絡(luò)范圍附近，之后進(jìn)入視覺(jué)監(jiān)測(cè)階段，視覺(jué)檢測(cè)單元采集到其與靶標(biāo)的位置偏差和姿態(tài)偏差并傳送到運(yùn)動(dòng)控制單元中的坐標(biāo)轉(zhuǎn)換單元進(jìn)行計(jì)算分析，得出加注口中心與上表面中心，即鎖緊機(jī)構(gòu)中心的位姿偏差，并進(jìn)行逆運(yùn)動(dòng)學(xué)閉環(huán)控制得到六個(gè)電動(dòng)缸位移，通過(guò)伺服控制器驅(qū)動(dòng)電動(dòng)缸運(yùn)動(dòng)。鎖緊之后，為力反饋階段，此時(shí)，力檢測(cè)單元采集到對(duì)接面得受力后，傳送至力閉環(huán)控制單元，得到位姿偏差信號(hào)，并進(jìn)行逆運(yùn)動(dòng)學(xué)閉環(huán)控制得到六個(gè)電動(dòng)缸位移，通過(guò)伺服控制器驅(qū)動(dòng)電動(dòng)缸運(yùn)動(dòng)。

本發(fā)明的方案經(jīng)過(guò)仿真計(jì)算，可實(shí)現(xiàn)加泄連接器的自動(dòng)對(duì)接功能，解決了目前加泄連接器均由人工對(duì)接完成的難題，在運(yùn)載火箭以1hz，振幅100mm左右晃動(dòng)的條件下，對(duì)接時(shí)間可達(dá)到15s內(nèi)，可實(shí)現(xiàn)快速全自動(dòng)化對(duì)接，為運(yùn)載火箭發(fā)射前緊急關(guān)機(jī)時(shí)的推進(jìn)劑泄回的無(wú)人值守問(wèn)題的解決提供了具體可實(shí)施的方案。