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

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

部署在低配置的嵌入式设备上.

本文中实现的 SLAM 系统以驱动的方式合入到整个 Player 平台中,易于修改替换,通过订阅相应传感器接
收其数据进行处理.整个 SLAM 系统分为 6 个模块,如图 3 所示:系统配置模块中完成初始化,Player 传感器数据
交互模块接收平台的传感器数据,接口通信模块对外提供接口与其他系统交互,数据管理及分发模块负责与算
法进行数据交互,数据处理算法模块完成数据处理矫正.在数据流向中,Player 传感器数据交互模块接收来自各
传感器的数据传递给数据管理及分发模块,数据管理及分发模块调用数据处理算法模块对各种传感器数据处
理后传递给算法,Player 传感器数据交互模块通过数据管理及分发模块获取算法由激光雷达数据计算出的局部
优化位姿,再经过数据管理及分发模块传到算法中进行位姿融合.

系统配置

Player传感器数据 数据处理及分发 增强的Cartographer算法
交互

接口通信 数据处理算法

Fig.3 Cartographer-based SLAM system
图 3 基于 Cartographer 的 SLAM 系统
2.1.2 基于多传感器的系统配置
系统配置模块中,整个系统首先需要向 Player 平台注册,以独特的名字标识,同时说明本系统需要的传感器;
然后对传感器进行订阅,加载配置文件,完成算法初始化;最后注册传感器数据处理的回调函数.
系统使用 Lua [21] 进行配置文件的加载,其小巧而速度快的特点适合应用于嵌入式系统的配置文件.在系统
停止建图处理完所有传感器数据后,结束当前轨迹,完成对当前优化图中子图位姿、扫描位姿以及扫描在子图
坐标系中匹配位姿的最终优化.最后,将当前优化好的地图信息通过 Protobuf [22] 进行序列化保存到文件中,可在
PC 端对保存的地图信息进行可视化.
2.2 基于数据交互的通信模型
2.2.1 Player 传感器数据交互
Player 传感器数据交互模块主要完成传感器数据及局部优化位姿的发送.
在 Player 传感器数据交互模块中,首先根据系统配置模块加载的参数调用数据管理及分发模块中的轨迹
构建函数,并完成位姿融合函数的初始化.在建图过程中,会不断接收到来自 Player 平台的各种传感器数据,
Player 传感器数据交互模块检查其采样频率是否高于配置中的设置:高于则丢弃;否则将其发送到数据管理及
分发模块.同时,按照激光数据的采样频率从数据管理及分发模块中获取局部优化后的位姿,将其再转发到数据
管理及分发模块,作为位姿融合的一个数据源.Player 传感器数据交互模块作为一个中间件模块,转发来自接口
通信模块或数据管理及分发模块的数据.本模块也是在系统启动后唯一与 Player 平台交互的模块,作为整个
SLAM 系统的数据发布源.
在建图终止后,通过数据管理及分发模块结束轨迹完成最终全局优化以及序列化地图信息保存到文件中.
2.2.2 数据管理及分发
数据管理及分发模块与 Player 传感器数据交互模块相同,也是一个中间件模块.模块首先调用算法中的轨
迹添加函数进行配置,并初始化数据处理算法模块.在建图过程中,循环调用数据处理算法模块对来自 Player 传
感器数据交互模块的传感器数据进行处理,将处理后的数据传递给算法.同时,将从 Player 传感器数据交互模块
发送的局部优化位姿传递给算法,算法将数据融合后得到下一时刻估计的位姿.