楊 康，熊瑞平，潘楚光，盧文翔，徐毅松

(四川大學(xué)機械工程學(xué)院，成都 610065)

0 引言

機器手逆運動學(xué)是指通過給定的機器手末端位置位姿求得機器手的各個旋轉(zhuǎn)軸角度。運動學(xué)逆解求解是機器人學(xué)的核心，對于機器手避障、克服奇點、提高機械臂靈活性等有極大用處[1]。

機器手逆解求解一般分為解析法與數(shù)值迭代法。解析法在求解多軸機器人時較為復(fù)雜，一般在特定結(jié)構(gòu)機器人逆解求解時使用。數(shù)值迭代法是將機器手逆運動學(xué)問題轉(zhuǎn)化成數(shù)學(xué)迭代優(yōu)化的過程，進行一步步接近最終結(jié)果從而達到最終姿態(tài)。DULEBA等[2]利用雅各比偽逆矩陣進行了機器手運動學(xué)逆解的求解。CHIRIKJIAN等[3]利用離散性微分方程來進行求解機器人雅各比偽逆矩陣，簡化了運算步驟。何理等[4]利用SVD分解求廣義逆來避免機器手運動過程中的奇異性問題，同時簡化了雅各比矩陣。楊揚[5]建立了基于運動軌跡的約束泛函，優(yōu)化了冗余機器人的逆解。

近年來智能算法也越來越應(yīng)用在機器手求逆解的過程中，進一步提高了機器手的工作精度。石建平[6]通過粒子群優(yōu)化算法進行了平面冗余機器人的運動學(xué)逆解求解，獲得了更快的收斂速度與更好的收斂質(zhì)量。劉洋[7]通過多目標(biāo)粒子群算法實現(xiàn)了機器人位置精度與姿態(tài)精度的控制，極大的滿足了實際應(yīng)用的需求。李國江等[8]利用多種群協(xié)同進化算法進行了繩索牽引并聯(lián)機器人末端位置補償，提高了末端位置的精度。吉陽珍等[9]利用組合優(yōu)化算法用于逆運動學(xué)求解，提高了精度以及穩(wěn)定性。鄭雪芳等[10]基于改進螢火蟲算法進行了冗余機器人的軌跡規(guī)劃。

由于數(shù)值解求解過程中存在累積誤差，且會陷入局部最優(yōu)的狀態(tài)，智能優(yōu)化算法又存在搜索范圍廣，搜索速度慢的問題，所以本文采取先用數(shù)值迭代法求出最接近期望位姿的機器手初步最優(yōu)角度解，再利用粒子群優(yōu)化算法在初步最優(yōu)角度解處偏置±10°進一步搜索優(yōu)化，提高機器手逆解精度與準(zhǔn)確度。

1 機器手迭代法

1.1 機器手逆運動學(xué)模型

本文以自由度為6的七軸機器人為主要研究對象，根據(jù)DH方法建立機器手模型，機器手的DH參數(shù)如表1所示。

表1 七軸機器人DH參數(shù)

長度ai(i=1,2,…,7)是指Xi-1軸從Zi-1軸移動到Zi軸的距離，扭角αi是沿Xi-1軸從Zi-1軸轉(zhuǎn)動到Zi-1軸的角度，偏置di是沿Zi軸從Xi-1軸移動到Xi的距離，關(guān)節(jié)角θi是沿Zi軸從Xi-1軸轉(zhuǎn)動到Xi軸的角度[1]。

由機器手運動學(xué)知識得到，根據(jù)DH參數(shù)[θαda]可以得出從坐標(biāo)系Xi-1Yi-1Zi-1到坐標(biāo)系XiYiZi的變換關(guān)系為：

(1)

將七軸機器人的7個旋轉(zhuǎn)矩陣從左到右依次相乘，能得到機器手末端坐標(biāo)系相對于基坐標(biāo)系的位姿矩陣：

(2)

式(2)表示機器手的末端位姿方程，矩陣R3×3、P分別表示機器手末端的位姿矩陣及位置矩陣，位姿矩陣表示機器手末端分別沿著X、Y、Z方向旋轉(zhuǎn)角度的3個自由度，位置矩陣代表著機器手的位置X、Y、Z位置坐標(biāo)。

1.2 機器手逆運動學(xué)分析函數(shù)

數(shù)值法求解思想是將機器手位姿求解過程轉(zhuǎn)化為牛頓迭代過程，每一步計算變化角度，一步步逼近最終結(jié)果。

機器手末端的位姿X由機器手位置x、y、z及末端繞x、y、z分別旋轉(zhuǎn)的角度θ、α、γ來確定。

(3)

X=f(q)

(4)

(5)

(6)

∴

f(q)=f(q0)+J×(q-q0)+δ(q0)

(7)

f(q)-f(q0)=J×Δq+δ(q0)

(8)

(9)

當(dāng)

(10)

此時，當(dāng)機器手的變化角度為Δq時，此時誤差最小，并且偽逆矩陣可以消除機器手在運動過程中產(chǎn)生的奇異性影響。

本文數(shù)值法利用偽逆矩陣，通過機器手期待的末端位姿來計算機器手每次的迭代角度變化。

步驟1：設(shè)置機器手的初始位姿矩陣TF或者初始角度q，以及期望的終端位姿TL，初始化迭代次數(shù)i=0，最大迭代次數(shù)il=300，設(shè)定迭代收斂條件位置及位姿誤差范數(shù)G，以及范數(shù)影響因子λ；

步驟2：計算出下一步機器手的變化角度Δq=JT(JJT)-1e；

步驟3：更新機器手各個旋轉(zhuǎn)軸角度qc=qc+Δq；

步驟4：更新(qc)，得到機器手角度變化后末端位置坐標(biāo)及繞X、Y、Z軸旋轉(zhuǎn)角度；

步驟5：計算機器手此時的雅各比矩陣J(qc)；

步驟6：期望末端位姿t=[xyzθαβ]，T中各元素分別表示位置x、y、z及繞三坐標(biāo)軸旋轉(zhuǎn)的角度θ、α、γ，計算末端位姿與此時的位姿之間的誤差e=t-f(qc)；

步驟7：迭代次數(shù)i=i+1;

2 粒子群算法

2.1 基本原理

粒子群算法是模仿鳥群覓食的模型進行修正優(yōu)化得到的算法，通過一個由m個粒子組成的群體在D維搜索空間中以一定的速度飛行，每個粒子在搜索時，考慮到了自己搜索到的歷史最好點和群體內(nèi)其它粒子的歷史最好點，在此基礎(chǔ)上進行位置與速度的變化直到到達歷史最好點[11-12]。

2.2 算法流程

粒子群算法位置與速度的關(guān)系：

(11)

基本粒子算法步驟：

步驟1：隨機初始化粒子群，包括隨機位置和速度；

步驟2：計算每個粒子的適應(yīng)值[13]；

步驟3：對于每個粒子，進行比較每次迭代后的適應(yīng)值并且提取粒子更優(yōu)的適應(yīng)值，提取歷史最優(yōu)值；

步驟4：判斷其是否滿足迭代停止規(guī)則，若滿足，跳出算法循環(huán)輸出結(jié)果。否則進行位置與速度更新，轉(zhuǎn)到第2步[10]。

2.3 適應(yīng)度函數(shù)

機器手的運動特性包括位置坐標(biāo)X、Y、Z與位姿即繞X、Y、Z三軸旋轉(zhuǎn)的角度θ、α、γ，本文算法的適應(yīng)度函數(shù)是機器手位姿誤差范數(shù)大小比較，尋求機器手誤差最小值。

通過機器手期望的位姿旋轉(zhuǎn)矩陣計算得到機器手末端位姿

t=[XL、YL、ZL、θL、αL、γL]

(12)

粒子群算法迭代過程中通過在初始范圍內(nèi)搜索變化的角度，通過計算產(chǎn)生的機器手末端位姿

f(qc)=[XP、YP、ZP、θP、αP、γP]

(13)

評價函數(shù)：

(14)

式中，G表示機器手期望中的位姿誤差與實際中的位姿誤差范數(shù)。

3 數(shù)值迭代法與粒子群算法混合算法

冗余機器手由于機器手旋轉(zhuǎn)軸數(shù)目更多，逆運動學(xué)解數(shù)目是無窮的，所以本質(zhì)上是一個多元多峰函數(shù)最優(yōu)解的求取問題，其適應(yīng)度函數(shù)是一個帶約束條件的復(fù)雜的方程組，計算量較大，若是直接利用粒子群算法在全局范圍內(nèi)進行角度搜索，搜索角度范圍太大，計算量繁重，收斂速度更慢精度不高，消耗算力也更多，且容易陷入局部最優(yōu)的范圍解內(nèi)。

數(shù)值法迭代優(yōu)勢在于迭代速度很快，程序簡單，目標(biāo)明確能夠短時間內(nèi)達到一個接近終點的目標(biāo)，但是由于需要利用到雅各比矩陣，在計算每次變化角度時省略了后面很多較小量級的角度，所以機器手每次迭代都存在較大的迭代累積誤差，且容易陷入局部循環(huán)的境地。

本文將數(shù)值法與粒子群算法相結(jié)合起來，先用數(shù)值法迭代進行初步計算，得到七軸機器人的初步旋轉(zhuǎn)角度，然后利用粒子群算法進行進一步搜索，搜索范圍在初步解上，每個軸偏轉(zhuǎn)10°，優(yōu)化精確機器手的位姿，此時消除了數(shù)值解過程中式(8)中δ(q0)產(chǎn)生的累積誤差，又簡化了粒子群算法的搜索范圍，降低了搜索成本，加快了搜索迭代收斂速度，因此很大程度上能夠搜索到較為精確的機器手逆解。

混合算法求機器手逆解的過程大概如下：

步驟1：輸入機器手DH參數(shù)，運用MATLAB工具箱創(chuàng)建七軸機器手；

步驟2：輸入機器手期望的終點末端位姿旋轉(zhuǎn)矩陣TL以及起點位姿或者起始角度等；

步驟4：初始化粒子群初始角度范圍θmax=[θ1+10°,θ2+10°,θ3+10°,θ4+10°,θ5+10°,θ6+10°,θ7+10°],θmin=[θ1-10°,θ2-10°,θ3-10°,θ4-10°,θ5-10°,θ6-10°,θ7-10°]在這個角度范圍內(nèi)隨機初始化角度粒子及相關(guān)粒子群參數(shù)；

步驟5：將粒子帶入粒子群算法求解機器人運動學(xué)逆解；

步驟6：輸出機器手逆解結(jié)果θ，逆運動學(xué)求解結(jié)束。

進行數(shù)值迭代法初步迭代后，得到初步的機器手角度以及初步的位姿范數(shù)誤差，再在此基礎(chǔ)上初始化粒子群角度范圍進行粒子群算法迭代得到進一步的精確解并得到此時的位姿范數(shù)誤差，比較初步迭代后與粒子群算法優(yōu)化后的結(jié)果。

4 實驗結(jié)果

經(jīng)過指定機器手的初始點位姿與期望的終點位姿，根據(jù)數(shù)值迭代與粒子群混合算法步驟進行仿真求解，如表2所示實驗初步設(shè)置的初始位置位姿與期望的初始位置位姿；如表3所示機器手進行初步迭代后的角度初步最優(yōu)解與初步誤差，誤差范數(shù)等；如表4所示混合優(yōu)化算法運行完畢的角度解以及此時的位置位姿誤差與誤差范數(shù)。

表2 初始位姿與期望位姿設(shè)置

表3 數(shù)值解初步求解后結(jié)果

表4 混合優(yōu)化后實驗結(jié)果

從表格中相關(guān)的實驗數(shù)據(jù)可以看出，經(jīng)過數(shù)值迭代法初步計算后的初步結(jié)果，初步迭代后的位姿誤差較大，且結(jié)果誤差始終無法進一步減小，在此角度基礎(chǔ)上偏置±10°，縮小搜索角度范圍，進行PSO算法搜索優(yōu)化后，誤差明顯大大減小。

小結(jié)：在整個混合優(yōu)化算法求解機器手逆解過程中，機器手逆解進行了二次精度優(yōu)化，在初步數(shù)值迭代法計算步驟中，機器手容易產(chǎn)生累積誤差，當(dāng)累積誤差到達一定程度時，位姿誤差很難降低到一個理想數(shù)值，在初步最優(yōu)角度解的基礎(chǔ)上偏置±10°利用PSO算法進行搜索，搜索結(jié)果顯示位姿誤差大大降低，降低了數(shù)值迭代法運行過程中存在的累積誤差狀況，同時搜索范圍的降低使得迭代收斂速度提高，逆解精度大大提高。

連續(xù)軌跡測試：

根據(jù)設(shè)定的七軸機器人初始角度θα=[0,pi/6,-pi/18,pi/3,pi/5,0,-pi/6]及混合優(yōu)化算法優(yōu)化過后的終點角度θβ=[0.31,0,1.04,-1.57,0.17,0.26,0.21]，在這兩個位姿間利用機器人工具箱插值10次，隨后利用樣條曲線得到機器人運動過程中的角度值結(jié)果如圖1所示，以及運行過程中機器手的位姿變化如圖2所示，位姿誤差范數(shù)如圖3所示。

圖1 機器手軌跡角度變化 圖2 機器手軌跡位姿誤差變化

圖3 機器手軌跡位姿誤差范數(shù)

分析實驗數(shù)據(jù)，七軸機器手在進行數(shù)值迭代法求逆解時，會產(chǎn)生累積誤差導(dǎo)致最終結(jié)果不能降低到一個理想的位置，在初步迭代結(jié)果上，選取位姿誤差最小時的角度解，在此角度基礎(chǔ)上偏置±10°進行二次優(yōu)化搜索，減小了粒子群的搜索角度范圍，加快了搜索速度，加快了迭代收斂速度，極大地提高了計算精度，同時在后續(xù)的軌跡規(guī)劃中，利用插值的思想，機器手的角度變化波動較為平滑，同時位姿誤差以及位姿誤差范數(shù)收斂性很強，機器手的運動更加平穩(wěn)，無較大波動，沖擊較小，因此混合優(yōu)化算法具有極強的利用研究價值。

5 結(jié)論

本文在分析了數(shù)值迭代法與粒子群算法在冗余機器人求逆解過程中的弊端后，提出了一種混合優(yōu)化算法，先利用數(shù)值迭代法接近末端執(zhí)行器期望的末端位姿，縮小搜索范圍，在迭代過程中提取出位姿誤差最小時的角度值，然后在此角度范圍偏置±10°，減小搜索范圍，進行粒子群智能優(yōu)化算法的搜索，進一步逼近結(jié)果，得到機器手優(yōu)化后的角度后，在后續(xù)的軌跡測試中，利用最終優(yōu)化結(jié)果與初始角度進行插值計算，機器手的角度變化更加柔和，波動更小，同時位姿誤差與位姿誤差范數(shù)的波動收斂性更強，機器手的運動更加平穩(wěn)，沖擊更小。實驗論證大大提高了機器手末端的位姿精度，提高了機器手在運動過程中的平穩(wěn)性，在實際的研究過程中具有極大的研究價值。