《机器人学基础》-第2章

第2章

机器人数学基础机器人的首要功能是通过自动控制完成各种作业的操作,进行作业操作的前提是描述机器人末端执行器和操作目标的空间位置和姿态。本章主要讲述刚体在空间中的描述方法,以及不同坐标系相互间转化的齐次变换方法,为后续的机器人运动学、动力学提供理论基础。2.1

刚体的位姿描述描述刚体的空间状态,首先要确定描述刚体所在的坐标系,即确定需要在哪个坐标系下描述刚体的空间状态,一个刚体在某一瞬时其空间状态是唯一的,但是在不同的坐标系下其描述是不同的。一个坐标系由原点和3个相互垂直的单位矢量组成,本书中坐标系均为右手系,如图所示。2.1

刚体的位姿描述刚体上的任何一点可以通过与其固连的坐标系描述,因此通过与刚体固连的坐标系可以完整地描述刚体的空间状态,如图所示。图中的四面体可以通过与之固连的坐标系{B}描述。2.1

刚体的位姿描述根据前述的坐标系的四个元素,坐标系{B}的原点

在坐标系{A}中的描述即为坐标系{B}在坐标系{A}中的位置。在本课程中位置用矢量表示,点在坐标系{A}的位置矢量

可以表示为其在坐标系{A}三个坐标轴上的投影矢量和。其中,XA

、YA

和ZA

分别代表坐标系{A}的三个坐标轴。

中的下标B代表坐标系{B},上标A代表在坐标系{A}下的描述。2.1

刚体的位姿描述坐标系{B}中三个坐标轴是三个单位矢量,在坐标系{A}三个坐标轴上的投影分别为2.1

刚体的位姿描述整理可得：定义

为旋转矩阵：2.1

刚体的位姿描述

是坐标系{B}的三个坐标轴在坐标系{A}中的表示。即若坐标系{B}与坐标系{A}初始位置重合,当坐标系{B}相对于坐标系{A}发生转动,此时坐标系{B}在坐标系{A}中描述就可以表示物体的姿态。如图所示,当坐标系{A}与坐标系{B}原点重合,由于P点的绝对位置不变,其在坐标系{A}和坐标系{B}的位置最终表示相等,P点在坐标系{A}和坐标系{B}的位置的描述为：2.1

刚体的位姿描述可得到：2.1

刚体的位姿描述化简得：由上式可知,P点在坐标系{B}下位置矢量转换到坐标系{A}下,只需左乘坐标系{B}在坐标系{A}下的旋转矩阵即可。进而可以得到,当坐标系{B}与坐标系{A}原点重合时,坐标系{B}下的任意矢量,在坐标系{A}下的描述均为坐标系{B}下的矢量左乘坐标系{B}在坐标系{A}下的姿态矩阵

。2.1

刚体的位姿描述由坐标系的定义可知,坐标系的轴线为三个相互垂直的单位矢量,则公式（2-5）中的三个坐标轴组成的矩阵和为正交矩阵,则姿态矩阵为两个正交矩阵的乘积,仍为正交矩阵,因此可知姿态矩阵

为正交矩阵。根据正交矩阵的特性,姿态矩阵的逆为其转置矩阵,则对于任意姿态矩阵R,可得2.1

刚体的位姿描述思考：如图所示,当坐标系{B}与坐标系{A}的原点不重合时,坐标系{B}在坐标系{A}下如何表示?2.1

刚体的位姿描述根据坐标系的4个元素基本元素,即原点位置和三个相互垂直的单位矢量,如果可以将坐标系的4个元素表示出,就可以实现坐标系{B}在坐标系{A}下描述。坐标系{B}原点在坐标系{A}中的位置为一个三维矢量,记为2.1

刚体的位姿描述坐标系{B}的三个相互垂直的矢量在坐标系{A}中的表示为姿态矩阵

。将原点位置和姿态矩阵结合,得到,便可以完整地描述坐标系{B}在坐标系{A}下的位置和姿态。但是为3行4列的矩阵,计算不方便,因此在该矩阵的最后增加一行[0001],将矩阵变为4行4列。该矩阵被称为齐次变换矩阵,由于齐次变换矩阵能够描述一个坐标系在另一个坐标系下的位置和姿态,因此又称位姿矩阵。一般用T

表示。2.1

刚体的位姿描述因此坐标系{B}在坐标系{A}下的齐次变换矩阵可以写为2.1

刚体的位姿描述如图所示,与坐标系{B}固连的任一点P,在坐标系{B}下的位置为,那怎么得到P点在坐标系{A}下的位置呢?2.1

刚体的位姿描述如图所示,可以看出P点在坐标系{A}下的位置由两个矢量相加便可以得到。即坐标系{B}的原点在坐标系{A}下的位置矢量和P点在坐标系{B}下的位置矢量相加。然而,两个矢量相加的必要条件是这两个矢量要在同一个坐标系下表示。因此首先要将P点在坐标系{B}下的位置矢量转换为在坐标系{A}下的位置矢量,根据公式(2-9),可以得到P点在坐标系{B}下的位置矢量在坐标系{A}下的位置矢量表示,则P点在坐标系{A}下的位置为2.1

刚体的位姿描述将

补一行,写为可以得到由上式可知，通过齐次变换矩阵,可以方便地计算得到一点在不同坐标系下的位置变换关系。2.1

刚体的位姿描述例2.1坐标系{A},{B},{C}如图所示,分别写出坐标系{B}和坐标系{C}相对于坐标系{A}的齐次变换矩阵

和,坐标系{D}相对于坐标系{A}的齐次变换矩阵

在图中画出坐标系{D}。2.1

刚体的位姿描述由齐次变换矩阵的定义和式(2-6)与式(2-11),可得到坐标系{D}在三角形柱上的表示如图所示。2.1

刚体的位姿描述综上所述,齐次变换矩阵,可以描述一个坐标系在另一个坐标系下的位置和姿态,同时可以计算一个坐标系上任一点在另一个坐标系下的位置。2.2

坐标系的齐次变换2.2.1基本旋转矩阵动坐标系O′UVW

与定坐标系OXYZ

的初始位置重合,动坐标系O′UVW

绕固定坐标系OXYZ

的X轴转动α角（如图）,根据公式(2-6),可以得到动坐标系O′UVW绕固定坐标系OXYZ

姿态矩阵,记为旋转矩阵R(X,α)。2.2

坐标系的齐次变换同理可得到动坐标系O′UVW绕定坐标系OXYZ的Y轴旋转β的姿态矩阵R(Y,β),和绕Z轴旋转γ的姿态矩阵R(Z,γ)等三个基本旋转矩阵2.2

坐标系的齐次变换2.2.2坐标系的相对变换和绝对变换如图所示,空间有三个坐标系{1}到{3},已知坐标系{2}在坐标系{1}下的旋转矩阵为,坐标系{3}在坐标系{2}下的旋转矩阵为

。根据式(2-5),可知可以得到即坐标系{3}在坐标系{1}下的旋转矩阵为2.2

坐标系的齐次变换坐标系{3}与坐标系{1}的相对位置关系可以由以下两种变换得到。变换方法1:空间有三个坐标系{1}到{3}初始位置重合,坐标系{2}和{3}一起相对于坐标系{1}旋转,此时坐标系{2}和{3}相对于坐标系{1}的旋转矩阵为;然后坐标系{3}相对于坐标系{2}旋转,此时坐标系{3}相对于坐标系{2}的旋转矩阵为

。两个旋转矩阵依据先后顺序右乘,得到坐标系{3}在坐标系{1}下的旋转矩阵

。2.2

坐标系的齐次变换变换方法2:空间有三个坐标系{1}到{3}初始位置重合,坐标系{3}相对于坐标系{1}和坐标系{2}旋转,此时坐标系{3}相对于坐标系{1}和坐标系{2}的旋转矩阵为;然后坐标系{2}与坐标系{3}一起相对于坐标系{1}旋转,此时坐标系{2}相对于坐标系{1}的旋转矩阵为

。两个旋转矩阵依据先后顺序左乘,得到坐标系{3}在坐标系{1}下的旋转矩阵

。2.2

坐标系的齐次变换如图所示,空间有三个坐标系{1}到{3},已知坐标系{2}在坐标系{1}下的齐次变换矩阵坐标系{3}在坐标系{2}下的齐次变换矩阵2.2

坐标系的齐次变换坐标系{3}在坐标系{2}下的齐次变换矩阵为

。根据齐次变换矩阵的定义和公式(2-13),可知坐标系{3}的原点在坐标系{1}下的位置另外通过计算可以得到简化可得：根据式(2-20),可得到坐标系{3}在坐标系{1}下的旋转矩阵

。根据齐次变换矩阵的定义,坐标系{3}在坐标系{1}下的齐次变换矩阵为2.2

坐标系的齐次变换坐标系{3}与坐标系{1}的相对位置关系也可以由以下两种变换得到。变换方法1:空间有三个坐标系{1}到{3}初始位置重合,坐标系{2}和{3}一起相对于坐标系{1}旋转和平移,此时坐标系{2}和{3}相对于坐标系{1}的旋转矩阵为;然后坐标系{3}相对于坐标系{2}旋转和平移,此时坐标系{3}相对于坐标系{2}的旋转矩阵为

。两个齐次变换矩阵依据先后顺序右乘,得到坐标系{3}在坐标系{1}下的齐次变换矩阵

。2.2

坐标系的齐次变换变换方法2:空间有三个坐标系{1}到{3}初始位置重合,坐标系{3}相对于坐标系{1}和坐标系{2}旋转和平移,此时坐标系{3}相对于坐标系{1}和坐标系{2}的旋转矩阵为;然后坐标系{2}与坐标系{3}一起相对于坐标系{1}旋转和平移,此时坐标系{2}相对于坐标系{1}的旋转矩阵为

。两个齐次变换矩阵依据先后顺序左乘,得到坐标系{3}在坐标系{1}下的齐次变换矩阵

。2.2

坐标系的齐次变换由此可以得到,动坐标系在固定坐标系中发生连续的齐次变换有2种情况:定义1:如果齐次变换是相对于固定坐标系中各坐标轴旋转或平移,则齐次变换为左乘,称为绝对变换。定义2:如果动坐标系相对于自身坐标系的当前坐标轴旋转或平移,则齐次变换为右乘,称为相对变换。2.2

坐标系的齐次变换例2.2坐标系{B}初始与坐标系{A}重合,坐标系{B}绕ZA

旋转-90°,再绕XB

旋转90°,最后沿YA

平移-7个单位,求此时坐标系{B}相对于坐标系{A}的齐次变换矩阵。坐标系{B}上固连一矢量,求此时P点在坐标系{A}下的位置。得到相同的坐标系{B}相对于坐标系{A}最终表示,还有什么变换方法?2.2

坐标系的齐次变换记坐标系{B}绕ZA

旋转-90°对应的齐次变换矩阵为T1,XB

旋转90°对应的齐次变换矩阵为T2,YA

平移-7个单位对应的齐次变换为T3。其中2.2

坐标系的齐次变换根据齐次变换的顺序,首先坐标系{B}绕ZA

旋转-90°,因为第一次变换直接写T1,接下来再绕XB

旋转90°,改变换为相对变换,因此为T1

右乘以T2,最后沿YA

平移-7个单位,该变换为绝对变换,因此再左乘T3。可以得到P点在坐标系{A}下的位置为2.2

坐标系的齐次变换由式(2-23),得到相同的变换,将式(2-23)从左往右看,即坐标系{B}做相对变换运动。整个变换过程如下:坐标系{B}与坐标系{A}初始重合,首先沿YB

平移-7个单位,然后绕ZB

旋转-90°,最后绕XB

旋转90°。将式(2-23)从右往左看,即坐标系{B}做绝对变换运动。整个变换过程如下:坐标系{B}与坐标系{A}初始重合,首先绕XA

旋转90°,然后绕ZA

旋转-90°,最后沿YA

平移-7个单位。将式(2-23)从中间往两边看,整个变换过程如下:坐标系{B}与坐标系{A}初始重合,首先绕ZB

旋转-90°,然后沿YA

平移-7个单位,最后绕XB

旋转90°。2.2

坐标系的齐次变换由例2.2可知,由于矩阵相乘不满足交换率,因此齐次变换和姿态变换顺序是不可调换的。2.2

坐标系的齐次变换2.2.3齐次变换的逆如图所示,坐标系{3}在坐标系{1}下的表示如式(2-22)所示,当坐标系{3}与坐标系{1}重合,此时,坐标系{3}在坐标系{1}下的齐次变换矩阵为单位矩阵。式(2-22)变为2.2

坐标系的齐次变换由式(2-25)可知,即为

的逆矩阵。假定

已知,则求的逆矩阵的问题即转化为求解

的问题。由式(2-25)可得则综合式(2-26)和式(2-27),可得到的逆为2.2

坐标系的齐次变换由式(2-28)可知,对于任意齐次变换矩阵其逆矩阵为2.3姿态的其他表示方法物体的姿态除了可以用旋转矩阵的描述方法外,根据不同的需要,还有很多其他的描述方法。2.3姿态的其他表示方法2.3.1固定角坐标系坐标系{2}和参考坐标系{1}初始位置重合,坐标系{2}首先绕X1

轴旋转γ角,然后绕Y1

轴旋转β角,最后绕Z1

旋转α角。每次旋转均为绕固定坐标系{1}的旋转轴。上述变换均为绕固定坐标系的轴线旋转,规定这种姿态表示方法为X-Y-Z固定角坐标系。2.3姿态的其他表示方法通过计算可以得到坐标系{2}和参考坐标系{1}的旋转矩阵,即式中,cα是cosα的简写,sα是sinα的简写。2.3姿态的其他表示方法思考:

由式(2-30),可以得到由旋转角到旋转矩阵的转换方法,那由旋转矩阵如何得到相应的旋转角?2.3姿态的其他表示方法令式中,为任意正交矩阵。由式(2-30)和式(2-31)中每个元素相等,可以得到9个方程。其中2.3姿态的其他表示方法由式(2-32),计算得到由式(2-33),计算得到式中,atan2(y,x)是双参变量的反正切函数。atan2(y,x)函数能通过判断y和x所在的象限,实现(-2π~2π)范围内对应的角度,避免了arctan(y/x)只能覆盖(-π~π)范围内角度的问题。2.3姿态的其他表示方法由由式(2-34)可知,β存在两个解,如只取β第二项的正解,则可以得到姿态矩阵到旋转角度的一一对应映射。但是如果β=±90°时,式(2-35)和式(2-36)中出现除以0的项,导致无解。此时求解α和γ便不能使用式(2-32)和式(2-33)。当β=90°时,式(2-30)变为此时,只能得到γ和α的差。2.3姿态的其他表示方法当β=-90°时,式(2-30)变为此时,只能得到γ和α的和。2.3姿态的其他表示方法2.3.2欧拉角坐标系{2}和参考坐标系{1}初始位置重合,坐标系{2}绕其自身的坐标轴旋转3次。这样三个一组的旋转被称为欧拉角。欧拉角可以有多种不同的组合,常用的欧拉角类型如表所示。采用欧拉角表示姿态的方法与采用固定角表示的计算方法相同,具体的计算方法不再赘述。2.3姿态的其他表示方法2.3.3等效轴角坐标系的表示方法坐标系{2}和参考坐标系{1}初始位置重合,坐标系{2}首先绕任意单位矢量r旋转θ,此时r被称为有限旋转的等效轴,坐标系{2}相对于参考坐标系{1}的姿态用R(r,θ)表示,该种姿态表示方法称为等效轴角坐标系表示法。2.3姿态的其他表示方法求解坐标系绕任意轴旋转后的姿态,需要经过如图所示的5个步骤:(1)绕X轴转α角,使r轴处于XZ平面内;(2)绕Y轴转-β角,使r轴与Z轴重合;(3)绕Z轴转动θ角;(4)绕Y轴转β角;(5)绕X轴转-α角。2.3姿态的其他表示方法经过上述5次旋转后的合成旋转矩阵为2.3姿态的其他表示方法再由图所示,可得到2.3姿态的其他表示方法将式(2-40)代入式(2-39),可得到2.3姿态的其他表示方法由式(2-41)可将绕任意轴的旋转变换成旋转矩阵的形式。其逆问题,即由旋转矩阵求解等效转轴和旋转角度的求解过程如下。定义单位四元数:由于,可以得到2.3姿态的其他表示方法将式(2-42)代入式(2-41),可得到令将式(2-44)和式(2-45)的对角线元素相加,可得到2.3姿态的其他表示方法结合式(2-44)、式(2-45)和式(