基于改进Cartographer的激光SLAM算法

黄禹翔,吴国新,左云波

(北京信息科技大学 机电系统测控北京市重点实验室,北京 100192)

0 引言

当机器人进入未知环境当中,通过自身携带的传感器如激光雷达构建地图,并估计自身位姿运动的技术,称为激光同步定位与地图构建 (simultaneous localization and mapping,SLAM)。激光SLAM一直是智能机器人领域研究的热点,目前该技术已应用于各种室内机器人中,包括扫地机器人、语音交互机器人等。激光SLAM发展至今,技术日渐成熟。黄永新[1]通过提取连续的点云数据,对里程计信息特征不明显的子图进行剔除,避免了闭环检测环节信息匹配错误。翁潇文等[2]采用数据配准的子地图,以观测数据为边构造全局地图,提高了构建具有回环检测地图的速度和精度。林国聪等[3]使用鲁棒代价函数,加入约束进行迭代优化计算最优位姿,避免了闭环检测假闭合的现象。基于多传感器融合的SLAM算法,任明宇等[4]通过贝叶斯方法将激光雷达和视觉信息融合得到更加精确的地图和定位。齐继超等[5]对特征点的深度信息进行提取,采取多策略的位姿估计方法,解决了激光和视觉信息融合在位姿计算中由于特征点不足导致的地图定位精度不高问题。章弘凯等[6]通过无迹卡尔曼滤波器将惯性测量单元(inertial measurement unit,IMU)的积分结果与点云配准信息融合,提出了一种多层次传感器数据融合的SLAM算法。对于激光SLAM后端优化方法,基于滤波只计算当前时刻的位姿,对于大场景的处理效果不是很好,基于图的优化更适用于大场景的建图,所以目前把基于图优化的SLAM算法作为主要研究的建图方案。

Cartographer算法是Google[7]在2016年提出的一种适用于大尺度建图的激光SLAM方法,利用基于网格映射和基于Ceres库的扫描匹配器,搭载各种传感器进行建图。前端融合了激光雷达和IMU传感器信息进行子图的更新从而得到局部地图,后端进行局部地图的优化,利用了分支定界法通过闭环检测消除误差得到全局地图。

Cartographer算法使用扫描匹配器对分辨率低的地图网格进行扫描匹配得到初始姿态估计,再对更高分辨率的地图进行扫描匹配提高初始位姿精度,通过已知的初始姿态估计可以在更高分辨率的地图上跳过扫描匹配器的各种位姿迭代[8]。针对激光雷达存在运动畸变的问题,单一算法对传感器存在着限制性,文献[9]将轮式里程计算法与传统的点云配准迭代最近点(iterative closest point,ICP)方法进行融合,设计了一种点云畸变处理的算法。文献[10]设计了一种混合的滤波算法,包括直通滤波和半径滤波,这两种滤波与体素滤波融合,提高了点云的质量,解决了异物遮挡和激光雷达测量误差所导致的点云离群问题。文献[11]在处理激光点云数据时,采用了体素栅格的滤波方法,通过栅格设置体素大小,在不考虑建图的实时性的情况下,提高了点云的密度,为建立深度地图提供了良好的点云环境。

本文在原有的Cartographer算法基础上进行改进,提出了一种激光SLAM算法。前端通过采用体素滤波降采样的方式去除一定的噪声和离群点,在保证不破坏点云初始特征的情况下,提高激光点云数据的配准速率;嵌入轮式里程计辅助算法去除运动畸变模块,对于激光帧上每个点云通过里程计估计位姿,通过坐标变换,映射到激光原点坐标系当中,转换为激光数据并发布,最后进行实验验证其有效性和可行性。

1 Cartographer算法

Cartographer主要分为前端的局部方法和后端的全局方法两部分,它们主要对机器人的位姿进行优化,该位姿由激光雷达扫描得到,分为平移量和旋转量。而IMU用来对重力方向进行估计,将激光雷达扫描的坐标投影到二维地图中。

前端首先进行数据提取,通过体素滤波和自适应体素滤波两种方法压缩激光雷达的数据,方便后续的数据处理,减少资源的占用。由于激光雷达每一帧扫描得到的点云数据较为庞大,存在着各种因素影响着点云处理后的效果。IMU和里程计对机器人的轨迹进行推算,对位姿进行估计,将位姿的估计值作为初值,得到的数据与雷达信息采用无迹卡尔曼滤波器进行融合并更新估计值,融合后的数据对激光雷达数据进行校正。帧间匹配采用相关性扫描匹配(correlation scan matching,CSM)方法,该方法是一个暴力搜索过程,构建一个似然场模型,需要良好的位姿初值,一般使用高频激光雷达采集数据,从而减少误差积累。激光雷达的每次扫描匹配形成子图,通过扫描得到的每一帧的点云与上一个子图进行比较,对子图进行像素概率值的更新。运动滤波器对传感器处于静止状态的帧数据进行剔除,不断更新子图,直到没有点云插入时,就得到完全优化的子图。该过程基于Ceres库的扫描匹配器计算点云位姿,从而将位姿求解问题转化成非线性的最小二乘问题[12]。后端插入子图,采用图优化的方法对子图进行优化,回环检测过程中采用分支定界法进行累积误差消除,将每一个子图与对应的位姿进行匹配,得到全局地图即完整的环境地图。Cartographer框架如图1所示。

图1 Cartographer框架

以激光雷达为中心,扫描距离为半径,扫描一周的数据作为一帧点云输出,通过不断对齐每一帧点云和子图坐标系的迭代过程进行子图的构建。假设激光扫描原点为O∈R2,其中R2是平面直角坐标系上所有的点组成的集合,激光雷达扫描得到的每一帧点云记为集合H,共由K个点组成,每个点被记作hk,则:

H={hk}k=1,2,…,K;hk∈R2

(1)

扫描到的点云中的一点在点云坐标系当中的位姿记为ξ=(ξx,ξy,ξθ),其中ξx和ξy为该点在点云坐标系相对于原点的平移量,ξθ为旋转角。从点云坐标系到对应子图坐标系的位姿变换用Tξ表示,则每一点变换到子图坐标系的位姿为:

(2)

等式右边的前半部分矩阵为旋转矩阵,后半部分的矩阵为平移矩阵,对应点云位姿的旋转和平移变换。

连续的扫描会形成子图,子图采用概率网格的方式进行映射。从给定的分辨率r将离散网格点映射成一个概率值,该值可认为是网格点被阻塞的概率,最接近某网格点的所有点构成该点的像素。

当概率网格中插入激光扫描得到的每一帧点云时,会计算得到一组击中点和丢失点,对于每一次击中,将距离最近的网格点插入进击中点的集合。对于每次丢失,将与每个像素相关联的网格点插入,这些点会与扫描中心和各个扫描点之间的一条射线相交,不包括击中点集合的网格点。未被扫描到的点会为其分配概率值。

在扫描帧插入前,Ceres扫描匹配器对扫描位姿相对于当前时刻的局部子图进行优化,扫描匹配器将寻找使子图中扫描点概率最大的位姿。该过程是一个非线性最小二乘问题:

(3)

式中:函数Msmooth为双三次方插值函数,对局部子图中扫描点的概率值进行平滑处理。函数值区间可能超出[0,1]区间外,但一般无影响。IMU通过测量角速度,用于扫描位姿的角分量和机器人旋转角的估计。

2 改进方法

2.1 改进体素滤波算法

Cartographer前端采用体素滤波器算法将通过激光雷达得到的点云数据构建一个三维体素网格,每个体素内用体素所有点的重心来近似表示该体素内所有的点,而该重心点不一定能代表原始点云中的点,可能会丢失点云的部分特征[13]。

针对此问题,本文采用体素内重心邻近点降采样的方法。首先将包围点云的最小长方盒均分成体积相同的多个立方体,计算每个立方体的重心,再寻找每个重心的邻近点来代表该立方体。该方法既实现了降采样减少了计算量,又保留了点云的原始特征。算法步骤如下:

首先将点云建立成k维树的形式,计算得到的点云总量设为N,设Dx、Dy、Dz分别为长方盒在各坐标轴上的长度。

每个均分的小立方体设置边长为d,体积为V,数量为n个。点云分割如图2所示。

图2 点云分割图解

各参数计算如下:

(4)

式中:s为比例系数;α为根据点云数量确定边长的比例因数;f()为向上取整函数;l、w和h为小立方体数量的计算参数。

将不属于该检测点云模型表面的异常值和噪声点去除,保留模型的微小特征点而不产生失真即为保持点云的初始特征。对点云数据集分别通过体素滤波法和改进方法进行处理,得到点云处理结果如图3所示。原始点云包含307 200个点,经过改进体素滤波处理后得到22 172个点,通过图像对比发现,改进算法在对点云数据降采样的同时,未破坏点云的整体结构,保持了点云的初始特征。

2.2 轮式里程计算法

机器人在使用Cartographer建图过程中,在每一帧激光扫描数据帧当中,每一个激光点云都在不同的位姿上生成。当机器人的位姿快速变换时,帧率低的激光雷达由于扫描数据不能瞬时得到,产生建图误差,在大尺度环境下,误差累积对建图精度影响较大[14]。

针对此问题,本文引入轮式里程计畸变去除模块。通过中央处理器(central processing unit,CPU)获取激光扫描数据,同时单片机发布里程计所测得的位姿数据,两者进行时间相关数据对齐,运用分段线性插值函数,找到激光帧上每一点云对应的里程计位姿,将激光点云在激光雷达坐标系变换到相应的里程计坐标系,再变换到基准坐标系,封装数据并发布,这样能大幅度地减少由机器人快速运动产生的位姿畸变,其流程如图4所示。

图4 轮式里程计辅助算法程序流程

假设当前激光帧的起始和结束时间分别为tb、te,每一激光点云数据的时间间隔为Δt。

如果里程计的数据恰好和激光雷达数据同步,则第一个和最后一个数据对应的时刻为tb、te,O[]为里程计计算位姿函数,则对应时刻位姿ξb、ξe为:

(5)

若在tb+Δt的时刻不能得到里程计位姿数据,则进行线性插值。假设里程计g、k时刻存在数据,且g

(6)

(7)

ξb、ξm、ξe可插值为一条二次曲线,用分段线性函数进行近似二次曲线并进行线性插值,可得到每个时间段机器人激光雷达坐标系对应的里程计位姿。

最后进行坐标转换,每个激光点存在对应位姿,设共存在i个激光点,且i=1,2,…,n。xi′为转换后的坐标,将其作为激光数据发布出去,r与a为激光点对应位姿在极坐标系下的半径与方位角,则:

(8)

2.3 回环检测环节优化

在改进激光SLAM算法的回环检测中,通过计算当前帧和历史关键帧之间的欧氏距离,判断该帧是否进行过回环,在满足距离约束的关键帧中选择一个满足时间约束和序列号约束的关键帧,作为待回环帧。

使用ICP算法构建待回环帧和当前帧位姿的误差函数。设当前帧一点的位姿为ξp,待回环帧对应点的位姿为ξq,ξp经过公式(2)变换后位姿为ξp′,误差函数为

(9)

若还未到达此回环,则准备下一次检测;若到达,则在优化段添加约束条件,降低误差。在相邻两帧点云之间根据原始地图数据计算相对位姿,在回环检测的过程中,回环检测相邻两帧点云扫描匹配若优于计算位姿,则将扫描匹配的变换矩阵作为边约束输入优化器进行优化求解。

3 仿真实验

在自主配置的服务器上进行仿真实验。PC测试端使用的系统是Ubuntu 20.04,图形处理器(graphics processing unit,GPU)为NVIDIA GeForce RTX3090,基于机器人操作系统(robot operating system,ROS)noetic平台进行数据发送。室内模拟地图由gazebo搭建,机器人运动的线速度设置为0.6 m/s,角速度设置为1.0 rad/s。使用模拟激光雷达采集点云数据。雷达参数设置如下:射线更新频率为5.5 s/次,雷达旋转一周发射360条射线,覆盖弧度为-3～3 rad,扫描半径为30 m,精度为0.01 m。建图时间为142.81 s。

图5(a)和(b)分别为Cartographer算法建立的地图和改进算法在相同时间下建立的地图,通过对比可以发现,未改进算法存在旋转漂移,改进算法地图墙体更为笔直,边缘轮廓更加明显。

图5 算法建图效果对比

两种算法建立地图所得到的机器人运动轨迹结果对比如图6所示,包括gazebo仿真环境二维地图的轨迹真值,Cartographer算法得到的轨迹和使用改进算法得到的轨迹。图中可以看出,原始算法得到的轨迹漂移误差累积较大,改进算法得到的轨迹漂移累积误差相对较小。

图6 算法轨迹结果对比

文献[15]提出了一种对SLAM算法运行得到的测试结果进行定量分析的方法,其中包含4种评价标准,能够较全面地评估算法。该方法通过将点云数据中两个轨迹节点的相对位姿与地面真值轨迹中两个轨迹节点的对应关系进行比较,计算平移分量和旋转分量的差值得到绝对误差,均方误差为差值的平方和与相对关系数量的比值的平方根。使用该方法对Cartographer算法与改进算法进行评价,结果如表1所示,“±”后的数字为误差范围。从表中可以看出,改进算法轨迹精度更高,绝对平移误差相较于Cartographer算法降低了17.5%,绝对旋转误差降低了31.1%。

表1 测试结果定量分析

对Cartographer和改进算法运行的轨迹结果定量分析如表2所示。绝对轨迹误差为机器人位姿的真实值与SLAM系统估计值之间的差值,通过将真实值和估计值的时间戳对齐之后进行计算得到。原始算法绝对轨迹误差的平均值为0.1 m,改进算法为0.09 m,定位精度更高。

表2 轨迹结果定量分析

4 结束语

本文提出了一种基于改进Cartographer的激光SLAM算法,针对点云数据密集计算量大的问题,采用重心邻近点降采样的体素滤波方法,在保持点云原始特征的同时,提高点云配准的效率。对于激光雷达由于机器人位姿变换产生的运动畸变,采用轮式里程计算法去除畸变,减小局部地图的漂移误差,提高建图精度。增加了边约束条件改善回环检测效果,减小建图中的累积误差。

通过Cartographer算法和改进算法构建地图的实验对比,改进算法在同一时间建图效率和精度更高。该算法可应用于室内大尺度环境建图。在此基础上,室内机器人如防疫机器人、扫地机器人等可以实现导航和避障的功能。Cartographer算法后端较为复杂,优化难度较大,这也是下一步需要解决的问题。