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

沈浩宇， 江先志

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

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

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

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

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

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

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

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

1.1 建立坐標系

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

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

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

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

1.2 機械臂正運動學分析

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

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

坐標系{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在上節已經求出，取：

(18)

得到

(19)

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

(20)

又取：

(21)

得到：

Cs2-Dc2=E。

(22)

(23)

1.3.3 求解肘關節角度θ3

由式(20)，取：

(24)

可得：

(25)

(26)

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

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

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

表2 各連桿的D-H參數和關節變量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 驗證坐標變換法的正運動學方程的正確性

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

圖5 機械臂關節插值軌跡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 驗證坐標變換法的逆運動學方程的正確性

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

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

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

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

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

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

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

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

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

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

3 機械臂樣機實驗驗證

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

3.1 制作機械臂樣機

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

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

3.2 上位機控制實驗

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

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

4 結論

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