6R工業(yè)機(jī)器人幾何求逆優(yōu)化算法及仿真分析*

程浩田，祝錫晶，馮昕宇，趙 晶，蔡展鵬，丁帥帥

(中北大學(xué)山西省先進(jìn)制造技術(shù)重點(diǎn)實(shí)驗(yàn)室，太原 030051)

0 引言

近些年，工業(yè)機(jī)器人發(fā)展非常迅速，已經(jīng)成為了我們?nèi)粘Ｉ钪胁豢苫蛉钡囊徊糠?。工業(yè)機(jī)器人主要通過各關(guān)節(jié)的連續(xù)運(yùn)動來控制末端執(zhí)行器完成各種復(fù)雜工作，所以對其進(jìn)行運(yùn)動學(xué)分析和軌跡規(guī)劃是非常必要的。軌跡規(guī)劃是機(jī)器人平穩(wěn)運(yùn)行的先決條件，通過對其各關(guān)節(jié)角位移、角速度以及角加速度的規(guī)劃，將有效降低機(jī)械臂運(yùn)動時(shí)產(chǎn)生的震動以及速度突變，并以此來提升運(yùn)動的穩(wěn)定性和精確性。而運(yùn)動學(xué)分析作為軌跡規(guī)劃的基礎(chǔ)，通?？梢苑譃檎\(yùn)動學(xué)和逆運(yùn)動學(xué)兩個(gè)部分[1-2]。其中逆運(yùn)動學(xué)是正運(yùn)動學(xué)的逆解，并且其能否準(zhǔn)確快速求解對機(jī)器人實(shí)時(shí)控制起著至關(guān)重要的作用。

針對該問題，國內(nèi)外學(xué)者做了大量深入的研究，王垚等[3]提出一種改進(jìn)幾何方法進(jìn)行逆運(yùn)動學(xué)求解；葛小川等[4]提出一種D-H法與四元數(shù)法相結(jié)合的逆解新算法；呂北軒等[5]運(yùn)用非線性規(guī)劃對小型機(jī)械臂逆運(yùn)動學(xué)進(jìn)行優(yōu)化求解；Ao T等[6]提出一種旋量法與代數(shù)法結(jié)合的機(jī)械臂逆解算法；Ma C等[7]利用人工神經(jīng)網(wǎng)絡(luò)來求解機(jī)械臂逆運(yùn)動學(xué)。雖然其中一些方法被證明具有一定的實(shí)際應(yīng)用價(jià)值，但是仍然存在諸多問題需要解決，如大量的矩陣運(yùn)算，復(fù)雜的數(shù)學(xué)模型以及較長的程序運(yùn)行時(shí)間。

基于上述內(nèi)容，本文根據(jù)SNR3-C30型機(jī)械臂的結(jié)構(gòu)特征擬采用Pieper法來構(gòu)建D-H坐標(biāo)系，并提出結(jié)合位姿分離思想的機(jī)械臂幾何求逆算法，該算法利用幾何法和旋轉(zhuǎn)矩陣特性，可以快速求出所有解，不必像傳統(tǒng)方法一樣進(jìn)行多次矩陣逆乘運(yùn)算，減少了計(jì)算量，而后對六自由度機(jī)械臂奇異點(diǎn)位置以及關(guān)節(jié)空間軌跡規(guī)劃進(jìn)行了分析。在MATLAB仿真環(huán)境下驗(yàn)證了改進(jìn)算法的有效性、機(jī)械臂運(yùn)動模型的正確性以及達(dá)到了機(jī)械臂關(guān)節(jié)空間軌跡規(guī)劃的預(yù)期目標(biāo)。

1 機(jī)械臂D-H建模

機(jī)械臂是由一系列連桿通過移動和轉(zhuǎn)動關(guān)節(jié)串聯(lián)而成的，對于不同型號的機(jī)械臂來說，每個(gè)連桿長度不同，其關(guān)節(jié)平移或旋轉(zhuǎn)的范圍也不同。Denavit和Hartenberg按照連桿間的相對位置，提出為每個(gè)連桿建立一個(gè)坐標(biāo)系[8]。確立坐標(biāo)系時(shí)需要遵循右手定則，且通過齊次變化矩陣來描述機(jī)械臂各關(guān)節(jié)連桿間的相對運(yùn)動關(guān)系。

1.1 坐標(biāo)系與參數(shù)的確立

機(jī)器人連桿i與連桿i-1的連接關(guān)系如圖1所示,關(guān)節(jié)軸線i和i-1分別與坐標(biāo)軸Zi和Zi-1重合，Xi-1軸與公法線ai-1軸重合，且方向沿ai-1由i-1軸指向i軸，Yi軸方向則按照右手定則確立。

圖1 相鄰兩連桿坐標(biāo)系

具體參數(shù)如下：轉(zhuǎn)角θi為Xi-1到Xi繞Zi旋轉(zhuǎn)的角度，di為從Xi-1到Xi沿Zi平移的距離，αi-1為從Zi-1到Zi沿Xi-1旋轉(zhuǎn)的角度，ai-1為從Zi-1到Zi沿Xi-1測量的距離[9]。相鄰兩連桿間坐標(biāo)系變化矩陣為：

(1)

1.2 機(jī)械臂D-H參數(shù)模型

本文所研究的SNR3-C30型機(jī)器人擁有6個(gè)關(guān)節(jié)，且每個(gè)關(guān)節(jié)均屬于轉(zhuǎn)動變量，將底座命名為連桿0，其余各關(guān)節(jié)連桿如圖2所示，分別為1、2、3、4、5、6。根據(jù)其三維模型及各連桿坐標(biāo)系之間的位置關(guān)系，建立D-H坐標(biāo)系，如圖3所示。

圖2 SNR3-C30機(jī)械臂三維模型 圖3 SNR3-C30型機(jī)械臂連桿坐標(biāo)系

通過測量得到SNR3-C30型機(jī)械臂D-H參數(shù)如表1所示。

表1 SNR3-C30型機(jī)械臂D-H參數(shù)

2 機(jī)械臂運(yùn)動學(xué)分析

為了控制機(jī)械臂的運(yùn)動，首先要建立運(yùn)動學(xué)模型，然后基于模型選擇運(yùn)動學(xué)方程，通過對其表示和求解來進(jìn)行運(yùn)動學(xué)分析。

2.1 運(yùn)動學(xué)正解

正運(yùn)動學(xué)是通過機(jī)械臂初始給定的各個(gè)關(guān)節(jié)角度來求解其末端執(zhí)行器的具體位姿，且該位姿具有唯一性。

(2)

將表1機(jī)械臂D-H參數(shù)代入式(1),得到各個(gè)連桿之間的奇次變化矩陣分別為：

為了簡化公式，上式中si代表sinθi，ci代表cosθi，s12代表sin(θ1+θ2),c12代表cos(θ1+θ2)，故式(2)中各元素計(jì)算結(jié)果如下所示：

nx=c1[c23(c4c5c6-s4s6)-s23s5c6]+s1(s4c5c6+c4s6)

ny=s1[c23(c4c5c6-s4s6)-s23s5c6] -c1(s4c5c6+c4s6)

nz=s23(s4s6-c4c5c6)-c23s5c6

ox=c1[c23(-c4c5s6-s4c6)+s23s5s6]+s1(c4c6-s4c5s6)

oy=s1[c23(-c4c5s6-s4c6)+s23s5s6]-c1(c4c6-s4c5s6)

oz=s23(s4c6+c4c5s6)+c23s5s6

ax=c1(-c23c4s5-s23c5)-s1s4s5

ay=s1(-c23c4s5-s23c5)+c1s4s5

az=s23c4s5-c23c5

px=c1(a1+a2c2+a3c23-d4s23)

py=s1(a1+a2c2+a3c23-d4s23)

pz= -a2s2-a3s23-d4c23

2.2 運(yùn)動學(xué)逆解

逆運(yùn)動學(xué)是通過初始給定的機(jī)械臂末端執(zhí)行器位姿來求解當(dāng)前各個(gè)關(guān)節(jié)角度，且解不具有唯一性。末端執(zhí)行器位姿矩陣為：

其中，R表示末端執(zhí)行器所在坐標(biāo)系相對基坐標(biāo)系的旋轉(zhuǎn)矩陣，P為笛卡爾空間中的位置矩陣。改進(jìn)算法將旋轉(zhuǎn)矩陣與位置矩陣從位姿矩陣中分離，利用位置矩陣結(jié)合幾何法單獨(dú)求解前三個(gè)關(guān)節(jié)角θ1,θ2,θ3，然后在此基礎(chǔ)上通過關(guān)節(jié)連桿4、5、6的旋轉(zhuǎn)矩陣來求解剩余關(guān)節(jié)角。

2.2.1 求解前3個(gè)關(guān)節(jié)角θ1,θ2,θ3

圖4 機(jī)械臂連桿幾何位置關(guān)系

由圖4可得機(jī)械臂位置關(guān)系函數(shù)表達(dá)式為：

Px=[a1+a2sinθ2+a3sin(θ2+θ3)+d4cos(θ2+θ3)]cosθ1

(3)

Py=[a1+a2sinθ2+a3sin(θ2+θ3)+d4cos(θ2+θ3)]sinθ1

(4)

Pz=l+a2cosθ2+a3cos(θ2+θ3)-d4sin(θ2+θ3)

(5)

(1)求解θ1

由機(jī)器人腕部參考點(diǎn)PW在XOY面上的投影得：

θ1=atan2(py,px)或θ1=atan2(-py,-px)

(2)求解θ2

將式(3)和式(5)聯(lián)立，平方后相加得：

(3)求解θ3

將θ2代入式(5)可得：

Pz-l-a2cosθ2=a3cos(θ2+θ3)-d4sin(θ2+θ3)

解得：θ3=

atan(a3,d4)-θ2

2.2.2 求解后3個(gè)關(guān)節(jié)角θ4,θ5,θ6

相鄰兩連桿間旋轉(zhuǎn)矩陣為：

由旋轉(zhuǎn)矩陣正交性質(zhì)可得：

(6)

(1)求解θ5

令等式(6)兩端(2,3)相等，可得：

θ5=arccos(-axc1s23-ays1s23-azc23)

(2)求解θ4

令上述等式(6)兩端(1,3)和(3,3)分別對應(yīng)相等，即得兩方程：

解得：

①當(dāng)0<θ5≤120° 時(shí)：

θ4=atan(-s1ax+c1ay,-c1c23ax-s1c23ay+s23az)

②-120°≤θ5<0 時(shí)：

θ4=atan(s1ax-c1ay,c1c23ax+s1c23ay-s23az)

(3)求解θ6

令上述等式(6)兩端(2,1)和(2,2)分別對應(yīng)相等，即得兩方程：

解得：

①當(dāng)0<θ5≤120° 時(shí)：

θ6=atan(c1s23ox+s1s23oy+c23oz,-c1s23nx-s1s23ny-c23nz)

②-120°≤θ5<0時(shí)：

θ6=atan(-c1s23ox-s1s23oy-c23oz,c1s23nx+s1s23ny-c23nz)

綜上所述，θ1,θ2,θ3,θ4,θ5,θ6的可行解全部求出，由于θ1,θ3,θ5,都存在兩個(gè)解的情況，通過組合，可行解最終有8組，并且關(guān)節(jié)角θ5不能為0，否則關(guān)節(jié)連桿4、6軸線重合，此時(shí)機(jī)械臂位于奇異點(diǎn)，機(jī)械臂系統(tǒng)報(bào)錯(cuò)，停止運(yùn)行。

3 機(jī)械臂仿真分析

MATLAB是一款集數(shù)據(jù)分析、計(jì)算以及可視化等多種功能于一體的仿真軟件，其Robotics Toolbox模塊是專門針對機(jī)器人研究所需要的建模、運(yùn)動學(xué)分析、軌跡規(guī)劃以及算法等方面而設(shè)計(jì)。本文仿真分析所處的仿真環(huán)境為：MATLAB (2019a)和Robotics Toolbox for MATLAB(release 10.3.1)。

3.1 機(jī)械臂運(yùn)動學(xué)建模

在Robotics Toolbox模塊中調(diào)用link函數(shù)，建立機(jī)械臂仿真模型，并用plot函數(shù)顯示其三維空間模型[10]。機(jī)械臂6個(gè)關(guān)節(jié)變量分別為[0 0 0 0 pi/2 0]時(shí)，其三維空間仿真模型如圖5所示。

圖5 SNR3-C30型機(jī)械臂三維空間仿真模型

3.2 機(jī)械臂正運(yùn)動學(xué)仿真

假設(shè)SNR3-C30型工業(yè)機(jī)器人的6個(gè)關(guān)節(jié)角度分別為0、pi/2、pi/3、0、-pi/2、0，它們共同組成一個(gè)向量q=[0 pi/2 pi/3 0 -pi/2 0]，利用fkine函數(shù)得到末端執(zhí)行器位姿矩陣T為：

(7)

經(jīng)過驗(yàn)證，該結(jié)果與上述方程(2)所計(jì)算結(jié)果完全一致，故證明正解算法正確，且模型建立無誤。

3.3 機(jī)械臂逆運(yùn)動學(xué)算法驗(yàn)證及仿真

假設(shè)SNR3-C30型工業(yè)機(jī)器人的末端執(zhí)行器位姿矩陣T如上式(7)所示，利用ikine函數(shù)得到機(jī)器人各個(gè)關(guān)節(jié)角度如下所示：q=[0.000 1.570 1.047 0.000 -1.570 0.000]。經(jīng)過驗(yàn)證，該結(jié)果與上述改進(jìn)逆解算法計(jì)算結(jié)果相同，并且與正解仿真選取的各關(guān)節(jié)角度一致，驗(yàn)證了該逆解算法的有效性。其次選取機(jī)器人末端執(zhí)行器的10組不同位姿，分別利用幾何法，逆乘法以及改進(jìn)算法進(jìn)行求逆運(yùn)算。為了使計(jì)算結(jié)果更加精確，將每組逆運(yùn)動迭代100次后的平均值作為最終結(jié)果，仿真結(jié)果如圖6所示。

圖6 改進(jìn)算法與幾何法、逆乘法運(yùn)算速度比較

從圖中可以得出幾何法、逆乘法以及改進(jìn)算法平均求解時(shí)間分別為2.062 s，2.16 s,1,981 s,改進(jìn)算法比幾何法運(yùn)算速度提高了3.92%，比逆乘法提高了8.29%，說明改進(jìn)算法具有優(yōu)越性，同時(shí)也可以發(fā)現(xiàn)改進(jìn)算法穩(wěn)定性相對較高，故該方法有較好的實(shí)際應(yīng)用價(jià)值。

4 機(jī)械臂軌跡規(guī)劃及仿真

機(jī)械臂軌跡規(guī)劃是指機(jī)械臂在執(zhí)行任務(wù)時(shí)，對其運(yùn)動過程中各關(guān)節(jié)位移，加速度，角加速度進(jìn)行仿真分析，最后規(guī)劃出符合預(yù)期目標(biāo)的運(yùn)動軌跡。目前，軌跡規(guī)劃主要分為笛卡爾空間軌跡規(guī)劃和關(guān)節(jié)空間軌跡規(guī)劃兩類[11-13]。這兩種規(guī)劃形式各有優(yōu)缺點(diǎn)。其中，在笛卡爾空間機(jī)械臂容易出現(xiàn)奇異位置，但是可以直觀地觀察到機(jī)械臂末端執(zhí)行器運(yùn)動軌跡。對于關(guān)節(jié)空間軌跡規(guī)劃來說，其計(jì)算量相對較少，并且可以讓操作者清楚地看到各關(guān)節(jié)實(shí)時(shí)變化，但是不能應(yīng)用于存在障礙物的場合。本文針對SCR3-C30型機(jī)械臂進(jìn)行軌跡規(guī)劃，讓其依次通過預(yù)先設(shè)置的起始點(diǎn)和目標(biāo)點(diǎn)，觀察機(jī)械臂運(yùn)行狀態(tài)，但是并不需要對具體軌跡分析，且機(jī)械臂關(guān)節(jié)運(yùn)行空間內(nèi)不存在任何障礙物，所以采用關(guān)節(jié)空間軌跡規(guī)劃方式。

關(guān)節(jié)空間軌跡規(guī)劃是通過關(guān)節(jié)角度函數(shù)來描述機(jī)械臂運(yùn)動軌跡的[14]。因?yàn)槠浜饬繕?biāo)準(zhǔn)不同所以將其大致分為B樣條曲線、NURBS曲線以及多項(xiàng)式插值三類，又因?yàn)槎囗?xiàng)式插值法所用的函數(shù)存在差異，故又細(xì)分為三次、五次、梯形、S型等。相比于其他多項(xiàng)式插值法，三次和五次是將機(jī)械臂各關(guān)節(jié)角位移、角速度、角加速度分別表示為時(shí)間的不同函數(shù)，其計(jì)算量小，程序運(yùn)行時(shí)間短，而且仿真后曲線平滑，數(shù)據(jù)較為直觀。故本文采用三次多項(xiàng)式插值法和五次多項(xiàng)式插值法分別對機(jī)械臂進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃，并且通過比較其仿真結(jié)果來選擇合理的規(guī)劃方式[15]。

4.1 仿真驗(yàn)證

設(shè)空間中初始點(diǎn)與終止點(diǎn)關(guān)節(jié)值分別為：q0=[0 0 0 0 0 0]和q1=[ pi/4 pi/3 pi/2 -pi/2 -pi/3 -pi/4]，運(yùn)動時(shí)間為2 s，且運(yùn)動過程無障礙，分別采用三次多項(xiàng)式插值和五次多項(xiàng)式插值進(jìn)行軌跡規(guī)劃，如圖7～圖12所示。

圖7 三次多項(xiàng)式插值法關(guān)節(jié)角度仿真曲線 圖8 五次多項(xiàng)式插值法關(guān)節(jié)角度仿真曲線

圖9 三次多項(xiàng)式插值法關(guān)節(jié)角速度仿真曲線 圖10 五次多項(xiàng)式插值法關(guān)節(jié)角速度仿真曲線

圖11 三次多項(xiàng)式插值法關(guān)節(jié)角加速度仿真曲線 圖12 五次多項(xiàng)式插值法關(guān)節(jié)角加速度仿真曲線

4.2 仿真結(jié)果分析

通過仿真結(jié)果分析得出，機(jī)器人在仿真過程中各個(gè)關(guān)節(jié)運(yùn)行正常且無碰撞現(xiàn)象，從而驗(yàn)證了機(jī)器人模型建立以及改進(jìn)的逆解算法的正確性。并且當(dāng)使用三次多項(xiàng)式插值法和五次多項(xiàng)式插值法分別進(jìn)行機(jī)械臂軌跡規(guī)劃時(shí)，其角度(圖7、圖8)和角速度(圖9、圖10)軌跡運(yùn)行平穩(wěn)，曲線大致相同，但對于角加速度仿真曲線(圖11、圖12)可以明顯看到使用三次多項(xiàng)式插值法規(guī)劃時(shí)，其初始點(diǎn)和終止點(diǎn)數(shù)值不為零，而機(jī)械臂在運(yùn)行開始和結(jié)束時(shí)其角加速度均應(yīng)為零，說明這種插值方法規(guī)劃的不符合實(shí)際，而且還會產(chǎn)生震動，對加工造成不利的影響。而使用五次多項(xiàng)式插值法進(jìn)行規(guī)劃后，在同樣的位置機(jī)械臂角加速度為零，與實(shí)際工作情況相符, 并且角加速度曲線平滑且連續(xù)，可以降低各關(guān)節(jié)間的摩擦損耗，更好地完成預(yù)期目標(biāo)。

5 結(jié)論

本文根據(jù)D-H參數(shù)法，建立SNR3-C30型機(jī)械臂連桿坐標(biāo)系，并且基于其末端執(zhí)行器位姿矩陣特性，提出一種改進(jìn)的幾何求逆算法，該算法分兩步求解6個(gè)關(guān)節(jié)角，與傳統(tǒng)算法相比，避免了大量的矩陣運(yùn)算，提高了運(yùn)行速度，節(jié)省了程序運(yùn)行時(shí)間，并在此基礎(chǔ)上對逆運(yùn)動學(xué)多解情況以及奇異點(diǎn)位置進(jìn)行了分析，運(yùn)用相關(guān)函數(shù)對機(jī)械臂進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃。最后利用MATLAB Robotics模塊進(jìn)行仿真，仿真結(jié)果均證明了改進(jìn)算法的有效性和優(yōu)越性，并且驗(yàn)證了當(dāng)運(yùn)用五次多項(xiàng)式插值法進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃時(shí)機(jī)械臂運(yùn)行更平穩(wěn)，為今后研究機(jī)械臂實(shí)時(shí)控制奠定了基礎(chǔ)。