Page 60 - 《软件学报》2020年第9期

张亮 等:扫地机器人增强位姿融合的 Cartographer 算法及系统实现 2681

K
argmin ∑ (1 M− smooth (T h k )) 2 (2)
ξ
ξ k = 1
其中,T ξ 表示扫描转换到对应子图帧中的位姿变换,位姿变换将扫描点 h k 从扫描转换到子图帧中,平滑函数
2
M smooth :R →R 将每个扫描点概率值平滑映射到局部子图中,目标是使插入扫描的所有扫描点概率值最大,构建
非线性最小二乘目标函数.
1.3 基于位姿增量的多传感器位姿融合
局部 SLAM 的目标是使用除了激光雷达之外的传感器数据来预测下一个扫描应该被插入到子图的位置.
在原本的位姿融合估计部分中,Cartographer 算法利用 IMU(inertial measurement unit)构建预测模型,扫描匹配器
与里程计构建观测模型,采取无损滤波器进行运动预测.当扫描匹配器估计的位姿或者里程计数据到来时,利用
观测值与系统状态变量均值的矢量差修正状态变量(只有 IMU 数据到来时,只进行预测不进行更新)及其协方
差.这种方法中存在的问题有:对各数据源的时间戳对准不够精确;IMU 位姿的融合过程只有在扫描匹配器估计
的位姿或里程计数据到来时才能做状态更新,这样会造成位姿估计较为滞后;没有通过线速度而是直接使用里
程计数据估计相邻时刻位姿增量,精度较低.
针对基于 UKF 方法 [14] 中位姿融合存在的问题,本文设计了一种基于位姿增量的多传感器位姿融合方法.
图 2 给出了该方法的具体描述.

基于当前优化位
扫描匹配优化的 从位姿中计算角 计算基于IMU
开始 姿时间戳删除位
位姿 速度与线速度 的tracker
姿、旧数据
将基于IMU的
tracker设为基于
里程计tracker以
及基于最新优化
位姿tracker的初
值

由估计的平移与 由基于最新优化 更新基于最 基于里程计计算
结束 旋转得到估计位 位姿的tracker估 新优化位姿 出的线速度估计
姿 计旋转 的tracker 平移

(a) 优化位姿的添加及位姿融合
更新基于里 从基于里程计的
从里程计数据计
开始 里程计数据 程计的 tracker计算角速 结束
算角速度
tracker 度

(b) 里程计数据的添加与计算
对跟踪器
的计算及
更新

否 有里程计数 否
有IMU数据？
据？
是
基于IMU数据计算 基于里程计角速 由位姿计算出的
位姿增量 度计算位姿增量 角速度计算位姿
增量

(c) 位姿增量跟踪器的计算与更新
Fig.2 Data addition and pose fusion
图 2 数据添加及位姿融合
该方法基于时间戳对齐数据,可以在 IMU 以及里程计等辅助传感器的帮助下提供更精确的下一时刻包含