基于多传感器融合的自主中心线及点对点导航控制方法，适用于退化果园环境

《Computers and Electronics in Agriculture》：Autonomous centerline and point-to-point navigation control method based on multi-sensor fusion in degraded orchards environments

自主导航技术在标准化果园中的研究提出多传感器融合定位框架，结合LiDAR、RTK-GNSS和IMU数据，采用LIO-SAM算法构建三维点云地图与二维占用网格地图，通过粒子滤波融合不同传感器信息实现高精度定位，并开发基于路径规划的自主导航系统，实验表明定位误差降低66.27%，行中心跟踪误差4.37厘米，满足果园作业需求。

　　

陈振宇|窦汉杰|翟长远|王志忠|高园园|王秀

北京市农林科学院智能装备研究中心，北京 100097，中国

摘要

自主导航技术在智能果园的生产和管理中发挥着越来越重要的作用。然而，标准化果园结构稀疏且特征退化的环境对现有的定位和导航方法提出了重大挑战。为了解决这些问题，本研究提出了一种多传感器融合定位框架，该框架整合了激光雷达（LiDAR）、实时动态校正全球导航卫星系统（RTK-GNSS）和惯性测量单元（IMU）的数据，以克服退化环境中的定位失败。在此框架的基础上，我们开发了一个能够进行稳健的树行中心线跟踪的自主导航系统，并引入了一种点对点导航控制方法，以实现精确的果园作业。使用LIO-SAM算法结合基于树干的点云提取方法构建了3D点云地图和2D占用网格地图。激光雷达点云为3D地图匹配提供测量更新，而紧密耦合的RTK和IMU数据提供运动估计。粒子滤波器融合这些测量数据以确保可靠的定位。评估实验包括地图构建精度、定位误差、导航精度和行中心跟踪，结果显示，与仅使用激光雷达的NDT匹配相比，所提出的多传感器融合方法将定位误差降低了66.27%。行中心跟踪误差为4.37厘米，转弯误差为20.18厘米，分别减少了69.86%和48.74%，满足了喷洒作业的中心线导航要求。在点对点导航测试中，平均纵向和横向误差分别为0.225米和0.088米，满足了收割、施肥和运输作业的精度要求。本研究为果园自主导航提供了全面的解决方案，并为复杂田间环境下的智能果园生产提供了实用技术。

引言

果园管理涉及一系列劳动密集型任务，包括施肥、修剪、喷洒农药、授粉、疏果和收割（Zhang等人，2025年；Sun等人，2025年；Zahid等人，2021年）。智能果园设备集成了环境感知、自主决策和无人操作等技术，实现了智能高效的果园管理。这些系统可以提高水果质量、降低生产成本并缓解劳动力短缺问题（Mhamed等人，2024年；Dou等人，2022年）。根据InsightAce Analytic的预测，到2031年，全球智能果园机械市场预计将达到78亿美元，这凸显了国际上对智能农业设备发展的日益关注。

自主导航技术使智能果园机械能够独立进入果园环境并高效覆盖指定工作区域，是智能果园系统的基本组成部分（Dou等人，2024年；Xie等人，2023年）。在发展的早期阶段，果园导航系统主要依赖于单一传感器解决方案，如全球导航卫星系统（GNSS）（Yue等人，2024年）、激光雷达（Jiang和Ahamed，2023年；Guevara等人，2024年）或视觉传感器（Zhou等人，2023年）。然而，GNSS在密集树冠覆盖下信号不稳定（Min等人，2008年；Raikwar等人，2022年），视觉传感器在变化复杂的光照条件下容易受到干扰（Yang等人，2022年），尽管激光雷达具有很强的环境适应性（Dou等人，2025年），但其点云特征在高度重复的标准化果园结构中的分辨率往往有限，导致定位鲁棒性降低（Xu等人，2022年）。特别是，现代标准化果园的特点是树木间距均匀、修剪模式一致以及树冠几何形状高度相似。这种结构同质性（Tian等人，2024年）导致空间特征重复且区分度低，尤其是沿果园行方向，使得基于激光雷达的感知容易受到特征退化和配准模糊的影响。这些特征形成了一个感知退化的环境，对单一传感器定位方法构成了根本性挑战。

基于多传感器融合的定位方法在果园环境中越来越受到关注，因为它们解决了单一传感器方法在复杂田间条件下的鲁棒性和适应性方面的局限性。早期研究侧重于基于环境线索的自适应传感器组合。例如，一些系统在GNSS信号弱或不可用时切换到基于激光雷达的定位，使用广义迭代最近点（GICP）算法，而在信号稳定时恢复到卫星定位（Li等人，2024年）。其他方法利用空间特性，在行间导航时使用激光雷达，在转弯操作时使用GNSS（Liu等人，2024年）。这些方法在特定条件下激活不同的传感器，从而形成相对简单的系统架构。然而，它们未能充分利用异构传感器的互补优势，且在动态或退化环境中经常出现定位不连续的问题。为了克服这些挑战，已经开发了更先进的传感器融合技术。由于激光雷达在户外果园中的强环境适应性，它经常与其他传感器融合以实现稳健的定位和导航。例如，一些系统将基于2D激光雷达的位置估计与电子指南针的方向数据相结合（Su等人，2024年）。然而，2D激光雷达的性能常常受到果园杂草等密集植被的干扰。其他方法使用RTK-GNSS作为点云对齐的初始估计，然后使用正态分布变换（NDT）算法进行手动路径规划（Xia等人，2023年），但这些方法严重依赖GNSS的可用性。尽管多传感器融合大大提高了定位的鲁棒性，但它常常忽略了现代标准化果园日益增长的结构同质性。这些环境的特点是行和植物间距均匀以及修剪一致，导致树冠形状、大小和结构的变化很小。因此，环境特征变得高度重复，降低了激光雷达点云的独特性，削弱了基于特征的定位效果。在这种情况下，现有的融合方法仍然难以处理退化的激光雷达输入和GNSS遮挡问题，这突显了需要更准确和有弹性的定位解决方案。

在果园中实现自主导航不仅需要准确的定位，还需要有效的路径规划和控制。为了减少对GNSS的依赖，已经探索了基于实时路径提取的各种导航方法。例如，深度学习模型已被用于实时图像分割，以提取果园中的行间导航路径（Navone等人，2023年）。提出了一种单阶段导航路径提取网络（NPENet），可以直接预测果园行的中心线，简化了自主导航的路径提取（Liu等人，2025年）。同样，2D激光雷达也被用于导航路径检测（Brown等人，2024年），3D激光雷达点云经过裁剪和欧几里得聚类处理，以提取果园行结构用于路径生成（Wang等人，2022年）。然而，实时路径提取方法无法提供作业设备在果园内的绝对位置。因此，它们难以完成自主行间转换和沿行中心线的精确导航等任务。这些局限性凸显了需要将农艺任务要求纳入路径规划和控制的导航系统的需求。

构建结构化的导航地图是解决上述挑战的有希望的解决方案。当前的果园映射主要依赖于视觉（Xiong等人，2023年；Chen等人，2021年）和基于激光雷达的SLAM算法（Cao等人，2025年；Li和Zhu，2024年），其中激光雷达SLAM因其在外部环境中的鲁棒性和捕获高精度空间数据的能力而成为主导方法。例如，HDL-Graph SLAM能够生成3D果园点云地图。使用这些地图，可以通过NDT_ICP配准实现定位；然而，路径规划仍然需要手动完成（Jiang等人，2024年）。同样，紧密耦合的激光雷达-IMU系统LIO-SAM可以构建3D地图，并将其投影到2D网格中用于导航规划（Wang等人，2024年）。其他方法从3D地图中提取树干点云，生成2D成本网格用于中心线路径规划和转弯（Chen等人，2024年）。尽管如此，这些方法往往忽略了导航到特定任务目标点的需求，限制了它们的实际应用性。总之，当前的果园导航策略往往忽略了生产任务的实际需求。很少有研究明确解决如行中心引导和精确目标点到达等农艺需求。此外，在具有挑战性田间条件下可靠运行的稳健定位方法仍然很少。

为了应对特征稀疏和结构重复的果园中的稳健定位挑战，结合激光雷达、IMU和RTK的多传感器定位系统是必不可少的。这种融合在退化条件下增强了定位的可靠性。此外，为了满足果园作业的多样化需求（如喷洒、施肥、收割和运输），自主导航系统必须能够进行自适应路径规划和任务特定控制，这对于完全自主的果园作业至关重要。本研究提出了一种针对各种果园作业的导航控制方法。针对退化果园环境，开发了一种基于激光雷达–IMU–RTK融合的定位策略。使用SLAM和基于点云的树干特征提取，生成了3D点云地图和相应的2D网格地图。在ROS框架下，定位、映射和路径规划模块被集成到一个完整的自主导航系统中。该系统支持中心线跟踪、自主转弯和精确的点对点任务执行。其性能通过映射精度、定位误差、航点控制精度和中心线导航精度的实验得到了验证。

系统架构

自主果园导航的系统架构

图1展示了自主果园导航系统的组成，包括以下核心组件：差分跟踪底盘（MID-01，YHSEN Technology Co., Ltd.，深圳）、工业计算机（Intel Core i7-8665U，16 GB RAM）、32线激光雷达传感器（C-32，LeiShen Intelligent System Co., Ltd.，深圳）、惯性测量单元（TL740D，Refine Tech Co., Ltd.，深圳）和GNSS RTK模块（CGI-410，上海Huace Navigation Technology Co., Ltd.）

果园地图构建的准确性

使用EVO工具包评估了果园3D点云地图的准确性。映射试验期间记录的RTK轨迹作为参考路径。如图12(a)所示，LIO-SAM估计的轨迹与参考轨迹非常吻合，实现了完整的闭环。然而，从第4行开始，在行间区域出现偏差，第5行和第6行的偏差逐渐增加。这些偏差可能是由于行间结构的重复性造成的

讨论

标准化果园的特点是树木间距均匀，树干形状和树冠轮廓高度相似。缺乏明显的几何或纹理特征——尤其是在行间区域——导致空间观测方差低。此外，风引起的叶片运动在激光雷达点云中引入了噪声和不稳定，进一步降低了特征质量，如图14所示。这些条件创造了一个感知退化的环境，其中基于激光雷达的配准受到影响

结论

本研究提出了一种基于粒子滤波的新型多传感器融合定位方法，适用于退化环境。该方法结合了激光雷达点云匹配与先验地图、基于IMU的连续运动预测和RTK进行高精度位置校正。通过在X–Y平面上差分IMU和RTK数据获得观测残差，并使用卡尔曼增益校正IMU预测的位置，从而得到精细的运动估计。

CRediT作者贡献声明

陈振宇：写作 – 审稿与编辑，撰写原始草稿，可视化，验证，软件，方法论，调查，概念化。窦汉杰：写作 – 审稿与编辑，撰写原始草稿，监督，项目管理，方法论，资金获取，概念化。翟长远：写作 – 审稿与编辑，撰写原始草稿，监督，资源获取，方法论，资金获取。王志忠：写作 – 审稿与编辑，形式分析，数据

利益冲突声明

作者声明他们没有已知的可能会影响本文工作的财务利益或个人关系。

致谢

本工作得到了国家自然科学基金（资助编号：NSFC32402417）、中国博士后科学基金（资助编号：2023M740312）、北京农林科学院的杰出科学家计划（资助编号：jkzx202212）以及北京科学技术协会的青年精英科学家资助计划（资助编号：BYESS2023201）的财政支持。