Cartographer 源码解析1.1 —— 算法整体结构
*转载请注明出处 @梦凝小筑1.1 Cartographer 整体结构解析:(1)数据获取(Input Sensor Data)Input Sensor Data:传感器输入,主要包含——激光雷达数据、底盘odom数据、imu数据、fixed frame pose?激光雷达数据:2d 扫描点云原生数据 ——> 体素滤波器(Voxel Filter) ——> 自...
*转载请注明出处 @梦凝小筑
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 进行优化搜索,提高效率,达到实时回环的作用。
笔者理解并不一定正确,仅供参考,如有错误,欢迎指正。
更多推荐
所有评论(0)