沈浩宇， 江先志

(浙江理工大學(xué) 機(jī)械與自動(dòng)控制學(xué)院， 浙江 杭州 310000)

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

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

1 基于坐標(biāo)變換法的運(yùn)動(dòng)學(xué)分析

機(jī)器人運(yùn)動(dòng)學(xué)研究的是機(jī)器人的運(yùn)動(dòng)特性，可分為正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)。正運(yùn)動(dòng)學(xué)方程描述的是機(jī)器人末端執(zhí)行器的位姿與機(jī)器人基座之間的運(yùn)動(dòng)關(guān)系；逆運(yùn)動(dòng)學(xué)方程是正運(yùn)動(dòng)學(xué)的逆過(guò)程，是在已知末端位姿矩陣的條件下求解滿足條件的關(guān)節(jié)角。逆運(yùn)動(dòng)學(xué)求解是對(duì)機(jī)器人進(jìn)行軌跡控制的基礎(chǔ)，是機(jī)器人控制領(lǐng)域特別重要的問(wèn)題。圖1體現(xiàn)了機(jī)械臂運(yùn)動(dòng)學(xué)在整個(gè)機(jī)械臂力位控制系統(tǒng)中的作用[8]。

圖1 運(yùn)動(dòng)學(xué)在力位控制系統(tǒng)中的作用Figure 1 Role of kinematics in force position control system

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

圖2 機(jī)械臂模型Figure 2 Model of robotic arm

1.1 建立坐標(biāo)系

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

圖3 坐標(biāo)系示意圖Figure 3 Coordinate system diagram

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

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

1.2 機(jī)械臂正運(yùn)動(dòng)學(xué)分析

坐標(biāo)系{1}相對(duì)于{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)分別代表坐標(biāo)系繞其自身的X軸逆時(shí)針旋轉(zhuǎn)θ、沿著X軸移動(dòng)dx的距離；同理，在后面的章節(jié)中出現(xiàn)的其他變量的旋轉(zhuǎn)平移變換以此類推。

坐標(biāo)系{2}相對(duì)于{1}的齊次變換矩陣為：

(2)

(3)

(4)

(5)

1.3 機(jī)械臂逆運(yùn)動(dòng)學(xué)分析

1.3.1 求解肩外展/內(nèi)收運(yùn)動(dòng)角度θ1

根據(jù)式(5)有：

(6)

(7)

又有：

(8)

(9)

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

(10)

由(10)得到：

c1x-s1z=dx1+dx2+dx3。

(11)

(12)

1.3.2 求解肩前屈/后伸運(yùn)動(dòng)角度θ2

(13)

(14)

且有：

(15)

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

(16)

由式(16)得到：

(17)

因?yàn)棣?在上節(jié)已經(jīng)求出，取：

(18)

得到

(19)

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

(20)

又?。?/p>

(21)

得到：

Cs2-Dc2=E。

(22)

(23)

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

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

(24)

可得：

(25)

(26)

綜上所述，式(12)、(23)和(26)即可求出機(jī)械臂的所有逆解。在對(duì)多解進(jìn)行選擇時(shí)，首先將超出取值范圍的解舍棄，若依然存在多解，再代入正運(yùn)動(dòng)學(xué)方程驗(yàn)算后進(jìn)行取舍。

2 對(duì)坐標(biāo)變換法進(jìn)行仿真驗(yàn)證

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

表2 各連桿的D-H參數(shù)和關(guān)節(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，經(jīng)計(jì)算可得P=P′。坐標(biāo)轉(zhuǎn)換法與D-H法求得的機(jī)械臂末端坐標(biāo)一致，因此建立的仿真模型可行。

2.1 驗(yàn)證坐標(biāo)變換法的正運(yùn)動(dòng)學(xué)方程的正確性

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

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

另外，運(yùn)用函數(shù)plot()分別輸出機(jī)械臂末端點(diǎn)在坐標(biāo)系{0}的X，Y，Z不同方向上隨時(shí)間t的變化曲線。如圖6所示，2種方法得到的曲線完全重合，充分驗(yàn)證了改進(jìn)的坐標(biāo)變換法的正運(yùn)動(dòng)學(xué)方程的正確性。

圖6 機(jī)械臂不同時(shí)間各方向運(yùn)動(dòng)軌跡Figure 6 Trajectory of robotic arm in various directions at different times

2.2 驗(yàn)證坐標(biāo)變換法的逆運(yùn)動(dòng)學(xué)方程的正確性

設(shè)置機(jī)械臂執(zhí)行末端由位姿T1到位姿T2進(jìn)行變換：

采用ctraj()函數(shù)在位姿T1和T2之間進(jìn)行插值。運(yùn)用ikine()函數(shù)逆解出每個(gè)插值位姿的關(guān)節(jié)角度，其中T1到T2運(yùn)動(dòng)時(shí)間為10 s。機(jī)械臂模型的位姿插值直線軌跡如圖7所示。

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

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

圖8 機(jī)械臂不同時(shí)間各關(guān)節(jié)運(yùn)動(dòng)角度Figure 8 Motion angle of each joint of robotic arm at different times

2.3 機(jī)械臂的仿真工作空間驗(yàn)證

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

圖9 3關(guān)節(jié)機(jī)械臂的運(yùn)動(dòng)空間Figure 9 Motion space of three-joint robotic arm

將已經(jīng)生成的50 000組3關(guān)節(jié)量，代入式(27)和(28)得到基于坐標(biāo)變換法的機(jī)械臂空間云點(diǎn)圖，新的云點(diǎn)圖在圖9的云點(diǎn)圖上進(jìn)行疊加，得到如圖10所示的運(yùn)動(dòng)空間。新的云點(diǎn)完全準(zhǔn)確地覆蓋了之前基于D-H坐標(biāo)法生成的每一個(gè)云點(diǎn)，驗(yàn)證了基于改進(jìn)的坐標(biāo)變換法的正運(yùn)動(dòng)學(xué)方程的正確性。

圖10 運(yùn)動(dòng)空間驗(yàn)證Figure 10 Verification of sports space

3 機(jī)械臂樣機(jī)實(shí)驗(yàn)驗(yàn)證

編寫上位機(jī)軟件進(jìn)行機(jī)械臂樣機(jī)的控制。

3.1 制作機(jī)械臂樣機(jī)

圖11所示為系統(tǒng)結(jié)構(gòu)和機(jī)械臂樣機(jī)。上位機(jī)的控制信息通過(guò)2個(gè)配對(duì)的藍(lán)牙模塊，無(wú)線傳輸至舵機(jī)控制板，控制板將上位機(jī)的指令變成舵機(jī)的運(yùn)動(dòng)角度和速度，實(shí)現(xiàn)機(jī)械臂的控制。機(jī)械臂按照設(shè)計(jì)尺寸進(jìn)行3D打印，并運(yùn)用3個(gè)舵機(jī)作為關(guān)節(jié)。電源采用2節(jié)18650型鋰電池串聯(lián)的方式，穩(wěn)壓至5 V給舵機(jī)控制板和藍(lán)牙供電。

圖11 系統(tǒng)結(jié)構(gòu)簡(jiǎn)圖和機(jī)械臂樣機(jī)Figure 11 System structure diagram and robotic arm prototype

3.2 上位機(jī)控制實(shí)驗(yàn)

在Visual Studio 2017平臺(tái)上采用VC++進(jìn)行上位機(jī)編程，軟件界面如圖12所示，功能分為6部分。軟件中調(diào)用MSComm控件進(jìn)行串口通信，Teechart控件實(shí)時(shí)顯示各個(gè)關(guān)節(jié)的角度曲線。將關(guān)節(jié)角度信息通過(guò)藍(lán)牙無(wú)線串口傳輸給舵機(jī)控制板，實(shí)現(xiàn)機(jī)械臂的控制。通過(guò)樣機(jī)實(shí)驗(yàn)，機(jī)械臂運(yùn)動(dòng)情況與坐標(biāo)變換法所計(jì)算出的坐標(biāo)完全一致。

圖12 上位機(jī)軟件界面Figure 12 PC software interface

4 結(jié)論

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