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

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

0 引言

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

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

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

由于數(shù)值解求解過程中存在累積誤差，且會陷入局部最優(yōu)的狀態(tài)，智能優(yōu)化算法又存在搜索范圍廣，搜索速度慢的問題，所以本文采取先用數(shù)值迭代法求出最接近期望位姿的機器手初步最優(yōu)角度解，再利用粒子群優(yōu)化算法在初步最優(yōu)角度解處偏置±10°進(jìn)一步搜索優(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 基本原理

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

2.2 算法流程

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

(11)

基本粒子算法步驟：

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

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

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

步驟4：判斷其是否滿足迭代停止規(guī)則，若滿足，跳出算法循環(huán)輸出結(jié)果。否則進(jìn)行位置與速度更新，轉(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)進(jìn)行角度搜索，搜索角度范圍太大，計算量繁重，收斂速度更慢精度不高，消耗算力也更多，且容易陷入局部最優(yōu)的范圍解內(nèi)。

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

本文將數(shù)值法與粒子群算法相結(jié)合起來，先用數(shù)值法迭代進(jìn)行初步計算，得到七軸機器人的初步旋轉(zhuǎn)角度，然后利用粒子群算法進(jìn)行進(jì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é)束。

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

4 實驗結(jié)果

經(jīng)過指定機器手的初始點位姿與期望的終點位姿，根據(jù)數(shù)值迭代與粒子群混合算法步驟進(jìn)行仿真求解，如表2所示實驗初步設(shè)置的初始位置位姿與期望的初始位置位姿；如表3所示機器手進(jìn)行初步迭代后的角度初步最優(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é)果誤差始終無法進(jìn)一步減小，在此角度基礎(chǔ)上偏置±10°，縮小搜索角度范圍，進(jìn)行PSO算法搜索優(yōu)化后，誤差明顯大大減小。

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

5 結(jié)論

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