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

第三章位姿描述和齐次变换一、行列式和矩阵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用分量的形式表示为:a和b的点乘为：将点乘和叉乘应用于右手笛卡尔坐标系的单位矢量i，j，k，有：3.2位姿描述与坐标变换3.2.1刚体位置姿态（位姿）描述a)位置的描述采用直角坐标描述点的位置，因此，刚体F的位置描述，即OB点在{A}中描述可用一个3×1的列矢量（位置矢量）表示，即其中Px、Py和Pz是点OB在{A}系中的三个坐标分量。b)姿态（方位）的描述采用旋转矩阵来表示刚体姿态（方位），即由{B}系的三个单位主矢量相对于坐标系{A}的方向余弦组成：既表示了刚体F在{A}系中的方位，也描述了{B}系在{A}系中的姿态。其中：),cos(cosABxxxByBzBxAyAzA3.2.2坐标变换如图所示，坐标系{B}与{A}方向相同，但原点不重合。坐标平移一、坐标平移此式称为平移方程。其中是B系中的原点在A系中的表示。二、坐标旋转坐标旋转如图所示，{B}与{A}有共同的坐标原点，但方位不同。令和分别是{A}和{B}中的单位主矢量，点P在两坐标系中各坐标轴上的坐标分量分别为：和利用点乘的性质和上式共同求解得将代入上面三式中并写成矩阵形式得所以有上式简写为：此式称为坐标旋转方程。其中旋转矩阵表示了坐标系{B}相对于{A}的方位，正好与刚体姿态的描述相同。同理也可得和都是正交矩阵，因此满足由与互逆，可得旋转矩阵的几何意义：旋转矩阵在几何上表示了发生相互旋转的两坐标系各主轴之间的相互方位关系。若把写成行向量的形式，则其中每一个元素都是一个列向量。容易得出满足六个约束条件（称正交条件）：因此写出三个基本的旋转矩阵，即分别绕x、y和z轴转θ角的旋转矩阵：xyzx’y'z'θθxyzx’y'z'θθxyzx’y'z'θθx’y’z’xyzx’y’z’xyzx’y’z’xyz例3.1若从基坐标系({B})到手爪坐标系({E})的旋转变换矩阵为。（1）画出两坐标系的相互方位关系（不考虑{E}的原点位置）；（2）如果给出OE({E}系的原点）在{B}中的位置矢量为（1，2，2），画出两坐标系的相对位姿关系；（3）求a，b，c的值。解：xEyEzExByBzB(1)xBzByBxEzEyExBzByBxEzEyE(1,2,2)(2)(3)a=0，b=1，c=0三、一般变换一般的情况：坐标系{B}的原点既不与{A}重合，方位也不相同。{C}系与｛Ｂ｝系原点重合，但方位不同，所以得{C}系与｛A｝系原点不重合，但方位相同，所以得进而有BOACOAPP和例3.2已知坐标系{B}初始位姿与{A}重合，首先{B}相对{A}的zA轴转30°，再沿{A}的xA轴移动10个单位，并沿{A}的yA轴移动5个单位。求位置矢量和旋转矩阵。若,求。xAxByAzAOAOByBzB30o30oxAxByAzAOAOByBzB30o30o(10,5,0)解：1000866.05.005.0866.0100030cos30sin030sin30cos)30,(00000zRRAB0510BOAP0562.12098.9BOABABAPPRP所以有：最后得：3.3齐次坐标与齐次变换复合变换式可以表示成等价的齐次变换式。简写成综合地表示了平移和旋转变换。1101PPRPBBOAABABOABABAPPRP113.3.1齐次坐标一般来说，以N+1维矢量表达N维位置矢量的方法称为齐次坐标表示法。在三维直角坐标系中，一个点可以表示为，它的齐次坐标就是，即满足Px=ωPx/ω，Py=ωPy/ω，Pz=ωPz/ω（ω是非零整数）。可以看出，在三维直角坐标系中，由于ω取值的不同，一个点的齐次坐标的表达不唯一。齐次坐标不仅可以规定点的位置（ω为非零整数），还可以用来规定矢量的方向（第四个元素为零时）。列向量(）表示空间的无穷远点，a，b和c称为它的方向数。分别代表了ox，oy和oz轴的无穷远点，用它们分别表示这三个坐标轴的方向。另外，代表坐标原点，没有意义。注意：位置矢量究竟是3×1的直角坐标还是4×1的齐次坐标，应根据上下文而定。在机器人研究中，齐次变换矩阵T为：3.3.2齐次变换比例变换透视变换位置矢量旋转变换11311333fPRT齐次变换矩阵是4×4的矩阵，它的完整形式可以看成是由四个子矩阵组成：纯旋转的齐次变换矩阵中P3×1为零矩阵，即，因此写出绕x，y和z轴旋转θ角的基本齐次变换矩阵为：纯平移的齐次变换矩阵中R3×3=I3×3（单位阵），因此可以写出沿x，y和z轴移动Px，Py和Pz单位的基本平移变换阵：从而定义复合变换。给定坐标系{A}，{B}和{C}，已知{B}相对{A}的描述为，{C}相对{B}的描述为，则有同理得出：即一个坐标系变换至另一坐标系的齐次变换矩阵等于依次经历中间坐标系各齐次变换矩阵的连乘积。PTPCBCBPTPBABAPTPCACAPTTPCBCABATTTBCABAC例3.4已知，画出{A}和{B}的相互位姿关系图。结论：齐次变换不仅可以表示同一点相对不同坐标系{B}和{A}中的变换，也可用来描述坐标系{B}相对于另一坐标系{A}的位姿，同时还可用来作为点的运动算子。例：书上P20例2.4。3.4齐次变换的性质1、绕固定坐标系依次进行的坐标系转换，各齐次变换矩阵按“从右向左”依次相乘原则进行运算（右乘）。一.变换过程的相对性=RPY角RPYRPY角反解：2、绕动坐标系依次进行的齐次变换，按“从左向右”的原则依次相乘(左乘）。=z-y-x欧拉角：相对于固定坐标系运动相对于活动坐标系运动齐次变换的相对性齐次坐标变换过程是可逆的.若有，则逆变换。二.变换过程的可逆性TTIABBA44所以有1010BOAABAOBBAPRPR10AOBBOABAABBAPPRRR10033I33IRRABBA0AOBBOABAPPR对应元素相等得所以得三.变换过程的封闭性因此有由上面两式得变换方程：{B}{W}{T}{G}{S}画出空间尺寸链图为:1TTTTTWTGTSGBSBW例3.5如图所示，从{0}系到{3}系依次经过{1}系和{2}系的变换,①用两种方法求和，第一种根据齐次变换矩阵的几何意义求解，另一种采用坐标系依次变换的方法；②求（用两种方法）；③画出{0}到{3}的空间尺寸链图。空间尺寸链图：{0}{1}{2}{3}3.5旋转变换通式一.旋转变换通式如果不是，要采用其对应的单位方向矢量令是过{A}系原点的单位矢量，求绕K旋转θ角到{B}系的旋转矩阵R(K,θ)，即。因此将上式展开得尺寸链图把上式右端相乘，并利用旋转矩阵的正交性质进行化简整理后得其中，sθ=sinθ;cθ=cosθ;Versθ=(1-cosθ)。如果与坐标轴重合，则可得到绕x,y和z轴旋转的基本旋转矩阵。cVerskkskVerskkskVerskkskVerskkcVerskkskVerskkskVerskkskVerskkcVerskkkRzzxzyyzxxyzyyzyxyxzzxyxx),(例：求绕过原点的轴线转动1200的旋转矩阵二.等效转轴与等效转角对于给定的旋转矩阵R令R=R(K,θ)，得任何一组经过有限次基本旋转变换后的复合旋转总可以等效成绕某一过原点的轴线转θ角的单一旋转。将方程两边的主对角线元素分别相加，得于是可得：再把方程两边的非对角元素成对相减得：将上式两边平方后再相加得：于是：两点注意:①多值性：K和θ的值不唯一。实际上，对于任意一组K和θ，都对应另一组-K和-θ，(K,θ)和(k,θ+n×360)对应的转动效果相同，θ的取值也有多种，一般取在0°到180°之间。例：求复合变换的等效转轴k和转角θ。)1()()()()()()(21sin222222zyxxyzxyzxyzxyzaononnaaotgonnaaosin2sin2sin2xyzzxyyzxonknakaok②病态情况：当转角θ很小时，转轴难确定；当θ接近0°或180°时，转轴完全不能确定，需另寻解法。Classisover.Bye-Bye!