机器人学导论（一）——空间描述和变换

本着看书一定要写出来才能理解的心态，我开设了这个专栏，目的有两个：一是督促自己看书学习；二是与大家一起分享自己的读书感悟，在大家的批评中成长。

这篇博文我们主要介绍空间描述和变换，相信大家对于其重要性都表示认同。举个例子，比如我们在做基于视觉的抓取时，不管是眼在手上（eye-in-hand），还是眼在手外（eye-to-hand），总是需要标定相机和执行器末端的关系的，那如何对这二者之间的关系进行表述呢？就是使用这里将要介绍的空间描述和变换。说白了，就是把我们学过的线性代数的知识拿过来用一用，然后对其在空间中的含义进行理解，而不是单纯的了解矩阵的数学运算。

我们知道，在我们生活的三维空间中，对于刚体而言，存在着六个自由度，其中三个用来描述质心位置，而另外三个自由度用来描述整个刚体绕质心的旋转。所以，我们下面先介绍位置描述，然后介绍姿态描述。

位置描述

关于位置描述其实大家是比较熟悉的，比如说下面的图例：

在这里，我们在坐标系{A}下对矢量进行表示：

用上面的有序三元组对我们在空间中的一个质点的位置进行表示，这三个量是完全独立的，因此有3个自由度。

姿态描述

上面我们谈到位置描述的时候，关心的对象是质点，因此它本身不需要对其姿态进行描述，故而没有引入旋转，但是当我们的研究对象是刚体的时候，则需要在刚体上固定一个坐标系，用来表示刚体的旋转了，这也就是我们引入姿态描述的原因。

如上图所示，我们关心机械臂末端夹持器末端中心的位姿（包括位置和姿态）在坐标系{A}下的表示。所以我们以该点作为原点，并且选择三个正交单位向量作为轴，建立一个坐标系{B}，这个坐标系本身就是我们所寻求的表示：

这个矩阵就叫做旋转矩阵，表示{B}相对于{A}的旋转。其中的三个分量分别为{B}的三个主轴向量在{A}中的表示，因此我们可以将上面的式子写为：

这样，我们就能够清晰地理解这个矩阵的由来了，并且由于旋转矩阵是利用{B}的三个主轴单位向量在{A}中的投影得到的，所以它有一些特殊性质，首先它是一个对称矩阵：

这个式子的话可以很容易从上一个式子汇总看出来，下面我们来看它的另一个性质，即“旋转矩阵的逆等于它的转置”：

拿单位阵中的第一行第一列的1来说，它其实就是坐标系{B}的X轴在坐标系{A}中各轴上的投影构成的行向量乘以它构成的列向量，也就等于{B}的X轴本身的长度1，同理，我们可以得到旋转矩阵的转置乘以旋转矩阵等于单位阵（可以从方向余弦推导），所以说“旋转矩阵的逆等于它的转置”。

可是旋转矩阵明明有9个值，为什么只占了3个自由度呢？因为这些量之间是有约束方程的：每一列的平方和为1，不同列相互正交，这与旋转矩阵的转置乘以旋转矩阵本身等于单位矩阵是一样的。这么一算，就有6个约束方程了，所以只有3个自由度，这一点我们后面将会详细分析。

坐标系的描述

在机器人学中，位置和姿态经常成对出现，于是我们将此组合称为坐标系，比如坐标系{B}在坐标系{A}中的表示如下：

值得注意的是，在John J.Craig的《Introduction to Robotics: Mechanics and Control》书中有这样一句表述：“一个参考系可以用一个坐标系相对于另一个坐标系的关系来描述”。也就是说，在这里，Craig将参考系虚化为了一种关系，这与我们平时说的参考系的理解其实不太一样。“参考系包括位置和姿态两个概念，大多数情况下被认为是这两个概念的结合。位置可由一个参考系表示，这个参考系中的旋转矩阵是单位阵，并且这个参考系中的位置矢量确定了被描述点的位置。同样，如果参考系中的位置矢量是零矢量，那么它表示的就是姿态。”从这段话来看，其实将参考系理解为一个变换就好了，比如说如果{B}是由{A}平移得到的，那么{B}的原点可以由旋转矩阵为单位阵，平移向量为{A}到{B}的矢量的这样一个组合来表示。后面我们介绍完映射之后，大家将进一步理解这本书里面的参考系的含义。

映射

这一节主要介绍平移坐标系的映射以及旋转坐标系的映射，其中平移坐标系映射的概念相比大家都比较熟悉，所以我们就不仔细讨论了：

P相对于{A}的位置可以表示为：

值得注意的是，我们已知P在{B}中的位置表示以及{B}相对于{A}的平移，想要计算的是P在{A}中的位置表示，能够列出上式的前提是，{B}相对于{A}不存在旋转。如果存在旋转的话，我们需要了解旋转坐标系的映射：

我们知道{A}的主轴X在{B}中的表示为：

这个式子的含义是{A}的X轴上的“1”在{B}中各个轴上的投影，或者反过来说，{B}的各个轴上的“1”往{A}的X轴上的投影。我们将其转置为行向量，然后左乘到上，中的各个分量为在{B}下的表示，所以就表示P点在{A}下X轴上的投影，也即，同理我们可以推出P点在{A}中Y轴、Z轴上的分量，从而得到：

显然，我们可以将其简化为：

这个映射将空间中的某个点P相对于{B}的描述转换成了该点相对于{A}的描述。

关于一般坐标系的描述

在这里，我们将讨论下图所示的一般情况：

按照我们在平移映射中所说，如果要运用向量加法求得P在{A}中的表示，则前提是{B}相对于{A}没有旋转，而在图2-7中，这一条件不再成立，所以我们首先需要绕{B}的原点旋转，使之姿态与{A}一样，记为坐标系{C}。此时P点在{C}坐标系下的表示为，而坐标系{C}与坐标系{A}的姿态一致，所以该表示等价于，于是我们可以得到下面的等式：

为了更加简洁的表示上面的式子，我们有：

其中我们定义齐次变换矩阵为：

注意该式仅仅只是用于简化公式推导，但是在计算机程序中一般不用它来进行计算，因为这个变换将把时间消耗在1、0的乘法运算上，这并不是我们想看到的结果。

算子

到目前为止，我们讨论的内容都集中于如何对一个向量在不同坐标系间的表示进行变换上，反过来想，我们可以将这种操作看作是一个矢量在固定坐标系下的变换。比如说，假设坐标系{B}由坐标系{A}平移得到，那么点P从坐标系{B}变换到坐标系{A}上可以理解为以{A}原点作为起点的矢量，按照进行平移得到。同理，假设坐标系{B}是由{A}旋转k角度得到，那么点P从坐标系{B}变换到坐标系{A}上可以理解为在坐标系{A}中直接将向量旋转k所得。就好比我们一开始{A}、{B}重合，然后将{A}旋转-k得到{A'}，然后在{A'}中寻求P的表示，这与我们在{A}中直接将P对应的矢量旋转k是一样的。

不论是平移还是旋转，首先假设初始时刻坐标系{A}、{B}重合，然后将点P固定在{B}中，使之随着{B}的变换而变换，此时，我们可以看见，坐标系{B}的变换即是点P在{A}中的坐标变换。

混合变换

如下图所示，已知，求：

此时我们可以列出下面的式子：

求得：

逆变换

假设我们已知{B}相对于{A}的变换，要求{A}相对于{B}的变换，也即求矩阵的逆。一个很直接的办法是直接对这个4*4的齐次变换矩阵求逆，另一种方法是利用我们上面说到的变换的性质求逆：

直接写出{A}相对于{B}的变换：

变换方程

这一小节比较简单，我们直接给出示例：

由上图可以列出等式如下：

如果我们一直其中任意4个变换，便可以求出剩下的一个未知变换。

姿态的其他描述方法

我们知道旋转矩阵是单位正交阵，又因为其行列式为1，所以被称为标准正交阵（非标准正交阵的行列式值为-1）。前面我们聊到旋转矩阵中有9个量，但是却只表示了3个自由度，是因为矩阵中的元素存在着6个约束：

这里我们假定旋转矩阵R为：

所以，旋转矩阵只有3个自由度。另一种方法是使用正交矩阵的凯莱公式，对于任何正交阵R，都存在一个反对称矩阵S（）满足：

由反对称矩阵的定义知：反对称矩阵主对角线上的元全为0，位于主对角线两侧对称的元反号。所以，一个三维的反对称矩阵可以由三个参数表示：

从上面可以看出来，任何3*3的旋转矩阵可以用3个参量确定。那我们能否找到一种表示方法，直接用三个参量表达姿态呢？大家是不是一下想起了诸如X-Y-Z固定角坐标系和Z-Y-X欧拉角坐标系？是的，这些表示方法被统一称为角坐标系表示法，共有24种。这类表示法均需要按照一定的顺序进行三次绕主轴的旋转，如果大家有了解过相关知识，或许知道还有一种表示法，只需要绕某一个轴旋转一次即可，它就是等效轴表示法，这种表示法基于“任何方位都可以通过选择适当的轴和角度得到”，下面我们将对这些知识点进行讨论。

首先介绍X-Y-Z固定角坐标系，为什么叫固定角坐标系？“固定”指的是旋转是绕着固定的参考系的各个主轴，比如：

我们对{B}进行旋转，每次都是绕着固定坐标系{A}的轴进行旋转的，我们常将绕着X、Y、Z的旋转称为pitch、yaw、roll，也即俯仰、偏航、翻滚。将三次旋转利用旋转矩阵写出并相乘：

乘积为：

接着我们介绍Z-Y-X欧拉角坐标系，在这种表示法中，我们每次旋转都是绕着{B}的主轴进行旋转，也就是说，并不是绕着固定坐标系{A}的轴进行旋转了：

在这种表示下，有：

关于这个式子应该怎么理解呢？我们最终的目标是求出{B}相对于{A}的旋转，对于固定角坐标系，每次旋转都是基于{A}的，因而是按照旋转次序，依次左乘；对于欧拉角则是利用中间坐标系变换，所以依次右乘。比如上面的Z-Y-X欧拉角，记{A}绕{A}的Z旋转得到{B'}，然后{B'}绕{B'}的Y旋转得到{B''}，最后{B''}绕{B''}的X旋转得到{B}，所以，我们可以写出：

式中的3个旋转角分别是绕着{A}的Z轴，{B'}的Y轴，{B‘’}的X轴旋转（所以相对于我们的旋转，是右乘，先乘以绕X的旋转，再乘以绕Y的旋转，最后才是绕Z的旋转），因此我们可以得出结论：X-Y-Z固定角与Z-Y-X欧拉角在同样的角度大小下，旋转所得到的最终结果是一样的，也就是说在这两种表示下，{B}相对于{A}的姿态一致。注意，这并不是巧合，是因为固定角表示下是基本旋转矩阵左乘，而欧拉角表示下是基本旋转矩阵的右乘，而恰好X-Y-Z与Z-Y-X是相反的旋转顺序，所以最终的效果就一样了。

然后介绍一下等效轴角坐标表示法，也即用一个单位矢量加上一个旋转角表示旋转：

因为我们的单位矢量长度恒为1，所以实际上确定它只需要两个参数，加上旋转角，也即三个参数，正好确定旋转所需要的3个自由度。当旋转轴K为一般轴时，等效旋转矩阵为：

除了上面的固定角坐标系表示法、欧拉角表示法和等效轴角坐标系表示法之外，我们再介绍另一种姿态表示法，这种表示法通过四个数值来表示，称为欧拉参数。

由等效旋转轴和等效旋转角定义的欧拉参数为：

我们将这四个变量平方相加，得到：

也即，这四个参数不是独立的，所以，仍然符合我们对于姿态的3自由度的认知。同时，由上面的式子可知，我们可以将一个姿态看作是四维空间中单位超球面上的一点，或者说，它是一个单位四元数。用这组参数表示的旋转矩阵为：

已知旋转矩阵求欧拉参数为：

本次介绍就到这里，欢迎大家讨论~