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

说明书

本发明提供了一种导航路径几何约束辅助的农业机械自主导航方法，属于智能农业机械领域。本发明利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量；利用自动驾驶系统路径跟踪结果，设置量测噪声方差的初始值，在线更新量测方差矩阵基于状态转移矩阵系统噪声方差矩阵量测矩阵及得到扩增状态估计值的前6个元素作为k时刻自主导航系统的状态输出。本发明能提高农机自主导航系统的精度、可靠性和自适应性。

技术领域

本发明涉及智能农业机械领域，具体涉及一种导航路径几何约束辅助的农业机械自主导航方法。

背景技术

农业机械自动驾驶基于导航定位感知系统、路径规划软件、转角传感器和转向执行机构实现农机的导航控制决策与路径跟踪。导航定位感知系统的精度和可靠性对自动驾驶系统性能影响较大，现有的自动驾驶系统多基于高精度卫星定位、视觉导航、惯性导航及其多源数据融合方法实现农机位姿的实时更新。单一的导航系统常常难以满足实际生产的需要，卫星定位易受传输空间电磁波干扰和环境遮挡的影响，视觉传感器受光照环境影响显著，惯性导航解算误差随时间累积，因此多源数据融合的导航方法能取长补短，实现高精度高可靠的导航参数更新，受到广泛的关注。常规的组合导航系统其性能受传感器精度和数据融合方法影响较大，近几年先进的数据融合方法获得快速发展，但高精度的几何空间定位系统价格仍居高不下，且先进数据融合方法对系统硬件要求也较高，造成研制高精度、高更新率的组合导航系统成本较高，且该状况短时间内难以改变，阻碍了农机自动驾驶系统的大面积推广应用。

发明内容

针对现有技术中存在不足，本发明提供了一种导航路径几何约束辅助的农业机械自主导航方法，改善农机自主导航系统的精度与可靠性，提高自主导航系统数据融合方法的自适应性。

本发明是通过以下技术手段实现上述技术目的的。

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

进一步的技术方案，所述扩增量测向量为其中z_k为量测向量，为约束量测向量，且(x_k，y_k)、(x_k-1，y_k-1)分别为k、k-1时刻农机在高斯平面坐标系下的位置，分别为k、k-1时刻的农机速度。

更进一步的技术方案，所述k、k-1时刻农机在高斯平面坐标系下的位置和农机速度满足如下关系式：

更进一步的技术方案，所述量测矩阵和量测方差矩阵分别为：

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

进一步的技术方案，所述扩增量测向量为d表示k时刻之前的滤波时间单位。

进一步的技术方案，所述状态转移矩阵和系统噪声方差矩阵分别为：

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