首先感谢古月居转载我的文章，但是本文有很多乱码和不清晰的地方影响阅读体验，如果对本文感兴趣，欢迎大家去看我的知乎原帖，同时欢迎留言讨论！

这章的Reference和第一章一样，需要一点理论力学和矩阵分析的基础

需要一点耐心（数学推导枯燥且烦人，坚持一下）

chapter3分了几部分，后续内容链接在文章末尾

Chapter3：Rigid-Body Motions

在chapter2中，可知至少需要6个数字来指定一个刚体在三维物理空间中的位置和方向。在chapter3中，将介绍描述刚体位置和姿态的方法，即通过附加参考系到刚体上进行描述。动系相对于固定参考系被表示为4x4的实矩阵，这个矩阵就是C空间的隐式表示，将10个约束条件应用于4x4实矩阵的16维空间，就可以得到描述刚体的6维空间。

同时，上述的矩阵不仅可以用来表示坐标系的位形，还可以用于平移和旋转一个矢量或一个坐标系，将一个向量或一个坐标系的表示从一个坐标系中的坐标更改为另一个坐标系中的坐标，这通过简单的线性代数操作都可以完成。

C空间的位置和方向的非欧几里德(非“平坦”)性质是选用矩阵表示的原因。

刚体的速度可以被简单地表示为一个点在  中的3个线速度和3个角速度，合起来被称为spatial velocity or twist，更一般地说，即使机器人的C空间可能不是一个向量空间，但C空间中任意一点的可行速度集合总是构成一个向量空间，这可能难以理解，举个例子，一个机器人，它的C空间是球面，球面不是平坦的，但在球面上的任何一点上，速度空间可以被认为是与球面上那一点相切的平面（向量空间）。

任何刚体运动都可以被分解为对固定参考系的平动以及绕固定轴的转动，运动合成之后形成类似螺旋的运动，即绕同一固定轴旋转和平移运动的合成。

本章把线速度和角速度表达成一个在  中的向量，力矩和力也一样，力矩和力组成的向量被称为spatial force or wrench。

之后有关向量的基础知识这里省略，需要注意的是，书中所有的坐标系都是惯性系，坐标为笛卡尔右手坐标系。

图1 坐标系

介绍完基本的名词描述，下面我们先来看平面中的刚体运动

考虑图中的灰色物体在平面内的运动，定系为  ,动系为  ,定系的单位轴向量为  和  ，动系的单位轴向量为  和  。

图2 平面刚体运动图

动系原点p在定系中的表达式为：

动系和定系的关系为：

这是最简单的情况，现在我们再引入一个动系 

图3 引入新的动系

以定系  为参考系，动系原点p可以用向量来表示： 

动系  的单位轴向量就可以写成矩阵P，同时矩阵P称为旋转矩阵（rotation matrix），旋转矩阵必须是单位正交阵。

同理，动系  就可以表示为：

当然，我们也可以用动系  为参考系来描述动系  ，那么动系c相对于b的表达可以写作： 

如果已知动系c相对于b的关系和动系b相对于定系s的关系，那么动系c相对于定系s的关系就可以这么计算：

由此可见，动系b可以用来转换一个点或坐标系的表示到定系s。

相信看到这里，大家脑袋里都有一个疑问，这么做究竟是为了什么，推导这个的意义是什么？继续往下看

现在假设平面内有一个刚体，刚体上有两个动系c和d，动系d的初始位置和定系s重合，动系c的初始位置和我们上面例子的c的描述一致，接着刚体移动了，动系d从初始位置移动到了  ,这个新位置的描述和我们上面例子的b一致，那么c的新位置在哪呢？显而易见，是不是c的新位置的描述可以用下式来表达呢：

可见，刚体的移动以这种方式来表达了，因此上述方程不被看作是坐标的改变，而是刚体位移(也称为刚体运动)。在这里可以明显的看出，旋转矩阵描述了刚体的定轴转动，而向量的变化描述了刚体的平动。

通过上面的例子，因此，我们可以看到一个旋转矩阵和向量组成的数对，如(P, p)，可以用于三个目的:

（1）表示刚体在定系中的位置，如图2

（2）改变所表示矢量或坐标系的参考系，如图3

（3）移动一个矢量或一个坐标系

之后我们把结论从二维开始向三维推广，考虑空间中的刚体运动问题。

图4 空间中刚体图

与二维问题一致，动系b相对于定系s的表示为：

动系b原点p：

各单位轴向量：

写成向量形式，就有：

由(R, p)给出的12个参数提供了刚体相对于定系的位置和方向的描述。因为刚体的方向只有三个自由度，在旋转矩阵中的9个元素中只有3个可以独立选择，这里的表示方法就有很多了，如指数坐标，三参数欧拉角，roll–pitch–yaw angles，Cayley–Rodrigues parameters，四元数等等，较为常用的就是欧拉角了，感兴趣可以翻看哈尔滨工业大学理论力学教研室编.理论力学Ⅱ（第五版）[M].高等教育出版社，2009的第五章，这本非常经典的理论力学书非常有用。

接下来进入对转动这一非常重要的性质的讨论，关于旋转矩阵的性质这里就省略了，旋转矩阵R有三种作用：

（1）表示方向

（2）改变一个向量或一个坐标系所表示的参考系

（3）旋转矢量或坐标系

（1）中，R被认为代表一个参考系，（2）和（3）中，R被认为是作用于矢量或坐标系的算子。

如下图，动系a，b，c都与定系s重合

因此，动系a，b，c可以表示为：

这样，p点在各个动系内的坐标为：

可以看出，动系b相对于a绕z轴逆时针旋转了90°，动系c相对于动系b绕y轴逆时针旋转了90°。

上面的表述默认情况是动系对定系，如  ，严格来说还不严谨，动系a相对于定系s的表达规范应为  ，动系a相对于b的表达为  。为了避免坐标系较多时混淆，推荐严谨一些的表达。

因此，有：

很容易可以看出上面两个矩阵互为逆矩阵，且互为转置，实际上，对于任意两相对坐标系符号记为d和e，均有：

那么如何改变参考系呢，方法很简单，只需要对矩阵做乘法就可以了，例如：

可以通过下标消去规则来描述这个性质：当两个旋转矩阵相乘时，如果第一个矩阵的第二个下标与第二个矩阵的第一个下标匹配，则两个下标“消去”，从而实现参照系的改变，规则如下：

向量的参照系也可以通过一个旋转矩阵改变，下标消去规则为:

最后是向量或者坐标系的旋转，注意R是一个旋转算子，而非方向，算子这样来表示：

与之对应的图形为：

那么绕坐标轴旋转的表述如下：

更一般的还有，当你绕某轴  旋转时，旋转矩阵为：

式中 

有了旋转矩阵，通过矩阵乘法就可以完成向量或者坐标系的旋转了。

现在我们有了基础的旋转矩阵，那如何描述刚体的转动呢？没错，那就是角速度，角速度的推导在此省略。

依旧是刚体绕  轴旋转  ,将角速度表示为向量形式，此时刚体的角速度就为：

有了角速度，那么刚体在坐标轴上的线速度就可以表示为：

既然是用坐标向量来描述角速度，那么坐标系的选择就很重要，可以选择定系s，也可以选择动系b，从简单的在定系中的角速度开始，记  为动系b相对于定系s的旋转矩阵，那么它对时间的一阶导数为 

 是一个3x3的矩阵，它的每一列的分量  分别表示定系中x，y，z轴的方向，设时间t时刚体在定系中的角速度为  ,那么此时刚体在定系坐标轴上的线速度就为：

这样就可以得出：

这里的  是一个3x3的斜对称阵，3x3斜对称阵的数学符号为  .

相信这里很多人就纳闷了，  不是个向量吗，怎么变成斜对称阵了，这里我们需要思考一下，当坐标系绕轴旋转的时候，会有下面这种情况出现：

那么对于  ，将它写成斜对称阵的如下形式是不是就可以描述上面的问题了呢：

这里需要对斜对称阵的性质加以介绍，对于斜对称阵A有：

现在我们给出一个向量  ,向量的斜对称阵记为  ，矩阵  ，总有下式成立：

有了斜对称阵这一工具，我们就可以这样表达下式：

这样角速度的斜对称阵就可以通过旋转矩阵求出,这就是角速度的求法了：

现在用  来表示刚体在动系中的角速度，怎么将动系中的角速度表达成在定系s中的角速度呢，这里要明白，角速度的大小是恒定的，只是在不同坐标系下有不同的表示而已，动系b相对于定系s的旋转矩阵为  ,那么根据我们上面提出的下标消去规则，就有：

再做一下进一步运算，注意  是对称阵，那么就有：

把上面的式子用斜对称阵来表示：

根据斜对称阵的性质以及上面推出的结论，就有：

经过这样的推导，总结一下：

R（t）表示定系旋转至动系的旋转矩阵，这样在各坐标系内的角速度为：

需要注意， 这里的 不是相对于动系b的角速度，而是把动系b看作固定不动绕动系b运动的角速度，这里需要强调，刚体的角速度只是因为在不同的坐标系内的表达不同而已，这个非常重要。

那么不同坐标系之间的角速度如何转换呢，很简单，只需要使用下标消去原则即可，操作和下式一致：