基于局部块特征优化的室内高精度激光SLAM方法

蔡 睿,章国宝*,朱宏伟

(1.东南大学 自动化学院,江苏 南京 211189; 2.南京深地智能建筑技术研究院,江苏 南京 210019)

近年来,随着传感器硬件技术的发展完善,多线三维激光雷达、惯性测量单元(Inertial Measurement Unit,IMU)和全球导航卫星系统等传感器在精度方面已经达到了较高的水准[1]。然而,实际测绘工程所需的并非简单对单个传感器采集信息的直接输出,往往是对多个不同传感器在实时运动状态下采集的多种信息的融合处理[2-3]。在这一背景下,激光SLAM(Simultaneous Localization and Mapping,即时定位与地图构建)算法即是以解决上述问题为目的,利用一种或多种传感器[4]的数据,同时进行测绘平台的定位和全局点云地图生成的技术。当前多数解决方案仅采用三维激光雷达数据作为传感器,或同时融合IMU等数据作为辅助[5-6],解决GNSS(Global Navigation Satellite System,全球导航卫星系统)在部分极端环境下无信号或信号弱、无法精准采集轨迹信息的难题。

目前,激光SLAM仍然是以三维激光雷达数据为主要信息来源,部分以IMU数据作为辅助提升平台位姿计算精度。与视觉相机相比,三维激光雷达可以避免受到环境亮度的影响,而IMU辅助位姿计算可以减弱激光点云数据在运动中产生的累积误差带来的影响。二十世纪以来,SLAM技术逐渐实际应用于室内扫地机器人、自主驾驶系统、测绘建图机器人等多个领域的产品中[7],且国内外涌现出一批优秀的解决方案,其中包含了Gmapping[8-9]、Cartographer[10]、LOAM(LiDAR Odometry and Mapping)[11]以及LeGO-LOAM(Lightweight and Ground-Optimized LOAM)[12]等。然而对于部分特殊环境,现有解决方案建图效果失真的情况时有发生。

针对几何特征匮乏的室内环境提出融合IMU数据的激光SLAM方法,分割地面点与非地面点后在点云位置信息和强度信息的深度图中划分局部块,在局部块内进行离群点筛除、几何光滑度梯度和强度梯度计算,作为此局部块的特征向量。IMU积分值作为求解位姿的初值,以两帧特征之间的差值作为目标函数迭代求解。方法融合了三维激光点云的几何信息、强度信息和IMU数据,提升了对室内几何特征匮乏环境的定位与建图系统的精度。

1 系统概述

本文设计系统接收三维激光点云原始数据和IMU数据,通过设计算法实现对室内环境的高精度定位与建图功能。系统主要组成部分包含如下模块。

① 数据预处理和分割。根据原始点云的坐标数据将其分割为地面点与非地面点两类,并对点云的坐标数据和强度数据分别投影至柱面形成深度图以便特征计算。

② 去除离群值和特征提取。对非地面范围内的坐标深度图和强度深度图分别划分局部块,对局部块内离群值进行剔除,并针对每个局部块分别计算子区域的特征值梯度值作为该局部块的特征向量。而对于地面点区域的坐标信息采用直接计算法向量的方式处理。

③ IMU数据积分。IMU的数据的采集频率是高于点云数据一个数量级的,对每两帧点云之间的IMU数据进行积分处理。这一模块和以上两个模块是并行的。

④ 特征匹配和帧间位姿求解。采用IMU数据积分模块的结果作为初值,以帧间特征差构建目标函数,利用L-M(Levenberg-Marquardt)方法迭代求解。

⑤ 地图构建。利用当前帧提取的地面法向量和全局地图地面法向量配准修正,最终更新地图。

系统框架如图1所示。

2 算法设计

2.1 数据预处理

2.1.1 地面点集分割

由于实际环境中地面环境较为单一,一般呈现类固定平面的状态,包含信息较少且相对变化较少。因此对初始点云先采用文献[13]中拟合地面平面并进行标记的方法分割为地面点集与非地面点集,非地面点用于提取特征,地面点用于提取平面法向量,地面点集分割效果如图2所示。

图2 地面点集分割效果

2.1.2 点云坐标与强度深度图生成

对于非地面点Vk=Pk-Gk,由于原始点云数据量较大,ICP(Iterative Closest Point,迭代最近点)等现有点云匹配方法对每个点都进行匹配处理导致运算量大,对运行设备造成较重的负担,因此本方法采取了投影到柱面得到深度图后再将其划分为若干局部块,对每个局部块单独计算特征用于匹配的方法。

(1)

(2)

图3 深度图生成示意图

2.1.3 特征提取

受参考文献[14]启发,在非地面点深度图上划分固定尺寸局部块并提取局部块的主要特征,而在地面点集合上采用提取法向量的方法,能够对特征信息丰富的非地面点和特征信息较少的非地面点分别进行处理。特征提取的具体步骤如下。

① 对非地面点深度图进行局部块的划分,每个局部块块宽为m,块高为n,通过对每个局部块计算特征来描述该局部块的主要特征,深度图划分局部块示意图如图4所示(其中被划分为地面点的像素点被置为无效值)。

图4 局部块划分示意图

② 对每个局部块的所有深度像素判断离群像素点(即该局部块中与其余像素点的深度值相差过大)并剔除,不进入后续的计算过程。

③ 对每个局部块选取中心点,以高为m/4,宽为n/4为界在该中心点周围建立4个子区域,每个子区域为一个同心块,如图5所示。

图5 局部块子区域示意图

④ 对中心点分别将4个子区域作为领域从内至外依次计算该点光滑度c1、c2、c3、c4。

(3)

⑤ 对每个子区域内的中心点以外的点,以其周边高为m/4、宽为n/4的邻域内的点计算c值,并对每个子区域q计算以其作为邻域的中心点的二维梯度(其中x和y并非原始点云数据的三维坐标的x值和y值,而是经过处理到深度图后点在图上的像素坐标):

(4)

经过式(4)计算后得到4个二维梯度值,作为该局部块B(i,j)的特征。

2.1.4 IMU数据积分

(5)

(6)

(7)

(8)

(9)

采用参考文献[15]中提出的帧间积分方法,得到相邻两点云帧的时刻k和k+1之间的相对运动增量为

(10)

(11)

(12)

2.2 雷达里程计

2.2.1 相邻帧点云配准

(13)

(14)

得到两个深度图每个局部块B(i,j)的特征向量之间的差值函数,由两个差值函数构建目标函数为

(15)

2.2.2 当前帧位姿估算

(16)

2.3 地图构建

2.3.1 地图配准与修正

在点云配准时,地面点可以提供俯仰角、横滚角和z轴方向的约束,而非地面点可以提供偏航角、x轴和y轴方向的约束[13]。非地面点的约束实现已经在雷达里程计部分完成了,而在每帧点云数据中均会进行地面点的筛选并提取地面点的法向量,地图配准与修正模块对当前帧的法向量与已构建地图的地面法向量进行匹配并优化当前帧位姿。

2.3.2 更新地图

(17)

3 实验结果与分析

3.1 实验设备与数据

为测试本文方法的实际运行效果,采用公开数据集和设备实测数据集共同验证的方式,并与主流算法LOAM、LeGO-LOAM的效果进行比对。

公开数据集选用了具有完备传感器数据和真值的KITTI数据集,便于比对3种方法的结果与真值之间的绝对位姿误差(Absolute Pose Error,APE)。使用到的传感器数据包括Velodyne HDL-64E LiDAR和OXTS RT 3003 IMU,采集场景包括住宅区、公路等,特征较为丰富。

实测数据集采用设备为RS-LiDAR-16混合固态激光16线雷达和HWT901B-485高精度10轴姿态角度传感器,如图6所示。由于实验设备限制无法提供ground-truth基准值进行数值分析,因此仅用作特殊场景的全局地图生成效果比对。采集场景包括几何特征信息单一且稀少的走廊环境和适用于测试提取点云强度特征影响的地下停车场,如图7所示。

图6 实测数据所用设备

图7 实测数据场景

3.2 几何特征匹配效果

由于KITTI数据集中不具备点云强度域信息,仅包含点云位置信息,但具备ground-truth基准值参考,可以有效进行不同算法之间的具体数值比对,因此选择其中的00、05、07、09序列数据,对本文提出的点云坐标强度图局部块特征向量提取与匹配方法进行可行性验证,并与主流算法的运行效果进行比对。

在运行数据集并分析效果的过程中,发现LOAM由于几何特征约束的单一性和缺少闭环环节容易逐渐累积误差,而LeGO-LOAM由于其闭环环节的存在,检测到闭环时对位姿的调整会导致位姿数据的突变,这一点在高度项上体现得尤其明显,如图8(a)中轨迹所示。

而本文方法在加强地面约束的基础上采用局部块的方法划定子区域并计算其光滑度梯度值作为表述该子区域的特征向量的方法,增强了对小范围区域的描述性,与原始LOAM中划分角特征点与面特征点并进行帧间匹配的方法相比,考虑了对特征点周边环境的描述,在KITTI数据集上的实践效果较好,减小了累积误差的影响,且不存在位姿数据突变的问题。图8(a)～图8(d)分别为LOAM、LeGO-LOAM和本文方法在KITTI00、05、07、09数据集上的运行轨迹与真值的比对,其中x轴、z轴分别为水平方向的横纵轴。

图9(a)～图9(d)分别为LOAM、LeGO-LOAM和本文方法在KITTI00、05、07、09数据集上的APE值比对图,表1为公共数据集轨迹APE结果。由此可以看出,本文方法在精度上获得了提升,且避免了位姿的突变。

表1 公共数据集轨迹APE结果 单位:m

图9 公共数据集轨迹误差比对

3.3 强度特征匹配结果

实地采集的数据中包含了点云位置信息、强度信息和IMU数据,因此用于测试附加强度特征匹配模块的效果。在使用LeGO-LOAM测试实地数据集的过程中发现,对于特征单一导致匹配较为困难的走廊环境,LeGO-LOAM会出现“跑飞”现象,即生成地图结果与实际相差较大。通过对比走廊环境与所用KITTI数据集的环境,得出出现这一情况的原因是走廊环境的地面较狭窄且壁面环境相似度高,导致了划分地面点和提取特征的困难。而附加了强度特征匹配模块的本文方法则会在这一环境下结合强度特征和IMU数据进行修正。对两种方法的效果进行了比对,如图10所示。

由图10可以看出,本文方法通过强度特征匹配和融合IMU数据有效改善了“跑飞”现象,增强了算法的稳定性。

而对于地下停车场场景,本文方法增加的强度特征约束项提高了帧间匹配的准确性,建图中墙壁、拐角等的“重影”现象有所减少,如图11所示。

图11 实测地下停车场环境效果比对

图11(a)和图11(b)分别为两种算法对同一场景的整体效果对比(图中红框标注区分别为图11(c)和图11(d)所在位置),图11(c)和图11(d)中红色椭圆标注区域分别为LOAM运行出现“重影”现象和本文方法“重影”改善效果。

3.4 结果分析

所设计的实验采用了公共数据集和实测数据集对本文方法与已有的LOAM和LeGO-LOAM算法的运行结果进行了比对。

通过对KITTI公共数据集00、05、07、09序列的建图并比对真值和计算误差,可见几何特征提取和匹配模块的改进提升了特征对环境的表述能力,进一步提升了匹配的精度,此外也避免了位姿突变的发生。而附加了强度特征匹配模块的本文方法在实测数据集上的结果也较已有算法获得了较大的提升,改善了两种已有算法运行结果中出现的“跑飞”和“重影”现象。

4 结束语

针对几何特征稀少、帧间匹配难度较大的环境,提出了一种基于深度图划分局部块并筛除离群点、提取块内点云光滑度特征信息和强度特征信息,同时融合IMU数据求解帧间位姿的定位建图方法。经过KITTI公共数据集的验证,几何特征提取与匹配模块的改进有效地提升了位姿估算的精度,降低了匹配误差的累积,提升了定位与建图的准确性。经实测数据集实验,强度特征与几何特征的提取和结合可以有效地改善已有算法中会出现的“跑飞”和“重影”现象,即对于几何信息较为匮乏的环境,融合点云几何信息与强度信息的方法比使用单一几何信息的匹配方法效果更好。