在农业劳动力成本不断攀升的背景下，智能农业机器人已成为提升生产效率、促进农业可持续发展的重要手段。果园管理由于地形复杂、植被密集、安全要求严格，对机器人的自主性提出了极高的要求。自主导航因此被视为智能果园机器人的核心能力，也是实现高效可靠田间作业的先决条件。
基于GNSS的导航系统在开放环境中定位精度高，但在山地和丘陵果园中，GNSS信号受树冠遮挡、多径干扰和地形屏蔽影响严重，导致定位精度下降和导航性能不稳定。因此，在果园环境中，可靠的感知与定位必须更多地依赖机载传感器。
近年来，基于激光雷达的导航因其高测距精度和抗光照变化能力强而受到广泛关注。然而，传统2D LiDAR只能捕捉平面信息，在果园环境中容易出现几何畸变。3D LiDAR虽能改善环境感知，但传感器成本高昂、计算需求大且难以大规模部署。此外，果园环境高度非结构化，植被茂密、地形复杂，常导致点云畸变和匹配误差。
视觉导航提供了另一种解决方案，因为它能提供丰富的环境表征和强大的泛化能力。然而，RGB-D传感器在远距离深度精度有限，且对光照变化敏感。视觉惯性里程计系统在光照变化或快速运动下仍会经历特征退化和姿态丢失。
为了克服单传感器感知的局限性，近年来的研究越来越多地关注多传感器融合。将多种传感模式整合，能显著提高农业环境中的鲁棒性。
高精度环境感知与点云融合
准确的环境感知是果园环境中可靠自主导航的前提。本研究采用点云融合策略，整合来自RGB-D相机和2D LiDAR的互补信息。在机器人操作系统框架下，激光雷达和RGB-D数据分别以LaserScan和PointCloud2消息形式获取。经过时间同步和坐标变换后，两个数据源融合生成统一的点云表示，作为后续SLAM和导航模块的输入。
融合过程始于RGB-D相机与激光雷达之间的外参标定，以确保准确的空间对齐。随后使用高度和距离约束对原始点云进行滤波，以消除无关数据。考虑到除草机器人的物理结构和果园环境特征，有效点云数据的高度限制在0.01–0.5米。此外，尽管2D LiDAR提供360°扫描能力，但有效感知区域受RGB-D相机视场限制，因此仅保留前方180°区域进行数据融合。距离阈值设定为0.5–5米。
经过空间滤波后，剩余点云进行一致性检查和尺度归一化，以确保不同传感器模态之间的兼容性。通过这一过程，冗余和噪声测量被有效抑制，而树干、田埂和障碍物等关键环境特征得以保留。在点云预处理阶段，采用面向任务的滤波策略来减少冗余信息并确保在资源受限的机载平台上的实时性能。首先应用基于高度的滤波去除有效导航高度以上的点，从而消除与地面导航无关的冠层结构。然后采用感兴趣区域滤波和距离阈值处理，仅保留机器人有效感知范围内的点。
为了在保留基本几何结构的同时进一步控制点云密度，应用体素网格下采样作为标准密度降低技术。体素分辨率在本研究中经验性地选择并设置为0.05米，考虑了机载Intel NUC平台的计算能力。融合点云最终被投影到二维占用栅格地图中。这一设计选择是基于行间果园导航是地面级任务，过多的三维信息会引入不必要的计算负担，而不会为导航精度带来成比例的好处。
多传感器融合定位与建图
准确且鲁棒的定位对于果园除草机器人的自主导航至关重要。本研究提出了一个多传感器融合定位与建图框架，整合了轮式里程计、视觉惯性里程计和2D LiDAR感知。
扩展卡尔曼滤波器用于融合来自多个传感器的运动信息，并在变化的环境条件下自适应调整其贡献，从而产生稳定连续的姿态估计。视觉惯性里程计在视觉退化情况下提供可靠的短期运动先验，而轮式里程计在低速运行和频繁停止时增强了运动连续性。为了进一步抑制长期漂移，应用自适应蒙特卡洛定位通过将融合点云观测与全局地图进行匹配来细化姿态估计。
基于优化后的姿态估计，采用基于Cartographer的SLAM框架来构建果园的全局一致二维占用栅格地图。在此框架中，融合姿态估计输出取代了原始Cartographer流程中使用的传统里程计输入，为扫描匹配和子图优化提供了更准确鲁棒的姿态先验。因此，建图过程受益于改进的运动估计，同时保留了原始的Cartographer建图结构，实现了可靠的导航，并为果园环境中自主除草操作奠定了坚实基础。
路径规划与避障
果园环境中的自主除草需要连续的行间跟踪能力，同时确保在静态和动态障碍物存在下的安全性。然而，复杂的果园特性——包括狭窄的行间距、不平坦的地形和频繁出现的障碍物——使得仅基于静态地图的传统导航方法不足以满足实时操作需求。
提出的导航框架基于多层动态代价地图架构，结合多目标顺序导航，在复杂的果园环境中实现稳定高效的行间作业。该架构由静态地图层、动态障碍物层和自适应扩展层组成。静态层提供源自先前建图的全局环境信息，并作为全局路径规划的基础。动态障碍物层整合来自2D LiDAR和RGB-D相机的实时感知数据，以检测新出现的障碍物，如树枝、杂草或移动物体。基于这些信息，自适应扩展层根据机器人的运动状态和周围环境动态调整障碍物膨胀，确保足够的安全裕度，同时避免不必要的路径偏离。
为了提高操作效率并克服传统单目标导航的局限性，引入了多目标顺序导航策略。设计了一个基于ROS框架的任务管理机制，其中一个独立的目标管理节点维护一个优先队列，包含对应于行入口、转弯位置和行出口的目标点。该系统自动发布导航目标，实时监控执行状态，并在当前目标达到后触发后续目标。
对于路径规划，采用全局-局部协同策略。使用A*算法生成全局路径，该算法基于静态地图和预定义的目标序列提供最优参考轨迹。局部规划和避障使用时间弹性带算法执行，该算法通过考虑运动学约束、障碍物接近度和时间效率来实时优化轨迹。当在全局路径上检测到障碍物时，局部规划器生成临时的避障轨迹，一旦障碍物被清除，机器人平滑地返回到全局路径。
考虑到除草机器人的底盘宽度明显小于果园中典型的行间距，沿行中心线的单次导航无法完全覆盖除草区域。因此，在导航框架中纳入了多遍覆盖路径规划策略以提高操作完整性。
对于每个行间通道，通过横向偏移参考中心线生成多条平行路径。相邻路径之间的间距基于除草机构的有效工作宽度和重叠要求确定，确保跨整个行间区域的连续覆盖。在地头，在预定义的缓冲区内规划U形转弯机动，以实现连续作业之间的平滑过渡，同时避开树干和其他障碍物。
由此产生的覆盖路径序列通过ROS导航系统中现有的多目标目标队列执行，其中每个平行作业和转弯机动的中间航点自动生成并按顺序执行。这一策略在任务规划层面运行，而底层定位、导航和轨迹跟踪模块保持不变。
实验结果
2024年6月18日，在中国重庆北碚区的一个柑橘园进行了实地实验，平均树间距为2.75米，行间距为4.18米。所有测试均在自然户外条件下进行，环境温度在27°C至32°C之间。
为了全面评估所提出的导航系统的性能，从全局定位和任务级导航两个角度设计实验。首先，进行了轨迹建图实验，以评估SLAM系统的全局定位精度。机器人沿果园行驾驶，估计轨迹与RTK地面真值数据进行比较，以计算绝对姿态误差，反映了系统的全局一致性和漂移特性。随后，进行了地图构建精度实验，其中使用融合的LiDAR-视觉SLAM框架生成二维占用栅格地图。将映射的树行与实地测量进行比较，以评估横向和纵向偏差。此外，进行了目标点到达实验，通过指挥机器人到达预定义的目标姿态并计算最终位置和航向误差来评估任务级导航性能。最后，在不同速度（0.5米/秒、1.0米/秒和1.5米/秒）下进行了导航实验，以分析运动速度对导航精度的影响，并验证所提出系统的鲁棒性。
轨迹估计性能
为了评估所提出系统的轨迹估计精度，在相同的果园条件下测试了三种定位方法——视觉惯性里程计、轮式-视觉惯性里程计和所提出的多传感器融合方法。使用高精度RTK系统作为地面真值。定量评估使用EVO工具包进行。所有实验均沿着典型果园环境中的闭环轨迹进行，该环境具有密集的冠层覆盖、不平坦的地形和重复的视觉纹理。
基于VIO的方法表现出显著的轨迹漂移，特别是在转弯机动期间。累积误差随时间迅速增加，导致与参考轨迹的明显偏离，并且无法保持闭环。这种行为主要是由于冠层遮挡和光照变化下的特征退化造成的，这对特征跟踪稳定性产生了不利影响并导致尺度不一致。
WVIO方法由于整合了轮式里程计，显示出改进的短期稳定性。在直线运动期间，轨迹偏差得到部分抑制。然而，在没有全局校正的情况下，由车轮打滑和不平坦地形引起的累积误差逐渐积累，尤其是在重复转弯期间。因此，估计轨迹在长距离上仍然偏离地面真值，这表明仅靠轮式-视觉融合不足以在果园环境中实现可靠的长距离导航。
相比之下，所提出的多传感器融合方法实现了最一致的轨迹估计和最高的定位精度。定量分析显示，最大定位误差为1.967米，而标准差、RMSE和平均误差分别为0.328米、0.587米和0.486米。这些结果表明，定位误差在大部分操作时间内得到良好控制，波动有限。
总体而言，与VIO和WVIO相比，所提出的方法展示了卓越的轨迹一致性和显著减少的漂移积累。这种定位精度和稳定性足以支持果园环境中稳定的行间导航和连续的自主除草操作，其中可靠的相对定位和长期轨迹一致性对于安全和高效的现场部署至关重要。
地图构建精度
为了评估所提出方法的建图性能，在原始Cartographer算法和整合了多传感器融合姿态估计的改进框架之间进行了对比实验。在建图过程中，机器人以0.5米/秒的恒定速度穿越果园行，同时连续收集LiDAR、RGB-D和运动数据。测试区域包括三行柑橘树，每行七棵树，每个实验重复20次以确保统计可靠性。
通过比较构建的地图与实际果园布局，使用列偏差、行偏差及其相应的统计分布来评估建图精度。定量结果进一步证实了这些观察。原始Cartographer方法产生的平均列偏差为19.26厘米，平均行偏差为19.91厘米，相应的标准差超过12厘米。这些结果表明，仅依赖基于LiDAR的扫描匹配和局部里程计为果园建图提供的鲁棒性和全局一致性有限，尤其是在重复转弯和长距离穿越的情况下。
相比之下，所提出的方法通过整合来自多传感器融合框架的高精度姿态估计，显著提高了建图精度。通过整合轮式里程计、视觉惯性里程计和全局约束，为扫描匹配提供了更可靠的运动先验，有效限制了误差传播。具体而言，平均列偏差降至7.28厘米，平均行偏差降至7.22厘米，分别比原始Cartographer算法减少了61.0%和64.0%。此外，标准偏差减少了59%以上，表明在重复试验中显著提高了稳定性和几何一致性。
总体而言，这些结果表明，整合多传感器融合姿态先验显著提高了果园环境中的建图保真度和可重复性。与原始Cartographer算法相比，所提出的框架产生了更规则的行结构和一致的树间距，这对于可靠的自主导航和后续的行间除草操作至关重要。
目标点到达精度
为了评估所提出导航系统在实际果园条件下的目标到达性能，设计了一条跨越三个果园行的往返轨迹。沿路径选择了五个代表性目标点。指挥机器人以三种操作速度到达每个目标，并对每种速度重复实验20次以确保统计可靠性。在每个目标处的最终机器人姿态使用机载定位系统记录。目标到达精度根据横向偏差、纵向偏差和绝对定位误差进行评估。
结果揭示了操作速度与目标到达精度之间的明确关系。当机器人速度从0.5米/秒增加到1.5米/秒时，平均横向偏差仅略有增加，从3.29厘米增加到3.83厘米，表明行居中性能在不同速度下保持稳定。相比之下，平均纵向偏差增加更为明显，从6.23厘米增加到10.65厘米，反映了沿行进方向对运动速度更强的敏感性。这种行为与行跟踪导航的特性一致，其中前进运动动力学和感知-控制延迟对纵向精度有更大影响。
跨目标点的详细比较显示，最大的定位误差持续出现在目标点D。在该位置，最大偏差在0.5米/秒和1.0米/秒时达到19.48厘米，在1.5米/秒时进一步增加到22.31厘米。目标点D位于果园行的中间附近，那里的视觉特征高度重复且独特地标稀疏。在这种情况下，定位变得更加困难，累积的不确定性更有可能表现为定位误差，尤其是在更高速度下。
总体而言，横向偏差在所有操作条件下都得到了良好控制，所有测试中的最大横向误差不超过7厘米。这表明所提出导航系统具有强大的行居中能力。尽管纵向偏差随速度增加，但观察到的误差水平仍保持在果园除草操作可接受的范围内。这些结果表明，所提出的系统可以在广泛的操作速度范围内实现可靠的目标到达性能，支持在实际果园条件下稳定的行间导航和连续的自主除草。
导航精度评估
为了评估所提出系统在实际果园条件下的导航精度，在0.5、1.0和1.5米/秒三种操作速度下进行了实验。在每个试验中，机器人沿着预定义的全局路径行驶，同时执行实时避障。使用自适应蒙特卡洛定位进行全局定位，高精度RTK测量作为性能评估的地面真值。
实验结果表明了导航精度与操作速度之间的明确关系。在0.5米/秒的速度下，机器人实现了高精度的路径跟踪，平均航向偏差为3.46°，平均横向和纵向偏差分别为3.11厘米和3.84厘米。这些结果表明在低速条件下的稳定和准确导航。
当操作速度增加到1.0米/秒时，导航误差适度增加。平均航向偏差上升到5.78°，而平均横向和纵向偏差分别增加到5.62厘米和5.85厘米。这种性能下降反映了在更高速度下对感知-规划-控制协调要求的增加，其中减少的反应时间和增加的运动动力学放大了跟踪误差。
在最高测试速度1.5米/秒时，导航误差进一步增加，但仍然保持在操作阈值内。平均航向偏差达到6.36°，而平均横向和纵向偏差分别达到6.09厘米和7.78厘米。值得指出的是，即使在最高速度下，平均横向偏差也保持在7厘米以下，这对于避免与树行碰撞至关重要。
在所有速度下，导航误差的标准偏差都相对较低，表明系统性能一致且可预测。在0.5米/秒时，横向偏差的标准偏差为2.06厘米，纵向偏差为2.25厘米，航向偏差为2.04°。随着速度的增加，这些值适度增加，但在1.5米/秒时，横向、纵向和航向偏差的标准偏差分别保持在3.84厘米、4.60厘米和4.08°。
这些结果共同证实，所提出的导航系统在0.5至1.5米/秒的速度范围内实现了可靠且准确的路径跟踪。尽管导航误差随速度增加，但所有测量指标仍远低于果园除草操作的安全阈值。系统的稳定性表现为所有测试条件下的低标准偏差，表明其在不同操作速度下的一致性能。这些发现验证了所提出的多层成本地图架构、多目标顺序导航和全局-局部路径规划策略的有效性，为在复杂和动态的果园环境中部署自主除草机器人提供了坚实的基础。
