机器人学机器人学战 强北京航空航天大学机器人研究所第第三三章章 机器人的位姿描述与坐标变换机器人的位姿描述与坐标变换第三章第三章 机器人的位姿描述与坐标变换机器人的位姿描述与坐标变换机器人 的位姿连杆I的 位姿Y XZYi XiZiYw XwZw3-1 刚体位姿的数学描述刚体位置 ：刚体姿态 ：单位主矢量￥ ￥假设机器人的连杆和关节都是刚体￥ ￥ OXYZZY XObnt9个元素，只有3个独立， 满足6个约束条件:刚体的位置和姿态：R是单位正交阵姿态矩阵R的特点：jZjXjYiZiXiYOiOj例：某刚体j在参考系i中的位置姿态1063-2 坐标变换（点的映射） 1、坐标平移（坐标系方位相同）jZjXjYPiZiXiYOiOj沿着不同轴向的组合平移：已知点P在j坐标系的坐标，平移j至i，求 点P在i坐标系的坐标。Y1 X1Z1Y2 X2Z2Y3 X3Z3三坐标的直角坐标机器人适用的机器人类型举例（有平移关节）YXZiZiXiYOijZjXjYPOj15例: 已知 求 P点在i坐标系中的坐标。 解答：2、坐标旋转（坐标系原点相同）ZiXiYiZjXjYjP坐标系j由坐标系i旋转而成求点P在i坐标系的坐标：已知点P在j坐标系的坐标：ZiZjXiXjYiYjPZiZjXiXjYiYjP姿态矢量矩 阵OXYZZY XObnt坐标系j相对 于i的方位旋转矩阵的性质：旋转矩阵绕一个坐标轴旋转的转动矩阵jZiZiXjYiYqqjXjZiZiXjYiYqqjXjZiZiXjYiYqqjX1）RX2）RY3）RZjZiZiXjYiYqqjXjZiZiXjYiYqqjXjZiZiXjYiYqqjX转动矩阵的特点：(1) 主对角线上有一个元素为1，其余均为转角的余弦/正弦；(2) 绕轴转动的次序与元素1所在的行、列号对应；(3) 元素1所在的行、列，其它元素均为0；(4) 从元素1所在行起，自上而下，先出现的正弦为负，后出现 的为正，反之依然。绕多个坐标轴旋转的转动矩阵1）、绕固定坐标系旋转坐标系坐标系坐标系iZiXjYiYqajXa qmZmXmYqjZ2）、绕运动坐标系旋转ZYZ欧拉角坐标系坐标系坐标系坐标系iX1XjX2XiYjY1Y2（Y )(1ZZijZ2Zjjqqffjfq注意：多个旋转矩阵连乘时，次序不同则含义不同。1）绕新的动坐标轴依次转动时，每个旋转矩阵要从左往右乘，即旋转矩阵的相乘顺序与转动次序相同；2）绕旧的固定坐标轴依次转动时，每个旋转矩阵要从右往左乘，即旋转矩阵的相乘顺序与转动次序相反。证明： 坐标系坐标系坐标系坐标系1）绕运动坐标系旋转iX1XjX2XiYjY1Y2（Y )(1ZZijZ2Zjjqqffjfq2）、绕固定坐标系旋转坐标系坐标系坐标系iZiXjYiYqajXa qmZmXmYqjZ证明与讨论:适用的机器人类型举例（有旋转关节）Y6X6Z6Y0 X0Z0Y7X7Z7例1: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z 轴转30度, 假设点P在 坐标系B的描述为PB=3,7,0T,求它在坐标 系A中的描述PA.3、坐标变换综合（平移+旋转）旋转部分平移部分推导(中间坐标系C)：iZiXiYOiZjXjYjPOjcZcXcYI（旋转）: c与j 原点重合，c与i姿态相同II（平移）: c与i 原点重合问题：是否可以先平移后旋转？iZiXiYOiZjXjYjPOjcZcXcY推导(中间坐标系C)：I（平移）: c与i原点重合，c与j姿态相同II（旋转）: c与i 姿态相同未知例1: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z 轴转30度,再沿A的X轴移动10个单位,并沿A的Y轴移动5个单位. 假设点P在 坐标系B的描述为PB=3,7,0T,求它在坐标系A中的 描述PA.AZAXAYOiPOjBZBXBY3-3 齐次坐标与齐次变换1、齐次坐标齐次坐标直角坐标1）点的齐次坐标：非零的比例因子2）坐标轴方向的齐次坐标：X轴：a,b,c称为方向数Y轴：Z轴：坐标原点无意义点AZAXAYOiiZiXiYOiOj2、齐次变换点P在i坐标系中的齐次坐标 ：点P在j坐标系中的齐次坐标 ：旋转矩阵与平移向量构成的齐次变换矩阵：齐次变换矩阵表示了坐标系j相对于坐标系i的位姿的含义：旋转的齐次变换平移加旋转变换平移的齐次变换iZiXiYOiZjXjYjPOjcZcXcYiZiXiYOiZjXjYjPOjcZcXcY例2: 已知坐标系B初始位姿与A重合,首先B相对于坐标系A的Z轴转30度,再沿A的X轴移动10个单位,并沿A的Y轴移动5个单位.假设点P在 坐标系B的描述为PB=3,7,0T,求它在坐标系A中的描述PA.解法1:解法2:练习题1：已知坐标系A初始位姿与B重合,首先A相对于坐标系B的Z轴转 30度,再沿B的X轴移动10个单位, 再相对于A的Y轴转60度，并沿 A的Z轴移动5个单位. 假设点P在坐标系A的描述为 =12,0,4T, 求它在坐标系B中的描述 ；解法1:3-4 旋转变换通式1、旋转矩阵通式：K旋转矩阵1)、坐标系j 相对于 i的姿态2)、定义两个坐标系i和j，i与i固连， j与j 固连； i和j 的Z轴与矢量K重合，旋转前，i与j重合，i与j 重合。绕通过原点的任意单位矢量K转 角的旋转矩阵3)、K绕K转 角j相对于i的Z轴转 角旋转变换的尺寸链图：iijj 利用旋转矩阵的正交性质:假设：整理得：旋转变换通式讨论：（1）（2）（3）例：坐标系B原来与A重合，将坐标系B绕过原点O的轴线转动，求旋转矩阵解答：1）2）3）带入旋转通式得：2、等效转轴与等效转角转轴和转角旋转矩阵12？1）将方程两边矩阵的主对角线元素分别相加，则2）将方程两边矩阵的非对角线元素成对相减得：将上式两边平方相加得：求得转角求得转轴 注意：1）多值性：2）存在病态情况：例：解：利用前面的公式可求得：任何一组绕过原点的轴线的复合转动 总等效于绕过原点的某一矢量的转动3、齐次变换通式 *讨论矢量K不通过原点的情况假设单位矢量 通过点求绕矢量K转 角的齐次变换矩阵1)、定义两个坐标系i和j，坐标 原点在P点，i与i固连， j与j 固连 ； i和j 的坐标轴分别与i和j的坐标 轴平行，旋转前，i与j重合， j与i 重合。KP2）旋转变换的尺寸链图：iijj例：坐标系B原来与A重合，将坐标系B绕过点P的矢量转动，该矢量经过点解答：1）2）带入旋转通式得：，求旋转矩阵。反之，如果求P点， 其值不唯一。3-5 机器人姿态的其他表示方法前面：用3*3的旋转矩阵表示刚体的姿态单位主矢量 旋转矩阵有9个元素，6个约束条件，3个独立元素； 计算编程时需要输入9个元素，不方便； 一般采用3个元素来表示。1、RPY角：（绕固定坐标轴X-Y-Z旋转）RPY角是描述船舶在海中航行时姿态的一种方法。XYZR（Z， ）：RollR（Y， ）：PitchR（X， ）：Yaw 翻滚偏航俯仰（1）船舶上建立的坐标系B相对于参考系A的方位描述如下：A和B 重合，首先，将B绕XA 转 角，再绕YA转 角 ，最后绕ZA转 角。 （2）如果已知机器人的姿态矩阵，如何求RPY角？双变量反正切函数if通常的选择if if if2、ZYX欧拉角：（绕动坐标轴Z-Y-X旋转）iX1XjX2XiYjY1Y2（Y )(1ZZijZ2Zjjqqffjfq坐标系B相对于参考系A的方位描述如下：RPY角3、ZYZ欧拉角已知姿 态矩阵 求欧拉 角坐标系B相对于参考系A的方位描述如下：上述描述姿态的方法称为角度设定法，共有24种，其中12种 RPY法和12种欧拉法，并且是对偶的，实际上只有12种不同 的旋转矩阵。确定旋转时是绕固定轴还是动轴非常重要。X Y Z的排列:ZXZXYZ3-6、自由矢量的变换前面讨论的是位置矢量的变换，如旋转、平移等；如何处理速度矢量、力矢量的变换问题？矢量分类： 1、自由矢量：由维数、大小、方向三要素规定，如速度矢量、纯力矩矢量； 2、线矢量：由维数、大小、方向、作用线四要素规定，如力矢量。对于自由矢量： 在不同坐标系的描述只与旋转矩阵有关，与坐标原点的位置 无关，对于速度矢量对于力矩矢量3-7、总结1、变换矩阵T的物理含义1）、坐标系的描述：如坐标系B相对于A的位姿；2）、同一点在不同坐标系A和B间的影射关系；3）、运动算子：表示同一坐标系中点运动前后的算子关系，如平移算子、旋转算子。2、变换矩阵T的相乘矩阵相乘的顺序一般不可换，特殊可换的情况为变换都是同参考系下的平移或绕同一坐标轴的旋转。3、变换矩阵求逆 例：两坐标系A和B，B先绕A的Z轴转30度，再沿A的X轴移动4 个单位，沿A的Y轴移动3个单位，已知 ，求 。解：1）2）