《機器人的位姿描述與坐標(biāo)變換》由會員分享，可在線閱讀，更多相關(guān)《機器人的位姿描述與坐標(biāo)變換（61頁珍藏版）》請在裝配圖網(wǎng)上搜索。

1、戰(zhàn) 強北京航空航天大學(xué)機器人研究所 機器人的位姿連桿I的位姿YX Z YiXi Zi YwXw Zw 3-1 剛體位姿的數(shù)學(xué)描述 000 zyxPoo )cos()cos()cos( )cos()cos()cos( )cos()cos()cos( 33 ZZZYZX YZYYYX XZXYXXZYXR OOOOOOOO剛體位置：剛體姿態(tài)：單位主矢量 ￥ ￥假設(shè)機器人的連桿和關(guān)節(jié)都是剛體￥ ￥ OX YZ Z YX Obn t 1 1 R RROO TOOOO9個元素，只有3個獨立，滿足6個約束條件: 0. 1. XZZYYX ZZYYXX OOOOOOOOOOOO OOOOOOOOOOOO ,

2、 PRO OOOO剛體的位置和姿態(tài)：R是單位正交陣姿態(tài)矩陣R的特點： )cos()cos()cos( )cos()cos()cos( )cos()cos()cos( 33 ZZZYZX YZYYYX XZXYXXZYXR OOOOOOOO jZjX jY iZiX iYOi Oj例：某剛體j在參考系i中的 位置 姿態(tài)?jiOO R 10 6?jioo P 3-2 坐標(biāo)變換（點的映射）1、坐標(biāo)平移（坐標(biāo)系方位相同） i Oj jiP P P jZjX jYP Oji P iZiX iYOi Oj沿著不同軸向的組合平移： zyxzyxPOji 000000 POOOPO jjii 已知點P在j坐標(biāo)

3、系的坐標(biāo)，平移j至i，求點P在i坐標(biāo)系的坐標(biāo)。 Y1X1 Z1 Y2X2 Z2 Y3X3 Z3三坐標(biāo)的直角坐標(biāo)機器人適用的機器人類型舉例（有平移關(guān)節(jié)） Y XZ iZiX iYOi jZjX jY POj15例: TjP 765已知 求 P點在i坐標(biāo)系中的坐標(biāo)。 T TTOjiji PPP 7215 0150765 解答： 2、坐標(biāo)旋轉(zhuǎn)（坐標(biāo)系原點相同）ZiX i Yi Zj X j Yj P坐標(biāo)系j由坐標(biāo)系i旋轉(zhuǎn)而成 Tiiii zyxP Tjjjj zyxP 求點P在i坐標(biāo)系的坐標(biāo)：已知點P在j坐標(biāo)系的坐標(biāo)： ZiZjX i Xj YiYjPjx jyjzcos( , )i j i jy

4、x Y X ix iyizcos( , ) cos( , )i j i j j i jy x Y X y Y Y cos( , ) cos( , ) cos( , ) i j i j j i j j i jy x Y X y Y Y z Y Z ( , )i jY X關(guān)于？ ),cos(),cos(),cos( ),cos(),cos(),cos( ),cos(),cos(),cos( jijjijjiji jijjijjiji jijjijjijii ZZzYZyXZxz ZYzYYyXYxy ZXzYXyXXxxP ZiZ jX i X j YiYjPjx jyjzix iyiz jjjj

5、ijiji jijiji jijijii zyxZZYZXZ ZYYYXY ZXYXXXP ),cos(),cos(),cos( ),cos(),cos(),cos( ),cos(),cos(),cos( Rji姿態(tài)矢量矩陣OX YZ Z YX Obn tPj )cos()cos()cos( )cos()cos()cos( )cos()cos()cos( ZZZYZX YZYYYX XZXYXXROO PRP jjii 坐標(biāo)系j相對于i的方位 Tijijji RRR 1旋轉(zhuǎn)矩陣的性質(zhì)： 旋轉(zhuǎn)矩陣 繞一個坐標(biāo)軸旋轉(zhuǎn)的轉(zhuǎn)動矩陣jZ iZ iX jY iYqqjX jZ iZiX jYiYqq jX

6、jZiZ iX jY iYq qjX 1）RX 2）RY3）RZ qq qqq cossin0 sincos0 001),( iji XR jZ iZiX jY iYqqjX jjjjijiji jijiji jijijii zyxZZYZXZ ZYYYXY ZXYXXXP ),cos(),cos(),cos( ),cos(),cos(),cos( ),cos(),cos(),cos( 100 0cossin 0sincos),( qq qqqiji ZR qq qqq cos0sin 010 sin0cos),( iji YR jZ iZiX jYiYqqjX jZiZiX jY iYq q

7、jX 轉(zhuǎn)動矩陣的特點：(1) 主對角線上有一個元素為1，其余均為轉(zhuǎn)角的余弦/正弦；(2) 繞軸轉(zhuǎn)動的次序與元素1所在的行、列號對應(yīng)；(3) 元素1所在的行、列，其它元素均為0；(4) 從元素1所在行起，自上而下，先出現(xiàn)的正弦為負(fù)，后出現(xiàn)的為正，反之依然。 100 0cossin 0sincos),( qq qqqiji ZR qq qqq cossin0 sincos0 001),( iji XR qq qqq cos0sin 010 sin0cos),( iji YR 繞多個坐標(biāo)軸旋轉(zhuǎn)的轉(zhuǎn)動矩陣1）、繞固定坐標(biāo)系旋轉(zhuǎn)),( iXR ),( qiZR ( , ) ?ji R q qqq qqq

8、 qq qqq cossin0 sincoscoscossin sinsincossincoscossin0 sincos0 001100 0cossin 0sincos),(R ji ),( iii ZYX坐標(biāo)系),( mmm ZYX坐標(biāo)系),( jjj ZYX坐標(biāo)系iZ iX jY iYq jX qmZmX mYq jZ),(),(),( qq XRZRR ji 2）、繞運動坐標(biāo)系旋轉(zhuǎn)),( iZR ),( 1 qYR ),( 2 ZR ),(),(),(),( qq ZRYRZRRji ZYZ歐拉角),( iii ZYX坐標(biāo)系),( 111 ZYX坐標(biāo)系),( 222 ZYX坐標(biāo)系),(

9、 jjj ZYX坐標(biāo)系 iX 1X jX2X iY jY 1Y2（Y )( 1ZZijZ 2Z q q q 注意：多個旋轉(zhuǎn)矩陣連乘時，次序不同則含義不同。1）繞新的動坐標(biāo)軸依次轉(zhuǎn)動時，每個旋轉(zhuǎn)矩陣要從左往右乘，即旋轉(zhuǎn)矩陣的相乘順序與轉(zhuǎn)動次序相同；2）繞舊的固定坐標(biāo)軸依次轉(zhuǎn)動時，每個旋轉(zhuǎn)矩陣要從右往左乘，即旋轉(zhuǎn)矩陣的相乘順序與轉(zhuǎn)動次序相反。 qqq qqq qqq cossinsinsinsin sinsincoscossincossinsincoscoscossin sincoscossinsincoscossinsincoscoscos 100 0cossin 0sincoscos0sin

10、010 sin0cos100 0cossin 0sincos),( qq qq qRji 證明： ),( iii ZYX坐標(biāo)系),( 111 ZYX坐標(biāo)系),( 222 ZYX坐標(biāo)系),( jjj ZYX坐標(biāo)系 jii iii jjj PZRYRZR PYRZR PZRPRP PYRPRP PZRPRP ),(),(),( ),(),( ),()3 ),()2 ),()1 21 21 111 212211 222 q q q 1）繞運動坐標(biāo)系旋轉(zhuǎn)),( iZR ),( 1 qYR ),( 2 ZR iX 1X jX2X iY jY 1Y2（Y )( 1ZZijZ 2Z q q q ?ji R

11、2）、繞固定坐標(biāo)系旋轉(zhuǎn)),( iii ZYX坐標(biāo)系),( mmm ZYX坐標(biāo)系),( jjj ZYX坐標(biāo)系iZ iX jY iYq jX qmZmX mYq jZ ),( qiZ),( iX jii mimmii jijjmm PZRXR PXRPRP PZRPRP ),(),( ),()2 ),()1 q q證明與討論: ?ji R 適用的機器人類型舉例（有旋轉(zhuǎn)關(guān)節(jié)）Y6 X6Z6 Y0 X0 Z0Y7 X7Z7 例1: 已知坐標(biāo)系B初始位姿與A重合,首先B相對于坐標(biāo)系A(chǔ)的Z軸轉(zhuǎn)30度, 假設(shè)點P在 坐標(biāo)系B的描述為PB=3,7,0T,求它在坐標(biāo)系A(chǔ)中的描述PA. 3、坐標(biāo)變換綜合（平移+旋

12、轉(zhuǎn)）PRPP Ojijjii 旋轉(zhuǎn)部分平移部分推導(dǎo)(中間坐標(biāo)系C)： iZiX iYOi Zj X j Yj PjPOj iP POji c ZcX cYI（旋轉(zhuǎn)）: c與j 原點重合，c與i姿態(tài)相同 jjijjcc RPRPP II（平移）: c與i 原點重合PRPPPP OjijjiOcici 問題：是否可以先平移后旋轉(zhuǎn)？ iZiX iYOi Zj X j Yj PjPOj iP POjicZ cX cY 推導(dǎo)(中間坐標(biāo)系C)：I（平移）: c與i原點重合，c與j姿態(tài)相同jOijOcc PPPPP jj II（旋轉(zhuǎn)）: c與i 姿態(tài)相同 jjiOi jjiOccijOcjiccii RPP

13、 RPPRPPRRPP j jj )( 未知jOc c jP P P 例1: 已知坐標(biāo)系B初始位姿與A重合,首先B相對于坐標(biāo)系A(chǔ)的Z軸轉(zhuǎn)30度,再沿A的X軸移動10個單位,并沿A的Y軸移動5個單位.假設(shè)點P在 坐標(biāo)系B的描述為PB=3,7,0T,求它在坐標(biāo)系A(chǔ)中的描述PA. 0562.12098.9 0500010073100 030cos30sin 030sin30cos )5,()10,()30,( oo oo AAAOBABAA YPXPPZRPRPP BBAZAX AYOi POj BZBX BY zyxP xa yb zc 3-3 齊次坐標(biāo)與齊次變換1、齊次坐標(biāo) cbaP 0齊次坐標(biāo)

14、直角坐標(biāo)1）點的齊次坐標(biāo)： TT T PP zyxP 2864,1432 非零的比例因子 2）坐標(biāo)軸方向的齊次坐標(biāo)： Tcba 0 T0001X軸：a,b,c稱為方向數(shù)Y軸：Z軸： T0010 T0100 TT 1000,0000坐標(biāo)原點無意義點 AZAX AYOi jXiZ iX iYPOjiOi P jYjZ Oj 2、齊次變換 441000 PRT Ojjiji i 141 iP點P在i坐標(biāo)系中的齊次坐標(biāo)：點P在j坐標(biāo)系中的齊次坐標(biāo)： 141 jP 旋轉(zhuǎn)矩陣與平移向量構(gòu)成的齊次變換矩陣： 11110001 iOjijjijOjijijji PPRPPPRPT iP 齊次變換矩陣Tji表示

15、了坐標(biāo)系j相對于坐標(biāo)系i的位姿的含義： 1000 4010 3001 1100T ji iX iY iZjXjYjZ POjijO iOjX jY jZ PjOi jjii TPP 134jOi p 旋轉(zhuǎn)的齊次變換平移加旋轉(zhuǎn)變換 1000 0),( RKR jiq 10001000 01000),()( 33 PRRPIKRPTransT jij OijijiOjOiji q 平移的齊次變換 1000)( 3*3 PIPTrans OjiOji 10001000 01000),()( 33 PRRPIKRPTransT jij OijijiOjOiji qiZiX iYOi Zj X j Yj

16、 PjPOj iP POji c ZcX cY iZiX iYOi Zj X j Yj PjPOj iP POjicZ cX cY 例2: 已知坐標(biāo)系B初始位姿與A重合,首先B相對于坐標(biāo)系A(chǔ)的Z軸轉(zhuǎn)30度,再沿A的X軸移動10個單位,并沿A的Y軸移動5個單位.假設(shè)點P在 坐標(biāo)系B的描述為PB=3,7,0T,求它在坐標(biāo)系A(chǔ)中的描述PA. 10562.12098.91073100001000030cos 30sin0030sin30cos10510010000100001 )(),( BB PZRYXTransTPP BAA 10562.12098.910731051001000030cos 30

17、sin0030sin30cosBTPP BAA 解法1:解法2: 練習(xí)題1：已知坐標(biāo)系A(chǔ)初始位姿與B重合,首先A相對于坐標(biāo)系B的Z軸轉(zhuǎn)30度,再沿B的X軸移動10個單位, 再相對于A的Y軸轉(zhuǎn)60度，并沿A的Z軸移動5個單位. 假設(shè)點P在坐標(biāo)系A(chǔ)的描述為 =12,0,4T,求它在坐標(biāo)系B中的描述 ；APBP 1892.5897.6 946.21 )15,()60,()30,()10,(21 ZTransYRZRXTransTTP ooB解法1: 3-4 旋轉(zhuǎn)變換通式1、旋轉(zhuǎn)矩陣通式： q iX iY iZ jX jY jZ O KqkkjkikK zyx 旋轉(zhuǎn)矩陣?),( qKR1)、RKR j

18、iq),(坐標(biāo)系j 相對于 i的姿態(tài)2)、定義兩個坐標(biāo)系i和j，i與i固連， j與j 固連； i和j 的Z軸與矢量K重合，旋轉(zhuǎn)前，i與j重合，i與j 重合。繞通過原點的任意單位矢量K轉(zhuǎn) 角的旋轉(zhuǎn)矩陣 zzz yyy xxxzzz yyy xxxjjii kon kon konaon aon aonRR 3)、 iX iY iZ jX jY jZ O KqiZiX iYjX jY繞K轉(zhuǎn) 角q j相對于i的Z軸轉(zhuǎn) 角qjZ 旋轉(zhuǎn)變換的尺寸鏈圖：ii jjRRRKRR jjjiiiji ),( q 1 ),(),( qq RZRRKRR jjiiji Tjjiiji RZRRKRR ),(),( q

19、q qq qqq zyx zyx zyxzzz yyy xxx kkk ooo nnnkon kon konKR 100 0cossin 0sincos),(利用旋轉(zhuǎn)矩陣的正交性質(zhì):ona naaoon aaoonn 01假設(shè)： )cos1(cossin qqqqqq verscs qqqqqq qqqqqq qqqqqqq cverskkskverskkskverskk skverskkcverskkskverskk skverskkskverskkcverskk),K(R zzxzyyzx xyzyyzyx yxzzxyxx整理得：旋轉(zhuǎn)變換通式0,1 zyx kkk討論：（1）（2）（3）

20、 qq qqq 100 0cossin 0sincos),(KR qq qqq cossin0 sincos0 001),(KR qq qqq cos0sin 010 sin0cos),(KR0,1 zxy kkk 0,1 xyz kkk 例：坐標(biāo)系B原來與A重合，將坐標(biāo)系B繞過原點O的軸線kjiKA 313131 轉(zhuǎn)動o120q，求旋轉(zhuǎn)矩陣)120,( oAKR解答：31 zyx kkk1）2）23120,23120sin,21120cos ooo vers3）帶入旋轉(zhuǎn)通式得： 010 001 100)120,( oAKR o7.54)31cos(a 2、等效轉(zhuǎn)軸與等效轉(zhuǎn)角轉(zhuǎn)軸和轉(zhuǎn)角旋轉(zhuǎn)矩陣

21、12？ zzz yyy xxx aon aon aon),( qKR qqqqqq qqqqqq qqqqqq cverskkskverskkskverskk skverskkcverskkskverskk skverskkskverskkcverskk zzxzyyzx xyzyyzyx yxzzxyxx zzz yyy xxx aon aon aon1）將方程兩邊矩陣的主對角線元素分別相加，則qqq ccverskkkaon zyxzyx 213)( 222 )1(21cos q zyx aon2）將方程兩邊矩陣的非對角線元素成對相減得： q q q sin2 sin2 sin2 zxy

22、yzx xyz kon kna kao qqqqqq qqqqqq qqqqqq cverskkskverskkskverskk skverskkcverskkskverskk skverskkskverskkcverskk zzxzyyzx xyzyyzyx yxzzxyxx zzz yyy xxx aon aon aon q q q sin2 sin2 sin2 zxy yzx xyz kon kna kao將上式兩邊平方相加得：)1( )()()(tan )()()(21sin sin4)()()( 222 222 2222 q q q zyx xyzxyz xyzxyz xyzxyz

23、aon onnaao onnaao onnaao求得轉(zhuǎn)角 qqq sin2,sin2,sin2 xyzzxyyzx onknakaok求得轉(zhuǎn)軸注意：1）多值性：。一般?。ㄊ呛侠斫狻６加腥我猓ǖ闹挡晃ㄒ?。例如對于和 o1800)360,(), ),(),qqq qqq onKK KKK2）存在病態(tài)情況：殊的解法。，轉(zhuǎn)軸不確定，需要特分母都很小，或和度時，由于上式的分子或的值接近當(dāng)0sin1800 qq )1( )()()(tan 222 q zyx xyzxyz aon onnaao 例：。和等效轉(zhuǎn)角的等效轉(zhuǎn)軸求復(fù)合旋轉(zhuǎn)矩陣q K)90,()90,( ZRYRRBA解： 010 001 1001

24、00 001 010001 010 100RBA 3tan,23sin,21cos qqq利用前面的公式可求得：kjiK kkk zyx 313131 31 )90,()90,()120,( ZRYRKR 任何一組繞過原點的軸線的復(fù)合轉(zhuǎn)動總等效于繞過原點的某一矢量的轉(zhuǎn)動 3、齊次變換通式*討論矢量K不通過原點的情況kkjkikK zyx 假設(shè)單位矢量 通過點kpjpipP zyx 求繞矢量K轉(zhuǎn) 角的齊次變換矩陣q1)、定義兩個坐標(biāo)系i和j，坐標(biāo)原點在P點，i與i固連， j與j 固連； i和j 的坐標(biāo)軸分別與i和j的坐標(biāo)軸平行，旋轉(zhuǎn)前，i與j重合， j與i 重合。 iX iY iZ jX jY

25、jZ O KqP iZiX iYjX jYjZ 2）旋轉(zhuǎn)變換的尺寸鏈圖：ii jjTTTT jjjiiiji 10 PI)P(TransT 31 33ii TjiTii T ji Tjj 10)( 31 331 PIPTransTT jjjj qq 10 0),(),( 31 13 KRKRotTji qqq 10 ),(),()(),()( 31 PPKRKRPTransKRotPTransTji 例：坐標(biāo)系B原來與A重合，將坐標(biāo)系B繞過點P的矢量kjiKA 313131 轉(zhuǎn)動o120q，該矢量經(jīng)過點解答： 010 001 100)120,(KR1）2）帶入旋轉(zhuǎn)通式得： q 1000 101

26、0 1001 2100),(KTTBA TAP 321，求旋轉(zhuǎn)矩陣。反之，如果求P點，其值不唯一。 3-5 機器人姿態(tài)的其他表示方法 )cos()Ycos()cos( )cos()cos()cos( )cos()cos()cos( 33 ZZZZX YZYYYX XZXYXXZYXR OOOOOOOO前面：用3*3的旋轉(zhuǎn)矩陣表示剛體的姿態(tài)單位主矢量旋轉(zhuǎn)矩陣有9個元素，6個約束條件，3個獨立元素；計算編程時需要輸入9個元素，不方便；一般采用3個元素來表示。 1、RPY角：（繞固定坐標(biāo)軸X-Y-Z旋轉(zhuǎn)）RPY角是描述船舶在海中航行時姿態(tài)的一種方法。XY Z a R（Z， ）：Roll R（Y， ）

27、：Pitch R（X， ）：Yaw翻滾偏航俯仰 （1）船舶上建立的坐標(biāo)系B相對于參考系A(chǔ)的方位描述如下： A和B 重合，首先，將B繞XA 轉(zhuǎn) 角，再繞YA轉(zhuǎn) 角，最后繞ZA轉(zhuǎn) 角。 / AAA ZBYBXBBA ),(),(),(),( AAAxyzBA XRYRZRR cs sccs sccs sc 00 0010 010 0100 00 ccscs sccssccssscs sscsccsssccc 333231 232221 131211 rrr rrr rrr ccscs sccssccssscs sscsccsssccc 333231 232221 131211 rrr rrr rr

28、r（2）如果已知機器人的姿態(tài)矩陣，如何求RPY角？2 211 21cos r r 2 2 2 2 211 21( ) ( ) cosc c s c r r 90 90 2 211 21cos r r tan2( , ) arctan yA y x x雙變量反正切函數(shù) tan2( 2, 2) 135 , tan2(2,2) 45A A cos 0 90, ),(2tan 0,90 ),(2tan 0,90 2212 2212 rrA rrA if通常的選擇2 231 11 21cos 0, tan2( , ),A r r r if if if 21 11tan2( )A r r 32 33tan

29、2( , )A r r ccscs sccssccssscs sscsccsssccc 333231 232221 131211 rrr rrr rrr 2、ZYX歐拉角：（繞動坐標(biāo)軸Z-Y-X旋轉(zhuǎn)） / BBB XBYBZBBA ccscs sccssccsssss sscsccsssccc cs sccs sccs sc XRYRZRR BBBzyxBA 00 0010 010 0100 00 ),(),(),(),( ccscs sccssccssscs sscsccsssccc ),(),(),(),( AAAxyzBA XRYRZRR iX 1X jX2X iYjY 1Y2（Y )

30、)( 1ZZijZ 2Z q q q坐標(biāo)系B相對于參考系A(chǔ)的方位描述如下：RPY角 3、ZYZ歐拉角 / BBB ZBYBZBBA ),(),(),(),( BBBzyzBA ZRYRZRR 333231 232221 131211 rrr rrr rrrcsscs ssccscsscccs sccssccssccc ),(2tan ),(2tan ),(2tan,0sin 3132 33232231 1323 rrA rrrA rrAif ),(2tan 0,180 ),(2tan 0,0,0sin 1112 1112 rrA rrAif 已知姿態(tài)矩陣求歐拉角 坐標(biāo)系B相對于參考系A(chǔ)的方位描

31、述如下： 上述描述姿態(tài)的方法稱為角度設(shè)定法，共有24種，其中12種RPY法和12種歐拉法，并且是對偶的，實際上只有12種不同的旋轉(zhuǎn)矩陣。確定旋轉(zhuǎn)時是繞固定軸還是動軸非常重要。X Y Z的排列: 1266/ 33331213 PCCCEULERRPY ZXZ XYZ 3-6、自由矢量的變換前面討論的是位置矢量的變換，如旋轉(zhuǎn)、平移等；如何處理速度矢量、力矢量的變換問題？矢量分類：1、自由矢量：由維數(shù)、大小、方向三要素規(guī)定，如速度矢量、純力矩矢量；2、線矢量：由維數(shù)、大小、方向、作用線四要素規(guī)定，如力矢量。對于自由矢量：在不同坐標(biāo)系的描述只與旋轉(zhuǎn)矩陣有關(guān)，與坐標(biāo)原點的位置無關(guān)，對于速度矢量 對于力矩

32、矢量BBAA VRV BBAA mRm 3-7、總結(jié)1、變換矩陣T的物理含義 1）、坐標(biāo)系的描述： 如坐標(biāo)系B相對于A的位姿； 2）、同一點在不同坐標(biāo)系A(chǔ)和B間的影射關(guān)系； 3）、運動算子： 表示同一坐標(biāo)系中點運動前后的算子關(guān)系，如平移算子、 旋轉(zhuǎn)算子。 10 PRT BoABABA2、變換矩陣T的相乘 矩陣相乘的順序一般不可換，特殊可換的情況為變換都是同參考系下的平移或繞同一坐標(biāo)軸的旋轉(zhuǎn)。 3、變換矩陣求逆例：兩坐標(biāo)系A(chǔ)和B，B先繞A的Z軸轉(zhuǎn)30度，再沿A的X軸移動4個單位，沿A的Y軸移動3個單位，已知 ，求 。 TAP 3,2,1 BP解：ABAAABBBBAA PTPTPPTP 1;1） 10 )( 1010 PRRT PRPRPRR PRTPRT BoATBATBAAB BoATBABoAABAoBTBAAB AoBABABBoABABA2）)0,3,4()30,( )30,()0,3,4( 11 TransZRot ZRotTransTT BAAB