文章目录
-
- [01 概述与预备知识](#01 概述与预备知识)
-
- [1.1 SLAM 是什么](#1.1 SLAM 是什么)
-
- [1.1.1 基本概念](#1.1.1 基本概念)
- [1.1.2 视觉 SLAM 框架](#1.1.2 视觉 SLAM 框架)
- [1.1.3 SLAM 问题的数学表述](#1.1.3 SLAM 问题的数学表述)
- [1.2 实践：编程基基础](#1.2 实践：编程基基础)
- [1.3 课后习题](#1.3 课后习题)
01 概述与预备知识
1.1 SLAM 是什么
1.1.1 基本概念
（1）SLAM 是 Simultaneous Localization and Mapping 的缩写，即同时定位与地图构建。它是指搭载特定传感器 的主体，在没有环境先验信息 的情况下，于运动过程中建立环境的模型。
（2）两大基本问题：
-
定位：我在什么地方？
-
建图：周围环境是什么样的？
（3）准确的定位需要精确的地图，精确的地图来自准确的定位。
（4）传感器
-
内质的（放置在机器人身上，用于感受机器人本体信息）：IMU（惯性测量单元）、激光、相机
-
外质的（安装于环境中，监测机器人的运动状态）：二维码、GPS导轨
（5）相机
以一定速率采集图像，形成视频。相较于激光传感器更便宜，信息更丰富。
相机以二维投影形式记录三维世界的信息，丢掉了距离信息。
分类：
-
单目相机：最常见的普通相机
-
双目相机（立体相机）：类似于人眼，通过视差计算出距离信息
-
深度相机（RGB-D）：发射端、接收端，获得RGB图和深度图
-
其他：全景相机、事件相机
1.1.2 视觉 SLAM 框架
-
前端：Visual Odometry（VO），视觉里程计的任务是估算邻近时刻的相机运动。主要方法：特征点法 、直接法
-
后端：Optimization，从带有噪声的数据中估计最优轨迹与地图。常用方法：最大后验估计、滤波器、图优化。
-
回环检测：Loop Closing，主要解决位置估计随时间漂移的问题。判断机器人是否到达过先前的位置，如果检测到回环，将信息提供给后端进行处理。
-
建图：Mapping，根据估计的轨迹，建立地图。主要分为度量地图 和拓扑地图两类。度量地图又分为稀疏地图和稠密地图，稀疏地图比较抽象，仅包含一部分有意义的信息------地标。进行导航时，就需要稠密地图（避免碰撞某些障碍物）。
1.1.3 SLAM 问题的数学表述
（1）数学表述
两个方程：运动方程和观测方程
{ x k = f ( x k − 1 , u k , w k ) z k , j = h ( y j , x k , v k , j ) (1-1) \left\{ \begin{matrix} \boldsymbol{x_k}=f(\boldsymbol{x_{k-1}},\boldsymbol{u_k}, \boldsymbol{w_k}) \\ \boldsymbol{z_{k,j}}=h(\boldsymbol{y_j},\boldsymbol{x_k},\boldsymbol{v_{k,j}}) \end{matrix} \right. \tag{1-1} {xk=f(xk−1,uk,wk)zk,j=h(yj,xk,vk,j)(1-1)
这里， x k \boldsymbol{x_k} xk 为 k k k 时刻的位置， u k \boldsymbol{u_k} uk 是运动传感器的读数或输入，比如相对上一位置前进1 m 或左转 10°； w k \boldsymbol{w_k} wk 为噪声。 z k , j \boldsymbol{z_{k,j}} zk,j 为机器人在 x k \boldsymbol{x_k} xk 位置时对路标 y j \boldsymbol{y_j} yj 的观测数据， v k , j \boldsymbol{v_{k,j}} vk,j 为此次观测的噪声。
对于简单的平面运动 ，机器人的位姿由横纵坐标和转角组成 x k = x 1 , x 2 , θ T \boldsymbol{x_k}=x_1, x_2, \\theta ^T xk=x1,x2,θT，变化量 u k = Δ x 1 , Δ x 2 , Δ θ T \boldsymbol{u_k}=\\Delta x_1, \\Delta x_2, \\Delta\\theta ^T uk=Δx1,Δx2,ΔθT，则运动方程可简化为
x 1 x 2 θ k = x 1 x 2 θ k − 1 + Δ x 1 Δ x 2 Δ θ k + w k (1-2) \left\\begin{matrix} { x }_{ 1 } \\\\ { x }_{ 2 } \\\\ \\theta \\end{matrix}\\rightk = \left\\begin{matrix} { x }_{ 1 } \\\\ { x }_{ 2 } \\\\ \\theta \\end{matrix}\\right{k-1} + \left\\begin{matrix} \\Delta{ x }_{ 1 } \\\\\\Delta{ x }_{ 2 } \\\\\\Delta\\theta \\end{matrix}\\right_{k} + \boldsymbol{w_k} \tag{1-2} x1x2θ k= x1x2θ k−1+ Δx1Δx2Δθ k+wk(1-2)
对于观测方程，以激光传感器为例，可以测量出此位置和路标之间的距离 r r r 和夹角 ϕ \phi ϕ，路标点位置记为 y j = y 1 , y 2 T j \boldsymbol{y_j}=y_1, y_2^{T_j } yj=y1,y2Tj，位姿为 x k = x 1 , x 2 T j \boldsymbol{x_k}=x_1, x_2^{T_j } xk=x1,x2Tj ，观测数据为 z k , j = r k , j , ϕ k , j T \boldsymbol{z_{k,j}}=r_{k,j}, \\phi_{k,j}^{T} zk,j=rk,j,ϕk,jT，在笛卡尔坐标系中，可以写为
r k , j ϕ k , j = ( y 1 , j − x 1 , k ) 2 + ( y 2 , j − x 2 , k ) 2 a r c t a n ( y 2 , j − x 2 , k ) ( y 1 , j − x 1 , k ) + v (1-3) \left\\begin{matrix} r_{k,j} \\\\\\phi_{k,j} \\end{matrix}\\right = \left\\begin{matrix} \\sqrt{{(y_{1,j}}-x_{1,k})\^2+{(y_{2,j}}-x_{2,k})\^2} \\\\ arctan{ (y_{2,j}-x_{2,k}) \\over (y_{1,j}-x_{1,k})} \\end{matrix}\\right + \boldsymbol{v} \tag{1-3} rk,jϕk,j=(y1,j−x1,k)2+(y2,j−x2,k)2 arctan(y1,j−x1,k)(y2,j−x2,k)+v(1-3)
当然，对于三维空间中的运动或其他形式传感器，上式有不同的参数化形式。
（2）SLAM 是一个状态估计问题
按运动和观测方程是否为线性，噪声是否符合高斯分布，分为线性/非线性 和高斯/非高斯系统。
-
线性高斯系统： 卡尔曼滤波
-
非线性非高斯系统： 扩展卡尔曼滤波、非线性优化
1.2 实践：编程基基础
- Ubuntu 系统
- C++
- 编辑器：Clion、VS Code
- 工具：cmake
- 库：Eigen、G2O、Ceres、OpenCV等