之前看高博的书已然写过一篇李群和李代数,但只是简单地介绍了相关的概念和用法,说得不够深入和系统。之前再次好好学习了一下机器人状态估计问题下的李群和李代数,准备好好聊聊这个问题,这会是一系列文章,大概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.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的欧拉角序列,对于上式有
考虑单个坐标轴的角速度及其旋转矩阵有
又有
等式就可以化简为
其中,S为
右乘
由此,就可以使用欧拉角
可以看到,时,S的逆阵不存在。这个也是1-2-3欧拉角序列的奇异点。
上式对于任意的欧拉序列均成立。
2.3 加上扰动的旋转
我们知道旋转不属于向量空间,而属于非交换群,所有旋转构成了SO3群。
所以,问题在于:当要对观测或运动线性化时,如何对旋转进行线性化?
有欧拉定理
由此对旋转角
因此有
如果是绕着某个主轴
那么,考虑
对于任意一个常向量,
这里使用了
合起来就有
上式对于所有欧拉序列都成立。
2.3.1 对旋转施加扰动
有泰勒展开式
注意到这个展开中
对一组欧拉角施以扰动
v 可以是任意向量,因此在等式两边消去就得到
上式可以理解成极小角旋转矩阵和未被扰动的旋转矩阵相乘。
将之代入到泰勒公式并乘开就得到对应的雅可比矩阵。
即上一小节的最后一个等式。
2.4 平移
上图中,向量间的关系如下:
上式没有说明坐标系,因此,取向量在惯性系i下的表示只需要添加下标i即可:
点P固定在载具上,或者被表达在载具坐标系v下,上式就有
对应地,载具的姿态就被记为
3. 与十四讲区别
注意到前一小节末尾到旋转矩阵为
在机器人学中,常常使用的是运动系
这个公式是基于左手定则的。
前面对应的一些旋转矩阵的表达也需要取转置,其余的推导都是通用的。
对于姿态
而其逆矩阵
如果觉得有用,还请点个赞: )
参考文献:《state estimation for robotics》