基于激光雷达/惯性测量单元/全球导航卫星系统（LIDAR/IMU/GNSS）的果园施药机器人组合导航方法研究报告

研究团队与发表信息

本研究由西北农林科技大学机械与电子工程学院的Wang Wang、Qin Jifeng、Huang Dezhao等团队完成，通讯作者为Yang Fuzeng和Wang Zheng。研究论文《Integrated navigation method for orchard-dosing robot based on LIDAR/IMU/GNSS》于2024年10月28日发表在开源期刊《Agronomy》（2024年第14卷，第2541页），隶属于MDPI出版集团。

学术背景与研究目标

科学领域：该研究属于农业机器人导航技术领域，涉及多传感器融合、同步定位与建图（SLAM）、路径规划等关键技术。

研究动机：中国作为全球果园面积（1280.8万公顷）和产量第一的国家，高效植保作业对提升果实品质至关重要。传统单一施药机器人因载药量有限需人工补给，而多机器人系统虽能提升效率，但其导航可靠性面临挑战——尤其在复杂果园环境中，施药机器人存在负载重、制动距离长等安全隐患，亟需高精度、高安全性的自主导航解决方案。

技术痛点：现有果园导航技术存在三大瓶颈：
1. 地图构建方面，传统激光雷达SLAM算法在杂草丛生果园中鲁棒性差（如LEGO-LOAM过度依赖地面特征）；
2. 全局定位方面，粒子滤波等方法计算复杂度高（如Blok等采用2D激光雷达的方案实时性不足）；
3. 路径规划方面，传统A*算法路径紧贴障碍物（平均安全距离仅21.3cm），存在碰撞风险。

研究目标：开发基于LIDAR/IMU/GNSS的多传感器组合导航方法，实现施药机器人的高精度定位、安全路径规划与动态避障。

研究方法与工作流程

1. 多传感器融合建图（LIO-SAM算法）

硬件配置：采用Hesai PandarXT-32三维激光雷达（水平分辨率0.18°，垂直分辨率1°）、Wheeltec N100九轴IMU（采样频率400Hz）和CHCNAV CGI-410移动站（RTK-GNSS定位误差<2cm）。

算法创新：
- 四因子图优化框架：融合IMU预积分因子（消除累积误差）、激光雷达里程计因子（校正IMU零偏）、GNSS因子（信号可靠时全局校准）和闭环检测因子（基于KD树搜索15m半径内的候选帧，ICP匹配优化位姿）。
- 点云后处理：通过高度截取（pass-through filtering）去除地面噪声，半径滤波（radius filtering）清除离散点，最终生成分辨率5cm的二维栅格地图。测试表明，所建三维点云地图能完整覆盖果树、道路等要素，无区域重叠或漂移现象。

2. 全局定位模型（RTK-GNSS/Kalman滤波）

坐标系统转换：
1. 建立地理坐标系→地磁坐标系→地图坐标系的转换模型，利用IMU初始磁方位角α对齐地图轴向，考虑激光雷达坐标系与IMU的90°水平偏角（β）及果园磁偏角ω（-4.4°）。
2. 通过UTM坐标局部化，推导变换矩阵：
$$
T_{m-u} = \begin{bmatrix} -\cosω & -\sinω \ \sinω & -\cosω \end{bmatrix}
$$

Kalman滤波融合：
- 状态矩阵：包含IMU计算的位姿、速度及航向信息，协方差矩阵Q；
- 观测矩阵：GNSS提供的经纬度、速度及航向数据，协方差矩阵R；
- 动态修正先验状态误差协方差矩阵，实现GNSS与IMU的互补（GNSS校正IMU累积误差，IMU在GNSS信号丢失时维持定位）。

测试结果：在果园四角进行的5组测试显示，初始定位平均误差2.125cm（最大3.60cm，最小0.30cm），标准差1cm，优于AMCL算法（2.68cm）和树干中心点匹配法（2.83cm）。

3. 路径规划与避障（改进A*+DWA算法）

改进A*算法：
- 前瞻预视框机制：在当前节点8邻域方向外扩矩形感知区，将预视框节点按ENU坐标系分类（如0.102π<φ<0.398π为东北方向），统计障碍物占据数构建代价函数：
$$
q(n) = \begin{cases} k{axial}\frac{n}{m} & \text{轴向节点} \ k{diag}\frac{n}{m} + k{a-d}(q{axial}(n-1)+q_{axial}(n+1)) & \text{对角节点} \end{cases}
$$
- 平滑处理：采用三阶贝塞尔曲线优化路径曲率，平均曲率变化率（MCCR）降至1.27×10⁻²，较传统A*算法（2.60×10⁻²）提升51%。

DWA动态避障：在速度空间（0.5m/s）采样线速度和角速度，基于实时激光雷达数据评分选择最优轨迹。测试中机器人成功避开速度0.15m/s的三轮运输车，保持1m安全距离。

主要研究结果

1. 地图构建：LIO-SAM算法在非结构化果园中表现优异，所建地图特征清晰（果树轮廓标准差<1cm），更新延迟<100ms。
   
2. 定位性能：GNSS/IMU组合定位最大绝对横向偏差9.82cm（均值4.16cm），在树冠遮挡场景下仍保持稳定。
   
3. 路径安全：改进A*算法路径平均安全距离50.75cm，较传统A*提升29.44cm；动态避障成功率100%。
   

结论与价值

科学价值：
- 提出首个融合LIO-SAM建图、RTK-GNSS全局定位与改进A*算法的果园机器人导航框架；
- 验证了多传感器紧耦合在复杂农业环境中的可行性，为后续研究提供方法论参考。

应用价值：
- 施药机器人导航系统可扩展至其他田间作业场景（如行栽作物）；
- 改进A*算法的”前瞻预视”机制可应用于物流AGV等需要安全避障的领域。

研究亮点

1. 创新算法：开发基于前瞻预视框的A*改进算法，解决传统方法路径紧贴障碍物的行业难题；
   
2. 工程实现：设计GNSS/地磁/地图坐标系转换模型，实现厘米级初始定位；
   
3. 系统集成：首次将LIO-SAM、Kalman滤波与DWA算法整合至果园机器人平台，完成从建图到避障的全流程验证。
   

未来方向

作者指出需进一步优化磁偏角校准方法，并探索AMCL算法与现有模型的融合，以提升抗磁干扰能力。路径规划时间（目前1.2s）的优化也将是下一步重点。