[发明专利]一种导航路径几何约束辅助的农业机械自主导航方法

权利要求书

1.一种导航路径几何约束辅助的农业机械自主导航方法，其特征在于，利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量；利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，在线更新量测方差矩阵基于状态转移矩阵系统噪声方差矩阵量测矩阵及得到扩增状态估计值的前6个元素即为k时刻自主导航系统的状态输出。

2.根据权利要求1所述的农业机械自主导航方法，其特征在于，所述扩增量测向量为其中z_k为量测向量，为约束量测向量，且(x_k，y_k)、(x_k-1，y_k-1)分别为k、k-1时刻农机在高斯平面坐标系下的位置，分别为k、k-1时刻的农机速度。

3.根据权利要求2所述的农业机械自主导航方法，其特征在于，所述k、k-1时刻农机在高斯平面坐标系下的位置和农机速度满足如下关系式：

4.根据权利要求2所述的农业机械自主导航方法，其特征在于，所述量测矩阵和量测方差矩阵分别为：

其中，d表示k时刻之前的滤波时间单位；分别表示x轴向位置、y轴向位置、速度、航向角、转向角、横向偏差和横向偏差一阶差分对应的量测噪声方差；分别表示速度v_k在x、y轴向上量测分量。

5.根据权利要求1所述的农业机械自主导航方法，其特征在于，所述扩增量测向量为d表示k时刻之前的滤波时间单位。

6.根据权利要求5所述的农业机械自主导航方法，其特征在于，所述状态转移矩阵和系统噪声方差矩阵分别为：

其中，Φ_k为状态量x_k的状态转移矩阵，I为单位矩阵，Q_k为量测噪声。

7.根据权利要求1所述的农业机械自主导航方法，其特征在于，所述利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，具体为：记录至k时刻路径跟踪横向偏差集合其中t为初始化时间内路径跟踪结果记录的数据个数，且t＞2；由计算得到横向偏差一阶差分量其中计算的方差，作为量测噪声方差的初始值。

8.根据权利要求1所述的农业机械自主导航方法，其特征在于，所述量测方差阵采用Sage-Husa最大后验估计在线更新。