第三章 位姿描述和齐次变换,一、行列式和矩阵,1. 行列式按照行（或列）展开法则：行列式等于它的任意一行（或列）各元素与其对应的代数余子式乘积之和。,3.1 相关知识回顾,3.列矩阵,4.矩阵相等：两同型矩阵（行数和列数都相等）对应元素相等。,2.行矩阵,（2）矩阵与数相乘：该数与矩阵各元素相乘。,5.单位矩阵：主对角线元素为1，其它所有的元素都为0的方阵。,6.矩阵的运算 （1）矩阵的加法：两同型矩阵的对应元素相加。,（3）矩阵与矩阵相乘：,（4） 矩阵的转置：把矩阵的行换成同序数的列，记为,7. 矩阵的逆（逆矩阵）,8. 分块矩阵：分块后的矩阵与普通矩阵的运算相同。,9. 正交矩阵：如果 ，则A为正交矩阵。它满足：,如果 是正交矩阵，则,行列式和矩阵的区别：矩阵是按一定方式排成的数表；行列式是一个数。,（b）左手坐标系,（a）右手坐标系,二、直角坐标系,若基矢量相互正交，即它们在原点o处两两相交成直角，则它们构成直角坐标系或笛卡儿坐标系。,斜角坐标系,若按右手法则绕oz轴转900可以使ox轴转向oy轴，则称为右手坐标系；按左手法则形成的坐标系称左手坐标系。,本课程使用右手坐标系。,其中是a和b两矢量间的夹角，如图所示。,三、矢量的点积（内乘积或标量积）,换句话说：一个矢量在另一个矢量上的投影等于该矢量与另一矢量方向上单位矢量的点积。,再令a=j （j 为a方向上的单位矢量），则,即两矢量方向上单位矢量的点乘等于两矢量夹角的余弦。,标量积,令b=i （i为b方向上的单位矢量），则,四、矢量的叉积（矢量积或叉乘积）,其中矢量c的模为:,其中是a和b间小于等于1800的夹角，若将a按右手法则绕c转角至b，右手拇指指向为c的正方向（如上图所示），c与a、b两者垂直。,则,叉乘积,a和b的点乘为：,将点乘和叉乘应用于右手笛卡尔坐标系的单位矢量i，j，k，有：,3.2 位姿描述与坐标变换,3.2.1刚体位置姿态（位姿）描述,a) 位置的描述,采用直角坐标描述点的位置，因此，刚体F的位置描述，即OB点在A中描述可用一个31的列矢量 （位置矢量）表示，即,其中Px、Py和Pz是点OB在A系中的三个坐标分量。,b) 姿态（方位）的描述,采用旋转矩阵来表示刚体姿态（方位） ，即由B系的三个单位主矢量相对于坐标系A的方向余弦组成：,既表示了刚体F在A系中的方位，也描述了B系在A系中的姿态。,其中：,xB yB zB,xA yA zA,3.2.2 坐标变换,如图所示，坐标系B与A方向相同，但原点不重合。,坐标平移,一、坐标平移,此式称为平移方程。其中 是B系中的原点在A系中的表示。,二、坐标旋转,坐标旋转,如图所示，B与A有共同的坐标原点，但方位不同。令 和 分别是A和B中的单位主矢量，点P 在两坐标系中各坐标轴上的坐标分量分别为：,和,利用点乘的性质和上式共同求解得,将 代入上面三式中并写成矩阵形式得,所以有,上式简写为：,此式称为坐标旋转方程。其中旋转矩阵 表示了坐标系B相对于A的方位，正好与刚体姿态的描述相同。同理也可得,和 都是正交矩阵，因此满足,由 与 互逆，可得,旋转矩阵的几何意义：旋转矩阵在几何上表示了发生相互旋转的两坐标系各主轴之间的相互方位关系。,若把 写成行向量的形式 ，则其中每一个元素都是一个列向量。容易得出 满足六个约束条件（称正交条件）：,因此写出三个基本的旋转矩阵，即分别绕x、y和z轴转角的旋转矩阵：,x y z,x y z,x y z,x y z,x y z,x y z,例3.1 若从基坐标系 (B)到手爪坐标系 (E)的旋转变换矩阵为 。（1）画出两坐标系的相互方位关系（不考虑E的原点位置）；（2）如果给出OE(E系的原点）在B中的位置矢量为（1，2，2），画出两坐标系的相对位姿关系；（3）求a，b，c的值。,解：,xE yE zE,xB yB zB,(1),(2),(3) a=0，b=1，c=0,三、一般变换,一般的情况：坐标系B的原点既不与A重合，方位也不相同。,C系与系原点重合，但方位不同，所以得,C系与A系原点不重合，但方位相同，所以得,进而有,和,例3.2 已知坐标系B初始位姿与A重合，首先B相对A的zA轴转30，再沿A的xA轴移动10个单位，并沿A的yA轴移动5个单位。求位置矢量 和旋转矩阵 。若 ,求 。,解：,所以有：,最后得：,3.3 齐次坐标与齐次变换,复合变换式 可以表示成等价的齐次变换式。,简写成,综合地表示了平移和旋转变换。,3.3.1 齐次坐标,一般来说，以N+1维矢量表达N维位置矢量的方法称为齐次坐标表示法。,在三维直角坐标系中，一个点可以表示 为 ，它的齐次坐标就是 ，即满足Px=Px/，Py=Py/，Pz=Pz/（是非零整数）。可以看出，在三维直角坐标系中，由于取值的不同，一个点的齐次坐标的表达不唯一。,齐次坐标不仅可以规定点的位置（为非零整数），还可以用来规定矢量的方向（第四个元素为零时）。列向量 ( ）表示空间的无穷远点，a，b和c称为它的方向数。,分别代表了ox，oy和oz轴的无穷远点，用它们分别表示这三个坐标轴的方向。另外， 代表坐标原点， 没有意义。,注意：位置矢量 究竟是31的直角坐标还是41的齐次坐标，应根据上下文而定。,在机器人研究中，齐次变换矩阵T为：,3.3.2 齐次变换,齐次变换矩阵是44的矩阵，它的完整形式可以看成是由四个子矩阵组成：,纯旋转的齐次变换矩阵中P31为零矩阵，即 ，因此写出绕x，y和z轴旋转角的基本齐次变换矩阵为：,纯平移的齐次变换矩阵中R33=I33（单位阵），因此可以写出沿x，y和z轴移动Px，Py和Pz单位的基本平移变换阵：,从而定义复合变换 。,给定坐标系A，B和C，已知B相对A的描述为 ，C相对B的描述为 ，则有,同理得出：,即一个坐标系变换至另一坐标系的齐次变换矩阵等于依次经历中间坐标系各齐次变换矩阵的连乘积。,例3.4 已知 ，画出A和B的相互位姿关系图。,结论：齐次变换 不仅可以表示同一点相对不同坐标系B和A中的变换，也可用来描述坐标系B相对于另一坐标系A的位姿，同时还可用来作为点的运动算子。,例：书上P20例2.4。,3.4 齐次变换的性质,1、绕固定坐标系依次进行的坐标系转换，各齐次变换矩阵按“从右向左”依次相乘原则进行运算（右乘）。,一.变换过程的相对性,=,RPY角反解：,2、绕动坐标系依次进行的齐次变换，按“从左向右”的原则依次相乘(左乘）。,=,z-y-x欧拉角：,相对于固定坐标系运动,相对于活动坐标系运动,齐次变换的相对性,齐次坐标变换过程是可逆的. 若有 ，则逆变换 。,二.变换过程的可逆性,所以有,对应元素相等得,所以得,三.变换过程的封闭性,因此有,由上面两式得变换方程：,画出空间尺寸链图为:,例 3.5 如图所示，从0系到3系依次经过1系和2系的变换,用两种方法求 和 ，第一种根据齐次变换矩阵的几何意义求解，另一种采用坐标系依次变换的方法；求 （用两种方法）； 画出0到3的空间尺寸链图。,空间尺寸链图：,3.5 旋转变换通式,一.旋转变换通式,令 是过A系原点的单位矢量，求绕K旋 转角到B系的旋转矩阵R(K,)，即 。,因此,将上式展开得,尺寸链图,把上式右端相乘，并利用旋转矩阵的正交性质,进行化简整理后得,其中，s=sin;c=cos;Vers=(1-cos)。,如果 与坐标轴重合，则可得到绕x,y和z轴旋转的基本旋转矩阵。,二. 等效转轴与等效转角,对于给定的旋转矩阵R,令R=R(K,)，得,任何一组经过有限次基本旋转变换后的复合旋转总可以等效成绕某一过原点的轴线转角的单一旋转。,将方程两边的主对角线元素分别相加，得,于是可得：,再把方程两边的非对角元素成对相减得：,将上式两边平方后再相加得：,于是：,两点注意:多值性：K和的值不唯一。实际上，对于任意一组K和，都对应另一组-K和-，(K,) 和(k, +n360)对应的转动效果相同，的取值也有多种，一般取在0到180之间。,例：求复合变换 的等效转轴k和转角。,病态情况：当转角很小时，转轴难确定；当接近0或 180时，转轴完全不能确定，需另寻解法。,Class is over.Bye-Bye!,