Cartographer 源码解析1.1 —— 算法整体结构

 *转载请注明出处 @梦凝小筑

1.1 Cartographer 整体结构

解析：

（1）数据获取（Input Sensor Data）

Input Sensor Data：传感器输入，主要包含——激光雷达数据、底盘odom数据、imu数据、fixed frame pose?

激光雷达数据：2d 扫描点云原生数据 ——> 体素滤波器（Voxel Filter） ——> 自适应体素滤波器 ——> 扫描匹配 (Scan Matching)

底盘odom数据：odom位置信息（包含 x , y , θ ）——> 姿态外推器 (PoseExtrapolator) ——> 扫描匹配

imu数据：imu数据（包含 两个方向的线加速度，角速度）——> 预处理 （ImuTracker）——> 姿态外推器 ——>扫描匹配

 

体素滤波器（Voxel Filter）：

    Voxel Grid 滤波器[1]对点云进行降采样, 即减少点云的数量规模, 同时保持点云的障碍物特征。

    基本思路是将点云数据划分在不同的体素栅格内，用体素栅格内所有点的重心来表示该栅格的环境数据。对于一个含有N个点的二维体素, 经过Voxel Grid滤波器处理后得到表示环境数据的点的计算公式为

姿态外推器 (PoseExtrapolator)：

    用于给扫描匹配 (Scan Matching)提供初值。

    输入：odom、imu、上一次前端匹配完成的 last_pose

    输出：下一次前端匹配的初始pose

    处理思路：

    odom和old_odom 计算 （x , y , θ ）增量 ——>线速度、角速度

    imu 预积分——>线速度、角速度

    last_pose 和 old_pose 计算（x , y , θ ）增量 ——>线速度、角速度

    估算出下一次前端匹配的初始pose，估算方法：

    对于角度姿态变化，更信任imu数据，如果有imu数据则使用imu数据

    对于位移姿态变化，更信任odom数据，如果有odom数据则使用odom数据

    若没有，则使用 last_pose计算出的数据

    在短时间内（sick liadar的频率是50hz,20ms）imu提供的角度增量，和odom提供的位移增量理论上是很准的，这和这些传感器的特性有关。

（2）局部建图（Local SLAM）

    若不清楚cartographer原理的，建议查看 Google Cartographer SLAM 原理

Local SLAM：外推器初始pose ——> 扫描匹配 ——>运动滤波器（Motion Filter）——>插入子图（submap）中

扫描匹配 (Scan Matching)：

    计算出，当前时刻相对于之前的一段时间，所应存在的最优位姿

    输入：外推器初始pose

    输出：扫描匹配最优位姿

    处理思路：非线性优化问题，建立最小二乘问题，该最小二乘问题在cartogrper中通过google自家的Ceres库进行求解

 

运动滤波器（Motion Filter）：

    为避免每个子图插入太多扫描帧（scan）数据，一旦扫描匹配器生成了新的pose，计算这次pose与last_pose 的变化量， 调用Motion Filter， 如果这次的变化不够重要（或者说变化很小），则扫描将被删除。当pose的变化大于某个距离，角度或时间阈值时，才会将scan插入到当前子图中。

 motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

子图（submap）：

    Cartographer在前端匹配环节区别与其它建图算法的主要是使用了Submap这一概念，每当或得一次 scan的数据后，便与当前最近建立的Submap去进行扫描匹配，使这一帧的 scan数据插入到Submap上最优的位置。在不断插入新数据帧的同时该Submap也得到了更新。一定量的数据组合成为一个Submap，当不再有新的scan插入到Submap时，就认为这个submap已经创建完成，接着会去创建下一个submap

    一个submap是通过几个连续的scan创建而成的，由5cm*5cm大小的概率栅格 构造而成

（3）全局优化（Gloab SLAM）

    通过scan matching得到的位姿估计在短时间内是可靠的，但是长时间会有累积误差。因此Cartographer应用了回环检测对累积误差进行优化(全局优化)。

Gloab SLAM：插入子图（submap）的扫描帧（scan）& 之前已创建完成的submap中所有（scan）——>回环检测（这里用优化算法分支定界（branch and bound））——>计算约束（Compute Constraints）——>位姿优化

回环检测：所有创建完成的submap以及当前的laser scan都会用作回环检测的matching。如果当前的scan和所有已创建完成的submap在距离上足够近，那么通过某种 match 策略就会找到该闭环。这里为了减少计算量，提高实时回环检测的效率，Cartographer应用了branch and bound(分支定界)优化方法进行优化搜索。

分支定界（branch and bound）： 一种优化搜索方法，效率优于暴力匹配法。详细介绍参见Google Cartographer SLAM 原理

位姿优化：如果得到一个足够好的匹配，到此处，回环检测部分已经结束了，已经检测到了回环得存在。接下来要根据当前scan的位姿和匹配到得最接近的submap中的某一个位姿来对所有的submap中的位姿进行优化，即使残差E最小。回环检测与回环优化过程中scan和submap的关系如图所示：

    回环的优化问题同样为非线性最小二乘问题，该最小二乘问题在cartogrper中通过google自家的Ceres库进行求解 。
 

总结：相对于其它slam算法，有以下不同

cartographer 引入了子图（submap）的概念，使前端匹配建立在当前帧与当前创建的submap(可理解为在这之前的一系列scan)进行匹配。

采用了回环检测，并利用 branch and bound 进行优化搜索，提高效率，达到实时回环的作用。

笔者理解并不一定正确，仅供参考，如有错误，欢迎指正。