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

1、1一、平面二連桿機器人手臂運動學(xué)平面二連桿機械手臂如圖 1 所示，連桿 1 長度 ，連桿 2 長度 。建立如圖 1 所示的 1 l 2 l坐標(biāo)系，其中， 為基礎(chǔ)坐標(biāo)系，固定在基座上， 、 為連體坐標(biāo) ) , ( 0 0 y x ) , ( 1 1 y x ) , ( 2 2 y x系，分別固結(jié)在連桿 1 和連桿 2 上并隨它們一起運動。關(guān)節(jié)角順時針為負(fù)逆時針為正。1 2 0 x0 y1 x1 y2 x2 y1 ?2 ? P

2、ABCD圖 1 平面雙連桿機器人示意圖1、用簡單的平面幾何關(guān)系建立運動學(xué)方程連桿 2 末段與中線交點處一點 P 在基礎(chǔ)坐標(biāo)系中的位置坐標(biāo)：（1） ) sin( sin) cos( cos2 1 2 1 12 1 2 1 1? ? ?? ? ?? ? ?? ? ?l l yl l xpp2、用 D-H 方法建立運動學(xué)方程假定 、 、 垂直于紙面向里。從 到 的齊次旋轉(zhuǎn)變換矩陣 0 z 1 z 2 z ) , , ( 0 0 0 z y x

3、 ) , , ( 1 1 1 z y x為：（2）? ? ? ???? ? ? ??? ??1 0 0 00 1 0 00 0 cos sin0 0 sin cos1 11 10 1? ?? ?T從 到 的齊次旋轉(zhuǎn)變換矩陣為： ) , , ( 1 1 1 z y x ) , , ( 2 2 2 z y x（3）? ? ? ???? ? ? ??? ??1 0 0 00 1 0 00 0 cos sin0 sin cos2 21 2 21

4、 2? ?? ? lT從 到 的齊次旋轉(zhuǎn)變換矩陣為： ) , , ( 0 0 0 z y x ) , , ( 2 2 2 z y x3由（6）式得到：（7） 2 221 121 1 ) sin ( ) cos ( l l y l x p p ? ? ? ? ? ?整理得到：（8） ) sin cos ( 2 1 1 12 22 12 2 ? ? p p p p y x l l l y x ? ? ? ? ?令（9）ppppp tg yx

5、?? ? cossin ? ?由（8）式得到：) sin sin cos (cos cos21 11 2 22 12 2p pppp px l l l y x ? ? ? ? ? ? ? ? ? ?（10） ) cos( cos211 2 22 12 2pppp px l l l y x ? ? ? ? ? ? ? ?由此可解出 。 1 ?（11）ppppp pxy arctg x ll l y x ?? ? ??? ? ?? ? ?

6、? ? ? ? cos 2 arccos12 22 12 21（3）求 2 ?由（6）式得到：（12） 2 122 1 222 1 2 )] sin( [ )] cos( [ l l y l x p p ? ? ? ? ? ? ? ? ? ?整理得到：（13） )] sin( ) cos( [ 2 2 1 2 1 22 12 22 2 ? ? ? ? ? ? ? ? ? ? ? p p p p y x l l l y x令（14）ppp