机器人原理及控制技术：3-3第3章-位姿描述和齐次变换

1、第三章 位姿描述和齐次变换3.3 齐次坐标及齐次坐标变换一、齐次坐标 所谓齐次坐标就是将一个原本是n维的向量用一个n+1维向量来表示。例如，二维点(x,y)的齐次坐标表示为(hx,hy,h)。由此可以看出，一个向量的齐次表示是不唯一的，齐次坐标的h取不同的值都表示的是同一个点，比如齐次坐标(8,4,2)、(4,2,1)表示的都是二维点(4,2)。 不同时为0的任意4个数称为 三维空间点的齐次坐标。齐次坐标与点的笛卡尔坐标的关系为：a= , b= , c= ，w为比例系数 显然，齐次坐标表达并不是唯一的，随w值的不同而不同。在计算机图学中，w 作为通用比例因子，它可取任意正值，但在机器人的运动分

2、析中，总是取w=1 。1、矢量的齐次坐标三维空间矢量式中i, j, k为x, y, z 轴上的单位矢量，V的齐次坐标表示式为例：可以表示为： V=3 4 5 1T 或 V=6 8 10 2T 或 V=-12 -16 -20 -4T 几个特定意义的齐次坐标：0, 0, 0, 1T坐标原点矢量的齐次坐标， 1 0 0 0TOX轴线上无穷远点；0 1 0 0TOY轴线上无穷远点； 0 0 1 0TOZ轴线上无穷远点；机器人研究所6第3节 坐标变换平移坐标变换 (Translation Transform)坐标系B与A方向相同，但原点不重合。图4 平移变换 此式称为平移方程。其中 是B系原点OB在A系

3、中的表示。PY1X1Z1Y2X2Z2Y3X3Z3三坐标的直角坐标机器人适用的机器人类型举例（有平移关节）YXZ机器人研究所9第3节 坐标变换旋转坐标变换 (Rotation Transform)B与A有共同的坐标原点，但方位不同。图5 旋转变换P机器人研究所10第3节 坐标变换复合变换 (Composite Transform) 图6 复合变换P机器人研究所11第4节 齐次坐标变换齐次坐标和齐次变换坐标变换 ，式中对于点 是非齐次的，将其等价为齐次变换形式：等价于齐次变换直角坐标齐次坐标机器人研究所12第4节 齐次坐标变换齐次坐标和齐次变换 称为齐次变换矩阵，对它有以下物理理解：描述坐标系 B

4、 相对于坐标系 A 的位姿；代表同一点 P 在两个坐标系 A 和 B 中描述之间的映射关系；表示同一坐标系中，点 P 运动前后的位姿关系。二、笛卡尔坐标系的齐次坐标变换笛卡尔坐标系 中的点 向另一坐标系 变换，变换后的坐标系 由下式计算： 式中 ：坐标系 的原点在坐标系 的坐标；：坐标系 的 轴对坐标系 的3个方向余弦；：坐标系 的 轴对坐标系 的3个方向余弦；：坐标系 的 轴对坐标系 的3个方向余弦；若 是 系的齐次坐标； 是 系的齐次坐标，则将此式写成矩阵 形式，有上式T是一个44阶矩阵，称为笛卡尔坐标系的齐次变换矩阵，它沟通了两个坐标系的关系，表示了在坐标系 中的点 ，经T变换后变成了坐

5、标系 中的点X意义：左上角的33矩阵 是两个坐标系之间的旋转变换矩阵，它描述了姿态关系；右上角的31矩阵 是两个坐标系之间的平移变换矩阵，它描述了位置关系，所以齐次坐标变换矩阵又称为位姿矩阵。 例题已知坐标系 向 系的齐次坐标变换如下式：试决定 系的原点在 系中的位置。机器人研究所18例2.1 已知坐标系B的初始位姿与A重合，首先B相对于坐标系A的zA轴转30，再沿A的xA轴移动12单位，并沿A的yA轴移动6单位。求位置矢量ApB0和旋转矩阵 。假设点p在坐标系B的描述为Bp=3,7,0T，求它在坐标系A中的描述Ap。18第4节 齐次坐标变换解：机器人研究所19例2.1 已知坐标系B的初始位姿

6、与A重合，首先B相对于坐标系A的zA轴转30，再沿A的xA轴移动12单位，并沿A的yA轴移动6单位。求位置矢量ApB0和旋转矩阵 。假设点p在坐标系B的描述为Bp=3,7,0T，求它在坐标系A中的描述Ap。19第4节 齐次坐标变换解：机器人研究所20第4节 齐次坐标变换例2.2 齐次变换矩阵描述坐标系B相对于A的位姿，平移变换矩阵旋转变换矩阵 表示绕过坐标原点的轴k旋转角度。 任何一个齐次坐标变换矩阵均可分解为一个平移变换矩阵与一个旋转变换矩阵的乘积，即： 机器人研究所24第3节 齐次坐标变换平移齐次变换(Homogeneous Transformation of Translation)A分

7、别沿B的X、Y、Z坐标轴平移a、b、c距离的平移齐次变换矩阵写为：机器人研究所25第4节 齐次坐标变换平移齐次变换(Homogeneous Transformation of Translation)对已知矢量 u=x, y, z, 1T 进行平移变换所得的矢量 v 为：机器人研究所26第4节 齐次坐标变换旋转齐次变换(Homogeneous Transformation of Rotation)机器人研究所27第4节 齐次坐标变换复合变换给定坐标系A，B和C，已知B相对A的描述为 ，C相对B的描述为 ，则有 同理可有：即一个坐标系变换至另一坐标系的齐次变换矩阵等于依次经历中间坐标系各齐次变换

8、矩阵的连乘积。 机器人研究所2828例2.3 已知点 u=7, 3, 2T ，将 u绕 z 轴旋转90得到点 v，再将点 v 绕 y轴旋转90得到点w，求点v、w的坐标。解：第4节 齐次坐标变换旋转变换机器人研究所2929例2.3 已知点 u=7, 3, 2T ，将 u绕 z 轴旋转90得到点 v，再将点 v 绕 y轴旋转90得到点w，求点v、w的坐标。解：如果把上述两变换组合在一起第4节 齐次坐标变换旋转变换机器人研究所30若改变旋转次序，首先使 u 绕 y 轴旋转90，再绕 z 轴旋转90，会使 u 变换至与 w 不同的位置w1。第4节 齐次坐标变换旋转次序对结果的影响机器人研究所31第4

9、节 齐次坐标变换例2.4 已知点u=7, 3, 2T ，将 u绕 z 轴旋转90得到点 v，再将点 v 绕 y轴旋转90得到点w，最后进行平移变换4, -3, 7T ，求最终的坐标。解：将上述三个变换组合在一起平移变换和旋转变换组合vwunzoyx机器人研究所32第4节 齐次坐标变换例2.4 已知点u=7, 3, 2T ，将 u绕 z 轴旋转90得到点 v，再将点 v 绕 y轴旋转90得到点w，最后进行平移变换4, -3, 7T ，求最终的坐标。解：将上述三个变换组合在一起平移变换和旋转变换组合zoyxvwun机器人研究所33第5节 齐次变换的性质1、变换过程的相对性绕固定坐标系依次进行的坐标

10、系转换，各齐次变换矩阵按“从右向左”依次相乘原则进行运算（右乘）. 3 2 1 坐标系的运动方式：B的初始方位与坐标系A重合，首先使B绕 xA旋转 角，再绕 yA转 角，最后绕zA转 角。机器人研究所34第5节 齐次变换的性质1、变换过程的相对性绕动坐标系依次进行的齐次变换，按“从左向右”的原则依次相乘(左乘）。 坐标系的运动方式：B的初始方位与坐标系A重合，首先使B绕 zB旋转 角，再绕 yB转 角，最后绕 xB转 角。机器人研究所35相对于固定坐标系运动相对于活动坐标系运动第5节 齐次变换的性质1、变换过程的相对性结论：1）变换顺序从右至左，运动是相对于固定参考系而言的；2）变换顺序从左至

11、右，运动是相对于运动坐标系而言的。方式1：方式2：2、变换过程的可逆性机器人研究所37第5节 齐次变换的性质2、变换过程的可逆性齐次变换过程是可逆的，逆变换就是使被变换的动坐标系返回到固定坐标系中。例如： 将被变换坐标系变回到原来的坐标系时，可以用变换T的逆 来实现。 例如X=TC 使C变换为X，若用X求C则为 X= TC=C=C式中为单位矩阵。2、变换过程的可逆性6、逆变换齐次变换的逆变换：齐次变换矩阵对于给定的 求等价于给定 和 计算6、逆变换若齐次坐标变换矩阵为：6、逆变换6、逆变换机器人研究所43第5节 齐次变换的性质从逆方向去看图，固定系的 x轴与动系的 z 轴方向一致，故x轴在动系

12、中可表示为0, 0, 1, 0T，同样固定系的 y 轴可表示为1, 0, 0, 0 T，z轴可表示为0, 1, 0, 0 T，而固定系的原点可表示为3, -7, -4 , 1 T 。2、变换过程的可逆性T表示B与A之间的变换，也即B在A中的描述；下面从另一角度分析一下A在B中的描述。机器人研究所44第5节 齐次变换的性质2、变换过程的可逆性于是，A在B系中的描述为：容易验证机器人研究所45第5节 齐次变换的性质2、变换过程的可逆性齐次变换逆变换的公式：已知变换矩阵为:其逆变换矩阵为：机器人研究所46第5节 齐次变换的性质2、变换过程的可逆性例题：已知齐次矩阵为：求 A-1机器人研究所47第5节

13、 齐次变换的性质2、变换过程的可逆性例题：已知齐次矩阵为：求 A-1解： -Pn = -01-02-(-1)3=3 -Po = -01-12-03= -2 -Pa = -11-02-03= -1则有第1节 位置和姿态的表示第2节 坐标变换第3节 齐次坐标变换第4节 齐次变换的性质第5节 旋转变换通式第三章 位姿描述和齐次变换机器人研究所49第6节 旋转变换通式1、旋转变换通式一般旋转变换指旋转轴线不与参考坐标系中的任何轴线重合，而是参考系中过原点的某一矢量，这一矢量的方向用单位矢量 表示。令 是过A系原点的单位矢量，求绕K旋转角到B系的旋转矩阵R(K,)，即 机器人研究所50第6节 旋转变换通

14、式1、旋转变换通式设K是某坐标系C的Z轴的单位向量，并设：这样，绕矢量k旋转就等于绕坐标系C的Z轴旋转，即 Rot(K,)=Rot(ZC,) 机器人研究所51第6节 旋转变换通式1、旋转变换通式如果被旋转的坐标系以参考坐标系描述时，记为Y,以坐标系C为参考系时记为X，Y与X的关系为：绕k轴旋转Y等效于绕坐标系C的Z轴旋转X,即将 代入得：机器人研究所52第6节 旋转变换通式1、旋转变换通式当kx=1, ky=kz=0时，即 K为 x 轴，此时其中：机器人研究所53第6节 旋转变换通式2、 等效转轴与等效转角任何一组经过有限次基本旋转变换后的复合旋转总可以等效成绕某一过原点的轴线转角的单一旋转。对于给定的旋转矩阵机器人研究所54第6节 旋转变换通式2、 等效转轴与等效转角球等效转轴 K 和等效转角，即解下面的方程组。机器人研究所55机器人研究所56第5节 旋转变换通式2、 等效转轴与等效转角例题：求复合变换 的等效转轴K和转角。解：1. 计算旋转矩阵 机器人研究所57第6节 旋