本發(fā)明涉及空間合作/非合作衛(wèi)星抓捕的技術(shù)領(lǐng)域，具體為一種基于對(duì)接環(huán)的捕獲目標(biāo)衛(wèi)星的方法與系統(tǒng)。

背景技術(shù)：

自主在軌服務(wù)是利用安裝在服務(wù)衛(wèi)星上的機(jī)械臂捕獲軌道上的衛(wèi)星進(jìn)行操作。比如，輔助未成功進(jìn)入預(yù)定軌道的衛(wèi)星入軌；對(duì)發(fā)生故障的衛(wèi)星進(jìn)行維修、更換故障元器件、輔助將未展開(kāi)機(jī)構(gòu)展開(kāi)；對(duì)燃料耗盡但其他系統(tǒng)工作正常的衛(wèi)星進(jìn)行燃料加注，延長(zhǎng)使用壽命；將廢棄衛(wèi)星和空間碎片進(jìn)行回收或者送入墳?zāi)管壍?。?duì)于大多數(shù)自主在軌服務(wù)任務(wù)而言，首先要解決操作目標(biāo)的抓捕問(wèn)題。目前，在軌捕獲技術(shù)仍然是航天領(lǐng)域具有挑戰(zhàn)性的研究方向，內(nèi)容涉及空間交會(huì)對(duì)接技術(shù)、目標(biāo)測(cè)量技術(shù)、高性能計(jì)算機(jī)軟硬件技術(shù)、先進(jìn)GNC技術(shù)等，具有跨學(xué)科、跨領(lǐng)域的特點(diǎn)。以往的目標(biāo)捕獲研究，如日本的ETS-VII項(xiàng)目、美國(guó)的“軌道快車(chē)項(xiàng)目”等主要針對(duì)的是合作目標(biāo)衛(wèi)星的捕獲。而對(duì)于非合作目標(biāo)，尤其是運(yùn)動(dòng)復(fù)雜的非合作目標(biāo)的捕獲，研究較少。非合作目標(biāo)抓捕面臨許多難題，如：

(1)非合作目標(biāo)運(yùn)動(dòng)形式較為復(fù)雜；

(2)非合作目標(biāo)抓捕的過(guò)程中，目標(biāo)衛(wèi)星與服務(wù)的空間機(jī)器人之間的相對(duì)位置測(cè)量較為困難；

(3)非合作目標(biāo)上未安裝用于目標(biāo)捕獲的手柄裝置。

因此，有必要進(jìn)行改進(jìn)。

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

為了解決上述技術(shù)問(wèn)題，本發(fā)明的目的是提供一種可快速抓捕目標(biāo)衛(wèi)星的方法與系統(tǒng)。

本發(fā)明所采用的技術(shù)方案是：

本發(fā)明提供一種基于對(duì)接環(huán)的捕獲目標(biāo)衛(wèi)星的方法，包括以下步驟：

S1、系統(tǒng)控制手眼相機(jī)測(cè)量得到空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr和相對(duì)姿態(tài)θ；

S2、系統(tǒng)根據(jù)對(duì)接環(huán)面的法向量z_A，求得所述空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr在對(duì)接環(huán)面的投影b_n，得出距離機(jī)械臂末端最近的抓捕點(diǎn)，并計(jì)算得到機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO；

S3、系統(tǒng)判斷所述相對(duì)位置偏差模||Δp||和相對(duì)姿態(tài)偏差模||ΔO||是否小于設(shè)定的臨界值ε_p和ε_o，若二者均小于臨界值，則機(jī)械臂末端的手爪閉合，實(shí)現(xiàn)目標(biāo)的捕獲；反之，則進(jìn)入S4；

S4、系統(tǒng)根據(jù)所得的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO，根據(jù)以下公式規(guī)劃?rùn)C(jī)械臂末端的線速度與角速度，

其中，K_p、K_v分別為規(guī)劃的比例、微分參數(shù)；分別表示目標(biāo)衛(wèi)星的線速度與角速度；

S5、系統(tǒng)根據(jù)S4中得到的機(jī)械臂末端規(guī)劃的線速度和角速度，通過(guò)雅可比矩陣反解，求得機(jī)械臂各關(guān)節(jié)的角速度；

S6、系統(tǒng)根據(jù)所得的關(guān)節(jié)角速度，驅(qū)動(dòng)機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)，直至機(jī)械臂末端與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差模和相對(duì)姿態(tài)偏差模小于設(shè)定的臨界值ε_p和ε_o，手抓閉合并完成目標(biāo)衛(wèi)星的捕獲。

作為該技術(shù)方案的改進(jìn)，所述機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp的計(jì)算公式為其中，R為對(duì)接環(huán)的半徑。

作為該技術(shù)方案的改進(jìn)，所述機(jī)械臂末端的線速度滿(mǎn)足

其中，t_s為設(shè)定的平滑啟動(dòng)時(shí)間，v_ed、v_em分別表示規(guī)劃的線速度以及設(shè)定的最大線速度值。

進(jìn)一步地，機(jī)械臂末端的角速度滿(mǎn)足

其中，ω_ed、ω_em分別表示規(guī)劃的角速度以及設(shè)定的最大角速度值。

進(jìn)一步地，所述臨界值ε_p小于等于50毫米，ε_o小于等于5°。

進(jìn)一步地，所述關(guān)節(jié)角的角速度求解公式為：

其中，表示關(guān)節(jié)角速度，表示在末端坐標(biāo)系中，關(guān)于機(jī)械臂參數(shù)的雅克比矩陣，分別表示在末端坐標(biāo)系中，機(jī)械臂末端相對(duì)于基座的速度與角速度。

另一方面，本發(fā)明還提供一種基于對(duì)接環(huán)的捕獲目標(biāo)衛(wèi)星的系統(tǒng)，包括：

第一模塊，用于執(zhí)行步驟S1、系統(tǒng)控制手眼相機(jī)測(cè)量得到空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr和相對(duì)姿態(tài)θ；

第二模塊，用于執(zhí)行步驟S2、系統(tǒng)根據(jù)對(duì)接環(huán)面的法向量z_A，求得所述空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr在對(duì)接環(huán)面的投影b_n，得出距離機(jī)械臂末端最近的抓捕點(diǎn)，并計(jì)算得到機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO；

第三模塊，用于執(zhí)行步驟S3、系統(tǒng)判斷所述相對(duì)位置偏差模||Δp||和相對(duì)姿態(tài)偏差模||ΔO||是否小于設(shè)定的臨界值ε_p和ε_o，若二者均小于臨界值，則機(jī)械臂末端的手爪閉合，實(shí)現(xiàn)目標(biāo)的捕獲；反之，則進(jìn)入S4；

第四模塊，用于執(zhí)行步驟S4、系統(tǒng)根據(jù)所得的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO，根據(jù)以下公式規(guī)劃?rùn)C(jī)械臂末端的線速度與角速度，

其中，K_p、K_v分別為規(guī)劃的比例、微分參數(shù)；分別表示目標(biāo)衛(wèi)星的線速度與角速度；

第五模塊，用于執(zhí)行步驟S5、系統(tǒng)根據(jù)S4中得到的機(jī)械臂末端規(guī)劃的線速度和角速度，通過(guò)雅可比矩陣反解，求得機(jī)械臂各關(guān)節(jié)的角速度；

第六模塊，用于執(zhí)行步驟S6、系統(tǒng)根據(jù)所得的關(guān)節(jié)角速度，驅(qū)動(dòng)機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)，直至機(jī)械臂末端與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差模和相對(duì)姿態(tài)偏差模小于設(shè)定的臨界值ε_p和ε_o，手抓閉合并完成目標(biāo)衛(wèi)星的捕獲。

本發(fā)明的有益效果是：

本發(fā)明提供的一種可快速抓捕目標(biāo)衛(wèi)星的方法與系統(tǒng)，通過(guò)利用空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置偏差Δr和二者z軸之間的夾角θ，通過(guò)運(yùn)算處理得到機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO，并將二者的模||Δp||和||ΔO||與設(shè)定的臨界值ε_p和ε_o的對(duì)比判斷，以及當(dāng)不滿(mǎn)足臨界值時(shí)通過(guò)規(guī)劃?rùn)C(jī)械臂末端的運(yùn)動(dòng)，進(jìn)而控制機(jī)械臂末端的手爪閉合，進(jìn)而實(shí)現(xiàn)目標(biāo)的捕獲，且達(dá)到時(shí)時(shí)接近的都是對(duì)接環(huán)上離得最近的一個(gè)點(diǎn)，即“動(dòng)態(tài)最近點(diǎn)”，實(shí)現(xiàn)快速抓捕的目的。

附圖說(shuō)明

下面結(jié)合附圖對(duì)本發(fā)明的具體實(shí)施方式作進(jìn)一步說(shuō)明：

圖1是本發(fā)明一實(shí)施例的機(jī)械臂末端與目標(biāo)星對(duì)接環(huán)相對(duì)位姿示意圖；

圖2是本發(fā)明一實(shí)施例的服務(wù)衛(wèi)星捕獲目標(biāo)衛(wèi)星結(jié)構(gòu)模型示意圖；

圖3是本發(fā)明一實(shí)施例的基于對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”捕獲的自主軌跡規(guī)劃流程示意圖；

圖4是本發(fā)明一實(shí)施例的服務(wù)衛(wèi)星捕獲目標(biāo)衛(wèi)星簡(jiǎn)化模型示意圖。

具體實(shí)施方式

需要說(shuō)明的是，在不沖突的情況下，本申請(qǐng)中的實(shí)施例及實(shí)施例中的特征可以相互組合。

本發(fā)明提供一種基于對(duì)接環(huán)的捕獲目標(biāo)衛(wèi)星的方法，包括以下步驟：

S1、系統(tǒng)控制手眼相機(jī)測(cè)量得到空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr和相對(duì)姿態(tài)θ；

S2、系統(tǒng)根據(jù)對(duì)接環(huán)面的法向量z_A，求得所述空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr在對(duì)接環(huán)面的投影b_n，得出距離機(jī)械臂末端最近的抓捕點(diǎn)，并計(jì)算得到機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO；

S3、系統(tǒng)判斷所述相對(duì)位置偏差模||Δp||和相對(duì)姿態(tài)偏差模||ΔO||是否小于設(shè)定的臨界值ε_p和ε_o，若二者均小于臨界值，則機(jī)械臂末端的手爪閉合，實(shí)現(xiàn)目標(biāo)的捕獲；反之，則進(jìn)入S4；

S4、系統(tǒng)根據(jù)所得的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO，根據(jù)以下公式規(guī)劃?rùn)C(jī)械臂末端的線速度與角速度，

其中，K_p、K_v分別為規(guī)劃的比例、微分參數(shù)；分別表示目標(biāo)衛(wèi)星的線速度與角速度；

S5、系統(tǒng)根據(jù)S4中得到的機(jī)械臂末端規(guī)劃的線速度和角速度，通過(guò)雅可比矩陣反解，求得機(jī)械臂各關(guān)節(jié)的角速度；

S6、系統(tǒng)根據(jù)所得的關(guān)節(jié)角速度，驅(qū)動(dòng)機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)，直至機(jī)械臂末端與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差模和相對(duì)姿態(tài)偏差模小于設(shè)定的臨界值ε_p和ε_o，手抓閉合并完成目標(biāo)衛(wèi)星的捕獲。

作為該技術(shù)方案的改進(jìn)，所述機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp的計(jì)算公式為其中，R為對(duì)接環(huán)的半徑。

作為該技術(shù)方案的改進(jìn)，所述機(jī)械臂末端的線速度滿(mǎn)足

其中，t_s為設(shè)定的平滑啟動(dòng)時(shí)間，v_ed、v_em分別表示規(guī)劃的線速度以及設(shè)定的最大線速度值。

進(jìn)一步地，機(jī)械臂末端的角速度滿(mǎn)足

其中，ω_ed、ω_em分別表示規(guī)劃的角速度以及設(shè)定的最大角速度值。

進(jìn)一步地，所述臨界值ε_p小于等于50毫米，ε_o小于等于5°。

進(jìn)一步地，所述關(guān)節(jié)角的角速度求解公式為：

其中，表示關(guān)節(jié)角速度，表示在末端坐標(biāo)系中，關(guān)于機(jī)械臂參數(shù)的雅克比矩陣，分別表示在末端坐標(biāo)系中，機(jī)械臂末端相對(duì)于基座的速度與角速度。

另一方面，本發(fā)明還提供一種基于對(duì)接環(huán)的捕獲目標(biāo)衛(wèi)星的系統(tǒng)，包括：

第一模塊，用于執(zhí)行步驟S1、系統(tǒng)控制手眼相機(jī)測(cè)量得到空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr和相對(duì)姿態(tài)θ；

第二模塊，用于執(zhí)行步驟S2、系統(tǒng)根據(jù)對(duì)接環(huán)面的法向量z_A，求得所述空間機(jī)器人機(jī)械臂末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr在對(duì)接環(huán)面的投影b_n，得出距離機(jī)械臂末端最近的抓捕點(diǎn)，并計(jì)算得到機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO；

第三模塊，用于執(zhí)行步驟S3、系統(tǒng)判斷所述相對(duì)位置偏差模||Δp||和相對(duì)姿態(tài)偏差模||ΔO||是否小于設(shè)定的臨界值ε_p和ε_o，若二者均小于臨界值，則機(jī)械臂末端的手爪閉合，實(shí)現(xiàn)目標(biāo)的捕獲；反之，則進(jìn)入S4；

第四模塊，用于執(zhí)行步驟S4、系統(tǒng)根據(jù)所得的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO，根據(jù)以下公式規(guī)劃?rùn)C(jī)械臂末端的線速度與角速度，

其中，K_p、K_v分別為規(guī)劃的比例、微分參數(shù)；分別表示目標(biāo)衛(wèi)星的線速度與角速度；

第五模塊，用于執(zhí)行步驟S5、系統(tǒng)根據(jù)S4中得到的機(jī)械臂末端規(guī)劃的線速度和角速度，通過(guò)雅可比矩陣反解，求得機(jī)械臂各關(guān)節(jié)的角速度；

第六模塊，用于執(zhí)行步驟S6、系統(tǒng)根據(jù)所得的關(guān)節(jié)角速度，驅(qū)動(dòng)機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)，直至機(jī)械臂末端與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差模和相對(duì)姿態(tài)偏差模小于設(shè)定的臨界值ε_p和ε_o，手抓閉合并完成目標(biāo)衛(wèi)星的捕獲。

參照?qǐng)D1，是本發(fā)明一實(shí)施例的機(jī)械臂末端與目標(biāo)星對(duì)接環(huán)相對(duì)位姿示意圖。

用于空間合作/非合作衛(wèi)星對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”捕獲的方法，其主要步驟包括：

S1：根據(jù)安裝在機(jī)械臂上的手眼相機(jī)測(cè)量，得到末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr和二者z軸之間的夾角(相對(duì)姿態(tài))θ。

S2：根據(jù)對(duì)接環(huán)面的法相量z_A，求出相對(duì)位置矢量Δr在對(duì)接環(huán)面的投影b_n。由于對(duì)接環(huán)的半徑R的大小已知，可以得出距離機(jī)械臂末端最近的抓捕點(diǎn)T，從而得到機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp，并通過(guò)計(jì)算求得機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)姿態(tài)偏差ΔO。

S3：判斷相對(duì)位置偏差模||Δp||和姿態(tài)偏差模||ΔO||是否小于初始時(shí)設(shè)定的臨界值ε_p和ε_o，若二者小于臨界值，則機(jī)械臂末端的手爪閉合，實(shí)現(xiàn)目標(biāo)的捕獲；反之，進(jìn)行下一步。

S4：由得到的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO，根據(jù)公式(1)規(guī)劃?rùn)C(jī)械臂末端的線速度與角速度，

S5：根據(jù)S4中得到的機(jī)械臂末端規(guī)劃的線速度和角速度，通過(guò)雅可比矩陣反解，求得機(jī)械臂各個(gè)關(guān)節(jié)角的角速度。

S6：由S5中得到的關(guān)節(jié)角速度，驅(qū)動(dòng)機(jī)械臂各個(gè)關(guān)節(jié)運(yùn)動(dòng)，直至機(jī)械臂末端與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位姿小于初始時(shí)設(shè)定的臨界值ε_p和ε_o，手抓閉合，完成目標(biāo)衛(wèi)星的捕獲。

參照?qǐng)D2，是本發(fā)明一實(shí)施例的服務(wù)衛(wèi)星捕獲目標(biāo)衛(wèi)星結(jié)構(gòu)模型示意圖。該服務(wù)衛(wèi)星由基座和搭載基座上的空間機(jī)械臂組成，機(jī)械臂是由連桿和基座組成，在機(jī)械臂末端安裝有測(cè)量目標(biāo)衛(wèi)星位姿信息的手眼相機(jī)。機(jī)械臂第一關(guān)節(jié)J₁連接空間機(jī)器人基座和連桿B₁，第二關(guān)節(jié)J₂連接連桿B₁和連桿B₂，以此類(lèi)推。

參照?qǐng)D3，是本發(fā)明的用于空間合作/非合作衛(wèi)星對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”捕獲的方法的一個(gè)實(shí)施例的流程示意圖。其包括步驟：

S1：根據(jù)安裝在機(jī)械臂上的手眼相機(jī)測(cè)量，得到末端點(diǎn)與對(duì)接環(huán)中心點(diǎn)的相對(duì)位置矢量Δr和二者z軸之間的夾角θ。

S2：根據(jù)對(duì)接環(huán)面的法相量z_A，求出相對(duì)位置矢量Δr在對(duì)接環(huán)面的投影b_n。由于對(duì)接環(huán)的半徑R的大小已知，可以得出距離機(jī)械臂末端最近的抓捕點(diǎn)T，從而得到機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp，通過(guò)計(jì)算求得機(jī)械臂末端點(diǎn)到對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)姿態(tài)偏差ΔO。

S3：判斷相對(duì)位置偏差模||Δp||和姿態(tài)偏差模||ΔO||是否小于初始時(shí)設(shè)定的臨界值ε_p和ε_o。若二者小于臨界值，則機(jī)械臂末端的手爪閉合，實(shí)現(xiàn)目標(biāo)的捕獲。反之，進(jìn)行下一步。所述臨界值ε_p小于等于50毫米，ε_o小于等于5°。

S4：由得到的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO，根據(jù)公式(1)規(guī)劃?rùn)C(jī)械臂末端的線速度與角速度。

S5：根據(jù)S4中得到的機(jī)械臂末端規(guī)劃的線速度和角速度，通過(guò)雅可比矩陣反解，求得機(jī)械臂末端各個(gè)關(guān)節(jié)角的角速度。

S6：由S5中得到的關(guān)節(jié)角速度，驅(qū)動(dòng)機(jī)械臂各個(gè)關(guān)節(jié)運(yùn)動(dòng)，直至機(jī)械臂末端與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位姿模小于初始時(shí)設(shè)定的臨界值ε_p和ε_o，手抓閉合，完成目標(biāo)衛(wèi)星的捕獲。

參照?qǐng)D4，作為本發(fā)明的一個(gè)實(shí)施例的物理模型示意圖，其中：

B₀：剛體0，即空間機(jī)器人基座；

B_i(i＝1,……,n)：剛體i，機(jī)械臂的第i個(gè)連桿；

J_i：連接B_i－1和B_i的關(guān)節(jié)；

C_i：B_i的質(zhì)心；

O_I：慣性系原點(diǎn)；

O_g：整個(gè)系統(tǒng)的質(zhì)心；

∑_I、∑_E：分別表示慣性系、機(jī)械臂末端坐標(biāo)系；

∑_i：B_i的固連坐標(biāo)系，Z_i正向?yàn)镴_i的旋轉(zhuǎn)方向；

ⁱA_j∈R^3×3：∑_j相對(duì)于∑_i的姿態(tài)變換矩陣，當(dāng)i缺省時(shí)，表示∑_j相對(duì)于慣性系的姿態(tài)變換矩陣，ⁱA_j＝[ⁱn_j,ⁱo_j,ⁱa_j]；

E,O：分別為單位矩陣和零矩陣；

m_i,M：m_i是B_i的質(zhì)量，且

ⁱI_j∈R^3×3：在坐標(biāo)系∑_i表示下，剛體B_j相對(duì)于其質(zhì)心的慣量矩陣，當(dāng)i缺省時(shí)，表示在慣性系中的表示；

k_i∈R³：表示J_i旋轉(zhuǎn)方向的單位矢量；

r_i∈R³：C_i的位置矢量；

r_g∈R³：系統(tǒng)質(zhì)心的位置矢量；

p_i∈R³：J_i的位置矢量；

p_e∈R³：機(jī)械臂末端的位置矢量；

a_i,b_i∈R³：在∑_i下，分別為從J_i指向C_i，C_i指向J_i+1的位置矢量；

l_i∈R³：從∑_i原點(diǎn)指向∑_i+1原點(diǎn)的矢量，l_i＝a_i+b_i；

分別表示∑i相對(duì)于∑j的線速度和角速度，在∑k中的表示，如果∑j或者∑k為慣性系，則可以省去相應(yīng)的符號(hào)j或者k；

v₀,ω₀∈R³：基座的線速度和角速度；

v_i，ω_i∈R³：B_i的線速度和角速度；

θ_i：桿件i旋轉(zhuǎn)的角度；

Θ∈Rⁿ：關(guān)節(jié)角速度，即Θ＝[θ₁,…,θ_n]；

Ψ_b∈R³：基座的姿態(tài)角，用z-y-x歐拉角表示，即Ψ_b＝[α_b,β_b,γ_b]^T

Ψ₀：基座姿態(tài)歐拉角，按照x-y-z歐拉角的形式表示；

Ψ_e：空間機(jī)器人機(jī)械臂末端的姿態(tài)歐拉角，按照x-y-z歐拉角的形式表示。

對(duì)于矢量的表示方法，作如下規(guī)定：ⁱv表示在固連于第i個(gè)剛體的坐標(biāo)系∑_i中的速度矢量，而當(dāng)v沒(méi)有左上標(biāo)i的標(biāo)識(shí)時(shí)表示在慣性坐標(biāo)系中的速度矢量。為簡(jiǎn)化兩個(gè)矢量叉乘計(jì)算，定義如下叉乘算子：若p＝[x,y,z]^T，則

因此，可以把兩個(gè)向量的叉乘計(jì)算轉(zhuǎn)化為一個(gè)矩陣乘上一個(gè)向量的運(yùn)算，即

可以得出，空間機(jī)器人機(jī)械臂中的各個(gè)剛體質(zhì)心的位置矢量如公式(3)所示。同理，空間機(jī)器人機(jī)械臂末端的位置矢量如公式(4)所示。

對(duì)公式(3)和公式(4)兩邊進(jìn)行求導(dǎo)，由此可以得出空間機(jī)器人機(jī)械臂中的各個(gè)剛體質(zhì)心的線速度以及空間機(jī)器人機(jī)械臂末端的線速度為，

根據(jù)空間機(jī)器人機(jī)械臂中各個(gè)剛體的運(yùn)動(dòng)規(guī)律，空間機(jī)器人機(jī)械臂中各個(gè)剛體質(zhì)心的角速度和機(jī)械臂末端的角速度可以表示為，

將式(6)和式(8)寫(xiě)成矩陣的形式：

在公式(9)中，J_b代表和空間機(jī)器人基座的運(yùn)動(dòng)狀態(tài)相關(guān)的雅克比矩陣。

在公式(10)中，塊矩陣J_bv和塊矩陣J_bw是J_b的分塊矩陣，二者分別與空間機(jī)器人基座的線速度和角速度相關(guān)。

J_m代表與空間機(jī)器人機(jī)械臂運(yùn)動(dòng)狀態(tài)相關(guān)的雅可比矩陣，其結(jié)果和與之對(duì)應(yīng)的基座固定的機(jī)器人的雅可比矩陣一致，即：

類(lèi)似地，J_mv、J_mw是J_m的分塊矩陣。

當(dāng)空間機(jī)器人系統(tǒng)處于自由漂浮狀態(tài)且該系統(tǒng)不受任何外力/力矩作用時(shí)，系統(tǒng)的動(dòng)量守恒。假設(shè)空間機(jī)器人系統(tǒng)在初始時(shí)線動(dòng)量和角動(dòng)量為零，那么整個(gè)空間機(jī)器人系統(tǒng)角動(dòng)量不但符合公式(12)完整約束方程，而且還符合公式(13)的非完整約束方程：

將式(5)和式(7)帶入到(13)可得

其中，

J_Ri＝[k₁,k₂,...,k_i,0,...,0]∈R^3xn (17)

r_0i＝r_i-r₀ (18)

參照?qǐng)D3所示的規(guī)劃流程，空間機(jī)械臂末端期望的線速度和期望的角速度按照公式(1)進(jìn)行規(guī)劃。

在公式(1)中，Kp、Kv分別為規(guī)劃的比例、微分參數(shù)(對(duì)應(yīng)于分解運(yùn)動(dòng)控制的PD參數(shù))；分別表示通過(guò)手眼相機(jī)測(cè)量出的目標(biāo)的運(yùn)動(dòng)速度；Δp表示械臂末端相對(duì)于目標(biāo)星對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的位置誤差；ΔO表示機(jī)械臂末端相對(duì)于目標(biāo)抓捕點(diǎn)的姿態(tài)指向誤差。以下，詳細(xì)說(shuō)明Δp和ΔO的求算方法。

根據(jù)安裝在機(jī)械臂末端的相機(jī)可以得出機(jī)械臂末端相對(duì)于對(duì)接環(huán)中心點(diǎn)的相對(duì)位置Δr和在空間機(jī)器人機(jī)械臂末端坐標(biāo)系下表示的目標(biāo)衛(wèi)星對(duì)接環(huán)平面的法向量z_A。機(jī)械臂末端坐標(biāo)軸z_E與對(duì)接環(huán)法向量z_A軸之間的夾角為，

θ＝arc cos(z_E·z_A) (19)

相對(duì)位置Δr在對(duì)接環(huán)平面法相向量z_A的投影a_n如下式所示。

a_n＝||Δr||z_A cosθ (20)

相對(duì)位置Δr在對(duì)接環(huán)平面的投影b_n如下式所示。

b_n＝Δr-a_n (21)

根據(jù)公式(21)可以得出，空間機(jī)器人機(jī)械臂末端點(diǎn)和對(duì)接環(huán)上捕獲點(diǎn)的相對(duì)位置矢量。

該方法中，僅期望機(jī)械臂末端坐標(biāo)軸z軸與捕獲點(diǎn)坐標(biāo)軸z軸重合，空間機(jī)器人末端點(diǎn)和對(duì)接環(huán)上捕獲點(diǎn)的相對(duì)姿態(tài)矢量如公式(23)所示。

ΔO_e＝e_n·θ (23)

在公式(23)中，e_n為旋轉(zhuǎn)軸，且e_n＝z_E×z_A,e_n＝[e_x e_y e_z]^T。

根據(jù)公式(22)和公式(23)，可以得出了空間機(jī)器人末端點(diǎn)和目標(biāo)星對(duì)接環(huán)“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差與相對(duì)姿態(tài)偏差。

在實(shí)際應(yīng)用中，機(jī)械臂末端的運(yùn)動(dòng)速度不能超過(guò)設(shè)計(jì)的最大值，且需要保證機(jī)械臂在啟動(dòng)過(guò)程的平穩(wěn)性。因而對(duì)空間機(jī)器人機(jī)械臂末端運(yùn)動(dòng)速度做下述處理。

對(duì)于機(jī)械臂末端的線速度，

對(duì)于機(jī)械臂末端的角速度，

其中，t_s為設(shè)定的平滑啟動(dòng)時(shí)間，如t_s＝2s。這樣，可以確保機(jī)械臂在0-t_s內(nèi)平滑啟動(dòng)到合成速度最大值為v_em(方向矢量仍然是v_em)；而當(dāng)啟動(dòng)后的抓捕過(guò)程中，機(jī)械臂末端速度≥v_em時(shí)，將其限制在v_em(方向矢量仍然是v_em)，這樣的處理在工程中非常重要。

將公式(12)代入公式(9)中，可以將空間機(jī)械臂的末端線速度和角速度如下表示：

當(dāng)機(jī)械臂末端線速度和角速度在機(jī)械臂末端坐標(biāo)系中表示時(shí)，可以得出，

在公式(29)中，

在公式(33)中，表示與空間機(jī)器人機(jī)械臂相關(guān)的雅可比矩陣，在末端坐標(biāo)系表示。當(dāng)采用x-y-z歐拉角表示空間機(jī)器人基座的姿態(tài)時(shí)，空間機(jī)器人基座的歐拉角的速率與基座姿態(tài)角速度的關(guān)系表達(dá)式如下。

對(duì)上式進(jìn)行數(shù)值積分，可以得出基座姿態(tài)的歐拉角表示。另外，空間機(jī)器人系統(tǒng)還滿(mǎn)足角動(dòng)量守恒方程，即

其中，

J_Ti＝[k₁×(r_i-p₁),k₂×(r_i-p₂),...,k_i×(r_i-p_i),0,...,0]∈R^3xn (41)

r_0g＝r_g-r₀ (42)

由于Hs是非奇異的，因此：

對(duì)于公式(43)中的各個(gè)矢量，當(dāng)同時(shí)在空間機(jī)器人機(jī)械臂的末端坐標(biāo)系表示時(shí)，最終空間機(jī)器人基座的角速度如下。

通過(guò)搭載在基座上的姿態(tài)傳感器，我們可以測(cè)量出空間機(jī)器人基座的姿態(tài)及姿態(tài)角速度，由此可得出如下關(guān)系。

公式(45)的左側(cè)部分表示在末端坐標(biāo)系下，空間機(jī)器人末端相對(duì)于空間機(jī)器人基座的相對(duì)速度，由此可以將公式(45)寫(xiě)為：

在公式(46)中，是在末端坐標(biāo)系下，機(jī)械臂末端相對(duì)于空間機(jī)器人基座的線速度。在末端坐標(biāo)系下，機(jī)械臂末端相對(duì)于空間機(jī)器人基座的角速度。而且二者同時(shí)在空間機(jī)器人末端坐標(biāo)系中表示。

則針對(duì)動(dòng)力學(xué)奇異回避問(wèn)題轉(zhuǎn)換為針對(duì)方程(47)的運(yùn)動(dòng)學(xué)奇異回避問(wèn)題，而雅可比矩陣只包含空間機(jī)器人運(yùn)動(dòng)學(xué)參數(shù)，具有簡(jiǎn)單的表達(dá)式。當(dāng)關(guān)節(jié)角速度確定后，可采用數(shù)值積分的方法得到關(guān)節(jié)角，其中一種最簡(jiǎn)單的方法為一步積分法。

通過(guò)公式(48)得到的關(guān)節(jié)角速度，驅(qū)動(dòng)機(jī)械臂各個(gè)關(guān)節(jié)運(yùn)動(dòng)，直至機(jī)械臂末端與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位姿小于初始時(shí)設(shè)定的臨界值ε_p和ε_o，手爪閉合，完成目標(biāo)衛(wèi)星的捕獲。

本發(fā)明提供的一種可快速抓捕目標(biāo)衛(wèi)星的方法與系統(tǒng)，通過(guò)利用空間機(jī)器人機(jī)械臂末端點(diǎn)與“動(dòng)態(tài)最近點(diǎn)”的相對(duì)位置偏差Δp和相對(duì)姿態(tài)偏差ΔO與設(shè)定的臨界值ε_p和ε_o的對(duì)比判斷，以及當(dāng)不滿(mǎn)足臨界值時(shí)通過(guò)規(guī)劃?rùn)C(jī)械臂末端的運(yùn)動(dòng)，進(jìn)而控制機(jī)械臂末端的手爪閉合，進(jìn)而實(shí)現(xiàn)目標(biāo)的捕獲，且達(dá)到時(shí)時(shí)接近的都是對(duì)接環(huán)上離得最近的一個(gè)點(diǎn)，即“動(dòng)態(tài)最近點(diǎn)”，實(shí)現(xiàn)快速抓捕的目的。

以上是對(duì)本發(fā)明的較佳實(shí)施進(jìn)行了具體說(shuō)明，但本發(fā)明創(chuàng)造并不限于所述實(shí)施例，熟悉本領(lǐng)域的技術(shù)人員在不違背本發(fā)明精神的前提下還可做出種種的等同變形或替換，這些等同的變形或替換均包含在本申請(qǐng)權(quán)利要求所限定的范圍內(nèi)。