用平面二連桿機(jī)器人為例貫穿運(yùn)動(dòng)學(xué)、雅可比、動(dòng)力學(xué)、軌跡規(guī)劃甚至控制與編程分析

1、一、平面二連桿機(jī)器人手臂運(yùn)動(dòng)學(xué)平面二連桿機(jī)械手臂如圖1所示，連卞f1長(zhǎng)度li,連日f2長(zhǎng)度12。建立如圖1所示的坐標(biāo)系，其中，（Xo，y0）為基礎(chǔ)坐標(biāo)系，固定在基座上，（Xi,yi）、（X2,y2）為連體坐標(biāo)系，分別固結(jié)在連桿1和連桿2上并隨它們一起運(yùn)動(dòng)。關(guān)節(jié)角順時(shí)針為負(fù)逆時(shí)針為正。yopB2CXo圖1平面雙連桿機(jī)器人示意圖1、用簡(jiǎn)單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程連桿2末段與中線交點(diǎn)處一點(diǎn)P在基礎(chǔ)坐標(biāo)系中的位置坐標(biāo)：xp=11cos%12cos（吊B2）（1）yp=11sin12sin（%朋）2、用D-H方法建立運(yùn)動(dòng)學(xué)方程假定Zo、z1、Z2垂直于紙面向里。從（Xo,yo,Zo）到（X1,y1，

2、乙）的齊次旋轉(zhuǎn)變換矩陣為:cos日-sin1OOosin4cos91OO1T=（2）O010_0001_從（Xi,y1，乙）到（X2,y2,Z2）的齊次旋轉(zhuǎn)變換矩陣為:cos-2-sin%O111sin日2cos*OO2T=（3）0010-00011從（Xo，yo,2。）到（X2,y2,z2）的齊次旋轉(zhuǎn)變換矩陣為:2T=：T2T=cossin0I0-sin力cosi0000i00cos與sinu200cos。12)sin(01+%)0I0-sin(由隊(duì))cosQ12)0000i0-sin%cosu2001icos1isind0i00i0lil00i(4)那么，連桿2末段與中線交點(diǎn)處一點(diǎn)P在基礎(chǔ)

3、坐標(biāo)系中的位置矢量為:0P=0T2P=cosR12)sin(q+82)I0I0-sinQ12)cos(二2)0000i011c0slil21isini0i0|0即,11c0sli12cos(即1isin6i+12sin(0iI0Ii叫)十%)-xp!(5)xp二11cos%12cos(i%)yp=1isiniLsinCiZ)(6)與用簡(jiǎn)單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程(i)相同。建立以上運(yùn)動(dòng)學(xué)方程后，若已知個(gè)連桿的關(guān)節(jié)角乙、%,就可以用運(yùn)動(dòng)學(xué)方程求出機(jī)械手臂末端位置坐標(biāo)，這可以用于運(yùn)動(dòng)學(xué)仿真。3、平面二連桿機(jī)器人手臂逆運(yùn)動(dòng)學(xué)可以用運(yùn)動(dòng)學(xué)方程求出機(jī)械手臂建立以上運(yùn)動(dòng)學(xué)方程后，若已知個(gè)機(jī)械臂的末端位

4、置,二連桿的關(guān)節(jié)角4、02,這叫機(jī)械臂的逆運(yùn)動(dòng)學(xué)。逆運(yùn)動(dòng)學(xué)可以用于對(duì)機(jī)械臂關(guān)節(jié)角和末端位置的控制。對(duì)于本例中平面二連桿機(jī)械臂，其逆運(yùn)動(dòng)學(xué)方程的建立就是已知末端位置(xp,yp)求相應(yīng)關(guān)節(jié)角4、%的過程。推倒如下。(i)問題xp=1icos912cos(9B2)yp=1isin%LsinGi%)已知末端位置坐標(biāo)(xp,yp),求關(guān)節(jié)角日仆可。pp由(6)式得到:(Xp-licos)2(ypliSin%)2=l；整理得到:22XpVpli,2一l2=2l1(xp0081ypSin)Xp,=tgiVp由(8)式得到:sin1Pp00slpp2xp222ypl1-122liXp(cos/cos入sin

5、sin")pp，cos?p2XP222yp1-122liXpcosi-7)cos1pp(8)(9)(10)由此可解出仇。Xp+yp+li2-l2=arccos2liXpcos3卜arctg"Xp(ii)(3)求-2由(6)式得到：Xp-12cos口z)2yp-l2sinCi矗)2f2整理得到：Xpypl2-li2=2l2XpcosQiz)ypsinQiz)Xpyp=tg%sin%cospp(I3)(I4)由(I4)式得到：Xpypl2-li2cosOi2)cospsinfiZTin%cos-pp2l2Xpcos-pp(I5)由此可解出日2。cos(瑪121p)2=arcco

6、s212xpypcosBp+arctg四xp二、平面二連桿機(jī)器人手臂的速度雅可比矩陣速度雅可比矩陣的定義：從關(guān)節(jié)速度向末端操作速度的線性變換。人為例推導(dǎo)速度雅可比矩陣。(16)現(xiàn)已二連桿平面機(jī)器xp=11c0sll2cos12)yp=l1sin112sin(%)上面的運(yùn)動(dòng)學(xué)方程兩邊對(duì)時(shí)間求導(dǎo)，得到下面的速度表達(dá)式:dXp"dTdyp"dT-l1sin。一12sin(x12)(丁-2)=l1cos1z12cos(f肌)G，)把上式寫成如下的矩陣形式：Xp.|-11sin入一12sin(時(shí)？2)l2sin(“)1%Lp111cos“+l2cos(4+82)12cos(&

7、+%)1e2令上式中的末端位置速度矢量:XpXX*p一(17)(18)關(guān)節(jié)角速度矢量I1|一IisinF-12sin(F丁2)矩陣|IicosN12cos。z)一I2sin(F2)12cos1，)J0i2)J(4,4)就是速度雅可比矩陣，實(shí)現(xiàn)從關(guān)節(jié)角速度向末端位置速度的轉(zhuǎn)變。(18)式可以寫成：X=J(Ui2)。速度雅可比矩陣可以進(jìn)一步寫成：、廠.、lsin%LsinQ隊(duì))IzsinQ%)JCjF)二.Iicos-1I2cos(-1-2)I2cosp1-2)一-(19)1 .J11J12=I21J221其中，xpJ11=-11sin一l2s1ndi12)“1XpJ12=12sin(i'

8、-2)"2J21yp(20)J22"1-:Yp2=11cosj12cos(f二12cosQ12)%)('cXpEXp_J11J12e9J)=11=12J21J22gypQp1曾石。21、平面二連桿機(jī)器人手臂的動(dòng)力學(xué)方程由此可知雅可比矩陣的定義:)推倒動(dòng)力學(xué)方程的方法很多，各有優(yōu)缺點(diǎn)。拉格朗日方法思路清晰、不考慮連桿之間的內(nèi)力，是推倒動(dòng)力學(xué)方程的常用方法。下面推導(dǎo)圖1所示的平面雙連桿機(jī)器人的動(dòng)力學(xué)方程。圖1中所示連桿均為均質(zhì)桿，其轉(zhuǎn)動(dòng)慣量分別是I1和|21、求兩連桿的拉格朗日函數(shù)(1)求系統(tǒng)總動(dòng)能連桿1的動(dòng)能為：凸與出2”；=111汨12236(21)求連桿2質(zhì)心D處

9、的線速度：對(duì)連桿2質(zhì)心位置求導(dǎo)得到其線速度。連桿2質(zhì)心位置為：XD1二11cos12cosg22)21二11sin912sin(1-匕)2(22)連桿2質(zhì)心速度為:Xd-11sin1-112sin(1%)(丁，)2(23)Yd=11cos111112cos(-1-2)(-1-2)2222212212T2/12Vd=XdyDu(11-121112cos2p1二1212(二121112cos12尸尸2442(24)連桿2的動(dòng)能:1 .212K2=ID(內(nèi)肌)m2VD2 21 ,1.22121.221.2.2,1.2(m2l2)(1u2)m2Klil21112cos12all22(l21112cos

10、r2Hl22 1224421 21.221,221,2,2=-m2(l1l21112cos冬)。m2l212m2(121112coslAll2 3623(25)系統(tǒng)總動(dòng)能：K=K1K21,.21.221.2.21,2.2=-m2(11-121112cos引-m212；,5m2(§121112cosi2"f21 .21.21.2121.22,1.21二(m211m111m212m21112cos%)m21；u2(m212m21112cos明)、22 662632(26)(2)求系統(tǒng)總勢(shì)能系統(tǒng)總勢(shì)能為：1.1.P=m1glisin%m2g(11sin1-12sin(%)(3)求

11、拉格朗日函數(shù)(27)L=K-P二(1m21；1ml1；26121,212.2121m212m21112cos12)%m212%(m212m21112cos%)中2626321-m1g11sin-m2g11sina112sin(1r)2(28)(4)列寫動(dòng)力學(xué)方程按照拉格朗日方程，對(duì)應(yīng)關(guān)節(jié)1、2的驅(qū)動(dòng)力矩分別為:.L：L-1二-ac口cQ:-1:-1.L.L2二-32工(29)-L21,21,2-二(m211二mJ-m212.可33.121m21112cos2尸1(-m212-m21112cos2尸232L21,21,21,21=(m211m111m212m21112cos內(nèi)(m212m2111

12、2cosi2)f23tl333212m21112sin-21-2m21112sin22:L2-1=(m2112,1、,1,、-(mim2)gl1cos1m2g12c0sqi12)221,21,2一、J,21,.、.m111m212m21112cos%)z(m212m21112cos12)%3332.12,1、，,1,、m21112sin12n12m21112sinn(m1m2)g11cosZm2g12cos(112)222（30）同理：:L12,121=-m21212(m212m21112cos2)332；:L121.12.1=(m212,m21112cos%)%m21212-m21112si

13、n121%t.33232:L1.21.1二一-m21112sin%m21112sin2耍2m2g12cos(川口2)f2222,1,21一.、.1,2.1一.21,八.、2=(-m212m21112cosi2)Zm212u2-m21112sin%m2g12cos(-1二2)32322(31)聯(lián)合（30）、（31）式，將動(dòng)力學(xué)方程寫成如下矩陣形式:m12工訕=3211,21.2.m111m212m21112cos12331,21,m212m21112cos大321m21；31,+m21112cos%21.2二m2123112話Li,.Hm21112sin2.01.-m21112sin%2-m21

14、112sin021 11(-m1+m2)g11cos日1+一m2g12cos。+%)2 21m2g12cos(81+4).21(32)四、平面二連桿機(jī)器人手臂的軌跡規(guī)劃軌跡規(guī)劃就是已知起點(diǎn)和終點(diǎn)的位置速度加速度等參數(shù)確定中間點(diǎn)的相應(yīng)參數(shù)的過程。軌跡規(guī)劃是機(jī)器人完成規(guī)定任務(wù)所必需的。它分為關(guān)節(jié)空間的軌跡規(guī)劃和直角坐標(biāo)空間的軌跡規(guī)劃、以及基于動(dòng)力學(xué)的軌跡規(guī)劃等幾種類型。關(guān)節(jié)空間的軌跡規(guī)劃就是已知某連桿起點(diǎn)和終點(diǎn)的角位置角速度角加速度等參數(shù)確定中間點(diǎn)的相應(yīng)參數(shù)的過程。如圖所示，一兩自由度機(jī)械手，已知兩連桿起點(diǎn)和終點(diǎn)的關(guān)節(jié)角,確定中間位置的關(guān)節(jié)角。（1）非歸一化和歸一化問題（2）末端位置的軌跡、關(guān)節(jié)空

15、間軌跡規(guī)劃的缺點(diǎn)。腳自由度機(jī)器人美審空間的非舊化注動(dòng)兩自由度機(jī)器人關(guān)節(jié)空間的歸一化運(yùn)動(dòng)三次多項(xiàng)式軌跡規(guī)劃要求：機(jī)器人上某關(guān)節(jié)在運(yùn)動(dòng)開始時(shí)刻片的角度為鳥，在時(shí)刻口運(yùn)動(dòng)到角度%.用三次多項(xiàng)式規(guī)劃該關(guān)節(jié)的運(yùn)動(dòng)*優(yōu)分=%+卬+7產(chǎn)+夕初始條件與末端條件;目=4.加）=6'事&力=%»遍=0*確定系數(shù).勺J,以確定我已啟澇（0的表達(dá)式a卡舉例：要求一個(gè)5軸機(jī)器人的第一關(guān)節(jié)在5秒之內(nèi)從初始角3。度運(yùn)動(dòng)到終端角75度，用三次多項(xiàng)式計(jì)算在第1、2、3、4秒時(shí)的關(guān)節(jié)角。磯L）=c0=30ftfl-30叩4二。+蠅5）+工?。?0）-75-g產(chǎn)。前士）C00工=5-4國(guó)力皿4+2cM5）

16、+女6）=01口=S由此得到位置速度和加速度的多項(xiàng)式方程如下：岫=30+5Aa-072?財(cái)=10L&-2,16?fl（/）=10,fi-432/代人時(shí)間求得：刈1）=34.68*&=必網(wǎng)”網(wǎng)3）=5916D磯%=7也32"圖5.1U例5.1中的關(guān)節(jié)位置、速度和加速度五次多項(xiàng)式軌跡規(guī)劃已知機(jī)器人上某關(guān)節(jié)在運(yùn)動(dòng)開始時(shí)亥以=0的角度為月、速度&、加速度在時(shí)刻運(yùn)動(dòng)到庭I,度斗、速度為，八加速度為,。要求用五次多項(xiàng)式規(guī)劃該關(guān)節(jié)的運(yùn)動(dòng)口8（，）=c.+cl+g/2+cjP+a/"+c#U1Z54+J初始條件與末端條件±vs（G=&、改右）=。、

17、我幻=孱/innnai日&）二斗£日7）-0=&/+-1確定系數(shù)/、”與、今cPc5?以確定日（t）、3g前t）的表達(dá)式口M同例5.1,且巳知初始加速度和末端減速度均為5度/秒2c解：由例5和給出的加速度值得到：心=3（re-o度/秒a=5度/秒2%=75"&=0度/秒%=7度/杪2將初始和末端邊界條件代入式（5.5）式（5.7）,得出：5=30C=0Cy-25Cj=1.6g=-0-58c5=0.0464進(jìn)而得到如下運(yùn)動(dòng)方程：ff（t）=30+2.5t2+1.6戶-0.5S?+0,0464/期-5r+4.8戶-232-+0.232/4=5+9&

18、;-6.96r1+5928.圖5.12是機(jī)器人關(guān)節(jié)的位置，速度和加速度曲線，其最大加速度為&7度/秒，圖5.12例5.3的關(guān)節(jié)位置，速度和加速度拋物線過渡的線性運(yùn)動(dòng)軌跡規(guī)劃(略)具有中間點(diǎn)以及用拋物線過渡的線性運(yùn)動(dòng)軌跡規(guī)劃(略)高次多項(xiàng)式運(yùn)動(dòng)軌跡規(guī)劃(略)直角坐標(biāo)空間的軌跡規(guī)劃(1)所有用于關(guān)節(jié)空間的軌跡規(guī)劃方法都可以用于直角坐標(biāo)空間軌跡規(guī)劃；(2)直角坐標(biāo)軌跡規(guī)劃必須不斷進(jìn)行逆運(yùn)動(dòng)學(xué)運(yùn)算，以便及時(shí)得到關(guān)節(jié)角。這個(gè)過程可以歸納為以下計(jì)算循環(huán)：(a)將時(shí)間增加一個(gè)增量；(b)利用所選擇的軌跡函數(shù)計(jì)算出手的位姿；(c)利用逆運(yùn)動(dòng)學(xué)方程計(jì)算相應(yīng)的關(guān)節(jié)變量；(d)將關(guān)節(jié)變量信息送給控制器；(e)返回到循環(huán)的開始。圖57具有加速和感速段的軌跡規(guī)劃例5.6一個(gè)兩自由度平面機(jī)器人要求從起點(diǎn)（3,0）沿直線運(yùn)動(dòng)到終點(diǎn)（8,14）0假諛?biāo){徑分為10段，求出機(jī)器人的關(guān)節(jié)變量每一根連桿的長(zhǎng)度為9英寸，解：直角坐標(biāo)空間中起點(diǎn)和終點(diǎn)間的直線可以描述為:或者y=O.8bc+7.6中間點(diǎn)的坐標(biāo)可以通過將起點(diǎn)和終點(diǎn)的工、y坐標(biāo)之差簡(jiǎn)單地加以分割得到，然后通過求解逆運(yùn)動(dòng)學(xué)方程得到對(duì)應(yīng)每個(gè)中間點(diǎn)的兩個(gè)關(guān)節(jié)的©結(jié)果見表5和圖547口表5.1例5.6的坐