一种导航路径几何约束辅助的农业机械自主导航方法

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


背景技术：

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


技术实现要素：

3.针对现有技术中存在不足，本发明提供了一种导航路径几何约束辅助的农业机械自主导航方法，改善农机自主导航系统的精度与可靠性，提高自主导航系统数据融合方法的自适应性。
4.本发明是通过以下技术手段实现上述技术目的的。
5.一种导航路径几何约束辅助的农业机械自主导航方法，利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量；利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，在线更新量测方差矩阵基于状态转移矩阵系统噪声方差矩阵量测矩阵及得到扩增状态估计值的前6个元素即为k时刻自主导航系统的状态输出。
6.进一步的技术方案，所述扩增量测向量为其中z
k
为量测向量，为约束量测向量，且(x
k
，y
k
)、(x
k
‑1，y
k
‑1)分别为k、k
‑
1时刻农机在高斯平面坐标系下的位置，分别为k、k
‑
1时刻的农机速度。
7.更进一步的技术方案，所述k、k
‑
1时刻农机在高斯平面坐标系下的位置和农机速度满足如下关系式：
[0008][0009]
更进一步的技术方案，所述量测矩阵和量测方差矩阵分别为：
[0010][0011][0012]
其中，d表示k时刻之前的滤波时间单位；分别表示x轴向位置、y轴向位置、速度、航向角、转向角、横向偏差和横向偏差一阶差分对应的量测噪声方差；分别表示速度v
k
在x、y轴向上量测分量。
[0013]
进一步的技术方案，所述扩增量测向量为d表示k时刻之前的滤波时间单位。
[0014]
进一步的技术方案，所述状态转移矩阵和系统噪声方差矩阵分别为：
[0015][0016][0017]
其中，φ
k
为状态量x
k
的状态转移矩阵，i为单位矩阵，q
k
为量测噪声。
[0018]
进一步的技术方案，所述利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，具体为：记录至k时刻路径跟踪横向偏差集合其中t为初始化时间内路径跟踪结果记录的数据个数，且t＞2；由计算得到横向偏差一阶差分量其中计算的方差，作为量测噪声方差的初始值。
[0019]
更进一步的技术方案，所述量测方差阵采用sage
‑
husa最大后验估计在线更新。
[0020]
本发明提出了一种导航路径几何约束辅助的农业机械自主导航方法，相比现有技术，具有以下有益效果：
[0021]
(1)本发明利用农业机械导航路径往返式直线作业和速度方向恒定的特征，构造自主导航系统的量测约束方程，基于量测约束条件构造约束量测向量，进一步建立扩增量测向量；本发明提高数据融合中先验信息的权重，降低数据融合对传感器量测精度的依赖性；在不增加额外传感器的情况下，改善了农机自主导航系统的精度与可靠性；
[0022]
(2)本发明基于自动驾驶路径跟踪结果中的横向偏差及其差分形式，构造约束量测向量方差矩阵，实现量测噪声方差初始值的设置，避免了容积卡尔曼滤波器量测更新中的秩亏引起的数值计算奇异性问题，提高了自主导航系统数据融合方法的自适应性。
附图说明
[0023]
图1为本发明所述农机载体坐标系、高斯平面坐标系和导航路径示意图；
[0024]
图2为本发明导航路径几何约束辅助的农业机械自主导航流程图。
具体实施方式
[0025]
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
[0026]
如图1所示，一种导航路径几何约束辅助的农业机械自主导航方法基于现有的自主导航系统(本实施例的自主导航系统选用双天线北斗rtk定位系统)，利用高斯平面坐标系(g系，原点为o
g
、横坐标为x、纵坐标为y)下的农机导航路径几何形状约束和农机载体坐标系(b系，原点为o
b
、横坐标为x
b
、纵坐标为y
b
)下的速度方向约束，降低农机组合导航系统对传感器成本的敏感性，其中v
k
、δ
k
分别表示k时刻的航向角、速度及横向偏差，δ
k
‑1表示k
‑
1时刻的横向偏差，δ
k
‑2表示k
‑
2时刻的横向偏差。
[0027]
如图2所示，所述导航路径几何约束辅助的农业机械自主导航方法基于高斯滤波框架下的ckf算法实现，利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量，利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，在线更新量测方差矩阵基于及得到扩增状态估计值的前6个元素即为k时刻自主导航系统的状态输出。
[0028]
所述导航路径几何约束辅助的农业机械自主导航方法在高斯滤波框架下实现，自主导航系统的状态方程和量测方程分别如下所示：
[0029]
x
k+1
＝φ
k
x
k
+w
k
ꢀꢀꢀ
(1)
[0030]
z
k
＝h
k
x
k
+v
k
ꢀꢀꢀ
(2)
[0031]
其中分别表示k时刻的状态转移矩阵和量测矩阵，分别表示k时刻的状态转移矩阵和量测矩阵，分别为k时刻的系统噪声和量测噪声，x0(初始状态)、w
k
、v
k
相互独立且满足分别表示初始状态、系统噪声和量测噪声，表示均值为方差为p的高斯分布；为实数域，m为量测维数，n为状态维数。
[0032]
所述自主导航系统的状态量依次表示k时刻平面坐标系中的x轴向位置和速度、y轴向位置和速度、航向角、转向角，即n＝6；以双天线北斗rtk定位系统输出位置(x
k
，y
k
)和航向角转速传感器测量速度v
k
及转角传感器输出转角δ
k
为量测输入，设容积卡尔曼滤波器的更新周期为t，即m＝5，根据农业机械运动学方程可定义状态转移矩阵为：
[0033][0034]
其中φ
66
＝1
‑
t/t，分别表示k时刻滤波器输出的状态结果，l为农机轴距，τ为转向机构的惯性时间常数。定义量测向量对应的量测矩阵为：
[0035][0036]
其中分别表示速度v
k
在x、y轴向上量测分量。
[0037]
所述自主导航系统的量测方程特征为：利用农业机械田间作业过程往返式直线路径中位置(几何形状约束)和速度方向约束，建立直线路径作业时自主导航系统量测约束方程，其形式如下：
[0038][0039][0040]
其中(x
k
，y
k
)、(x
k
‑1，y
k
‑1)、(x
k
‑2，y
k
‑2)为农机在k、k
‑
1、k
‑
2时刻在高斯平面坐标系下的位置，为对应时刻的速度。考虑到上述方程所含信息的冗余性，量测约束条件可进一步简化成：
[0041][0042]
加入量测约束条件后可构造约束量测向量其形式如下：
[0043][0044]
自主导航系统的扩增量测向量同时考虑量测向量和约束量测向量，即扩增量测向量其对应的量测矩阵和量测噪声方差矩阵定义为：
[0045]
[0046][0047]
其中d表示k时刻之前的滤波时间单位，分别表示x轴向位置、y轴向位置、速度、航向角、转向角、横向偏差和横向偏差一阶差分对应的量测噪声方差。
[0048]
所述自主导航系统的状态方程特征为：扩增状态向量对应的状态转移矩阵和系统噪声方差矩阵定义为：
[0049][0050][0051]
其中，i为单位矩阵。
[0052]
为了与约束量测方程(6)对应，优选d＝2，以减少计算量。
[0053]
利用自动驾驶系统路径跟踪结果，实现约束量测向量方差矩阵的设置与更新，步骤为：记录至k时刻路径跟踪横向偏差集合其中t为初始化时间内路径跟踪结果记录的数据个数，且t＞2，由计算得到横向偏差一阶差分量其中k
‑
t
‑
1≤j≤k，计算的方差作为量测噪声方差的初始设置值。
[0054]
本发明的导航路径几何约束辅助的农业机械自主导航方法采用sage
‑
husa最大后验估计在线更新量测方差阵实现组合导航状态变量的自适应估计与配置更新。
[0055]
本发明的导航路径几何约束辅助的农业机械自主导航方法利用容积卡尔曼滤波器，基于及得到扩增状态估计值(为现有技术)，其中的前6个元素即为k时刻自主导航系统的状态输出。
[0056]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。