基于混沌粒子群優(yōu)化算法的空間機械臂軌跡規(guī)劃算法

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

（哈爾濱工業(yè)大學(xué) 航天學(xué)院，哈爾濱 150080）

基于混沌粒子群優(yōu)化算法的空間機械臂軌跡規(guī)劃算法

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

（哈爾濱工業(yè)大學(xué) 航天學(xué)院，哈爾濱 150080）

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

空間機械臂；軌跡規(guī)劃；混沌粒子群優(yōu)化算法；優(yōu)化

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

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

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

1 軌跡規(guī)劃問題的描述

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

1.1 優(yōu)化目標函數(shù)的定義

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

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

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

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

式(6)中kΨ為權(quán)系數(shù)，其根據(jù)軌跡規(guī)劃的精度要求而定，不同的精度要求只需調(diào)整kΨ，而目標函數(shù)J≤ 1即表示目標函數(shù)已經(jīng)符合精度要求。

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

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

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

式中，kδΨ0為姿態(tài)角約束的權(quán)系數(shù)，

1.2 關(guān)節(jié)參數(shù)方程的選取

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

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

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

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

2 基于混沌粒子群的規(guī)劃算法

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

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

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

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

混沌序列通常采用如下Logistic方程產(chǎn)生：

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

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

1）初始化算法參數(shù)，令k=0，設(shè)置慣性因子、學(xué)習(xí)因子等參數(shù)；

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

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

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

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

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

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

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

3 數(shù)值算例

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

表1 關(guān)節(jié)轉(zhuǎn)角范圍表Tab.1 The range of joint angular

表2 關(guān)節(jié)角速度和角加速度限制表Tab.2 The range of joint angular velocity and acceleration

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

通過仿真計算搜索得到最優(yōu)參數(shù)矩陣a：

目標函數(shù)最優(yōu)值為：J*=2.9848。

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

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

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

圖2 優(yōu)化后關(guān)節(jié)角運動曲線Fig.2 Trajectories of optimized joint angular

圖3 優(yōu)化后關(guān)節(jié)角速度曲線Fig.3 Trajectories of optimized joint angular velocity

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

圖4 優(yōu)化后關(guān)節(jié)角加速度曲線Fig.4 Trajectories of optimized joint Angular acceleration

圖5 梯形規(guī)劃關(guān)節(jié)角曲線Fig.5 Trajectories of trapezoidal planning joint Angular

圖6 基座姿態(tài)角變化曲線Fig.6 Trajectories of base attitude Angular

4 結(jié) 論

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

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]李巖，蔡遠文.基于遺傳算法的空間自由漂浮機械臂系統(tǒng)運動規(guī)劃[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]王明，黃攀峰，劉正雄，等.自由漂浮空間機器人最小基座反作用軌跡規(guī)劃[J].宇航學(xué)報，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

國家自然科學(xué)基金資助（61304108）；國防基礎(chǔ)預(yù)研項目支持（B2320110004）；上海航天科技創(chuàng)新基金資助項目

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

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