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

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大小的概率栅格\large \left [ p_{min},p_{max} \right ] 构造而成

(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 进行优化搜索,提高效率,达到实时回环的作用。

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

  

 

 

Logo

权威|前沿|技术|干货|国内首个API全生命周期开发者社区

更多推荐