Authors

Abstract

To address the instability of trajectory tracking in unmanned orchard inspection vehicles caused by satellite navigation signal attenuation and track slippage, an integrated control method based on pose correction and an improved Stanley controller was proposed. First, a pose-correction algorithm based on the interacting multiple-model extended Kalman filter (IMM-EKF) was developed. By incorporating the nonlinear dynamic characteristics of tracked vehicles, the algorithm improved the robustness of pose estimation under varying motion states and measurement conditions. Second, an improved Stanley trajectory-tracking controller incorporating feedforward control and dynamic parameter adjustment was designed based on the corrected pose estimates. By accounting for path curvature in advance, the controller optimized the control commands and reduced lateral and heading errors. Simulations conducted using the Robot Operating System (ROS) platform and field tests performed in an actual vineyard were used to evaluate the proposed method. At a representative operating speed of 1.0 m/s, the mean lateral error during continuous multi-route trajectory tracking was 0.019 m, and the mean root-mean-square error (RMSE) was 0.021 m. These results demonstrate that the proposed method can improve trajectory-tracking accuracy, stability, and smoothness under the tested orchard conditions.

Abstract in Chinese

针对果园复杂环境下卫星导航信号衰减与履带滑移引起的无人巡检车轨迹跟踪不稳定问题，本文提出了一种基于位姿修正与改进 Stanley 算法的综合控制方法。首先，构建了基于交互式多模型扩展卡尔曼滤波（IMM-EKF）的位姿修正算法。该算法结合履带车辆非线性动力学特征，提高了车辆在运动状态和观测条件变化下的位姿估计鲁棒性。其次，基于修正后的位姿数据，设计了一种融合前馈控制策略与动态参数调整的改进 Stanley 轨迹跟踪控制器。该控制器通过提前考虑路径曲率信息，对轨迹跟踪控制指令进行优化，从而减小横向误差与航向误差。基于机器人操作系统（Robot Operating System, ROS）平台的仿真分析与真实葡萄园环境下的实车试验结果表明，在 1 m/s 的典型作业速度下，车辆在多路线连续轨迹跟踪中的平均横向误差为 0.019 m，平均均方根误差（RMSE）为 0.021 m。结果表明，所提方法在测试果园条件下能够提高车辆轨迹跟踪的精度、稳定性与平顺性。