一种融合IMU的手持旋轴激光雷达定位建图方法

赵宇轩,贾克斌*,陈嘉平

(1.北京工业大学 信息学部,北京 100124; 2.先进信息网络北京实验室,北京 100124)

随着同步定位与建图(Simultaneous Localization and Mappi-ng,SLAM)技术的不断发展,其已经广泛应用于2D/3D建图、智能机器人导航等领域[1]。多数时间,机器人可以通过GPS、GNSS等设备获取位置信息进行定位。但当机器人处在地下停车场、隧道等封闭环境时,GPS、GNSS设备定位便会失效。机器人无法获得自身位置定位从而无法进行导航任务,而SLAM技术为此需求提供了一种解决方法[2]。

激光SLAM通过激光雷达直接获取距离信息,提供三维点云。激光雷达在有着高测量精度的同时,受周边环境影响较小,无论白天黑夜都能正常工作[3]。激光雷达视野更广,直接获取点云也使建立地图变得更加方便[4]。但激光雷达多部署于车辆载具,这导致其使用门槛较高、便携性差,无法快速进行数据采集。随着激光雷达产业的不断发展,手持旋轴式激光雷达扫描设备也逐渐走入人们的视野,满足了人们对于便携性的需求[5],手持旋轴式激光雷达不仅轻便易用,还有着更大的扫描范围,如图1所示。

图1 建图范围对比

LOAM(LiDAR Odometry and Mapping in Real-time)算法[6]提取边缘特征点与平面特征点分别进行配准,属于基于特征的方法,这使得LOAM可以实时快速地进行定位与地图构建。同时,随着研究的不断深入,如今已有多种基于LOAM的改进算法。文献[7]提出,在LOAM的基础上引入附加特征点,增强SLAM系统在结构化场景中的配准能力。文献[8]提出一个根据当前激光帧内点云距离分布信息,采用一种模糊算法,调整系统参数,增强SLAM系统对环境空间变化适应性的方法。当今应用较为广泛的主流LOAM改进算法有对地面点分割聚类的轻量级SLAM算法LeGO-LOAM[9]和通过紧耦合惯性测量单元(Inertial Measurement unit,IMU)实现更好定位建图效果的SLAM算法LIO-SAM[10]。

主流激光SLAM算法可以满足大多数定位建图需求,但均未考虑手持旋轴激光雷达的旋转定位建图场景。LOAM算法假设雷达运动为匀速模型,通过雷达里程计对点云进行畸变去除,在激光雷达水平放置且低速运动时尚可达成较好效果,但在激光雷达旋转场景下不足以满足需求。LeGO-LOAM算法针对载具设备上水平放置的雷达,通过提取地面点聚类的方法定位建图,对地面点的要求同样不满足手持旋轴激光雷达的旋转过程。LIO-SAM算法通过远高于激光雷达扫描频率的IMU传感器提供数据,对激光雷达点云进行畸变去除,可以满足激光雷达旋转的应用场景。然而设备运行过程中,在一个旋转周期的部分时间内,激光雷达的部分扫描区间会朝向上空开阔区域。此区域基本无法接收返回的激光点,易导致特征点稀疏问题的发生,这对基于特征的SLAM算法的稳定性有很大影响。为提升手持旋轴激光雷达使用的稳定性,受上述工作启发,本文将一种基于点云强度信息的特征点引入LIO-SAM算法中,增加参与配准的特征点数量,并实时判断系统所处环境是否存在退化风险,动态调整滤波数值设置,保证SLAM系统的稳定性。

1 算法流程

本文的算法流程路线如图2所示,输入为10 Hz的激光雷达数据与200 Hz的IMU数据。

图2 算法流程路线

主要流程分为以下3部分。

① 首先对IMU与激光雷达数据进行处理,对IMU进行预积分操作,根据IMU信息对激光雷达进行点云畸变矫正。

② 随后对矫正过的点云进行特征点提取,分别提取边缘特征点、强度特征点以及平面点,动态调整滤波数值设置并构建局部地图。

③ 最后进行配准优化和回环检测与图优化,发布激光里程计。

1.1 数据处理

W代表世界坐标系,I代表IMU坐标系,T21代表从坐标系1到坐标系2的变换矩阵。则可以规定IMU坐标系到世界坐标系变换矩阵为TWI=(RWI,PWI),若RT表示旋转状态,PT表示位置向量,VT表示速度,bT表示IMU偏置,则状态x可以被表示如下:

x=[RT,PT,VT,bT]T

(1)

1.1.1 IMU数据处理

IMU在SLAM系统运行中发挥重要作用,IMU以远高于雷达频率的速度来提供位姿、加速度、角速度等数据来更好地推算雷达的状态。IMU的角速度与加速度计算公式如下:

(2)

(3)

(4)

(5)

(6)

随后使用文献[11]的方法进行IMU预积分,提升运算效率。

1.1.2 激光雷达点云去畸变

在实际采集数据过程中,激光雷达在运动,且点云数据并非瞬时获取。这会导致一帧内的点云数据产生运动畸变,需要将一帧内所有点云投影至扫描帧开始时刻,完成畸变的去除。通过插值的方法来确定点的位姿:

(7)

(8)

tcur=treal+tScanStart

(9)

式中:tcur为当前点在全局中的时间;tb与tf分别为此雷达帧前后最近的两帧IMU时间;Rb与Rf为计算的旋转角度;treal为一点p相对于扫描帧开始时刻的时间;tScanStart与tScanEnd分别为激光雷达扫描帧开始与结束时刻;Podm为位移增量。后经插值得到当前点关于扫描开始时刻的角度变换Rcur与位移Pcur。根据所求的三轴旋转角度与三位移量构成六自由度数据后,即可得出此点相对于开始时刻的变换矩阵Tcur。从而可将点变换至扫描开始时刻:

TstartΔT=Tcur

(10)

(11)

pori=ΔRpcur+Δp

(12)

式中:pcur为当前点未去畸变坐标;Tstart为开始时刻点位姿,经ΔT=(ΔR,Δp)变换至开始时刻位置pori完成畸变去除。

1.2 特征提取

改进算法将一帧内的点云分为边缘特征点、平面点的同时,额外提取强度特征点来增加参与配准的激光点数量。激光雷达接收的强度返回值会受到多种因素的影响而变化,但在很短的雷达帧间时间内,假定强度信息变化趋势的整体一致性,提取出周围强度值变化较大的点作为强度特征点参与到点云配准。对一帧点云Pk内的点进行曲率c与强度变化率m的计算:

(13)

(14)

式中:i为Pk中一点;S为与i在同一扫描线的周围连续多个点;j为S中一点;r为扫描点的深度值;I为点的强度值;k为第k帧。将每根扫描线分为6个区域,每个区域划定内分别根据强度变化率与曲率进行排序。设置mth与cth分别作为强度特征点与边缘特征点的阈值,将数据大于阈值的几个点提取作为强度特征点、边缘特征点,并根据相邻两点列索引差,剔除雷达由于射向地面导致线束中断而被误判为边缘特征点的点。为保证特征点不过于密集,每选取一个边缘特征点或强度特征点时,会将其同一扫描线周围点标识为不可选取。强度特征点与边缘特征点的选取原理不同,不会影响它们周围点的选取与否。例如将点i选取为强度特征点后,点i周围点不能被选为强度特征点,但点i周围点依旧可选为边缘特征点。提取的强度特征点主要为不同材质的边界等周边环境复杂的点,如图3所示。图3(a)显示了激光雷达扫描到地面上一条涂有漆面的标识线,由于表面反射强度值不同,可以从中提取出强度特征点,如图3(b)所示。然而,有些边缘特征点同样满足周围反射强度值变化大的条件,但将边缘特征点改提取为新的强度特征点并无意义,所以选择提取的强度特征点是强度变化率mi满足阈值mth且曲率ci不满足边缘特征点阈值cth的点。且为防止扫描时由于距离过近,强度值跳变较大,被误判为强度特征点,被选取点的深度ri应大于深度阈值rth,选取的强度特征点应满足式(15):

图3 强度特征点选取

mi>mth∩cirth

(15)

1.3 动态滤波数值设置与局部地图构建

在数据采集过程提取特征点后,系统会对特征点进行一次降采样滤波。然而周围的场景总是在变化的,单一滤波数值设置只满足部分场景。例如适用于户外特征点较多的滤波数值,在进入通道等退化环境时会容易导致建图失败,且激光雷达处于旋转过程时,特征点数量变化更加频繁。这就需要在运行过程中实时调节滤波数值保证系统运行稳定。本文使用一种简洁快速的方法实时调整滤波数值,达到防止退化的目的并保持系统运行的效率。

在系统提取特征点后,根据提取边缘特征点数量Nf对比设置正常特征点数量Nnor与有退化风险特征点数量Ndeg,判断系统所处状态从而实时调整相应的数值,判断方式如下:

(16)

根据以上3种不同情况,实时动态地调整滤波数值。当特征点数量较少时,认为系统有退化风险,则选取小数值,多保留特征点,当特征点数量足够时,选取较大数值保证系统运算效率,通过此方法提升SLAM系统的稳定性。

随后,通过提取关键帧,在当前扫描帧周围进行局部地图构建,构成的局部地图包含强度特征点局部地图Mi、边缘特征点局部地图Me和平面点局部地图Mp,为配准优化构建条件。同时判断此帧内强度特征点数量,若数量较少,则认为此帧为强度特征退化环境,此帧内的强度特征点将不参与后续配准优化,以防对配准优化产生负面作用。

1.4 配准优化

将参与配准的特征点与局部地图进行配准,对于强度特征点与边缘特征点进行点-线最小距离优化:

(17)

对于面点进行点-面最小距离优化:

(18)

式中:Pfeature为提取出的点;p1、p2、p3分别为在局部地图中搜索出的与Pfeature同标签的点。随后使用优化方法求解,使残差最小化,当优化问题收敛时,可以得到激光里程计从而建立地图。

2 实验结果与分析

2.1 实验系统概述

使用便携式手持旋轴激光扫描仪进行实验,如图4所示。

图4 实验设备

其中激光雷达为16线,垂直视角30°,水平分辨率0.2°,采集频率为10 Hz,IMU采样频率200 Hz,在Ubuntu18.04操作系统下使用ROS melodic机器人操作系统,使用手持旋轴激光扫描仪器绕建筑物行走一周,在此之间经过一个封闭通道,如图5所示。

图5 实验环境

2.2 特征点提取对比

在实验过程中,将改进算法与LIO-SAM算法提取的特征点进行对比,结果如图6所示。在图6(a)采用的LIO-SAM算法中,绿色代表曲率较大的边缘特征点,粉色点代表提取的面点。在图6(b)采用的改进算法中,不仅提取绿色边缘特征点、粉色的平面点,还有额外的蓝色强度特征点。从图6(a)与图6(b)的对比中可知两种算法提取特征点不同,改进算法可以在运行过程中为SLAM系统提取更多数量的特征点。

2.3 退化环境效果对比

在实验过程中,在易于退化封闭通道环境中,两种算法结果对比如图7所示。

图7 局部场景

图7(a)中,LIO-SAM算法在进入封闭环境时,未判断是否有退化风险,没有实时调整滤波数值。进入通道时发生退化问题,发生定位建图错误。

图7(b)中,改进算法检测到进入易退化环境特征点变少后,改变滤波数值设置以应对退化环境,保证系统的稳定性,平稳通过通道场景。

2.4 定位轨迹对比

整体轨迹效果对比如图8所示,手持旋轴激光雷达绕建筑物行走一周。图8(a)采用的LIO-SAM算法的里程计轨迹终点相对于起点有较大偏移。图8(c)中,改进算法的轨迹终点与出发点接近。使用EVO工具对里程计轨迹数据进行绘制,图8(b)中,LIO-SAM算法轨迹已经产生偏差,图8 (d)中,改进算法轨迹绕建筑物一圈,与图5相似。为明确对比实验结果,对轨迹数据起止坐标进行对比分析,结果如表1所示。

表1 定位轨迹对比 单位:m

表1中,Δx、Δy、Δz、Δd分别为终点坐标与起点坐标的x、y、z轴的坐标差值以及相对于起点的距离差值。相比于水平方向上的差值Δx与Δy由于人工采集数据时可能造成的误差,垂直方向上的偏移量Δz更加可观。由表1 LIO-SAM算法与改进算法对比可知,改进算法在垂直偏移量Δz与相对于起点的距离差值Δd均优于LIO-SAM算法。结合图8与表1,改进算法在手持旋轴激光雷达的定位轨迹应用上优于LIO-SAM算法。

2.5 建图效果对比

对比手持旋轴激光雷达使用LIO-SAM算法与改进算法的建图效果如图9所示。

图9 建图效果对比

图9(a)中,LIO-SAM算法由于里程计轨迹错误估计,导致扫描中激光雷达建图出现较大偏差,产生图9(a)中圈出的重影问题。图9(b)中,改进算法则有着较好的表现。综上,改进算法在手持旋轴激光雷达的建图效果上优于LIO-SAM算法。

2.6 长距离定位建图对比

SLAM系统的定位建图误差会在运行过程中逐渐累积,目前实验使用的IMU精度尚有较大提升空间,误差在旋转且长距离场景下会相对明显。使用EVO工具对里程计轨迹数据进行绘制,图10(a)与图10(b)分别代表定位轨迹与定位建图过程中x、y、z三轴的坐标值。其中,蓝色代表LIO-SAM算法,绿色代表改进算法。结合图10(a)与图10(b)可知,两算法在定位建图过程中x、y轴的数据大体一致,但LIO-SAM算法的z轴数据随着系统运行,出现相对更多偏移,如图10(b)所示。图10(c)中蓝色框处出现相对较明显的倾斜。而图10(d)中改进算法的地图相对较为平坦。对比建图效果与坐标偏移,改进算法在手持旋轴激光雷达的长距离定位建图效果较LIO-SAM算法也有一定提升。

图10 长距离定位建图实验结果对比

3 结束语

本文在LIO-SAM算法提取边缘特征点与平面点的基础上,计算出一种基于点云强度信息的强度特征点,增加特征点数量,并在定位建图过程中,实时动态判断SLAM系统所处情况是否存在退化风险,动态调整滤波数值设置,保证系统稳定运行。实验结果表明,改进算法在手持旋轴激光雷达应用中,相比LIO-SAM算法有更好的定位建图效果。但较快速转弯场景与出现的动态物体对系统仍有一定影响,后续需进一步改进。