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

武 涛 李彦明 徐长赓 刘汉文 陈小倩 刘成良

(1.上海交通大学机械与动力工程学院， 上海 200240； 2.上海上实现代农业开发有限公司， 上海 202183)

0 引言

在水稻直播机实际的自主作业过程中，传感器易受到环境状况的干扰导致测量输出不准确或者某些状态难以测得。此外，由于水田环境结构复杂多变，也会给直播机带来许多不确定性扰动[1-3]。因此，为提高水稻直播机在自动驾驶过程中的抗干扰能力以及得到较为准确合理的状态值，需采取一定的状态估计方法。

许多学者在自动驾驶车辆状态估计方法上进行了大量的探索研究[4-11]。作为一种基于模型在隐马尔可夫链上的递归贝叶斯估计，卡尔曼滤波(Kalman filter, KF)相关理论已非常成熟[12-13]，被证明是无约束系统最小方差意义下的最优估计，目前是无约束动态系统状态估计应用最普遍的方法[14-17]。基于线性模型的KF针对非线性模型无法实现精确估计，而采取将非线性模型线性化的扩展卡尔曼滤波(Extended Kalman filter, EKF)和基于UT变换的无迹卡尔曼滤波(Unscented Kalman filter, UKF)能较精确地实现对无约束非线性模型状态的估计[18-22]。文献[18]提出一种基于EKF的多传感融合通用框架，用于微型飞行器导航定位。该算法理论上能处理无限多类型传感器及其延迟，以及传感器相对或绝对测量，最终对飞行器的状态信息进行估计。文献[20]分别采用EKF和UKF估计航天器的位置和速度，结果表明UKF的平均估计精度较高，因为使用先验测量分布的无迹变换(UKF)比基于线性近似的EKF更接近原始分布。文献[22]使用UKF融合GPS、IMU和车轮编码器的测量值对车辆姿态进行估计，使用Stanley控制器作为路径跟踪控制算法，提高了车辆自动驾驶的定位精度和鲁棒性。虽然卡尔曼滤波被广泛应用，但是它不能处理状态量的约束，有可能出现不符合物理实际的估计值，并且它要求系统模型必须准确，否则可能出现发散的情况。

基于滚动优化原理的滚动时域估计(Moving horizon estimation, MHE)通过将状态估计问题转化为固定时域的优化问题，能较好地处理状态量的约束和带有不确定性的非线性系统，目前在跟踪定位、故障诊断、参数估计等领域得到了广泛的研究与应用[23-30]。文献[25]使用MHE状态估计方法解决传感器具有时滞、丢失值和不同频率的问题，此外，还引入了线性模型MHE的高效计算方法，但仅适用于可以找到线性MHE问题解析解的情况。文献[27] 针对移动机器人的位姿估计问题提出一种基于MHE高效通用的多传感器融合方案，此方案基于多线程架构，降低了计算成本。利用仿真和真实实验数据对该方案进行测试，并与UKF进行比较，结果表明MHE估计性能优于UKF。文献[30]基于纵滑侧偏联合工况下的八自由度整车模型，并考虑车辆系统现实存在的物理约束，使用MHE对车辆的速度等参数进行了估计，估计结果较为符合实际情况。

针对无人水稻直播机自动驾驶过程中难以获得精确的车辆模型以及传感器测量存在误差等问题，本文设计一种基于滚动时域的车辆运动状态估计方法，在每个采样区间内通过求解优化问题的极小值估计出车辆在当前时刻的状态值：横向偏差和航向角偏差，以提高车辆的抗干扰能力和路径跟踪控制精度。

1 滚动时域估计原理

基于模型预测滚动优化原理来讨论MHE的数学原理，受扰动离散的非线性状态空间表达式为

(1)

式中k——采样时刻x(k)——状态变量

u(k)——控制输入w(k)——外部干扰

z(k)——测量向量v(k)——测量噪声

f、h——非线性过程和测量模型

状态值和测量值历史序列为

(2)

(3)

式中Qi、Ri——正定的惩罚矩阵，反映对模型干扰和测量噪声的程度，通常为方差的对角矩阵

通过最小化式(3)可以求出历史所有状态的估计值，只取最后一个估计值作为当前时刻的状态值。新的采样时刻到来，将新的测量数据补充进测量序列，在线重新求解式(3)。但随着时间的增加，数据越来越多，优化问题越来越复杂，直至不可解。并且随着时间的推移，过久的数据对当前状态的影响逐渐降低，因此，引入固定时域窗口N，考虑固定窗口的滚动时域估计，只使用最近N个状态值和测量值，有

(4)

将系统模型作为等式约束，加上状态和控制的不等式约束，用优化问题替代全信息MHE问题,有

(5)

(6)

其中

(7)

式中Fk、Hk——系统模型和测量模型雅可比矩阵

由此，通过MHE算法将状态估计问题转换成固定时域的优化问题，处理了系统约束、模型干扰和非线性以及测量噪声，能提高估计的合理性和准确性。

2 基于MHE的直播机运动状态估计器

2.1 带扰动的直播机运动学模型

要控制车辆首先要对车辆的运动建立数学模型，模型越精确对车辆的运动描述就越精确。由于在高度非结构化水田环境下，直播机动力学模型参数难以获取，因此建立简单易用的运动学模型。不考虑车辆在垂直方向的运动，假设车身系统为刚性，左右侧车轮具有相同的转向角度和速度，忽略车辆动力学特性，将直播机简化为二轮车模型，如图1所示。图中，M(xm,ym)为车体控制点；A(xa,ya)、B(xb,yb)为期望路径P上的两点;θp为航向角的期望值。用横向偏差、航向角偏差、前轮转角、行驶速度和前后轮轴距等参数建立直播机的运动学模型，从而描述直播机的运动状态。

图1 水稻直播机运动学模型Fig.1 Kinematic model of rice seeding machine

由于期望路径AB固定，所以航向角偏差的导数与航向角速度相等，即

(8)

式中v——速度，m/s

R——转弯半径，m

θ——航向角，(°)

θe——航向角偏差，(°)

由图1中几何关系可得

(9)

式中L——前后轮轴距，m

δ——前轮转角，控制输入，(°)

因此

(10)

横向偏差的导数即为速度v在该方向上的分量，即

(11)

式中de——横向偏差，m

以横向偏差de和航向角偏差θe为状态变量，同时考虑水田中地况复杂多变具有许多不确定性干扰，为避免未知干扰导致建模不准确，引入复合干扰项：横向位置干扰|w1|≤P和航向角干扰|w2|≤K，P和K为干扰的最大值，由式(10)、(11)可得带干扰的直播机运动学模型为

(12)

在实际应用中使用的是车辆离散模型，因此需对连续模型进行离散化。令采样周期为T，对式(12)进行前向欧拉离散化得

(13)

式(13)离散模型的状态量是基于横向偏差和航向角偏差，可由车辆装载的RTK-GPS传感器测出的位姿信息与期望位姿信息计算，由此得测量模型

(14)

式中v1、v2——测量噪声

2.2 MHE估计器方案

式(13)、(14)为直播机离散的非线性状态空间表达式，可代入式(1)，通过MHE的求解公式(5)进行直播机运动状态：横向偏差和航向角偏差的估计。可得MHE算法流程：

(3)根据式(6)、(7)计算时刻k处状态的后验估计协方差矩阵Pk。

(4)在k+1时刻，得到新状态的测量值，更新窗口N内的数据，重复步骤(2)、(3)。

优化问题(式(5))的计算成本高度依赖于窗口N的大小，如果计算时间超过了传感器的最大采样周期，估计出的状态则不是当前采样时刻的，需采取一定的方法提高MHE的计算效率。如图2所示，提出一种基于多线程的系统架构，为M个主要传感器的数据接收、分析与处理开辟单独的线程，用来构建测量矩阵，最后传递给MHE求解线程进行优化问题的求解。多个线程并行工作，可减少MHE总体计算时间，提高MHE采样频率。

图2 多线程MHE求解架构Fig.2 Multi-threading architecture of MHE

图3 多频率传感器解决方案Fig.3 Solution of multi-frequency sensors

如图3所示(图中τ为所有测量传感器采样周期最大值)，该多线程架构也能解决多个传感器具有不同的测量频率。图中不同多边形代表每个传感器产生的有效值，虽然不同传感器的数据可能会在不同时刻收到，但每个传感器线程都会在有新数据到达时更新数据，因此传给MHE求解线程的数据都是最新窗口内的。针对传感器出现的异常值使用马氏距离阈值法进行检验剔除，计算式为

(15)

ζj——马氏距离阈值

3 仿真验证

使用Matlab仿真验证MHE状态估计算法的有效性。根据图4控制系统结构图建立相应的仿真模型。控制算法采用文献[3]提出的快速终端滑模控制器，控制车辆从起点移动到终点，通过向直播机运动学模型和状态值加高斯白噪声来模拟系统干扰和测量误差。使用MHE和EKF对带噪声的状态进行估计并与系统状态真值比较，观察MHE和EKF的估计效果。直播机系统控制仿真参数同文献[3]，MHE仿真参数选取如表1所示。

图4 控制系统结构图Fig.4 Block diagram of control system

考虑MHE计算时间不能超过传感器采样周期0.05 s，经过仿真测试选取时域窗口值N等于5，单次计算所需时间约为0.01 s，满足在线实时估计要求。使用高效的优化控制软件CasADi表述优化问题，然后使用内点法求解该优化问题。图5、6是整个行驶过程中车辆横纵坐标和航向角变化，蓝色曲线是仿真得到的系统真值，红色曲线是带噪声的测量值，绿色曲线在图5、6中分别是使用MHE和EKF估计出的状态值。可以看出，测量值具有很大的不确定性，在真值附近波动，使用MHE和EKF能有效地从测量值中估计出较为准确的状态值。

表1 MHE仿真参数Tab.1 Simulation parameters of MHE

图5 MHE状态估计结果Fig.5 States estimation results with MHE

图6 EKF状态估计结果Fig.6 States estimation results with EKF

图7为MHE和EKF状态估计绝对误差散点图，平均绝对误差如表2所示。可以看出，MHE和EKF状态估计误差总体上比测量值误差小很多，说明二者具备从不确定信息中提取精确信息的能力。此外，MHE状态估计误差也比EKF状态估计误差小，说明基于优化方法求解的滚动时域状态估计优于传统的基于估计误差协方差的状态估计，证明了本文所提MHE状态估计算法的有效性。

图7 MHE与EKF状态估计绝对误差Fig.7 Estimation error of MHE and EKF

表2 状态估计平均绝对误差Tab.2 Mean absolute error of states estimation

4 试验验证

4.1 试验平台

试验采用中国洋马农机有限公司生产的YR10D型水稻直播机，对该型号直播机进行自动驾驶改造，如图8所示。自动驾驶系统主控制器采用装载Linux系统的工业计算机，具有4核1.6 GHz高性能处理器、1 GB DDR3运行内存、4路带隔离的3线 RS-232串口，1路CAN总线以及1路100 M以太网等通信接口。上位机主控制器通过CAN/RS-232的通信方式与车身各子系统进行数据交互与协作，完成直播机自动驾驶和自主作业。下位机控制器为自行设计的STM32嵌入式控制器，具备CAN/RS-232通信方式。采用北京天海科公司生产的DWQT-CAN-360-G型转角传感器，精度为0.1°，通信方式为CAN总线。RTK-GPS使用4G网络差分基站，动态定位精度在±0.02 m，数据刷新率为20 Hz，通信方式为RS-232。方向盘使用直流电机驱动，采用位置-速度双闭环控制，通信方式为RS-232。

图8 无人驾驶水稻直播机Fig.8 Unmanned rice seeding machine

该无人水稻直播机具备自动驾驶作业、精量播种、种箱容量检测及堵塞报警、自动倒车和全地块封边作业等功能。同时在阿里云服务器上搭建云监控管理系统，能实现远程监控、路径规划及多机协同作业管理。贯彻了“智慧在云，智能在端，管控在屏”的未来无人农场设计理念。

4.2 水稻直播机水田无人作业试验

图10 横向偏差与航向角偏差估计结果Fig.10 Estimation results of lateral deviation and heading angle deviation

为验证本文所提MHE估计算法对无人水稻直播机运动状态的估计效果，在上海市崇明区上实现代农业开发有限公司建立的无人农场里使用图8所示直播机进行实际无人播种作业。图9为直播机一次试验时部分作业轨迹，可以看出，虽然直播机行驶轨迹总体较为平稳，但由于水田中具有许多不确定性干扰以及传感器测量带有噪声，直播机行驶轨迹在某些区域也会出现一定抖动。为了获取更加准确的状态值从而抑制这些抖动，获取8段直线行驶路径的直播机运动状态de和θe，使用MHE算法进行估计。

图9 直播机作业轨迹Fig.9 Driving paths of rice seeding machine

选取路径4分析时域窗口N的大小对MHE估计效果的影响。图10为N等于1、3、5、10时，使用MHE对横向偏差与航向角偏差的估计结果。可以看出，由于水田中具有许多不确定性干扰以及传感器测量带有噪声，测量值在短时间内存在较大跳变，控制器算出的控制量也会不稳定，从而导致车辆发生抖振。可以看出，MHE估计后的状态变化相比测量值更加平稳，具有一定抑制干扰的能力。根据MHE的原理可知，N等于1时MHE算法是一种EKF算法，此时估计值与测量值基本吻合，只在波峰波谷处对测量值的不稳定具有一定的抑制作用。N越大对系统干扰和测量误差的抑制能力越强，但过大不能反映正常的状态变化，因为随着时间的推移，越早的数据对当前时刻的参考意义越小。如N等于10时，MHE估计值偏离测量值过多，不符合实际情况。因此，为保证MHE对系统干扰和测量误差的抑制具有良好的效果，同时也能较好地反映状态的真实变化，一般N取3～5，此时整体估计效果也优于EKF算法(N=1)。

图11为N=3时，使用MHE算法对直播机 8段路径横向偏差与航向角偏差的估计结果。可以看出，8段路径中横向偏差基本保持在-0.1～0.1 m，航向角偏差基本保持在-2°～2°，但横向偏差测量值出现全程不平稳现象。MHE估计算法能有效改善这种现象，使得状态值变化平稳同时也能反映出实际变化趋势。说明MHE能有效消除系统干扰和测量误差对横向偏差的影响。相对来说航向角偏差变化较平稳，没有较多噪声，MHE估计算法对减小其变化幅值起到一定作用。对试验数据分析发现，每段路径相对于理想轨迹都会有一定的横向位置和航向角稳态偏差。如表3，各段路径稳态偏差基本一致，其中横向稳态偏差均值为-0.068 8 m，航向角稳态偏差均值为-1.467 0°。使用MHE估计后各稳态偏差大幅下降，横向稳态偏差与航向角稳态偏差均值分别为-0.019 4 m和-0.366 5°，说明MHE估计算法能消除干扰给状态带来的稳态偏差。

图11 所有路径横向偏差与航向角偏差估计结果Fig.11 Estimation results of lateral deviation and heading angle deviation of all driving path

各段路径估计结果显示，本文提出的基于滚动时域的水稻直播机运动状态估计算法能从不确定的状态信息中得到较为准确的状态值，能较好地消除系统干扰和测量误差 ，对下一环节的精确控制具有实际意义。

表3 各段路径横向与航向角稳态偏差Tab.3 Steady-state error of lateral and heading angle

5 结论

(1)基于带系统扰动的非线性水稻直播机运动学模型和带噪声的测量模型，提出一种基于滚动时域的无人水稻直播机运动状态估计算法。同时，为提高MHE状态估计算法的计算效率和解决传感器采样频率不同的问题，提出一种基于多线程的MHE求解架构。使用Matlab/CasADi进行系统仿真，结果表明，本文提出的MHE状态估计算法能有效补偿系统干扰和消除测量噪声，得到接近理论真值的状态，而且MHE的估计效果优于EKF。

(2)在上海市崇明区无人农场进行了水稻直播机无人作业试验，使用MHE算法对某段直线路径中测得的横向偏差和航向角偏差值进行估计。试验结果显示，时域窗口N取3～5时，MHE对补偿系统干扰和消除测量误差具有良好的效果，同时也能较好地反映状态值的真实变化。

(3)使用MHE状态估计算法对水稻直播机8段路径的横向偏差与航向角偏差进行估计。结果显示，MHE算法对消除横向位置、航向角的稳态误差和抑制测量值的不平稳性具有良好的效果，同时也能反映出实际状态变化趋势，证明了MHE状态估计算法在补偿系统干扰和消除测量误差方面的优异性。