Simultaneous Localization and Mapping (SLAM)问题介绍

SLAM中文翻译同时建图与定位，是一种广泛存在于生活中的问题：当我们探索一个未知空间的时候，就是一个建图与定位的过程，一边在空间中行走，一边观察空间中的标记物，并在脑海中逐渐构建一副地图，根据这幅地图，我们可以判定如何从这个空间中离开，判定当前所在的未知。SLAM技术目前主要应用于移动机器人技术，包括仓储机器人以及自动驾驶。

SLAM通常包含3种不断重复的操作：

移动：机器人移动到一个新位置，通常可以根据移动指令及运动模型推断得到新的位置，但是由于移动过程包含了不可消除的噪声，所以计算得到的位置并不准确，这里比较关心车辆的运动模型Motion model观测环境中的地标点：当移动到一个新位置后，机器人可以感知得到环境中新的地标点，这些地标点会被加入到地图中。我们需要根据机器人位姿和感知数据，确定地标点在地图上的位置。由于传感器感知也包含了各类噪声，所以同样无法准确计算地标点的位置。这里比较关心逆向观测模型（Inverse observation model）将观测到的地标点和地图上已有的地标点关联起来。在移动过程中，机器人可能会在多个位置观测到同一个地标，如果计算准确以及不存在噪声，那么机器人在不同位置观测相同的地标得到的位置应该是相同的，但是实际过程中，噪声以及计算误差会使得计算的位置有比较大的偏差。这时可以通过将数据关联起来，消除运动过程和观测过程的累计误差。这里比较关心观测模型（Direct observation model）

针对这3个操作，这里只探索平面上的机器人移动，对于三维空间上的移动这里暂不考虑。

SLAM中的各种关系：

三类模型介绍

Motion-model通常机器人的移动是根据发出的指令信号$\mathbb u$和扰动量决定： $$ \mathcal R \leftarrow \mathbb f(\mathcal R, \mathbb u, \mathcal c) $$ 通常机器人的状态可以用$\mathcal R_2 = (x, y, \theta)$表示，三维是用$\mathcal R_3=(x, y, z, roll, pitch, yaw)$表示。例如在低速情况下，可以用自行车模型作为运动模型。模型具体根据机器人的移动方式来选择。Observation-model机器人通过某个传感器S观测到某个地标点$\mathcal L_i$,则有测量值$\mathbb y_i$： $$ \mathbb y_i = h(\mathcal R, S, \mathcal L_i) $$Inverse-observation-model通过测量值和车辆的状态，我们可以得到地标$\mathcal L_i$: $$ \mathcal L_i = g(\mathcal R, S, \mathbb y_i) $$ 理想情况下，g是h的逆，但是很多时候h是不可逆的，例如单目视觉里。

EKF-SLAM

​ 这里假设已经了解EKF—SLAM算法了，如果不清楚，可以参看EKF

这里先给出EKF和SLAM以及机器人的动作对应关系表。

EventSLAMEKFMoveRobot Motionekf prediction观测到新的LandmarkLandmark 初始化添加状态观测到已有Landmark地图修正EKF-correction地图上已有landmark损毁Landmark 删除状态删除

接下来，我们分别讨论每一类事件，并讨论如何维护地图。

map

map中主要保存的是已有的地标点，而在ekf-slam中，通常将机器人的状态和地标的状态一起保存维护： $$ \mathbb x = \begin{bmatrix} \mathcal R \ \mathcal M \end{bmatrix} = \begin{bmatrix} \mathcal R \ \mathcal L_1 \ \vdots \ \mathcal L_n \end{bmatrix} $$ 在ekf中，需要用均值$\mathbb{\bar x} $和方差$P$表示状态$\mathbb x$, $$ \mathbb {\bar x} = \begin{bmatrix} \mathcal {\bar R} \ \mathcal {\bar M} \end{bmatrix} = \begin{bmatrix} \mathcal {\bar R} \ \mathcal {\bar L_1} \ \vdots \ \mathcal {\bar L_n} \end{bmatrix} $$

$$ P = \begin{bmatrix} P_{\mathcal R \mathcal R} \quad P_{\mathcal R \mathcal M} \ P_{\mathcal M \mathcal R} \quad P_{\mathcal M \mathcal M} \end{bmatrix} = \begin{bmatrix} P_{\mathcal R \mathcal R} \quad P_{\mathcal R \mathcal L_1} \quad \cdots \quad P_{\mathcal R \mathcal L_n} \ P_{\mathcal L_1 \mathcal R} \quad P_{\mathcal L_1 \mathcal L_1} \quad \cdots \quad P_{\mathcal L1 \mathcal L_n} \ \vdots \quad \quad \vdots \quad \quad \ddots \quad \vdots \ P_{\mathcal L_n \mathcal R} \quad P_{\mathcal L_n \mathcal L_1} \quad \cdots \quad P_{\mathcal L_n \mathcal L_n} \end{bmatrix} $$

所以关键就是如何维护均值跟方差。

地图初始化

初始时，地图上没有地标点，并且所有状态都是确定的，不存在不确定状态，$\mathbb {\bar x}和方差P$分别是： $$ \mathbb {\bar x} = \begin{bmatrix} 0 \ 0 \ 0\end{bmatrix} $$

$$ P = \begin{bmatrix} 0 \quad 0 \quad 0 \ 0 \quad 0 \quad 0 \ 0 \quad 0 \quad 0 \end{bmatrix} $$

Robot Motion

机器人的运动只会影响机器人的状态，对地标的状态没有影响，通常来说根据车辆移动模型我们知道： $$ \mathbb x = \mathbb f(\mathbb x, \mathbb u, \mathbb n) $$ 所以EKF的预测步骤有： $$ \mathbb {\bar x} = \mathbb f(\mathbb {\bar x}, \mathbb u, 0) \ P = F_{\mathbb x} P F_{\mathbb x}^T + F_{\mathbb n}NF_{\mathbb n}^T \ $$ 其中， $$ F_{\mathbb x} = \frac {\partial \mathbb f(\mathbb {\bar x}, \mathbb u)} {\partial \mathbb x} \ F_{\mathbb n} = \frac {\partial \mathbb f(\mathbb {\bar x}, \mathbb u)} {\partial \mathbb n} $$ $F_{\mathbb n} N F_{\mathbb n}^T$实际中，也可以直接写成过程噪声矩阵$Q$。

由于机器人的移动只影响了机器人的状态，也就是： $$ \mathcal R \leftarrow f_{\mathcal R} (\mathcal R, \mathbb u, \mathbb n) \ \mathcal M \leftarrow \mathcal M $$ 所以可以确定雅克比矩阵$F_{\mathbb x}$一定是稀疏的，可以表示为： $$ F_{\mathbb x} = \begin {bmatrix} \frac{\partial f_\mathcal R} {\partial \mathcal R} \quad 0 \ 0 \quad I \end {bmatrix} $$

$$ F_{\mathbb n} = \begin {bmatrix} \frac {\partial f_{\mathcal R}} {\partial \mathbb n} \ 0 \end {bmatrix} $$

​ Robot motion 时，地图数据更新。左侧是状态，右侧是方差。其中左侧深灰色是机器人状态，右侧深灰色表示的是机器人状态的协方差矩阵，浅灰色分别表示$\mathcal R \mathcal M$ 和$\mathcal M \mathcal R$的协方差矩阵。

为了减少无用计算，实际的预测过程则按照以下的几条公式求解： $$ \mathcal {\bar R} \leftarrow f_{\mathcal R} (\mathcal {\bar R}, \mathbb u, 0) $$

$$ P_{\mathcal R \mathcal R} \leftarrow \frac {\partial f_\mathcal R} {\partial \mathcal R} P_{\mathcal R \mathcal R} \frac {\partial f_\mathcal R} {\partial \mathcal R}^T + Q $$

$$ P_{\mathcal R \mathcal M} \leftarrow \frac {\partial f_\mathcal R} {\partial \mathcal R} P_{\mathcal R \mathcal M} $$

$$ P_{\mathcal M \mathcal R} \leftarrow P^T_{\mathcal R \mathcal M} $$

观测到已有地标

先讨论当观测到已有地标时如何更新地图状态。假设测量函数为： $$ \mathbb y = h(\mathbb x) + \mathbb v $$ 其中，$\mathbb y$是测量值，$\mathbb x$是当前的状态，$\mathbb v$是观测噪声，$h$是观测函数，EKF过程基本可以写成如下形式： $$ \mathbb {\bar z} = \mathbb y - h(\mathbb {\bar x}) $$

$$ Z = H_{\mathbb x} P H_{\mathbb x}^T + R $$

$$ K = P H_{\mathbb x}^T Z^{-1} $$

$$ \mathbb {\bar x} = \mathbb {\bar x} + K \mathbb {\bar z} $$

$$ P = P - K Z K^T $$

其中，$H_{\mathbb x} = \frac {\partial h(\mathbb {\bar x})} {\partial \mathbb x}$。

在Ekf-slam中，地标的状态，只跟Robot状态，传感器以及地标有关，所以观测模型可以写成如下形式： $$ \mathbb y_i = h_i(\mathcal R, \mathcal S, \mathcal L_i) + \mathbb v $$ 所以$H_{\mathbb x}$是一个稀疏的矩阵，其形式如下： $$ H_{\mathbb x} = \begin{bmatrix}H_{\mathcal R} \quad 0 \quad \cdots \quad 0 \quad H_{\mathcal L_i} \quad 0 \quad \cdots \quad 0 \end{bmatrix} $$ 其中， $$ H_{\mathcal R} = \frac {\partial h_i(\mathcal R, \mathcal S, \mathcal L_i)} {\partial \mathcal R} $$

$$ H_{\mathcal L_i} = \frac {\partial h_i({\mathcal R, \mathcal S, \mathcal L_i})} {\partial \mathcal L_i} $$

​ 观测到已有目标时更新地图的状态，左侧是地图状态，右侧是协方差矩阵。

完整的更新公式如下： $$ \mathbb {\bar z} = \mathbb y_i - h_i(\mathcal {\bar R}, \mathcal S, \mathcal {\bar L_i}) $$

$$ Z = \begin{bmatrix} H_\mathcal R \quad H_\mathcal {L_i} \end{bmatrix} \begin{bmatrix} P_{\mathcal R \mathcal R} \quad P_{\mathcal R \mathcal L_i} \ P_{\mathcal L_i \mathcal R} \quad P_{\mathcal L_i \mathcal L_i} \end{bmatrix} \begin{bmatrix} H^T_\mathcal R \ H^T_\mathcal {L_i} \end{bmatrix} + R $$

$$ K = \begin{bmatrix} P_{\mathcal R \mathcal R} \quad P_{\mathcal R \mathcal L_i} \ P_{\mathcal L_i \mathcal R} \quad P_{\mathcal L_i \mathcal L_i} \end{bmatrix} \begin{bmatrix} H^T_\mathcal R \ H^T_\mathcal {L_i} \end{bmatrix} Z^{-1} $$

$$ \mathbb {\bar x} \leftarrow \mathbb {\bar x} + K \mathbb {\bar z} $$

$$ P \leftarrow P - KZK^T $$

地标初始化

当观测到新地标时，需要将地标初始化并加入到地图中维护状态。这里需要根据观测模型是不是可逆分解成两种情况。

​ 新添加地标时，状态向量和协方差矩阵要更新的部分。

如果观测模型可逆，也就是$h$存在反函数$g$，这时地标位置： $$ \mathcal L_{n + 1} = g(\mathcal R, \mathcal S, \mathbb u_) $$ 下面分别计算$\mathcal {\bar L_{n+1}}和P_{\mathcal R \mathcal L_{n+1}}$, $$ \mathcal {\bar L_{n+1}} = g(\mathcal {\bar R}, \mathcal S, \mathbb y_{n+1}) $$

$$ G_{\mathcal R} = \frac {\partial g(\mathcal {\bar R}, \mathcal S, \mathbb y_{n+1})} {\partial \mathcal R} $$

$$ G_{\mathbb y_{n+1}} = \frac {\partial g(\mathcal {\bar R}, \mathcal S, \mathbb y_{n + 1})} {\partial \mathbb y_{n+1}} $$

$$ P_{\mathcal L \mathcal L} = G_\mathcal R P_{\mathcal R \mathcal R} G^T_\mathcal R + G_{\mathbb y_{\mathbb n + 1}} R G_{\mathbb y_{\mathbb n + 1}}^T $$

$$ P_{\mathcal L \mathbb x} = G_\mathcal R P_{\mathcal R \mathbb x} = G_\mathcal R \begin{bmatrix} P_{\mathcal R \mathcal R} \quad P_{\mathcal R \mathcal M} \end{bmatrix} $$

所以完整的更新如下： $$ \mathbb {\bar x} = \begin{bmatrix} \mathbb {\bar x} \ \mathcal {\bar L_{n+1}} \end{bmatrix} $$

$$ P = \begin{bmatrix} P \quad P^T_{\mathcal L \mathbb x} \ P_{\mathcal L \mathbb x} \quad P_{\mathcal L \mathcal L} \end{bmatrix} $$

如果观测模型不可逆，通常是依据某个先验信息求解，比如单目相机会依据摄像机的内外参数求解，一般反向映射函数可以写成如下形式： $$ \mathcal L_{n+1} = g(\mathcal R, \mathcal S, \mathbb y_{n+1}, \mathbb s) $$ 其中$\mathbb s$是先验信息。假设$\mathbb s$是均值，$S$是其方差，则有

$$ \mathcal {\bar L_{n+1}} = g(\mathcal {\bar R}, \mathcal S, \mathbb y_{n+1}, \mathbb s) $$

$$ G_{\mathcal R} = \frac {\partial g(\mathcal {\bar R}, \mathcal S, \mathbb y_{n+1}, \mathbb s)} {\partial \mathcal R} $$

$$ G_{\mathbb y_{n+1}} = \frac {\partial g(\mathcal {\bar R}, \mathcal S, \mathbb y_{n + 1}, \mathbb s)} {\partial \mathbb y_{n+1}} $$

$$ G_\mathbb s = \frac {\partial g(\mathcal {\bar R}, \mathcal S, \mathbb y_{n + 1}, \mathbb s)} {\partial \mathbb s} $$

$$ P_{\mathcal L \mathcal L} = G_\mathcal R P_{\mathcal R \mathcal R} G^T_\mathcal R + G_{\mathbb y_{\mathbb n + 1}} R G_{\mathbb y_{\mathbb n + 1}}^T + G_\mathbb s S G^T_\mathbb s $$

$$ P_{\mathcal L \mathbb x} = G_\mathcal R P_{\mathcal R \mathbb x} = G_\mathcal R \begin{bmatrix} P_{\mathcal R \mathcal R} \quad P_{\mathcal R \mathcal M} \end{bmatrix} $$

所以完整的更新如下： $$ \mathbb {\bar x} = \begin{bmatrix} \mathbb {\bar x} \ \mathcal {\bar L_{n+1}} \end{bmatrix} $$

$$ P = \begin{bmatrix} P \quad P^T_{\mathcal L \mathbb x} \ P_{\mathcal L \mathbb x} \quad P_{\mathcal L \mathcal L} \end{bmatrix} $$