柴梦娜,刘元盛,任丽军

(1.北京联合大学 智慧城市学院,北京 100101；2.北京联合大学 机器人学院,北京 100101；3.北京联合大学 北京市信息服务工程重点实验室,北京 100101)

1 引 言

同时定位与建图(simultaneous localization and mapping,SLAM)问题可以描述为机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位,同时在自身定位的基础上建造增量式地图,实现机器人的自主定位和导航。1986年，在旧金山举行的ICRA会议上首次提出将基于概率的估算方法应用于局部地图构建[1]。1985年Cheeseman和Smith撰写的文章创立的原理可作为求解SLAM问题的数学基础[2]。1987年,Smith和Cheeseman等人提出基于卡尔曼滤波的SLAM算法理论框架,该理论被广泛应用[3]。随着技术的逐渐发展,衍生出许多基于概率的方法,主要包括:扩展卡尔曼滤波(Extended Kalman Filter,EKF)方法[4-7]、无迹卡尔曼滤波(Unscented Kalman Filter,UKF)方法[8-10]、粒子滤波器(Particle Filter,PF)方法[11-13]等基于滤波的算法。基于卡尔曼滤波的方法要求估计的问题必须基于高斯假设下,不能很好地应用于车辆的非线性系统。粒子滤波在重采样过程可能导致粒子耗尽,使得无法进行很好的位姿估计。

1992年,Besl P J等人提出迭代最近点(Iterative Closest Point,ICP)算法[14]。相对于基于滤波的方法。该算法忽略点云运动方程直接采用点云的特征进行配准。其通过寻找两个点云集的匹配点,并计算出两个点云的变换参数,来进行位姿估计与建图。2003年,Peter Biber等人首次提出二维正态分布变换(Normal Distributions Transform,NDT)数据配准[15]。该算法将扫描所占用的空间细分为单元格,根据单元内的点分布,为每个单元计算概率密度函数。通过评价各单元格映射点的概率分布的和得到变换参数的得分函数,并使用牛顿迭代算法最小化得分函数,从而得到最佳变换。NDT算法是一种整体因素决定的匹配。其在配准过程中,避免点与点对应的复杂问题,相比于ICP算法,该算法配准效率高,速度快。2007年,Martin Magnusson提出三维NDT算法,为三维点云的空间变换提供了理论基础[16]。

在SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧的位姿,因此会造成较大的累计误差。回环检测最初应用于视觉SLAM,在激光雷达的应用中较少。它能够使得机器人更加准确地去识别自己曾经去过的位置,通过进行辨识过去的位置,检测回环来解决位姿的漂移问题,使得地图的建立更加准确。传统的视觉SLAM 中回环检测方法为视觉词带模型。董蕊芳等人提出一种基于改进TF-IDF的视觉SLAM回环检测算法[17]。首先,采用图像中的直线作为特征来进行回环检测,接下来通过二进制线特征描述子来进行视觉词典的构建。徐建鹏等人提出一种基于Fast-RCNN神经网络的回环检测方法,利用图像语义特征等信息构建二维语义特征向量图,并利用非线性累计误差计算两帧图像间的相似度[18]。张剑华等提出点云片段匹配约束提升回环检测的方法[19]。T.Röhling等人提出一种对三维点云数据进行基于直方图的相似测量的回环检测方法[20]。

为解决大规模场景下进行同时定位与建图存在较大累积误差的问题,本文提出一种基于激光点云NDT特征的两步回环检测方法,并将所提方法应用于完整SLAM框架。首先对点云进行预处理,滤除噪点,之后采用重叠网格对点云进行三维NDT配准与回环检测。回环检测中,充分利用NDT配准中网格特性,采用两步法。第一步,利用各网格点云方差矩阵特征值对点云进行外观描述,并进行分类,通过各类数量为各类型分配权重,构造两帧间相似函数,进行粗回环检测。第二步,通过网格点云均值点距原点距离保证点云具有旋转不变性,进行精确回环检测。最后,在“小旋风”智能车平台进行验证。

2 改进NDT点云匹配SLAM方法

首先给出了完整SLAM系统流程。之后,介绍了传统NDT算法,并给出三维点云NDT配准流程。最后,提出对三维点云的NDT匹配的回环检测算法,并进行理论分析。

2.1 基于NDT特征的SLAM完整框架

本文采用完整SLAM框架,首先对点云进行预处理。然后,对相邻点云进行配准,同时利用网格正态分布信息进行两步回环检测。回环检测中,首先判断两帧点云是否符合粗回环检测配准条件,符合粗回环检测条件下,再进行精确闭环检测。采用非线性优化方法进行优化,完成最终建图。文中采用SLAM系统流程图如图1所示。

2.2 传统NDT算法

NDT算法是一种配准算法,应用于多维点的统计模型。将曲面用一组局部概率密度函数进行表示,每个函数描述曲面的一部分。当使用NDT进行扫描配准时,目标是找到当前扫描的位置,使当前扫描点位于参考扫描表面的可能性最大化。待优化的参数为当前扫描的姿态估计,包括旋转矩阵与平移矩阵。最好的姿态估计是使似然函数最大化,对应地最小化其负对数函数的概率。由于其在配准过程中不利用对应点的特征计算和匹配,所以时间比其他方法快,提高了算法的运行效率。对于不同分辨率的点云,配准效果也比较好。

图1 SLAM系统流程图

(1)

(2)

NDT算法流程图如图2所示。三维NDT算法配准步骤如下:

(3)

(4)

(5)

(3)对于要配准的点云,通过变换T将其转换到参考点云的网格中,如式(6)所示:

(6)

图2 NDT算法流程图

(4)根据正态分布参数计算每个转换点的概率密度,如式(7)所示:

(7)

(5)NDT配准得分通过对每个转换点计算出的概率密度相加得到,如式(8)所示:

(8)

(9)

(10)

(11)

(12)

2.3 基于激光点云NDT特征的回环检测

本文所提回环检测中,使用重叠的NDT单元以增加每个网格中激光点数,并最小化点云单元空间离散问题。如果每个体素的边长为b,则体素中心点之间的距离为b/2。图3为4个NDT单元排列图。

图3 NDT单元排列图

回环检测中分两步,第一步:利用NDT各网格的特征值属性,对网格外观进行分类,包括线性,平面和球形,通过各类数量分配权重,并构造两帧间相似函数,进行粗回环检测。在满足第一步的条件下执行第二步。第二步:利用各网格均值到坐标原点距离总和进行精确回环检测,以此保证点云的旋转不变性。

2.3.1 点云外观描述进行粗回环检测

图4 单元格类型图

·如果λ2/λ3≤te,分布是线性的；

·如果是非线性的并且λ1/λ2≤te,分布为平面的；

·如果是非线性、非平面的分布为球形(换句话说,如果没有特征值比另一个特征值大1/te倍)。

根据球形类、平面类和线性类,将每个网格分为三大子类,ns球面子类、np平面子类和nl线性子类。

(13)

每种类型在扫描的点云中占据的比例不同,因此不同的类型应该占据不同的权重。用wi表示i类型所占权重。用式(14)进行计算。将每帧图像由类型与其权重表示如式(15)。

(14)

F={(ws,S),(wp,P),(wl,L)}≜vF

(15)

对F和G两幅图像的相似度评分,采用其L1范数形式,值越大表明两幅图像越相似。设置阈值,至此完成了粗回环检测。

(16)

2.3.2 网格均值进行精确回环检测

激光雷达位置到特定表面的距离也是重要的信息。在符合粗回环条件后,根据网格类型,将各网格均值点到坐标原点间的距离进行存储。

(17)

与具有很少网格的扫描相比,具有许多网格的扫描倾向于具有更大的差异值。为了量化两次扫描F和G之间的差异,使用每次扫描中占用的NDT网格的总数,对F和G各类型距离进行归一化。SFi表示F扫描中i类型外观网格均值距原点的欧几里得距离之和。max(SF,SG)/min(SF,SG)作为归一化因子。设置误差阈值,确定是否两帧间重合度高,确定是否为回环。

(18)

3 实验结果与分析

本文提出的框架使用从Velodyne16线激光雷达收集的数据集进行验证。实验平台为“小旋风”无人驾驶智能车,如图5所示。图6为实验环境和无人驾驶汽车的行驶路线,全程约800 m。图6中A点为起点和终点,起点与终点形成闭合,此环境可以很好检验算法对于回环回路的性能。为验证算法的有效性,根据上述的地图构建方案,本文对真实的环境进行验证,并对采集数据结果进行了分析。为了保证本文实验的可对比性,在同一场景用传统NDT算法和文献[20]方法进行实验,并与本文算法进行对比分析。

图5 “小旋风”无人驾驶智能车

采集数据时,车辆行驶速度约为10 km/h,整个场景包含3507帧,路面较平坦。将对于激光雷达向前方向定为X轴,垂直激光雷达方向定为Y轴,垂直地面方向定义为Z轴。SLAM建图时,以起点为坐标原点,进行两帧之间的配准,将新的点云转换到局部坐标系下,实现增量式地图创建。

图6 实验场景

传统NDT建图结果，采用的网格为不重叠网格,网格边长设置为2米,最大补偿设置为0.1,最大迭代次数设置为30,最小转换差异为0.01。根据式(3)到式(12)的流程进行传统三维NDT配准。图7为传统三维NDT建图场景俯视图,图8为侧面图和回环位置对比图。

图7 传统NDT建图(俯视图)

图8 传统NDT建图(局部图)

本文算法建图结果，在根据传统方法进行三维NDT点云配准的同时,采用本文所提方法进行实时回环检测。在进行NDT配准时,采用的点云网格边长为2,最大步长设置为0.1,最大迭代次数设置为30,最小转换差异设置为0.01。在进行粗回环检测时,网格分类阈值te设置为e-5,L1范数设置为0.3。进行精确回环检测时,两帧距原点距离之和差异与上一帧点云距离比例阈值为0.02。图9为本文算法所建图的俯视图,图10中给出了回环处的细节图。

图9 改进算法建图(俯视图)

图10 改进算法建图(局部图)

将传统NDT建图轨迹和改进算法建图轨迹与GPS轨迹进行对比,如图11所示。图12将回环处误差放大,原点处为回环处。

图11 三种算法与GPS轨迹对比图

图12 回环处局部图

将两种方法与GPS轨迹进行逐帧对比。图13为各帧距离误差对比图,图14为各帧X方向距离误差对比图,图15为各帧Y方向距离误差对比图。图16为各帧配准用时对比图。

图13 距离误差对比图

图14 X方向距离误差对比图

从图7的俯视图可以看出在回环处X和Y方向存在明显误差,从图8中可以看出传统方法建图在Z方向同样存在极大误差。与传统方法相比,在保证其他参数设置不变的情况下,采用重叠网格,并加入本文的回环检测方法。图9中回环处已没有明显误差,图10中可以看出在Z处同样不存在明显误差。在3486帧处为回环处,三种方法与GPS轨迹误差对比如表1所示。

图15 Y方向距离误差对比图

图16 各帧配准用时对比图

表1 与GPS轨迹误差对比

从图13到图15可看出本文提出方法,相对于传统方法,已明显减小累积误差。与文献[20]方法相比,本文精度整体配准提高31 %。在采用本文算法进行回环检测时,进去精确回环检测次数为351帧,运用粗回环检测已排除90 %不存在回环检测帧,极大提高了算法的运行效率。如图16三种方法配准时长对比中可看出,相对于传统方法,加入回环检测后,相应的配准时间边长,但是相对于文献[20]采用基于直方图的方法,由于本文算法充分利用了NDT网格特征,并在回环检测时分两步走,极大地降低了配准时长。表2为三种算法配准用时对比。本文方法能够在保证效率的前提下达到更加精确的回环检测；与使用直方图方法进行回环检测相比,本文中的方法在运算效率上有了极大提高,在建图精度上也提高了30 %结果。充分考虑到NDT建图特性,本文方法更适合NDT条件下的回环检测。

表2 建图时长与回环检测用时对比

4 结 论

本文针对进行大规模同时定位与建图存在较大累积误差的问题,提出一种基于激光点云NDT特征的两步回环检测方法,并将其运用于完整SLAM框架中进行实验与分析。该方法充分利用了NDT配准时各网格均值与方差特征,实现了同一场景下对传统建图结果的回环检测,有效减少了累积误差。并采用重叠网格进行点云配准与回环检测。回环检测的第一步:通过网格点云方差矩阵特征值对点云进行外观描述,并进行分类,通过各类数量为各类型分配权重,构造两帧间相似函数,进行粗回环检测。第二步:通过网格点云均值点距原点距离,进行精确回环检测,保证点云的旋转不变性。与基于直方图的回环检测相比,极大提高了运算速度。论文的研究结果对基于NDT的大规模同时定位与建图具有指导意义。后期将在其中加入回环预测,仅在回环概率高的地方进行回环检测,进一步提高回环检测效率。