概率机器人（Probability Robotics）笔记 Chapter 10: SLAM

1. 简介

SLAM问题的输入只有测量和控制，没有地图和位姿估计。

有两种同等重要的SLAM问题：

1. 在线SLAM（online SLAM）:
   同时估计地图和即时位姿这两个后验，即$p(x_t,m|z_{1:t},u_{1:t})$。
   称为在线SLAM的原因是，只估计时间$t$存留的变量。
   很多在线SLAM算法是递增的，即只处理当前一帧观测和控制。
2. 完整SLAM（full SLAM）:
   估计整个位姿轨迹与地图，即$p(x_{1:t},m|z_{1:t},u_{1:t})$。
   对之前的状态KaTeX parse error: Can't use function '\.' in math mode at position 5: x_1,\̲.̲.. x_{t-1}在状态空间上进行积分，既可以得到在线SLAM。

SLAM问题的另一个特点与估计问题的本质相关，即包含一个连续和一个离散问题：
连续问题与地图中物体的位置和机器人的位姿相关；
离散问题与物体的关联相关。
有时，显式估计地标的关联是有利的，在线后验为$p(x_t,m,c_t|z_{1:t},u_{1:t})$。

不管是哪种SLAM问题，估计全部后验分布都是最好的，但通常不可行：

1. 连续参数空间维度太高（地图）；
2. 离散关联变量的数量太大，很多算法的地图包括上万个特征。

本章介绍EKF算法，解决在线SLAM问题。

2. EKF SLAM

2.1 设定与假设

最早、最有影响力的SLAM算法就是基于EKF的。
EKF SLAM算法使用最大可能性数据关联，将EKF用于在线SLAM问题。
包含了多个近似和有限制的假设：

1. 特征地图：地标数量较少（如一千个），且EKF本来就希望特征的模糊度小。
   EKF SLAM通常需要在特征探测上下功夫。
2. 高斯噪声: 假设运动和测量有高斯噪声。
   后验的不确定性尽量小，否则线性化误差大。
3. 正测量: 只能处理观测到地标这种正信息，而不能处理没看到地标的负信息。

2.2 已知关联下的SLAM

即只解决SLAM问题的连续部分。
同时估计地标的位置和机器人的位姿，需要把地标的坐标加入状态向量中，即
$y_t=(xy\theta m_{1,x}{m}_{1,y}{s}_1...m_{N,x}{m}_{N,y}{S}_N)$
$N$是地图中地标总数，EKF估计在线后验$p(y_t|z_{1:t},u_{1:t})$。

算法 EKF_SLAM_known_correspondences(${\mu }_{t-1},{\Sigma }_{t-1},u_t,z_t,c_t$):

02························
$$F_x=\left(\begin{array}{cccccc}1& 0& 0& 0& \cdots & 0\\ 0& 1& 0& 0& \cdots & 0\\ 0& 0& 1& 0& \cdots & 0\end{array}\right)$$
矩阵右边是2N列个0。
是系数矩阵。

03························
$${\bar{\mu }}_t={\mu }_{t-1}+F_x^T\left(\begin{array}{c}-\frac{v_t}{{\omega }_t}\sin {\mu }_{t-1,\theta }+\frac{v_t}{w_t}\sin ({\mu }_{t-1,\theta }+{\omega }_t\Delta t)\\ \frac{v_t}{{\omega }_t}\cos {\mu }_{t-1,\theta }-\frac{v_t}{w_t}\cos ({\mu }_{t-1,\theta }+{\omega }_t\Delta t)\\ {\omega }_t\Delta t\end{array}\right)$$
用运动模型来更新均值

04························
$$G_t=I+F_x^T\left(\begin{array}{ccc}0& 0& -\frac{v_t}{{\omega }_t}\sin {\mu }_{t-1,\theta }+\frac{v_t}{w_t}\sin ({\mu }_{t-1,\theta }+{\omega }_t\Delta t)\\ 0& 0& \frac{v_t}{{\omega }_t}\cos {\mu }_{t-1,\theta }-\frac{v_t}{w_t}\cos ({\mu }_{t-1,\theta }+{\omega }_t\Delta t)\\ 0& 0& {\omega }_t\Delta t\end{array}\right)F_x$$
$G$是雅可比，用于泰勒展开，即
$g(u_t,x_{t-1})\approx g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1})$

05························
$${\bar{\Sigma }}_t=G_t{\Sigma }_{t-1}{G}_t^T+F_x^T{R}_t{F}_x$$
更新方差

06························
$$Q_t=\left(\begin{array}{ccc}{\sigma }_r& 0& 0\\ 0& {\sigma }_{\varphi }& 0\\ 0& 0& {\sigma }_s\end{array}\right)$$
观测的协方差矩阵

07····for all observed features $z_t^i=(r_t^i{\varphi }_t^i{s}_t^i{)}^T$ do
08········$j=c_t^i$
09········if landmark $j$ never seen before
10············compute feature coordinates $({\bar{\mu }}_{j,x}{\bar{\mu }}_{j,y}{\bar{\mu }}_{j,s}{)}^T$ based on measurement and ${\bar{\mu }}_t$
对初次观测的特征点的坐标进行初始化。
11········endif
12········$\delta =({\delta }_x{\delta }_y{)}^T=({\bar{\mu }}_{j,x}-{\bar{\mu }}_{t,x}{\bar{\mu }}_{j,y}-{\bar{\mu }}_{t,y})$
计算这个特征点与机器人预测位姿的相对坐标$\delta$
13········$q={\delta }^T\delta$
计算特征点与机器人的距离的平方（标量）
14········${\hat{z}}_t^i=(\sqrt{q}\text{atan2}({\delta }_y,{\delta }_x)-{\bar{\mu }}_{t,\theta }{\bar{\mu }}_{j,s}{)}^T$
15····························
$$F_{x,j}=\left(\begin{array}{cccccccc}1& 0& 0& 0\cdots 0& 0& 0& 0& 0\cdots 0\\ 0& 1& 0& 0\cdots 0& 0& 0& 0& 0\cdots 0\\ 0& 0& 1& 0\cdots 0& 0& 0& 0& 0\cdots 0\\ 0& 0& 0& 0\cdots 0& 1& 0& 0& 0\cdots 0\\ 0& 0& 0& 0\cdots 0& 0& 1& 0& 0\cdots 0\\ 0& 0& 0& \underset{2j-2}{0\cdots 0}& 0& 0& 1& \underset{2N-2j}{0\cdots 0}\end{array}\right)$$

16····························
$$H_t^i=\frac{1}{q}\left(\begin{array}{cccccc}\sqrt{q}{\delta }_x& -\sqrt{q}{\delta }_y& 0& -\sqrt{q}{\delta }_x& \sqrt{q}{\delta }_y& 0\\ {\delta }_y& delt{a}_x& -1& {\delta }_y& -{\delta }_x& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right)F_{x,j}$$
观测的雅可比矩阵
17········$K_t^i={\bar{\Sigma }}_t{H}_t^{iT}(H_t^i{\bar{\Sigma }}_t{H}_t^{iT}+Q_t{)}^{-1}$
卡尔曼增益矩阵的维度是是3x(3N+3)，并且不稀疏；
即任何一个测量给整个状态都传递了新的信息，而不是仅有当前观测到的特征。
这说明，观测到一个地标，能减少所有地标和机器人状态的不确定性。
18····endfor
19····${\mu }_t={\bar{\mu }}_t+{\sum }_i{K}_t^i(z_t^i-{\hat{z}}_t^i)$
20····${\Sigma }_t=(I-{\sum }_i{K}_t^i{H}_t^i){\bar{\Sigma }}_t$
21····return ${\mu }_t,{\Sigma }_t$

运动预测部分不对地图进行修改。

2.3 数学证明

先略过…

3. 关联未知下的EKF SLAM

未完待续…