3 3 刚体位姿描述和齐次变换刚体位姿描述和齐次变换n n在研究机器人运动学时，常常涉及到物在研究机器人运动学时，常常涉及到物体在空间的位置和姿态，这里所指的物体在空间的位置和姿态，这里所指的物体包括机器人的连杆、工具和工件等等。体包括机器人的连杆、工具和工件等等。n n为了描述物体的位姿，首先要建立参考为了描述物体的位姿，首先要建立参考坐标系（坐标系（Reference Coordinate System或或Fixed Frame）。）。参考坐标系一般设置为参考坐标系一般设置为直角坐标系，特殊情况下也可以采用其直角坐标系，特殊情况下也可以采用其他坐标系。他坐标系。2021/6/713.1 刚体位姿描述刚体位姿描述3.1.1 位置的描述（位置矢量）位置的描述（位置矢量）2021/6/72n n在运动学中，可以认为物体是刚体。如在运动学中，可以认为物体是刚体。如图，设置参考坐标系图，设置参考坐标系OXYZ，在刚体上在刚体上建立附体直角坐标系建立附体直角坐标系QUVW（称为运动称为运动坐标系坐标系Moving Frame）n n在直角坐标系中，一个点的位置可以用在直角坐标系中，一个点的位置可以用31位置矢量来表示。位置矢量来表示。n n如点如点P的位置可的位置可n n以写成以写成2021/6/73n n在涉及到多个坐标系时，为了注明是在在涉及到多个坐标系时，为了注明是在哪一个坐标系中，可以利用左上标表示，哪一个坐标系中，可以利用左上标表示，如如n n表示该矢量是在坐标系表示该矢量是在坐标系A中定义的。中定义的。2021/6/743.1.2 姿态（方位）的描述（旋转矩阵）姿态（方位）的描述（旋转矩阵）n n为了规定空间某刚体为了规定空间某刚体B的方位，在刚体上的方位，在刚体上固联一直角坐标系固联一直角坐标系B，用坐标系用坐标系B的的三个单位主矢量三个单位主矢量 、 、 相对于坐标系相对于坐标系A的方向余弦组成的的方向余弦组成的33矩阵矩阵 或或2021/6/75n n来表示刚体来表示刚体B相对于参考坐标系相对于参考坐标系 A 的的方位或姿态。称为旋转矩阵。当相对关方位或姿态。称为旋转矩阵。当相对关系明确时，可以去掉左侧的上下标。系明确时，可以去掉左侧的上下标。n n旋转矩阵是正交矩阵，其旋转矩阵是正交矩阵，其9个元素中只有个元素中只有三个是独立的。三个是独立的。n n绕绕x轴、轴、 y轴、轴、 z轴旋转轴旋转角的旋转矩阵分角的旋转矩阵分别为别为2021/6/762021/6/77n n3.1.3 位姿的描述位姿的描述n n为了描述刚体为了描述刚体B在空间的位置和姿态，通在空间的位置和姿态，通常将表示姿态的坐标系常将表示姿态的坐标系B固联在刚体固联在刚体B上，其原点选择有特征的点，如质心或某上，其原点选择有特征的点，如质心或某一顶点、中点等。坐标系一顶点、中点等。坐标系B的原点在的原点在参考坐标系参考坐标系A的位置矢量称为刚体的位置矢量称为刚体B在在参考坐标系中的位置，坐标系参考坐标系中的位置，坐标系B 在参在参考坐标系中的姿态矩阵称为刚体在参考坐考坐标系中的姿态矩阵称为刚体在参考坐标系中的姿态。可以写成标系中的姿态。可以写成2021/6/78 3.2 坐标变换坐标变换（Coordinate Transformation） 空间中一点在不同的坐标系中的描述空间中一点在不同的坐标系中的描述是不同的。为了在不同坐标系中正确地描是不同的。为了在不同坐标系中正确地描述同一点，就需要采用坐标变换。述同一点，就需要采用坐标变换。3.2.1 坐标平移变换坐标平移变换 设坐标系设坐标系B与与A具有相同的方位，具有相同的方位，但原点不重合，坐标系但原点不重合，坐标系B在参考坐标在参考坐标系系 A中的位置矢量为中的位置矢量为 ，点，点p在在 B 中的位置矢量为中的位置矢量为 ，则它相对于参，则它相对于参考坐标系考坐标系A的位置矢量为的位置矢量为2021/6/792021/6/7103.2.2 坐标旋转变换坐标旋转变换2021/6/711n n设坐标系设坐标系B与与A具有共同的原点，但两具有共同的原点，但两者的方位不同，点者的方位不同，点p在坐标系在坐标系B 中的位置中的位置矢量为矢量为 ，则该点在参考坐标系，则该点在参考坐标系 A 中中的位置矢量为的位置矢量为n n式中式中 为坐标系为坐标系B相对于坐标系相对于坐标系 A 的姿的姿态矩阵，或称旋转矩阵。态矩阵，或称旋转矩阵。2021/6/7123.3 齐次坐标和齐次变换齐次坐标和齐次变换3.3.1 点的齐次坐标点的齐次坐标三维空间中某点的坐标可以写成三维空间中某点的坐标可以写成则该点的齐次坐标为则该点的齐次坐标为其中其中w是一不为是一不为0的数。的数。2021/6/713齐次坐标是用四个坐标值表示三维空间中的点齐次坐标是用四个坐标值表示三维空间中的点齐次坐标是用四个坐标值表示三维空间中的点齐次坐标是用四个坐标值表示三维空间中的点当当当当w w变化时，该式表示三维空间中一系列点的集变化时，该式表示三维空间中一系列点的集变化时，该式表示三维空间中一系列点的集变化时，该式表示三维空间中一系列点的集合，所有点都位于坐标系原点和合，所有点都位于坐标系原点和合，所有点都位于坐标系原点和合，所有点都位于坐标系原点和p p点的连线上，点的连线上，点的连线上，点的连线上，当当当当w w等于无穷大时，表示原点；当等于无穷大时，表示原点；当等于无穷大时，表示原点；当等于无穷大时，表示原点；当w w等于等于等于等于0 0时，时，时，时，表示无穷远处的点，代表一个方向。表示无穷远处的点，代表一个方向。表示无穷远处的点，代表一个方向。表示无穷远处的点，代表一个方向。一般用一般用一般用一般用 ， ，表示表示表示表示x,y,zx,y,z三个坐标轴的方向，而用三个坐标轴的方向，而用三个坐标轴的方向，而用三个坐标轴的方向，而用表示原点。表示原点。表示原点。表示原点。2021/6/7143.3.2 3.3.2 齐次变换矩阵齐次变换矩阵齐次变换矩阵齐次变换矩阵（Homogeneous Transformation MatrixHomogeneous Transformation Matrix）所谓齐次变换矩阵是一所谓齐次变换矩阵是一所谓齐次变换矩阵是一所谓齐次变换矩阵是一4444矩阵，是由姿态矩阵矩阵，是由姿态矩阵矩阵，是由姿态矩阵矩阵，是由姿态矩阵和位置矩阵合成的，具体合成形式如下：和位置矩阵合成的，具体合成形式如下：和位置矩阵合成的，具体合成形式如下：和位置矩阵合成的，具体合成形式如下：左上角左上角左上角左上角3333矩阵为姿态矩阵；右上角矩阵为姿态矩阵；右上角矩阵为姿态矩阵；右上角矩阵为姿态矩阵；右上角3131矩阵为位矩阵为位矩阵为位矩阵为位置矩阵。齐次变换矩阵可以将刚体的位置和姿置矩阵。齐次变换矩阵可以将刚体的位置和姿置矩阵。齐次变换矩阵可以将刚体的位置和姿置矩阵。齐次变换矩阵可以将刚体的位置和姿态用一个矩阵表达。态用一个矩阵表达。态用一个矩阵表达。态用一个矩阵表达。2021/6/715齐次变换矩阵的用途齐次变换矩阵的用途齐次变换矩阵的用途齐次变换矩阵的用途例例例例1 1 点的旋转：参考坐标系中有一点点的旋转：参考坐标系中有一点点的旋转：参考坐标系中有一点点的旋转：参考坐标系中有一点求该点绕求该点绕求该点绕求该点绕z z轴旋转轴旋转轴旋转轴旋转9090后的新位置。后的新位置。后的新位置。后的新位置。解：绕解：绕解：绕解：绕z z轴旋转轴旋转轴旋转轴旋转9090的齐次变换矩阵为的齐次变换矩阵为的齐次变换矩阵为的齐次变换矩阵为2021/6/716旋转后的新位置为旋转后的新位置为旋转后的新位置为旋转后的新位置为例例例例2 2 点的平移：将上例中的点的平移：将上例中的点的平移：将上例中的点的平移：将上例中的p p点沿点沿点沿点沿x x轴平移轴平移轴平移轴平移3 3个单位，个单位，个单位，个单位，沿沿沿沿y y轴平移轴平移轴平移轴平移5 5个单位，沿个单位，沿个单位，沿个单位，沿z z轴平移轴平移轴平移轴平移7 7个单位，求平移个单位，求平移个单位，求平移个单位，求平移后点的新坐标。后点的新坐标。后点的新坐标。后点的新坐标。解：本例中的齐次变换矩阵为解：本例中的齐次变换矩阵为解：本例中的齐次变换矩阵为解：本例中的齐次变换矩阵为2021/6/717平移后的新位置为平移后的新位置为平移后的新位置为平移后的新位置为例例例例3 3 例例例例1 1中的点中的点中的点中的点p p首先绕首先绕首先绕首先绕z z轴旋转轴旋转轴旋转轴旋转90,90,然后平移然后平移然后平移然后平移3,5,73,5,7个单位。个单位。个单位。个单位。解：作上述旋转平移的齐次变换矩阵为解：作上述旋转平移的齐次变换矩阵为解：作上述旋转平移的齐次变换矩阵为解：作上述旋转平移的齐次变换矩阵为2021/6/718变换后点的新位置为变换后点的新位置为本例中，如果先平移，后旋转，结果又如何本例中，如果先平移，后旋转，结果又如何？2021/6/7193.3.3 齐次变换矩阵的运算齐次变换矩阵的运算 通过上述例子可以看出，平移变换和旋通过上述例子可以看出，平移变换和旋转变换都可以通过齐次变换矩阵来实现。转变换都可以通过齐次变换矩阵来实现。平移齐次变换矩阵平移齐次变换矩阵2021/6/720旋转齐次变换矩阵旋转齐次变换矩阵2021/6/721 当连续进行多次变换时，其变换结果当连续进行多次变换时，其变换结果是几个变换矩阵的乘积。当变换相对于是几个变换矩阵的乘积。当变换相对于固定坐标系（即参考坐标系）时，按变固定坐标系（即参考坐标系）时，按变换顺序将矩阵左乘；当变换相对于运动换顺序将矩阵左乘；当变换相对于运动坐标系（即新坐标系）时，按变换顺序坐标系（即新坐标系）时，按变换顺序将矩阵右乘。将矩阵右乘。2021/6/722例例3.4 某一坐标系某一坐标系O1UVW变换前与参考变换前与参考 坐标系重合，首先将其绕坐标系重合，首先将其绕X轴旋转轴旋转90， 然后将其平移然后将其平移5，3，7，1T；求变换求变换 后的新位姿。后的新位姿。解：变换前运动坐标系与参考系重合，所解：变换前运动坐标系与参考系重合，所 以其位姿矩阵为以其位姿矩阵为2021/6/723 由于变换是相对于参考系进行，所以变换后新由于变换是相对于参考系进行，所以变换后新由于变换是相对于参考系进行，所以变换后新由于变换是相对于参考系进行，所以变换后新坐标系的位姿为坐标系的位姿为坐标系的位姿为坐标系的位姿为2021/6/724例例例例3.5 3.5 上例中，如果变换均相对于运动坐标系进行，上例中，如果变换均相对于运动坐标系进行，上例中，如果变换均相对于运动坐标系进行，上例中，如果变换均相对于运动坐标系进行， 求变换后的新位姿。求变换后的新位姿。求变换后的新位姿。求变换后的新位姿。解：由于变换相对于运动坐标系进行，解：由于变换相对于运动坐标系进行，解：由于变换相对于运动坐标系进行，解：由于变换相对于运动坐标系进行，2021/6/725n n3.3.4 3.3.4 齐次变换矩阵的逆矩阵齐次变换矩阵的逆矩阵齐次变换矩阵的逆矩阵齐次变换矩阵的逆矩阵n n齐次变换矩阵由于其本身的特点，其逆矩阵的齐次变换矩阵由于其本身的特点，其逆矩阵的齐次变换矩阵由于其本身的特点，其逆矩阵的齐次变换矩阵由于其本身的特点，其逆矩阵的求法比较简单。求法比较简单。求法比较简单。求法比较简单。n n则其逆矩阵为则其逆矩阵为则其逆矩阵为则其逆矩阵为2021/6/7263.4 姿态的表示方法姿态的表示方法 前述用前述用33旋转矩阵表示刚体（运动坐标旋转矩阵表示刚体（运动坐标系）的姿态，由于该矩阵的正交性，其系）的姿态，由于该矩阵的正交性，其9个元素中只有个元素中只有3个是独立的。下面介绍个是独立的。下面介绍RPY姿态表示法和欧拉角姿态表示法。姿态表示法和欧拉角姿态表示法。2021/6/7273.4.1 RPY表示法表示法 RPY角是描述船舶在海中航行时姿态的角是描述船舶在海中航行时姿态的一种方法。如图，一种方法。如图， 将绕将绕z轴的旋转称为轴的旋转称为回转（回转（Roll），）， 将绕将绕y轴的旋转称为俯轴的旋转称为俯仰（仰（Pitch），），将绕将绕x轴的旋转称为偏转轴的旋转称为偏转（Yaw）。）。操作臂手爪姿态的规定如此操作臂手爪姿态的规定如此相似，相似， 沿手指向外为沿手指向外为z轴，轴， 两手指连线两手指连线的方向为的方向为y轴，轴，x轴按右手定则确定。轴按右手定则确定。2021/6/7282021/6/729采用采用采用采用RPYRPY方法描述手爪姿态的规则如下：方法描述手爪姿态的规则如下：方法描述手爪姿态的规则如下：方法描述手爪姿态的规则如下：将手爪坐标系首先将手爪坐标系首先将手爪坐标系首先将手爪坐标系首先绕绕绕绕x x轴转动轴转动轴转动轴转动 角，然角，然角，然角，然后绕后绕后绕后绕y y轴转动轴转动轴转动轴转动 角，角，角，角，最后绕最后绕最后绕最后绕z z轴转动轴转动轴转动轴转动 角。角。角。角。而且每次转动都是而且每次转动都是而且每次转动都是而且每次转动都是相对于参考系进行相对于参考系进行相对于参考系进行相对于参考系进行的。的。的。的。2021/6/730由于每次转动都是相对于参考系进行的，按照矩阵由于每次转动都是相对于参考系进行的，按照矩阵由于每次转动都是相对于参考系进行的，按照矩阵由于每次转动都是相对于参考系进行的，按照矩阵左乘的规则，则获得的姿态矩阵为左乘的规则，则获得的姿态矩阵为左乘的规则，则获得的姿态矩阵为左乘的规则，则获得的姿态矩阵为2021/6/731其中，其中，其中，其中， ，依此类推。，依此类推。，依此类推。，依此类推。现在讨论姿态的逆问题（现在讨论姿态的逆问题（现在讨论姿态的逆问题（现在讨论姿态的逆问题（RPYRPY的逆解），即当位的逆解），即当位的逆解），即当位的逆解），即当位姿矩阵已知时，求姿矩阵已知时，求姿矩阵已知时，求姿矩阵已知时，求 ， ， 角度应为多少，角度应为多少，角度应为多少，角度应为多少，令令令令2021/6/732当当 时，可得时，可得当当 时，则逆解出现退化，只能求时，则逆解出现退化，只能求出出与与的和或差，的和或差， 此时可设定此时可设定值，值， 例如例如令令0，从而解出从而解出值。值。2021/6/7333.4.2 3.4.2 z-y-xz-y-x欧拉角欧拉角欧拉角欧拉角 此时姿态变换规则如下：首先绕运动此时姿态变换规则如下：首先绕运动此时姿态变换规则如下：首先绕运动此时姿态变换规则如下：首先绕运动坐标系坐标系坐标系坐标系z z轴转动轴转动轴转动轴转动 角，然后绕旋转后的角，然后绕旋转后的角，然后绕旋转后的角，然后绕旋转后的y y轴轴轴轴转动转动转动转动 角，最后绕再次旋转后的角，最后绕再次旋转后的角，最后绕再次旋转后的角，最后绕再次旋转后的x x轴转动轴转动轴转动轴转动 角。角。角。角。2021/6/7342021/6/735n n由于此时变换是相对于运动坐标系进行的，所以由于此时变换是相对于运动坐标系进行的，所以由于此时变换是相对于运动坐标系进行的，所以由于此时变换是相对于运动坐标系进行的，所以矩阵应按右乘顺序进行。矩阵应按右乘顺序进行。矩阵应按右乘顺序进行。矩阵应按右乘顺序进行。n n此时获得的姿态矩阵与此时获得的姿态矩阵与此时获得的姿态矩阵与此时获得的姿态矩阵与RPYRPY方法的完全一样，其方法的完全一样，其方法的完全一样，其方法的完全一样，其逆解公式也完全适用。逆解公式也完全适用。逆解公式也完全适用。逆解公式也完全适用。2021/6/7363.4.3 z-y-z欧拉角欧拉角此时姿态变换规则如下：首先绕运动坐标此时姿态变换规则如下：首先绕运动坐标系系z轴转动轴转动角，然后绕旋转后的角，然后绕旋转后的y轴转动轴转动角，最后绕再次旋转后的角，最后绕再次旋转后的z轴转动轴转动角。角。2021/6/737n n其逆解求解公式如下：当其逆解求解公式如下：当 ，2021/6/738部分资料从网络收集整理而来，供大家参考，感谢您的关注！