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

1、一、平面二連桿機(jī)器人手臂運(yùn)動(dòng)學(xué)平面二連桿機(jī)械手臂如圖1所示，連桿1長(zhǎng)度11 ,連桿2長(zhǎng)度丨2。建立如圖1所示的坐標(biāo)系，其中，（Xo，y）為基礎(chǔ)坐標(biāo)系，固定在基座上，（X1,yj、（X2,y2）為連體坐標(biāo)系，分別固結(jié)在連桿1和連桿2上并隨它們一起運(yùn)動(dòng)。關(guān)節(jié)角順時(shí)針為負(fù)逆時(shí)針為正。圖1平面雙連桿機(jī)器人示意圖1、用簡(jiǎn)單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程連桿2末段與中線交點(diǎn)處一點(diǎn)P在基礎(chǔ)坐標(biāo)系中的位置坐標(biāo):(1)Xp =丨1 C0S1 丨 2COS（ “2）yp =丨1 sin 哥 12Sin（R寸2）2、用D-H方法建立運(yùn)動(dòng)學(xué)方程(2)假定zo、Z1、Z2垂直于紙面向里。從（Xo,yo,Zo）到（羽1,乙

2、）的齊次旋轉(zhuǎn)變換矩陣為:-cosd-si nd0011t =sin qcos0000100001 一從（X1, y1, zO到（X2, y2,Z2）的齊次旋轉(zhuǎn)變換矩陣為：cosi s ini 0 丨11si n 日2cos 日 20 0J =（ 3）001 0-000 1 一從（X0，y,Z0）到（X2,y2,Z2）的齊次旋轉(zhuǎn)變換矩陣為：2t =0t 2t 二co1-sin 30 0-cosB2_sinT20Usi ndco眇0 0sin 日 2cos 日 200001 00010.000 1_0001一_cos(也+ 日 2)-sin( +日,2）011 cossi n(d十日2）cos(d

3、 +日2)0l1 si n00101 0001那么，連桿2末段與中線交點(diǎn)處一點(diǎn) P在基礎(chǔ)坐標(biāo)系中的位置矢量為:0p =0cos(哥 v2)si n但 1 + 日2)I0-si n(円v2)cos()i丁2)000010hcosyl2l1 si01010丄(5)即,Xpl1 sind +l2 sin但 1 十日2)yp0Zp- 1 一1 11 一I01 cosy l2 cos1= hcosK bcosG= 1sin 十 12sin(可 丁2)=2)(6)與用簡(jiǎn)單的平面幾何關(guān)系建立運(yùn)動(dòng)學(xué)方程（1）相同。建立以上運(yùn)動(dòng)學(xué)方程后，若已知個(gè)連桿的關(guān)節(jié)角二1、二2，就可以用運(yùn)動(dòng)學(xué)方程求出機(jī)械手臂末端位置坐標(biāo)

4、，這可以用于運(yùn)動(dòng)學(xué)仿真。3、平面二連桿機(jī)器人手臂逆運(yùn)動(dòng)學(xué)建立以上運(yùn)動(dòng)學(xué)方程后，若已知個(gè)機(jī)械臂的末端位置，可以用運(yùn)動(dòng)學(xué)方程求出機(jī)械手臂 二連桿的關(guān)節(jié)角 齊、二2，這叫機(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é)角 齊、r2的過(guò)程。推倒如下。（1）問(wèn)題Xp = I1 cos弓 12cos（弓專）yp =l1 si nt 12Si n（t如已知末端位置坐標(biāo)（xp,yp），求關(guān)節(jié)角 片、二2。（2）求齊由（6）式得到:2 2 2(Xp-liC0S3) (ypjsi門切 J整理得到:2 2

5、 2 2xp yp l112 /(XpCOSR ypsin“)ypsin pCOS dp(8)(9)由（8）式得到:2 2. 2. 2xpyph122liXp(cos cospsin sinp)cos dp2 2 2 2Xp yp li -I22IiXpcos %COS - r)（10）由此可解出韋。弓二 arccosx： +y： +l； i；2liXpcos 1 + arctg 如Xp(11)(3)求-2由(6)式得到：(12)(13)Xp2 cosQi 匕)2 yp2 sin(y E)2 =li2整理得到：sin珀 cos%(14)Xpy： I； - I； =2l2XpCos(6R) yp

6、Sin-)Xp=tg，yp由(14)式得到：Xp yp lj-l；氾cos(3 llcosdp si n( Ds in 入(15)cosf pcosG =2 - = p)COS dp由此可解出去。=arccosx2+y2+l2-h2a 1cos 0 p2l2Xpy p arctg 片xp二、平面二連桿機(jī)器人手臂的速度雅可比矩陣速度雅可比矩陣的定義：從關(guān)節(jié)速度向末端操作速度的線性變換。人為例推導(dǎo)速度雅可比矩陣。xp = l1 cosy l2COS（N v2）yp = l1 sin 耳 12sin（刁 v2）上面的運(yùn)動(dòng)學(xué)方程兩邊對(duì)時(shí)間求導(dǎo)，得到下面的速度表達(dá)式：（16）現(xiàn)已二連桿平面機(jī)器dxpdy

7、p二 l1 cos3 I2cos（k v2）（円 6）（17）-l1 sin % l2sin（円v2）（円6）把上式寫成如下的矩陣形式:Xp lisi n： 從 一cosr _l2sin（R v2） l2cos（丁r2）-l2 sin（d +日2）1日1I2 cos +鶏）仏（18）令上式中的末端位置速度矢量和；ypj關(guān)節(jié)角速度矢量矩陣TW1 一以咱 如-Ssg 如（卄2） lUiCOSNI2COSU1二2） l 2 COS何二2）JU門2）就是速度雅可比矩陣，實(shí)現(xiàn)從關(guān)節(jié)角速度向末端位置速度的轉(zhuǎn)變。（18）式可以寫成：X十2）心速度雅可比矩陣可以進(jìn)一步寫成：11 si門刊一J2Sin（刊 R）

8、 -2 si門（二1 匕）| |1 COSK I2COSG 二2） I2COSQ1 二 2）J12J 22J11IL j 21（19）其中,cXpJ11 二 二 li sin * 一I2 sin（r ：爲(wèi)）SXp.J1212 sin（i ：；，-.2）釦J21l1 COSE l2 C0S（3j2）1J22l2 COS（刊V2）.-2由此可知雅可比矩陣的定義：（20）Jg）.J11.J 21Jl2J 22-：Yp01cXp 1如賀2三、平面二連桿機(jī)器人手臂的動(dòng)力學(xué)方程推倒動(dòng)力學(xué)方程的方法很多，各有優(yōu)缺點(diǎn)。拉格朗日方法思路清晰、不考慮連桿之間的 內(nèi)力，是推倒動(dòng)力學(xué)方程的常用方法。 下面推導(dǎo)圖1所示

9、的平面雙連桿機(jī)器人的動(dòng)力學(xué)方程。圖1中所示連桿均為均質(zhì)桿，其轉(zhuǎn)動(dòng)慣量分別是I1和|21、求兩連桿的拉格朗日函數(shù)（1 ）求系統(tǒng)總動(dòng)能連桿1的動(dòng)能為：1Q =2崇122 （ 21）= 1（1m1l12p12 =1 m1l12122 36求連桿2質(zhì)心D處的線速度：對(duì)連桿2質(zhì)心位置求導(dǎo)得到其線速度。 連桿2質(zhì)心位置為:xD 二 l1 cos1l2COS 丁2 ）（22）2yDTsiz尹哨如連桿2質(zhì)心速度為：Xd - -hsi nt 已 一 in （片 1）（弓比）2（ 23）Yd * Cost 已一J COS（3 D m）2*4*4*4V； =xD +yD =（h2 +T； +l1l2Cos82）d2

10、+（T； +H2COS 日 2）巧日 2442（24）連桿2的動(dòng)能:1 2 1 2K2 IdGi r)2 m2vD=1 (丄 m2l；)( r v2 )2 m2 (l2 1| hl2 cosr)寸-l|-2 ( 12 hl 2 cosv2 Hu 2 1224421 21 2212212 2m2(liI2 I1I2 co2p1口2丨2二2m2( I2 hLcos)吋22 3623(25)系統(tǒng)總動(dòng)能：K = K1 K21 21 2212 212 2m2(l-! -l2 l1l2 co2p1m2l2%m2( l2 l1l2cos2p22 36231 2 1 2 1 2 1 2 1 2 2 1 2 1

11、-(m2l1m1l1m2l2m2l1l2cos21m2l2 ( m2l2m2l1l2cos2 2(26)(2)求系統(tǒng)總勢(shì)能系統(tǒng)總勢(shì)能為：P=1gghsi1l2sin(r 七)(27)(3 )求拉格朗日函數(shù)L 二 K -P1 2121=(m2l1m1l1m2*2 6 611 2 1 2 m2l1l2COSR)眉口2丨2二2 61l2 sin(K乙)2(-m2l；31m2 hl 2 cost 2)斗2(28)(4 )列寫動(dòng)力學(xué)方程按照拉格朗日方程，對(duì)應(yīng)關(guān)節(jié)1、2的驅(qū)動(dòng)力矩分別為:,:Ld cL2(29):L=血112 3m11m2l|3 2 2- 1 2 1 m2l1l2 co2( m2l；m2l

12、1l 2 cos2232.:Li=(m2l1212 1 2 12 1m2l2m2l1l2 co2k1 ( m2l2m2hl2 cosr2)二23332-m?l 1122sin 二2巧2m2l1l2 sin 些22.L11-( m m2)gl1cosi1m2gl2 cos v2)2-1 2 1 2 1m2l2 m2hl2 cos v2)( m2l2m2l1l2 co22332- 1 2 1 1-m2l1l2sin 丁2刊 丁2m2l1l2sin 丁2丁2 ( m1m2)gl1 cosm2gl2cos(r 丁2)2 2 2(30)同理:(1 m2l2231m2l112 cos2 k12.1. 2-

13、1 mdil22 COS K 門 1 2 h m212 】231mhhl2sin R*221 . 1m2l1l2 sin J2冇m2gl2cos 2)2 2121 121-212=( m2l2m2l1l2cos2y1m2l2*m2l1l2sin)2m2gl2 cos(-1v2)32322(31)聯(lián)合（30）、（ 31）式，將動(dòng)力學(xué)方程寫成如下矩陣形式:m2l121m1l123 1 1 1m2l2?3 2 21 2m2l2 m2l1l2 cos2311 m2l；3 2 21+ - m2l112 cos日21 m2l|3 2 2_- 15 2 一le-J0m2l1l2 sin 日21m2l1l2

14、sin 日20A十；1m2l1l2 cos j2,.Hm2l1l2 sini!00門1日；+1(丁1 +m2)gl1cos 日 10齊21+?m2gl2 cos +日2)12m2gl2cos + 日2)(32)四、平面二連桿機(jī)器人手臂的軌跡規(guī)劃軌跡規(guī)劃就是已知起點(diǎn)和終點(diǎn)的位置速度加速度等參數(shù)確定中間點(diǎn)的相應(yīng)參數(shù)的過(guò)程。 軌跡規(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ù)的過(guò)程。 如圖所示，一兩自由度機(jī)械手，已知兩連桿起點(diǎn)和終點(diǎn)

15、的關(guān)節(jié)角， 確定中間位置的關(guān)節(jié)角。(1)非歸一化和歸一化問(wèn)題(2)末端位置的軌跡、關(guān)節(jié)空間軌跡 規(guī)劃的缺點(diǎn)。哺自由度機(jī)器扎犬審空同鴨非舊化込勸兩自由庭機(jī)器人關(guān)節(jié)空間的歸一優(yōu)運(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) V)= +邙+C3幾初始條杵與未端祭件：-述=2啲)=&嚴(yán)確定系數(shù)鈿兮Cj .以確定理:；id方的表達(dá)式a衛(wèi)舉例：要求一個(gè)5軸機(jī)器人的第一關(guān)節(jié)在5秒之內(nèi)從初始角30度運(yùn)動(dòng)到終端角75 度，用三次多項(xiàng)式計(jì)算在第1、2、3、4秒時(shí)的關(guān)節(jié)角。S(fj) = c0 = 30fcq - 30卿+ G+門俘珊刃-75如皿=

16、G 二。pi = 5-4鮒 q 5 +地為+女矽)=0Uj = 772由此得到位跟速度和加I速度的多頂武方程如下：&(0 = 30 + 5A1 -附=10L& - 2A6i2 fl(/)二 10.fi 432/ft人時(shí)間求得：協(xié)=34.68*袖=必開”&=59.16D。=皿妙五次多項(xiàng)式軌跡規(guī)劃已知機(jī)器人上某關(guān)節(jié)在運(yùn)動(dòng)開始時(shí)刻專=0的角度為q、速度矗、 加速度在時(shí)刻兮運(yùn)動(dòng)到舷&八 速度為6、加速度為6 要求 用五次多項(xiàng)式規(guī)劃該關(guān)節(jié)的運(yùn)動(dòng)。40t) =+cf + cf*初始條件與末端條件：*m(珀)=烤、總(鮎)=o、罠fj)=雖 !&(切=B f、日(fi = 0 i 段心、=19/ +-1確定

17、系數(shù) %巾、令 6，以確定0(t)d(t)諾(t)的表達(dá)式。W 同例51,且巳知初始加速度和隸端減速度均為5度/抄3解：由例5和給出的加速度值得到：= 30* 反0度/秒 9, = 5度/秒2坷=方 & = 0度/妙場(chǎng)=-5度/秒丄游初始和末端邊界條件代入式5,5)-式(5.7 ),得出:Co = 30Cj = 0c2 = 2.5Cj = 1.6c4 = T.58q = 0,0464進(jìn)而得到如下運(yùn)動(dòng)方程；fl(r) = 30 + 2.5? + 1.6? - 0.58r4 + 0.0464rJ強(qiáng) - Sr + 48尸-2J2tJ + 0.232?tf(f) = 5 + 9.6r -+ 0.928

18、/3圖5.12是機(jī)器人關(guān)節(jié)的位置、速度利I加速度曲線，其疑大加速度為&7度/秒J圖工12例乞3的關(guān)節(jié)位置、速度和加速度拋物線過(guò)渡的線性運(yùn)動(dòng)軌跡規(guī)劃（略）具有中間點(diǎn)以及用拋物線過(guò)渡的線性運(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è)過(guò)程可以歸納為以下計(jì)算循環(huán)：（a）將時(shí)間增加一個(gè)增量；（b）禾U用所選擇的軌跡函數(shù)計(jì)算出手的位姿；（c）利用逆運(yùn)動(dòng)學(xué)方程計(jì)算相應(yīng)的關(guān)節(jié)變量；（d）將關(guān)節(jié)變量信息送給控制器;（e）返回到循環(huán)的開始。圖眾了

19、具有加雀和減速段的勒跡規(guī)劃例5.6個(gè)兩自由度平面機(jī)篙人要求從起點(diǎn)(3+ 10)浴直線運(yùn)動(dòng)到終點(diǎn)(8, 14 )0假設(shè)弼徑分為10段，求出機(jī)器人的關(guān)節(jié)變童 每一很連桿的長(zhǎng)度為9英寸J解：直角坐標(biāo)空間中起點(diǎn)和終點(diǎn)間的Jt線可以描述為:或者y = 0.8x + 7.6中間點(diǎn)的坐標(biāo)可H通過(guò)將起點(diǎn)和終點(diǎn)的工、y坐標(biāo)之差簡(jiǎn)單地加以分劃得到，然后通過(guò)求 解逆運(yùn)動(dòng)學(xué)方程得到對(duì)應(yīng)毎個(gè)中間點(diǎn)的兩個(gè)關(guān)節(jié)角。結(jié)果見表5和圖5J7o表5.1例56的坐標(biāo)和關(guān)節(jié)角#Xy%鬲131018.810923.510.419104.03410.819.5100.444.51L220.295.85511,621.390.965.51222.58577612424.180,186.512+826742971322&267.8107.513.630.860.71181433+95