冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)的多子群粒子群求解

王 誠，候 力

（1.上海民航職業(yè)技術(shù)學(xué)院，上海200232；2.四川大學(xué)，四川 成都 610065）

1 引言

機(jī)械臂運(yùn)動(dòng)學(xué)逆解是機(jī)械臂關(guān)節(jié)空間軌跡規(guī)劃、機(jī)械臂軌跡跟蹤控制的基礎(chǔ)，運(yùn)動(dòng)學(xué)逆解的求解精度直接關(guān)系到機(jī)械臂的控制精度[1]，因此研究機(jī)械臂運(yùn)動(dòng)學(xué)逆解問題具有重要意義。

傳統(tǒng)的逆運(yùn)動(dòng)學(xué)求解方法包括封閉求解法和數(shù)值求解法，封閉求解法[2]計(jì)算速度較快，但是只適用于特定構(gòu)型的機(jī)械臂，通用性較差；代數(shù)數(shù)值求解法包括梯度投影法、最小范數(shù)法等，此類方法主要通過迭代計(jì)算求解[3]，但是計(jì)算過程中存在迭代積累誤差。隨著人工智能的興起，機(jī)械臂逆運(yùn)動(dòng)學(xué)的智能算法求解得到充分關(guān)注和廣泛應(yīng)用，核心思想是將逆運(yùn)動(dòng)學(xué)求解問題轉(zhuǎn)化為目標(biāo)優(yōu)化問題，包括粒子群算法、遺傳算法等。文獻(xiàn)[4]比較了封閉求解法和數(shù)值求解法，得出了數(shù)值求解法通用性好、封閉求解法求解快速的結(jié)論；文獻(xiàn)[5]使用3層RBF神經(jīng)網(wǎng)絡(luò)求解逆運(yùn)動(dòng)學(xué)方程，并提出多種群遺傳算法優(yōu)化神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)和參數(shù)，提高了逆解計(jì)算準(zhǔn)確率，但是此方法計(jì)算量較大，算法較為復(fù)雜；文獻(xiàn)[6]使用粒子群算法，以目標(biāo)點(diǎn)位姿誤差最小為優(yōu)化目標(biāo)求得了逆解最優(yōu)解，但是位姿權(quán)重固定，兩者之間的相對(duì)重要性無法自適應(yīng)變化。

針對(duì)冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題，提出了多子群粒子群算法的求解方法。構(gòu)造了由多個(gè)子群組成的粒子群算法，不同子群的粒子對(duì)應(yīng)不同的進(jìn)化方法，從而提高算法的搜索性能和穩(wěn)定性能，最終提高了機(jī)械臂運(yùn)動(dòng)學(xué)逆解的精度。

2 逆運(yùn)動(dòng)學(xué)問題描述與建模

2.1 逆運(yùn)動(dòng)學(xué)問題描述

以Comau NJ-220型機(jī)械臂為研究對(duì)象，如圖1所示。

圖1 Comau NJ-220型機(jī)械臂模型Fig.1 Comau NJ-220 Manipulator Model

在此機(jī)械臂基座和各轉(zhuǎn)動(dòng)關(guān)節(jié)處各建立坐標(biāo)系，基座坐標(biāo)系記為Ox0y0z0，關(guān)節(jié)（1～6）坐標(biāo)系分別記為Ox1y1z1～Ox6y6z6，圖中只給出了各坐標(biāo)系的X軸和Z軸，Y軸可根據(jù)右手定則進(jìn)行確定。圖1所示機(jī)械臂的參數(shù)，如表1所示。

表1 Comau NJ-220型機(jī)械臂參數(shù)Tab.1 Parameters of Comau NJ-220 Manipulator

表1中θi為關(guān)節(jié)i的關(guān)節(jié)角，表中第2列給出的數(shù)據(jù)為關(guān)節(jié)i的轉(zhuǎn)動(dòng)范圍；αi為扭角，表示沿X i-1軸從Z i-1轉(zhuǎn)動(dòng)到Z i的角度；a i為連桿長度，d i為偏置量。

使用D-H法［7］，經(jīng)過旋轉(zhuǎn)和平移，得到從坐標(biāo)系i-1到坐標(biāo)系i的齊次變換矩陣為：

式中：R—旋轉(zhuǎn)算子；D—平移算子。

由末端執(zhí)行器坐標(biāo)系到機(jī)械臂基坐標(biāo)系的變換矩陣為：

式中：n、o、a—末端執(zhí)行器在三個(gè)方向上的單位矢量；R e=[noa]—末端執(zhí)行器相對(duì)于基坐標(biāo)系的姿態(tài)矩陣；P e—末端執(zhí)行器坐標(biāo)系原點(diǎn)在基坐標(biāo)系的位置矢量。

2.2 逆運(yùn)動(dòng)學(xué)求解目標(biāo)函數(shù)

機(jī)械臂逆運(yùn)動(dòng)學(xué)問題求解思路為：根據(jù)給定的末端執(zhí)行器位姿[R e，P e]，解算出或搜索出一組關(guān)節(jié)角序列，將此關(guān)節(jié)角序列代入到式（2）中，得到此關(guān)節(jié)角序列對(duì)應(yīng)的末端執(zhí)行器位姿[R r，P r]，求解目標(biāo)為末端執(zhí)行器的位置誤差和姿態(tài)誤差矩陣和最小。姿態(tài)誤差矩陣中包含9個(gè)變量，且互相之間非完全獨(dú)立，若直接根據(jù)姿態(tài)誤差矩陣建立目標(biāo)函數(shù)必然會(huì)增加優(yōu)化目標(biāo)變量。為了減少優(yōu)化變量，將姿態(tài)誤差表示為四元素法［8］，即，式中表示旋轉(zhuǎn)變量，則姿態(tài)誤差為E0=α。位置誤差為，式中‖‖表示歐幾里得距離。

根據(jù)以上分析，構(gòu)造目標(biāo)函數(shù)為：

式中：w p、w o—位置誤差權(quán)值和姿態(tài)誤差權(quán)值。將位置誤差權(quán)值固定為1，姿態(tài)誤差權(quán)值隨機(jī)械臂結(jié)構(gòu)自適應(yīng)變化，即：

式中：λ—所有連桿長度和偏距的和；P d—目標(biāo)位置與基坐標(biāo)系原點(diǎn)間距離，k—系數(shù)，取（3.5～5.5）。

3 多子群粒子群算法求解

經(jīng)過第2節(jié)的分析，建立了以θ1～θ6為優(yōu)化對(duì)象，以式（3）為優(yōu)化目標(biāo)，以表1第二列關(guān)節(jié)角運(yùn)動(dòng)范圍為優(yōu)化空間的優(yōu)化問題，為了對(duì)此問題進(jìn)行求解，提出了多子群粒子群算法用于搜索逆運(yùn)動(dòng)學(xué)最優(yōu)解。

3.1 粒子群算法

在粒子群算法中，粒子速度在自身慣性、自身歷史最優(yōu)和種群最優(yōu)位置的牽引下進(jìn)行更新[9]，即：

式中：v id(t)、x id(t)—粒子i迭代t次時(shí)在d維上的速度和位置；p id—粒子i迭代至當(dāng)前時(shí)刻在d維上的最優(yōu)位置；p gd—種群迭代至當(dāng)前時(shí)刻在d維上的最優(yōu)位置；c1、c2—自身學(xué)習(xí)因子和種群學(xué)習(xí)因子；rand()—（0，1）間的隨機(jī)數(shù)。

3.2 多子群協(xié)同與多策略進(jìn)化

不同搜索策略或不同進(jìn)化策略的粒子具有不同的搜索特點(diǎn)，就可以應(yīng)對(duì)不同特點(diǎn)的搜索尋優(yōu)問題，由不同搜索策略粒子子群組成的種群相比于單一種群在面對(duì)不同尋優(yōu)問題時(shí)具有更好的搜索性能，因此本文提出了多子群粒子群算法。

3.2.1 多策略進(jìn)化粒子

根據(jù)慣性權(quán)重對(duì)粒子探索能力的影響，設(shè)置三種進(jìn)化策略，分別為大范圍搜索策略、精細(xì)搜索策略和速度自適應(yīng)更新策略。

在粒子尋優(yōu)過程中，慣性權(quán)重對(duì)粒子的探索能力和開發(fā)能力影響較大[10]，慣性權(quán)重較大時(shí)粒子的探索能力較強(qiáng)，能夠大范圍移動(dòng)，而探索之前未到達(dá)區(qū)域，利于全局搜索；慣性權(quán)重較小時(shí)粒子的開發(fā)能力較強(qiáng)，能夠在某一區(qū)域精細(xì)搜索，利于局部搜索和算法收斂。

對(duì)于同一搜索策略下的粒子，希望粒子的慣性權(quán)重隨著算法的迭代而不斷調(diào)整，從而動(dòng)態(tài)調(diào)整算法的探索能力和開發(fā)能力，使用線性遞減方法自適應(yīng)調(diào)整慣性權(quán)重，即

式中：w—自適應(yīng)權(quán)重；wmax、wmin—慣性權(quán)重最大和最小值；tmax—最大迭代次數(shù)。

通過設(shè)置不同的慣性權(quán)重最值，實(shí)現(xiàn)不同搜索策略，即大范圍搜索策略的慣性權(quán)重變化為：

細(xì)致搜索策略的慣性權(quán)重變化為：

當(dāng)本次搜索的粒子優(yōu)于上一代搜索粒子時(shí)，按照基本粒子群算法中速度更新方法依然要改變搜索方向，這種不加區(qū)分的粒子搜索方法顯然不合理，因此本文設(shè)置速度自適應(yīng)更新策略，基本思想為：當(dāng)本次搜索的粒子優(yōu)于上代粒子時(shí)，依然沿此方向繼續(xù)搜索；當(dāng)本次搜索粒子次于上代粒子時(shí)，則在歷史最優(yōu)和種群最優(yōu)牽引下運(yùn)動(dòng)。速度自適應(yīng)更新策略為：

式中：f(x)—適應(yīng)度函數(shù)。

3.2.2 多種群協(xié)同

式（5）給出了大范圍搜索策略，此類粒子具有強(qiáng)探索能力；式（6）給出了細(xì)致搜索策略，此類粒子具有強(qiáng)開發(fā)能力；式（7）給出了速度自適應(yīng)更新策略，此類粒子具有快速搜索能力。若單獨(dú)使用細(xì)致搜索策略，算法能夠在小范圍內(nèi)精細(xì)搜索，但是難以從一個(gè)區(qū)域運(yùn)動(dòng)至另一區(qū)域進(jìn)行大范圍搜索；同樣的，單獨(dú)使用大范圍搜索策略，粒子大范圍瀏覽區(qū)域，而沒有進(jìn)行細(xì)致搜索，難以搜索到最優(yōu)解。因此本文提出了由以上3類粒子子群組成的粒子群算法，稱為多子群粒子群算法。3個(gè)子群的粒子按照自身搜索策略各自進(jìn)行搜索，每完成一次迭代通過共享子群最優(yōu)位置實(shí)現(xiàn)信息交流，如此循環(huán)往復(fù)直至算法結(jié)束。

3.3 多子群粒子群算法流程

根據(jù)粒子群算法原理、多子群粒子群算法構(gòu)造方法，制定多子群粒子群算法流程為：

（1）初始化算法參數(shù)，包括子群規(guī)模、粒子維度、搜索區(qū)域等；

（2）將3個(gè)子群的粒子進(jìn)行隨機(jī)初始化；

（3）各子群按各自搜索方式搜索最優(yōu)位置，每完成一次迭代，共享子群最優(yōu)粒子位置；

（4）比較子群最優(yōu)粒子，挑選出種群最優(yōu)粒子，使用種群最優(yōu)粒子替換各子群最優(yōu)粒子；

（5）是否達(dá)到最大迭代次數(shù)，若是則結(jié)束，輸出種群最優(yōu)粒子；若否則轉(zhuǎn)至（3）。

4 運(yùn)動(dòng)學(xué)逆解仿真分析

同時(shí)使用粒子群算法和多子群粒子群算法對(duì)機(jī)械臂逆運(yùn)動(dòng)學(xué)問題進(jìn)行求解，對(duì)比兩種算法的收斂性能和穩(wěn)定性能，其在收斂性能包括收斂速度和收斂精度兩個(gè)方面。

4.1 收斂性能分析

仿真驗(yàn)證在Matlab環(huán)境下進(jìn)行，機(jī)械臂構(gòu)型及參數(shù)、搜索空間、搜索目標(biāo)在前文中已經(jīng)給出，在此明確粒子編碼方式：需要優(yōu)化的參數(shù)為θ1～θ6，因此粒子維度設(shè)置為6，采用十進(jìn)制編碼方式。

多子群粒子群算法參數(shù)設(shè)置為：子群規(guī)模設(shè)置為40，即種群規(guī)模為120，算法最大迭代次數(shù)tmax=50，學(xué)習(xí)因子c1=1.8、c2=2.2。給定精度要求為：位置誤差不大于0.1mm，姿態(tài)誤差不大于10-4rad。當(dāng)搜索精度達(dá)到此要求時(shí)，對(duì)應(yīng)的關(guān)節(jié)角序列才滿足使用要求。

分別使用粒子群算法和多子群粒子群算法對(duì)機(jī)械臂逆運(yùn)動(dòng)學(xué)進(jìn)行求解，每種算法各獨(dú)立運(yùn)行30次，選擇兩種算法各自的最優(yōu)解，目標(biāo)函數(shù)、位置誤差及姿態(tài)誤差隨迭代次數(shù)的變化過程，如圖2所示。

圖2 優(yōu)化結(jié)果迭代曲線Fig.2 Iteration Curve of Optimizing Result

由圖2（a）可以看出，使用多子群粒子群算法搜索最優(yōu)解時(shí)，目標(biāo)函數(shù)值隨迭代次數(shù)快速下降，收斂速度明顯快于粒子群算法；迭代至250次時(shí)，多子群粒子群算法搜索的目標(biāo)函數(shù)值在10-4以下，而粒子群算法搜索的目標(biāo)函數(shù)值數(shù)量級(jí)為1，說明了兩種算法的收斂精度具有極大差距。

由圖2（b）和圖2（c）可以看出，使用多子群粒子群算法搜索的最優(yōu)粒子對(duì)應(yīng)的位置誤差在10-4mm以下，姿態(tài)誤差在10-8rad以下，能夠滿足事先設(shè)定的精度要求。而使用粒子群算法搜索的最優(yōu)粒子對(duì)應(yīng)的位置誤差為0.32mm，姿態(tài)誤差為2.2×10-3rad，沒有達(dá)到實(shí)現(xiàn)設(shè)定的精度要求。綜上所述，多子群粒子群算法的搜索精度和速度遠(yuǎn)遠(yuǎn)優(yōu)于粒子群算法，這是因?yàn)樵诙嘧尤毫Ｗ尤核惴ㄖ?，使用多個(gè)子群組成種群，而不同子群的粒子搜索策略不同，因此可以應(yīng)對(duì)不同特點(diǎn)的尋優(yōu)問題，達(dá)到了提高算法收斂精度和收斂速度的效果。多子群粒子群算法最終搜索到的最優(yōu)粒子為（-0.092，0.306，-0.743，-2.17，-0.001，-0.002）rad。

4.2 穩(wěn)定性能分析

前文是針對(duì)工作空間中某一隨機(jī)選取的點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)求解，為了分析多子群粒子群算法的穩(wěn)定性和魯棒性，從工作空間中隨機(jī)選取25個(gè)點(diǎn)，分別使用多子群粒子群算法和粒子群算法進(jìn)行求解，兩種算法設(shè)置相同的迭代次數(shù)，25個(gè)點(diǎn)的求解結(jié)果，如圖3所示。

圖3 算法穩(wěn)定性分析Fig.3 Stability Analysis of Algorithms

從圖中可以看出，兩類算法的穩(wěn)定性均較好，但是粒子群算法的收斂精度不高，而多種群粒子群算法的收斂精度極高。另外，多子群粒子群算法的穩(wěn)定性略好于粒子群算法，這是因?yàn)槎嘧尤毫Ｗ尤核惴ǖ姆N群由多類型粒子組成，可以應(yīng)對(duì)不同特點(diǎn)的尋優(yōu)問題，所以具有更好的穩(wěn)定性和更強(qiáng)的魯棒性。

5 結(jié)論

研究了機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題，提出了多子群粒子群算法的求解方法，經(jīng)仿真驗(yàn)證，得出了以下結(jié)論：（1）多子群粒子群算法求得的最優(yōu)粒子對(duì)應(yīng)的姿態(tài)誤差和位置誤差能夠滿足設(shè)定的精度要求，可以應(yīng)用于實(shí)際；（2）多子群粒子群算法的收斂速度和收斂精度均優(yōu)于粒子群算法；（3）多子群粒子群算法的穩(wěn)定性好于粒子群算法，魯棒性也強(qiáng)于粒子群算法。