这篇学术论文由上海交通大学的王泽林、杨立民、高峰和王良玉共同完成，题为《FEVO-LOAM: 特征提取与垂直优化的激光雷达里程计与建图》，发表于2022年10月的 IEEE Robotics and Automation Letters 期刊第7卷第4期。该研究属于机器人学领域，特别是同时定位与建图 这一核心方向。研究的动机在于解决当前激光雷达SLAM方法中普遍存在的一个关键问题：垂直方向误差较大。尽管基于激光雷达的SLAM方法在定位与建图精度上已表现出色，但由于传统方法（如经典的LOAM及其变体）主要依赖边缘点和平面点特征进行匹配，而这些特征对于垂直方向的平移和俯仰角变化的约束不敏感，尤其是在建筑物密集、存在大量垂直或近似垂直结构的环境中，容易导致垂直方向的累积漂移。因此，本研究的目标是提出一个高效且高精度的激光雷达SLAM框架，旨在显著降低激光雷达SLAM的垂直误差，同时保持相近的计算开销。

该研究提出的FEVO-LOAM框架是一个完整的激光雷达SLAM系统，其工作流程可清晰分为两大模块：扫描注册、以及里程计与建图。整个系统处理一帧输入点云P_k的过程包含多个精心设计的步骤。

首先，在扫描注册阶段，系统进行地面分割和特征提取。地面分割采用粗分割与精分割相结合的新方法。粗分割阶段，算法选取垂直角度最小的若干条有效激光束的点（假设这些点大多为地面点），使用RANSAC算法估计一个粗略的地面平面方程。为了解决估计平面与真实地面存在夹角导致的远近点判断不准的问题，研究者创新性地使用点云到平面的距离与该点到雷达原点的距离之比作为判断条件，而非简单的绝对距离阈值，从而能更稳健地包含远处的地面点。精分割阶段，则对粗分割得到的地面点构建3D KD-Tree，计算每个点的法向量，并通过法向量与估计地面法向量的夹角进行二次筛选，以滤除错误分类的点，最终得到精确的地面点集G_k。非地面点则送入特征提取模块。特征提取方面，本研究提出了一种新的曲率定义方法，以替代LOAM中基于相邻点距离的简单计算。新方法首先通过一个连续性判断参数识别并剔除因遮挡或表面不连续产生的“离散点”（这些点被认为不稳定，直接归类为平面点）。对于连续点，则在其所属的激光束上分别向前、向后选取若干邻近点，形成两个点集，计算各自的协方差矩阵，并获取对应最大特征值的特征向量（代表该局部点集的主方向）。曲率定义为这两个特征向量模长的乘积除以它们点积的绝对值，这实质上反映了两个局部线段方向夹角的余弦值的倒数。曲率越大，夹角越接近90度，该点就越可能是一个良好的角点。此外，如果协方差矩阵的最大特征值不明显大于其他特征值，说明该点附近不存在良好的线特征，也会被归类为平面点。这种方法能提取出更具判别性的特征点，减少误分类。

其次，在里程计与建图阶段，系统并行运行两个算法：高频的激光雷达里程计和低频但更精确的激光雷达建图。激光雷达里程计采用扫描到扫描的匹配方式。在假设扫描周期内运动匀速的前提下，先对点云进行运动失真补偿。然后，将当前帧提取的角点和平面点与上一帧的对应特征点进行匹配，分别构建点到线距离和点到面距离的残差项。由于新特征提取方法得到了更优质的角点，在优化目标函数中赋予了角点残差更大的权重。最终通过Levenberg-Marquardt非线性优化方法迭代求解两帧之间的位姿变换T_{k-1}^k，提供高频的位姿估计。

激光雷达建图是本研究最核心的创新部分，采用扫描到地图的匹配方式，旨在获得更全局一致的精确位姿。研究者发现，传统LOAM方法在建图优化中仅使用角点和平面点残差，导致对垂直平移和俯仰角变化的约束不足。为此，他们提出了两项关键改进：引入了两种新的残差约束，并采用了两步Levenberg-Marquardt优化策略。第一步优化，主要目标是估计垂直平移和俯仰角。除了使用角点残差约束水平变换、使用地面点残差约束角度变换外，专门新增了垂直残差和俯仰残差。垂直残差是一种特殊的点到面距离，它利用一个角点及其在局部地图中找到的两个最近邻平面点构成一个平面（要求该平面近似平行于地面），计算当前角点到该平面的距离，以此约束垂直方向的平移。俯仰残差则计算由当前帧一个角点和其两个邻近平面点构成的平面，与由地图中对应匹配点构成的平面，二者法向量之间夹角的正弦值，用以约束俯仰角的变化。第一步优化得到一个包含估计垂直平移t̂_z和俯仰角θ̂_pitch的位姿。第二步优化，则将第一步估计的t̂_z和θ̂_pitch作为约束项加入到目标函数中，与传统的角点、平面点残差一起，共同优化求解最终精确的位姿T_k^w。这种两步法既利用了新残差对垂直方向参数的直接优化能力，又避免了将其作为固定值引入可能带来的不鲁棒性，通过软约束的形式引导优化方向。最后，将优化后的点云经体素滤波下采样后注册到全局地图中。系统通过一个变换集成模块，融合高频里程计和低频建图的位姿输出，最终得到既实时又精确的轨迹。

研究的主要结果通过公共数据集和真实环境实验进行了全面验证。在KITTI里程计数据集上的实验表明，FEVO-LOAM在11个序列中的9个上取得了最高的精度，其平均平移误差和旋转误差均为对比方法（包括LOAM、LEGO-LOAM、HDL-Graph-SLAM）中最低的。尤为突出的是，其垂直平移误差仅为0.1557%，相较于其他方法降低了一半以上，这直接证明了该方法在降低垂直误差方面的卓越成效。消融实验进一步证实了各创新模块的必要性：当移除新提出的垂直残差和俯仰残差后（对应方法称为Ours-NVP），性能虽仍优于原始LOAM（这归功于改进的特征提取），但显著低于完整的FEVO-LOAM，其中俯仰残差的贡献最大。在计算效率方面，FEVO-LOAM平均处理频率超过10Hz，在精度和计算成本之间取得了良好平衡。

在真实环境实验中，研究者将算法部署于一台名为“SJTU-Hexapod-Mini”的六足机器人上，在一个工业厂区进行了约1.2公里的闭环路径测试。机器人搭载32线激光雷达，在行走振动等挑战下运行算法。由于环境难以获取GPS数据，他们以高性能激光惯性紧耦合框架LIO-SAM的结果作为参照基准。实验结果显示，FEVO-LOAM的建图结果精度高，无需回环检测即可成功实现轨迹闭合。而对比的其他LOAM类方法轨迹则出现了明显的向上漂移，无法闭合。这强有力地证明了在真实复杂环境中，FEVO-LOAM能够有效克服垂直方向误差累积的问题，具备更高的实用性和鲁棒性。与其他方法相比，其均方根误差也显著更低。

本研究的结论是，成功提出了一个旨在降低激光雷达SLAM垂直误差的高效高精度框架FEVO-LOAM。其科学价值在于系统性地分析和解决了LOAM类方法在垂直方向上的固有弱点，通过创新的地面分割、基于协方差分析的曲率定义以及在建图模块中引入垂直与俯仰残差并结合两步优化策略，实现了理论上的突破。应用价值则体现在显著提升了移动机器人（如六足机器人、自动驾驶车辆等）在复杂、尤其是含有大量垂直结构环境下的长期定位与建图精度和可靠性，为机器人自主导航提供了更强大的技术支撑。

该研究的亮点突出体现在以下几个方面：一是问题导向明确，精准定位了当前主流激光雷达SLAM算法的性能瓶颈——垂直误差；二是方法创新性强，不仅改进了前端特征提取的鲁棒性和判别性，更在后端优化中创造性地引入了针对垂直方向参数的新残差约束和优化策略，形成了完整的技术解决方案；三是验证充分，通过标准数据集量化对比、消融实验分析各模块贡献、以及极具挑战性的真实机器人闭环实验，多层次、全方位地证明了方法的有效性和优越性；四是工程实用性好，在显著提升精度的同时保持了实时性，具备了在实际机器人系统上部署的潜力。

此外，论文在讨论部分也展望了未来的工作方向，指出将探索与惯性测量单元（IMU） 等其他传感器进行融合，以期为位姿求解提供更多先验信息，从而获得更稳定、更精确的定位结果。这为进一步提升系统性能指明了可行的技术路径。FEVO-LOAM是一项在激光雷达SLAM领域具有重要理论和实践贡献的研究工作。