6R工業機器人幾何求逆優化算法及仿真分析*

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

(中北大學山西省先進制造技術重點實驗室，太原 030051)

0 引言

近些年，工業機器人發展非常迅速，已經成為了我們日常生活中不可或缺的一部分。工業機器人主要通過各關節的連續運動來控制末端執行器完成各種復雜工作，所以對其進行運動學分析和軌跡規劃是非常必要的。軌跡規劃是機器人平穩運行的先決條件，通過對其各關節角位移、角速度以及角加速度的規劃，將有效降低機械臂運動時產生的震動以及速度突變，并以此來提升運動的穩定性和精確性。而運動學分析作為軌跡規劃的基礎，通?？梢苑譃檎\動學和逆運動學兩個部分[1-2]。其中逆運動學是正運動學的逆解，并且其能否準確快速求解對機器人實時控制起著至關重要的作用。

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

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

1 機械臂D-H建模

機械臂是由一系列連桿通過移動和轉動關節串聯而成的，對于不同型號的機械臂來說，每個連桿長度不同，其關節平移或旋轉的范圍也不同。Denavit和Hartenberg按照連桿間的相對位置，提出為每個連桿建立一個坐標系[8]。確立坐標系時需要遵循右手定則，且通過齊次變化矩陣來描述機械臂各關節連桿間的相對運動關系。

1.1 坐標系與參數的確立

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

圖1 相鄰兩連桿坐標系

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

(1)

1.2 機械臂D-H參數模型

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

圖2 SNR3-C30機械臂三維模型 圖3 SNR3-C30型機械臂連桿坐標系

通過測量得到SNR3-C30型機械臂D-H參數如表1所示。

表1 SNR3-C30型機械臂D-H參數

2 機械臂運動學分析

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

2.1 運動學正解

正運動學是通過機械臂初始給定的各個關節角度來求解其末端執行器的具體位姿，且該位姿具有唯一性。

(2)

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

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

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 運動學逆解

逆運動學是通過初始給定的機械臂末端執行器位姿來求解當前各個關節角度，且解不具有唯一性。末端執行器位姿矩陣為：

其中，R表示末端執行器所在坐標系相對基坐標系的旋轉矩陣，P為笛卡爾空間中的位置矩陣。改進算法將旋轉矩陣與位置矩陣從位姿矩陣中分離，利用位置矩陣結合幾何法單獨求解前三個關節角θ1,θ2,θ3，然后在此基礎上通過關節連桿4、5、6的旋轉矩陣來求解剩余關節角。

2.2.1 求解前3個關節角θ1,θ2,θ3

圖4 機械臂連桿幾何位置關系

由圖4可得機械臂位置關系函數表達式為：

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

由機器人腕部參考點PW在XOY面上的投影得：

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

(2)求解θ2

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

(3)求解θ3

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

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

解得：θ3=

atan(a3,d4)-θ2

2.2.2 求解后3個關節角θ4,θ5,θ6

相鄰兩連桿間旋轉矩陣為：

由旋轉矩陣正交性質可得：

(6)

(1)求解θ5

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

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

(2)求解θ4

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

解得：

①當0<θ5≤120° 時：

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

②-120°≤θ5<0 時：

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

(3)求解θ6

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

解得：

①當0<θ5≤120° 時：

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

②-120°≤θ5<0時：

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

綜上所述，θ1,θ2,θ3,θ4,θ5,θ6的可行解全部求出，由于θ1,θ3,θ5,都存在兩個解的情況，通過組合，可行解最終有8組，并且關節角θ5不能為0，否則關節連桿4、6軸線重合，此時機械臂位于奇異點，機械臂系統報錯，停止運行。

3 機械臂仿真分析

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

3.1 機械臂運動學建模

在Robotics Toolbox模塊中調用link函數，建立機械臂仿真模型，并用plot函數顯示其三維空間模型[10]。機械臂6個關節變量分別為[0 0 0 0 pi/2 0]時，其三維空間仿真模型如圖5所示。

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

3.2 機械臂正運動學仿真

假設SNR3-C30型工業機器人的6個關節角度分別為0、pi/2、pi/3、0、-pi/2、0，它們共同組成一個向量q=[0 pi/2 pi/3 0 -pi/2 0]，利用fkine函數得到末端執行器位姿矩陣T為：

(7)

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

3.3 機械臂逆運動學算法驗證及仿真

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

圖6 改進算法與幾何法、逆乘法運算速度比較

從圖中可以得出幾何法、逆乘法以及改進算法平均求解時間分別為2.062 s，2.16 s,1,981 s,改進算法比幾何法運算速度提高了3.92%，比逆乘法提高了8.29%，說明改進算法具有優越性，同時也可以發現改進算法穩定性相對較高，故該方法有較好的實際應用價值。

4 機械臂軌跡規劃及仿真

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

關節空間軌跡規劃是通過關節角度函數來描述機械臂運動軌跡的[14]。因為其衡量標準不同所以將其大致分為B樣條曲線、NURBS曲線以及多項式插值三類，又因為多項式插值法所用的函數存在差異，故又細分為三次、五次、梯形、S型等。相比于其他多項式插值法，三次和五次是將機械臂各關節角位移、角速度、角加速度分別表示為時間的不同函數，其計算量小，程序運行時間短，而且仿真后曲線平滑，數據較為直觀。故本文采用三次多項式插值法和五次多項式插值法分別對機械臂進行關節空間軌跡規劃，并且通過比較其仿真結果來選擇合理的規劃方式[15]。

4.1 仿真驗證

設空間中初始點與終止點關節值分別為：q0=[0 0 0 0 0 0]和q1=[ pi/4 pi/3 pi/2 -pi/2 -pi/3 -pi/4]，運動時間為2 s，且運動過程無障礙，分別采用三次多項式插值和五次多項式插值進行軌跡規劃，如圖7～圖12所示。

圖7 三次多項式插值法關節角度仿真曲線 圖8 五次多項式插值法關節角度仿真曲線

圖9 三次多項式插值法關節角速度仿真曲線 圖10 五次多項式插值法關節角速度仿真曲線

圖11 三次多項式插值法關節角加速度仿真曲線 圖12 五次多項式插值法關節角加速度仿真曲線

4.2 仿真結果分析

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

5 結論

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