改進幾何法求解逆運動學(xué)的仿真和實驗研究

王 垚，何秋生，李曉云，盧 冉

(太原科技大學(xué)電子信息工程學(xué)院，太原 030024)

機器人的逆運動學(xué)是研究機器人的基礎(chǔ)，深入研究機器人逆運動學(xué)，尋求一種準確可靠的逆運動學(xué)求解方法可以為后續(xù)的軌跡規(guī)劃減小控制誤差，為機器人實時控制縮短執(zhí)行時間[1]。目前，研究機器人的逆運動學(xué)解法的相關(guān)文獻很多；文獻[2]中的偽逆法利用雅克比矩陣分析機器人的逆運動學(xué)，該法存在大量的矩陣運算，在尋求最優(yōu)解時易陷入局部極值；文獻[3]采用的數(shù)值迭代法因其迭代性質(zhì)導(dǎo)致求解速度慢，不直觀且不易理解；文獻[4]采用的旋量法對Paden-Kahan子問題依賴性強；文獻[5]根據(jù)機器人連桿位型分類求逆解的方法普適性不強。

近年來，國內(nèi)外許多學(xué)者研究用人工神經(jīng)網(wǎng)絡(luò)算法、遺傳算法等智能算法求逆解；文獻[6]提出的RBF神經(jīng)網(wǎng)絡(luò)與正交最小二乘法結(jié)合的求解方法和文獻[7]提出的人工神經(jīng)網(wǎng)絡(luò)法提高了控制精度但實時性較差不利于機器人的在線控制，文獻[8]提出用遺傳算法優(yōu)化BP神經(jīng)網(wǎng)絡(luò)權(quán)值和閾值但仍存在早熟、收斂等問題。

幾何法因其具有計算誤差小和控制精度高的優(yōu)點，被廣泛應(yīng)用在機器人的在線控制中。本文結(jié)合Denso-VP6242機器人的結(jié)構(gòu)特點，采用D-H參數(shù)法建立連桿坐標(biāo)系，通過幾何法給出機器人逆運動學(xué)的求解過程。

1 D-H參數(shù)法建立連桿坐標(biāo)系

本文選用的是六自由度串聯(lián)機器人Denso-VP6242，其結(jié)構(gòu)如圖1所示。

圖1 Denso機器人結(jié)構(gòu)
Fig.1 Structure of Denso robot

采用D-H參數(shù)法對Denso機器人建立連桿坐標(biāo)系。建立連桿坐標(biāo)系[9]的具體步驟為：(1)設(shè)定機器人各關(guān)節(jié)的初始位型如圖1；(2)畫出6個關(guān)節(jié)軸線的延長線，以關(guān)節(jié)軸線的方向為連桿坐標(biāo)系zi的方向；(3)若相鄰兩關(guān)節(jié)軸平行，以相鄰連桿的公垂線和關(guān)節(jié)軸線的交點作為連桿坐標(biāo)系的原點，連桿坐標(biāo)系xi的方向為公垂線的方向；若相鄰兩關(guān)節(jié)軸線相交則以軸線交點作為連桿坐標(biāo)系的原點，連桿坐標(biāo)系xi的方向為垂直于相交兩條關(guān)節(jié)軸線所在平面的方向；(4)用右手法則確定連桿坐標(biāo)系yi的方向。Denso機器人的連桿坐標(biāo)系如圖2所示。

圖2 Denso機器人的連桿坐標(biāo)系
Fig.2 Coordinates of Denso robot

其中變量θi表示繞zi軸從xi-1旋轉(zhuǎn)到xi的角度；di表示沿zi軸從xi-1移動到xi的距離；ai表示沿xi軸從zi-1移動到zi的距離；αi表示繞xi軸從zi-1旋轉(zhuǎn)到zi的角度；Pw是腕部參考點，P是機器人末端點，Denso-VP6242機器人的D-H參數(shù)如表1所示。

表1 Denso機器人的連桿參數(shù)
Tab.1 Parameters of Denso robot

連桿iθi/(rad)αi/(rad)ai/(m)di/(m)變量范圍/(rad)1θ1π/200.125-2.7925,2.79252θ200.2100-2.0944,2.09443θ3-π/2-0.0750-2.7925,0.33164θ4π/200.210-2.7925,2.79255θ5-π/200-2.0944,2.09446θ6000.070-6.2832,6.2832

2 傳統(tǒng)幾何法求解關(guān)節(jié)角θ1，θ2和θ3

逆運動學(xué)是實現(xiàn)機器人路徑規(guī)劃、實時控制的基礎(chǔ)，研究一種簡單高效的逆運動學(xué)解法具有重要的意義。根據(jù)機器人末端位姿矩陣T6求解機器人各個關(guān)節(jié)對應(yīng)的角度值q，對應(yīng)關(guān)系可表示為：

q=Denso_IPK(T6)

(1)

根據(jù)六自由度串聯(lián)機器人Denso-VP6242的幾何結(jié)構(gòu)計算與位置相關(guān)的關(guān)節(jié)角度θ1，θ2和θ3的具體步驟如下：

(1)根據(jù)機器人末端在笛卡爾空間中的位置P的坐標(biāo)和機器人末端的接近矢量a求解腕部參考點Pw的位置坐標(biāo)，表達式為：

Pw=[x,y,z]=[px,py,pz]-d6·[ax,ay,az]

(2)

(2)機器人在xoy平面上的投影如圖3所示，圖中pw為腕部參考點Pw在xoy平面上的投影。

圖3 機器人在xoy平面的投影
Fig.3 Robot’s projection onxoy

由此可得：θ1=arctan2(y,x)

(3)

根據(jù)式(3)和x，y的符號可在關(guān)節(jié)變量范圍內(nèi)求出唯一的θ1.

從圖4中應(yīng)用平面幾何關(guān)系可以得出關(guān)節(jié)角θ3的表達式為：

θ3=α1+α2

(4)

圖4 機器人在zop平面的投影
Fig.4 Robot’s projection on zop

同理可得關(guān)節(jié)角θ2的表達式為：

θ2=β1-β2

(5)

β2=β3=atctan2(l,c)；

3 改進幾何法求解關(guān)節(jié)角θ4，θ5和θ6

由于傳統(tǒng)幾何法通過分離變換矩陣中的變量推算關(guān)節(jié)角度，計算過程中矩陣運算量大、存在誤差，求解非常困難[10-11]。因此考慮研究一種改進幾何法，簡化求解過程，達到提高控制精度的目的。

改進幾何法求解關(guān)節(jié)角θ4，θ5和θ6的思路為：首先根據(jù)給定的機器人姿態(tài)轉(zhuǎn)換為RPY組合表示的姿態(tài)角，然后由姿態(tài)角和六自由度機器人關(guān)節(jié)角的關(guān)系求解θ4，θ5和θ6.

RPY組合即用橫滾(roll)、俯仰(pitch)和偏轉(zhuǎn)(yaw)組合變換表示機器人末端姿態(tài)，其中橫滾(roll)角γ表示機器人末端相對于基坐標(biāo)系的z軸旋轉(zhuǎn)γ角，俯仰(pitch)角β表示機器人末端相對于基坐標(biāo)系的y軸旋轉(zhuǎn)β角，偏轉(zhuǎn)(yaw)角為α表示機器人末端相對于基坐標(biāo)系的x軸旋轉(zhuǎn)為α角，如圖5所示。

圖5 RPY組合變換表示姿態(tài)
Fig.5 RPY showing posture

當(dāng)已知機器人的位姿矩陣時，可通過旋轉(zhuǎn)矩陣RT6與RPY組合表示的姿態(tài)角關(guān)系表達式(6)求解偏轉(zhuǎn)角α，俯仰角β，橫滾角γ，表達式為：

PRY(γ,β,α)=

(6)

式(6)中：cα=cos(α)，sα=sin(α)

當(dāng)機器人末端相對于基坐標(biāo)系的偏轉(zhuǎn)角α，俯仰角β，橫滾角γ已知時，根據(jù)機器人的六個關(guān)節(jié)角與末端姿態(tài)角的關(guān)系：

(7)

由此可求得機器人的關(guān)節(jié)角θ4，θ5和θ6.

在求逆過程中，出現(xiàn)一個關(guān)節(jié)角對應(yīng)多種解法的情況，但在實際控制過程中不允許出現(xiàn)多解情況，本文以路徑最短為優(yōu)選策略選取一組最優(yōu)解。設(shè)定優(yōu)選目標(biāo)函數(shù)為：

(8)

求解各個關(guān)節(jié)角最優(yōu)解的算法流程如圖6所示。

4 仿真和實驗驗證

圖6 最優(yōu)解算法流程圖
Fig.6 Algorithm flow chart of optimal solution

設(shè)定機器人姿態(tài)始終為[α，β，γ]=[0，-pi，0]，機器人末端位置取圓弧曲線上的點，圓弧曲線表達式為式(9):

(9)

圖7 機器人仿真模型
Fig.7 The simulink modle of robot

圓弧曲線和期望末端位置采樣點如圖8所示。

當(dāng)k=0:π/20:2π時，可得期望末端位置點的坐標(biāo)p=x(k),y(k),z(k)，然后求解逆運動學(xué)得相應(yīng)的關(guān)節(jié)角度。通過對比傳統(tǒng)幾何法和改進幾何法的位置誤差和姿態(tài)誤差，驗證改進幾何法的優(yōu)越性，其中姿態(tài)誤差為實際旋轉(zhuǎn)矩陣與期望旋轉(zhuǎn)矩陣所有元素的絕對誤差。

圖8 目標(biāo)圓弧曲線及采樣點
Fig.8 Task trajectory and sample points

采用傳統(tǒng)的幾何法求得位置誤差為:

(10)

式(10)中：[x1,y1,z1]為采用傳統(tǒng)的幾何法求得實際末端位置坐標(biāo)。

同理，采用改進的幾何法求得實際末端位置坐標(biāo)為p2=[x2,y2,z2]時位置誤差為：

(11)

式(11)中：[x2,y2,z2]為采用改進的幾何法求得實際末端位置坐標(biāo)。仿真結(jié)果如圖9所示。

圖9 傳統(tǒng)幾何法和改進幾何法的位置誤差
Fig.9 Position error of traditional and advanced Geometr

采用傳統(tǒng)幾何法的姿態(tài)誤差er1和采用改進幾何法的姿態(tài)誤差er2仿真結(jié)果如圖10所示。

仿真結(jié)果看出：與傳統(tǒng)幾何法相比，采用改進幾何法位置誤差和姿態(tài)誤差小，求解精度高，驗證了本文研究的改進幾何法的優(yōu)越性。

圖10 傳統(tǒng)幾何法和改進幾何法的姿態(tài)誤差
Fig.10 Posture error of traditional and advanced Geometry

在實驗室現(xiàn)有的六自由度機器人Denso-VP6242控制平臺上進行實驗，驗證本文研究的改進幾何法在實時機器人控制平臺中的可行性。以繪制圓弧為例，分別采用傳統(tǒng)幾何法和改進幾何法控制機器人，并將機器人畫出的圓弧進行比較，圖11是實驗結(jié)果。

圖11(a)為期望圓弧曲線，(b)為采用傳統(tǒng)幾何法控制機器人畫出的圓弧曲線，(c)為采用改進幾何法控制機器人畫出的圓弧曲線，目標(biāo)圓弧是直徑為0.05 m的圓，圖11中圖(c)比(b)的直徑更接近0.05 m，曲線更為光滑。實驗結(jié)果驗證了該方法的可行性和優(yōu)越性。

5 結(jié) 論

(1)研究了一種改進幾何法。借助robotics toolbox進行逆運動學(xué)仿真，結(jié)果表明該方法可以減小機器人位姿誤差，提高控制精度。其中，位置誤差在10-16數(shù)量級，姿態(tài)誤差在10-31數(shù)量級。

圖11 實驗結(jié)果
Fig.11Theexperimentalresults

(2)將改進幾何法應(yīng)用到實際機器人控制平臺中，實驗結(jié)果表明改進幾何法的可靠性和優(yōu)越性，為研究機器人軌跡規(guī)劃和實時控制奠定了基礎(chǔ)。