【SLAM笔记】三维刚体运动

SLAM笔记专栏：https://blog.csdn.net/weixin_44543463/category_10925276.html

一、旋转矩阵

1.1 向量

向量与坐标是两个不同的概念。向量只是有大小和方向的量。只有当指定了三维空间中某个坐标系的时候，才能谈论向量在此坐标系下的坐标。因此向量的坐标，既和向量本身有关，也和坐标系的选取有关。
向量的内积描述了向量之间的投影关系。

向量的外积，方向垂直于这两个向量，大小为|a||b|sin<a,b>。

1.2 欧氏变换

两个坐标系之间的旋转、平移关系，统称为坐标系之间的变换关系。机器人运动过程中，往往会设定一个惯性坐标系（即世界坐标系），可以认为它是固定不动的。机器人则是一个移动坐标系。
如果需要知道某个向量在机器人坐标系中与世界坐标系中如何转换的，就需要先得到该向量对机器人坐标系的坐标值，再根据机器人位姿转换到世界坐标系中，这个转换关系用矩阵T来描述。
机器人移动是一个刚体运动，即同一个向量在各个坐标系下的长度和夹角都不会发生变化，这种变化就是欧式变换。

（1）旋转矩阵
假设某个单位正交基( $\boldsymbol{e_1}$ , $\boldsymbol {e_2}$ , $\boldsymbol{e_3}$ )经过了一次旋转，变成了( $\boldsymbol {e_1}'$ , $\boldsymbol{e_2}'$ , $\boldsymbol {e_3}'$ )，则对于同一个向量 $\boldsymbol{a}$ （该向量没有随坐标系旋转而运动），则可知

用矩阵表示如下

为了表示两个坐标之间的变换关系，等式两边同乘[ $\boldsymbol{e_1}^T$ $\boldsymbol{e_2}^T$ $\boldsymbol{e_3}^T$ ]T，则左侧矩阵变成了单位矩阵：

中间这个行列式为1的正交矩阵，就是所谓的旋转矩阵。
同时，此旋转矩阵的逆描述了一个相反的旋转。

（2)平移矩阵
平移矩阵十分简单，只需要将平移量加到旋转之后的坐标上就可以了。

1.3 变换矩阵

使用下面这种形式进行变换时，变化多次之后往往会变得过于复杂且不满足线性关系。

因此通常我们会使用如下的齐次坐标和变换矩阵进行变换。即把三维向量末尾加一，变成四维向量，称为齐次坐标 $\tilde{a}$ 。将旋转矩阵和平移矩阵写在同一个矩阵里面，这个矩阵T称为变换矩阵。

引入齐次坐标就可以实现多个变换矩阵的连乘，得到一个总的变换矩阵，实现多次变换的累加。

二、角轴和欧拉角

2.1 问题提出

旋转矩阵有九个量，但一次旋转只有三个自由度，变换矩阵用十六个量表达六自由度变换，表达方式可能冗余。同时旋转矩阵本身要求必须是正交矩阵，变换矩阵一样都需要约束条件，有些情况下这些约束会使求解变得困难。

2.2 角轴

（1）定义
任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们可以使用一个向量，其方向与旋转轴一致，长度等于旋转角，这种向量就称为旋转向量或角轴。
使用角轴表示方法只需要一个三维向量即可描述旋转。同样对于变换矩阵，我们使用一个角轴和一个平移向量即可表达。
（2）角轴与旋转矩阵的转换
假设有一个旋转轴为 $\boldsymbol {n}$ ，角度为θ的旋转，显然旋转向量为θ $\boldsymbol {n}$ 。由角轴转化为旋转矩阵，可以使用罗德里格斯公式：