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

王 誠，候 力

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

1 引言

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

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

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

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

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

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

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

在此機(jī)械臂基座和各轉(zhuǎn)動關(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)動范圍；αi為扭角，表示沿X i-1軸從Z i-1轉(zhuǎn)動到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í)行器在三個方向上的單位矢量；R e=[noa]—末端執(zhí)行器相對于基坐標(biāo)系的姿態(tài)矩陣；P e—末端執(zhí)行器坐標(biāo)系原點在基坐標(biāo)系的位置矢量。

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

機(jī)械臂逆運(yùn)動學(xué)問題求解思路為：根據(jù)給定的末端執(zhí)行器位姿[R e，P e]，解算出或搜索出一組關(guān)節(jié)角序列，將此關(guān)節(jié)角序列代入到式（2）中，得到此關(guān)節(jié)角序列對應(yīng)的末端執(zhí)行器位姿[R r，P r]，求解目標(biāo)為末端執(zhí)行器的位置誤差和姿態(tài)誤差矩陣和最小。姿態(tài)誤差矩陣中包含9個變量，且互相之間非完全獨(dú)立，若直接根據(jù)姿態(tài)誤差矩陣建立目標(biāo)函數(shù)必然會增加優(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)系原點間距離，k—系數(shù)，取（3.5～5.5）。

3 多子群粒子群算法求解

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

3.1 粒子群算法

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

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

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

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

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

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

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

對于同一搜索策略下的粒子，希望粒子的慣性權(quán)重隨著算法的迭代而不斷調(diào)整，從而動態(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)重最值，實現(xiàn)不同搜索策略，即大范圍搜索策略的慣性權(quán)重變化為：

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

當(dāng)本次搜索的粒子優(yōu)于上一代搜索粒子時，按照基本粒子群算法中速度更新方法依然要改變搜索方向，這種不加區(qū)分的粒子搜索方法顯然不合理，因此本文設(shè)置速度自適應(yīng)更新策略，基本思想為：當(dāng)本次搜索的粒子優(yōu)于上代粒子時，依然沿此方向繼續(xù)搜索；當(dāng)本次搜索粒子次于上代粒子時，則在歷史最優(yōu)和種群最優(yōu)牽引下運(yùn)動。速度自適應(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ì)搜索，但是難以從一個區(qū)域運(yùn)動至另一區(qū)域進(jìn)行大范圍搜索；同樣的，單獨(dú)使用大范圍搜索策略，粒子大范圍瀏覽區(qū)域，而沒有進(jìn)行細(xì)致搜索，難以搜索到最優(yōu)解。因此本文提出了由以上3類粒子子群組成的粒子群算法，稱為多子群粒子群算法。3個子群的粒子按照自身搜索策略各自進(jìn)行搜索，每完成一次迭代通過共享子群最優(yōu)位置實現(xiàn)信息交流，如此循環(huán)往復(fù)直至算法結(jié)束。

3.3 多子群粒子群算法流程

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

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

（2）將3個子群的粒子進(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)動學(xué)逆解仿真分析

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

4.1 收斂性能分析

仿真驗證在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á)到此要求時，對應(yīng)的關(guān)節(jié)角序列才滿足使用要求。

分別使用粒子群算法和多子群粒子群算法對機(jī)械臂逆運(yùn)動學(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)解時，目標(biāo)函數(shù)值隨迭代次數(shù)快速下降，收斂速度明顯快于粒子群算法；迭代至250次時，多子群粒子群算法搜索的目標(biāo)函數(shù)值在10-4以下，而粒子群算法搜索的目標(biāo)函數(shù)值數(shù)量級為1，說明了兩種算法的收斂精度具有極大差距。

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

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

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

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

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

5 結(jié)論

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