自由漂浮空間機器人力矩最優軌跡規劃算法

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

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

自由漂浮空間機器人力矩最優軌跡規劃算法

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

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

針對自由漂浮空間機器人的軌跡規劃問題，提出一種基于粒子群優化算法的機械臂關節角驅動力矩最優軌跡規劃算法.首先通過對自由漂浮空間機器人系統的動力學方程進行分析，給出了以機械臂關節角驅動力矩為目標函數的軌跡最優控制算法，并采用高階多項式插值方法逼近機械臂關節角軌跡，結合粒子群優化算法對機械臂關節角軌跡進行優化求解.數值仿真表明，規劃出的關節角軌跡平滑連續，在完成自由漂浮空間機器人姿態調整任務的同時，機械臂關節角驅動力矩降至最低.

自由漂浮;空間機器人;動力學方程;粒子群優化算法;力矩最優

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

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

為節省機械臂運動過程中伺服電機消耗的能量，本文通過對系統動力學方程進行分析，提出了以機械臂關節角驅動力矩為最優控制目標函數的軌跡規劃算法，結合粒子群算法對目標函數進行優化求解，并且將粒子群優化算法在自由漂浮空間機器人軌跡規劃中的尋優性能同遺傳算法進行了比較.本文最后對力矩最優軌跡規劃算法進行仿真，在完成空間機器人姿態調整任務的同時，系統消耗的能量降到了最低.

1 自由漂浮空間機器人系統

1.1 自由漂浮空間機器人系統動力學

考慮由載體航天器和二連桿機械臂組成的空間機器人在平面內的運動(如圖1所示)，連接機器人各構件的關節均為只有1個旋轉自由度的轉動關節.

圖1 空間機器人系統結構

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

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

其中:qm=［q1q2］T為機械臂的相對關節角矢量;Cb和Cm分別為與載體航天器和機械臂運動相關的非線性力項與對應的姿態角速度的乘積，其中

則系統動力學方程可以轉化為

由式(1)可得

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

則系統動力學方程可以轉化為

1.2 自由漂浮空間機器人系統非完整運動規劃

自由漂浮空間機器人系統的系統動量及動量矩守恒.由于自由漂浮空間機器人系統動量矩守恒方程不可積分，因此，系統的運動受非完整性約束.非完整運動規劃問題即為:在給定系統的初始狀態和終止狀態的情況下，設計適當的控制輸入，使系統在有限時間內沿某一軌線從給定的初始狀態運動到終止狀態.假設系統在初始時刻的動量矩為零，則系統的動量矩守恒方程可以寫成［9］

上式中Bi(i=0，1，2)是系統慣性參數和機械臂關節角q1、q2的非線性函數.

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

式中

當給定空間機器人系統的初始和期望狀態qin和qdes時，非完整運動規劃問題轉化為通過優化目標函數尋找控制輸入˙qm，確定系統狀態轉移的最優軌線.

2 自由漂浮空間機器人關節力矩最優軌跡規劃

空間機械臂的運動由關節角處的伺服電機驅動，需要消耗系統的電能.為了提高系統電能的利用效率，本文從機械臂關節角控制力矩優化提出了自由漂浮空間機器人力矩最優軌跡規劃算法.定義力矩最優控制的目標函數為

其中t0和T分別是空間機器人系統運動過程的初始和終止時間，通過計算系統動力學方程的變形(4)可以求得空間機械臂關節角處的驅動力矩τ1和τ2.

基于機械臂關節角驅動力矩最優的軌跡規劃問題可以表示成如下求解非線性約束優化問題:

懲罰函數法是求解非線性約束優化問題常用的一種方法，它根據約束條件的特點構造某種懲罰函數并加入到目標函數中，將約束優化問題轉化為無約束優化問題.此處引入罰函數法，則式(7)改寫為

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

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

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

當懲罰因子α確定時，基于機械臂關節角驅動力矩最優的自由漂浮空間機器人軌跡規劃問題便轉化為尋找插值多項式的系數b使目標函數(9)最小的問題.

3 粒子群優化算法在自由漂浮空間機器人軌跡規劃算法中的應用

3.1 粒子群算法的基本原理

在對式(9)進行參數尋優的過程中引入粒子群優化算法［10］.粒子群優化算法模擬鳥類的捕食行為，優化目標函數的每個解都是搜索空間的1個“粒子”，所有粒子都對應于1個由目標函數確定的適應度值.粒子在搜索空間中以一定的速度和方向飛行，通過對環境的學習與適應，根據個體與群體的飛行經驗的綜合分析結果來動態調整飛行速度和飛行方向［11］.

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

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

3.2 粒子群算法在力矩最優軌跡規劃中的應用

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

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

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

Step2 根據目標函數式(9)計算每個粒子的適應度J(Xi(k));

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

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

Step5 根據式(10)和(11)調整當前粒子的位置xi和速度vi;

Step6 當達到最大迭代次數Nmax時，終止迭代;否則返回Step2.

4 仿真實例

為了驗證本文所提出的自由漂浮空間機器人力矩最優軌跡規劃算法的有效性，在Matlab7.0環境下進行仿真，模型采用文獻［9］給出的空間機器人的物理參數，如表1所示.

表1 空間機器人系統參數

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

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

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

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

圖2 載體姿態角、關節角1、關節角2軌跡

圖3 關節角1角速度、關節角2角速度

圖4 關節角1加速度、關節角2加速度

圖5 系統運動過程

圖6 目標函數值

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

5 結論

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

［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］.機器人，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］吳啟迪，汪鐳.智能微粒群算法研究及其應用［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.

國家自然科學基金資助項目(60774062、61004072);高等學校博士學科點專項科研基金資助項目(20070213061、20102302110031);教育部留學回國人員科研啟動基金資助;黑龍江省留學回國人員科學基金資助項目(LC08C01);中央高校基本科研業務費專項資金資助項目(HIT.NSRIF.2009003);哈爾濱市科技創新人才研究專項基金資助項目(2010RFLXG001).

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

(編輯 張 宏)