胡慶雷，王永智，石 忠，3

(1.哈爾濱工業(yè)大學(xué) 航天學(xué)院，150001 哈爾濱，huqinglei@hit.edu.cn;2.中國民航大學(xué) 航空自動化學(xué)院，300300 天津;3.濱州職業(yè)學(xué)院，256603 山東濱州)

自由漂浮空間機(jī)器人力矩最優(yōu)軌跡規(guī)劃算法

胡慶雷1，王永智2，石 忠2，3

(1.哈爾濱工業(yè)大學(xué) 航天學(xué)院，150001 哈爾濱，huqinglei@hit.edu.cn;2.中國民航大學(xué) 航空自動化學(xué)院，300300 天津;3.濱州職業(yè)學(xué)院，256603 山東濱州)

針對自由漂浮空間機(jī)器人的軌跡規(guī)劃問題，提出一種基于粒子群優(yōu)化算法的機(jī)械臂關(guān)節(jié)角驅(qū)動力矩最優(yōu)軌跡規(guī)劃算法.首先通過對自由漂浮空間機(jī)器人系統(tǒng)的動力學(xué)方程進(jìn)行分析，給出了以機(jī)械臂關(guān)節(jié)角驅(qū)動力矩為目標(biāo)函數(shù)的軌跡最優(yōu)控制算法，并采用高階多項式插值方法逼近機(jī)械臂關(guān)節(jié)角軌跡，結(jié)合粒子群優(yōu)化算法對機(jī)械臂關(guān)節(jié)角軌跡進(jìn)行優(yōu)化求解.數(shù)值仿真表明，規(guī)劃出的關(guān)節(jié)角軌跡平滑連續(xù)，在完成自由漂浮空間機(jī)器人姿態(tài)調(diào)整任務(wù)的同時，機(jī)械臂關(guān)節(jié)角驅(qū)動力矩降至最低.

自由漂浮;空間機(jī)器人;動力學(xué)方程;粒子群優(yōu)化算法;力矩最優(yōu)

空間機(jī)器人在未來的空間活動中將被廣泛應(yīng)用于衛(wèi)星的捕捉、維修、燃料補(bǔ)給等空間在軌服務(wù)［1－2］.自由漂浮空間機(jī)器人可以僅依靠機(jī)械臂的運(yùn)動，同時調(diào)整載體航天器和機(jī)械臂末端執(zhí)行器的位姿，節(jié)省了姿控燃料.機(jī)械臂由關(guān)節(jié)處的伺服電機(jī)輸出的控制力矩驅(qū)動，消耗系統(tǒng)電能.因此，為節(jié)省系統(tǒng)能量的消耗，需要通過規(guī)劃空間機(jī)械臂的運(yùn)動，使空間機(jī)器人在完成姿態(tài)調(diào)整任務(wù)的同時，將系統(tǒng)能量消耗降至最低.

近年來，針對不同的工程背景和優(yōu)化目標(biāo)，許多學(xué)者提出了相應(yīng)的空間機(jī)器人運(yùn)動規(guī)劃方案.Dubowsky等［3］采用增強(qiáng)擾動圖技術(shù)規(guī)劃空間機(jī)械臂運(yùn)動，使機(jī)械臂對載體姿態(tài)的擾動最小，但這種方法仍然需要消耗燃料才能維持載體航天器姿態(tài)的不變.Fernandes等［4］將帶有空間機(jī)械臂的航天器姿態(tài)控制問題轉(zhuǎn)化為非完整運(yùn)動規(guī)劃問題，基于最優(yōu)控制方法求解非完整運(yùn)動規(guī)劃問題，定義機(jī)械臂關(guān)節(jié)角耗散能為優(yōu)化目標(biāo)函數(shù)，在最優(yōu)控制算法中采用修整牛頓迭代算法尋優(yōu)，該算法需要進(jìn)行微分求導(dǎo)，計算量較大.張奇志等［5］在此基礎(chǔ)上，將遺傳算法引入到空間機(jī)械臂非完整運(yùn)動規(guī)劃中.針對空間機(jī)器人的擾動特性，黃攀峰等［6］定義了空間機(jī)械臂與載體航天器之間的擾動映射關(guān)系，基于遺傳算法提出了空間機(jī)器人最小擾動路徑規(guī)劃算法，但該算法僅適用于自由飛行空間機(jī)器人，仍然需要消耗姿控燃料調(diào)整載體航天器的姿態(tài).徐文福等［7－8］利用最優(yōu)控制方法，探討了自由漂浮空間機(jī)器人系統(tǒng)存在動力學(xué)約束情況下的非完整運(yùn)動規(guī)劃方法，利用遺傳算法進(jìn)行軌跡的優(yōu)化求解，并且對捕捉空間目標(biāo)及捕捉目標(biāo)后自由漂浮空間機(jī)器人的?？亢椭匦露ㄏ驎r的軌跡規(guī)劃算法進(jìn)行了相應(yīng)的研究.

為節(jié)省機(jī)械臂運(yùn)動過程中伺服電機(jī)消耗的能量，本文通過對系統(tǒng)動力學(xué)方程進(jìn)行分析，提出了以機(jī)械臂關(guān)節(jié)角驅(qū)動力矩為最優(yōu)控制目標(biāo)函數(shù)的軌跡規(guī)劃算法，結(jié)合粒子群算法對目標(biāo)函數(shù)進(jìn)行優(yōu)化求解，并且將粒子群優(yōu)化算法在自由漂浮空間機(jī)器人軌跡規(guī)劃中的尋優(yōu)性能同遺傳算法進(jìn)行了比較.本文最后對力矩最優(yōu)軌跡規(guī)劃算法進(jìn)行仿真，在完成空間機(jī)器人姿態(tài)調(diào)整任務(wù)的同時，系統(tǒng)消耗的能量降到了最低.

1 自由漂浮空間機(jī)器人系統(tǒng)

1.1 自由漂浮空間機(jī)器人系統(tǒng)動力學(xué)

考慮由載體航天器和二連桿機(jī)械臂組成的空間機(jī)器人在平面內(nèi)的運(yùn)動(如圖1所示)，連接機(jī)器人各構(gòu)件的關(guān)節(jié)均為只有1個旋轉(zhuǎn)自由度的轉(zhuǎn)動關(guān)節(jié).

圖1 空間機(jī)器人系統(tǒng)結(jié)構(gòu)

當(dāng)空間機(jī)器人系統(tǒng)處于自由漂浮狀態(tài)時，忽略系統(tǒng)所受的微弱重力作用，因此系統(tǒng)的勢能為零.將系統(tǒng)總動能表達(dá)式代入拉格朗日方程中即可得到系統(tǒng)的動力學(xué)方程，表達(dá)式如下:

其中:M(q)為正定對稱的系統(tǒng)慣量矩陣;q=［θq1q2］T為系統(tǒng)相對關(guān)節(jié)角矢量;C(q，˙q)為含有離心力和科氏力的矩陣;τ=［0 τTm］T，τm表示機(jī)械臂關(guān)節(jié)處輸出的控制力矩.令

其中:qm=［q1q2］T為機(jī)械臂的相對關(guān)節(jié)角矢量;Cb和Cm分別為與載體航天器和機(jī)械臂運(yùn)動相關(guān)的非線性力項與對應(yīng)的姿態(tài)角速度的乘積，其中

則系統(tǒng)動力學(xué)方程可以轉(zhuǎn)化為

由式(1)可得

將式(3)代入式(2)中經(jīng)過整理可得

則系統(tǒng)動力學(xué)方程可以轉(zhuǎn)化為

1.2 自由漂浮空間機(jī)器人系統(tǒng)非完整運(yùn)動規(guī)劃

自由漂浮空間機(jī)器人系統(tǒng)的系統(tǒng)動量及動量矩守恒.由于自由漂浮空間機(jī)器人系統(tǒng)動量矩守恒方程不可積分，因此，系統(tǒng)的運(yùn)動受非完整性約束.非完整運(yùn)動規(guī)劃問題即為:在給定系統(tǒng)的初始狀態(tài)和終止?fàn)顟B(tài)的情況下，設(shè)計適當(dāng)?shù)目刂戚斎?，使系統(tǒng)在有限時間內(nèi)沿某一軌線從給定的初始狀態(tài)運(yùn)動到終止?fàn)顟B(tài).假設(shè)系統(tǒng)在初始時刻的動量矩為零，則系統(tǒng)的動量矩守恒方程可以寫成［9］

上式中Bi(i=0，1，2)是系統(tǒng)慣性參數(shù)和機(jī)械臂關(guān)節(jié)角q1、q2的非線性函數(shù).

選取機(jī)械臂相對關(guān)節(jié)角速度˙qm為輸入變量，由式(5)可以得到

式中

當(dāng)給定空間機(jī)器人系統(tǒng)的初始和期望狀態(tài)qin和qdes時，非完整運(yùn)動規(guī)劃問題轉(zhuǎn)化為通過優(yōu)化目標(biāo)函數(shù)尋找控制輸入˙qm，確定系統(tǒng)狀態(tài)轉(zhuǎn)移的最優(yōu)軌線.

2 自由漂浮空間機(jī)器人關(guān)節(jié)力矩最優(yōu)軌跡規(guī)劃

空間機(jī)械臂的運(yùn)動由關(guān)節(jié)角處的伺服電機(jī)驅(qū)動，需要消耗系統(tǒng)的電能.為了提高系統(tǒng)電能的利用效率，本文從機(jī)械臂關(guān)節(jié)角控制力矩優(yōu)化提出了自由漂浮空間機(jī)器人力矩最優(yōu)軌跡規(guī)劃算法.定義力矩最優(yōu)控制的目標(biāo)函數(shù)為

其中t0和T分別是空間機(jī)器人系統(tǒng)運(yùn)動過程的初始和終止時間，通過計算系統(tǒng)動力學(xué)方程的變形(4)可以求得空間機(jī)械臂關(guān)節(jié)角處的驅(qū)動力矩τ1和τ2.

基于機(jī)械臂關(guān)節(jié)角驅(qū)動力矩最優(yōu)的軌跡規(guī)劃問題可以表示成如下求解非線性約束優(yōu)化問題:

懲罰函數(shù)法是求解非線性約束優(yōu)化問題常用的一種方法，它根據(jù)約束條件的特點(diǎn)構(gòu)造某種懲罰函數(shù)并加入到目標(biāo)函數(shù)中，將約束優(yōu)化問題轉(zhuǎn)化為無約束優(yōu)化問題.此處引入罰函數(shù)法，則式(7)改寫為

其中:α為懲罰因子;θfin是載體的期望姿態(tài)角;θ(T)是在給定輸入˙qm(T)根據(jù)式(5)求得的載體姿態(tài)角.在進(jìn)行數(shù)值求解的過程中，使用四階龍格-庫塔方法，該方法計算精度高，程序?qū)崿F(xiàn)容易.

機(jī)械臂關(guān)節(jié)角q1和q2的運(yùn)動軌跡是時間t的非線性函數(shù)，故可以采用時間t的高階多項式逼近.式(6)中的控制輸入可以寫成

由式(6)和式(8)可知，θ(T)是插值多項式的系數(shù)b的函數(shù)，則有g(shù)(b，t).因此，目標(biāo)函數(shù)可以寫成

當(dāng)懲罰因子α確定時，基于機(jī)械臂關(guān)節(jié)角驅(qū)動力矩最優(yōu)的自由漂浮空間機(jī)器人軌跡規(guī)劃問題便轉(zhuǎn)化為尋找插值多項式的系數(shù)b使目標(biāo)函數(shù)(9)最小的問題.

3 粒子群優(yōu)化算法在自由漂浮空間機(jī)器人軌跡規(guī)劃算法中的應(yīng)用

3.1 粒子群算法的基本原理

在對式(9)進(jìn)行參數(shù)尋優(yōu)的過程中引入粒子群優(yōu)化算法［10］.粒子群優(yōu)化算法模擬鳥類的捕食行為，優(yōu)化目標(biāo)函數(shù)的每個解都是搜索空間的1個“粒子”，所有粒子都對應(yīng)于1個由目標(biāo)函數(shù)確定的適應(yīng)度值.粒子在搜索空間中以一定的速度和方向飛行，通過對環(huán)境的學(xué)習(xí)與適應(yīng)，根據(jù)個體與群體的飛行經(jīng)驗的綜合分析結(jié)果來動態(tài)調(diào)整飛行速度和飛行方向［11］.

在D維連續(xù)空間中，粒子i在第d維子空間中的飛行速度vid和位置xid根據(jù)下式進(jìn)行更新:

其中:pgd是整體粒子群的歷史最優(yōu)位置記錄;pid是當(dāng)前微粒的歷史最優(yōu)位置記錄;ω為慣性權(quán)值;c1和c2為加速度常數(shù);rand(1)為隨機(jī)變量.粒子i在d維子空間中的位置xid對應(yīng)于目標(biāo)函數(shù)(9)的解，即插值多項式的系數(shù)b.

3.2 粒子群算法在力矩最優(yōu)軌跡規(guī)劃中的應(yīng)用

由前文分析可得，基于粒子群算法的自由漂浮空間機(jī)器人力矩最優(yōu)軌跡規(guī)劃算法如下:

首先，設(shè)定種群的粒子個數(shù)N和算法最大迭代次數(shù)Nmax，初始化粒子群算法參數(shù).定義種群的第 i個粒子，其中和分別表示用于逼近關(guān)節(jié)角q1和q2軌跡的高階多項式系數(shù);每個粒子的最優(yōu)位置pid設(shè)為其初始位置xi，pid中的最優(yōu)值設(shè)為pgd.

Step1 令迭代次數(shù)k=1，初始化含有N個粒子(X1(k)，X2(k)，…，XN(k))的種群;

Step2 根據(jù)目標(biāo)函數(shù)式(9)計算每個粒子的適應(yīng)度J(Xi(k));

Step3將每個微粒的適應(yīng)值J(Xi(k))與其經(jīng)歷過的最優(yōu)位置pid進(jìn)行比較，如果優(yōu)于pid，則將其作為當(dāng)前的最優(yōu)位置pgd;

Step4 將每個微粒的適應(yīng)值J(Xi(k))與群體所經(jīng)歷過的最優(yōu)位置pgd進(jìn)行比較，如果優(yōu)于pgd，則將其作為群體最優(yōu)位置pgd;

Step5 根據(jù)式(10)和(11)調(diào)整當(dāng)前粒子的位置xi和速度vi;

Step6 當(dāng)達(dá)到最大迭代次數(shù)Nmax時，終止迭代;否則返回Step2.

4 仿真實例

為了驗證本文所提出的自由漂浮空間機(jī)器人力矩最優(yōu)軌跡規(guī)劃算法的有效性，在Matlab7.0環(huán)境下進(jìn)行仿真，模型采用文獻(xiàn)［9］給出的空間機(jī)器人的物理參數(shù)，如表1所示.

表1 空間機(jī)器人系統(tǒng)參數(shù)

系統(tǒng)運(yùn)動時間T=10，為保證粒子群優(yōu)化算法的收斂性，采用文獻(xiàn)［12］的中參數(shù):慣性權(quán)值ω=0.729 8，最大迭代次數(shù)取200，加速度常數(shù)c1=c2=1.496 2，初始化群體個數(shù)N=25.

空間機(jī)器人系統(tǒng)的初始和期望姿態(tài)分別為:qin=(-40°，90°，30°)，qdes=(0°，-60°，90°)，初始及期望狀態(tài)下的姿態(tài)角速度、加速度均為零.

通過粒子群算法優(yōu)化目標(biāo)函數(shù)(9)，可以求得高階多項式的系數(shù)b.仿真結(jié)果如圖2～圖7所示.從仿真結(jié)果可以看出，利用本文算法規(guī)劃出的軌跡平滑連續(xù)，機(jī)械臂關(guān)節(jié)角速度和角加速度分別被限制在(－0.5，0.5)rad/s及(－0.2，0.3)rad/s2范圍內(nèi)，較小的角(加)速度保證了系統(tǒng)運(yùn)行的穩(wěn)定.圖5顯示了空間機(jī)器人系統(tǒng)僅通過機(jī)械臂關(guān)節(jié)的運(yùn)動同時調(diào)整載體航天器和空間機(jī)械臂位姿的運(yùn)動過程.圖6為最優(yōu)目標(biāo)函數(shù)值隨著迭代次數(shù)增加的變化情況，較快的收斂速度驗證了粒子群算法在求解非線性無約束優(yōu)化問題上的適用性.

為驗證粒子群優(yōu)化算法在搜索自由漂浮空間機(jī)器人關(guān)節(jié)角驅(qū)動力矩最優(yōu)軌跡上的優(yōu)越性，分別采用粒子群優(yōu)化算法和遺傳算法對式(9)進(jìn)行優(yōu)化求解.2種算法的迭代次數(shù)均選為200，種群大小均選為25，遺傳算法中采用20位的二進(jìn)制編碼表示決策變量，交叉概率和變異概率分別為0.8和0.01.粒子群算法其他參數(shù)與上例相同.2種算法尋優(yōu)過程如下:經(jīng)過200代尋優(yōu)計算，分別使用PSO和GA優(yōu)化目標(biāo)函數(shù)得到的最優(yōu)值為1.12×106和1.79×106.圖7分別是2種算法的進(jìn)化過程，使用PSO算法尋優(yōu)時，起始階段優(yōu)化得出的目標(biāo)函數(shù)值較大，但經(jīng)過大約30代左右的進(jìn)化即可快速收斂于最優(yōu)解;而遺傳算法的收斂速度較慢.從上述仿真實驗及前文的算法對比分析可以看出，PSO算法在多數(shù)的情況下，比遺傳算法更適合應(yīng)用于自由漂浮空間機(jī)器人力矩最優(yōu)軌跡規(guī)劃算法，計算速度和效率更高.

圖2 載體姿態(tài)角、關(guān)節(jié)角1、關(guān)節(jié)角2軌跡

圖3 關(guān)節(jié)角1角速度、關(guān)節(jié)角2角速度

圖4 關(guān)節(jié)角1加速度、關(guān)節(jié)角2加速度

圖5 系統(tǒng)運(yùn)動過程

圖6 目標(biāo)函數(shù)值

圖7 粒子群算法與遺傳算法的搜索性能對比

5 結(jié)論

空間機(jī)械臂由關(guān)節(jié)角處的伺服電機(jī)驅(qū)動，需要消耗系統(tǒng)電能.為提高自由漂浮空間機(jī)器人系統(tǒng)電能的使用效率，本文通過對系統(tǒng)動力學(xué)方程的分析，提出了基于機(jī)械臂關(guān)節(jié)角控制力矩最優(yōu)的軌跡規(guī)劃算法，并結(jié)合粒子群優(yōu)化算法尋找最優(yōu)姿態(tài)軌跡.最后，通過實例仿真驗證了本文提出的自由漂浮空間機(jī)器人力矩最優(yōu)軌跡規(guī)劃算法的有效性，在完成空間機(jī)器人位姿調(diào)整任務(wù)的同時，機(jī)械臂關(guān)節(jié)處輸出的控制力矩得到了優(yōu)化，節(jié)省了系統(tǒng)寶貴的能源.

［1］PAPADOPOULOS E，DUBOWSKY S.Dynamic singularities in free-floating space manipulators［J］.Journal of Dynamics Systems Measurement and Control Transactions of the ASME，1993，115(1):44－52.

［2］WANG H L，XIE Y CH.Adaptive jacobian position/force tracking control of free-flying manipulators［J］.Journal of Robotics and Autonomous Systems，2009，57:173－181.

［3］DUBOWSKY S，TORRES M.Path planning for space manipulators to minimize spacecraft attitude disturbances［C］//Proceedings of the 1991 IEEE International Conference on Robotics and Automation.Piscataway:IEEE，1991:2522－2528.

［4］FERNANDES C，GURVITS L，LI Z X.Attitude control of space platform/manipulator system using internal motion［C］//Proceedings of the 1992 IEEE International Conference on Robotics and Automation.Piscataway:IEEE，1992:893－898.

［5］張奇志，戈新生，劉延柱.基于小波逼近的空間機(jī)械臂非完整運(yùn)動最優(yōu)規(guī)劃的遺傳算法［J］.機(jī)器人，1999，21(5):357 －362，369.

［6］HUANG P F，CHEN K，XU Y SH.Optimal path planning for minimizing disturbance of space robot［C］//Proceedings of 9th International Conference on Control，Automation，Robotics and Vision.Pisacataway:IEEE，2006:1－6.

［7］XU W F，LIANG B，QIANG W Y，et al.Trajectory planning of space robot system for target berthing and reorientation after capturing［C］//Proceedings of the 6th World Congress on Intelligent Control and Automation.Dalian:［s.n.］，2006:8981 －8985.

［8］XU W F，LIANG B，XU Y SH.Autonomous Path Planning and Experiment Study of Free-Floating Space Robot for Target Capturing［J］.Journal of Intelligent and Robotic Systems，2008，51(3):303－331.

［9］TORTOPIDIS I，PAPADOPOULOS E.On point-to-point planning for underactuated space manipulator systems［J］.Journal and Autonomous Systems，2007，55:122 －131.

［10］EBERHART R，KENNEDY J.A new optimizer using particles swarm theory［C］//Proceedings of the Sixth International Symposium on Micro Machine and Human Science.Nagoya:［s.n.］，1995:39 －43.

［11］吳啟迪，汪鐳.智能微粒群算法研究及其應(yīng)用［M］.南京:江蘇教育出版社，2005:15－18.

［12］CLERC M，KENNEDY J.The particle swarm explosion，stability，and convergence in a multi-dimensional complex space［J］.IEEE Transaction on Evolutionary Computation，2002，6(1):58 －73.

Minimum torque trajectory planning algorithm for free-floating space robot

HU Qing-lei1，WANG Yong-zhi2，SHI Zhong2，3

(1.School of Astronuantics，Harbin Institute of Technology，150001 Harbin，China，huqinglei@hit.edu.cn;2.College of Aeronautical Automation，Civil Aviation University of China，300300 Tianjin，China;3.Binzhou Vocation College，256603 Shandong Binzhou，China)

Considering the problem of trajectory planning for Free-Floating Space Robot(FFSR)，a particle swarm optimization(PSO)based on minimum torque trajectory planning scheme is presented.By analyzing the dynamic equation of system，an optimal control algorithm is applied to FFSR trajectory planning，in which the driving torque of manipulator joint angles is considered as the objective function.Moreover，the high order polynomial is used to approach the trajectories of the joint angles.To achieve the optimal trajectory，the PSO algorithm is implemented so that the planned joint angle trajectories are smooth and continuous.Numerical simulation results show that the proposed methodology is effective and available for solving FFSR trajectory planning problem.With accomplishing the space robot attitude adjusting mission，the driving torque is reduced to the least.free-floating;space robot;dynamic equation;particle swarm optimization;minimizing torque

free-floating;space robot;dynamic equation;particle swarm optimization;minimizing torque

V42

A

0367－6234(2011)11－0020－05

2010－08－18.

國家自然科學(xué)基金資助項目(60774062、61004072);高等學(xué)校博士學(xué)科點(diǎn)專項科研基金資助項目(20070213061、20102302110031);教育部留學(xué)回國人員科研啟動基金資助;黑龍江省留學(xué)回國人員科學(xué)基金資助項目(LC08C01);中央高?；究蒲袠I(yè)務(wù)費(fèi)專項資金資助項目(HIT.NSRIF.2009003);哈爾濱市科技創(chuàng)新人才研究專項基金資助項目(2010RFLXG001).

胡慶雷(1979—)，男，副教授，博士生導(dǎo)師.

(編輯 張 宏)