一个数的亲和数iiic++_李群和李代数（一）：刚体的三维运动

之前看高博的书已然写过一篇李群和李代数，但只是简单地介绍了相关的概念和用法，说得不够深入和系统。之前再次好好学习了一下机器人状态估计问题下的李群和李代数，准备好好聊聊这个问题，这会是一系列文章，大概4、5篇左右，希望能把李群和李代数讲清楚，对大家有所帮助。

再次说明，李群和李代数是一个很广泛的话题，我们聊的只是机器人位姿估计这个大背景下的相关概念和知识。今天先来一个开胃小菜，回顾下刚体在三维空间中的运动。

除非特殊说明，本章聊到的坐标系均为右手系。

2019.08.11：把取反对称矩阵的符号由

改为

,和后面的李群、李代数进行区分。

1. 旋转

先来看看各种旋转表示法。

1.1 旋转矩阵

对于原点相同的两个坐标系

和

，那么一个向量

可以表示为：

由此，可以得到一个

到

的旋转矩阵：

旋转矩阵有如下性质：

1.1.1 基本旋转矩阵

1.2 欧拉角

一个旋转可以分解成3次分别绕X，Y，Z 轴的旋转来表示。

• 一种为3-1-3顺序，依次为自旋角（spin angle）
  - 章动角（nutation angle）
  - 进动角（precession angle）
  . 因此，
  。
• 一种为1-2-3顺序，依次为翻滚（roll） - 航偏（yaw）- 俯仰（pitch)。

对于三参数的表示方式必然存在奇异点。对于3-1-3序列，

是奇异点，此时改变另外两个角会对旋转造成同样的效果。对于1-2-3序列，

是奇异点。

需要注意的是，该问题仅存在于想从旋转矩阵计算欧拉角的过程中。

1.3 旋转向量

任意旋转都可以用一个旋转轴和一个旋转角来描述。那么，我们可以用一个向量，其方向为旋转轴

的方向，大小则为旋转角

，这种向量就是

旋转向量，表示为

.

对应的旋转矩阵可以用罗德里格斯公式来表示

该旋转矩阵与转轴

在哪一个坐标系下无关，因为

.

1.3.1 无穷小旋转

在无穷小旋转的情况下，有

可以看到，无穷小旋转对应的旋转矩阵与坐标轴旋转的次序无关。

1.4 欧拉参数

定义一个变量的组合，

这四个参数称为欧拉参数。它们之间存在约束

1.5 四元数

由此，可以引出四元数

。这里，

是标量。

引入四元数的左手形式的复合算子

和右手形式的复合算子

（左右乘）。

对应的有

1.5.1 四元数的运算

这里只列出关于四元数的简单运算。

现有两个四元数

, 向量表示分别为

。

1. 加减法

2. 乘法

乘法将两个四元数的每一项两两相乘然后相加。注意其满足之前所述的四元数的性质。

该乘法定义下，两个实四元数的乘积仍然是实四元数。由于最后一项外积的存在，四元数的乘法不满足交换律，除非

和

在$$ 中共线（此时外积为零）。

3. 共轭

共轭即将虚部取为相反数：

四元数共轭与其自身相乘可得一个实四元数，实部为模长的平方。

4. 模长

可知，两个四元数乘积的模即为二者模的乘积。所以，单位四元数相乘后仍然得到单位四元数。

5. 逆

可见，对于单位四元数，其逆和共轭相同。按照定义，四元数和自己逆的乘积为实四元数

. 最后，四元数乘积的逆有和矩阵相乘相似的性质：

6. 数乘和点乘

四元数的数乘与点乘与向量相同：

1.5.2 四元数表示旋转

在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

四元数和旋转矩阵的关系为：

设矩阵

, 则由上式可以推得：

要用四元数来计算旋转，首先，用一个虚四元数来表示三维空间点：

则旋转后的点p' 则为

2. 旋转运动学

假设参考系

相对

旋转，角速度为

。而

相对

旋转角速度为

。

的模即为旋转速率，而它的方向为

瞬时旋转轴。

在两个坐标系下观察到的旋转运动是不一样的。对于两个坐标系的向量时间导数有：

相当于基向量不动。

可以有

现在，希望得到任意向量在两个坐标系下的向量时间导数：

则，可以利用2系中的向量来表示1系中看到的时间导数：

对于任意向量

上式均成立，

该式刻画了惯性系下速度与运动系下速度的变换关系。

进一步，把2系下的角速度记为

，有

再左乘一个旋转矩阵，就可以得到1系中观察到的惯性时间导数：

2.1 角速度与旋转矩阵

从

开始，对等式两边求导，

利用先前的公式，用角速度

来表达

再进一步有

得到

此即泊松公式。给定2系下测量到的角速度，再对上式进行积分，就可以得到1到2的旋转矩阵。

相对地有

2.2 欧拉角下的表示

假设使用1-2-3的欧拉角序列，对于上式有

考虑单个坐标轴的角速度及其旋转矩阵有

是单位阵的第i列。

又有

等式就可以化简为

其中，S为

右乘

相当于取某个矩阵的第i列。

由此，就可以使用欧拉角

和欧拉角速度

来表示角速度

。同样有

可以看到，

时，S的逆阵不存在。这个也是1-2-3欧拉角序列的奇异点。

上式对于任意的欧拉序列均成立。

2.3 加上扰动的旋转

我们知道旋转不属于向量空间，而属于非交换群，所有旋转构成了SO3群。

所以，问题在于：当要对观测或运动线性化时，如何对旋转进行线性化？

有欧拉定理

由此对旋转角

求导可以得到：

因此有

如果是绕着某个主轴

进行旋转，就会有

代表单位矩阵的第

列。

那么，考虑

的旋转序列，

对于任意一个常向量，

这里使用了

合起来就有

上式对于所有欧拉序列都成立。

2.3.1 对旋转施加扰动

有泰勒展开式

注意到这个展开中

是一个

无约束的量，但是大部分旋转的表达是有约束的，因此 不能像上面那样直接泰勒展开。只有 欧拉角表示法没有约束限制，所以这里我们选择欧拉角表示法。

对一组欧拉角施以扰动

，

v 可以是任意向量，因此在等式两边消去就得到

上式可以理解成极小角旋转矩阵和未被扰动的旋转矩阵相乘。

将之代入到泰勒公式并乘开就得到对应的雅可比矩阵。

即上一小节的最后一个等式。

2.4 平移

上图中，向量间的关系如下：

上式没有说明坐标系，因此，取向量在惯性系i下的表示只需要添加下标i即可：

点P固定在载具上，或者被表达在载具坐标系v下，上式就有

对应地，载具的姿态就被记为

3. 与十四讲区别

注意到前一小节末尾到旋转矩阵为

，即

。和之前一直使用到

不同。

在机器人学中，常常使用的是运动系

相对于惯性系

的旋转（

）。之前的右手系是采用

来构建的（由F1旋转到F2）。二者之间的关系是一个转置：

。因此罗德里格斯公式与十四讲之中的相反。

这个公式是基于左手定则的。

前面对应的一些旋转矩阵的表达也需要取转置，其余的推导都是通用的。

对于姿态

有

而其逆矩阵

可以表示为

---

如果觉得有用，还请点个赞: )

参考文献：《state estimation for robotics》