基于坐標變換法的3關節(jié)機械臂運動學研究

沈浩宇， 江先志

(浙江理工大學 機械與自動控制學院， 浙江 杭州 310000)

在21世紀，機器人技術取得了巨大發(fā)展，機器人的應用遍及工業(yè)、農業(yè)和醫(yī)療等諸多領域。隨著行業(yè)間的深度融合，機器人應用范圍將不斷得到拓展[1-2]。對機器人運動學分析時，首先要建立機器人坐標系，最為常用的方法之一是1965年Hartenberg等在文獻[3]提到的D-H參數法。D-H參數法嚴格定義了每個坐標系的建立規(guī)則[4]，存在觀察抽象、坐標系轉換不靈活、參數容易混亂以及理論模型與實體模型出入大等缺點；另外對于2個相鄰的平行關節(jié)，傳統D-H參數法得到的齊次變換矩陣是一個奇異矩陣，無法對這部分關節(jié)進行運動學分析[5]。串聯機械臂逆運動學比較復雜，原因是求解已知末端位姿的機械臂的關節(jié)角時，存在無解或者多解現象。機械臂運動只能有一組解，若在解的選擇時出現錯誤，機械臂便會出現飛車現象[6-7]。

課題組以3關節(jié)機械臂為研究對象，對上述問題進行探索、優(yōu)化和求解，并進行了驗證。

1 基于坐標變換法的運動學分析

機器人運動學研究的是機器人的運動特性，可分為正運動學和逆運動學。正運動學方程描述的是機器人末端執(zhí)行器的位姿與機器人基座之間的運動關系；逆運動學方程是正運動學的逆過程，是在已知末端位姿矩陣的條件下求解滿足條件的關節(jié)角。逆運動學求解是對機器人進行軌跡控制的基礎，是機器人控制領域特別重要的問題。圖1體現了機械臂運動學在整個機械臂力位控制系統中的作用[8]。

圖1 運動學在力位控制系統中的作用Figure 1 Role of kinematics in force position control system

3關節(jié)機械臂由基座、大臂、小臂和腕部組成，如圖2所示?；c大臂之間由肩部的外展/內收關節(jié)、肩部的前屈/后伸關節(jié)共同連接，大臂和小臂之間由肘關節(jié)連接。

圖2 機械臂模型Figure 2 Model of robotic arm

1.1 建立坐標系

坐標變換法在3關節(jié)機械臂中建立的坐標系如圖3所示。O0,O1,O2,O3和O4分別表示各個坐標系的原點，θ1，θ2和θ3分別表示3個關節(jié)的運動角度，逆時針為正向轉動方向。坐標系{1}和固定坐標系{0}初始位姿重合，肩部的外展/內收關節(jié)的自由度由坐標系{1}繞其自身Y軸的旋轉變換實現；坐標系{2}相對于坐標{1}的X，Y和Z軸正方向偏移dx1，dy1和dz1，肩部的前屈/后伸關節(jié)的自由度由繞其自身X軸的旋轉變換實現；坐標系{3}相對于坐標{2}的X，Y和Z軸正方向偏移dx2，dy2和dz2，肘關節(jié)的自由度由繞其自身X軸的旋轉變換實現；坐標系{4}相對于坐標{3}X，Y和Z軸正方向偏移分別為dx3，dy3和dz3，O4為機械臂末端點。

圖3 坐標系示意圖Figure 3 Coordinate system diagram

在肩外展/內收運動角度θ1=0，肩前屈/后伸運動角度θ2=0，肘關節(jié)運動角度θ3=0時，機械臂的姿態(tài)正好和圖4一致。機械臂的主要技術參數如表1所示。

表1 3關節(jié)機械臂的主要技術參數Table 1 Main technical parameters of three-joint robotic arm

1.2 機械臂正運動學分析

坐標系{1}相對于{0}的齊次變換矩陣為：

式(1)中，s1=sinθ1，c1=cosθ1；同理，在后面的章節(jié)中，有s2=sinθ2,s3=sinθ3,c2=cosθ2,c3=cosθ3。另外Rot(x,θ)、Trans(x,dx)分別代表坐標系繞其自身的X軸逆時針旋轉θ、沿著X軸移動dx的距離；同理，在后面的章節(jié)中出現的其他變量的旋轉平移變換以此類推。

坐標系{2}相對于{1}的齊次變換矩陣為：

(2)

(3)

(4)

(5)

1.3 機械臂逆運動學分析

1.3.1 求解肩外展/內收運動角度θ1

根據式(5)有：

(6)

(7)

又有：

(8)

(9)

結合式(7)～(9)得到：

(10)

由(10)得到：

c1x-s1z=dx1+dx2+dx3。

(11)

(12)

1.3.2 求解肩前屈/后伸運動角度θ2

(13)

(14)

且有：

(15)

結合式(13)～(15)得到：

(16)

由式(16)得到：

(17)

因為θ1在上節(jié)已經求出，?。?/p>

(18)

得到

(19)

聯立式(17)～(19)，消去θ3，得：

(20)

又?。?/p>

(21)

得到：

Cs2-Dc2=E。

(22)

(23)

1.3.3 求解肘關節(jié)角度θ3

由式(20)，?。?/p>

(24)

可得：

(25)

(26)

綜上所述，式(12)、(23)和(26)即可求出機械臂的所有逆解。在對多解進行選擇時，首先將超出取值范圍的解舍棄，若依然存在多解，再代入正運動學方程驗算后進行取舍。

2 對坐標變換法進行仿真驗證

圖4 MATLAB中建立的3關節(jié)模型Figure 4 Three-joint model established by MATLAB

表2 各連桿的D-H參數和關節(jié)變量Table 2 D-H parameters and joint variables of each link

(27)

其中dx1=30 mm，dx2=30 mm，dx3=0 mm；dy1=5 mm，dy2=-5 mm，dy3=0 mm；dz1=-10 mm，dz2=-70 mm，dz3=-85 mm。

P=Rot(y,-90°)Rot(z,-90°)P′。

(28)

2種方法初始角度均為[0 0 0〗T，經計算可得P=P′。坐標轉換法與D-H法求得的機械臂末端坐標一致，因此建立的仿真模型可行。

2.1 驗證坐標變換法的正運動學方程的正確性

設置關節(jié)變量,由q1=[0 67.981 114.591]到q2=[-181.373 5.79 71.486]進行變換，采用jtraj()函數在這2關節(jié)位置之間進行插值，其中q1到q2運動時間為10 s。如圖5所示插值得到的數據通過函數plot(Robot,q)和函數plot3(x,y,z)進行圖形輸出。分別得到了基于D-H方法的實線軌跡和基于改進方法的虛線軌跡，其中實線的末端運動軌跡逐漸覆蓋掉了預先顯示虛線軌跡。

圖5 機械臂關節(jié)插值軌跡Figure 5 Interpolated trajectory of robot joints

另外，運用函數plot()分別輸出機械臂末端點在坐標系{0}的X，Y，Z不同方向上隨時間t的變化曲線。如圖6所示，2種方法得到的曲線完全重合，充分驗證了改進的坐標變換法的正運動學方程的正確性。

圖6 機械臂不同時間各方向運動軌跡Figure 6 Trajectory of robotic arm in various directions at different times

2.2 驗證坐標變換法的逆運動學方程的正確性

設置機械臂執(zhí)行末端由位姿T1到位姿T2進行變換：

采用ctraj()函數在位姿T1和T2之間進行插值。運用ikine()函數逆解出每個插值位姿的關節(jié)角度，其中T1到T2運動時間為10 s。機械臂模型的位姿插值直線軌跡如圖7所示。

圖7 機械臂直線插值軌跡Figure 7 Linear interpolation trajectory of robot

另外，運用plot()函數分別輸出機械臂隨時間t的各關節(jié)q1，q2和q3的角度變化曲線。如圖8所示，2種方法逆解出的關節(jié)變化曲線完全重合，充分證明了改進的坐標變換法的逆運動學方程的正確性。

圖8 機械臂不同時間各關節(jié)運動角度Figure 8 Motion angle of each joint of robotic arm at different times

2.3 機械臂的仿真工作空間驗證

工作空間是機械手末端能夠到達的空間范圍，即末端能夠到達的目標點集合。若給定末端位姿位于工作空間內，則反解存在，否則反解不存在[9-10]。根據表2中機械臂各關節(jié)變量的范圍，采用蒙特卡洛法[11-13]求取機械臂的運動空間。運用MATLAB中的函數rand()，生成50 000組3關節(jié)量，代入函數fkine()，得到50 000個末端點，生成運動空間云點圖，如圖9所示。

圖9 3關節(jié)機械臂的運動空間Figure 9 Motion space of three-joint robotic arm

將已經生成的50 000組3關節(jié)量，代入式(27)和(28)得到基于坐標變換法的機械臂空間云點圖，新的云點圖在圖9的云點圖上進行疊加，得到如圖10所示的運動空間。新的云點完全準確地覆蓋了之前基于D-H坐標法生成的每一個云點，驗證了基于改進的坐標變換法的正運動學方程的正確性。

圖10 運動空間驗證Figure 10 Verification of sports space

3 機械臂樣機實驗驗證

編寫上位機軟件進行機械臂樣機的控制。

3.1 制作機械臂樣機

圖11所示為系統結構和機械臂樣機。上位機的控制信息通過2個配對的藍牙模塊，無線傳輸至舵機控制板，控制板將上位機的指令變成舵機的運動角度和速度，實現機械臂的控制。機械臂按照設計尺寸進行3D打印，并運用3個舵機作為關節(jié)。電源采用2節(jié)18650型鋰電池串聯的方式，穩(wěn)壓至5 V給舵機控制板和藍牙供電。

圖11 系統結構簡圖和機械臂樣機Figure 11 System structure diagram and robotic arm prototype

3.2 上位機控制實驗

在Visual Studio 2017平臺上采用VC++進行上位機編程，軟件界面如圖12所示，功能分為6部分。軟件中調用MSComm控件進行串口通信，Teechart控件實時顯示各個關節(jié)的角度曲線。將關節(jié)角度信息通過藍牙無線串口傳輸給舵機控制板，實現機械臂的控制。通過樣機實驗，機械臂運動情況與坐標變換法所計算出的坐標完全一致。

圖12 上位機軟件界面Figure 12 PC software interface

4 結論

課題組運用坐標變換法完成了對3關節(jié)機械臂模型的正、逆運動學分析和運動空間的計算。同時采用D-H參數法，在MATLAB的Robotics Toolbox中搭建機械臂模型完成了對坐標變換法的驗證。坐標變換法根據關節(jié)之間的相對位置關系，利用平移和旋轉的齊次變換矩陣將坐標系建立在關節(jié)上或其他合適的位置，坐標軸的方向靈活選取。相比于D-H法觀察抽象、坐標系轉換不靈活、參數容易混亂以及所建立的連桿模型與實體模型出入大的缺點，坐標變換法具有靈活方便、形象直觀以及便于計算的優(yōu)點，且彌補了傳統D-H法在分析平行關節(jié)時的缺陷。