第三章 位姿描述和齐次变换

1、一、行列式和矩阵一、行列式和矩阵1. 行列式按照行（或列）展开法则行列式按照行（或列）展开法则：行列式等于它的任意一行：行列式等于它的任意一行（或列）各元素与其对应的代数余子式乘积之和。（或列）各元素与其对应的代数余子式乘积之和。3.列矩阵列矩阵4.矩阵相等：矩阵相等：两同型矩阵（行数和列数都相等）对应元素相等。两同型矩阵（行数和列数都相等）对应元素相等。 2.行矩阵行矩阵该数与矩阵各元素相乘。该数与矩阵各元素相乘。5.单位矩阵：单位矩阵：主对角线元素为主对角线元素为1，其它所，其它所有的元素都为有的元素都为0的方阵。的方阵。6.矩阵的运算矩阵的运算两同型矩阵的对应元素相加。两同型矩阵的对应元

2、素相加。 （3）矩阵与矩阵相乘：）矩阵与矩阵相乘：（4） 矩阵的转置：矩阵的转置：把矩阵的行换成同序数的列，记为把矩阵的行换成同序数的列，记为7. 矩阵的逆（逆矩阵）矩阵的逆（逆矩阵）8. 分块矩阵：分块矩阵：分块后的矩阵与普通矩阵的运算相同。分块后的矩阵与普通矩阵的运算相同。9. 正交矩阵：正交矩阵：如果如果 ，则，则A为正交矩阵。它满足：为正交矩阵。它满足：如果如果 是正交矩阵，则是正交矩阵，则矩阵是按一定方式排成的数表；行列式是矩阵是按一定方式排成的数表；行列式是一个数。一个数。 （b）左手坐标系左手坐标系 （a）右手坐标系右手坐标系二、直角坐标系二、直角坐标系 若基矢量相互正交，即它们

3、在原点若基矢量相互正交，即它们在原点o处两处两两相交成直角，则它们构成两相交成直角，则它们构成直角坐标系或笛卡直角坐标系或笛卡儿坐标系。儿坐标系。斜角坐标系斜角坐标系若按右手法则绕若按右手法则绕oz轴转轴转900可以使可以使ox轴转向轴转向oy轴，则称为右手坐标系轴，则称为右手坐标系；按左手法则形成的按左手法则形成的坐标系称左手坐标系坐标系称左手坐标系。 本课程使用右手坐标系本课程使用右手坐标系。三、矢量的点积（内乘积或标量积）三、矢量的点积（内乘积或标量积）换句话说：换句话说： 再令再令a=j （j 为为a方向上的单位矢量），则方向上的单位矢量），则 标量积标量积四、矢量的叉积（矢量积或叉乘

4、积）四、矢量的叉积（矢量积或叉乘积）其中矢量其中矢量c c的模为的模为: : 其中其中是是a和和b间小于等于间小于等于1800的夹角，若将的夹角，若将a按右手法则绕按右手法则绕c转转角至角至b，右手拇指指向为右手拇指指向为c的正方向（如上图所示的正方向（如上图所示），），c与与a、b两者垂直。两者垂直。则则 叉乘积叉乘积 若若a和和b用分量的形式表示为用分量的形式表示为:a和和b的点乘为：的点乘为：将点乘和叉乘应用于右手笛卡尔坐标系的单位矢量将点乘和叉乘应用于右手笛卡尔坐标系的单位矢量i，j，k，有：有：a) 位置的描述位置的描述 采用直角坐标描述点的位置，因此，刚体采用直角坐标描述点的位置，

5、因此，刚体F的位置描述，即的位置描述，即OB点在点在A中描述可用一个中描述可用一个31的列矢量的列矢量 （位置矢量）表示，即（位置矢量）表示，即其中其中Px、Py和和Pz是点是点OB在在A系中的三个坐标分量。系中的三个坐标分量。 采用旋转矩阵来表示刚体姿态（方位）采用旋转矩阵来表示刚体姿态（方位） ，即由，即由B系的三个系的三个单位主矢量相对于坐标系单位主矢量相对于坐标系A的方向余弦组成：的方向余弦组成： 既表示了刚体既表示了刚体F在在A系中的方位，也描述了系中的方位，也描述了B系在系在A系中系中的姿态。的姿态。其中：其中： ),cos(cosABxx xB yB zB xA yA zA 如图

6、所示，坐标系如图所示，坐标系B与与A方向相同，但原点不重合。方向相同，但原点不重合。 坐标平移坐标平移 此式称为此式称为平移方程平移方程。其中。其中 是是B系中的原点在系中的原点在A系中的表示。系中的表示。 坐标旋转坐标旋转 如图所示，如图所示，B与与A有共同的坐标原点，但方位不同。令有共同的坐标原点，但方位不同。令 和和 分别是分别是A和和B中的单位主矢量，点中的单位主矢量，点P 在两在两坐标系中各坐标轴上的坐标分量分别为：坐标系中各坐标轴上的坐标分量分别为：和和 利用点乘的性质和上式共同求解得利用点乘的性质和上式共同求解得将将 代入上面三式中并写成矩阵形式得代入上面三式中并写成矩阵形式得所

7、以有所以有上式简写为：上式简写为： 此式称为此式称为坐标旋转方程坐标旋转方程。其中旋转矩阵。其中旋转矩阵 表示了坐标系表示了坐标系B相相对于对于A的方位，正好与刚体姿态的描述相同。同理也可得的方位，正好与刚体姿态的描述相同。同理也可得 和和 都是正交矩阵，因此满足都是正交矩阵，因此满足 由由 与与 互逆，可得互逆，可得 旋转矩阵的几何意义：旋转矩阵的几何意义：旋转矩阵在几何上表示了发生相互旋旋转矩阵在几何上表示了发生相互旋转的两坐标系各主轴之间的相互方位关系。转的两坐标系各主轴之间的相互方位关系。 若把若把 写成行向量的形式写成行向量的形式 ，则其中，则其中每一个元素都是一个列向量。容易得出每

8、一个元素都是一个列向量。容易得出 满足六个约束条件满足六个约束条件（称正交条件）：（称正交条件）：因此写出三个基本的旋转矩阵，即分别绕因此写出三个基本的旋转矩阵，即分别绕x、y和和z轴转轴转角的旋转角的旋转矩阵：矩阵：x y zxyzx y zxyzx y zxyz例例3.1 若从基坐标系若从基坐标系 (B)到手爪坐标系到手爪坐标系 (E)的旋转变换的旋转变换矩阵为矩阵为 。（。（1）画出两坐标系的相互方位关系（不考虑）画出两坐标系的相互方位关系（不考虑E的的原点位置）；（原点位置）；（2）如果给出）如果给出OE(E系的原点）在系的原点）在B中的位置矢中的位置矢量为（量为（1，2，2），画出两

9、坐标系的相对位姿关系；（），画出两坐标系的相对位姿关系；（3）求）求a，b，c的值。的值。解：解：xE yE zExB yB zB(1)(2)(3) a=0，b=1，c=0三、一般变换三、一般变换一般的情况：坐标系一般的情况：坐标系B的原点既不与的原点既不与A重合，方位也不相同。重合，方位也不相同。C系与系原点重合，但系与系原点重合，但方位不同，所以得方位不同，所以得C系与系与A系原点不重合，系原点不重合，但方位相同，所以得但方位相同，所以得进而有进而有BOACOAPP和和例例3.2 已知坐标系已知坐标系B初始位姿与初始位姿与A重合，首先重合，首先B相对相对A的的zA轴轴转转30，再沿，再沿A

10、的的xA轴移动轴移动10个单位，并沿个单位，并沿A的的yA轴移动轴移动5个单个单位。求位置矢量位。求位置矢量 和旋转矩阵和旋转矩阵 。若。若 ,求求 。解：解：1000866. 05 . 005 . 0866. 0100030cos30sin030sin30cos)30,(00000zRRAB0510BOAP0562.12098. 9BOABABAPPRP所以有：所以有：最后得：最后得：简写成简写成 综合地表示了平移和旋转变换。综合地表示了平移和旋转变换。1101PPRPBBOAABABOABABAPPRP113.3.1 齐次坐标齐次坐标 一般来说，以一般来说，以N+1维矢量表达维矢量表达N维

11、位置矢量的方法称为维位置矢量的方法称为 在三维直角坐标系中，一个点可以表示在三维直角坐标系中，一个点可以表示 为为 ，它的齐它的齐次坐标就是次坐标就是 ，即满足，即满足Px=Px/，Py=Py/，Pz=Pz/（是非零整数）。可以看出，在三维直角坐标系中，是非零整数）。可以看出，在三维直角坐标系中，由于由于取值的不同，一个点的齐次坐标的表达不唯一。取值的不同，一个点的齐次坐标的表达不唯一。 齐次坐标不仅可以规定点的位置（齐次坐标不仅可以规定点的位置（为非零整数），还可以为非零整数），还可以用来规定矢量的方向（第四个元素为零时）。列向量用来规定矢量的方向（第四个元素为零时）。列向量 ( ）表示空间

12、的无穷远点，）表示空间的无穷远点，a，b和和c称为它的方向称为它的方向数。数。 分别代表了分别代表了ox，oy和和oz轴的无穷远轴的无穷远点，用它们分别表示这三个坐标轴的方向。另外，点，用它们分别表示这三个坐标轴的方向。另外， 代表代表坐标原点，坐标原点， 没有意义。没有意义。注意：注意：位置矢量位置矢量 究竟是究竟是31的直角坐标还是的直角坐标还是41的齐次坐标，应的齐次坐标，应根据上下文而定。根据上下文而定。在机器人研究中在机器人研究中，齐次变换矩阵，齐次变换矩阵T为：为：3.3.2 齐次变换齐次变换比例变换透视变换位置矢量旋转变换11311333fPRT 齐次变换矩阵是齐次变换矩阵是44

13、的矩阵，它的完整形式可以看成是由的矩阵，它的完整形式可以看成是由四个子矩阵组成：四个子矩阵组成： 中中P31为零矩阵，即为零矩阵，即 ，因此写出绕因此写出绕x，y和和z轴旋转轴旋转角的基本齐次变换矩阵为：角的基本齐次变换矩阵为： 中中R33=I33（单位阵），因此可以单位阵），因此可以写出沿写出沿x，y和和z轴移动轴移动Px，Py和和Pz单位的基本平移变换阵：单位的基本平移变换阵：从而定义复合变换从而定义复合变换 。 给定坐标系给定坐标系A，B和和C，已知已知B相对相对A的描述为的描述为 ，C相对相对B的描述为的描述为 ，则有，则有 同理得出：同理得出：即即PTPCBCBPTPBABAPTPC

14、ACAPTTPCBCABATTTBCABAC例例3.4 已知已知 ，画出，画出A和和B的相互位姿关系图。的相互位姿关系图。齐次变换齐次变换 不仅可以表示同一点相对不同坐标系不仅可以表示同一点相对不同坐标系B和和A中中的变换，也可用来描述坐标系的变换，也可用来描述坐标系B相对于另一坐标系相对于另一坐标系A的位姿，的位姿，同时还可用来作为点的运动算子。同时还可用来作为点的运动算子。例：书上例：书上P20例例2.4。1、绕固定坐标系依次进行的坐标、绕固定坐标系依次进行的坐标系转换，各齐次变换矩阵按系转换，各齐次变换矩阵按“从右从右向左向左”依次相乘原则进行运算（右依次相乘原则进行运算（右乘）。乘）。

15、一一.变换过程的相对性变换过程的相对性= RPY角角RPYRPY角角反解：反解： 2、绕动坐标系依次进行的齐次变换，按、绕动坐标系依次进行的齐次变换，按“从左向右从左向右”的原则依的原则依次相乘次相乘(左乘）。左乘）。=z-y-x欧拉角：欧拉角：相对于固定坐标系运动相对于固定坐标系运动相对于活动坐标系运动相对于活动坐标系运动齐次变换的相对性齐次变换的相对性齐次坐标变换过程是可逆的齐次坐标变换过程是可逆的. 若有若有 ，则逆变换，则逆变换 。二二.变换过程的可逆性变换过程的可逆性TTIABBA44所以有所以有1010BOAABAOBBAPRPR10AOBBOABAABBAPPRRR10033I3

16、3 IRRABBA0AOBBOABAPPR对应元素相等得对应元素相等得所以得所以得三三.变换过程的封闭性变换过程的封闭性因此有因此有由上面两式得变换方程：由上面两式得变换方程：BWTGS画出空间尺画出空间尺寸链图为寸链图为:1TTTTTWTGTSGBSBW例例 3.5 如图所示，从如图所示，从0系到系到3系依次经过系依次经过1系和系和2系的变系的变换换,用两种方法求用两种方法求 和和 ，第一种根据齐次变换矩阵的几何，第一种根据齐次变换矩阵的几何意义求解，另一种采用坐标系依次变换的方法；求意义求解，另一种采用坐标系依次变换的方法；求 （用两（用两种方法）；种方法）； 画出画出0到到3的空间尺寸链

17、图。的空间尺寸链图。 空间尺寸链图：空间尺寸链图：一一.旋转变换通式旋转变换通式 如果不是，要如果不是，要采用其对应的采用其对应的单位方向矢量单位方向矢量令令 是过是过A系原点的系原点的单位矢量单位矢量，求绕，求绕K旋旋转转角到角到B系的旋转矩阵系的旋转矩阵R(K,)，即即 。因此因此将上式展开得将上式展开得把上式右端相乘，并利用旋转矩阵的正交性质把上式右端相乘，并利用旋转矩阵的正交性质进行化简整理后得进行化简整理后得其中，其中，s=sin;c=cos;Vers=(1-cos)。 如果如果 与坐标轴重合，则可得到绕与坐标轴重合，则可得到绕x,y和和z轴旋转的基本旋转矩阵。轴旋转的基本旋转矩阵。

18、cVerskkskVerskkskVerskkskVerskkcVerskkskVerskkskVerskkskVerskkcVerskkkRzzxzyyzxxyzyyzyxyxzzxyxx),(例：例：求绕过原点的轴线求绕过原点的轴线 转动转动1200的旋转矩阵的旋转矩阵 对于给定的旋转矩阵对于给定的旋转矩阵R令令R=R(K,)，得得 任何一组经过有限次基本旋转变换后的复合旋转总可以等任何一组经过有限次基本旋转变换后的复合旋转总可以等效成绕某一过原点的轴线转效成绕某一过原点的轴线转角的单一旋转。角的单一旋转。将方程两边的主对角线元素分别相加，得将方程两边的主对角线元素分别相加，得于是可得：于是可得：再把方程两边的非对角元素成对相减得：再把方程两边的非对角元素成对相减得：将上式两边平方后再相加得：将上式两边平方后再相加得：于是：于是：两点注意两点注意:K和和的值不唯一。实际上，对于任意一组的值不唯一。实际上，对于任意一组K和和，都对应另一组都对应另一组-K和和-，(K,) 和和(k, +n360)对应的转