1、机器人学、机器视觉与控制（2）三维空间位姿描述内容:三维空间姿态描述正交旋转矩阵三角度数表示法奇异点及万向节锁双向量表示法 绕任意向量旋转单位四元数平移与旋转组合三维空间位姿描述三维情况实际上是前一节讨论的二维情况的延伸。我们在二维坐标系上增加一个额外的坐标轴，通常用z表示，它同时与x轴和y轴正交。Z轴的方向服从右手规则，并构成右手坐标系。欧拉旋转定理：任何两个独立的正交坐标系都可以通过一系列（不超过3次）相对于坐标轴的旋转联系起来，但其中两次旋转不能绕同一条轴线。需要注意的是，在绕坐标轴连续旋转的情况下要注意旋转的顺序，旋转角度相同而顺序不同可能得到完全不一样的结果。下图是两种不同顺序的旋转
2、，得到的结果完全不同。注意：1.我们可以用右手的三根指头简单便捷地表示空间坐标系，具体方向如右图。2.这些旋转都要遵循右手准则，即按照右图所示旋转才是正方向。在上节课中，我们介绍了姿势和姿势的概念，即一种描述一个坐标系与另一个坐标系的平移和定向的方法。我们使用的符号是，我们使用下标字母B表明我们谈论坐标系的姿态B相对于坐标系A.所以我们读这个符号为B对A，由此扩展到三维空间如下：三维空间向量变换的表示方法三维空间的姿态变换的相关性质如下，这些性质跟二维空间的情形差不多对于多重的相对坐标转换同样可以按照相应的规则表示，下图展示的是一个等效的变换，即红线和蓝线都表示从O到B的变换。正如在二维情况下
3、一样，我们可以用相对于参考坐标系的坐标轴单位向量表示它们所在坐标系的方向。每一个单位向量有3个元素，它们组成了33阶正交矩阵：正交旋转矩阵上式将一个相对于坐标系B的向量旋转为相对于坐标系A的向量。分别绕x,y,z轴旋转角后的标准正交矩阵可表示为：我们对这个公式的来源以及性质做一些说明：我们从上一讲得到的2D坐标变换出发，将2维空间扩展到三维空间之中，相应可以得到一个新的33坐标变换矩阵，它的三个列向量分别表示新的x,y,z坐标变换。要得到最终的三维坐标变换公式，关键是求出B相对于A的变换矩阵。从二维空间坐标变换我们已经知道：变换矩阵是一个属于SO（2）的特殊正交群。现在对于三维空间而言它就是属
4、于SO（3）的特殊正交群，旋转矩阵的相关性质如下：根据上面的性质结合二维坐标中的情况，我们可以得到三维坐标旋转变换矩阵的表达式如下：可以看出：这些旋转矩阵每一列都是单位长度的向量，每一列都与其他一列正交。对于绕z轴的旋转，情况特殊，因为我们可以直接将其看成二维空间绕着垂直于纸面向外（也就是垂直屏幕向外）的旋转得到，所以二维旋转和绕Z轴的三维旋转是等价的，即上面坐标变换中关于z轴的旋转与二维空间旋转变换R是等价的。MATLAB工具箱也提供了一些函数用来计算这些基本旋转矩阵。比如绕X轴旋转90度可以调用rotx()函数，调用后直接得到旋转矩阵：R=rotx(pi/2)R=1.0000 0 0 0
5、0.0000 -1.0000 0 1.0000 0.0000说明：在正交旋转矩阵中，从左到右的列向量代表了旋转后的各个轴相对于当前坐标系的方向。上面的例子中，新的坐标系的x轴仍在以前x轴方向（1,0,0），而新的y轴在以前z轴方向，即（0,0,1），而新的z轴在以前y轴反方向（0，-1,0）。X轴没有变的原因在于该旋转是绕x轴发生的。行向量相反，它们代表了当前坐标系的各个轴在新坐标系中的方向。为了说明旋转的复合，我们接着上面的结果再继续绕着y轴旋转90度：R=rotx(pi/2)*roty(pi/2)R=0.0000 0 1.0000 1.0000 0.0000 -0.0000 -0.0000
6、 1.0000 0.0000用trplot（R）命令作出两次旋转的图形，左边是绕着x轴旋转90度的图形，右边是在此基础上继续绕着y轴旋转90度的图形注意：旋转的次序很重要，即使同样的旋转，如果次序不对得到的结果就完全不一样。我们之前讲欧拉旋转定理时通过三维旋转直观地看到了这种差别。而现在我们通过矩阵变换重新演示上面的例子即如果改成先绕y轴旋转再绕x轴旋转将会得到如下结果：R=roty(pi/2)*rotx(pi/2)R=0.0000 1.0000 0.0000 0 0.0000 -1.0000 -1.0000 0.0000 0.0000对比先绕x再绕y的旋转顺序所得到的结果不一样，从矩阵变换的
7、角度也说明了空间中旋转顺序的不可交换性。说明：正交矩阵有9个元素，但它们不是独立的。每一列都是单位长度，这提供了3个约束。列向量之间相互正交，又提供了另外3个约束。9个元素加上6个约束，实际上只有3个独立的值。因此，回忆一下欧拉旋转定理，它指出任何旋转都可以用不超过3次绕坐标轴的旋转表示。这意味着我们可以将任意地旋转分解为一组绕三个坐标轴转动的角度，我们将在下面讨论。三角度数表示法：旋转定理要求绕3个轴依次旋转，但不能绕同一轴线连续旋转两次。旋转顺序分为两种：欧拉式和卡尔丹式。欧拉式：绕一个特定的轴重复旋转，但不是连续的：XYX XZY YXY YZY ZXZ或ZYZ。卡尔丹式的特点是绕3个不
8、同的轴旋转：XYZ XZY YZX YXZ ZXY或ZYX。这些序列统称为欧拉角，共有12种形式。ZYZ序列的欧拉角表示为：欧拉角是一个三维向量，表示为：如果想计算T=(0.1,0.2,0.3)的等价旋转矩阵，可以在MATLAB中用函数这样实现：R=rotz(0.1)*roty(0.2)*rotz(0.3)R=0.9021 -0.3836 0.1977 0.3875 0.9216 0.0198 -0.1898 0.0587 0.9801或者用eul2r函数写成更简单的形式：R=eul2r(0.1,0.2,0.3)R=0.9021 -0.3836 0.1977 0.3875 0.9216 0.0
9、198 -0.1898 0.0587 0.9801如果求上面的问题的逆命题，即求出给定旋转矩阵的欧拉角，可以用tr2eul函数：gamma=tr2eul(R)gamma=0.1000 0.2000 0.3000如果为负值，比如下面的情况：R=eul2r(0.1,-0.2,0.3)R=0.9021 -0.3836 -0.1977 0.3875 0.9216 -0.0198 0.1898 -0.0587 0.9801反函数为：tr2eul(R)ans=-3.0416 0.2000 -2.8416我们发现其结果跟之前的旋转角度完全不一样，但这组欧拉角对应的旋转矩阵仍与前一组是相同的。其实两组不同的欧
10、拉角对应同一个旋转矩阵，说明从旋转矩阵到欧拉角的映射不唯一，而工具箱的函数返回值始终为正。对于=0的情况 R=eul2r(0.1,0,0.3)R=0.9211 -0.3894 0 0.3894 0.9211 0 0 0 1.0000反函数返回的角度值为：tr2eul(R)ans=0 0 0.4000这个结果跟原来的值完全不同。实际上，从我们得到这种情况下的旋转矩阵：最终化简的结果只是一个+的函数，对于逆运算而言只能确定这个和的值。要得到其中的每个值，只能按惯例取=0。另一种广泛使用的旋转角顺序是横滚俯仰偏航角，即：这个表示方法用于描述船舶、飞机和车辆姿态时非常直观。这个表示方法指分别绕x,y,
11、z轴的旋转。这个xyz角序列也是专业上的卡尔丹角，也称泰特布莱恩角或导航角。对于航空和地面车辆而言，通常定义x轴为前进的方向、z轴垂直向下、y轴指向右手方向。奇异点及万向节锁之前讨论的三旋转角度表示方法中，一个根本的问题在于奇异点。当中间的旋转轴平行于第一个或第三个旋转轴时这种情况就会发生，对于万向节锁同样存在这个问题。图中是用于导航的陀螺仪，其核心是三个互相正交的框架，它们能使安装在其中的稳定体相对宇宙静止。因为无论飞船怎样飞行，陀螺仪内部的稳定平台都不会受到额外的力矩。而通过测量这些万向框架的轴相对于稳定平台的转动角度就可以确定飞船的航行姿态。如果陀螺仪中间的内万向框架与外万向框架的轴对齐
12、，即它们的旋转轴平行，此时陀螺仪只有两个有效的旋转轴，而不是原来的三个，我们称之为丢失了一个自由度。双向量表示法对于关节臂式机器人，按照图式的方法，我们定义一个坐标系。机器人两指之间为姿态向量，往外的a向量为接近向量，第三个向量可由a向量与o向量正交得到。使用工具箱函数，我们可以通过a向量和o向量计算出n向量。例如执行以下运算：a=1 0 0;o=0 1 0;oa2r(o,a)ans=0 0 1 0 1 0 -1 0 0绕任意向量旋转对于空间中两个任意姿态的坐标系，总可以在空间中找到某个轴，使其中一个坐标系绕该轴旋转一个角度就能与另一个坐标系姿态重合。为了找到绕着旋转的轴，那就是我们要旋转的轴
13、必须与旋转保持不变。任何在旋转轴上的点都是旋转的。这意味着旋转轴必须是矩阵r的特征向量，旋转矩阵有三个特征向量。而正交矩阵总有一个实特征值1，此时Rv=v化简为Rv=v，这意味着此时特征向量v不随旋转发生改变，而旋转是以这个向量为轴发生的。反过来，使用罗德里格斯旋转方程，我们可以从角度和向量计算出相应的旋转矩阵：运用工具箱函数angvec2r可以计算相关的旋转矩阵：R=angvec2r(pi/2,1 0 0)R=1.0000 0 0 0 0.0000 -1.0000 0 1.0000 0.0000单位四元数四元数是复数的一种扩展，也称为超复数，记做一个标量加上一个向量。四元数乘法不可交换，这种
14、不可交换性正好符合坐标系旋转的情况。我们可以使用工具箱函数Quaternion的类来实现四元数。构造函数将传递的参数转换成四元数：q=Quaternion(rpy2tr(0.1,0.2,0.3)q=0.98186 我们用单位四元数来描述坐标系的旋转，它可以看成是绕单位向量n旋转了角。该旋转与四元数的关系为：单位四元数的性质如下：我们可以使用工具箱函数来表示相关性质：单位四元数：q.normans=1.0000求一个四元数的共轭：q.inv()ans=0.98186 求一个四元数乘以它的共轭：q*q.inv()ans=1 结果是一个单位四元数，表示一个无效旋转关于四元数的相关性质可在MATLAB
15、中通过help Quaternion命令查询相关文档平移与旋转组合三维空间相对位姿表示，即两个坐标系之间位置和姿态的变化，我们曾经讨论了几种不同的姿态表示法，现在我们需要将它们与平移变换相结合，创造出完整的相对位姿的表示方法。这其中包含2个分量，一个平动分量和一个转动分量。我们可以把平移分量表示为矢量，然后用一组3个欧拉角表示转动分量。或者，我们可以用一个矢量加上一组3个滚转、俯仰、偏航角来表示它。或者我们可以把它表示为一个向量加上四元数。第四个选项是将它表示为一个齐次变换。这其中有两种最实用的表示方法：四元数向量对和44齐次变换矩阵。由于四元数的相关性质上面已经讨论过，我们接下来着重讨论44
16、齐次变换矩阵齐次变换矩阵表示旋转和转换之前已经讨论过二维空间的变换，三维只需要在此基础上增加Z轴并对矩阵进行扩展：化成向量形式：具体的变换如右图所示：齐次变换矩阵的相关性质如下：44齐次变换在机器人学和计算机视觉中都非常有用。我们可以在MATLAB中通过以下函数实现变换的合成：T=transl(1,0,0)*trotx(pi/2)*transl(0,1,0)T=1.0000 0 0 1.0000 0 0.0000 -1.0000 0.0000 0 1.0000 0.0000 1.0000 0 0 0 1.0000函数transl创建了一个有平移但无旋转的相对姿态，而函数trotx则返回一个绕x轴旋转/2的44齐次变换矩阵：旋转部分与rotx（pi/2）相同，平移部分为零。我们可以将以上函数组合对应的坐标系变换描述如下：首先沿着x轴方向前进一个单位长度，然后绕x轴旋转90，接着再沿新的y轴，也就是原来的x轴前进一个单位长度。所得矩阵的最后一列表示了沿原来坐标系的x轴和z轴各平移一个单位长度的结果。要提取矩阵T的旋转矩阵部分，可用t2r函数实现：t2r(T)ans=1.0000 0 00 0.0000 -1.00000 1.0000 0.0000平移部分是一个向量，可用transl函数提取：transl(T)ans=1.0000 0.0000 1.0000
