机器人的位姿描述与坐标变换

战 强北京航空航天大学机器人研究所 机器人的位姿连杆I的位姿YX Z YiXi Zi YwXw Zw 3-1 刚体位姿的数学描述 000 zyxPoo )cos()cos()cos( )cos()cos()cos( )cos()cos()cos( 33 ZZZYZX YZYYYX XZXYXXZYXR OOOOOOOO刚体位置：刚体姿态：单位主矢量 ￥ ￥假设机器人的连杆和关节都是刚体￥ ￥ OX YZ Z YX Obn t 1 1 R RROO TOOOO9个元素，只有3个独立，满足6个约束条件: 0. 1. XZZYYX ZZYYXX OOOOOOOOOOOO OOOOOOOOOOOO , PRO OOOO刚体的位置和姿态：R是单位正交阵姿态矩阵R的特点： )cos()cos()cos( )cos()cos()cos( )cos()cos()cos( 33 ZZZYZX YZYYYX XZXYXXZYXR OOOOOOOO jZjX jY iZiX iYOi Oj例：某刚体j在参考系i中的 位置 姿态?jiOO R 10 6?jioo P 3-2 坐标变换（点的映射）1、坐标平移（坐标系方位相同） i Oj jiP P P jZjX jYP Oji P iZiX iYOi Oj沿着不同轴向的组合平移： zyxzyxPOji 000000 POOOPO jjii 已知点P在j坐标系的坐标，平移j至i，求点P在i坐标系的坐标。 Y1X1 Z1 Y2X2 Z2 Y3X3 Z3三坐标的直角坐标机器人适用的机器人类型举例（有平移关节） Y XZ iZiX iYOi jZjX jY POj15例: TjP 765已知 求 P点在i坐标系中的坐标。 T TTOjiji PPP 7215 0150765 解答： 2、坐标旋转（坐标系原点相同）ZiX i Yi Zj X j Yj P坐标系j由坐标系i旋转而成 Tiiii zyxP Tjjjj zyxP 求点P在i坐标系的坐标：已知点P在j坐标系的坐标： ZiZjX i Xj YiYjPjx jyjzcos( , )i j i jy 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关于？ ),cos(),cos(),cos( ),cos(),cos(),cos( ),cos(),cos(),cos( jijjijjiji jijjijjiji jijjijjijii ZZzYZyXZxz ZYzYYyXYxy ZXzYXyXXxxP ZiZ jX i X j YiYjPjx jyjzix iyiz jjjjijiji jijiji jijijii zyxZZYZXZ ZYYYXY ZXYXXXP ),cos(),cos(),cos( ),cos(),cos(),cos( ),cos(),cos(),cos( Rji姿态矢量矩阵OX YZ Z YX Obn tPj )cos()cos()cos( )cos()cos()cos( )cos()cos()cos( ZZZYZX YZYYYX XZXYXXROO PRP jjii 坐标系j相对于i的方位 Tijijji RRR 1旋转矩阵的性质： 旋转矩阵 绕一个坐标轴旋转的转动矩阵jZ iZ iX jY iYqqjX jZ iZiX jYiYqq jXjZiZ 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 qjX 转动矩阵的特点：(1) 主对角线上有一个元素为1，其余均为转角的余弦/正弦；(2) 绕轴转动的次序与元素1所在的行、列号对应；(3) 元素1所在的行、列，其它元素均为0；(4) 从元素1所在行起，自上而下，先出现的正弦为负，后出现的为正，反之依然。 100 0cossin 0sincos),( qq qqqiji ZR qq qqq cossin0 sincos0 001),( iji XR qq qqq cos0sin 010 sin0cos),( iji YR 绕多个坐标轴旋转的转动矩阵1）、绕固定坐标系旋转),( iXR ),( qiZR ( , ) ?ji R q qqq qqq qq qqq cossin0 sincoscoscossin sinsincossincoscossin0 sincos0 001100 0cossin 0sincos),(R ji ),( iii ZYX坐标系),( mmm ZYX坐标系),( jjj ZYX坐标系iZ iX jY iYq jX qmZmX mYq jZ),(),(),( qq XRZRR ji 2）、绕运动坐标系旋转),( iZR ),( 1 qYR ),( 2 ZR ),(),(),(),( qq ZRYRZRRji ZYZ欧拉角),( iii ZYX坐标系),( 111 ZYX坐标系),( 222 ZYX坐标系),( jjj ZYX坐标系 iX 1X jX2X iY jY 1Y2（Y )( 1ZZijZ 2Z q q q 注意：多个旋转矩阵连乘时，次序不同则含义不同。1）绕新的动坐标轴依次转动时，每个旋转矩阵要从左往右乘，即旋转矩阵的相乘顺序与转动次序相同；2）绕旧的固定坐标轴依次转动时，每个旋转矩阵要从右往左乘，即旋转矩阵的相乘顺序与转动次序相反。 qqq qqq qqq cossinsinsinsin sinsincoscossincossinsincoscoscossin sincoscossinsincoscossinsincoscoscos 100 0cossin 0sincoscos0sin 010 sin0cos100 0cossin 0sincos),( qq qq qRji 证明： ),( iii ZYX坐标系),( 111 ZYX坐标系),( 222 ZYX坐标系),( jjj ZYX坐标系 jii iii jjj PZRYRZR PYRZR PZRPRP PYRPRP PZRPRP ),(),(),( ),(),( ),()3 ),()2 ),()1 21 21 111 212211 222 q q q 1）绕运动坐标系旋转),( iZR ),( 1 qYR ),( 2 ZR iX 1X jX2X iY jY 1Y2（Y )( 1ZZijZ 2Z q q q ?ji R 2）、绕固定坐标系旋转),( iii ZYX坐标系),( mmm ZYX坐标系),( jjj ZYX坐标系iZ iX jY iYq jX qmZmX mYq jZ ),( qiZ),( iX jii mimmii jijjmm PZRXR PXRPRP PZRPRP ),(),( ),()2 ),()1 q q证明与讨论: ?ji R 适用的机器人类型举例（有旋转关节）Y6 X6Z6 Y0 X0 Z0Y7 X7Z7 例1: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z轴转30度, 假设点P在 坐标系B的描述为PB=3,7,0T,求它在坐标系A中的描述PA. 3、坐标变换综合（平移+旋转）PRPP Ojijjii 旋转部分平移部分推导(中间坐标系C)： iZiX iYOi Zj X j Yj PjPOj iP POji c ZcX cYI（旋转）: c与j 原点重合，c与i姿态相同 jjijjcc RPRPP II（平移）: c与i 原点重合PRPPPP OjijjiOcici 问题：是否可以先平移后旋转？ iZiX iYOi Zj X j Yj PjPOj iP POjicZ cX cY 推导(中间坐标系C)：I（平移）: c与i原点重合，c与j姿态相同jOijOcc PPPPP jj II（旋转）: c与i 姿态相同 jjiOi jjiOccijOcjiccii RPP RPPRPPRRPP j jj )( 未知jOc c jP P P 例1: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z轴转30度,再沿A的X轴移动10个单位,并沿A的Y轴移动5个单位.假设点P在 坐标系B的描述为PB=3,7,0T,求它在坐标系A中的描述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 齐次坐标与齐次变换1、齐次坐标 cbaP 0齐次坐标直角坐标1）点的齐次坐标： TT T PP zyxP 2864,1432 非零的比例因子 2）坐标轴方向的齐次坐标： Tcba 0 T0001X轴：a,b,c称为方向数Y轴：Z轴： T0010 T0100 TT 1000,0000坐标原点无意义点 AZAX AYOi jXiZ iX iYPOjiOi P jYjZ Oj 2、齐次变换 441000 PRT Ojjiji i 141 iP点P在i坐标系中的齐次坐标：点P在j坐标系中的齐次坐标： 141 jP 旋转矩阵与平移向量构成的齐次变换矩阵： 11110001 iOjijjijOjijijji PPRPPPRPT iP 齐次变换矩阵Tji表示了坐标系j相对于坐标系i的位姿的含义： 1000 4010 3001 1100T ji iX iY iZjXjYjZ POjijO iOjX jY jZ PjOi jjii TPP 134jOi p 旋转的齐次变换平移加旋转变换 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 PjPOj iP POji c ZcX cY iZiX iYOi Zj X j Yj PjPOj iP POjicZ cX cY 例2: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z轴转30度,再沿A的X轴移动10个单位,并沿A的Y轴移动5个单位.假设点P在 坐标系B的描述为PB=3,7,0T,求它在坐标系A中的描述PA. 10562.12098.91073100001000030cos 30sin0030sin30cos10510010000100001 )(),( BB PZRYXTransTPP BAA 10562.12098.910731051001000030cos 30sin0030sin30cosBTPP BAA 解法1:解法2: 练习题1：已知坐标系A初始位姿与B重合,首先A相对于坐标系B的Z轴转30度,再沿B的X轴移动10个单位, 再相对于A的Y轴转60度，并沿A的Z轴移动5个单位. 假设点P在坐标系A的描述为 =12,0,4T,求它在坐标系B中的描述 ；APBP 1892.5897.6 946.21 )15,()60,()30,()10,(21 ZTransYRZRXTransTTP ooB解法1: 3-4 旋转变换通式1、旋转矩阵通式： q iX iY iZ jX jY jZ O KqkkjkikK zyx 旋转矩阵?),( qKR1)、RKR jiq),(坐标系j 相对于 i的姿态2)、定义两个坐标系i和j，i与i固连， j与j 固连； i和j 的Z轴与矢量K重合，旋转前，i与j重合，i与j 重合。绕通过原点的任意单位矢量K转 角的旋转矩阵 zzz yyy xxxzzz yyy xxxjjii kon kon konaon aon aonRR 3)、 iX iY iZ jX jY jZ O KqiZiX iYjX jY绕K转 角q j相对于i的Z轴转 角qjZ 旋转变换的尺寸链图：ii jjRRRKRR jjjiiiji ),( q 1 ),(),( qq RZRRKRR jjiiji Tjjiiji RZRRKRR ),(),( qq qq qqq zyx zyx zyxzzz yyy xxx kkk ooo nnnkon kon konKR 100 0cossin 0sincos),(利用旋转矩阵的正交性质:ona naaoon aaoonn 01假设： )cos1(cossin qqqqqq verscs qqqqqq qqqqqq qqqqqqq cverskkskverskkskverskk skverskkcverskkskverskk skverskkskverskkcverskk),K(R zzxzyyzx xyzyyzyx yxzzxyxx整理得：旋转变换通式0,1 zyx kkk讨论：（1）（2）（3） 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 例：坐标系B原来与A重合，将坐标系B绕过原点O的轴线kjiKA 313131 转动o120q，求旋转矩阵)120,( oAKR解答：31 zyx kkk1）2）23120,23120sin,21120cos ooo vers3）带入旋转通式得： 010 001 100)120,( oAKR o7.54)31cos(a 2、等效转轴与等效转角转轴和转角旋转矩阵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 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 aon onnaao onnaao onnaao求得转角 qqq sin2,sin2,sin2 xyzzxyyzx onknakaok求得转轴注意：1）多值性：。一般取（是合理解。都有任意（的值不唯一。例如对于和 o1800)360,(), ),(),qqq qqq onKK KKK2）存在病态情况：殊的解法。，转轴不确定，需要特分母都很小，或和度时，由于上式的分子或的值接近当0sin1800 qq )1( )()()(tan 222 q zyx xyzxyz aon onnaao 例：。和等效转角的等效转轴求复合旋转矩阵q K)90,()90,( ZRYRRBA解： 010 001 100100 001 010001 010 100RBA 3tan,23sin,21cos qqq利用前面的公式可求得：kjiK kkk zyx 313131 31 )90,()90,()120,( ZRYRKR 任何一组绕过原点的轴线的复合转动总等效于绕过原点的某一矢量的转动 3、齐次变换通式*讨论矢量K不通过原点的情况kkjkikK zyx 假设单位矢量 通过点kpjpipP zyx 求绕矢量K转 角的齐次变换矩阵q1)、定义两个坐标系i和j，坐标原点在P点，i与i固连， j与j 固连； i和j 的坐标轴分别与i和j的坐标轴平行，旋转前，i与j重合， j与i 重合。 iX iY iZ jX jY jZ O KqP iZiX iYjX jYjZ 2）旋转变换的尺寸链图：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 例：坐标系B原来与A重合，将坐标系B绕过点P的矢量kjiKA 313131 转动o120q，该矢量经过点解答： 010 001 100)120,(KR1）2）带入旋转通式得： q 1000 1010 1001 2100),(KTTBA TAP 321，求旋转矩阵。反之，如果求P点，其值不唯一。 3-5 机器人姿态的其他表示方法 )cos()Ycos()cos( )cos()cos()cos( )cos()cos()cos( 33 ZZZZX YZYYYX XZXYXXZYXR OOOOOOOO前面：用3*3的旋转矩阵表示刚体的姿态单位主矢量旋转矩阵有9个元素，6个约束条件，3个独立元素；计算编程时需要输入9个元素，不方便；一般采用3个元素来表示。 1、RPY角：（绕固定坐标轴X-Y-Z旋转）RPY角是描述船舶在海中航行时姿态的一种方法。XY Z a R（Z， ）：Roll R（Y， ）：Pitch R（X， ）：Yaw翻滚偏航俯仰 （1）船舶上建立的坐标系B相对于参考系A的方位描述如下： A和B 重合，首先，将B绕XA 转 角，再绕YA转 角，最后绕ZA转 角。 / 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 rrr（2）如果已知机器人的姿态矩阵，如何求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双变量反正切函数 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 33tan2( , )A r r ccscs sccssccssscs sscsccsssccc 333231 232221 131211 rrr rrr rrr 2、ZYX欧拉角：（绕动坐标轴Z-Y-X旋转） / 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 ) )( 1ZZijZ 2Z q q q坐标系B相对于参考系A的方位描述如下：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 已知姿态矩阵求欧拉角 坐标系B相对于参考系A的方位描述如下： 上述描述姿态的方法称为角度设定法，共有24种，其中12种RPY法和12种欧拉法，并且是对偶的，实际上只有12种不同的旋转矩阵。确定旋转时是绕固定轴还是动轴非常重要。X Y Z的排列: 1266/ 33331213 PCCCEULERRPY ZXZ XYZ 3-6、自由矢量的变换前面讨论的是位置矢量的变换，如旋转、平移等；如何处理速度矢量、力矢量的变换问题？矢量分类：1、自由矢量：由维数、大小、方向三要素规定，如速度矢量、纯力矩矢量；2、线矢量：由维数、大小、方向、作用线四要素规定，如力矢量。对于自由矢量：在不同坐标系的描述只与旋转矩阵有关，与坐标原点的位置无关，对于速度矢量 对于力矩矢量BBAA VRV BBAA mRm 3-7、总结1、变换矩阵T的物理含义 1）、坐标系的描述： 如坐标系B相对于A的位姿； 2）、同一点在不同坐标系A和B间的影射关系； 3）、运动算子： 表示同一坐标系中点运动前后的算子关系，如平移算子、 旋转算子。 10 PRT BoABABA2、变换矩阵T的相乘 矩阵相乘的顺序一般不可换，特殊可换的情况为变换都是同参考系下的平移或绕同一坐标轴的旋转。 3、变换矩阵求逆例：两坐标系A和B，B先绕A的Z轴转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