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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，ω_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ī)械臂末端相對于基座的速度與角速度。

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

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

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

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

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

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

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

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

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

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

附圖說明

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

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

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

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

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

具體實施方式

需要說明的是，在不沖突的情況下，本申請中的實施例及實施例中的特征可以相互組合。

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

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

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

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

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

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

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

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

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

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

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

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

其中，ω_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ī)械臂末端相對于基座的速度與角速度。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

參照圖4，作為本發(fā)明的一個實施例的物理模型示意圖，其中：

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

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

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

C_i：B_i的質(zhì)心；

O_I：慣性系原點；

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

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

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

ⁱA_j∈R^3×3：∑_j相對于∑_i的姿態(tài)變換矩陣，當(dāng)i缺省時，表示∑_j相對于慣性系的姿態(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相對于其質(zhì)心的慣量矩陣，當(dāng)i缺省時，表示在慣性系中的表示；

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原點指向∑_i+1原點的矢量，l_i＝a_i+b_i；

分別表示∑i相對于∑j的線速度和角速度，在∑k中的表示，如果∑j或者∑k為慣性系，則可以省去相應(yīng)的符號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歐拉角的形式表示。

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

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

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

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

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

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

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

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

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

類似地，J_mv、J_mw是J_m的分塊矩陣。

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

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

其中，

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

r_0i＝r_i-r₀ (18)

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

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

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

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

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

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

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

b_n＝Δr-a_n (21)

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

該方法中，僅期望機(jī)械臂末端坐標(biāo)軸z軸與捕獲點坐標(biāo)軸z軸重合，空間機(jī)器人末端點和對接環(huán)上捕獲點的相對姿態(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ī)器人末端點和目標(biāo)星對接環(huán)“動態(tài)最近點”的相對位置偏差與相對姿態(tài)偏差。

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

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

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

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

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

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

在公式(29)中，

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

對上式進(jìn)行數(shù)值積分，可以得出基座姿態(tài)的歐拉角表示。另外，空間機(jī)器人系統(tǒ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是非奇異的，因此：

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

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

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

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

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

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

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

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