基于特征提取改进的LeGO-LOAM方法

杨书涛,郁汉琪,戴红卫,李佩娟,杜俊峰,李 睿

(1. 南京工程学院工业中心/创新创业学院, 江苏 南京 211167;2. 南京绿航生态农业有限公司, 江苏 南京 211500)

随着移动机器人的快速发展,其在物流、无人驾驶和智能家居等多个领域获得广泛应用.这些机器人通过使用传感器,如摄像头、激光雷达,能够实现环境感知和自主移动,并成功完成各种复杂任务[1].机器人自主移动前需要进行同步定位和建图(simultaneous localization and mapping,SLAM),以实现环境感知和机器人定位[2].

目前激光SLAM研究已成为热点,研究主要集中在特征提取、点云分割、回环检测等方面,特征提取是激光SLAM中的一个关键环节[3];在激光雷达获取的原始点云数据中存在大量的噪声和冗余信息,通过特征提取,可以保留最具代表性的关键特征,过滤掉无用信息和干扰信号,从而为后续的定位和建图提供更加稳定可靠的数据源[4-5].特征提取可以帮助SLAM算法识别并匹配当前帧与历史帧之间的对应关系,从而确定机器人在空间中的位置和姿态,并计算出其相对运动关系,提高定位精度.最早期的激光SLAM方法主要使用欧式距离或者点之间的其他几何信息来计算匹配程度,从而构建地图,但该方法未能提取有意义的特征点,导致匹配误差大,且需要大量的计算资源与运算时间[6];文献[7]提出基于特征匹配的激光雷达建图(lidar odometry and mapping,LOAM)算法,通过对扫描点云数据进行高频操作提取边缘和平面特征点,但其对地面判断不够准确;文献[8]在LOAM算法的基础上提出轻量化激光雷达建图(lightweight and groud-optimized lidar odometry and mapping,LeGO-LOAM)算法,采用地面分割方法和点云聚类方法来提取特征点,并在LOAM算法的基础上加入IMU减小地图匹配误差.在特征选择与匹配方面,这些算法虽然可以通过各种特征提取方法获得关键特征点,但处理并不完善,存在匹配误差较大的问题.

本文提出一种基于特征提取改进的LeGO-LOAM方法,采用自适应特征提取方法在不同距离下保证特征提取的准确性,通过基于随机一致性采样优化的地面特征滤除方法来去除不可靠特征点,通过相邻帧中相同聚类对象的移动距离去除动态目标,使得后期提取的特征点更加合理.

1 特征点提取优化算法设计

1.1 自适应特征点提取

激光雷达是基于线扫描获取点云信息,本文移动机器人安装Velodyne16线激光雷达,雷达基础坐标系定义为:Y轴正方向水平向前,Z轴正方向垂直向上,如图1所示.其水平视场为360°,水平扫描分辨率为0.2°,垂直扫描分辨率仅为2°,所以整个环境点云是稀疏的.

图1 Velodyne16线激光雷达坐标系的定义

传统的特征提取方法通常采用关键点前后5个点的深度差值和的平方作为判断指标[9],这种方法容易导致特征点的错误识别和遗漏.通过研究分组策略,本文采用一种自适应特征提取算法来解决这个问题[10].在计算关键点与邻近点的深度差值时,先根据关键点的深度值进行对比,如果误差超过一定阈值,则将其直接视为噪点并剔除;关键点i的深度越小,就需要更多的邻近点,相反,关键点的深度越大,则需要较少的邻近点.邻近点数S在范围(4,10)内进行自适应波动,对应的水平包角θ在0.8°～2°.图2为基于深度变化的自适应曲率计算示意图.由图2可见:当深度R1较小时,相应的角度θ1包含的距离l1较短,为了获取更多的临近点来计算曲率,需要扩大角度θ2来增强曲率信息的可靠性;当深度R2较大时,θ1包含的距离l2过长,此时需要缩小θ3来计算曲率.当关键点的曲率小于阈值时,判定其为平面点,而当曲率大于阈值时,则判定为角点.

图2 基于深度变化的自适应曲率计算示意图

关键点i的水平曲率计算式为:

(1)

式中:ri为点i的三维坐标,m;S为同一行激光扫描线上的连续点集.

点集S在关键点i两侧平均分布,临近点集S的值与关键点i的深度值Ri存在自适应关系:

S∈[(Ri≤10,10),(8,10

(6,2030)]

(2)

平面特征的阈值设置为0.1,角点特征的阈值设置为0.5[11].当计算得到的水平曲率C<0.1时,关键点i被归类为平面特征点;而当C>0.5时,则被归类为角特征点.

1.2 地面点云特征点滤除

在特征提取过程中会得到许多点云特征点,过多的特征点并不能显著提高精度,反而会增加系统资源的消耗.在结构丰富的室外环境中,平面特征点的数量较多,这些平面特征点通常聚集在地面点云上;同时,地面上的特征点分布相似,这种相似性可能导致闭环检测时的误匹配.基于以上考虑,本文采用基于随机一致性采样(RANSAC)的地面点云特征滤除方法[12].

Velodyne16线激光雷达直发射角在±15°之间,在垂直发射角为正的激光线上不会出现地面点.但实际户外环境地面倾斜或不平,激光线的垂直发射角度选择在[-15°,1°]范围内搜索地面点,包含地面平面模型的内点和外点,符合平面模型的点称为内点,不符合的点称为外点.在每帧点云中选择3个点P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)作为内点进行计算,并假设它们满足某个平面模型方程xcosα+ycosβ+zcosγ=D,其中,(cosα,cosβ,cosγ)为地面的法向量,D为激光原点到地面的距离,地面点云的描述如图3所示,距离误差在预设的阈值内(距离阈值设置为0.1 m),将剩余点云带入模型进行求解,就认为满足平面模型,并将其添加到内点集合I={p1,…,pi,…,pn};进行k次迭代,比较每次产生的数学模型所包含的内点数量,保存内点数最多的模型,得到相对更合理的地面数学模型解;最后将所有的内点标记为地面点云.识别出地面点云后,不对地面点云进行后续操作,在处理最后一帧点云数据时,地面点云以黑色标记显示,图4(a)为LeGO-LOAM提取地面特征点,图4(b)为优化后地面特征点.由图4可见,优化后滤除地面特征有一定效果.

图3 地面点云和地平面法向量

1.3 动态点滤除

在室外环境下,由于行人和行驶车辆等原因导致动态点较多,在去除动态点之前,需要先对点云数据进行分割聚类,并区分出动态对象和静态对象.在真实的物理世界中,很多物体通过地面相互连接.如果在分割时不单独考虑接地点,这些通过地面连接的物体可以被划分为一类物体.去除接地点则可以根据点云的连接情况对三维世界中未连接的物体进行分割.

相邻点示意图如图5所示,图5中:O为LiDAR的坐标原点;A为点云中的任意点;B为相邻激光线上的相同方位角点;C为同一激光线上的相邻点;β为OA与AC之间的夹角,β为判断A与C是否位于同一物体上提供有价值信息,计算式为:

图5 相邻点示意图

(3)

根据文献[13]将邻角阈值定义为60°,如果相邻点之间的夹角大于60°则可以认为它们来自于同一物体.通过上述分割,最终点云中的点数大于30,则认为分割有效[13];如果点数小于30,则可以将这些点视为离群点或干扰点,在进行特征匹配前将特征点去除.这种方法可以有效地去除离群点和干扰点.聚类后进行动态消除,具体步骤为:

1) 对物体进行聚类后,即可知道每种类型的物体所包含点的个数,该物体质心计算式为:

(4)

2) 激光雷达采样频率非常快,即使激光雷达随着底盘不断移动,也可以认为每次聚类后物体所含的点数基本保持不变,或者在一定范围内发生变化,根据点的数量,可以匹配相邻两帧之间的对象.

3) 匹配对象可以计算出移动距离,移动距离用质心之间的变化量来表示,此时假设:a) 静止物体的数量应远远多于运动物体的数量;b) 不同静止物体的移动距离应基本保持不变或在一定范围内变化;c) 静止物体和运动物体之间的距离是可分辨的.基于以上假设,可以通过移动距离消除动态物体,对每个物体的移动距离进行计算和排序,首先去除最大的10%,因为这些可能是移动的物体;然后去除最小的10%,这些可能是匹配异常的物体.

2 仿真分析与室外试验验证

2.1 仿真分析

KITTI数据集[14-15]由德国卡尔斯鲁厄理工学院和美国丰田理工学院共同创建.该数据集被广泛用于评估自动驾驶场景下计算机视觉算法的性能.KITTI数据集不仅提供原始点云数据,还包含轨迹真值数据(ground truth),可以用来分析LiDAR里程表的精度.选取的序列为09,时间约为165 s.

通过轨迹精度评测软件EVO可直观体现SLAM算法的里程计与真实里程计的整体偏差.本文在KITTI09数据集上将各类算法和改进算法进行全局里程计偏差对比,结果如图6、图7所示.为进一步深入分析,将LeGO-LOAM、LIO-SAM轨迹数据中的X、Y、Z轴坐标分别与ground truth进行详细对比,结果见表1.

表1 激光雷达里程计的误差数据 m

图6 生成轨迹对比

图7 X、Y、Z轴运动估计的比较

由图6可见,本文改进算法比其他算法真实里程计偏差较低.由于KITTI09数据集具备64线激光雷达,点云数据丰富,因此,所有算法均能够实现低漂移的运动估计;由图7可见,所有算法误差主要集中在Y轴上,即垂直方向的偏差较大,与其他算法相比,本文算法在降低3个轴向上的最大误差方面表现出良好效果,其中对Y轴误差的改善尤为显著;由表1可见,本文算法比加入了IMU预积分约束的LIO-SAM能获得更小的地图匹配误差,计算得出的最大误差相较于初始算法减少了23.9%,平均误差也呈现出下降,降幅达到19.1%.

综上所述,通过多重观察角度的图像分析和数值数据对比,本文算法不仅在降低三轴最大误差方面表现出优越性能,尤其是在Y轴方向上取得了显著的改善.这些结果为该算法在实际应用中的准确性和可靠性提供了有力支持.为了验证优化后运动估计的稳定性,进行5组运动估计试验,结果如图8所示.由图8可见,三轴坐标基本不变,说明本文方法具有良好的鲁棒性和稳点性.

图8 5组试验运动估计比较

2.2 实际户外环境试验

试验使用松灵公司研制的BUNKER底盘,车身尺寸为1 023 mm×778 mm×400 mm.车身前端配置Velodyne16线激光雷达,该雷达支持TCP/IP协议,通过网线将激光雷达与计算机连接.运行算法的计算机配置为英伟达公司生产的Jeston TX2,系统为Ubuntu18.04.试验平台框架如图9所示.

图9 试验平台框架

为了评估优化后的SLAM效果,在户外环境中进行轨迹测试,测试结果如图10所示.由图10可见,标记出的底盘实际运动路径具有复杂多变的路况,地面坡度变化频繁,路边有许多车辆、树木和行人,这些动态元素对于点云处理过程中的特征点去除造成了重大干扰.如果特征点误判为摆动的叶子,运动估计结果将产生偏差.生成的全局点云图如图11所示.图11能够高度还原室外环境,包括道路两侧的建筑物和台阶,轮廓清晰可见.

图10 实际运动路径图

图11 全局点云图

为了进一步评估系统性能,将记录的LiDAR点云数据保存为Bag文件,并进行5次离线试验,对比每次试验产生的轨迹发现,5次重复试验生成的轨迹高度重叠.将LiDAR点云数据记录过程中计算的里程表数据作为真值,并对比记录5次试验的里程表误差,结果如表2所示.由表2可见,与在线生成的里程数据表相比,5个离线试验计算出的最大位置误差小于1 m,表现出良好的鲁棒性和稳定性.这些结果进一步证明了本文算法的可靠性和鲁棒性.

表2 五组试验与激光里程表误差 m

3 结语

本文提出了基于自适应特征提取及滤除地面不可靠特征点和动态点的即时定位建图算法.在特征提取方面,该算法能够准确、高效地提取出地图中的关键特征点,为后续的定位和建图任务奠定基础.同时,可有效解决地面特征点的滤除和动态点的消除,确保定位建图的精度和稳定性.仿真结果表明,优化后计算出的最大误差降低了23.1%,平均误差降低了19.1%.5个离线试验计算出的最大位置误差小于1 m,表现了良好的鲁棒性和稳定性.然而,当前的算法在闭环检测方面可能还存在一定的局限性,需要进一步的研究和改进,提升定位建图算法的性能和可靠性.