视觉SLAM ch3—三维空间的刚体运动

Johaden

已于 2024-08-14 20:11:41 修改

阅读量930

点赞数 19

分类专栏：视觉Slam 文章标签：机器学习算法人工智能 SLAM ubuntu linux

于 2024-08-10 21:14:30 首次发布

本文链接：https://blog.youkuaiyun.com/Johaden/article/details/141023487

版权

如果对于某些线性代数的知识不太牢固，可以看一下我的另一篇博客，写了一些基础知识并推荐了一些视频。

旋转矩阵单元所需的线代基础知识https://blog.youkuaiyun.com/Johaden/article/details/141023668

一、旋转矩阵

1.点、向量、坐标系

在数学中，特别是在线性代数领域，一个三维空间中的坐标可以被视为一个向量与线性空间基的乘积，这是因为线性空间的基提供了一种方式来表示该空间中的所有向量。为了理解这一点，首先需要明确什么是线性空间的基。

线性空间的基是一组向量，这组向量满足两个条件：第一，它们线性无关，即没有向量可以通过其他向量的线性组合来表示；第二，它们能够生成整个线性空间(向量的数量不能少于空间的维数、秩与维数相等)，即线性空间中的任何向量都可以通过这组基向量的线性组合来唯一表示。在三维空间中，基通常由三个线性无关的向量组成，这三个向量分别对应于空间中的三个坐标轴。若三个向量两两垂直就是一个标准正交基（下面的就是），组成如下的矩阵为正交矩阵（A’A=E）（两个基矩阵各自是正交矩阵，相乘后仍是正交矩阵）。

已知正交矩阵后，可以推出转置=逆，即 $A'=A^{-1}$ 。

坐标的取值与向量本身有关也与坐标系（线性空间的基）选取有关。

机器人中有很多种坐标系：①世界系/惯性系（World） ②机体系（body） ③传感器参考系（Sensor）。不同坐标系存在变换关系。传感器捕获的数据都在Sensor系里，但是分析位姿时不关注传感器的移动，更多关注机体的移动，所以需要进行Sensor=>body=>world的变换。第一个变换我们已知（机器人在制作过程中就已知了，人为给定。以及制作时传感器相对于机体的位置也已知，可以进行转换）。第二个translate需要估计，需要在运动过程中估计机器人在世界的位置和姿态（SLAM本质）。

运算：

内积定义：

外积定义(在几何代数中，“∧”符号表示把向量变为矩阵，形式为反对称矩阵)：

计算用到了行列式的拆分知识（注意，i，j，k都为单位向量，模为1）。写成相乘的格式利于后面的编程和推导。

几何意义：方向满足右手定则，大小为 $|a||b|\sin \theta$ ，为平行四边形的有向面积。

坐标：

当我们说一个三维空间中的坐标是一个向量乘以线性空间的基时，实际上是在说任何一个点（或向量）在这个三维空间中的位置都可以通过其在三个基向量方向上的分量（即坐标）来唯一确定。换句话说，如果我们用v表示一个向量，用 $e_{1}$ , $e_{2}$ , $e_{3}$ 表示三维空间的一组基，那么该向量可以表示为：

其中，x,y,z是该向量相对于基 $e_{1}$ , $e_{2}$ , $e_{3}$ 的坐标。这种表示方法说明了向量与基之间的关系，即向量可以看作是其坐标与基向量的线性组合。

2.位姿变换和旋转矩阵

（1）位姿变换：

旋转变换

坐标系（e1，e2，e3）发生一次旋转变为（e1'，e2'，e3'）

对于一个固定向量a，它的坐标如何变化？

坐标关系：

$[e_{1},e_{2},e_{3}]\begin{bmatrix} a_{1}\\ a_{2} \\ a_{3} \end{bmatrix}=[e_{1}',e_{2}',e_{3}']\begin{bmatrix} a_{1}'\\ a_{2}' \\ a_{3}' \end{bmatrix}$

左乘 $\begin{bmatrix} e_{1}^{T}\\ e_{2}^{T} \\ e_{3}^{T} \end{bmatrix}$ 得：

我们把这样一个3×3的矩阵R称为旋转矩阵。

旋转矩阵有以下两个特性：①正交矩阵 ②行列式为+1（充要）

。 n为3时为三维空间的旋转，特殊正交群可以看作一个集合。

同理， $a'=R^{-1}a=R^{T}a$ （R为正交矩阵，转置就是逆，出于复杂性考虑，常用转置）

示例，从1位置以 $R_{12}$ 为旋转矩阵旋转到2位置（R12表示把姿态由2系对齐到1系的旋转矩阵 ）：

$a_{1}=R_{12}a_{2}$

$a_{2}=R_{21}a_{1}$

$R_{21}=R_{12}^{-1}=R_{12}^{T}$

进一步，可以有三个坐标系的递推：

欧式变换

$a_{1}=R_{12}a_{2}+t_{12}$ （ $t_{12}$ 表示把2系的坐标原点对齐到1系的平移量）

在多层坐标系的变换时，不得不写成

$c=R_{2}(R_{1}a+t_{1})+t_{2}$

直观来看就是：①旋转 ②平移

$a'=Ra+t$

3.变换矩阵与齐次坐标

上面的形式在变换多次之后会过于复杂。因此，我们要引入齐次坐标和变换矩阵重写

式。

中间红框的矩阵称为变换矩阵T。记 $\widetilde{a}=\begin{bmatrix} a\\1 \end{bmatrix}$ 为齐次坐标（四维）。

这样变换利于连续坐标系转换，如：

特殊欧式群(旋转+平移)：

此处不是简单的取反，需要排除旋转的影响，因为是先旋转后平移，平移量受旋转量影响。类比y=kx+b，左右移动时不是直接调整b的值，而是调整y=k（x+b/k）中b/k的值。

若世界坐标系为 $T_{w}$ ，机器人坐标系为 $T_{R}$ ，一个点的世界坐标为 $P_{w}$ ，机器人坐标系下为 $P_{R}$ ，那么满足： $P_{R}=T_{Rw}P_{w}$ （注意T的下标与R的下标一致，“就近原则”）

其中的 $T_{Rw}$ 意思就是从w到R进行平移，如果把 $T_{Rw}$ 后面乘一个0向量（与T一样，三个0一个1），结果就是矩阵的平移t（很容易，直接稍微算一下，把R全消掉了，只剩t了）， $T_{Rw}$ 是世界坐标系原点在机器人坐标系下的表示，反之，可以自己思考一下。

4.旋转向量(角轴)

旋转向量由一个方向向量和一个标量组成，其中方向向量定义了旋转的轴，而标量则定义了绕该轴旋转的角度。旋转向量的长度等于旋转角度的弧度值。

旋转向量可以用一个三维向量 $r=[\Theta _{x},\Theta _{y},\Theta _{z}]^{T}$ 来表示，其中 θx, θy, 和 θz 分别是绕 x, y, z 轴旋转的弧度数。但是，实际上旋转向量的表示是规范化的，即它的方向表示旋转轴的方向，而它的模长表示旋转角度。所以一个旋转向量 r 可以表示为：