(1)传感器信息读取
主要负责读取、预处理IMU、里程计(编码器)、激光雷达等传感器的信息。由于每个传感器的生成频率不同,因此所有传感器数据还要进行时间同步。
(2)前端里程计
主要负责实现机器人位姿估计。由于IMU、里程计(编码器)、激光雷达等传单一传感器的数据误差比较大,采用多传感器融合的方案可以最大程度减少机器人位姿估计的误差。
前端里程计采用多传感器融合的方式估计机器人的位姿信息,同时绘制局部栅格地图。
采用多传感器融合的前端里程计在机器人位姿估计时,只是在局部地图中进行了位姿计算,每次的估计都会存在一定误差,累计漂移依旧无法避免。长时间运行后,累计误差会越来越大,位姿估计也会不准,因此需要后端优化和回环检测来解决漂移问题。
(3)回环检测
主要负责判断机器人是否到达先前经过的位置。一旦检测到回环,将信息提供给后端,后端羽化算法根据一系列的数据,消除轨迹的累计误差,最终得到全局一致的轨迹和地图。
但是需要注意:基于激光雷达点云的回环检测可能会残生误检,比如两个距离间隔相同的走廊,激光点云完全相同。
(4)后端优化
主要负责后端接收不同时刻前端里程计的机器人位姿,以及回环检测的信息,进行全局一致的轨迹优化和地图纠正。
(5)建图
主要负责根据估计的轨迹和激光雷达数据进行栅格地图的绘制。
常见的地图包括尺度地图、拓扑地图、语义地图。其中,尺度地图具有真实物理尺寸,包括栅格地图、特征地图、点云地图;拓扑地图不具备真实的物理尺寸,只表示连通关系和距离;语义地图是加标签的尺度地图,可用于SLAM与深度学习的结合。
占据栅格地图(Occupancy Grid)是将地图栅格化,每个栅格的值可以分为可行区域、不可达区域、未知区域。其中,激光束穿过的区域为可行区域;激光点云所在的栅格标记为为不可达区域(障碍物区域),未知区域为还未感知的区域。
(1)一次扫描
二维激光雷达按照一定频率扫描一圈数据输出激光测距信息完成一次扫描。
(2)扫描匹配
为计算每一次扫描对应的机器人位姿,需要进行扫描匹配。扫描匹配方法:
- scan to scan: 将当前scan数据与上一次的scan数据进行匹配
- scan to map: 将当前scan数据与地图进行匹配。
常见二维扫描匹配算法:点到点(ICP)、点到线(PL-ICP、ICL)。
(3)子地图模式
扫描的本质是点云,最简单的方案是将所有的scan按时间顺序放到栅格地图(全图)中,但会有累计误差和运动的影响。
子地图模式是将邻近的扫描归入一个局部的子地图,再将子地图凭拼接,每个子地图内部固定,不需要重复计算;同时子地图有独立坐标,位姿可以单独优化调整,所以子回环检测时,可以将子地图作为基本单元,在处理回环检测、地图更新方面更加方便。
- scan-to-submap:扫描数据与子地图进行匹配
- map-to-map:地图与地图进行匹配