初识SLAM到认识李群学习笔记

最新推荐文章于 2024-02-15 13:07:45 发布

肖永威

最新推荐文章于 2024-02-15 13:07:45 发布

阅读量590

点赞数

分类专栏：人工智能及Python 文章标签： SLAM 李群李代数机器人

本文链接：https://blog.csdn.net/xiaoyw71/article/details/109502706

版权

人工智能及Python 专栏收录该内容

127 篇文章 40 订阅

订阅专栏

本文为学习高翔、张涛等著的《视觉SLAM十四讲》的学习笔记，通过研究视觉SLAM，从三维空间刚体运动引出群论和李代数。

1. 初识SLAM

单目相机以二维的形式记录了三维的世界，缺少场景的深度维度，没有直观的距离感，也就是相机与物体间的距离无法获得。
而双目相机和深度相机，可以通过两个相机之间的距离（基线），以及大量的计算才能估计出距离，有了空间感。

在这里插入图片描述

在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。

后端优化与回环监测

度量地图
度量地图强调精确的表示地图中物体的位置关系，通常用稀疏（Sparse）与稠密（dense）对其分类。
在二维度量地图中体现为许多个小格子（Grid），而在三维度量地图中则体现为许多小方块（Voxel）。通常，一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。

机器人运动SLAM数学描述：

运动轨迹：机器人连续运动离散化，变成离散时刻 $t = 1, . . ., K$ 当中发生的事情。在每个时刻，用 $x$ 表示机器人自身位置。于是时刻位置记为 $x_{1},...,x_{K}$ ，构成了机器人的轨迹。
地图路标：假设地图由路标组成，每个时刻观测到一部分路标有 $N$ 个，用 $y_{1},...,y_{2}$ 表示。

运动：我们分析从 $k - 1$ 时刻到 $k$ 时刻，机器人的位置 $x$ 是如何变化的。
$x_{k} = f(x_{j-1},u_{k},w_{k})$ （2.1）
这里， $u_{k}$ 是运动传感器的读数或输入， $w_{k}$ 是该过程中的噪声。
观测：假设机器人在 $k$ 时刻于 $x_{k}$ 处探测到某个路标 $y_{j}$ 。
$z_{k,j} = h(y_{j},x_{k},v_{k,j})$ (2.2)
这里， $v_{k,j}$ 是这次观测的噪声。

2. 三维空间刚体运动

2.2. 点与向量、坐标系

点是空间中的基本元素，没有长度、体积，两个点连接起来就构成了向量，向量也可以看做从一个点到另一个点的一个箭头。当我们指定向量所在的三维空间的某个坐标系时，才可以谈论该向量在此坐标系下的坐标。

用线性代数描述，三维空间中的某个点的坐标用 $\mathbb{R}^3$ 来表述。假设在这个线性空间内，我们找到该空间的一组基 $e_{1},e_{2},e_{3})$ ，那么，任意向量 $a$ 在这组基下就有一个坐标：
$[e_{1},e_{2},e_{3}]\begin{bmatrix}a_{1}\\ a_{2}\\ a_{3}\end{bmatrix} = a_{1}e_{1}+a_{2}e_{2}+a_{3}e_{3}$

这里 $a_{1},a_{2},a_{3})^T$ 称为 $a$ 在此基下的坐标。

坐标系通常由3个正交的坐标轴组成（也可以非正交）。例如，当给定 $x$ 和 $y$ 轴时， $z$ 轴就可以通过右手（或左手）法则由 $\times y$ 定义出来。

关于向量内积、外积：
$a . b = ∣ a ∣ ∣ b ∣ c o s (a, b)$
$\times b = \begin{Vmatrix}e_{1} & e_{2} & e_{3}\\ a_{1} & a_{2} & a_{3}\\ b_{1} & b_{2} & b_{3} \end{Vmatrix} = \begin{bmatrix}a_{2}b_{3}-a_{3}b_{2}\\ a_{3}b_{1}-a_{1}b_{3}\\ a_{1}b_{2}-a_{2}b_{1}\end{bmatrix} = \begin{bmatrix}0 & -a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2} & a_{1} & 0\end{bmatrix} b \overset{\underset{\mathrm{def}}{}}{=} a^{\wedge }b$

对于外积运算，我们引入 $^{\wedge}$ 符号，把 $a$ 写成一个矩阵，事实上是个反对称矩阵，将 $^{\wedge}$ 符号记成反对称矩阵。

$a^{\wedge } = \begin{bmatrix}0 & -a_{3} & a_{2}\\ a_{3} & 0 & -a_{1}\\ -a_{2} & a_{1} & 0\end{bmatrix}$

2.2. 坐标系变换与刚体运行

在实际机器人场景中，相机是一个移动坐标系 $x_{c},y_{c},z_{c}$ ，机器人运动系统被定义为惯性坐标系（或者叫世界坐标系） $x_{w},y_{w},z_{w}$ 。
在这里插入图片描述

例如相机视野中的向量 $p$ ，在相机移动坐标系下坐标为 $p_{c}$ ，从世界坐标系看为 $p_{w}$ ，如何转换呢？需要一个矩阵 $T$ 。

两个坐标系之间的运动由一个旋转加上一个平移组成，这种运行称为刚体运动。坐标做旋转和平移变换，称为欧式变换。

我们再假设某个单位正交基经过变换转变成 $e_{1}{'},e_{2}{'},e_{3}{'})$ 。而对于同一个向量 $a$ （没有随坐标系而变），在两个坐标系下的坐标分别为 $a_{1}, a_{2},a_{3}]^{T}$ 和 $a_{1}{'}, a_{2}{'},a_{3}{'}]^{T}$ 。

$[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}$ ，那么变换为：

$\begin{bmatrix}a_{1}\\ a_{2}\\ a_{3}\end{bmatrix} = \begin{bmatrix} e_{1}^{T}e_{1}{'} & e_{1}^{T}e_{2}{'} & e_{1}^{T}e_{2}{'}\\ e_{2}^{T}e_{1}{'} & e_{2}^{T}e_{2}{'} & e_{2}^{T}e_{2}{'}\\ e_{3}^{T}e_{1}{'} & e_{3}^{T}e_{2}{'} & e_{3}^{T}e_{2}{'} \end{bmatrix} \begin{bmatrix}a_{1}{'}\\ a_{2}{'}\\ a_{3}{'}\end{bmatrix} \overset{\underset{\mathrm{def}}{}}{=} Ra{'}$

把中间的矩阵拿出来，定义为一个矩阵 $R$ 。这个矩阵由两组基之间的内积组成，刻画了旋转前后同一向量的坐标变换关系。只要旋转一样，这个矩阵就是一样的。因此称为旋转矩阵：

$=\begin{bmatrix} e_{1}^{T}e_{1}{'} & e_{1}^{T}e_{2}{'} & e_{1}^{T}e_{2}{'}\\ e_{2}^{T}e_{1}{'} & e_{2}^{T}e_{2}{'} & e_{2}^{T}e_{2}{'}\\ e_{3}^{T}e_{1}{'} & e_{3}^{T}e_{2}{'} & e_{3}^{T}e_{2}{'} \end{bmatrix}$

同时，该矩阵各个分量是两个坐标系基的内积，由于基向量的长度为1，所以实际上是各基向量夹角的余弦值。

$A$ 是一个 $n$ 阶方阵， $A^{т}$ 是 $A$ 的转置。如果有 $A^{т}A=E$ （单位阵），即 $A^{т}$ 等于 $A$ 的逆，则称 $A$ 是正交矩阵。

旋转矩阵约束，必须是一个正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将 $n$ 维旋转矩阵的集合定义如下：

$\left \{ R \in \mathbb{R}^{n \times n} | R R^{T} = I ,det(R) = 1 \right \}$

其中， $S O (n)$ 是特殊正交群。 $S O (3)$ 是指三维空间的旋转。

欧式变换描述为：
旋转：
$a{'} = R^{-1}a = R^{T}a$
加上平移：
$a{'} = Ra + t$

则，实际上述相机例子的变换为：

$p_{w} = R_{wc}p_{c} + t_{wc}$

其中， $R_{wc}$ 是指“把相机移动坐标系 $c$ ”变换到世界坐标系 $w$ 中。（由于向量乘在这个矩阵的右边，它的下标是从右读到左）

这种三维变换在多次变换的情况下，比较啰嗦，通过数学技巧，在一个三维向量的末尾添加1，将变成四位向量，称为齐次坐标。

$\begin{bmatrix}a'\\ 1\end{bmatrix}=\begin{bmatrix}R & t\\ 0^{T} & 1\end{bmatrix}\begin{bmatrix}a\\ 1\end{bmatrix}\overset{\underset{\mathrm{def}}{}}{=} T\begin{bmatrix}a\\ 1\end{bmatrix}$

我们用 $\tilde{a}$ 表示 $a$ 的齐次坐标，则两次变换叠加有较好的表现形式：
$\tilde{b} = T_{1}\tilde{a} , \tilde{c}=T_{2}\tilde{b} \Rightarrow \tilde{c} = T_{2}T_{1}\tilde{a}$

这样的符号还是挺麻烦的，我们在不引起歧义的情况下，可以写成： $b = T a$ 。

关于变换矩阵 $T$ ，其左上角为旋转矩阵，右侧为平移向量，左下角为 $0$ 向量，右下角为1。这种矩阵称为特殊欧式矩阵：

$SE(3)=\left \{ T= \begin{bmatrix}R & t\\ 0^{T} & 1\end{bmatrix} \in \mathbb{R}^{4 \times 4} \right \} | R \in SO(3) , t \in \mathbb{R}^{4}$

与 $S O (3)$ 一样，求解该矩阵的逆表示一个方向的变换：
$T^{-1} = \begin{bmatrix}R^{T} & -R^{T}t\\ 0^{T} & 1\end{bmatrix}$

2.3. 旋转向量和欧拉角

旋转矩阵表达旋转有两个缺点：

$S O (3)$ 的旋转矩阵有9个量，但一次选择只用了3个自由度，变换矩阵有16个量表达6个自由度，有没有更紧凑的方式；
旋转矩阵必须是正交矩阵的约束，且行列式为1，求解困难。

因此，我们用一个三维向量表达旋转，用一个六维向量变大变换。事实上，任意旋转都可以用一个旋转轴和一个旋转角来表达。同样，变换矩阵则使用一个旋转向量和一个平移向量表达一次变换，这时的变量维数正好是六维。

从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明：

$\theta I + (1 - cos \theta)nn^{T} + sin \theta n^{\wedge}$

因此：
$\theta = arccos \frac{tr(R)-1}{2}$

$t r ()$ 是指求迹（trace），即是求矩阵的对角线元素之和。

欧拉角定义了一种直观的方式描述旋转——用3个分离的转角，分解成3次绕 $Z Y X$ 的分别旋转。

$Z Y X$ 转角相当于把任意旋转分解以下3个轴的转角：

绕物体的 $Z$ 轴旋转，得到偏航角yaw。
绕旋转之后的 $Y$ 轴旋转，得到俯仰角pitch。
绕旋转之后的 $X$ 轴旋转，得到滚转角roll。

最后，可以用 $r,p,y]^{T}$ 三维向量描述任意旋转。

2.4. 四元素

四元素是一种扩展复数，一个四元数 $q$ 拥有一个实部和3个虚部，本质上是一种扩展复数，描述3维的量，用四元数

$q = q_{0} + q_{1}i+q_{2}j+q_{3}k$

$\left \{\begin{matrix} i^{2}= j^{2} = k^{2} = -1 \\ ij = k,ji = -k \\ jk = i, kj = -i\\ ki = j,ik = -j \end{matrix}\right.$
如果把 $i, j, k$ 看作三个坐标轴，那么他们与自己的乘法和复数一样，相互之间的乘法和外积一样。有时，可以用一个标量和一个向量表达四元数：
$q=[s,v]^{T} , s= q_{0} \in \mathbb{R}, v=[q_{1},q_{2},q_{3}]^{T} \in \mathbb{R}^{3}$

从四元数到旋转矩阵的变换关系：

$vv^{T} + s^{2}I +2sv^{\wedge} + (v^{\wedge})^{2}$

最后，四元数到旋转向量的转换公式如下：

$\left \{\begin{matrix} \theta = 2arccos q_{0} \\ [n_{x},n_{y},n_{z}]^{T} = \frac {[q_{1},q_{2},q_{3}]^{T}}{sin \frac{\theta}{2}} \end{matrix}\right.$

2.5. 其他几种变换方式

相似变换：
$T_{S}=\begin{bmatrix} sR &t \\ 0^{T} & 1 \end{bmatrix}$

3. 李群与李代数基础

三维旋转矩阵构成了特殊正交群 $S O (3)$ ：

$\left \{ R \in \mathbb{R}^{3 \times 3} | R R^{T} = I ,det(R) = 1 \right \}$

变换矩阵构成了特殊欧氏群 $S E (3)$ ：

$SE(3)=\left \{ T= \begin{bmatrix}R & t\\ 0^{T} & 1\end{bmatrix} \in \mathbb{R}^{4 \times 4} \right \} | R \in SO(3) , t \in \mathbb{R}^{4}$

旋转矩阵、变换矩阵对加法是不封闭的，对乘法是封闭的。

$R_{1}R_{2} \in SO(3) , T_{1}T_{2} \in SE(3)$

对于这种只有一个（良好的）运算的集合，我们称之为群。

3.1. 群

群就是对称性，是一种集合加上一种运算的代数结构。集合记作 $A$ ，运算记作 $.$ ，则群记作 $G = (A, .)$ 。群要求运算满足以下4个条件：

封闭性： $\forall a_{1},a_{2} \in A,a_{1}.a_{2} \in A$
结合律： $\forall a_{1},a_{2},a_{3} \in A,(a_{1}.a_{2}).a_{3} = a_{1}.(a_{2}.a_{3})$
幺元： $\exists \in A,s.t. \forall a \in A,a_{0}.a = a.a_{0} = a$
逆： $\forall \in A, \exists a^{-1} \in A, s.t. a.a^{-1} = a_{0}$

其中， $s . t .$ 表示约束条件。

矩阵中常见的群有：

一般线性群 $G L (n)$ 指 $\times n$ 的可逆矩阵，它们对矩阵乘法成群。
特殊正交群 $S O (n)$ 就是旋转矩阵群，其中 $S O (2)$ 和 $S O (3)$ 最为常见。
特殊欧式群 $S E (n)$ 就是 $n$ 维欧氏变换，如 $S E (2)$ 和 $S E (3)$ 。

李群是指具有连续（光滑）性质的群， $S O (n)$ 和 $S E (n)$ 在实数空间上是连续的，可以直观想象一个刚体能够连续地在空间中运动。

$\forall$ 表示所有，全部意思，Any的倒写。

3.2. 李代数的引出

我们说， $R$ 是某个相机的旋转，它会随着时间连续地变换，即为时间的函数： $R (t)$ 。其旋转矩阵，有：

$R(t)R(t)^{T} = I$

我们对时间求导，经过推导，找到一个三维向量 $\phi(t) \in \mathbb{R}^{3}$ ，使之有：

$\dot{R}(t)R(t)^{T} = \phi(t)^{\wedge}$

等式两边同乘 $R (t)$ ，由于 $R$ 为正交矩阵。

$\dot{R}(t) = \phi(t)^{\wedge}R(t)$

我们根据导数的定义，把 $R (t)$ 在 $t = 0$ 附近进行一阶泰勒展开：

$\approx R(t_{0}) + \dot{R}(t_{0})(t-t_{0}) \\ =I + \phi(t_{0})^{\wedge(t)}$

我们看到 $\phi$ 反映了 $R$ 的导数性质。同时在 $t_{0}$ 附近，设 $\phi$ 保持为常数 $\phi(t_{0})=\phi_{0}$ 。

上式是一个关于 $R$ 微分方程，而且有初始值 $R (0) = I$ ，解得：

$exp(\phi_{0}^{\wedge}t)$

旋转矩阵 $R$ 与另一个反对称矩阵 $\phi_{0}^{\wedge}t$ 通过指数关系发生关联：

$\phi$ 是对应到 $S O (3)$ 上的李代数 $s o (3)$
李群与李代数间的指数/对数映射。

3.3. 李代数

每个李群都对应一种李代数。每个李群都有李代数，李代数描述了李群的局部性质。

李代数 $s o (3)$

$\left \{\phi \in \mathbb{R}^{3}, \Phi = \phi^{\wedge} \in \mathbb{R}^{3} \right \}$

$s o (3)$ 是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。

$exp(\phi_{0}^{\wedge}t)$

李代数 $s e (3)$

$\left\{ \xi = \begin{bmatrix}\rho \\ \phi\end{bmatrix} \in \mathbb{R}^{6},\rho \in \mathbb{R}^{3},\phi \in so(3), \xi^{\wedge} = \begin{bmatrix} \phi^{\wedge}& \rho \\ 0^{T} & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \right\}$