基于位姿分離的機械臂解析求逆優(yōu)化算法

隋 濤, 孔劉君, 姜 昊, 蔣 強

(沈陽理工大學 自動化與電氣工程學院, 沈陽 110159)

機械臂作為機器人的重要執(zhí)行機構(gòu),可以模擬人們的手臂去完成各項工作[1],其自主操作能力和執(zhí)行大量任務(wù)的能力已經(jīng)引起了研究人員和工業(yè)界的廣泛關(guān)注.機械臂在執(zhí)行各項任務(wù)時,通常需要解決逆向運動學問題,這是實現(xiàn)機械臂運動控制[2]、軌跡規(guī)劃[3]和避障[4]的前提.

機械臂的逆向運動學問題常被稱作是“運動學的珠穆朗瑪峰問題”[5],其本質(zhì)是完成從機械臂工作空間到關(guān)節(jié)空間的映射[6],這種映射關(guān)系具有非線性和多耦合性.多年來國內(nèi)外許多優(yōu)秀學者對機械臂的逆向運動學問題開展研究.蔡改貧等[7]提出的APSO-LM-BP神經(jīng)網(wǎng)絡(luò)算法解決了傳統(tǒng)神經(jīng)網(wǎng)絡(luò)求取逆解時容易陷入局部極小值的缺陷.KHAN等[8]提出的雙粒子群算法分別對位置和姿態(tài)問題進行優(yōu)化,改善了收斂性能.TOZ[9]提出的基于混沌的渦旋搜索算法有效降低了最大迭代次數(shù).上述幾種智能算法雖然在不同維度上進行了改進,但在穩(wěn)定性、控制精度等方面仍面臨巨大挑戰(zhàn),而解析法恰巧避免了上述問題.韓碩[10]與程浩田[11]將幾何法與矩陣逆乘相結(jié)合,求解精度較高,但算法較為抽象繁瑣.李海[12]減少了矩陣逆乘的次數(shù),但仍停留在對4維矩陣的處理層面,運算量較大.

針對上述問題,本文以ROKAE XB7型機械臂為研究對象,提出一種結(jié)合位姿分離思想的解析求逆優(yōu)化算法.該算法將4維矩陣運算分別降低為1維和3維矩陣運算,在保證其計算精度的同時,能夠有效避免以往解析法的復(fù)雜求解過程,提高運算效率.通過蒙特卡洛法獲取仿真數(shù)據(jù)點,并在Matlab仿真環(huán)境下驗證改進算法的穩(wěn)定性與有效性.

1 機械臂正向運動學

1.1 正向運動學建模

本文所研究的ROKAE XB7型機械臂具有6個轉(zhuǎn)動關(guān)節(jié),前3個關(guān)節(jié)決定機械臂所能到達的位置,后3個關(guān)節(jié)決定其到達指定位置時的姿態(tài).根據(jù)機械臂的結(jié)構(gòu)特征及外形尺寸,采用改進型D-H方法[13]建立如圖1所示的連桿坐標系.

圖1 機械臂連桿坐標系Fig.1 Coordinate system of manipulator link

改進型D-H參數(shù)法建系得到的相鄰兩連桿坐標系變換矩陣[14]為

(1)

式中:αi-1為連桿扭轉(zhuǎn)角;ai-1為連桿長度;di為連桿偏距;θi為關(guān)節(jié)角.機械臂的D-H參數(shù)如表1所示.

表1 機械臂D-H參數(shù)Tab.1 D-H parameters of manipulator

1.2 正向運動學求解

機械臂的正向運動學求解是指已知機械臂各關(guān)節(jié)變量,求取末端位姿的過程[15].根據(jù)表1中的D-H參數(shù)和式(1),得到機械臂末端坐標系相對于基坐標系的變換矩陣,即

(2)

(3)

式中:R3×3為旋轉(zhuǎn)矩陣,表示機械臂的末端姿態(tài);P3×1為位置矢量,表示機械臂的末端位置;s23=sin(θ2+θ3);c23=cos(θ2+θ3);si=sinθi;ci=cosθi.

2 機械臂逆向運動學

(4)

2.1 位置逆解的求取

根據(jù)位置矢量P3×1求取前3個關(guān)節(jié)角.由px/py可以求解θ1,即

(5)

對px和pz進行變換整理可得

(a3s23+d4c23+a2s2)2

(6)

經(jīng)化簡及三角恒等變換可得

θ3=atan 2(a3,d4)-

(7)

(8)

對pz進行變換整理可得

pz=-s2(a3c3+a2-d4s3)+c2(-a3s3-d4c3)

(9)

對上式進行三角恒等變換可得

θ2=atan 2(K3,K2)-

(10)

(11)

2.2 姿態(tài)逆解的求取

根據(jù)旋轉(zhuǎn)矩陣R3×3求取后3個關(guān)節(jié)角.由旋轉(zhuǎn)矩陣性質(zhì)可知

(12)

(13)

令式(13)兩邊矩陣的元素(2,3)相等,則

(14)

由式(15)可求得θ5,即

θ5=atan 2(s5,c5)

(15)

令式(13)兩邊矩陣的元素(1,3)和(3,3)相等,則

(16)

由式(16)可求得θ4,即

θ4=atan 2(r13s1-r23c1,r13c1c23+

r23s1c23-r33s23)

(17)

令式(13)兩邊矩陣的元素(2,1)和(2,2)相等,則

(18)

由式(18)可求得θ6,即

θ6=atan 2(r12c1s23+r22s1s23+r32c23,

-r11c1s23-r21s1s23-r31c23)

(19)

2.3 最優(yōu)解的選取

通過式(5)～(9)可求得機械臂的8組逆解,然而,在實際應(yīng)用中往往僅需1組最優(yōu)逆解即可,故可采取以下步驟選取最優(yōu)解:

1) 舍去不滿足各關(guān)節(jié)轉(zhuǎn)角范圍的解.

2) 基于節(jié)省能耗與最佳柔順性原則,采用性能函數(shù)來確定最優(yōu)解,函數(shù)值越小,說明性能越好.性能函數(shù)定義為

(20)

式中:ωi為第i關(guān)節(jié)對應(yīng)的權(quán)重(i越大,ωi越小);Δθi為第i關(guān)節(jié)逆解的角度與當前角度的差值.

3 機械臂工作空間分析

機械臂的工作空間是指其末端所能到達的范圍.對工作空間進行分析,可為后續(xù)仿真驗證部分提供大量實驗數(shù)據(jù).在Matlab中通過編程方式完成對機械臂工作空間的求解(蒙特卡洛法)[18],具體流程如圖2所示.

圖2 機械臂工作空間求解流程Fig.2 Flow chart for solution of manipulator workspace

取N=30 000,得到如圖3所示的機械臂工作空間點云圖.由圖3可知,ROKAE XB7型機械臂工作性能良好,其工作范圍是一個半徑約為0.707 m的球體.由于受到機體構(gòu)造的約束,機械臂的可達空間存在一個較小缺口,因此,在選取實驗數(shù)據(jù)時應(yīng)避免選擇缺口處的不可達工作點.

圖3 機械臂工作空間點云圖Fig.3 Point cloud maps of manipulator workspace

4 機械臂仿真分析

采用CPU為Intel Core(TM) i7-6700HQ 2.60GHZ的計算機,利用Matlab仿真軟件對機械臂的運動學模型及正逆運動學算法進行驗證分析.

4.1 正向運動學仿真驗證

在機械臂的關(guān)節(jié)空間內(nèi),隨意選擇一組關(guān)節(jié)角[π/3,π/5,-π/4,π/6,π/4,π/5],其3D仿真模型如圖4所示,其中qi為關(guān)節(jié)變量.

圖4 機械臂模型Fig.4 Model of manipulator

基于機械臂正向運動學方程設(shè)定上述選定的關(guān)節(jié)角,得到機械臂末端的位姿矩陣,即

(21)

為了驗證正解算法的正確性,調(diào)用Robotics Toolbox中的fkine函數(shù)進行驗證,結(jié)果表明求解結(jié)果與式(21)一致,驗證了正解算法及所建模型的正確性.

4.2 逆解算法單點測試

按照式(21)設(shè)定機械臂末端期望位姿,將其代入基于本文逆解算法的Matlab程序中,得到如表2所示的8組逆解.

表2 機械臂8組逆解Tab.2 Eight inverse solutions of manipulator

由表2可知,第2組逆解結(jié)果與初始設(shè)定的關(guān)節(jié)角度一致.為了驗證其余各組逆解的準確性,將余下7組逆解依次代入正向運動學方程.經(jīng)驗證得知,各組計算結(jié)果均與式(21)吻合(忽略仿真計算導致的浮點誤差).在機械臂的工作空間內(nèi)隨機選取多個工作點進行逆解算法驗證后發(fā)現(xiàn),該算法具有較高控制精度與良好穩(wěn)定性.

4.3 逆解算法連續(xù)軌跡測試

在機械臂工作空間內(nèi),任取起始位置Tstart和終止位置Tend,并已知起始位置所對應(yīng)的關(guān)節(jié)角θstart.三個設(shè)定條件具體數(shù)據(jù)為

(22)

(23)

θstart=[0,π/4,-π/18,π/5,-π/4,π/4]

(24)

利用Matlab Robotics工具箱中的ctraj函數(shù)生成10個路徑點,采用本文提出的算法得到如圖5所示的連續(xù)軌跡逆解擬合曲線.由圖5可知,連續(xù)軌跡逆解曲線光滑,并未出現(xiàn)較大跳變,證實了所提逆解算法及最優(yōu)解選取原則應(yīng)用于后續(xù)機械臂軌跡規(guī)劃、運動控制等方面研究的可行性.

圖5 連續(xù)軌跡逆解擬合曲線Fig.5 Fitting curves of continuous trajectory inverse solutions

4.4 逆解算法對比分析

基于李海[12]提出的解析求逆優(yōu)化算法思路,對本文搭建的機械臂模型進行逆向運動學算法設(shè)計.在設(shè)計算法的過程中發(fā)現(xiàn)該算法雖然較傳統(tǒng)解析法減少了矩陣逆乘的次數(shù),但仍需對4維矩陣進行高達3次的逆乘運算,公式推導過程繁瑣復(fù)雜.而本文提出的逆解算法充分利用機械臂的結(jié)構(gòu)特點及位姿矩陣特性,可以間接實現(xiàn)矩陣降維運算,降低了逆解算法的設(shè)計難度.同時,本文提出的逆解算法有效避免了其他算法[10-11]設(shè)計過程的抽象、不直觀的問題.

為了進一步證實算法的優(yōu)越性,在機械臂的工作空間內(nèi),任取10個不同位姿,利用Matlab軟件采用本文所提算法與李海[12]所提算法對各位姿分別進行50次求逆運算,將兩個算法的各位姿平均求解時間進行對比,結(jié)果如圖6所示.本文所提算法與李海[12]所提算法對10個位姿的平均求解時間分別為0.197 ms、0.298 ms,本文算法求解效率提高了33.89%,表明本文所提算法減少了求解過程的程序運行時間,具有更好的實時性.

圖6 不同逆解算法的求解時間對比Fig.6 Comparison of solution time of different inverse solution algorithms

5 結(jié) 論

采用改進型D-H參數(shù)法搭建ROKAE XB7型機械臂運動學模型.結(jié)合位姿分離思想提出一種解析求逆優(yōu)化算法.利用蒙特卡洛法對機械臂的工作空間進行分析,獲取了機械臂可達工作點信息范圍.通過Matlab仿真軟件驗證了所提算法的穩(wěn)定性與優(yōu)越性.與現(xiàn)階段解析求逆優(yōu)化算法相比,本文算法克服了求解復(fù)雜繁瑣、計算成本高等諸多弊端.本文算法的提出不僅可為機械臂后續(xù)相關(guān)研究奠定基礎(chǔ),同時可為多關(guān)節(jié)機械臂逆向運動學求解提供新思路.