基于滚动时域的无人水稻直播机运动状态估计

Motion States Estimation for Unmanned Rice Seeding Machine Based on Moving Horizon Estimation

针对农业机械自动驾驶中非线性车辆模型具有未知干扰输入,以及测量输出不确定等问题,提出一种基于滚动时域的车辆运动状态估计方法(MHE)。将状态估计问题转化为固定时域的优化问题并充分考虑约束条件,从而实现对带约束非线性模型状态的估计。为提高MHE的计算效率并且考虑传感器采样频率不同,以及可能出现测量值缺失或异常,设计出一种多线程运行架构,使MHE更适合实际应用。使用Matlab建立水稻直播机自动驾驶仿真系统,仿真结果表明,MHE算法能有效补偿系统干扰和消除测量噪声,MHE估计出的横纵位置和航向角相比扩展卡尔曼滤波(EKF)估计出的更接近系统真值。使用MHE状态估计算法对水稻直播机无人作业过程中测得的横向偏差与航向角偏差进行估计。结果表明,时域窗口N取3~5时,MHE算法对消除状态的稳态误差和抑制测量值的不平稳性具有良好的效果,同时也能较好地反映状态值的真实变化,证明了MHE算法在补偿系统干扰和消除测量误差方面的优异性。

Aiming at the problems that there is a lot of uncertain disturbance in the nonlinear vehicle model of agricultural machinery,and the measurement is often with noise,the moving horizon estimation(MHE)method for vehicle motion state was proposed.The state estimation problem was transformed into a fixed time domain optimization problem and the constraint conditions were fully considered.In order to improve the computational efficiency of MHE,taking into account the different sampling frequencies of sensors and the possibility of missing or abnormal measurement values,a multi-threading architecture was designed.The multi-threading architecture also can make MHE more suitable for practical applications.The automatic driving simulation system of the rice seeding machine was established by Matlab.The simulation results showed that MHE can effectively suppress system disturbance and measurement noise.The x and y positions and heading angle estimated by MHE were closer to the truth value than those estimated by extended Kalman filter(EKF).MHE was used to estimate the lateral deviation and heading angle deviation measured during the autonomous driving process of the rice seeding machine.The results showed that when the time domain window N was 3~5,the MHE algorithm had a good effect on suppressing the jump of measurement value,and it can also reflect the real trend of state value.It proved that MHE had excellent performance in suppressing system disturbance and measurement noise.