融合激光和IMU的Cartographer算法实践应用

REF：扫地机器人增强位姿融合的 Cartographer 算法及系统实现

1. Cartographer算法

作为实时的室内建图算法，能生成分辨率 r=5cm 的栅格地图，前端将最新的激光雷达扫描数据在相邻的子图上完成扫描匹配，得到一个在短时间内准确的最佳插入位置后，将扫描插入到子图中，扫描匹配中,位姿估计的误差会在整个地图中随时间逐渐累积，通过回环检测加约束进行优化消除误差；算法分为局部 SLAM和全局 SLAM两部分，两部分均会对激光雷达观测的位姿进行优化

45e0ec40882b9b053852e8f2083820a0.png

• 局部 SLAM： 使用非线性优化将每个连续的扫描与子图进行匹配（扫描匹配） 目标是使用除了激光雷达之外的传感器数据，预测下一个扫描应该被插入到子图的位置，一次激光雷达采集的数据形成一帧扫描，重复迭代对齐多次扫描和子图坐标帧来构建子图，多个扫描构成的子图由概率网格表示
• 融合基于位姿增量的多传感器位姿 基于时间戳对齐数据,可以在 IMU 以及里程计等辅助传感器的帮助下提供更精确的下一时刻包含旋转和平移的位姿估计.在位姿融合中,因为 IMU 和里程计的采集速率比激光雷达传感器高很多,因而在一定时间段内可能没有激光扫描匹配估计的位姿数据,这时需要通过 IMU和里程计数据进行推断.

044b8b87d1a31852b863b1f1b3468d9a.png

• 全局 SLAM： 消除局部 SLAM 累积的误差，将不同的子图以最匹配的位姿融合 采用基于 DFS 的分支定界法进行回环的检测，将可能性子集表示为一棵树中的节点，根节点表示所有可能的解，每个节点的子节点是其父节点的分区，一起表示同一组的可能性，叶节点表示一个可行解

2. 应用分析

基于增强Cartographer算法的SLAM系统，整个 SLAM 系统分为 6 个模块：

• A：系统配置模块：完成初始化
• B：传感器数据交互模块：接收平台的传感器数据，完成传感器数据及局部优化位姿的发送
• C：接口通信模块：对外提供接口与其他系统交互
• D：数据管理及分发模块：负责与算法进行数据交互
• E：数据处理算法模块：完成数据处理矫正
• F：Cartographer算法模块

在数据流向中，根据A加载的参数调用D中的轨迹构建函数，完成位姿融合函数的初始化，B接收来自各传感器C的数据传递给D，D调用E对各种传感器数据处理后传递给F，B通过D获取F计算出的局部优化位姿，再经过D传到F中进行位姿融合

d83285286dc0b5b909e6361a59019807.png

算法的建图质量评估公式，其中：

• N：相对关系的数量
• trans(⋅)和 rot(⋅)：分别用于分离和加权平移和旋转分量
• δ和δ *： 分别表示估计值和真实值
• δi,j=xiΘxj：表示从节点 xi 到节点 xj 的相对变换关系
• Θ：表示标准运动合成的逆运算符

18c69a847c872a1f68815be120f7c5d5.png