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

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

Key words: sweeping robot; Cartographer; multi-sensor pose fusion; SLAM system

[1]
同步定位与地图构建(simultaneous localization and mapping,简称 SLAM)产生于机器人领域 ,它指的是:机
器人从未知环境的未知地点出发,在运动过程中,通过重复观测到的各传感器数据定位自身位置,再根据自身位
置构建周围环境的增量式地图,从而达到同时定位和地图构建的目的.服务机器人为实现高智能化,在复杂的非
结构场景完成任务,SLAM 技术是必要条件 [2,3] .
[4]
目前广泛应用于 SLAM 上的传感器分为摄像头和激光雷达两种,对应视觉 SLAM 和激光 SLAM.在实际
使用中,单个传感器难以适应复杂多变的环境要求,需要惯性测量单元以及里程计等传感器辅助,增强环境的适
应性.视觉 SLAM 的优点是具有语义信息、成本低,但易受光照影响,同时计算量较大.激光 SLAM 具有可靠性
高、精度高、地图可用于路径规划等优点,是目前最主流的定位导航方法,缺点是无语义信息且成本较高.
SLAM 本质是个系统状态估计问题,即:在给定系统输入的条件下,估计出机器人的位姿和地图点的坐标,延
[6]
[5]
伸出基于贝叶斯滤波 以及基于图优化 的两类 SLAM 算法.
基于贝叶斯滤波的方法是一种贝叶斯迭代状态估计理论,即先对机器人运动进行建模,构造出贴合物理场
景的运动方程和观测方程,进行状态预测和测量更新.滤波类算法最大的问题是无法处理大尺度场景的建图.由
于滤波类算法是基于递归计算的,下一时刻的估计值依赖于上一时刻的估计,因而在大尺度场景下,由于系统参
数和传感器观测数据的不准确性,会造成误差慢慢累积,一旦当前时刻的估计出现偏差,无法修正该误差,最终
将无法获得一致性的地图.基于图优化 SLAM 的出现,解决了大尺度场景建图的问题.图优化 SLAM 分为前端和
后端两个模块:前端先从里程计获得初始的位姿,然后通过激光雷达数据连续帧匹配,求得激光约束后的位姿;
[7]
后端采用回环检测,构造闭环约束,通过最小化观测和估计残差求得优化后的位姿 .在前端中会产生由估计所
引入的误差,随着运动范围的扩大,该误差会逐渐累积,导致最后产生错误的结果.为解决该问题,在后端使用闭
环检测进行全局优化,通过判断机器人是否回到历史中的某一点,添加约束较强的闭环约束,消除误差累积.
图优化的 SLAM 核心是构建位姿图,图中每个节点表示不同时刻机器人的位姿,边由各个节点间的变换关
系构成.其中:节点是优化变量;边是误差项,随着时间的累积,这个误差会越来越大.图构建完成后,需要调整机器
人位姿去满足边构成的约束,最终目的是求解机器人位姿使目标函数即误差平方函数最小,可表示为
F x ex )Ω ex ) (1)
(, ,x z
(, ,x z
( ) = ∑ ij
, ∈C i j , i j , i j i j , i j
其中,x i ,x j 表示机器人位姿,z i,j 表示测量值,Ω i,j 即权重矩阵.
本文的研究内容包括:
(1) 以应用日趋广泛的扫地机器人为研究对象,依托于低成本硬件嵌入式扫地机器人,以 Cartographer 算
[8]
法 为核心,在 Player 系统平台上,融合激光雷达、惯性导航仪以及里程计等多种传感器数据来感知
环境、构建地图,并在各种实际场景进行实验;
(2) 针对 Cartographer 中,位姿融合采用 UKF(unscented kalman filter)滤波方法带来计算量较大且提供下
一时刻位姿估计不够准确的问题,本文提出了一个基于位姿增量的多传感器位姿融合方法的增强
Cartographer 算法;
(3) 基于扫地机器人 Player 平台,实现了一个基于增强 Cartographer 算法的 SLAM 系统,系统分为 6 个模
块:系统配置模块完成初始配置,其中,接口通信模块完成与平台上其他系统的交互,Player 传感器数
据交互模块完成与 Player 平台的交互,数据管理及分发模块完成与算法交互,算法模块完成建图,数据
处理算法模块进行数据处理.

1 增强位姿融合的 Cartographer 算法
本节在 Cartographer 算法的基础上设计了基于增强型的位姿融合方法:首先给出增强型位姿融合的 SLAM
算法框架,该算法包含局部 SLAM(前端)和全局 SLAM(后端)两部分;然后,对局部 SLAM 的组成和基于位姿增量
的多传感器位姿融合进行分析设计;最后分析了全局优化过程中基于分支定界法加速回环检测的算法.