本發(fā)明涉及一種空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡規(guī)劃方法，屬于機(jī)器人軌跡規(guī)劃技術(shù)領(lǐng)域。

背景技術(shù)：

隨著航天事業(yè)的發(fā)展，航天器規(guī)模日益增大，類(lèi)似于大型空間站、大型高軌通訊衛(wèi)星的航天器得到廣泛使用，這些航天器的尺寸通常可達(dá)到數(shù)十米甚至是百米量級(jí)。對(duì)此類(lèi)航天器的在軌組裝、維護(hù)、維修、部件更換等任務(wù)要求更強(qiáng)的空間在軌服務(wù)能力。

目前的在軌維修通常依靠宇航員出艙完成，這增大了任務(wù)的復(fù)雜性和危險(xiǎn)性，提高了任務(wù)成本。因此研究具備在軌服務(wù)能力的空間機(jī)器人成為各個(gè)航天大國(guó)的研究熱點(diǎn)，例如日本的ETS-VII、美國(guó)的軌道快車(chē)等任務(wù)驗(yàn)證了空間機(jī)器人在軌捕獲衛(wèi)星并進(jìn)行維護(hù)的關(guān)鍵技術(shù)。(Oda,M."Space robot experiments on NASDA's ETS-VII satellite-preliminary overview of the experiment results."Robotics&Automation.proceedings.IEEE International Conference on 2(1999):1390-1395vol.2.Motaghedi,Pejmun."On-orbit performance of the Orbital Express Capture System."Proceedings of SPIE-The International Society for Optical Engineering 6958(2008):69580E-69580E-12.)

但是現(xiàn)有的機(jī)器人通常僅帶有一部機(jī)械臂，維護(hù)的對(duì)象為小型的合作或非合作目標(biāo)。當(dāng)面對(duì)大型航天器時(shí)，此類(lèi)空間機(jī)器人的應(yīng)用受到限制：

1.僅帶有一部機(jī)械臂的空間機(jī)器人在對(duì)大型航天器維護(hù)時(shí)，需要空間機(jī)器人與目標(biāo)航天器間精確的的相對(duì)位置和姿態(tài)控制，以保證僅有的機(jī)械臂完成更換部件等工作；

2.大型航天器的維護(hù)任務(wù)通常比較復(fù)雜，很可能需要進(jìn)行多處維修；因此僅帶有一部機(jī)械臂的空間機(jī)器人在完成在某點(diǎn)的維修操作后，必須通過(guò)自身攜帶的軌道控制發(fā)動(dòng)機(jī)實(shí)現(xiàn)位置轉(zhuǎn)移。由于空間機(jī)器人與目標(biāo)航天器的相對(duì)距離很小，且后者表面形狀復(fù)雜，在空間機(jī)器人位置轉(zhuǎn)移過(guò)程中，容易與目標(biāo)航天器發(fā)生碰撞。

因此提出基于帶有多個(gè)機(jī)械臂的空間機(jī)器人在大型航天器表面完成在軌服 務(wù)任務(wù)，為保證其在航天器表面位置轉(zhuǎn)移過(guò)程中的可控性和安全性，采用多個(gè)機(jī)械臂依次捕獲航天器表面的不同位置，實(shí)現(xiàn)位置轉(zhuǎn)移。在該移動(dòng)階段的控制中，系統(tǒng)軌跡規(guī)劃是首要解決的難題。

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

本發(fā)明的目的在于：克服現(xiàn)有技術(shù)的不足，提供了一種空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡規(guī)劃方法，解決了空間移動(dòng)多臂機(jī)器人在大型航天器表面完成在軌服務(wù)任務(wù)時(shí)，多個(gè)機(jī)械臂的移動(dòng)軌跡規(guī)劃問(wèn)題。

本發(fā)明的技術(shù)解決方案：

一種空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡規(guī)劃方法，包含以下步驟：

(1)根據(jù)空間移動(dòng)多臂機(jī)器人具體位置轉(zhuǎn)移任務(wù)要求，確定第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置；

(2)根據(jù)步驟(1)中確定的第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置，解算出第一機(jī)械臂兩端的位移、速度和加速度的時(shí)間歷程以及第二機(jī)械臂兩端的位移、速度和加速度的時(shí)間歷程；

(3)規(guī)劃第一機(jī)械臂的運(yùn)動(dòng)，根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)方程以及步驟(2)中確定的第一機(jī)械臂兩端的移動(dòng)軌跡，解算出第一機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程；

(4)規(guī)劃第二機(jī)械臂的運(yùn)動(dòng)，根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)方程以及步驟(2)中確定的第二機(jī)械臂兩端的移動(dòng)軌跡，解算出第二機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程；

(5)根據(jù)步驟(3)中確定的第一機(jī)械臂各個(gè)關(guān)節(jié)角位移以及步驟(4)中確定的第二機(jī)械臂各個(gè)關(guān)節(jié)角位移，確定空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡；

(6)根據(jù)所述空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡，判斷兩個(gè)機(jī)械臂之間、機(jī)械臂與機(jī)器人平臺(tái)之間、機(jī)械臂與目標(biāo)航天器之間是否發(fā)生碰撞，若均未發(fā)生碰撞，則空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡安全，否則表示移動(dòng)軌跡不安全，返回步驟(1)重新規(guī)劃在多臂移動(dòng)過(guò)程中第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置。

本發(fā)明與現(xiàn)有技術(shù)相比的有益效果為：

(1)本發(fā)明提出的空間移動(dòng)多臂機(jī)器人的軌跡規(guī)劃方法，提出了空間移動(dòng) 多臂機(jī)器人在大型航天器表面完成在軌服務(wù)任務(wù)時(shí)，解決了空間移動(dòng)多臂機(jī)器人在航天器表面移動(dòng)過(guò)程的軌跡規(guī)劃問(wèn)題：首先，針對(duì)連接目標(biāo)航天器和空間機(jī)器人本體的機(jī)械臂，規(guī)劃其各個(gè)關(guān)節(jié)角，實(shí)現(xiàn)空間機(jī)器人本體期望的運(yùn)動(dòng)；其次，針對(duì)移動(dòng)過(guò)程中僅連接空間機(jī)器人本體的機(jī)械臂，規(guī)劃其上的關(guān)節(jié)角，實(shí)現(xiàn)在機(jī)器人本體大范圍平動(dòng)的情況下，精確到達(dá)期望的移動(dòng)捕獲點(diǎn)。通過(guò)這樣的處理，將復(fù)雜移動(dòng)過(guò)程簡(jiǎn)單化，便于使用各種先進(jìn)的關(guān)節(jié)角求解算法。

(2)本發(fā)明的空間移動(dòng)多臂機(jī)器人的軌跡規(guī)劃方法，基于機(jī)械臂、機(jī)器人平臺(tái)和目標(biāo)航天器的幾何學(xué)關(guān)系，設(shè)計(jì)了碰撞檢測(cè)方法，將復(fù)雜的類(lèi)似于機(jī)器人這種多體系統(tǒng)的碰撞問(wèn)題，轉(zhuǎn)化為簡(jiǎn)單的點(diǎn)、線、面關(guān)系判斷，簡(jiǎn)化了計(jì)算流程。并且，提出的碰撞檢測(cè)方法能夠推廣至所有空間機(jī)器人，保障空間機(jī)器人在軌操作過(guò)程的安全性。

附圖說(shuō)明

圖1為空間移動(dòng)多臂機(jī)器人移動(dòng)軌跡規(guī)劃流程圖；

圖2為空間移動(dòng)多臂機(jī)器人某一步移動(dòng)過(guò)程圖，其中，圖2(a)為空間移動(dòng)多臂機(jī)器人初始狀態(tài)，圖2(b)為空間移動(dòng)多臂機(jī)器人終止?fàn)顟B(tài)；

圖3為空間移動(dòng)多臂機(jī)器人及機(jī)械臂構(gòu)型圖，其中，圖3(a)為空間移動(dòng)多臂機(jī)器人構(gòu)型圖，圖3(b)為機(jī)械臂構(gòu)型圖；

圖4為第二機(jī)械臂末端移動(dòng)軌跡圖；

圖5為機(jī)器人平臺(tái)移動(dòng)軌跡圖；

圖6為第一機(jī)械臂各關(guān)節(jié)的移動(dòng)軌跡圖；

圖7為第二機(jī)械臂各關(guān)節(jié)的移動(dòng)軌跡圖；

圖8為機(jī)械臂與機(jī)器人平臺(tái)碰撞檢測(cè)模型；

圖9為特殊情況下機(jī)械臂與機(jī)器人平臺(tái)碰撞檢測(cè)模型，其中，圖9(a)為機(jī)械臂和機(jī)器人平臺(tái)在機(jī)器人平臺(tái)x-y截面的投影圖，圖9(b)為機(jī)械臂和機(jī)器人平臺(tái)在機(jī)器人平臺(tái)y-z截面的投影圖；

圖10為機(jī)械臂與目標(biāo)航天器碰撞檢測(cè)模型；

圖11為兩個(gè)機(jī)械臂之間碰撞檢測(cè)模型；

圖12為機(jī)械臂碰撞示意圖。

具體實(shí)施方式

本發(fā)明提供了一種空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡規(guī)劃方法，能夠有效地 規(guī)劃出空間移動(dòng)多臂機(jī)器人在大型航天器表面移動(dòng)過(guò)程的軌跡，并保障空間移動(dòng)多臂機(jī)器人在軌操作過(guò)程的安全性，而且本發(fā)明所提出的軌跡規(guī)劃方法能夠應(yīng)用于今后所有的空間移動(dòng)多臂機(jī)器人任務(wù)設(shè)計(jì)，所提的方案切實(shí)可行，便于推廣；本發(fā)明提出的碰撞檢測(cè)同樣適用于任意構(gòu)型、執(zhí)行任意任務(wù)的空間機(jī)器人，因此適用范圍很廣，具有很大的市場(chǎng)競(jìng)爭(zhēng)力和先發(fā)優(yōu)勢(shì)，彌補(bǔ)了相關(guān)領(lǐng)域?qū)嵱眯苑桨负屠碚撗芯績(jī)?nèi)容的空白，具有很強(qiáng)的工程實(shí)用和理論指導(dǎo)意義。

下面以某一步移動(dòng)為例，說(shuō)明本發(fā)明所提出的空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡規(guī)劃方法的具體步驟。如圖2(a)所示，任務(wù)假定第一機(jī)械臂末端已捕獲目標(biāo)航天器，第二機(jī)械臂末端未捕獲目標(biāo)航天器，期望其到達(dá)目標(biāo)航天器上某一指定位置，如圖2(b)所示。移動(dòng)過(guò)程分為兩部分，分別由第一機(jī)械臂和第二機(jī)械臂完成。對(duì)于第一機(jī)械臂，其末端與目標(biāo)航天器固定，另一端與機(jī)器人平臺(tái)固定，通過(guò)臂上各關(guān)節(jié)的運(yùn)動(dòng)帶動(dòng)機(jī)器人平臺(tái)平動(dòng)，使機(jī)器人平臺(tái)到達(dá)期望位置；對(duì)于第二機(jī)械臂，其固定于機(jī)器人平臺(tái)的一端隨機(jī)器人平臺(tái)運(yùn)動(dòng)，同時(shí)通過(guò)臂上各關(guān)節(jié)的運(yùn)動(dòng)使其末端到達(dá)目標(biāo)航天器上期望位置，相當(dāng)于完成一個(gè)動(dòng)基座的單臂機(jī)器人的定點(diǎn)抓捕操作。所述移動(dòng)軌跡規(guī)劃方法，首先根據(jù)第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置，基于五次多項(xiàng)式規(guī)劃方法解算出第一機(jī)械臂兩端的移動(dòng)軌跡以及第二機(jī)械臂兩端的移動(dòng)軌跡；進(jìn)而根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)方程解算出第一機(jī)械臂各個(gè)關(guān)節(jié)的移動(dòng)軌跡以及第二機(jī)械臂各個(gè)關(guān)節(jié)的移動(dòng)軌跡，從而確定空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡；最后根據(jù)所述空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡，判斷兩個(gè)機(jī)械臂之間、機(jī)械臂與機(jī)器人平臺(tái)之間、機(jī)械臂與目標(biāo)航天器之間是否發(fā)生碰撞，若均未發(fā)生碰撞，則空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡安全，否則表示移動(dòng)軌跡不安全，需重新規(guī)劃在多臂移動(dòng)過(guò)程中第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置。

如圖1所示，本發(fā)明提供的一種空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡規(guī)劃方法，包含以下步驟：

(1)根據(jù)空間移動(dòng)多臂機(jī)器人具體位置轉(zhuǎn)移任務(wù)要求，確定第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置；所述第一機(jī)械臂是指同時(shí)連接目標(biāo)航天器和機(jī)器人平臺(tái)的機(jī)械臂，第二機(jī)械臂是指僅連接機(jī)器人平 臺(tái)的機(jī)械臂。所述機(jī)器人平臺(tái)是連接機(jī)械臂和慣性空間的中心體結(jié)構(gòu)，其體坐標(biāo)系原點(diǎn)位于機(jī)器人平臺(tái)質(zhì)心O，x，y與z軸為機(jī)器人平臺(tái)慣性主軸，且構(gòu)成右手坐標(biāo)系。所述第一機(jī)械臂和第二機(jī)械臂均為七自由度機(jī)械臂，即均包含七個(gè)關(guān)節(jié)和七節(jié)機(jī)械臂，每節(jié)機(jī)械臂相對(duì)其內(nèi)接體均只有一個(gè)轉(zhuǎn)動(dòng)自由度。第j機(jī)械臂系統(tǒng)中的第i節(jié)機(jī)械臂體坐標(biāo)系原點(diǎn)位于該節(jié)機(jī)械臂與其內(nèi)接體的鉸鏈處，坐標(biāo)系各軸與第i節(jié)機(jī)械臂固聯(lián)，j＝1,2，i＝1,2,..,7。在以下各步驟內(nèi)容中，變量的右上標(biāo)“j”表示有關(guān)第j機(jī)械臂系統(tǒng)的相關(guān)變量，變量的右下標(biāo)“i”表示有關(guān)第i節(jié)機(jī)械臂的相關(guān)變量。

(2)根據(jù)步驟(1)中確定的第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置，通過(guò)五次多項(xiàng)式規(guī)劃方法解算出第一機(jī)械臂兩端的位移、速度和加速度的時(shí)間歷程以及第二機(jī)械臂兩端的位移、速度和加速度的時(shí)間歷程，具體為：

其中，r_w表示第一機(jī)械臂或第二機(jī)械臂某一端的平動(dòng)位移，表示第一機(jī)械臂或第二機(jī)械臂某一端的速度,表示第一機(jī)械臂或第二機(jī)械臂某一端的加速度,r_w0表示第一機(jī)械臂或第二機(jī)械臂某一端的初始位置,r_wf表示第一機(jī)械臂或第二機(jī)械臂某一端所需到達(dá)的位置，w＝t1時(shí)對(duì)應(yīng)第一機(jī)械臂末端，w＝t2時(shí)對(duì)應(yīng)第二機(jī)械臂末端，w＝b時(shí)對(duì)應(yīng)第一機(jī)械臂或第二機(jī)械臂連接機(jī)器人平臺(tái)的一端，t_f是總移動(dòng)時(shí)間，t∈[0，t_f]。

(3)規(guī)劃第一機(jī)械臂的運(yùn)動(dòng)，根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)方程以及步驟(2)中確定的第一機(jī)械臂兩端的移動(dòng)軌跡，解算出第一機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

(3.1)結(jié)合機(jī)械臂的運(yùn)動(dòng)學(xué)關(guān)系，寫(xiě)出第一機(jī)械臂末端位置，機(jī)器人平臺(tái)位置與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

其中，r_t1表示第一機(jī)械臂末端相對(duì)機(jī)器人平臺(tái)質(zhì)心的位置，r_b表示機(jī)器人平臺(tái)質(zhì)心的平動(dòng)位移，r_b1表示第一機(jī)械臂的安裝位置，r_i表示第i節(jié)機(jī)械臂體坐標(biāo)系中坐標(biāo)原點(diǎn)相對(duì)該節(jié)末端的矢徑，i＝1,2,..,7；是根據(jù)第一機(jī)械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機(jī)械臂體坐標(biāo)系與機(jī)器人平臺(tái)坐標(biāo)系之間的轉(zhuǎn)換矩陣；

對(duì)(3.1)中的等式關(guān)系求導(dǎo)，得到第一機(jī)械臂末端速度與其他部位間的相對(duì)關(guān)系

其中，和為第一機(jī)械臂末端和機(jī)器人平臺(tái)的速度，機(jī)器人平臺(tái)的速度即為機(jī)械臂另一端的速度，r_i7表示在第i節(jié)機(jī)械臂坐標(biāo)系中第i節(jié)機(jī)械臂坐標(biāo)系原點(diǎn)到機(jī)械臂末端的矢徑，Γ_i表示第i節(jié)關(guān)節(jié)的旋轉(zhuǎn)方向，表示第一機(jī)械臂的第i個(gè)關(guān)節(jié)角速度；

(3.2)為保證第一機(jī)械臂末端垂直指向目標(biāo)航天器表面，需通過(guò)改變第一機(jī)械臂各關(guān)節(jié)旋轉(zhuǎn)角度以調(diào)整第一機(jī)械臂末端姿態(tài)。結(jié)合機(jī)械臂的運(yùn)動(dòng)學(xué)方程，寫(xiě)出第一機(jī)械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

其中，是根據(jù)第一機(jī)械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機(jī)械臂體坐標(biāo)系與第7節(jié)機(jī)械臂體坐標(biāo)系之間的轉(zhuǎn)換矩陣；ω_t1是第一機(jī)械臂末端姿態(tài)角速度，且通過(guò)五次多項(xiàng)式規(guī)劃得到，具體為：

σ_m1和n_e1由第一機(jī)械臂末端初始姿態(tài)與期望姿態(tài)之間的姿態(tài)誤差四元數(shù)得出

(3.3)將步驟(3.1)中得到的第一機(jī)械臂末端速度與其他部位間的相對(duì)關(guān)系與步驟(3.2)中得到的第一機(jī)械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系聯(lián)立，通過(guò)廣義雅可比矩陣J描述空間機(jī)器人平臺(tái)質(zhì)心和第一機(jī)械臂末端運(yùn)動(dòng)速度與關(guān)節(jié)速度之間的關(guān)系，寫(xiě)出第一機(jī)械臂的運(yùn)動(dòng)學(xué)方程

其中在移動(dòng)過(guò)程中，第一機(jī)械臂的末端與目標(biāo)航天器固定，僅通過(guò)第一機(jī)械臂上各關(guān)節(jié)的運(yùn)動(dòng)帶動(dòng)機(jī)器人平臺(tái)平動(dòng)，則只需考慮機(jī)器人平臺(tái)位置與第一機(jī)械臂各關(guān)節(jié)的運(yùn)動(dòng)學(xué)關(guān)系，即ω_t1＝0_3×1，

(3.4)對(duì)步驟(3.3)中得到的第一機(jī)械臂的運(yùn)動(dòng)學(xué)方程利用逆運(yùn)動(dòng)學(xué)反解，得到第一機(jī)器臂各關(guān)節(jié)的移動(dòng)軌跡，即得到第一機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

其中J⁺為廣義雅可比矩陣J的Moore-Pseudo廣義逆，記為J⁺＝J^T(JJ^T)^-1； 和分別是和J⁺的一階導(dǎo)數(shù)。

(4)規(guī)劃第二機(jī)械臂的運(yùn)動(dòng)，根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)方程以及步驟(2)中確定的第二機(jī)械臂兩端的移動(dòng)軌跡，解算出第二機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

(4.1)結(jié)合機(jī)械臂的運(yùn)動(dòng)學(xué)關(guān)系，寫(xiě)出第二機(jī)械臂末端位置，機(jī)器人平臺(tái)位置與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

其中，r_t2表示第二機(jī)械臂末端相對(duì)機(jī)器人平臺(tái)質(zhì)心的位置，r_b表示機(jī)器人平臺(tái)質(zhì)心的平動(dòng)位移，r_b2表示第二機(jī)械臂的安裝位置，r_i表示第i節(jié)機(jī)械臂體坐標(biāo)系中坐標(biāo)原點(diǎn)相對(duì)該節(jié)末端的矢徑，i＝1,2,..,7；是根據(jù)第二機(jī)械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機(jī)械臂體坐標(biāo)系與機(jī)器人平臺(tái)坐標(biāo)系之間的轉(zhuǎn)換矩陣；

對(duì)(4.1)中的等式關(guān)系求導(dǎo)，得到第二機(jī)械臂末端速度與其他部位間的相對(duì)關(guān)系

其中，和為第二機(jī)械臂末端和機(jī)器人平臺(tái)的速度，機(jī)器人平臺(tái)的速度即為機(jī)械臂另一端的速度，r_i7表示在第i節(jié)機(jī)械臂坐標(biāo)系中第i節(jié)機(jī)械臂坐標(biāo)系原點(diǎn)到機(jī)械臂末端的矢徑，Γ_i表示第i節(jié)關(guān)節(jié)的旋轉(zhuǎn)方向，表示第二機(jī)械臂的 第i個(gè)關(guān)節(jié)角速度；

(4.2)為保證第二機(jī)械臂末端垂直指向目標(biāo)航天器表面，需通過(guò)改變第二機(jī)械臂各關(guān)節(jié)旋轉(zhuǎn)角度以調(diào)整第二機(jī)械臂末端姿態(tài)。結(jié)合機(jī)械臂的運(yùn)動(dòng)學(xué)方程，寫(xiě)出第二機(jī)械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系

其中，是根據(jù)第二機(jī)械臂各關(guān)節(jié)轉(zhuǎn)角得到的第i節(jié)機(jī)械臂體坐標(biāo)系與第7節(jié)機(jī)械臂體坐標(biāo)系之間的轉(zhuǎn)換矩陣；ω_t2是第二機(jī)械臂末端姿態(tài)角速度，且通過(guò)五次多項(xiàng)式規(guī)劃得到，具體為：

σ_m2和n_e2由第二機(jī)械臂末端初始姿態(tài)與期望姿態(tài)之間的姿態(tài)誤差四元數(shù)得出

(4.3)將步驟(4.1)中得到的第二機(jī)械臂末端速度與其他部位間的相對(duì)關(guān)系與步驟(4.2)中得到的第二機(jī)械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系聯(lián)立，通過(guò)廣義雅可比矩陣J描述空間機(jī)器人平臺(tái)質(zhì)心和第二機(jī)械臂末端運(yùn)動(dòng)速度與關(guān)節(jié)速度之間的關(guān)系，寫(xiě)出第二機(jī)械臂的運(yùn)動(dòng)學(xué)方程

其中在移動(dòng)過(guò)程中，第二機(jī)械臂僅連接機(jī)器人平臺(tái)而末端自由，因而第二機(jī)械臂要完成的是一個(gè)動(dòng)基座的單臂機(jī)器人的定點(diǎn)抓捕任務(wù)，需同時(shí)考慮第二機(jī)械臂末端位置，機(jī)器人平臺(tái)位置與第二機(jī)械臂各關(guān)節(jié)的運(yùn)動(dòng)學(xué)關(guān)系，即

(4.4)對(duì)步驟(4.3)中得到的第二機(jī)械臂的運(yùn)動(dòng)學(xué)方程利用逆運(yùn)動(dòng)學(xué)反解，得到第二機(jī)器臂各關(guān)節(jié)的移動(dòng)軌跡，即得到第二機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

其中J⁺為廣義雅可比矩陣J的Moore-Pseudo廣義逆，記為J⁺＝J^T(JJ^T)^-1； 和分別是和J⁺的一階導(dǎo)數(shù)。

(5)根據(jù)步驟(3)中確定的第一機(jī)械臂各個(gè)關(guān)節(jié)角位移以及步驟(4)中確定的第二機(jī)械臂各個(gè)關(guān)節(jié)角位移，確定空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡，即各節(jié)機(jī)械臂的移動(dòng)軌跡，具體為：

其中，依次為第一機(jī)械臂的七節(jié)機(jī)械臂的移動(dòng)軌跡，變量的右上標(biāo)表示有關(guān)第一機(jī)械臂系統(tǒng)的相關(guān)變量，變量的右下標(biāo)表示有關(guān)第i節(jié)機(jī)械臂的相關(guān)變量，i＝1,2,...,7；依次為第二機(jī)械臂的七節(jié)機(jī)械臂的移動(dòng)軌跡，變量的右上標(biāo)表示有關(guān)第二機(jī)械臂系統(tǒng)的相關(guān)變量，變量的右下標(biāo)表示有關(guān)第i節(jié)機(jī)械臂的相關(guān)變量，i＝1,2,...,7。

(6)根據(jù)所述空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡，判斷兩個(gè)機(jī)械臂之間、機(jī)械臂與機(jī)器人平臺(tái)之間、機(jī)械臂與目標(biāo)航天器之間是否發(fā)生碰撞，具體為：

(6.1)機(jī)械臂與機(jī)器人平臺(tái)碰撞檢測(cè)

機(jī)械臂與機(jī)器人平臺(tái)之間的碰撞問(wèn)題和各節(jié)機(jī)械臂與機(jī)器人平臺(tái)軸線的幾何關(guān)系有關(guān)，由此將三維空間的碰撞檢查問(wèn)題轉(zhuǎn)化為各節(jié)機(jī)械臂在機(jī)器人平臺(tái)二維橫截面上的投影與橫截面中心之間的最小距離問(wèn)題。

將各節(jié)機(jī)械臂和機(jī)器人平臺(tái)投影到機(jī)器人平臺(tái)y-z截面，若機(jī)器人平臺(tái)y-z截面中心O到各節(jié)機(jī)械臂的最小距離OP大于最小安全距離a，則保證機(jī)械臂與機(jī)器人平臺(tái)不碰撞，具體為：

記第j機(jī)械臂的第i節(jié)機(jī)械臂兩端和在y-z截面的坐標(biāo)為和 中間變量變量的右上標(biāo)“j”表示有關(guān)第j機(jī)械臂系統(tǒng)的相關(guān)變量，變量的右下標(biāo)“i-1”和“i”表示有關(guān)第i節(jié)機(jī)械臂兩端的相關(guān)變量；

若0≤λ₁≤1，則O到的垂足在線段上，線段上距離O最近的點(diǎn)P為該垂足點(diǎn)，那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng)時(shí)，發(fā)生碰撞；

若λ₁>1，則O到的垂足不在線段上，線段上距離O最近的點(diǎn)P為那么當(dāng)時(shí)，不發(fā)生碰撞；時(shí)，發(fā)生碰撞；

若λ₁<0，則O到的垂足不在線段上，線段上距離O最近的點(diǎn)P為那么當(dāng)不發(fā)生碰撞；則發(fā)生碰撞；

(6.2)機(jī)械臂與目標(biāo)航天器碰撞檢測(cè)

機(jī)械臂與目標(biāo)航天器之間的碰撞問(wèn)題和各節(jié)機(jī)械臂與目標(biāo)航天器軸線的幾何關(guān)系有關(guān)，由此將三維空間的碰撞檢查問(wèn)題轉(zhuǎn)化為各節(jié)機(jī)械臂在目標(biāo)航天器二維橫截面上的投影與橫截面中心之間的最小距離問(wèn)題。

將各節(jié)機(jī)械臂和目標(biāo)航天器投影到目標(biāo)航天器x-y截面，若目標(biāo)航天器x-y截面中心O′到各節(jié)機(jī)械臂的最小距離O′Q大于最小安全距離r，則保證機(jī)械臂與目標(biāo)航天器不碰撞，具體為：

記第j機(jī)械臂的第i節(jié)機(jī)械臂兩端r_i-1,j和在x-y截面的坐標(biāo)為和 中間變量變量的右上標(biāo)“j”表示有關(guān)第j機(jī)械臂系統(tǒng)的相關(guān)變量，變量的右下標(biāo)“i-1”和“i”表示有關(guān)第i節(jié)機(jī)械臂兩端的相關(guān)變量；

若0≤λ₂≤1，則O′到的垂足在線段上，線段上距離O′最近的點(diǎn)Q為該垂足點(diǎn)，那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng)時(shí)，發(fā)生碰撞；

若λ₂>1，則O′到的垂足不在線段上，線段上距離O′最近的點(diǎn)Q為那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng)時(shí)，發(fā)生碰撞；

若λ₂<0，則O′到的垂足不在線段上，線段上距離O′最近的點(diǎn)Q為那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng) 時(shí)，發(fā)生碰撞；

(6.3)兩個(gè)機(jī)械臂之間碰撞檢測(cè)

任意兩節(jié)機(jī)械臂之間的最小距離L大于0，則保證機(jī)械臂之間不碰撞，具體為：

記中間變量u、v：

其中變量的右上標(biāo)“j₁”表示有關(guān)第j₁機(jī)械臂系統(tǒng)的相關(guān)變量，“j₂”表示有關(guān)第j₂機(jī)械臂系統(tǒng)的相關(guān)變量，變量的右下標(biāo)“i-1”和“i”表示有關(guān)第i節(jié)機(jī)械臂兩端的相關(guān)變量，“k-1”和“k”表示有關(guān)第k節(jié)機(jī)械臂兩端的相關(guān)變量，j₁＝1,2，j₂＝1,2，i＝1,2,..,7，k＝1,2,..,7，且j₁＝j(luò)₂和i＝k不同時(shí)成立。

當(dāng)且僅當(dāng)兩連桿所在直線的公垂線的兩個(gè)垂足均在連桿上且兩個(gè)垂足之間的距離為零，即0≤u≤1、0≤v≤1、時(shí)，兩節(jié)機(jī)械臂發(fā)生碰撞，否則不發(fā)生碰撞。

若均未發(fā)生碰撞，則空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡安全，否則表示移動(dòng)軌跡不安全，返回步驟(1)重新規(guī)劃在多臂移動(dòng)過(guò)程中第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置。

下面結(jié)合附圖對(duì)本發(fā)明的工作過(guò)程和工作原理做進(jìn)一步解釋。

所述帶有三部機(jī)械臂的空間移動(dòng)多臂機(jī)器人如圖3(a)所示，機(jī)械臂構(gòu)型如圖3(b)所示。三部機(jī)械臂中，第一機(jī)械臂和第二機(jī)械臂用于移動(dòng)任務(wù)，第 三機(jī)械臂在空間機(jī)器人移動(dòng)到目標(biāo)位置后執(zhí)行在軌操作任務(wù)。圖2表示任務(wù)中180～240s階段，如圖2(a)所示，該階段初始時(shí)第一機(jī)械臂末端已捕獲目標(biāo)航天器，第二機(jī)械臂末端未捕獲目標(biāo)航天器，期望終止時(shí)第二機(jī)械臂末端到達(dá)目標(biāo)航天器上某一指定位置，如圖2(b)所示。移動(dòng)過(guò)程中，第一機(jī)械臂的末端與目標(biāo)航天器固定不動(dòng)，七關(guān)節(jié)可動(dòng)并帶動(dòng)機(jī)器人平臺(tái)發(fā)生平動(dòng)；同時(shí)第二機(jī)械臂從待命狀態(tài)開(kāi)始運(yùn)動(dòng)，在60s內(nèi)完成對(duì)目標(biāo)點(diǎn)的捕獲；而第三機(jī)械臂保持原始狀態(tài)。

(1)根據(jù)空間移動(dòng)多臂機(jī)器人具體位置轉(zhuǎn)移任務(wù)要求，確定第一機(jī)械臂末端位置不變，第二機(jī)械臂末端從位置(2.25,-1.95,-1.1)m到達(dá)位置(1.85,-1,1.2)m，連接第一機(jī)械臂和第二機(jī)械臂另一端的機(jī)器人平臺(tái)從位置(0,0,0)到達(dá)位置(0,0,1)m；

(2)根據(jù)步驟(1)中確定的第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置，通過(guò)五次多項(xiàng)式規(guī)劃方法解算出第一機(jī)械臂兩端的位移、速度和加速度的時(shí)間歷程以及第二機(jī)械臂兩端的位移、速度和加速度的時(shí)間歷程，具體為：

第一機(jī)械臂末端位置不變，第二機(jī)械臂末端移動(dòng)軌跡如圖4所示，連接第一機(jī)械臂和第二機(jī)械臂另一端的機(jī)器人平臺(tái)移動(dòng)軌跡如圖5所示。圖4和圖5說(shuō)明五次多項(xiàng)式規(guī)劃的移動(dòng)軌跡平滑、連貫，滿(mǎn)足操作要求。

(3)規(guī)劃第一機(jī)械臂的運(yùn)動(dòng)，根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)方程以及步驟(2)中確定的第一機(jī)械臂兩端的移動(dòng)軌跡，解算出第一機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

將第一機(jī)械臂末端速度與其他部位間的相對(duì)關(guān)系與第一機(jī)械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系聯(lián)立，通過(guò)廣義雅可比矩陣J描述空間機(jī)器人平臺(tái)質(zhì)心和第一機(jī)械臂末端運(yùn)動(dòng)速度與關(guān)節(jié)速度之間的關(guān)系，寫(xiě)出第一機(jī)械臂 的運(yùn)動(dòng)學(xué)方程

利用逆運(yùn)動(dòng)學(xué)反解，得到第一機(jī)器臂各關(guān)節(jié)的移動(dòng)軌跡，即得到第一機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

第一機(jī)器臂各關(guān)節(jié)的移動(dòng)軌跡如圖6所示。圖6說(shuō)明所提方法能夠?qū)崿F(xiàn)連接目標(biāo)航天器和機(jī)器人平臺(tái)的機(jī)械臂的軌跡規(guī)劃，該過(guò)程中第一機(jī)械臂各關(guān)節(jié)的角運(yùn)動(dòng)軌跡連續(xù)而平穩(wěn)，能夠在機(jī)器人平臺(tái)發(fā)生大范圍平動(dòng)同時(shí)保證機(jī)械臂移動(dòng)軌跡平滑、連貫，滿(mǎn)足操作要求。

(4)規(guī)劃第二機(jī)械臂的運(yùn)動(dòng)，根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)方程以及步驟(2)中確定的第二機(jī)械臂兩端的移動(dòng)軌跡，解算出第二機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

將第二機(jī)械臂末端速度與其他部位間的相對(duì)關(guān)系與第二機(jī)械臂末端姿態(tài)與各關(guān)節(jié)旋轉(zhuǎn)角度之間的等式關(guān)系聯(lián)立，通過(guò)廣義雅可比矩陣J描述空間機(jī)器人平臺(tái)質(zhì)心和第二機(jī)械臂末端運(yùn)動(dòng)速度與關(guān)節(jié)速度之間的關(guān)系，寫(xiě)出第二機(jī)械臂的運(yùn)動(dòng)學(xué)方程

利用逆運(yùn)動(dòng)學(xué)反解，得到第二機(jī)器臂各關(guān)節(jié)的移動(dòng)軌跡，即得到第二機(jī)械臂各個(gè)關(guān)節(jié)角位移、速度和加速度的時(shí)間歷程，具體為：

第二機(jī)器臂各關(guān)節(jié)的移動(dòng)軌跡如圖7所示。圖7說(shuō)明所提方法能夠?qū)崿F(xiàn)僅連接機(jī)器人平臺(tái)的機(jī)械臂抓捕目標(biāo)航天器上期望位置點(diǎn)的軌跡規(guī)劃，該過(guò)程中第二機(jī)械臂各關(guān)節(jié)的角運(yùn)動(dòng)軌跡連續(xù)而平穩(wěn)，能夠在機(jī)器人平臺(tái)發(fā)生大范圍平動(dòng)同時(shí)保證機(jī)械臂末端能夠精確到達(dá)期望位置，并且移動(dòng)軌跡平滑、連貫，滿(mǎn)足操作要求。

(5)根據(jù)步驟(3)中確定的第一機(jī)械臂各個(gè)關(guān)節(jié)角位移以及步驟(4)中 確定的第二機(jī)械臂各個(gè)關(guān)節(jié)角位移，確定空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡，即各節(jié)機(jī)械臂的移動(dòng)軌跡，具體為：

(6)根據(jù)所述空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡，判斷兩個(gè)機(jī)械臂之間、機(jī)械臂與機(jī)器人平臺(tái)之間、機(jī)械臂與目標(biāo)航天器之間是否發(fā)生碰撞，若均未發(fā)生碰撞，則空間移動(dòng)多臂機(jī)器人的移動(dòng)軌跡安全，否則表示移動(dòng)軌跡不安全，返回步驟(1)重新規(guī)劃在多臂移動(dòng)過(guò)程中第一機(jī)械臂兩端所需到達(dá)的位置以及第二機(jī)械臂兩端所需到達(dá)的位置。具體為：

(6.1)機(jī)械臂與機(jī)器人平臺(tái)碰撞檢測(cè)

根據(jù)空間機(jī)器人的簡(jiǎn)化模型，機(jī)器人平臺(tái)是一個(gè)邊長(zhǎng)為a＝0.8m，高為h＝1.2m的六棱柱，由于分析的是各連桿與機(jī)器人平臺(tái)軸線的位置關(guān)系，采用一個(gè)圓柱包絡(luò)體作為近似模型，將各連桿與機(jī)器人平臺(tái)投影到y(tǒng)-z截面，要求機(jī)器人平臺(tái)y-z截面中心O到各節(jié)機(jī)械臂的最小距離OP大于六邊形邊長(zhǎng)a，如圖8所示。

若O到的垂足在線段上，線段上距離O最近的點(diǎn)P為該垂足點(diǎn)，那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng) 時(shí)，發(fā)生碰撞；

若O到的垂足P不在線段上，線段上距離O最近的點(diǎn)P為 那么當(dāng)時(shí)，不發(fā)生碰撞；時(shí)，發(fā)生 碰撞；

若O到的垂足P不在線段上，線段上距離O最近的點(diǎn)P為 那么當(dāng)不發(fā)生碰撞；則發(fā)生碰撞；

另外，由于機(jī)器人平臺(tái)是個(gè)有限長(zhǎng)度的模型，機(jī)械臂構(gòu)型可能出現(xiàn)如圖9(a)和(b)中所示情況。這時(shí)只要該連桿上位于機(jī)器人平臺(tái)邊緣附近的點(diǎn)D與機(jī)器人平臺(tái)軸線的距離大于asin60°，就能保證機(jī)械臂與機(jī)器人平臺(tái)不碰撞。

(6.2)機(jī)械臂與目標(biāo)航天器碰撞檢測(cè)

目標(biāo)航天器簡(jiǎn)化模型是一個(gè)“圓柱”結(jié)構(gòu)，將各連桿與目標(biāo)航天器投影到x-y截面，要求目標(biāo)航天器x-y截面中心O′到各節(jié)機(jī)械臂的最小距離O′Q大于圓柱半徑r＝2.5m，如圖10所示。

若O′到的垂足在線段上，線段上距離O′最近的點(diǎn)Q為該垂足點(diǎn)，那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng) 時(shí)，發(fā)生碰撞；

若O′到的垂足不在線段上，線段上距離O′最近的點(diǎn)Q為 那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng)時(shí)，發(fā)生碰撞；

若O′到的垂足不在線段上，線段上距離O′最近的點(diǎn)Q為 那么當(dāng)時(shí)，不發(fā)生碰撞；當(dāng)時(shí)，發(fā)生碰撞；

(6.3)兩個(gè)機(jī)械臂之間碰撞檢測(cè)

如圖11所示，當(dāng)且僅當(dāng)兩連桿所在直線的公垂線的兩個(gè)垂足均在連桿上且兩個(gè)垂足之間的距離為零，即0≤u≤1、0≤v≤1、 時(shí)，兩節(jié)機(jī)械臂發(fā)生碰撞，否則不發(fā)生碰撞。

為了說(shuō)明碰撞檢測(cè)算法的結(jié)果，將每次檢測(cè)結(jié)果用matlab軟件中fprintf('有碰撞\n')或fprintf('無(wú)碰撞\n')語(yǔ)句輸出，并采用計(jì)數(shù)方式統(tǒng)計(jì)機(jī)械臂與機(jī)器人平臺(tái)之間，機(jī)械臂與目標(biāo)航天器之間以及機(jī)械臂與機(jī)械臂之間的碰撞情況。

在對(duì)所規(guī)劃的單步移動(dòng)過(guò)程進(jìn)行碰撞檢測(cè)之前，首先給出一組特殊算例，以驗(yàn)證所設(shè)計(jì)的碰撞檢測(cè)算法合理有效。

顯而易見(jiàn)，由于機(jī)器人與目標(biāo)航天器在z軸方向距離過(guò)近，兩個(gè)機(jī)械臂都插入到目標(biāo)航天器中。如圖12所示，第一機(jī)械臂的桿3～7和第二機(jī)械臂的桿4～7都與目標(biāo)航天器發(fā)生碰撞，而機(jī)械臂與自身機(jī)器人平臺(tái)之間，以及機(jī)械臂與機(jī)械臂之間無(wú)碰撞。表格1碰撞情況輸出結(jié)果給出了詳細(xì)的碰撞情況，

表格2給出了碰撞次數(shù)統(tǒng)計(jì)結(jié)果，表明檢測(cè)結(jié)果與實(shí)際情況一致。

表格1碰撞情況輸出結(jié)果

表格2碰撞統(tǒng)計(jì)結(jié)果

確定了該方法的有效性，進(jìn)一步通過(guò)這種碰撞檢測(cè)對(duì)所規(guī)劃的單步移動(dòng)過(guò)程的安全性進(jìn)行驗(yàn)證。表格1碰撞情況輸出結(jié)果給出了詳細(xì)的碰撞情況，表格4給出了碰撞次數(shù)統(tǒng)計(jì)結(jié)果。顯然，統(tǒng)計(jì)結(jié)果都為零，證明了機(jī)械臂與機(jī)器人平臺(tái)之間，機(jī)械臂與目標(biāo)航天器之間以及機(jī)械臂與機(jī)械臂之間均無(wú)碰撞發(fā)生，即說(shuō)明所規(guī)劃的軌跡是安全的。

表格3單步移動(dòng)過(guò)程碰撞情況輸出結(jié)果

表格4單步移動(dòng)過(guò)程碰撞統(tǒng)計(jì)結(jié)果

本發(fā)明保護(hù)范圍不僅局限于本實(shí)施例，本實(shí)施例用于解釋本發(fā)明，凡與本發(fā)明在相同原理和構(gòu)思條件下的變更或修改均在本發(fā)明公開(kāi)的保護(hù)范圍之內(nèi)。