基于混沌粒子群優化算法的空間機械臂軌跡規劃算法

夏紅偉，翟彥斌，馬廣程，鄧 雅，王常虹

（哈爾濱工業大學 航天學院，哈爾濱 150080）

基于混沌粒子群優化算法的空間機械臂軌跡規劃算法

夏紅偉，翟彥斌，馬廣程，鄧 雅，王常虹

（哈爾濱工業大學 航天學院，哈爾濱 150080）

針對自由漂浮狀態下的空間機械臂系統，研究了基座姿態擾動最小的軌跡規劃問題。首先通過正弦函數參數化機械臂各個關節，在機械臂關節角速度、角加速度以及基座姿態變化范圍受限的約束條件下，定義了基座姿態擾動最小的目標函數，然后提出了基于混沌粒子群算法的軌跡優化策略，并給出了具體求解步驟。數值算例結果表明，在滿足系統的約束條件下，機械臂關節變化平緩，不存在角速度突變的情況，并且比標準粒子群算法具有更快的收斂速度，在優化軌跡下進行運動仿真，結果表明終止時刻基座姿態擾動為1.3708°(三軸合成)，而梯形規劃的姿態擾動為8.5459°，優化后使得姿態的擾動減小84%，從而說明所提出的算法能夠有效減小機械臂運動對基座姿態的擾動。

空間機械臂；軌跡規劃；混沌粒子群優化算法；優化

空間機械臂的基座不是固定的，機械臂與其基座之間存在動力學耦合，導致機械臂的運動會對基座的位置和姿態產生擾動，為了保證航天器的正常工作，需要保證基座的姿態保持不變或者將其變化限制在一定范圍內[1-4]。對此，很多學者研究了空間機械臂的軌跡規劃算法，以減小機械臂對基座姿態的擾動。

Yoshida等[4]基于廣義Jacobi矩陣逆的方法實現對基座姿態無擾的機械臂軌跡規劃，但廣義Jacobi矩陣在某些情況下會存在奇異的問題，導致規劃算法的可行空間受限。李巖等人[5]提出了基于遺傳算法規劃關節運動實現基座的姿態重穩，但遺傳算法實現起來較為復雜，算法收斂時間過長。王明等人[6]提出基于粒子群算法實現自由飛行狀態下的基座干擾力矩最小的軌跡規劃問題，但是沒有涉及自由漂浮狀態下基座姿態擾動最小的問題。

對此，本文針對空間機械臂基座姿態擾動最小的軌跡規劃問題進行研究。首先定義了自由漂浮狀態下基座姿態擾動最小的目標函數，加入關節角速度、角加速度以及基座姿態變化限制的約束，采用正弦函數參數化機械臂關節角，提出了基于混沌粒子群算法的軌跡優化方法，并通過數值仿真驗證算法的有效性。

1 軌跡規劃問題的描述

本文所研究的自由漂浮狀態下空間機械臂的動力學方程為[5]：

1.1 優化目標函數的定義

為了實現機械臂基座姿態擾動最小，其關節軌跡規劃目標為：通過規劃機械臂各個關節角的運動使其滿足下式(2)～(4)的約束，并且在運動結束后，基座姿態變化最小。

式中，θi0、θid分別為第i個關節角的初始角度與期望角度值，以及分別為機械臂關節角、角速度和角加速度的范圍。

首先研究關節角速度和角加速度不受限制的情況，此時關節軌跡規劃的目標可以表示為：

根據式(5)，可以得到基座姿態擾動最小的機械臂軌跡規劃問題的目標函數如式(6)所示：

式(6)中kΨ為權系數，其根據軌跡規劃的精度要求而定，不同的精度要求只需調整kΨ，而目標函數J≤ 1即表示目標函數已經符合精度要求。

考慮到實際空間機械臂系統執行器件的能力，為了保證機械臂運行的平穩性與可靠性，各個關節角的角速度與角加速度必須限定在一定的范圍內[6]，因此，需要進一步研究關節角速度、角加速度受限的情況下目標函數的描述。在此，引入關節角速度、角加速度代價函數，如式(7)、(8)所示：

結合式(6)，關節角速度、角加速度受限情況下的目標函數如式(9)所示：

在實際的系統中，為了基座(或衛星本體)能正常工作，要求基座的姿態變化限定在一定的范圍之內，而式(9)的目標函數只體現了終止時刻基座的姿態約束，無法約束機械臂運動過程中基座姿態的變化范圍，因此，修正式(9)所示的優化目標函數為：

式中，kδΨ0為姿態角約束的權系數，

1.2 關節參數方程的選取

目前機械臂軌跡規劃中，常采用多項式函數對機械臂的關節函數進行參數化，但是這種函數僅能滿足式(2)(3)所示的等式約束，對不等式(4)的約束則難以表示。而正弦函數能夠限制函數的取值范圍，并且具有很好的平滑性，因此在本文中采用正弦函數對關節函數進行參數化。各個關節角度如式(12)所示：

將約束方程(2)～(4)代入到式(12)中可得：

由式(13)～(17)可以看出，關節函數經過參數化后，每個關節函數僅含有兩個未知參數ai6和ai7。因此，對于具有n個關節的空間機械臂而言，如果未知參數矩陣確定時，機械臂的關節軌跡即可唯一確定，其中矩陣a為：

因此本文所研究的空間機械臂軌跡規劃問題可以描述為：通過求解未知參數矩陣a，使得式(11)所示的目標函數達到最小。

2 基于混沌粒子群的規劃算法

粒子群算法采用實數編碼，不需要選擇、交叉以及變異等操作，算法結構簡單、計算速度快[7]。但是對于標準粒子群算法來說，在計算過程中，如果某一個粒子發現一個當前最優位置，其他粒子則會快速向此粒子逼近，如果這個最優位置只是一個局部最優位置，尤其是在慣性因子較小的時候，粒子群則會一直在局部最優位置附近搜索，而無法在整個解空間內進行全局最優位置搜索，出現所謂的早熟情況。

對此，本文引入混沌粒子群法（CPSO），混沌粒子群法借助混沌運動的隨機性和遍歷性的優點，保證了粒子群體的多樣性，提高了粒子群算法擺脫局部極值點的能力，并且保持了粒子群優化算法收斂速度快的優點[8]。其基本思想可以從以下兩方面說明：

① 為了保證粒子群算法初始群體具有的隨機性特征，通過混沌序列初始化群體粒子的位置和速度，與此同時還提高了個體搜索的遍歷性和群體的多樣性，最后從大量粒子中選擇出較優部分作為算法的初始群體。

② 在每步迭代過程中，把當前群體最優粒子為初始位置，產生新的混沌群體，把這個新群體的最優粒子代替原群體的一個粒子，這樣，在局部最優位置周圍通過混沌序列進行搜索，可以有效避免群體陷入局部最優位置，并快速找到全局的最優位置。

混沌序列通常采用如下Logistic方程產生：

式中，μ為控制參量。當取μ=4，并且0＜z0＜1時，式(19)生成的序列完全處于混沌狀態。因此，由任意初始值z0∈(0,1)，可以迭代出一個混沌序列z1,z2,z3,… 。

基于上面的分析，本文提出的軌跡規劃方法的具體步驟如下所述：

1）初始化算法參數，令k=0，設置慣性因子、學習因子等參數；

2）利用混沌序列產生N個初始群體及其速度值。首先產生12個0至1之間的隨機數，利用每個隨機數通過Logistic方程產生N維向量,，利用12個混沌序列產生N個初始粒子位置；然后計算產生的混沌粒子群體的適應值，并從N個初始群體中選擇位置較優的M個粒子作為算法的初始群體，并通過混沌序列產生M個初始速度。

3）更新每個粒子的歷史最優位置Pi以及全局最優位置Pg。

4）計算M個粒子的速度，根據粒子速度更新M個粒子的位置；

5）以全局最優位置為初始條件產生新混沌序列，用新混沌群體的最優位置替換當前群體的粒子。

式中，ai、bi分別為第i維粒子位置取值的下、上限。

以zi0為初始狀態，利用Logistic方程產生新的混沌序列，并通過式(21)把混沌序列映射到解空間，得到m個新的粒子位置：

6) 若滿足停止迭代條件，則終止計算，否則返回步驟3)。

3 數值算例

為了驗證本文所提出的方法的有效性，本節針對具有6個自由度的單臂機械臂進行數值仿真。假設空間機械臂關節初始和期望角度分別為：

表1 關節轉角范圍表Tab.1 The range of joint angular

表2 關節角速度和角加速度限制表Tab.2 The range of joint angular velocity and acceleration

初始混沌粒子群數量N=50，求解過程中種群數量M=20，學習因子c1=c2=2.0，粒子速度限制為，每次迭代內混沌序列數量m=10，最大迭代次數kmax=50。

通過仿真計算搜索得到最優參數矩陣a：

目標函數最優值為：J*=2.9848。

圖1中給出在相同條件下標準粒子群(PSO)算法與混沌粒子群(CPSO)算法對應的目標函數最優值隨迭代次數的變化曲線?？梢钥闯?，CPSO一直優于PSO算法，并且迭代50步后，CPSO的最優值為2.9848，而PSO算法的最優值為9.4982，說明CPSO算法能有效的避免標準粒子群算法的早熟現象，并且具有更快的收斂速度。

將CPSO計算得到的最優參數矩陣代入式(12)中，可以得到規劃的機械臂關節運動，規劃得到的關節角變化曲線如圖2～圖4所示。圖2為關節角運動，圖3和圖4為規劃的關節角速度和角加速度曲線，可以看出，關節變化平緩，不存在角速度突變的情況，而且關節角速度和角加速度均在表2所示的約束范圍內。

圖1 PSO與CPSO算法最優值變化曲線Fig.1 The optimal value of PSO and CPSO vs.steps

圖2 優化后關節角運動曲線Fig.2 Trajectories of optimized joint angular

圖3 優化后關節角速度曲線Fig.3 Trajectories of optimized joint angular velocity

為了驗證算法優化的效果，在相同條件下，關節軌跡采用梯形軌跡規劃，規劃的關節角運動如圖5所示，兩種軌跡下機械臂基座姿態角變化曲線仿真結果如圖6所示。通過圖中觀察曲線可知，當關節角沿著優化得到的軌跡進行運動過程中，基座姿態角變化均在15°范圍內，在終止時刻姿態角均趨向于初始狀態。并且終止時刻，基座姿態擾動為1.3708°(三軸合成)，而梯形規劃的姿態擾動為8.5459°，優化后使得姿態的擾動減小84%，這說明了機械臂軌跡優化的有效性。

圖4 優化后關節角加速度曲線Fig.4 Trajectories of optimized joint Angular acceleration

圖5 梯形規劃關節角曲線Fig.5 Trajectories of trapezoidal planning joint Angular

圖6 基座姿態角變化曲線Fig.6 Trajectories of base attitude Angular

4 結 論

研究了自由漂浮狀態下的空間機械臂基座姿態擾動最小的軌跡規劃問題，通過正弦函數參數化機械臂的各個關節角，提出了基于混沌粒子群算法求解關節最優軌跡的方法，并通過數值仿真驗證了本文規劃得到的軌跡在滿足關節角速度、角加速度以及基座姿態變化限制等約束條件下，機械臂關節變化平緩，不存在角速度突變的情況。與梯形規劃相比，使用所提出的優化方法可以使姿態的擾動減小84%，從而說明了所提出的機械臂軌跡優化方法的有效性。

References)

[1]Rutkovskii V Y,Sukhanov V M,Glumov V M.Motion equations and control of the free-flying space manipulator in the reconfiguration mode[J].Automation and Remote Control,2010,71(1): 70-86.

[2]Nanos K,Papadopoulos E.On the use of free-floating space robots in the presence of angular momentum[J].Intelligent Service Robotics,2011,4(1): 3-15.

[3]Wang H,Xie Y.Adaptive Jacobian position/force tracking control of free-flying manipulators[J].Robotics and Autonomous Systems,2009,57(2): 173-181.

[4]Yoshida K,Hashizume K,Abiko S.Zero reaction maneuver: flight validation with ETS-VII space robot and extension to kinematically redundant arm[C]//IEEE International Conference on Robotics and Automation,2001,1:441-446.

[5]李巖，蔡遠文.基于遺傳算法的空間自由漂浮機械臂系統運動規劃[J].航天控制，2012(01)：40-48.LI Yan,CAI Yuan-wen.The motion planning for free flying space manipulator based on genetic algorithm[J].Aerospace Control,2012(01): 40-48.

[6]王明，黃攀峰，劉正雄，等.自由漂浮空間機器人最小基座反作用軌跡規劃[J].宇航學報，2011，10：2152-2157.WANG Ming,HUANG Pan-feng,LIU Zheng-xiong,et al.Trajectory planning for minimizing base reaction of free-floating space robot[J].Journal of Astronautics,2011(10): 2152-2157.

[7]Goh C K,Tan K C,Liu D S,et al.A competitive and cooperative co-evolutionary approach to multi-objective particle swarm optimization algorithm design[J].European Journal of Operational Research,2010,202(1): 42-54.

[8]Hong W C.Chaotic particle swarm optimization algorithm in a support vector regression electric load forecasting model[J].Energy Conversion and Management,2009,50(1): 105-117.

Path planning algorithm of space manipulator based on chaos particle swarm optimization algorithm

XIA Hong-wei,ZHAI Yan-bin,MA Guang-cheng,DENG Ya,WANG Chang-hong
(School of Astronautics,Harbin Institute of Technology,Harbin 150080,China)

A path planning algorithm of free-floating space manipulator is studied in order to obtain minimum disturbance of base attitude caused by manipulator’s movement.The manipulator joints are parameterized by sine function,and the object of base attitude disturbance minimum is given within the limit of joints angular velocity,angular acceleration and base attitude range.After introducing the chaos particle swarm optimization(CPSO) algorithm as the problem solving strategy,the problem’s solving steps are given.Numerical example results show that under the condition of system constraint,the mechanical arm joints change smoothly without angular velocity mutation,and have faster convergence speed than the standard particle swarm optimization(PSO) algorithm.The simulation results obtained under the optimal trajectory show that the base attitude disturbance is 1.3708°(triaxial synthesis) at termination time,which is reduced by more than 84% than the trapezoidal planning attitude disturbance(8.5459°),and this reveals that the proposed algorithm can effectively reduce the base attitude disturbance caused by manipulator’s movement.

space manipulator; path planning; chaos particle swarm optimization algorithm; optimization

TP24

：A

1005-6734(2014)02-0211-06

10.13695/j.cnki.12-1222/o3.2014.02.013

2013-09-20；

：2014-1-14

國家自然科學基金資助（61304108）；國防基礎預研項目支持（B2320110004）；上海航天科技創新基金資助項目

夏紅偉（1979—），男，博士，主要從事空間飛行器的控制及仿真技術方面的研究。E-mail：hw_xia@163.com

聯 系 人：王常虹（1961—），男，教授，主要研究方向為導航、制導與控制。Email: cwang@hit.edu.cn.