基于仿真的多因子噪声感知自适应纯追踪结合因果扩展卡尔曼滤波-萨维茨基-戈雷位姿预处理用于履带式农业机器人

《Sensors》：Simulation-Based Multi-Factor Noise-Aware Adaptive Pure Pursuit with Causal EKF-SG Pose Preprocessing for Tracked Agricultural Robots

　　

在类温室环境中作业的自主履带式农业机器人，准确且平滑的路径跟踪十分重要。现有自适应前视纯追踪方法主要依据车速或路径几何调整前视距离，时变定位可靠性的影响未被充分考虑。研究人员提出一种噪声感知自适应纯追踪控制器，将扩展卡尔曼滤波(EKF)估计与因果萨维茨基-戈雷(SG)端点平滑相结合，并通过由新息导出的定位噪声指示、归一化车速、横向误差和路径曲率共同构造有界前视律。在给定定位扰动下的直线、圆弧、S形和U形参考路径数值仿真中，0.5 m位置噪声条件下，所提方法横向均方根误差(RMSE)为0.087 m、角速度均方根(RMS)为0.28 rad/s，传统固定前视纯追踪为0.112 m与0.36 rad/s；相比比例-积分-微分(PID)、Stanley、模型预测控制(MPC)与常规纯追踪，所提方法在跟踪精度与控制平滑性间取得有利平衡，且比MPC计算效率更高，同时保持几何控制的低计算负担优势。灵敏度分析中，位置噪声由0.1 m增至0.8 m时，所提方法RMSE相对增幅36.5%，常规纯追踪为103.4%。结果表明该轻量级噪声感知策略在给定数值条件下可改善跟踪精度、控制平滑性与定位扰动容忍度，为低速温室农业机器人提供实用设计参考。

研究背景方面，受保护农业智能化发展使温室自主导航需求上升。与开放道路或结构化工业场景不同，温室具有通道狭窄、路径曲率频繁变化、作物遮挡严重以及金属骨架强多径反射等特征。履带式移动机器人受非完整约束，不能通过横向速度直接转向，而需左右履带差速耦合实现转弯，因此路径跟踪控制器须适应复杂路径几何、环境感知噪声与底盘非线性动态响应。经典纯追踪(Pure Pursuit, PP)原理简单、计算负担低、实时性强，但固定前视距离难以满足复杂温室与变曲率路径的高精度要求。Stanley控制器用横向与航向误差改进几何校正，但噪声、侧滑和速度升高会引起转向振荡；PID简单，但微分项放大高频测量噪声，复杂路径易超调与抖动；模型预测控制(MPC)可纳入预览与约束，但在嵌入式农业平台上在线优化增加计算与调参负担；现有自适应纯追踪多按车速、路径曲率等调整前视距离，定位可靠性对前视调节的直接影响在类温室履带场景仍研究不足。研究人员因此在仿真可行性评估范围内，开展融合位姿预处理与新息定位噪声指示的有界自适应纯追踪研究，相关论文发表在《Sensors》。

主要关键技术方法方面，研究人员构建定位观测含随机与间歇扰动模型，采用扩展卡尔曼滤波(EKF)做状态预测与修正，再用窗口长度11、多项式阶数3的因果萨维茨基-戈雷(SG)端点平滑处理EKF后验位姿；以EKF新息平方归一化构造低通记忆式定位噪声指示；联合归一化车速、横向误差、路径曲率与定位噪声设计有界多因子自适应前视距离律；纯追踪曲率指令由滤波位姿、前视距离与目标点航向误差生成，并转为差速履带速度指令。仿真在MATLAB R2021b/Simulink以0.02 s离散周期、0.5 m/s定速、角速度限1.2 rad/s下，于直线、圆、S形、U形理想参考路径及0.1/0.3/0.5/0.8 m规定位置噪声等级上开展，无现场实测温室样本队列。

研究结果方面，2. Method中，2.1 Localization Signal Source and Noise Characterization说明观测模型分离标称位姿、随机测量噪声与间歇定位扰动，观测先经EKF再经因果SG平滑后进入控制。2.2 Noise-Robust Pose Estimation Framework给出状态含位置、线速度与航向，离散状态转移与观测模型，EKF预测与修正公式，以及11样本3阶因果SG平滑；新息及其协方差保留用于定位质量评估，SG位姿用于最近路径点与误差计算。2.3 Multi-Factor Dynamic Adaptive Look-Ahead Distance Design将车速、横向误差、曲率归一化，用新息平方与低通系数0.92、更新系数0.08得到定位噪声指示，再由基前视距离与权重系数计算有界前视距离；车速与噪声项增大前视以抗短时常波动，误差与曲率项减小前视以提升局部校正，参数经分阶段启发式整定。2.4 Adaptive Pure-Pursuit Control Law and Stability Analysis给出纯追踪曲率命令κ=2 sin θ / L_d，差速履带执行；取横向误差与航向误差构造李雅普诺夫函数V，在参考曲率有界、车速有限、前视距离有界假设下，自适应纯追踪对两类误差构成负反馈，李雅普诺夫函数非增，依LaSalle不变原理误差有界并趋近平衡点。

1. 3.
   Simulation Experiments and Results中，3.1 Simulation Platform and Parameter Settings说明在MATLAB R2021b/Simulink建立含定位输出、EKF–SG预处理、动态前视、自适应纯追踪的闭环仿真；四类路径用数学式定义，位置噪声主对比0.5 m，灵敏度0.1/0.3/0.5/0.8 m，航向噪声2°、脉冲扰动独立施加，EKF与SG提高送入控制器信号连续性。3.2 Typical Path-Tracking Results表明直线上所提方法回靠快、角速度变化小；圆弧上依曲率自动缩短前视、转向连续无尖峰；S形上入弯前缩前视、出弯恢复前视，抑制延迟与超调；U形大角度转弯处形成更紧凑轨迹，避免固定前视的外扩与Stanley式角速度剧变；EKF–SG使原始含高斯与视距非视距(NLOS)突扰位姿变为连续输入。
2. 4.
   Performance Evaluation and Robustness Validation中，4.1 Comparison of Tracking Performance在0.5 m位置噪声下所提方法RMSE 0.087 m、最大横向误差0.198 m、角速度RMS 0.28 rad/s，优于固定前视纯追踪的0.112 m、0.267 m、0.36 rad/s，且单步计算低于MPC。4.2 Robustness Against Localization Noise显示0.1 m到0.8 m位置噪声下，PID、Stanley、MPC、常规纯追踪、所提方法相对RMSE增幅依次增大，所提方法36.5%最小、常规纯追踪103.4%最大，新息残差增大会抬高噪声指示并增大前视从而降低短测量波动敏感度。4.3 Dynamic Characteristics of Look-Ahead Distance在S形路径上车速恒定0.5 m/s，前视距离随高曲率区下降、曲率减小时回升，横向误差与噪声估计进一步调制，且始终处于预设上下界。4.4 Ablation Study on Adaptive Control Modules表明完整模型RMSE与角速度RMS最低，去曲率项RMSE升幅最大，去定位噪声项角速度RMS升幅最大，四因子在给定数值条件下互补；研究人员同时指明该方法不含避障与重规划、未建详细履带滑移与作动器动态、结果为单次仿真描述而非统计验证。

讨论与结论部分总结为，该方法在理想化数值条件下展示出比常规纯追踪更优的精度、平滑性与噪声鲁棒性，且计算负担低于MPC，保留纯追踪结构简单与实时特征；局限包括无真实温室现场实验与实测定位数据集、路径为理想基准、运动为差速履带运动学简化模型、因果SG可能引入处理延迟、性能依赖归一化值与权重、结果属单次运行描述而无重复试验不确定性区间。未来需实测温室定位数据、硬件实验、履带滑移与动态建模、参数灵敏分析及重复试验统计评估。

研究人员在Conclusions中给出：本研究通过理想类温室参考路径数值仿真，考察了履带式农业机器人噪声感知自适应纯追踪策略；控制器融合EKF位姿估计、因果SG端点平滑，以及联合车速、横向误差、路径曲率与新息导出定位噪声指示的有界前视律。在规定的数值定位扰动下，EKF–SG流水线提供更平滑位姿反馈并降低所建随机与间歇测量扰动对后续控制输入的影响；多因子自适应前视按仿真车辆状态、路径几何、跟踪误差与定位噪声指示调节前视距离，0.5 m位置噪声下所提方法RMSE 0.087 m、角速度RMS 0.28 rad/s，常规固定前视为0.112 m与0.36 rad/s，0.1 m到0.8 m相对RMSE增幅所提36.5%、常规103.4%，表明规定数值扰动下敏感度更低但属单次描述结果；记录计算时间显示所提控制器比MPC比较器计算负担更低，具类温室实时导航潜力但须在真实履带机器人验证；研究人员承认无真实温室田间实验与实测数据集、路径理想化、未建履带滑移与作动器动态、SG可能引入延迟、结果无重复试验统计检验，未来应补充实测数据、硬件实验与动态建模及统计评估。