用平面三連桿機器人為例貫穿運動學(xué)雅可比動力學(xué)軌跡規(guī)劃甚至控制與編程

1、(2)連桿3長度為l3。建(X1, yj、(X2,y2)、起運動。 關(guān)節(jié)角順、平面二連桿機器人手臂運動學(xué)平面二連桿機械手臂如圖 1所示，連桿1長度li，連桿2長度12 , 立如圖1所示的坐標(biāo)系，其中，(x0,y0)為基礎(chǔ)坐標(biāo)系，固定在基座上， (x3, y3)為連體坐標(biāo)系，分別固結(jié)在連桿 1、連桿2、連桿3上并隨它們 時針為負逆時針為正。1y3pyoy2D2B2Cx1y11Xo圖1平面雙連桿機器人示意圖1、用簡單的平面幾何關(guān)系建立運動學(xué)方程連桿2末段與中線交點處一點P在基礎(chǔ)坐標(biāo)系中的位置坐標(biāo):Xpl1cos1l 2 cos( 12)+l 3 cos( 123)yphsin1l 2 sin( 1

2、2)+1 3sin( 123)(1)2、用D-H方法建立運動學(xué)方程假定Zo、Z1、Z2垂直于紙面向外。從(Xo,y°,Zo)到(為，丫1,乙)的齊次旋轉(zhuǎn)變換矩陣為:cos( 123)sin( 123)0l1 cos 1 l2 cos( 12)sin( 123) cos( 123)0l1 sin 1 l2 sin( 12)00100001即,Xp I1COS 1 I 2C0S（ 12）+l 3C0S（ 123）（ 6）yp lisin 1 I 2Sin（ 12）+13sin（ 123）COS 1sin1 00osin 1cos1001 00100001從（xyZ1）到（X2,y2, Z

3、2)的齊次旋轉(zhuǎn)變換矩陣為:cos 2sin2 0i11sin 2cos2002 00100001(3)從（X2,y2,Z2）到（X3, y3, Z3）的齊次旋轉(zhuǎn)變換矩陣為:cos 3sin 30l2；Tsin 3cos 30000100001(3)從（Xo，yo, Zo）到（X3, y3, Z3）的齊次旋轉(zhuǎn)變換矩陣為:cos 1sin 100cos 2sin 20l1sin 1cos 100sin 2cos 2000010001000010001cos 3sin 30l2sin 3cos 30000100001(4)cos( !23)sin(1 23)0 I1 cos 1I2 cos( 12)

4、I303sin( !p 0t 3p123)cos(1 23) 0h sin !l2 sin( 12)0001000001111 cos 1I2 cos( 12)I3 cos( 123)Xphsin !l2 si n( 12) lbsin( 123 )yp0Zp11那么，連桿2末段與中線交點處一點P在基礎(chǔ)坐標(biāo)系中的位置矢量為:(5)結(jié)論：（6）與用簡單的平面幾何關(guān)系建立運動學(xué)方程（1）相同。補充：正解用于仿真，逆解用于控制建立以上運動學(xué)方程后，若已知個連桿的關(guān)節(jié)角1、2、3 ，就可以用運動學(xué)方程求出機械手臂末端位置坐標(biāo)，這可以用于運動學(xué)仿真3、平面二連桿機器人手臂逆運動學(xué)二、平面二連桿機器人手臂

5、的速度雅可比矩陣 速度雅可比矩陣的定義： 人為例推導(dǎo)速度雅可比矩陣。從關(guān)節(jié)速度向末端操作速度的線性變換。現(xiàn)已二連桿平面機器XpI1 cos 1 l 2 cos( 1yp I1 sin 1 l 2sin( 12)+l 3 cos( 123)2)+13 sin( 123)上面的運動學(xué)方程兩邊對時間求導(dǎo)，得到下面的速度表達式:dXph sin 11 L sin( 1dtl1 cos 1 & l2 cos( 1 dt2) (&2) (&&) 13 si n(&) l2cos(3)3)&)&)(17)把上式寫成如下的矩陣形式:XPypl1 si n

6、 1 l2 si n(l1 cos 1 l2 cos(2)2 )l2 si n( 1 l2 cos(2)2)(18)令上式中的末端位置速度矢量Xp關(guān)節(jié)角速度矢量yp矩陣l1 sin 111 cos 1l2 si n( 112 cos(12)2)l2 sin( 112 cos( 12)2)J( 1, 2)J(2)就是速度雅可比矩陣，實現(xiàn)從關(guān)節(jié)角速度向末端位置速度的轉(zhuǎn)變。(18)式可以寫成:J( 1, 2)速度雅可比矩陣可以進一步寫成:J( 1, 2)l1l1 si n 1 l2si n( 1cos 112 cos( 12)l2 sin( 1 2)2 ) l2 cos( 1 2)J11J 21J1

7、2J22其中,(19)J11XP1l1 sin 1l2 sin( 12 )J12xpl2s in( 12)2J21yp111 cos 1l2 cos( 12 )J22ypl2 cos( 12)(20)xpxPJ 11 J 124J( 1, 2),12J 21J 22y PyP由此可知雅可比矩陣的定義:1 2(21)三、平面二連桿機器人手臂的動力學(xué)方程推倒動力學(xué)方程的方法很多，各有優(yōu)缺點。拉格朗日方法思路清晰、不考慮連桿之間的 內(nèi)力，是推倒動力學(xué)方程的常用方法。 下面推導(dǎo)圖1所示的平面雙連桿機器人的動力學(xué)方程。圖1中所示連桿均為均質(zhì)桿，其轉(zhuǎn)動慣量分別是I1和|21、求兩連桿的拉格朗日函數(shù)（1）求

8、系統(tǒng)總動能連桿1的動能為：1 2K1I a 12（21）112212 2(m1l1 ) 1m1l112 36求連桿2質(zhì)心D處的線速度：對連桿 2質(zhì)心位置求導(dǎo)得到其線速度。連桿2質(zhì)心位置為：1XdI1 COS 1 I 2 COS( 12 )22( 22)1yDI1 s in 1Dsi n( 12)2連桿2質(zhì)心速度為：Xdl1 si n 1112 s in(21l1 cos 11l2 cos(21 2 ) ( 12)（23）Yd12)(12)v；2221 . 2XDyD(l1 l24l 112 cos 2) 11 . 2 2l2 241112 cos 2 ) 1 2(24)2連桿2的動能:K2I

9、d ( 12'221/1 |2V 2(12m2l2)( 11 2 1 2m2(l1 l2231 2m?V d22 1 22)m2(l12l 11 2 COS 2 ) 11 l2_ l241 2 m2l2 61112 COS 2) 11 2(尹 l1l2COS2)12系統(tǒng)總動能:K K1 K21123l21m1l126 1 1如仙2(2m2lfl il 2 COS 2 )6皿如 2(£l；23l1l2 cos(25)(2)求系統(tǒng)總勢能 系統(tǒng)總勢能為:P 1m1gl1 sin(3 )求拉格朗日函數(shù)1 - 2m2l261m2l1l2 cos 2)1 (2223-m2l|6 2 21

10、*2 COS 2) 1(3 m2l23(26)im211l 2 cos 2 ) 1 21mbg(l1s in 1l2s in( 12)(27)L K P( m2l121 m11122 61-m2l226m1gl1 s in 1 m2gl1 s inm21112 cos 2) 121l2 sin( 12)2-m2l226(丄 m2l；3(28)(4 )列寫動力學(xué)方程按照拉格朗日方程，對應(yīng)關(guān)節(jié)1、2的驅(qū)動力矩分別為:(29)1 2 1 m1l1m2l331 2m21112 cos 2) 1( m21231 m211l 2 COS 2 ) 22同理:212(m2l1m1l13m2l1l2 sin 2

11、m2lil2 sinm2)ghcos(1m2l223】m2l； m2l1l2 cos3m?l * 2 sin 22-m2gl2cos(212m2l231 .1 2 m211l 2 sin 22m112 cos 2)2) 12)(1m2l2231m2l112 sin 22(1m2lf 1m2l1l2cos321 m2l1l2 cos2cos 2 )1m2 hl 22(1 m2*31 m2bl2cos1m?l 112 cos 2) 22/ 1 231(m1m2 )gl1 cos 121m2l23sin2) 21 m2l1l2 sin 221m2gl 2 cos(21122) 1 m2l23m2l1

12、l2 sin21m2gl2 cos(2(30)2)1m2gl2 cos( 12)(32)(31)聯(lián)合(30)、( 31)式，將動力學(xué)方程寫成如下矩陣形式：2 1 2121 21 ,m2l1m1l1m2l2 m21112 cos2m2l2m212 cos 233321211. 2m2l2m?l 112 cos 2m2l2232310m2l1l2 sin 22211 ,.2m2l1l2 sin2022m2hl2Sin 201 2(如1m2)gl1 cos 1m2gl2 cos( 12)20 0 1 2-m2gl2cos( 12)2四、平面二連桿機器人手臂的軌跡規(guī)劃軌跡規(guī)劃就是已知起點和終點的位置速

13、度加速度等參數(shù)確定中間點的相應(yīng)參數(shù)的過程。 軌跡規(guī)劃是機器人完成規(guī)定任務(wù)所必需的。 它分為關(guān)節(jié)空間的軌跡規(guī)劃和直角坐標(biāo)空間的軌 跡規(guī)劃、以及基于動力學(xué)的軌跡規(guī)劃等幾種類型。關(guān)節(jié)空間的軌跡規(guī)劃就是已知某連桿起點和終點的角位置角速度角加速度等參數(shù)確定 中間點的相應(yīng)參數(shù)的過程。 如圖所示，一兩自由度機械手，已知兩連桿起點和終點的關(guān)節(jié)角， 確定中間位置的關(guān)節(jié)角。（1）非歸一化和歸一化問題（2）末端位置的軌跡、關(guān)節(jié)空間軌跡 規(guī)劃的缺點。擱育由度鞏器扎侯節(jié)亨問的打Tt運勾三次多項式軌跡規(guī)劃要求,機曙人上茶黃節(jié)在運動幵姥時削百的角度世左時號N氓動科為懐號“匡三次宕頊?zhǔn)絼P劃諛哉節(jié)的運旬二 q +帀 +

14、76;/ +©廣初始茶徉與水端舉件：*0（牛與”如2卜踴屈毎散 Ci' <71 -以硒宦鳳A晁I 3血的在達武® f舉例：要求一個5軸機器人的第一關(guān)節(jié)在5秒之內(nèi)從初始角30度運動到終端角75用三次多項式計算在第1、2、3、4秒時的關(guān)節(jié)角。9（?. = = 30c2 - 54 tcj = -0-72»（tf） - Co + 口+ 白冋二 75就占） Ci " 0* G + 2c斯）+ 哉加-0由此得到位理、速度和丿川速度的多項點方程如下：盹” 30 * 54<: - 0.72r3 鴉H - 10.8t - 11&1鮒）-1QJ

15、- 4321代人時何求得；#（） - 34,68'«（2） = 4J.S4P 瞪）=59,16* 能）=九迂五次多項式軌跡規(guī)劃已知機器人上某關(guān)節(jié)在運動開始時刻滬0的角度為烤、速度a、 加速度自，在時刻運動到角度鄉(xiāng)、速度為®、加遠度為N。要求 用五次多項武規(guī)劃該關(guān)節(jié)的運動(X：)二 q + qr + cji1+c+ cj*初始條件與苯端條件：，m仏W灰右=o x 3億)二自屮I*&(ij) = 3$、西汀=0 &ij) =確定系數(shù)eq、cr勺、知cr c5 r以確定&(t)、Q(t)、Q(t：的表達式口 aM 同側(cè)且已知初藍加建度和未竭裁逵度均

16、為§度r秒氣解；由例5和給出的加速度儉得到：&. 30'玄0度/秒 予度/秒z9f 75* 坷=0 慶 / 秒 9f 5 7t / 秒3特初猛和未端邊界兼件代入式(55)-式(5.7),得出:5 = 30Ci = 0c3 - 25Cy = L6a -0,W5 0.0464進而得到如丁運動方程：9(0 = 30 4 2JH + L6P - OJgf4 + 0.046 4P的)-Sr + 4.8P - 2.322 + 0*232 r49(。- 5 + W - 6.96fz + 0.928?圖52足機器人關(guān)節(jié)的位宣"速度和加連屢曲線，其最大加速度為乩7度f秒粘80

17、°12 秒 345圖工12例錄3的關(guān)節(jié)位置、速度和加速度拋物線過渡的線性運動軌跡規(guī)劃（略）具有中間點以及用拋物線過渡的線性運動軌跡規(guī)劃（略） 高次多項式運動軌跡規(guī)劃（略）直角坐標(biāo)空間的軌跡規(guī)劃（1）所有用于關(guān)節(jié)空間的軌跡規(guī)劃方法都可以用于直角坐標(biāo) 空間軌跡規(guī)劃；（2）直角坐標(biāo)軌跡規(guī)劃必須不斷進行逆運動學(xué)運算，以便及時得到關(guān)節(jié)角。 這個過程可以歸納為以下計算循環(huán)：（a）將時間增加一個增量；（b）禾U用所選擇的軌跡函數(shù)計算出手的位姿；（c）利用逆運動學(xué)方程計算相應(yīng)的關(guān)節(jié)變量；（d）將關(guān)節(jié)變量信息送給控制器；（e）返回到循環(huán)的開始。例5.6 個兩自由度平面機器人要岸從起A (3, 10)

18、直線運動到終點(8. 14)。伯対 徑分為10段.求出機券人的關(guān)節(jié)每一根連桿的長尺為9英寸軋解：克角坐標(biāo)空間中媒點和終點間的直線可以措述為：或者y 0& + 7.6中間點的坐標(biāo)可以通過將起點和終點的x、y坐標(biāo)之壟簡竝地加以分割得到，然后通過耒 解逆運動學(xué)方程得到對應(yīng)毎個中間點的財個關(guān)節(jié)角。結(jié)果見衣5和圖5.17,表5.1例5.6的坐標(biāo)和關(guān)節(jié)角#Xy6$13101&810923.510.419104.03410.819.5100.444.511.220.295.85511.621.390.965.51222.585.77612.424.180.186.5'12.82674.29713.228.267.8107.513.630.860.71181433.952.8中間點數(shù)圖5.17例5.6機器人的關(guān)節(jié)位置例5.7 加利福尼亞理工學(xué)院實臉宅研究使用的一個3自由度機壽人有兩根連桿.毎根連桿卡 9英寸。如圖5.1