SLAM基础篇01 SLAM基础篇02

最近在看高翔和张涛写的《视觉》SLAM十四讲，记录学习内容，方便以后复习。这一系列文章更像是这本书的阅读笔记，所以结构背景与这本书相似；建议有时间的学生阅读原著，写得更详细、更容易理解。欢迎大家讨论学习视觉SLAM相关知识。

SLAM(simultaneous localization and mapping)顾名思义，定位和建图两件事可以通过即时定位和地图构建或并发建图和定位来解决。 SLAM 问题的本质:估计运动主体本身和周围环境空间的不确定性。主流的slam激光有两种技术应用。slam(基于激光雷达lidar 构图导航)和视觉slam(vslam,基于单/双目摄像头视觉建图导航)。

经典的视觉 SLAM 框架是研究人员在过去十年中总结的结果。框架本身及其包含的算法已基本确定，并已在许多视觉程序库和机器人程序库中提供。依靠这些算法，我们可以构建视觉 SLAM 在正常的工作环境中实时定位和构建系统。如上图所示，我们的八个视觉slam分为几步：

因为相机通常在某个时刻收集数据，我们只关心这些时刻的位置和地图。这将连续的运动变成了离散时刻 t = 1, . . . , K 其中发生的事情。在这些时刻，使用 x \bm{x} x 表示萝卜本身的位置。所以每一刻的位置都被记录为 x1, . . . , xK，它们构成了机器人的轨迹。在地图方面，我们设置了许多路标（Landmark）由此组成，每时每刻，传感器都会测量一部分路标点，得到它们的观测数据。不妨设置路标点 N 个，用 y1, . . . , yN表示它们。 机器人的运动方程可以用下表示： x k = f ( x k ? 1 , u k , w k ) \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}, \boldsymbol{w}_{k}\right) xk =f(xk−1​,uk​,wk​) 其中， u k u_k uk​是运动传感器的读数， w k w_k wk​为噪声。 机器人的观测方程可用下式表示： z k , j = h ( y j , x k , v k , j ) \boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}, \boldsymbol{v}_{k, j}\right) zk,j​=h(yj​,xk​,vk,j​) 这里， v k , j v_{k,j} vk,j​是这次观测的噪声。

这两个方程描述了最基本的 SLAM 问题：当我们知道运动测量的读数 u，以及传感器的读数 z 时，如何求解定位问题（估计 x）和建图问题（估计 y）？这时，我们把 SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

在机器人学中，我们习惯用一个坐标系来表达一个刚体的位姿，机器人的位置和姿态，其实就是机器人坐标系在世界坐标系的表达。我们使用齐次矩阵T来表达两个坐标系的变换关系，很显然其次矩阵T应该包括旋转和平移两个部分。 对于刚体的旋转，我们使用旋转矩阵 R \boldsymbol R R表示，她是一个行列式为1的正交矩阵。 我们可以吧旋转矩阵的的集合定义如下： S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } . S O(n)=\left\{R \in \mathbb{R}^{n \times n} \mid R R^{T}=I, \operatorname{det}(R)=1\right\} . SO(n)={ R∈Rn×n∣RRT=I,det(R)=1}.

S O ( n ) S O(n) SO(n) 是特殊正交群 (Special Orthogonal Group) 的意思。“群” 的内容等会儿还会再说。这个集合由 n n n 维空间的旋转矩阵组成, 特别的, S O ( 3 ) S O(3) SO(3) 就是三维空间的旋转 了。通过旋转矩阵, 我们可以直接谈论两个坐标系之间的旋转变换, 而不用再从基开始谈 起了。换句话说, 旋转矩阵可以描述刚体的旋转。

对于刚体的平移，我们可以用一个平移向量 t \boldsymbol t t来表示。那么把向量 a \boldsymbol a a移动到 a ′ \boldsymbol a^{\prime} a′,可以表达为： a ′ = R a + t a^{\prime}=R a+t a′=Ra+t

对于上面的这个式子，我们用矩阵的形式表达出来就是： [ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] ≜ T [ a 1 ] \left[\begin{array}{l}a^{\prime} \\ 1\end{array}\right]=\left[\begin{array}{ll}R & t \\ 0^{T} & 1\end{array}\right]\left[\begin{array}{l}a \\ 1\end{array}\right] \triangleq T\left[\begin{array}{l}a \\ 1\end{array}\right] [a′1​]=[R0T​t1​][a1​]≜T[a1​] 这里把我们向量的末尾加了一个“1”，变成了四维齐次坐标。这个T就是所谓的齐次变换矩阵，它有比较特殊的结构：左上为旋转矩阵，右侧为平移向量。左下为向量，右下为1。这种矩阵又称为特殊欧式群(Special Euclidean Group)

S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . S E(3)=\left\{T=\left[\begin{array}{cc} R & t \\ 0^{T} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in S O(3), t \in \mathbb{R}^{3}\right\} . SE(3)={ T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}. 与 S O ( 3 ) S O(3) SO(3) 一样, 求解该矩阵的逆表示一个反向的变换: T − 1 = [ R T − R T t 0 T 1 ] . T^{-1}=\left[\begin{array}{cc} R^{T} & -R^{T} t \\ 0^{T} & 1 \end{array}\right] . T−1=[RT0T​−RTt1​].

这里额外补充一点向量外积的知识：对于向量 a , b ∈ R 3 \boldsymbol a, \boldsymbol b \in \mathbb{R}^{3} a,b∈R3,外积可以表达为： a × b = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b ≜ a ∧ b . a \times b=\left[\begin{array}{ccc}i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3}\end{array}\right]=\left[\begin{array}{c}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{array}\right]=\left[\begin{array}{ccc}0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0\end{array}\right] b \triangleq a^{\wedge} b . a×b=⎣ ⎡​ia1​b1​​ja2​b2​​ka3​b3​​⎦ ⎤​=⎣ ⎡​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​⎦