SLAM基础原理介绍

ref：Simultaneous Localization and Mapping: Part I/II

1. 起源

当移动机器人在未知环境中进行相对地标观测时，所有地标位置的估计值必然相互关联，要实现定位与地图构建问题的完整解决方案，就需要建立由车辆姿态和每个地标位置组成的联合状态量，并在每次完成地标观测后进行更新，这意味着估计器必须处理包含海量状态参数的向量（数量级与地图中维护的地标数量相当），其计算复杂度将随地标数量的平方呈指数级增长。

如果将地图构建与定位问题整合为单一估计问题时，其本身具有收敛性来自地标之间的关联性，这些关联性越强，解决方案反而越好。

2. SLAM结构

在SLAM中，平台的运动轨迹和所有地标的位置都可以在线估算，而不需要任何先验位置信息，相当于求一下概率分布，计算所有时间点k的概率分布，描述了在已知截至时间k及之前的观测数据、控制输入k−1以及车辆初始状态的前提下，地标位置与车辆状态的联合后验概率：

通过递归式方式分析SLAM问题，要求预先定义状态转移模型和观测模型，分别用于描述控制输入和观测值的影响机制，随着观测次数的增加，地标估计值之间的相关性会呈现单调递增特征：

3. 直接方法降低更新复杂度

降低计算复杂度的直接方法，是通过挖掘SLAM问题的内在结构，重新构建核心的更新运动模型和观测更新模型来减少运算需求。运动模型更新计算可通过状态增强 技术进行优化，而观测更新计算则可采用分区形式 的更新方程来控制计算量，这两种方法都能在降低运算成本的同时获得最优SLAM估计结果。

状态增强：状态增强本质上是一种稀疏操作，由于车辆运动模型仅影响位姿状态，协方差预测可以改写为具有地标数量线性复杂度的形式，将新地标点添加到状态向量中的过程具有类似的形式，新地图地标点的初始化是根据机器人姿态和观测值进行的，状态增强的基本思想可以应用于任何新状态是现有状态子集的函数的情况，状态通过新增的姿态x 进行扩展，并通过边缘化处理移除前一时态x。采用这种形式后，扩展卡尔曼滤波器的预测步骤以及新增地标点的过程，都可以简化为与地标数量呈线性关系的计算。

分区更新：分区更新主要分为两种基本类型。第一种方法在全局地图的局部区域进行操作，并维护全局引用坐标，这种方案在压缩扩展卡尔曼滤波器中有使用；第二种方法生成具有独立局部坐标系的短期子地图，该方法在局部地图序列化算法中使用；第二种方法的结构更简洁，需要实时更新的地标数量被限制在局部子地图坐标系描述的范围内，因此观测率更新与整体地图规模无关。为获取最优全局估计，系统定期将子地图与全局地图进行配准，并对两个地图共有的特征点执行约束更新，完成更新后，系统将生成新的子地图并继续迭代。

4. FastSLAM

FastSLAM的核心结构是一个RB化状态模型，模型通过加权采样表示运动轨迹，并通过解析方法计算状态分布图，即通过粒子滤波实现位姿状态的递归估计，通过EKF实现地图状态的递归估计，每个观测到的地标都被单独处理为从已知位姿进行EKF测量更新，而未观测到的地标保持不变（对于每个粒子，对观测到的地标执行EKF更新，作为具有已知车辆位姿的简单映射操作）。

上图展示FastSLAM算法中单个机器人轨迹的实现过程，椭球体分布区域表示每个更新阶段的提议分布，系统会从这些分布中采样出机器人姿态，当假设该姿态准确无误时，观测到的地标点数据将被更新，因此，单个粒子的地图精度完全取决于其轨迹的准确性，通过大量这样的轨迹数据，最终能够构建出机器人位置的概率模型。FastSLAM存在因无法遗忘历史轨迹而导致的退化问题，因此，当重采样过程耗尽这些历史信息时，系统就会出现统计精度下降。