在树木环境中，通过鲁棒的降维技术，为带腿的农业机器人实现仅依赖激光雷达的连续性SLAM（ simultaneous Localization and Mapping）

《Computers and Electronics in Agriculture》：Consistent lidar-only SLAM for legged agricultural robots in arboreal environments via robust dimensionality reduction

　　保拉·纳扎特-布尔戈斯（Paola Nazate-Burgos）| 米格尔·托雷斯-托里蒂（Miguel Torres-Torriti）| 黄寿东（Shoudong Huang）| 费尔南多·奥阿特·切因（Fernando Auat Cheein）
智利天主教大学工程学院电气工程系

摘要
在树状环境中进行同时定位与地图构建（SLAM）面临着独特的挑战，这些挑战源于密集的树叶、不均匀的地形以及全球导航卫星系统（GNSS）信号的衰减。尽管现有的3D激光雷达SLAM方法已经非常先进，但它们通常需要集成惯性测量单元（IMU），并假设平台运动是平滑的，同时以达到最高精度为目标，这使得这些方法不适用于在不规则地形上运动的腿部机器人，或者当惯性传感器不可靠时。本文提出了一种SLAM方法，其目标是在树状环境中实现一致的定位，而不是追求最高精度，该方法通过降维来实现这一目标。该方法依赖于像树干这样的空间上持续存在的垂直结构。我们改进了豪斯多夫距离（Hausdorff distance），并将其用于3D激光雷达数据切片的2D投影的匹配，作为一种稳健的距离度量方式，从而无需复杂的特征提取、IMU集成或GNSS校正。这种方法特别适合腿部机器人和手持式测绘系统，因为在这些环境中，不连续的运动会使得现有方法的假设不再成立。当缺乏明显的垂直结构时，例如在非常年轻的果园、棚架系统中或植物种植非常稀疏的情况下，该方法的效果不佳。

我们在模拟环境、受控的实地测试以及三个真实世界数据集（CitrusFarm、Bacchus和Pullally）中对该方法进行了评估，其中包括一个使用四足机器人收集的新数据集。我们的综合评估结果表明，当由于平台运动突然或不可靠而无法使用IMU数据时，所提出的方法在不同环境中的定位精度保持在0.40米到1.34米的范围内，尤其是在Pullally数据集（该数据集包含腿部机器人）中的表现尤为稳健（三次试验的平均定位精度为0.40 ± 0.05米）。而那些专为IMU集成和密集3D点云匹配设计的方法，在平坦地形上使用轮式机器人时表现更好（在CitrusFarm数据集中的平均定位精度为0.16–0.28米），但在腿部机器人平台上则会崩溃（在Pullally数据集中定位精度超过27米）。我们将所提出的方法与A-LOAM、LeGO-LOAM、DLO、MOLA和AG-LOAM进行了比较。结果表明，我们的方法以牺牲峰值性能为代价，换取了在不同数据集和平台类型下的稳健性。

1. 引言
农业行业正在通过智能农业技术的整合经历快速转型，自主系统在应对劳动力短缺、劳动力成本上升、减少环境影响以及确保生产者经济可持续性方面发挥着关键作用（Mahto等人，2024年；Torres-Torriti和Nazate-Burgos，2022年）。自动化基本农业任务以及在林业和果树果园中部署移动机器人面临的一个重要障碍是实施自主导航策略的难度，因为大多数现有的同时定位与地图构建（SLAM）算法都是为结构化的室内或城市环境设计和优化的。最近的文献（Aguiar等人，2022年；Debeunne和Vivet，2020年；Liu等人，2022年；Li和Zhu，2024年）已经认识到SLAM技术在农业领域的适用性，并指出需要改进当前的SLAM策略，特别是在树状环境中（Sier等人，2023年；Wang等人，2024年）。像果树果园和森林种植园这样的环境引入了独特的限制因素——包括不均匀的地形、密集且重叠的植被、季节性的外观变化以及高度重复的结构（Cremona等人，2022年；Debeunne和Vivet，2020年）——这些因素限制了基于视觉或特征的算法的性能。此外，全球导航卫星系统（GNSS）信号经常由于树叶和树冠的遮挡而衰减或完全中断，导致高精度定位变得不可靠。另外，许多室内或城市环境中的SLAM策略严重依赖惯性测量单元（IMU）来补偿里程计漂移和运动动态（Torres-Torriti和Nazate-Burgos，2022年）。然而，即使IMU经过完全校准，并且使用了基于精确运动模型的滤波器，它仍然会产生惯性漂移和定位误差，因为强地形变化会导致平移加速度和角速度测量的大幅干扰，从而损害机器人的姿态估计以及随之而来的地图构建过程（Cremona等人，2022年；Sier等人，2023年；Wang等人，2024年）。

鉴于这些挑战，本文提出了一种仅使用激光雷达的SLAM方法，适用于GNSS信号受阻且地形不均匀的树状环境，如图1所示，其中密集的树冠经常阻挡GNSS信号，使得高精度的厘米级定位变得不可行。该方法特别适合在树状环境中运动不平稳且存在持续垂直结构（如树干、葡萄园支柱和种植园桩）的机器人。当缺乏明显的垂直结构时，例如在非常年轻的果园、棚架系统或植物种植非常稀疏的情况下，该方法的效果较差。

所提出的方法通过解决使用3D激光雷达扫描片段的2D投影的定位问题来引入降维步骤，这些片段可能是树干和较低树冠部分的投影，并使用改进的豪斯多夫距离（MHD）（Donoso-Aguirre等人，2008年）作为稳健的距离度量方式，无需显式的特征提取和数据关联阶段，也不需要匹配大型3D点云；具体过程如图1所示。这通过减轻由树叶和动态障碍物引起的异常值来提高对准精度和稳健性。该算法无需依赖视觉特征、惯性校正或GNSS反馈即可构建3D地图，因此非常适合在密集的果树种植园或林业中的户外部署。该方法基于作者在高度重复和几何形状不明确的环境中的稳健定位技术方面的早期工作（Torres-Torriti等人，2022年；Bustos等人，2007年），以及Nazate-Burgos等人（2025-05-23）提出的初步结果。

下载：下载高分辨率图像（2MB）
下载：下载全尺寸图像

1.1. 贡献与范围
本工作为农业机器人技术做出了以下贡献：
(i) 当IMU集成不可行时，系统地评估SLAM性能：我们证明了为IMU辅助操作设计的方法在传感器不可靠时可能会崩溃，而更简单的方法在不同条件下的性能保持一致。我们将所提出的SLAM方法与A-LOAM（Qin Tong，2019年；Zhang等人，2014年）、LeGO-LOAM（Shan和Englot，2018年）、DLO（Chen等人，2022年）、MOLA（Blanco-Claraco，2025年）和AG-LOAM（Teng等人，2025年）进行了比较。在不同环境和三个真实世界数据集（CitrusFarm（Teng等人，2023年）、Bacchus长期数据集（BLT）（Polvara等人，2024年）以及作者收集的Pullally数据集（https://github.com/RAL-UC/Pullally_Dataset）中对这些方法的性能进行了评估。树状环境和平台的多样性有助于全面评估所提方法与基准算法（Jiang等人，2025年）的能力和局限性。
(ii) 将稳健的距离度量方法应用于农业环境：我们将改进的豪斯多夫距离（此前用于采矿和沿海导航）适应于树状SLAM的特定挑战，包括为树冠环境设计基于概率的高度过滤方法。
(iii) 在腿部机器人平台上的评估：与使用轮式机器人的现有农业SLAM基准测试不同，我们证明了在经历不连续运动动态的四足机器人上的可行性，这种运动动态使得标准运动模型假设不再成立。
(iv) 综合性能指标：我们报告了三个用于模拟和受控环境的地图指标，同时在所有数据集中评估了七个标准化的轨迹指标，提供了比典型的农业SLAM研究更全面的评估。
(v) 开源代码和数据集：我们发布了完整的代码实现以及包含四足机器人数据和不均匀地形的Pullally数据集：https://github.com/RAL-UC/RoSA_SLAM；https://github.com/RAL-UC/Pullally_Dataset。
需要注意的是，本工作关注的是基于IMU的方法不适用的场景。当有高质量的IMU数据和平滑的平台运动时，紧密耦合的激光雷达-惯性方法可能会表现出更好的性能。我们的贡献在于展示了在那些假设不成立的情况下，该方法仍能可靠运行。

所提出的SLAM方法被设计为一种基础导航能力，可以支持一系列下游任务。具体来说，配备了树状环境中可靠定位和地图构建功能的腿部机器人可以用于：(i) 通过沿树行携带多光谱或RGB-D相机进行表型和作物监测（Yandun Narvaez等人，2017年）；(ii) 通过导航到特定的地理参考位置进行精确喷洒或有针对性的除草（Calderara-Cea等人，2024年）；(iii) 通过沿预定轨迹绘制树冠体积来进行产量估算（Gené-Mola等人，2020年；Underwood等人，2016年）；以及(iv) 通过配备机械臂和相应的工具进行花朵疏剪、树枝修剪和精细水果采摘（Kefalas等人，2025年；Navone等人，2025年；Bhattarai等人，2024年）。腿部机器人特别适合这些任务，因为它们能够穿越果园地面常见的不平整且根部受损的地形，而这些地形会阻碍轮式平台的移动。

1.2. 与树状环境中的激光雷达SLAM相关的工作
对于树状农业环境中的SLAM的研究相对较新，因此与大量的室内和结构化城市环境中的SLAM方法相比，现有方法非常少（Torres-Torriti和Nazate-Burgos，2022年）。尽管视觉SLAM算法由于其适用于位置识别而在室内环境中很受欢迎，但由于它们对初始化、光照和距离的敏感性，单独使用时在支持自主导航方面并不可靠（Shan等人，2020年），因此本文的重点是基于激光雷达的SLAM方法。在农业树状环境中的自主导航方面，最早的实现之一是在Auat Cheein等人（2011年）提出的，他们使用了一种基于地标的SLAM算法（EIF-SLAM），该算法结合了2D激光雷达和图像处理来检测树干。自从Auat Cheein等人（2011年）首次将SLAM应用于农业果园以来，Aguiar等人（2022年）、Xia等人（2023年）提出了两种基于IMU辅助的3D激光雷达的SLAM方法用于棚架和果树果园，Nie等人（2022年）提出了一种无需IMU的方法。Aguiar等人（2022年）提出的VineSLAM方法从3D点云中提取点和半平面特征，并在6自由度（6-DOF）的平移和旋转空间中使用迭代最近点法（ICP）实现激光雷达里程计。VineSLAM使用粒子滤波器在3D体素图中跟踪点和平面特征，而不是2D网格图。VineSLAM在Aveleda葡萄园（葡萄牙）的测试中实现了0.4–1.6米的RMS定位误差，优于LeGO-LOAM（Shan和Englot，2018年），后者的均方根（RMS）定位误差为0.9–22.5米。Xia等人（2023年）提出的方法使用正态分布变换（NDT）进行3D点云匹配和里程计估计，但将3D激光雷达里程计与GNSS信号结合以初始化估计的姿态。结果表明，将激光雷达里程计与GNSS数据融合可以提高果园环境中的RTK GNSS定位精度。Nie等人（2022年）提出的无IMU方法也使用ICP进行3D点云匹配，但引入了基于密度的空间聚类算法（DBSCAN）将点云拟合到模型化为圆柱形的树干上。实验结果表明，Nie等人（2022年）提出的方法的定位误差与LOAM（Zhang等人，2014年）、LIO-SAM（Shan等人，2020年）和Cartographer（Hess等人，2016年）获得的定位误差相似，其RMS定位精度为0.15 ± 0.08米，而LOAM为0.13 ± 0.05米，Cartographer为0.19 ± 0.11米。LIO-SAM在5次测试中有4次未能收敛。

关于最初为室内环境设计的流行2D激光雷达SLAM方法，Li和Zhu（2024年）在模拟果园环境中以及使用Bacchus数据集（Polvara等人，2024年）的真实世界数据中测试了Hector SLAM（Kohlbrecher等人，2011年）、GMapping（Grisetti等人，2007年）和Cartographer（Hess等人，2016年）。使用Bacchus数据集获得的结果显示，这些方法的RMS定位误差分别为0.29米、0.15米和0.11米。需要注意的是，这些方法所得到的精度与基于3D激光雷达的方法相当，但需要轮式里程计或惯性测量单元（IMU）的测量数据，并且同样需要平滑的运动模型来过滤运动动力学和姿态估计。Jiang等人（2025年）在缺乏GNSS信号的温室种植环境中，对15种代表性的3D激光雷达SLAM方法进行了全面评估，这些方法包括仅使用激光雷达、激光雷达结合惯性测量单元以及激光雷达结合视觉和惯性测量的方法。研究结果表明，基于直接点云配准的方法优于基于特征提取的方法，并在几何重复性类似于本研究中考虑的林木行的挑战性环境中发现了姿态漂移和地图绘制失败的问题。早期的实时激光雷达SLAM系统，如LOAM（Zhang等人，2014年），通过边缘和平面特征匹配将高频率的里程计与低频率的地图绘制解耦，建立了基于特征的范式。尽管在结构化场景中有效，但这种方法可能对不稳定的几何特征敏感，并且在植被丰富的环境中容易累积漂移。LEGO-LOAM（Shan和Englot，2018年）通过引入地面分割和基于簇的噪声抑制来提高计算效率和鲁棒性，使得资源有限的地面车辆能够可靠运行。其二阶段优化利用地面约束来降低复杂性，但其对一致地面几何形状的依赖性可能限制了其在崎岖或带沟渠的农业地形以及密集种植的果园中的性能。最近的方法更倾向于使用密集扫描匹配策略以避免脆弱的特征提取。Chen等人（2022年）提出的直接激光雷达里程计（DLO）采用了一个完全密集的广义ICP前端，结合自适应关键帧和激进的数据结构重用，在资源受限的平台上实现了高精度和实时性能。然而，DLO缺乏处理动态植被或长期漂移的明确机制，而这在农业应用中是关键的。MOLA（Blanco-Claraco，2025年）提出了一个以视图为基础的地图表示和ICP内紧密耦合的速度估计为中心的模块化SLAM框架，实现了无需IMU的操作和对运动畸变的鲁棒性。虽然高度灵活并在多种数据集上得到验证，但其在重复性行作物环境中的性能仍需进一步研究。AG-LOAM（Teng等人，2025年）通过将密集的广义ICP（GICP）里程计与适应性的地图更新策略相结合，明确针对农业环境进行了设计，该策略在不稳定运动期间抑制不可靠的测量并过滤动态植被。在实际果园中的实验结果表明，其鲁棒性和精度优于通用SLAM系统，尽管没有实现闭环。

总体而言，文献表明从基于特征的方法明显转向了密集的、自适应的SLAM方法。对于农业环境而言，对几何重复性、植被动态和运动引起的畸变的鲁棒性变得比地图完整性或架构通用性更为重要。为了比较所提出方法的性能，选择了A-LOAM、LeGO-LOAM、DLO、MOLA和AG-LOAM进行评估。A-LOAM和LeGO-LOAM是广泛使用的SLAM算法，而DLO、MOLA和AG-LOAM是先进的算法，可能是无需IMU/GNSS的基于激光雷达的SLAM的更好选项。表1总结了所选的激光雷达里程计和SLAM基准方法的比较情况。另一方面，表2对其适用于农业应用的适宜性进行了定性评估。

表1. 滑激光雷达里程计和SLAM基准方法的比较总结。

方法 年份 扫描表示 配准策略 地图类型 闭环 IMU依赖性 地面分割 动态过滤 计算效率 农业验证
LOAM（Zhang等人，2014年） 2014 特征（边缘+平面） 点到线 & 点到平面（L–M） S2S + S2M（双频率） 全局体素化PC × 中等 ×
LeGO-LOAM（Shan和Englot，2018年） 2018 簇分割后的特征 两步解耦（L–M） S2S + S2M 全局体素化PC ～a 可选 ? ～b 高 ×
DLO（Chen等人，2022年） 2022 密集（体素降采样） GICP（NanoGICP） S2S + S2M（关键帧子图） 关键帧数据库 × 可选 × 非常高 ×
MOLA（Blanco-Claraco，2025年） 2025 密集（自适应体素） 速度积分ICP S2M（帧到地图） 基于视图（简单地图）+ 多层度量 ～d × 高 ～e
AG-LOAM（Teng等人，2025年） 2025 密集（自适应体素） GICP（帧到地图） S2M（级联流程） 适应性一致性地图 × ? 高 ?

符号说明：? 表示支持；～ 表示部分支持/可选；× 表示不支持。
缩写说明：S2S = 扫描到扫描；S2M = 扫描到地图；PC = 点云；L–M = Levenberg–Marquardt；GICP = 广义ICP。

A-LOAM的闭环模式使用基于ICP的扫描匹配；a表示缺乏基于描述符的检测器。
b基于簇的噪声抑制可以过滤小的孤立物体（例如草叶），但不明确跟踪或移除跨帧的动态植被。
cDLO是一个前端里程计系统；闭环被作者标记为未来的工作。
dMOLA支持GNSS引导的ICP闭环；基于拓扑描述符的检测被标记为未来的工作。
eMOLA在阿尔梅里亚森林序列（背包式，32束激光雷达）上进行了测试，这些序列具有与种植园农业相关的几何特性，但没有使用专门的农业基准。

表2. 滑激光雷达里程计和SLAM方法在农业应用中的适宜性评估。

| 方法 | 植被鲁棒性 | 不均匀地形处理 | 缺乏GNSS操作 | 嵌入式硬件适宜性 | 单激光雷达是否足够 | 主要农业限制 |
|---------|-----------|--------------|-------------|-----------------|-----------------|
| LOAM | 低 | 特征提取在植被不均匀时下降 | | 敏感于突然运动 | ? |
| LeGO-LOAM | 中等 | 簇分割过滤小植被但不过滤密集树冠 | | 两步优化假设地面平坦 | ? |
| DLO | 中等 | 密集匹配对特征一致性不太敏感 | | 无需IMU | ? |
| MOLA | 中等至高 | 速度积分ICP对剧烈运动鲁棒 | | 需要环境调整cth | |
| AG-LOAM | 高 | 自适应映射器过滤运动失真的点 | | 无需IMU | ? |

值得注意的是，这些方法与基于3D激光雷达的方法相比，能够达到可比的精度，但需要轮式里程计或IMU的测量数据，并且同样需要平滑的运动模型来处理运动 dynamics 和姿态估计。Jiang等人（2025年）在缺乏GNSS信号的温室种植环境中对15种代表性的3D激光雷达SLAM方法进行了综合评估，涵盖了仅使用激光雷达、结合激光雷达和惯性测量以及结合激光雷达、视觉和惯性测量的方法。研究发现，基于直接点云配准的方法优于基于特征提取的方法，并在几何重复性类似本文研究的林木行的挑战性环境中报告了姿态漂移和地图绘制失败问题。早期的实时激光雷达SLAM系统，如LOAM（Zhang等人，2014年），通过将高频率的里程计与低频率的地图绘制解耦来建立基于特征的范式。尽管在结构化场景中有效，但这种方法可能对不稳定的几何特征敏感，并且在植被丰富的环境中容易累积漂移。LEGO-LOAM（Shan和Englot，2018年）通过引入地面分割和基于簇的噪声抑制提高了计算效率和鲁棒性，使得资源有限的地面车辆能够可靠运行。其两阶段优化利用地面约束来降低复杂性，但对一致地面几何形状的依赖性可能限制其在粗糙或带沟渠的农业地形以及密集种植的果园中的性能。最近的方法更倾向于使用密集扫描匹配策略以避免脆弱特征提取。Chen等人（2022年）提出的直接激光雷达里程计（DLO）采用了一个完全密集的基于广义ICP的前端，结合自适应的关键帧和激进的数据结构重用，在资源受限的平台上实现了高精度和实时性能。然而，DLO缺乏处理动态植被或长期漂移的明确机制，而这些在农业应用中至关重要。MOLA（Blanco-Claraco，2025年）提出了一个以视图为基础的地图表示为中心的模块化SLAM框架，并在ICP内紧密耦合速度估计，实现了无需IMU的操作和对运动畸变的鲁棒性。尽管在多种数据集上得到了验证，但在重复性行作物环境中的性能仍需进一步研究。AG-LOAM（Teng等人，2025年）通过将密集的广义ICP里程计与适应性地图更新策略相结合，明确针对农业环境进行了设计，该策略在不稳定运动期间抑制不可靠的测量并过滤动态植被。在真实果园中的实验结果表明，其鲁棒性和精度优于通用SLAM系统，尽管没有实现闭环。

总体而言，文献表明从基于特征的方法明显转向了密集的、自适应的SLAM方法。对于农业环境而言，对几何重复性、植被动态和运动引起的畸变的鲁棒性比地图完整性或架构通用性更为重要。选择A-LOAM、LeGO-LOAM、DLO、MOLA和AG-LOAM进行性能比较评估。A-LOAM和LeGO-LOAM是广泛使用的SLAM算法，而DLO、MOLA和AG-LOAM是先进的算法，可能是无需IMU/GNSS的基于激光雷达的SLAM的更好选择。表1总结了所选的激光雷达里程计和SLAM基准方法的比较情况。表2则对所选基准方法在农业应用中的适宜性进行了定性评估。

**所提出方法的概述**
所提出的方法如图2的块图所示，包括三个主要阶段：3D扫描处理和概率投影、扫描匹配以及状态和地图更新。该方法从第一个激光雷达扫描Z0开始初始化，该扫描存储在世界坐标系F0中，同时也与全局地图坐标系M0相关联。后续在传感器坐标系Fs中的扫描必须与之前的扫描进行匹配，以获得机器人平台的相对位移和旋转变化，这些变化在图中用Δxt表示。机器人姿态的相对变化用于更新状态估计xt和地图，同时更新与扫描匹配后检测到的新占用区域相对应的地图增量ΔMt。该过程以迭代增量方式进行，因此z?1块分别表示上一次迭代的机器人状态和环境地图的记忆存储xt?1和Mt?1。以下小节将详细说明所提出的SLAM方法在没有GNSS和IMU测量的树木环境中的每个主要阶段。图3的流程图提供了SLAM过程的完整端到端视图，补充了第2.1节中的数学描述，包括3D扫描处理、使用修改后的Hausdorff距离进行扫描到地图匹配的定位、EKF状态预测、扫描到地图的Hausdorff距离最小化、EKF测量更新和占用网格地图更新，以及收敛和初始化的决策分支。

**2.1 扫描处理**
3D激光雷达测量路径上最近物体的距离ri,js=r(θi,?k)s，其中θi=i?Δθs，i=1,2,…,Nθ，?j=i?Δ?s，j=1,2,…,N?，这些距离是相对于传感器参考坐标系Fs的平面（通常与地面平行）的。Δθs和Δ?s分别是激光雷达传感器的方位角和高程角分辨率，Nθ和N?分别定义了相应的样本数量。因此，时间t的3D激光雷达扫描是一组极坐标测量值：(1)Zts=zt,i,js=defrt,i,js,θt,is,?t,js。极坐标zt,i,js可以在传感器参考坐标系Fs的笛卡尔坐标zt,i,js=rt,i,js,yt,is,zt,js中表示为：zt,i,js=rt,i,jscos(θt,is)cos(?t,js),yt,i,js=rt,i,jssin(θt,is)cos(?t,js),zt,i,js=rt,i,jssin(?t,js)。所提出方法的一个关键步骤是将3D扫描切片（1）的概率投影到地面平面上，如图4所示，该图显示了不同树木上的3D点被丢弃或在传感器坐标系Xs和Ys的2D平面上投影，通过将测量值的垂直坐标zs设置为零，即zs=0。例如，图4中的点1、2、3被投影到地面平面作为三个不同的测量值，而点5和6以及点8和9则因为它们在树干上的位置相似而被投影为单个点5和8，从而增强了对该位置存在树木的信心。相比之下，像点4这样的远距离点或树冠高处的点根据测量概率被丢弃，不会被投影到地面平面上。

所提出方法的一个关键步骤是将3D扫描片段的概率投影到地面平面上，如图4所示，这表明不同树木上的3D点被丢弃或投影到传感器坐标系轴的2D平面上。测量概率p(Zts|xt,Mt)可以理解为在当前机器人状态xt和地图Mt信息给定的情况下测量的正确性（Mutz等人，2021年）。换句话说，如果机器人真实位置不是xt，或者现实世界包含与地图Mt中不同的元素，那么观测值真正对应于假设状态和地图的观测值的可能性就较低。为了保持计算的可行性，采用网格地图的SLAM策略通常假设地图的单元是独立的（Siciliano和Khatib，2008年），因此给定xt和Mt的测量值zt,i,js∈Zts的条件概率可以重写为对应单元上测量的条件概率的乘积：(2)p(Zts|xt,Mt)=∏zt,i,js∈Zts,i=1,…,Nθ,j=1,…,N?p(zt,i,js|xt,Mt)，其中每个测量的概率可以计算为激光束位于最近物体（例如树干）一定距离的条件概率pd和ph（下面定义）：p(zt,i,js|xt,Mt)=pd·DTsw(xt,i,js,yt,i,js;xt),Mt|xt,Mt)。D:R2→R+表示地图Mt中占用区域的距离变换，定义为：D(p,Mt)=defDMt(p)=minm∈?Mt‖p?m‖1，对于所有p∈R2，Tsw∈SE(2)是将测量坐标(xs,ys)从传感器坐标系转换为世界坐标系(xw,yw)的2D变换，可以用齐次变换矩阵T=[R∈SO(2)|t∈R2]=defR(θt)|pt表示，如下所示：(3)Tsw(xs,ys;xt)=(xs,ys)→(xw,yw)=Txsys1T=R(θt)xsysT+pt，其中R(θ)=cos(θ)?sin(θ)sin(θ)cos(θ)是2D旋转矩阵。距离变换D(p,Mt)在2D平面上形成一个标量场，其中每个值代表到地图Mt中对应环境对象的边界?Mt上最近点的距离。如果平均树冠高度为z?，那么累积分布F(z?,λ)=1?e?λz?=0.5，这意味着λ=ln(2)/z?。为了排除可靠性较低的测量点zts，根据以下指示函数对测量值应用一个阈值：(6)1pd(?)>βdph(?)>βh(zts)=1，如果pdDTsw(xts,yts;xt)，Mt|xt,Mt≥βd且phzt,i,js|xt,Mt≥βh，否则为0，其中βd的值由以下公式给出：βd=e?1/(2πσs2)≈1/4σs，βh的值由以下公式给出：βh=ln(2)/z?e?ln(2)/z?，z?=1/(2ln(2)z?)。βd的值根据范围测量的分布（4）设定，以便选中的距离测量值在传感器范围误差的±1σ范围内，即选择最接近预期零均值误差的65%的测量值。βh的值根据高度测量的分布（5）计算，以便在高度低于平均树冠高度z?时选择测量值。最后，使用指示函数（6）从3D测量值生成给定机器人姿态xt的世界坐标中的2D测量值集合，并丢弃高度信息zts：(7)Zˉtw(xt)=(xtw,ytw)=(xtw,ytw)=Tsw(xs,ys;xt)，?zts=(xs,ys,zs)∈Zts，使得1pd(?)>βdph(?)>βh(zts)=1。2.2. 通过修改后的Hausdorff距离进行扫描到地图匹配的定位为了解决机器人姿态问题并找到xt=(xt,yt,θ)，通过最小化测量值Zˉtw(xt)与地图Mt之间的修改后的Hausdorff距离（Torres-Torriti等人，2022年；Donoso-Aguirre等人，2008年）来解决扫描到地图匹配问题（Li等人，2025年；Torres-Torriti和Nazate-Burgos，2022年）。这种技术适用于非结构化环境，因为它不需要从原始测量值中提取几何特征，也不需要解决特征到地标的数据关联问题。机器人定位问题可以表述为找到平移Δpt和旋转Δθt来更新机器人的位置pt?1及其姿态θt?1，使得当前测量值Zts与之前的测量值Zt?1s或地图Mt?1一致。正式地，估计的姿态向量x?t?是使得地图Mt?1中的物体边界?Mt?1与从激光雷达测量值Zˉtw(xt)获得的物体边界的观察位置之间的匹配误差最小化的向量（见公式（7）：(8)x?t?=(x?t?,y?t?,θ?t?)=argminx?=(x?,y?,θ?)h?K(Mt?1,Zˉtw(x?))，其中，对于两组点ai∈A，i=1,2,…,|A|和bj∈B，j=1,2,…,|B|：h?K(A,B)=1/K∑i=|B||B|?Khi(A,Bi)，是所谓的修改后的Hausdorff距离，hi(A,B)是第i个部分（定向）Hausdorff距离：(9)hi(A,Bi)=defmaxb∈Bimina∈A‖b?a‖1，(10)=maxb∈BiD(b,A)，Bi是去掉前i-1个点的集合B，这些点已经计算了定向Hausdorff距离。更具体地说，如果B1=B，则Bi+1=Bi?bi?，其中bi?=argmaxb∈BiD(b,A)。由于hi(A,Bi)≥hi(Bj)对于i该研究中的小跑步态的占空比为0.5，相位偏移也为0.5。这里的占空比指的是接触阶段持续时间与步态总持续时间的比例，相位偏移则是相对于前后左右腿相应阶段的相对位置。步态周期被设置为每秒一步。在这种步速下，腿部与地面的间歇性接触是导致惯性测量单元（IMU）测量数据失真的主要因素，这一点在第1章“引言”和第3.5章“真实果园中的实验评估”中有详细讨论。充满电的电池标称使用寿命为4小时。在我们的实验中，考虑到机器人的运动轨迹和地形条件，电池大约可以使用2小时，因此我们每1.5小时更换一次备用电池。外部GNSS RTK系统和激光雷达所需能源由单独的电池提供，以增加整个系统的自主运行时间。实验过程中未使用机器人的内部计算机，而是采用了一台配备Intel? Core? i7 3.8 GHz处理器（12核）的外部笔记本电脑，该电脑依靠自身电源运行，并安装了Ubuntu 18.04 64位操作系统及ROS Melodic软件框架。

关于传感器，这款四足机器人配备了Velodyne VLP-16激光雷达和ArduSimple RTK3B GNSS系统（见图6(d)）。Velodyne VLP-16激光雷达拥有16个近红外通道（波长903纳米），垂直视场角为30°（水平面以上/以下±15°），垂直分辨率为2.0°；水平视场角为360°，水平分辨率可在0.1°到0.4°之间调节。其最大探测距离为100米，距离精度为±3厘米。根据水平分辨率设置的不同，旋转速率介于4到20赫兹之间，每秒可产生约300,000个数据点，平均功耗约为8瓦。激光雷达被设置为0.4°的水平分辨率进行扫描，每0.25秒（即4赫兹）完成一次360°的全范围扫描。ArduSimple RTK3BPro GNSS基于Septentrio? Mosaic-X5接收芯片，用于以厘米级精度进行定位测量，采样率为1赫兹。GNSS系统在基站和移动接收端均使用了校准过的Survey GNSS Tripleband+L-band天线。

在性能评估方面，机器人的姿态估计和环境地图的构建是相互关联的。因此，与大多数仅报告轨迹估计精度指标的研究不同，本文同时报告了轨迹估计误差和地图精度。评估结果采用了Kümmerle等人（2009年）、Zi等人（2020年）以及Sturm等人（2012年）提出的指标，这些指标考虑了平移和旋转方向的绝对轨迹误差（ATE）和相对位移/姿态误差（RPE），同时也采用了常见的均方根（RMS）定位误差指标（Li和Zhu，2024年）。使用标准化指标的重要性在于确保所呈现的结果能够与其他SLAM算法或在不同数据集上使用相同算法得到的结果进行比较。这些指标是通过Python工具包evo（Grupp，2017年）实现的，用于评估和比较基于里程计和SLAM算法得到的轨迹。地图质量通过精确度和灵敏度指标来评估，这些指标比传统的最近邻平均距离（ADDN）指标更能准确反映地图的正确性和完整性（Zi等人，2020年）。每个性能指标的数学定义见附录。

为了验证所提出的SLAM算法的性能，首先在模拟的树丛环境中进行了测试。模拟测试结果见附录。

在真实树丛环境中的实验验证之前，研究人员在了一个设有标记锥体的受控室外环境中对该方法进行了评估，这些锥体代表了树木的“树干”，如图6所示。实验使用了配备Velodyne VLP-16激光雷达和ArduSimple RTK3B GNSS系统的Unitree Go1四足机器人（见图6(d)），以及运行Ubuntu 18.04 64位操作系统和ROS Melodic的笔记本电脑。GNSS系统使用Septentrio? Mosaic-X5接收芯片，能够以厘米级精度进行定位测量。机器人被编程为沿着6 × 2米的矩形路径移动。这条16米长的路径被机器人重复走了七次，以构建受控环境数据集并获取统计置信区间（Ne=7）。该方法在第2节中有详细描述，其性能评估采用了第3.2节中提出的指标。为了对比，还评估了A-LOAM、LeGO-LOAM、DLO、MOLA和AG-LOAM算法，这些算法在实验中没有使用惯性测量单元（IMU）或GNSS测量数据。

**下载：** 高清晰度图片（2MB）
**下载：** 完整尺寸图片

**图6. 受控场地环境实验中的机器人和交通锥体：**
(a) 实验的模拟视图；
(b) 场地上交通锥体的俯视图；
(c) 四足机器人的正面视图；
(d) 受控场地实验；
(e) 交通锥体和测试矩形参考路径的透视视图。

**3.4.1 受控环境下的基线性能**
实验结果见表3、表4、图7和图8。与A-LOAM、DLO和MOLA相比，所提出的方法在参考全局位置时，定位和角度误差显著更低，表明其精度更高且变异性更小。与LeGO-LOAM和AG-LOAM相比，该方法产生的定位和角度误差也稍小。所有方法在定位位移误差方面表现相当，误差约为1厘米，但DLO和MOLA除外。这表明它们在估计相对运动方面的能力相当。在某些扫描中，DLO和MOLA未能正确处理交通锥体的测量点云数据，这可能是导致它们在矩形路径的第一个转弯后开始偏离的原因（见图7(c)、图7(d)、图8(c)和图8(d)）。尽管A-LOAM的位移误差与LeGO-LOAM和所提方法相当，但其角度位移误差却是后者的20多倍，这导致A-LOAM在方向上产生累积误差（见图7(a)和图8(a)），从而生成了扭曲的地图。LeGO-LOAM和AG-LOAM的轨迹估计在多次运行中相对一致，尽管在某些运行中会出现错误的方向估计，尤其是在第二个转弯处（见图7(b)和图7(e)）。错误的方向估计会导致地图不完整或产生误判，尽管LeGO-LOAM和AG-LOAM仍能校正机器人的位置（见图8(b)和图8(e)）。相比之下，所提出的方法在多次运行中轨迹估计结果一致，且估计地图与真实地图的相似度很高（见图7(f)和图8(f)）。所提方法的地图估计性能最高，精度超过92%，灵敏度超过80%（见表4）。LeGO-LOAM和AG-LOAM的精度和灵敏度排名第二，介于10%到25%之间，而A-LOAM、DLO和MOLA的精度低于1%。在这种情况下，由于将单元格简单地分类为“占用”或“自由”会导致精度指标失效，因为它们只是预测了占多数的人类活动区域。不过从相对角度来看，所提方法的准确率也高于其他方法。

**表3. 受控室外环境中的姿态估计误差**
| 方法 | 位置误差（米） | 角度误差（弧度） |
|---------------------------------|-----------------|-------------------|
| 所提方法 | 0.079 ± 0.009 | 0.917 ± 0.137 |
| A-LOAM | 0.097 ± 0.025 | 0.559 |
| LeGO-LOAM | 1.682 ± 0.165 | 0.190 ± 0.053 |
| MOLA | 0.439 ± 0.064 | 1.306 ± 0.106 |
| AG-LOAM | 1.897 ± 0.132 | |

**表4. 控制室外环境中的地图估计性能**
| 方法 | 精确度（%） | 灵敏度（%） |
|---------------------------------|-----------------|-------------------|
| 所提方法 | 92.3 ± 0.029 | 80.0 |
| A-LOAM | 82.1 ± 0.029 | 77.7 |
| LeGO-LOAM | 82.1 ± 0.015 | 78.3 |
| DLO | 81.0 ± 0.013 | 81.0 |
| MOLA | 80.5 ± 0.015 | 77.5 |
| AG-LOAM | 80.6 ± 0.018 | 77.2 |

**图7. A-LOAM、LeGO-LOAM、DLO、MOLA和所提方法在受控室外环境中的估计轨迹**
**图8. A-LOAM、LeGO-LOAM、DLO、MOLA和所提方法在受控室外环境中的最终地图**
地图分辨率为0.01米/像素。

**3.5 真实果园中的实验评估**
所提出的方法使用了三个真实果园环境中的数据集进行评估：两个公开可用的数据集CitrusFarm（Teng等人，2023年）和Bacchus（Polvara等人，2024年），以及作者收集的Pullally数据集（https://github.com/RAL-UC/Pullally_Dataset）。选择数据集的关键标准是地形的复杂性，这种不规则地形对机器人导航和环境感知构成了挑战，因为它会干扰稳定的运动和精确的感知。正如Li和Zhu（2024年）的模拟研究所示，地形粗糙度会妨碍移动平台的平稳控制，并在传感器数据中引入噪声，从而影响地图绘制、物体检测和空间理解。下面总结了每个数据集的主要特征。

与控制环境实验不同，在控制环境实验中，由于锥体的位置事先已知，因此可以直接构建真实地图；而三个真实世界数据集（CitrusFarm、Bacchus、Pullally）均没有这样的参考地图来评估精度、灵敏度和准确性。虽然RTK-GNSS轨迹数据可用并作为姿态评估的基准，但要生成果园的精确2D占用地图需要专门的、高精度的测量任务（例如地面激光扫描或机载激光雷达），而这并非原始数据集采集的一部分。未来工作计划中计划开展针对真实果树园区的参考地图生成工作。

**3.5.1 数据集描述**
CitrusFarm数据集是在加州大学河滨分校的农业试验站使用Clearpath Jackal轮式移动机器人采集的（Teng等人，2023年）。该机器人配备了Velodyne VLP-16 3D激光雷达，具有360°的水平视场角（FOV）和0.2°的角分辨率，30°的垂直视场角和2°的角分辨率，最大探测距离为100米，采样率为0.1赫兹。SwiftNav Duro GPS-RTK系统提供了小于0.02米的空间定位误差（采样率0.1赫兹）。机器人还配备了普通单色相机、立体RGB相机、红绿近红外相机、热成像相机以及轮式里程计和IMU传感器，但这些传感器在本次比较中并未使用。数据集包含了三个柑橘果园中不同生长阶段的树木的7.5公里轨迹，总数据量为1.3TB。本研究使用了第05序列，其中包含了沿7排柑橘树行驶的865.33米轨迹。这段序列之所以被选中，是因为其具有具有挑战性的条件，包括树木密度高和不平坦的地形，这些条件非常适合用来对提出的算法A-LOAM、LeGO-LOAM、DLO、MOLA和AG-LOAM进行基准测试。

**Bacchus数据集：**Bacchus数据集是在希腊Epanomi的Ktima Gerovassiliou葡萄园收集的，使用了Saga Robotics的Thorvald II移动机器人，该机器人配备了四轮驱动和转向（4WD4S）系统（Polvara等人，2024年）。该平台配备了Ouster OS1-16三维激光雷达（360°水平视场角，32°垂直视场角，120米探测距离，10赫兹的更新频率），并且ground-truth定位由Trimble BX992 RTK-GNSS系统提供，精度达到厘米级别。机器人还配备了惯性测量单元（IMU）、2D激光雷达和RGB-D相机，但这些数据并未用于提出算法的基准测试。该数据集包含了持续数月的长期数据采集活动，以捕捉作物的生长情况。数据集包括了沿着五个相邻葡萄园走廊的轨迹。本研究中使用了2022年6月8日的采集序列，路径为B→G→F→A→B，长度为102.3米。这个序列记录于晚春时期，此时葡萄藤的叶子非常茂密，形成了类似墙状的走廊，由于植物的均匀性而没有明显的几何特征，因此对精确定位造成了困难。与Citrus数据集类似，不平坦的地形也会降低激光雷达的匹配精度，从而影响SLAM算法的性能。

**Pullally数据集：**该数据集由作者在智利的Valparaiso的Pullally实验果园收集，使用的机器人是Unitree Go1四足机器人。与Citrus数据集中的机器人类似，该机器人也配备了Velodyne VLP-16激光雷达和ArduSimple RTK3BPro GNSS-RTK系统，以及校准过的Survey GNSS Tripleband+L-band天线。Ground-truth位置信息以1赫兹的频率记录，精度达到亚厘米级别。机器人沿着混合种类的果园（包含橙子、柠檬、李子和桃子树）中的预定义轨迹移动。记录是在冬季进行的，在此期间柑橘树仍然有叶子，而其他树木的叶子已经脱落。为了获得统计置信区间，该轨迹被重复了三次。图9(a)展示了用于采集活动的果园区域的卫星图像，其中机器人参考路径用红色标出。该数据集的主要挑战包括树木高度不一、叶片覆盖情况复杂以及地形极不规则。这个数据集的一个特殊特点是，由于传感器安装在有腿的机器人上，因此激光雷达点云的位置连续性较差。与轮式机器人不同，有腿的机器人自然会产生不连续的腿-地面接触力，这些力会传递到机器人主体，从而干扰IMU的测量结果，即使使用高精度的IMU也无法准确生成惯性里程计数据。这一点，加上密集树冠导致的GNSS信号丢失，激发了在这种树木环境中进行定位的新方法。图9(b)展示了基于GNSS获得的机器人姿态重建的果园3D模型，而图9(c)展示了使用所提出方法估计的姿态重建的3D模型。需要注意的是，尽管所提出的方法基于将3D激光雷达扫描数据通过概率投影到2D平面并进行匹配来减少数据量，但由于定位精度较高，因此从估计的姿态重建的地图仍然能够充分保留树冠的3D细节，具体细节将在结果讨论部分进一步说明。

考虑到的数据集的主要特征总结在表5中。需要注意的是，每个数据集使用的激光雷达都是广泛采用的型号，具有相似的特性。然而，平台类型、运动平顺性和果园植物种类差异较大，因此有助于在不同真实世界条件下进行全面评估。为了便于比较，所有地图的显示分辨率为0.01米/像素。考虑到不同数据集使用的激光雷达精度约为±0.03米，这个分辨率是合理的。

**性能评估：**表6、表7和表8展示了将A-LOAM、LeGO-LOAM、DLO、MOLA和AG-LOAM应用于Citrus、Bacchus和Pullally数据集时得到的姿态估计指标的比较。图10、图11、图12和图13分别展示了这三个数据集的估计轨迹和地图的图形对比。结果表明，所提出的方法在不同果园环境中的平均姿态估计精度较高。不同数据集的位置估计误差均值范围在0.40米到1.34米之间，而方向误差均值范围在0.05弧度到0.67弧度（2.86°–38.39°）之间。方向误差的较大差异不仅归因于环境特征和 Platform 的运动平顺性（例如Bacchus数据集中树冠的均匀性使得点云匹配更加困难），还与轨迹的转弯次数以及每次转弯时机器人方向估计错误的概率有关（例如Pullally数据集中的之字形轨迹）。所有方法的平均位置和角度位移误差相对较低，但平均位移误差可能无法准确反映累积误差对最终姿态估计的影响。特别是ep%指标考虑了轨迹的长度，从而能够公平地比较不同长度路径上的累积误差。在所有数据集中，所提出的方法在路径长度上的位置误差和终点估计误差都较低。因此，这些结果证实了所提出的方法能够在不同果园环境中稳健地解决定位问题，并且不受轨迹长度或形状的影响。

结果还表明，Pullally数据集最具挑战性。A-LOAM、DLO和MOLA在所有运行中都无法正确估计每行结束处的机器人转弯方向（见图12(a)、12(c)和12(d)）。另一方面，LeGO-LOAM仅在三个轨迹中的一个上正确估计了转弯方向。LeGO-LOAM在两个情况下仅能正确重建第一行的轨迹（见图12(b)），但在第二行时，小的方向估计误差开始累积。在穿过果园的第三行之前，LeGO-LOAM在第二转弯时产生了相当大的角度估计误差（见图12(b)）。AG-LOAM在三个运行中有两个表现良好（见图12(e)）。然而，在第二次运行中，错误的方向估计导致AG-LOAM偏离了正确的路径。与其他方法相比，结果表明AG-LOAM虽然能够恢复方向，但仍然存在一些累积误差，使其无法完成正确的循环闭合。表9总结了每次扫描的平均总处理时间和最大计算帧率。总处理时间包括SLAM算法的不同步骤，如数据预处理、特征提取或异常值移除、扫描到地图的匹配、里程计计算和映射等。整个计算周期的耗时是通过在新激光雷达扫描之前和扫描处理流程结束后读取高精度CPU时钟来测量的。每次循环的初始和最终时钟读数之间的差异被平均以计算每次扫描的平均总处理时间。最大帧率是每次扫描处理周期最短时间的倒数。需要注意的是，这些数据无法直接与原始作者的工作结果进行比较，因为它们是在不同的硬件和数据集上测量的，而且某些差异可能取决于点云配准时考虑的点密度等特定参数设置。不过，这些数值提供了实时应用这些策略的量级指标和参考依据。实时处理需求还将取决于特定应用场景，例如机器人平台的预期运行速度。所提出的方法在没有任何特殊代码优化的情况下，每次扫描的处理时间几乎是A-LOAM和LeGO-LOAM的两倍，是AG-LOAM的五倍。目前正在进行的工作旨在优化代码，以提高扫描处理速率。

**结论：****讨论**

第3节中呈现的实验结果允许我们全面评估所提出的方法与五种基准方法（A-LOAM、LeGO-LOAM、DLO、MOLA和AG-LOAM）在四个逐渐具有挑战性的测试环境中的表现：一个有交通锥的受控户外环境，以及三个真实的果园数据集（CitrusFarm、Bacchus和Pullally）。以下小节总结了主要发现，确定了每种方法表现最佳的条件，并讨论了准确性、鲁棒性和平台通用性之间的权衡。

**4.1. 受控环境中的性能**

受控户外环境包括在平坦地形上围绕交通锥的简单矩形轨迹，这为在所有方法都不受不平地形或变化植被干扰的情况下比较基线定位准确性提供了最清晰的视角。所提出的方法在所有方法中实现了最低的平均位置误差（e?p=0.079±0.009米）和最低的RMS位置误差（eRMSp=0.092±0.011米）（见表3）。LeGO-LOAM和AG-LOAM是最接近的竞争对手，其RMS位置误差分别为0.156米和0.394米，而A-LOAM（eRMSp=1.289米）、MOLA（2.430米）和DLO（6.400米）的误差明显更大。DLO和MOLA在这个环境中的困难是值得注意的。DLO依赖于密集的3D点云匹配，在缺乏足够显著几何特征的情况下（交通锥是小型、稀疏的目标），匹配会退化并导致较大的位置漂移（ep%=41.08%）。MOLA的速度积分ICP虽然通常对剧烈的平台运动具有鲁棒性，但在这里也表现不佳，产生ep%=14.24%。这两种方法最初主要是为丰富、特征密集的户外或森林环境设计和验证的，而受控实验中稀疏的锥体几何结构对于基于特征的3D匹配来说是不利的。相比之下，所提出的方法的2D投影和Hausdorff距离匹配非常适合紧凑、垂直延伸的目标，即使这些目标很稀疏，因为投影将相关的结构信息集中在一个稳定的2D点集中。

AG-LOAM的性能相对较好（eRMSp=0.394米），得益于其在机器人腿部运动周期中拒绝运动扭曲点的自适应地图更新机制。LeGO-LOAM实现了有竞争力的平均误差，但在不同运行中的方差较大（CRMSp=0.054米），反映了其对不平表面地面分割质量的敏感性。所提出的方法在所有七次重复实验中都表现出最高的一致性（见图7），证实了即使在腿部机器人脉冲式步态带来的挑战性条件下，概率性2D投影和修改后的Hausdorff距离匹配也能提供稳定且可重复的定位解决方案。

**4.2. 在真实果园环境中的性能**

**4.2.1. CitrusFarm数据集**

在CitrusFarm数据集上，使用轮式机器人在相对平滑的地形上收集数据，A-LOAM（eRMSp=0.282米）、LeGO-LOAM（0.160米）、DLO（0.407米）和AG-LOAM（0.360米）都实现了亚米的RMS位置误差（见表6），而所提出的方法记录了更高的误差1.725米。这一模式与预期一致：在平滑的轮式平台上，A-LOAM、LeGO-LOAM和DLO的3D匹配流程受益于连续的、无畸变的点云，允许进行精确的特征提取和全3D匹配。所提出的方法故意丢弃了垂直维度，仅保留2D树干级别的投影，因此在3D信息既丰富又可靠的情况下，必然会牺牲一些准确性以换取鲁棒性。

MOLA是基准方法中的一个显著例外，在CitrusFarm上记录了灾难性的RMS误差33.252米。这种失败归因于其恒定速度假设被CitrusFarm轨迹中的长直线段和突然转弯所违反，这些情况产生了与其运动模型不兼容的大速度不连续性。非常高的端点误差（eend=66.203米）证实了MOLA在这个轨迹上发散，而不仅仅是累积了缓慢的漂移。

在CitrusFarm上成功的基准方法中，DLO和AG-LOAM在RMS误差方面与A-LOAM相当，但累积误差较大（DLO：0.088%；AG-LOAM：0.078% vs. A-LOAM：0.058% 和 LeGO-LOAM：0.034%），表明LeGO-LOAM在这种环境下提供了最佳的漂移控制。所提出的方法在所有七次重复实验中表现最一致（见图10），证实了概率性2D投影和修改后的Hausdorff距离匹配即使在腿部机器人脉冲式步态带来的挑战性条件下也能提供稳定且可重复的定位解决方案。

**4.2.2. Bacchus数据集**

Bacchus葡萄园数据集的特点是均匀的棚架冠层形成了长长的无特征走廊，这对基于特征的3D SLAM来说是最具挑战性的环境。走廊几何结构的退化导致所有3D基准方法都产生了较大的误差：DLO（eRMSp=35.408米）、MOLA（36.122米）和AG-LOAM（33.861米）都无法在葡萄园小径上保持定位。LeGO-LOAM也大幅偏离（eRMSp=5.649米），甚至在CitrusFarm上表现良好的A-LOAM也显著下降（eRMSp=2.113米）。所提出的方法是唯一一个实现了亚米RMS误差（eRMSp=1.126米）和最小路径长度位置误差（ep%=0.930%）的方法，以及最小的端点误差（eend=0.564米 vs. A-LOAM的1.236米，见表7）。

DLO、MOLA和AG-LOAM在Bacchus上的退化值得特别讨论。DLO的密集匹配依赖于足够的3D几何变化；晚春时葡萄园棚架的平面墙状结构和密集的植被几乎不提供显著的几何特征，导致扫描匹配退化。同样，MOLA的基于ICP的方法，即使增加了速度积分，也无法补偿重复走廊几何中缺乏明显地标的问题。AG-LOAM的自适应映射器在特征稀少的环境中会拒绝运动扭曲的点，进一步加剧了问题：在特征稀少的环境中，已经有限的明显点集通过自适应拒绝步骤进一步减少，导致可靠的姿态估计不足。所提出的方法在Bacchus上的鲁棒性直接来自其使用2D树干投影：即使在棚架葡萄园中，地面水平的葡萄杆和树干基部也能产生一个独特的、可重复的2D点模式，这比基准方法使用的3D植被返回更具有区分性。

**4.2.3. Pullally数据集**

Pullally果园数据集使用Unitree Go1四足机器人在高度不平的地形上收集数据，跨越了多种树种的果园行，并进行了三次重复实验，揭示了所提出方法与所有基准方法之间的最大差异。如表8所示，所提出的方法在所有三次实验中都表现一致（eRMSp=0.470±0.064米），而A-LOAM表现糟糕（27.490±5.644米），DLO也大幅偏离（17.729±1.783米），MOLA也失败（21.446±3.598米）。LeGO-LOAM和AG-LOAM表现中等：LeGO-LOAM的平均RMS误差为4.141米，但方差很大（±4.027米），在三次实验中有一次成功，但在另外两次中偏离，这可能是由于其依赖于适当的地面分割；AG-LOAM的表现比LeGO-LOAM更一致，但仍记录了较大的eRMSp=2.123±2.513米和较大的运行间变异性。

A-LOAM在Pullally数据集上的失败可以解释为其严重依赖于IMU积分进行点云畸变校正：机器人小跑步态产生的脉冲式加速度导致激光雷达扫描不连续，使得每次转弯时的扫描匹配偏离。DLO尽管不使用IMU积分，但依赖于密集的3D匹配，同样容易受到腿部步态引入的扫描间不连续性的影响。MOLA的恒定速度模型在连续平台上可以有效平滑运动，但直接被四足运动的脉冲性质所违反，导致每次扫描的ICP初始对齐估计不佳。AG-LOAM的运动稳定性标准在穿越崎岖地形时抑制了地图更新，部分减轻了腿部步态的影响，解释了其相对于A-LOAM、DLO和MOLA更好的性能（eRMSp=2.123米）。然而，较大的置信区间（±2.513米）表明AG-LOAM的性能在重复实验中并不可靠，可能是因为其地图更新抑制逻辑是为轮式崎岖地形平台校准的，对于四足机器人更快的步态周期来说过于保守，间歇性地导致地图更新不足，从而在转弯时引起定位漂移。

所提出的方法对所有这些失败模式都具有鲁棒性，因为它不依赖于3D特征提取、点云畸变校正或扫描间的连续性。2D地面投影自然过滤掉了腿部步态引入的垂直扰动，修改后的Hausdorff距离对异常值的鲁棒性吸收了由平台滚动和俯仰引起的投影扫描中的残余错位。结果得到的ep%=0.268±0.033%和eend=0.276±0.127米证实了在整个150米轨迹和所有三次重复实验中漂移得到了很好的控制。

**4.3. 在不同平台和环境中的性能比较**

综合来看，所有四个实验环境的结果揭示了一个清晰且一致的模式。所提出的方法是唯一一个在所有评估的环境和平台上都实现了有竞争力或最佳性能的方法。相比之下，五种基准方法在特定条件下表现优异，但在其他条件下则显著下降，如下所述。

A-LOAM和LeGO-LOAM在结构化的平滑轮式平台上（CitrusFarm）表现最佳，那里连续的点云采集和可靠的IMU辅助畸变校正实现了精确的3D特征匹配。这两种方法在Bacchus葡萄园棚架上表现显著下降，在Pullally腿部机器人数据集中也失败。DLO对平台连续性和特征可用性同样敏感：在CitrusFarm上具有竞争力，但在Bacchus和Pullally上崩溃。MOLA在CitrusFarm的直线-转弯轨迹上失败，原因是速度模型被违反，在Pullally上则是由于腿部步态的不连续性；AG-LOAM在腿部机器人Pullally数据集上的表现比LeGO-LOAM更一致，但仍然记录了较大的方差。

所提出的方法在CitrusFarm数据集上显示出了其主要限制，即在2D投影上进行了降维，牺牲了在平滑轮式平台上通过全3D匹配可实现的准确性。然而，这种权衡是有意且可量化的：CitrusFarm的RMS误差为1.725米，这是为了在Pullally腿部机器人数据集上实现小于0.5米的误差和低方差而付出的代价，在特征退化的Bacchus数据集上所有基准方法都失败了。对于实践者来说，关键的决定标准是平台动态和环境几何：对于具有丰富3D结构的平滑轮式机器人，具有全3D匹配和IMU积分的方法更合适；对于腿部、手持或人形平台，在具有均匀或稀疏3D特征的环境中，或者当由于脉冲式运动或GNSS不可用而无法保证IMU可靠性时，所提出的方法更合适。

**4.4. 未来的局限性和工作方向**

本研究的几个局限性值得明确讨论。首先，CitrusFarm和Bacchus的评估基于单次轨迹采集（Ne=1），这排除了置信区间的计算，并限制了对这些数据集性能变异性的统计描述。计划在未来工作中创建新的具有多次运行的不同树木环境的数据集，如Pullally实验中所做的那样，以提供完整的统计比较。

其次，所提出的方法的2D投影丢弃了高程信息，这限制了其在近似平坦地形上操作的适用性。在非常陡峭的斜坡或多层树木环境中，地面投影假设可能会引入EKF过程噪声模型无法完全捕捉的系统误差。将方法扩展到具有地形高程映射的完整6-DOF姿态估计框架将解决这一限制。

最后，所有五种基准方法和所提出的方法都没有进行闭环处理。特别是在长距离重复果园穿越时，加入闭环机制将减少所有方法的累积漂移，这是未来工作的一个重要方向。

**5. 结论**

本文提出了一种仅使用激光雷达的SLAM方法，适用于树木农业环境，消除了对惯性里程计和GNSS定位的依赖。该方法通过将3D激光雷达扫描概率投影到地面平面，并使用修改后的Hausdorff距离解决2D扫描到地图的匹配问题，适用于IMU积分不可靠的平台：特别是腿部机器人、手持映射系统和人形平台，因为间歇性的腿部-地面接触力会损坏惯性测量。该方法在模拟中、在受控的户外环境中以及三个真实世界果园数据集（CitrusFarm、Bacchus和Pullally）中进行了评估，并与五种基准方法进行了比较，这些方法涵盖了从经典基于特征的方法（A-LOAM、LeGO-LOAM）到最新的密集和适应农业的方法（DLO、MOLA、AG-LOAM）。

结果表明，所提出的方法在平滑轮式平台上牺牲了峰值精度，以在各种平台和环境中实现一致的鲁棒性。这种二维地面平面投影自然能够抑制四足动物小跑步态所带来的垂直扰动，而改进后的Hausdorff距离对异常值的鲁棒性则能够弥补由于平台滚动和俯仰所导致的扫描偏差，这解释了在非连续运动条件下该方法始终具备的优势。尽管基准方法在光滑轮式平台（CitrusFarm）上能够实现更高的精确度，但在非连续运动平台（Pullally）上表现不佳，而我们的方法在这两种条件下都能保持稳定的性能。本研究的一个局限性在于，由于数据集的限制，CitrusFarm和Bacchus的评估仅基于单次轨迹运行，而Pullally数据集包含了三次重复运行的数据。目前正在进行的工作包括创建新的树木环境数据集，这些数据集包含所有路径的多次重复运行以及地面真实网格地图，以便更好地表征SLAM算法的性能变异性。

CRediT作者贡献声明：
Paola Nazate-Burgos：撰写原始稿件、审稿与编辑、数据可视化、方法论研究、形式化分析、数据整理。
Miguel Torres-Torriti：撰写原始稿件、审稿与编辑、项目指导、软件开发、资源调配、项目管理、资金筹措、概念构思。
Shoudong Huang：撰写原始稿件、项目指导、研究设计、形式化分析。
Fernando Auat Cheein：审稿与编辑、验证工作、项目指导、概念构思。