之前看高博的书已然写过一篇李群和李代数,但只是简单地介绍了相关的概念和用法,说得不够深入和系统。之前再次好好学习了一下机器人状态估计问题下的李群和李代数,准备好好聊聊这个问题,这会是一系列文章,大概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》