基于改进动态窗口和A*算法的农场巡检机器人定位与导航技术

《Franklin Open》：Positioning and Navigation Technology of Farm Inspection Robot Based on Improved Dynamic Window and A* Algorithm

　　

作为智慧农业的关键终端，农场巡检机器人的定位与导航技术直接影响巡检作业的连续性和可靠性。为了实现机器人在农场环境中的毫米级定位和高效路径规划，本研究通过量化障碍物密度动态调整A*算法启发式函数的权重，并在动态窗口法（Dynamic Window Approach, DWA）中引入重合度评估函数以增强算法的预测能力。利用人工地标和多分辨率地图改进的相关扫描匹配（Correlation Scan Matching, CSM）定位方法增强了在复杂环境中的定位稳定性。结果表明，改进算法在30×30栅格地图上的总路径长度为48.470米，总转折角为351.78°，比传统算法缩短了6.4%。在5个地标观测下，改进定位算法在X和Y方向上的平均误差分别为1.998毫米和2.417毫米，航向角误差为0.335°。即使在人工地标被遮挡时，仍能保持4毫米的精度，并在实际农场环境中成功避开动态障碍物。该技术通过路径规划与定位方法的协同优化，有效提升了农场巡检机器人在复杂环境下的导航鲁棒性和定位精度，为智慧农业的自主巡检提供了技术支持。

**论文解读：基于改进动态窗口和A*算法的农场巡检机器人定位与导航技术**

**研究背景与问题**

随着全球农业向精准化和智能化转型，作物全流程监测需求日益迫切。农场巡检机器人配备多种传感器，可实时采集田间作物生长、病虫害、土壤湿度等信息，被视为实现智慧农业的关键执行终端。定位与导航技术实时估计机器人在全局坐标系下的位置和航向，并根据任务规划可行路径，确保安全行驶至目标区域。然而，农场场景存在光照变化剧烈、作物行结构相似、动态动物和人员穿梭频繁等问题，传统定位与导航算法在此环境下易出现漂移、路径冗余和避障滞后，限制了巡检作业的连续性和可靠性。现有研究多聚焦于室内或结构化道路场景，依赖全球定位系统、超宽带或视觉特征进行定位，但在农场中因信号遮挡和光照干扰导致性能下降。传统动态窗口法（DWA）仅预测下一时刻轨迹，缺乏对远距离障碍物的前瞻性评估，导致路径冗余和频繁转弯；传统A*算法基于静态权重计算成本，难以反映作物生长或动物运动引起的环境变化。此外，农场缺乏持久且易识别的自然特征，进一步放大了定位累积误差的风险。因此，为实现适应农场复杂光照和动态障碍物的定位与导航，研究人员开展了本研究。

**研究内容与结论**

本研究提出一种融合改进A*和DWA的混合路径规划（PP）策略，并引入结合人工地标和多分辨率地图的改进相关扫描匹配（CSM）定位方法，以实现毫米级定位精度和高效平滑的自主导航。研究通过量化障碍物密度动态调整A*算法启发式函数权重，并利用改进Floyd方法优化路径拐角和冗余节点；同时在DWA中引入重合度评估函数，减少轨迹偏差并增强前瞻避障能力。定位方面，基于改进CSM算法，利用人工地标和多分辨率地图，通过分层搜索策略提升复杂环境下的定位稳定性。研究结论表明，改进算法在30×30栅格地图上总路径长度48.470米，总转折角351.78°，比传统算法缩短6.4%；在5个地标观测下，改进定位算法在X和Y方向平均误差分别为1.998毫米和2.417毫米，航向角误差0.335°，即使地标被遮挡仍保持4毫米精度，并在实际农场中成功避开动态障碍物。该技术通过路径规划与定位方法的协同优化，有效提升了农场巡检机器人在复杂环境下的导航鲁棒性和定位精度，为智慧农业自主巡检提供了技术支持。论文发表在《Franklin Open》。

**关键技术方法**

研究采用的关键技术方法包括：（1）改进A*算法：通过量化障碍物密度动态调整启发式函数权重，并利用改进Floyd方法优化路径冗余节点和拐角；（2）改进DWA：在评估函数中引入重合度评估函数，计算模拟轨迹与起点到终点直线路径的重合度，增强前瞻避障能力；（3）改进CSM定位：结合人工地标（RFID标签和激光反射器）与多分辨率地图，通过分层搜索策略（从低分辨率到高分辨率）实现快速精确匹配；（4）路径跟踪控制：采用PID与模型预测控制（MPC）相结合的定位不确定性补偿策略，并集成自适应阈值逻辑，在定位误差超过阈值时自动降速或触发全局重规划。样本队列来源为模拟农场环境（30×30栅格地图）及实际10 m×15 m农场区域，包含静态植物、动物和未知动态障碍物。

**研究结果**

**4.1. 基于改进DWA和A*算法的路径规划算法验证**

在20×20和30×30栅格地图上，改进A*算法相比传统A*和参考文献[10][11]方法，路径更短、转折更少。例如，30×30地图中改进算法路径长度48.470米，总转折角351.78°，而传统A*分别为51.763米和589.34°。在动态障碍物测试中，改进A*-DWA算法能主动触发局部重规划，路径更平滑。在2、3、4个静态障碍物场景下，改进算法在总时间、路径长度和平均偏差上均优于传统A*-DWA及参考文献[12][13]方法，尤其在3个障碍物时传统算法发生碰撞，而改进算法无碰撞且总时间仅357.752秒。在动态障碍物速度从0.3 m/s增至3.0 m/s时，系统平均响应时间从85.6 ms升至124.5 ms，最大响应时间仍低于140 ms，避障成功率达到98%以上。在频繁重规划场景下，重规划间隔≥200 ms时系统性能稳定，300 ms间隔为最优平衡点。在近距离突发障碍物测试中，距离≥2 m时避障成功率达100%，即使0.5 m距离仍达85%成功率。

**4.2. 基于改进CSM算法的定位与导航方法验证**

在人工地标观测数量测试中，5个地标下X、Y方向平均误差分别为1.998毫米和2.417毫米，航向角误差0.335°，优于3个和4个地标。与自适应蒙特卡洛定位（AMCL）、Venus、传统CSM及参考文献[7]方法相比，改进CSM在X、Y方向平均误差（2.033 mm、2.954 mm）和航向角误差（0.612°）均最低，且最大误差和均方差更小。在地标遮挡场景下，即使两个地标被遮挡，平均定位误差仍保持在4 mm以内。在实际农场试验中，机器人成功检测到动态动物并主动重规划路径，安全到达目标点。在实时性方面，改进CSM定位时间仅32.7 ms（5个地标），远低于传统CSM（87.4 ms）和AMCL（124.6 ms）。在更大规模农场（50×50 m至1000×1000 m）测试中，定位误差始终在4.5 mm以内，航向角误差低于1.5°，路径规划时间在500×500 m以内小于80秒。在雨、尘、雪三种极端天气下，定位误差虽有所增加（尘条件下X/Y误差达5.233 mm/5.941 mm，航向角1.67°），但未出现定位失败。在三个生长阶段的农场自主巡检中，早期种植和收获后阶段任务成功率达100%，盛长期虽有一次短暂定位丢失，但通过增量地图更新机制在3秒内恢复，任务成功率仍达90%。

**结论与讨论**

本研究通过改进A*算法和DWA的混合路径规划，以及基于人工地标和多分辨率地图的改进CSM定位方法，实现了农场巡检机器人的毫米级定位和高效平滑导航，解决了传统算法在农场场景中的路径冗余、避障滞后和定位漂移问题。研究结论为：改进算法在模拟和实际环境中展现出优越的路径优化能力、避障可靠性和定位精度，为智慧农业自主巡检提供了技术支撑。当前研究聚焦于单机器人场景，存在对人工地标依赖和缺少多机器人协作验证的局限性。未来工作将向四个方向推进：开发自供电仿生低功耗地标技术以减少对预装静态地标的依赖；构建融合多源传感器（如视觉、IMU）的分布式协作导航框架，设计动态任务分配算法；基于改进DWA-A*算法引入时空窗口路径预留和优先级交叉管理，解决多机器人路径冲突；探索最优地标布局和抗遮挡设计，利用历史位姿、邻机共享数据或辅助环境特征保持定位精度。