在无结构化果园中的稳健自主导航：结合LIO-SAM与基于NDT的重定位技术及贝叶斯可通行性映射

《Computers and Electronics in Agriculture》：Robust autonomous navigation in unstructured orchards: Integrated LIO-SAM with NDT-based relocalization and Bayesian traversability mapping

　　

Tao Liu|Yifan Bian|Baohua Zhang|Leijing Bai|Guoxi Wang|Guangzhao Tian

• •
  将NDT重定位技术整合到LIO-SAM因子图中，以消除定位偏差。
• •
  利用贝叶斯广义核推理方法对果园的通行性进行建模。
• •
  通过改进的双树RRT*算法实现高效路径规划。
• •
  验证了在无GPS信号的复杂果园环境中的可靠导航能力。

引言

农业机器人在农业领域的应用日益增多，显著提升了农业的生产效率与效益（Erfani等人，2019；Graf Plessen，2019；T. Liu等人，2024；Padhiary等人，2024）。然而，非结构化环境给农业机器人的地图构建与自主导航带来了巨大挑战（Guan等人，2021；Jones等人，2019）。在果园中进行自主导航对于监测、喷药和采收等任务而言至关重要。与结构化的室内或城市环境不同，农业果园具有高度的特殊性（Padhiary等人，2024）。其不规则的地形、复杂多样的作物结构，以及缺乏人工定位特征——如树木遮挡、杂草干扰、土壤状况和地面不平——都给环境映射、实时定位和自主路径规划带来了极大困难（Bai等人，2023；Emmi等人，2021；Hiremath等人，2014）。因此，设计高效且可靠的环境映射与自主路径规划系统，对于此类环境中的农业机器人发展而言极为重要。

要在果园中成功应用智能农业技术，就必须研究农业机器人的地图构建、定位和导航能力（Aguiar等人，2020）。同步定位与地图构建（SLAM）主要包括基于视觉和雷达的方法。视觉SLAM已被广泛用于定位和导航。Engel等人（2014）提出了一种可直接处理大规模环境并实现实时定位与地图构建的单目视觉SLAM系统。不过，单目相机在获取深度信息方面存在局限，这会增加计算成本，降低感知质量，进而影响地图构建和定位的精度与速度（Forster等人，2017）。与普通相机相比，RGB-D相机受光线变化的影响较小，还能直接生成深度图，从而减轻计算负担。Campos等人（2021）提出了对单目、双目、RGB-D、视觉惯性等多种传感器模态的统一适配方案，这是一种在视觉SLAM领域具有强大鲁棒性和通用性的经典开源解决方案。但在果园这类环境中，该方法存在两个主要缺陷：它无法快速进行大规模远距离感知，而且相机的视野容易被果园的树枝遮挡。总体而言，尽管基于视觉的方法适合位置识别，在纹理丰富的环境中表现良好，但其性能并不适合果园环境。

相比之下，激光雷达能够独立于光线和纹理条件提供精确的距离测量数据。不过，早期的仅依赖激光雷达的SLAM方法，如最初的激光雷达测距与地图构建（LOAM）（Zhang & Singh，2014）及其简化版轻量级优化版激光雷达测距与地图构建（LeGO-LOAM）（Shan & Englot，2018），虽然能够实现低误差的测距功能，但在特征信息不足的环境中仍易出现累积误差。为提升系统的鲁棒性，将激光雷达与惯性测量单元（IMU）的数据相结合的做法越来越普遍。但最初的松散耦合方式在剧烈运动条件下往往效果不佳（Shan等人，2020；Wang等人，2023）。这就促使人们开发出了紧密耦合的激光雷达-IMU测距框架，如LIO-SAM（Shan等人，2020）和快速光检测与惯性测距（FAST-LIO）（Xu & Zhang，2021），这类框架能在状态估计层面深度融合传感器数据，因此在无GPS信号的动态环境中表现出更优异的性能。然而，对于在果园这类非结构化环境中的长期运行而言，仍存在一个关键限制：这些系统主要依靠环闭合检测来纠正累积的定位偏差（Hussain等人，2025）。而环闭合需要重新访问之前已测绘过的位置，而在广阔、单调且不断变化的农业种植区，这种事件发生的频率很低，也不太可靠。因此，即便是最先进的激光雷达-IMU测距系统，在此类场景下也可能会出现无限制的定位偏差，从而难以实现可靠的长期自主导航。虽然基于滤波器的方法如FAST-LIO2具备出色的效率，但我们选择了采用因子图优化的LIO-SAM框架。因子图架构本身就支持将异步的全局约束条件——尤其是我们提出的基于NDT的重定位模块——无缝集成到优化过程中。这一功能对于在大型果园中纠正累积定位偏差至关重要，而且不会像基于滤波器的重置方法那样出现轨迹不连续的问题。

除了精准定位之外，确保机器人能够在崎岖、潮湿且不规则的果园地形中安全移动同样重要（L. Liu等人，2023；Q. Liu等人，2025；Patle等人，2019）。果园中的树木、树枝和果实构成了复杂的立体结构，可能会干扰传感器的感知范围和精度。通常会通过可行性评估来根据坡度、粗糙度等物理特征确定可通行的区域（Sevastopoulos & Konstantopoulos，2022）。然而，那些依赖确定性阈值的传统几何方法，往往对传感器噪声以及果园中激光雷达点云的固有稀疏性较为敏感。与此不同，本研究采用的基于BGK的方法能够明确建模空间不确定性，通过计算后验均值和方差来提供概率意义上的安全裕度，这对于在非结构化环境中保持导航的稳定性至关重要。在农业领域，人们已经探索了多种路径规划方法。像A*及其变体这样的确定性图形搜索方法虽能保证找到最优路径，但在连续且高维的空间中计算成本较高（Sedighi等人，2019；Shi等人，2020）。基于深度强化学习的数据驱动方法虽然具备适应性，但需要大量的训练数据，而且在现实农场中面临不可预测的情况时可能缺乏足够的鲁棒性（Li等人，2022）。相比之下，基于采样的算法，如RRT及其渐近最优版本RRT*，由于具有概率完备性，且能够在高维配置空间中高效搜索，因此非常适合非结构化环境（Kuffner & LaValle，2000）。

尽管在激光雷达-IMU测距及路径规划算法方面已取得一定进展，但现有系统在特征信息严重不足的复杂果园环境中仍常常失效。因此，本研究假设事先已经拥有该果园环境的3D点云地图，而这张地图可以利用相同的LIO-SAM框架并通过环闭合功能离线构建。在线导航过程中，重定位模块便会利用这张预构建的地图来进行全局姿态校正。本研究要解决的核心问题，是无人地面车辆在长期使用过程中出现的定位偏差问题，这一问题还因缺乏可靠的概率地形评估而愈发严重。为弥补这一缺陷，本文的主要贡献如下：

• •
  我们提出了一种集成了多传感器感知、定位与规划功能的框架，用于在无GPS信号的果园环境中实现点对点的自主导航。实际测试表明，该框架具备多项关键功能，包括有限的姿态估计范围、基于概率的可行性地图绘制，以及无需依赖人工地标或频繁进行环闭合操作即可生成的无碰撞全局路径。
• •
  在本研究中，我们通过将基于NDT的主动重定位模块整合到LIO-SAM的因子图中，为其针对果园环境进行了优化升级。该模块引入了基于地图级别的配准约束，从而提升了轨迹的连续性，使得在特征重复的果园种植行中也能保持有限的姿态估计范围。
• •
  我们借助基于贝叶斯广义核推理的可行性分析方法，为复杂地形定制了专门的可行地图。通过根据无人地面车辆的运动学约束，将复杂的3D点云转换为概率性的2D障碍物网格，该方法能够提供可靠的地形安全性评估结果，更有利于后续的路径规划工作。
• •
  在所构建的二维可行性地图基础上，我们采用了基于双树RRT*的规划器来生成无碰撞的全局路径。实验结果显示，该规划方法在计算效率方面表现良好，且在测试的果园场景中能够有效地避免路径冲突。

章节要点

果园用无人地面车辆的设计与组装

本研究使用了一款自主型无人地面车辆。这款车辆集成了多传感器系统、计算单元和运动控制系统，从而能够在农业果园环境中实现精准的感知与自主导航。它的设计充分考虑了果园中多变的地形和植被条件。其主要传感器是一款16通道激光雷达，即使在光线较弱的条件下，也能在100米的范围内准确采集环境数据。

实验环境与结果

主要的实验地点位于中国江苏省宿迁市泗阳县大康庄园。该地拥有多样化的果园地形，因此非常适合用于评估在非结构化农业环境中的地图构建、定位和自主导航功能。实验在四个不同的地点进行，如图3所示。

在大康庄园，我们选取了四种具有代表性的果园环境作为实验场所。实验地点1（见图3b）的特点是存在较深的沟壑，地面泥泞且不平整

结论

本研究针对非结构化果园环境所面临的特殊挑战，开发并评估了一种集成式的自主导航框架。该框架在多种真实的田间环境中经过了测试，展现了其多项核心功能，包括在复杂且杂乱的果园环境中实现连续的状态估计、基于概率的可行性地图绘制，以及生成无碰撞的路径。其在定位方面的主要优势在于能够实现无缝的

利益冲突声明

作者声明，他们不存在任何可能影响本文研究结果的已知利益冲突或个人关系。

致谢

本研究得到了国家自然科学基金（项目编号：32472010）、江苏省农业科技创新基金（JASTIF）（资助编号：CX(24)3027）以及江苏省自然科学基金（资助编号：BK20231478）的支持。