摘要：针对传统SLAM系统前端里程计基于固定阈值提取特征点易导致位姿估计精度降低的问题，该文提出一种自适应点云特征提取方法，通过引入非线性衰减系数，根据点云到激光雷达中心的距离自适应调整特征提取阈值，同时附加非线性权重，提高系统位姿估计的鲁棒性。该文基于FAST-LIO2算法，利用M2DGR数据集验证了所提方法的可靠性，实验结果表明，本文方法的定位和建图精度优于原算法和同类型算法，在提高精度的同时保证了算法的运行效率，有效提升了算法的适用性。
引用：[1] 刘梦涵,王坚,马运涛,等. 一种融合自适应点云特征提取的激光SLAM方法[J]. 测绘科学, 2024, 49 (09): 155-163. DOI:10.16251/j.cnki.1009-2307.2024.09.016.
引言
同步定位与地图构建(simultaneous localization and mapping, SLAM)技术是全球卫星导航系统(global navigation satellite system, GNSS)盲环境导航和定位的主要技术手段之一 [1-2] ,能够在未知环境中借助传感器实时定位自身位置，并构建三维点云地图，已经被广泛应用于智能机器人、无人机、自动驾驶等领域 [3] ,有效推动了综合导航、定位与授时体系的发展。根据传感器的不同，SLAM方法主要分为基于视觉传感器、基于激光雷达两类 [4] ,其中基于激光雷达的方法相较于前者，具有受光照变化影响小、可全天候工作等优点，更能捕捉环境中的结构信息，实现精准定位和稠密三维地图构建。
近年来，随着三维激光雷达的普及，许多优秀的激光SLAM算法被提出：LOAM [5] 通过计算点云曲率提取特征点，构建点到线、点到平面的残差约束，截至目前该算法仍保持在激光SLAM的前列，但该方法未考虑闭环矫正，在几何特征不足的场景效果不佳；LeGO-LOAM [6] 在此基础上进行改进，先将点云分类后再进行特征匹配，通过点云分割减小计算量，同时使用最近点迭代(iterative closest point, ICP) [7] 方法进行闭环检测；LIO-SAM系统 [8] 基于因子图引入激光雷达关键帧的滑动窗口降低计算复杂度，联合IMU预积分因子、GPS因子、激光雷达获得更小的匹配误差；FAST-LIO [9] 基于扩展卡尔曼滤波算法，是一个快速、稳健、通用的激光惯性里程计框架，该方法提出一个新的计算卡尔曼增益的公式，提高点云配准实时性和准确性；在FAST-LIO基础上提出的FAST-LIO2 [10] ,直接将当前帧原始点云与局部地图配准进行位姿估计，同时使用新的数据结构ikd-tree保存地图，实现增量更新地图，提高了算法效率和准确性。
通过前期研究和深入分析，发现尽管当前激光SLAM解决方案众多，但其前端里程计通常是基于人为设定的固定阈值进行特征提取。以LOAM为代表，通过计算点线/点面距离构造残差，当有关残差的设定函数大于某个固定阈值时，当前点即为边缘/平面特征点。由于激光雷达扫描得到的点云近距离密集远距离稀疏，远距离的点云易受到更多不确定因素的影响，因此，在激光雷达测距范围内对所有点云使用固定的全局阈值或固定的局部阈值进行特征提取，易导致特征点分布不均、点云空间约束不足、特征点误判等问题，进而影响激光SLAM系统的定位与建图精度。
针对上述问题，本文提出了一种非线性自适应特征提取方法，以激光雷达和IMU作为数据输入信息源，在前端引入非线性衰减系数动态调整特征提取阈值，同时加入非线性权重增强算法对距离的敏感性，并基于紧耦合迭代卡尔曼滤波器构建激光惯性里程计，实现特征点在空间上均匀分布，减小残差提高位姿估计质量；最后使用ikd-tree实现全局地图点维护和地图更新，构建完整的SLAM系统。系统结构如图1所示。
1 SLAM经典框架
SLAM技术系统框架如图2所示，大致可以分为传感器数据读取、前端里程计、后端优化、回环检测和建图5个部分，其中前端里程计是一个求解机器人运动变换的过程，为后端提供初始的数据，可以分为特征提取、特征匹配、数据关联 [11-13] ,是SLAM的关键问题。本文主要针对前端里程计特征提取进行优化，为更好地体现方法的可靠性，本文基于FAST-LIO2算法进行改进，该算法不提取点面特征点且没有后端优化、回环检测步骤，能够更清晰地体现算法效果。
2 理论基础
流形是指局部具有欧几里得性质的空间，为了更好地理解分析数据，可以使用流形来简洁有效的描述空间。
2.1 运动学模型
经典的SLAM模型可以用一组运动方程和观测方程来描述。运动方程是用于记录某时刻机器人位姿是如何变化的函数，SLAM中的定位问题就是估计位姿。
2.2 状态估计
SLAM最终是为了解决定位和建图问题，根据2.1节模型，可以归结为通过内部传感器测量值和噪声值，估计机器人和路标的最优状态量，即状态估计。
2.2.1 残差计算
前端里程计在进行状态估计时会产生残差，即位姿估计过程中实际测量值与估计值间的差异，残差越小表示估计位姿更接近真实位姿。因此为了提高地图准确性和一致性，系统通过最小化残差来优化位姿估计值，使其与实际测量值更接近。
2.2.2 状态迭代更新
利用迭代卡尔曼滤波器(Iterated Kalman Filter, IKF)将传感器测量值与先前状态估计值结合，以获得当前机器人状态的最优估计。
3 特征点提取策略
前端里程计位姿估计面临特征点误匹配导致误差太大、特征点数目太少导致位姿估计失败两个问题，这两个问题可以通过控制特征点输出来解决 [15] 。因此，受文献 [15]的启发，本文提出一种非线性自适应特征提取方法，通过引入非线性衰减系数，实现随距离变化动态调整特征提取阈值，提高系统位姿估计精度。
3.1 固定阈值的特征提取
以LOAM [5] 为例，基于特征点的里程计通过选取与当前点云处于同一激光线束上的前后五个最近邻点，计算当前点的曲率。升序排序所有点的曲率并与设定阈值进行对比，当曲率大于阈值时该点为边缘特征点，反之则为平面特征点。FAST-LIO2算法与之类似。
基于固定阈值的特征提取方法具有计算速度快、效果稳定等优点，但该方法仍存在一些局限性：阈值需要根据场景变化不断调整，导致算法适用性下降；在不同场景下，易导致特征点分布不均、位姿估算失败等问题。
3.2 分段自适应特征提取
根据激光雷达扫描特点，未知环境中的物体距离激光雷达中心较近时，扫描得到的点云受不确定因素影响较小。因此可以通过提高式(9)中的阈值a来增加特征点输出数量，增加近距离点云在位姿估计中所占的比重。随当前点与雷达中心距离增加，数据可靠性逐渐降低，则可以通过减小阈值a来降低其在位姿估计中所占的比重。
文献 [16]提出一种基于距离的自适应特征提取方法，该方法通过划分距离区间，并在不同距离区间内设置阈值函数，实现随距离变化计算特征提取阈值，阈值控制策略。
该方法虽然能达到根据距离自适应提取不同数量特征点的目的，但需要设置合理的距离区间和初始阈值，无法更好地模拟数据可靠性随距离衰减的特性，存在一定的局限性。
3.3 非线性自适应特征提取
使用线性函数调整阈值时，易导致特征提取阈值随距离增加而急剧下降，因此，为更好地模拟数据可靠性衰减特性，解决3.2节方法存在的问题，提升算法适用性，本文提出一种非线性自适应特征提取方法。
计算当前点到激光雷达中心的距离；引入非线性衰减系数，构建Logistic Sigmoid函数，随距离变化自适应计算用于特征点选择的阈值a。相较于其他非线性函数，该函数能够更灵活、更精确地控制衰减点和衰减速率，保证衰减平滑过渡，不会突然跳变，并且对于不同激光雷达可根据扫描特点灵活修改衰减开始距离。本文阈值控制策略如图3所示。
4 实验分析
本文以FAST-LIO2算法为研究对象，在算法前端进行特征提取时，融合非线性自适应特征提取方法，采用M2DGR [17] 数据集进行验证分析，该数据集地面真实轨迹由运动捕捉获得。实验平台采用处理器为12代的14核Intel CoreTM i7-12700H CPU,内存为15.3 GB的惠普笔记本电脑，Ubuntu20.04、ROS Noetic Ninjemys操作系统。为验证本算法定位建图准确性和稳定性，将经典FAST-LIO2算法、文献 [16]的方法、本文方法进行对比分析，分别选取M2DGR数据集中两个不同场景下采集的street_08序列、hall_02序列进行实验，并利用绝对位姿误差(absolute pose error, APE)评定SLAM定位精度 。
4.1 street_08序列实验分析
本节使用M2DGR数据集street_08序列，针对经典FAST-LIO2算法与融合分段自适应特征提取、本文方法展开对比实验，该序列为存在连续转弯的户外采集序列，实验得到3组定位轨迹，得到的实验结果如图4所示。
由图4可以看出，3个算法轨迹整体虽没有明显漂移，但在转弯处本文方法得到的轨迹优于前两种方法，与地面真值更加吻合，且误差更小。3个算法运行得到的点云局部地图如图5所示。
从图5可以看出，融合分段自适应特征提取的方法得到的点云地图与原算法没有明显区别，本文算法相较于其他两种算法建图效果更好，环境纹理信息更加清晰。
本文进一步对比分析了3个算法的APE,实验结果如表1所示。
由表1可以看出，本文方法的定位精度优于其他两种方法，相较于原算法，绝对位姿误差的RMSE下降了18.7%;相较于融合分段自适应特征提取的方法，绝对位姿误差的RMSE下降了7.3%。
4.2 hall_02序列实验分析
本节使用M2DGR数据集hall_02序列，针对经典FAST-LIO2算法与融合分段自适应特征提取、本文方法展开对比实验，该序列为存在多处转弯的室内采集序列，实验得到了3组定位轨迹，实验结果如图6所示。
由图6可以看出，本文算法得到的轨迹优于前两种方法，与地面真值更加吻合，且误差更小。3个算法运行得到的点云局部地图如图7所示。
从图7可以看出，融合分段自适应特征提取的方法得到的点云地图与原算法没有明显区别，本文算法相较于其他两种算法建图效果更好，环境纹理信息更加清晰。
本文进一步对比分析了3个算法的APE,实验结果如表2所示。
由表2可以看出，本文方法的定位精度优于其他两种方法，相较于原算法，绝对位姿误差的RMSE下降了12.6%;相较于融合分段自适应特征提取的方法，绝对位姿误差的RMSE下降了4.7%。
4.3 时间效率
本文进一步对比分析了经典FAST-LIO2算法与融合分段自适应特征提取、本文方法的时间效率，实验结果如表3所示。
由表3可以看出，本文方法相较于其他两种方法平均运行时间多出约1 s, 话题发布频率相当，表明本文方法在保证精度的同时，时间效率损失在可接受范围内，在实际应用中有较高实用价值。
5 结束语
针对SLAM系统基于固定阈值提取特征点易导致位姿估计精度降低等问题，本文提出一种自适应点云特征提取方法。该方法通过引入非线性衰减系数，基于当前点到激光雷达中心的距离自适应调整特征提取阈值，同时附加非线性权重，有效提高了系统位姿估计的精度。在M2DGR数据集上的实验结果表明，融合本文算法后的定位精度和建图效果优于经典FAST-LIO2算法与同类型算法，在提高算法精度的同时保持了算法运行效率，具有较好的适用性和鲁棒性。
