基于双天线GNSS和预瞄追踪模型的农机自动导航控制方法与流程

本发明涉及一种农机定位测姿及自动导航控制方法，尤其涉及一种基于双天线gnss和预瞄追踪模型的农机自动导航控制方法，属于农业机械gnss自动导航领域。

背景技术：

作为精准农业的一项核心关键技术，农业机械自动导航广泛应用于耕作、播种、施肥、喷药、收获等农业生产过程。在农机自动导航系统中，农机位置姿态测量方法和导航路径跟踪控制方法是农机导航系统的两大核心技术。目前农机导航系统中，普遍使用具有厘米级定位精度的rtk-gps。相对于多卫星定位系统，在复杂情况下单卫星系统可能无法提供足够的卫星，或是卫星信号质量不稳定，导致导航系统的中断。拥有足够的卫星数是精密导航的前提，冗余的观测量能够增加卫星导航系统的可靠性。随着gnss四大卫星系统的逐渐成熟，全球的用户将使用支持多星系统的板卡，获得更多的导航定位卫星的信号，农机导航系统采用支持多星系统载波相位差分技术的板卡将无形中提高农机导航定位的精度和稳定性。

gps最早是为精确授时和定位服务设计，但随着对它的深入研究和应用挖掘，其潜在的高精度姿态测量能力得到了广泛的关注。传统的高精度测姿是利用惯性器件来实现的，其设备复杂、价格昂贵、维修困难、动态精度较差。航向角和横滚角是农业机械自动导航控制中的重要参数，过去普遍采用单天线gps/陀螺仪组合进行航向角的测量，测量精度容易受车体震动和陀螺仪零偏漂移的影响，在农机停车重启动过程中由于卡尔曼滤波器初始化可能造成航向角短时间的不准确，导致农机偏离预设路径影响作业质量。在开沟等慢速作业中，由于相邻定位两点距离小造成gps航向测量噪声大从而影响组合航向角精度。横滚角的测量过去普遍采用高精度ins来实现的，通过陀螺和加速度计感应运动过程中的角速度以及线加速度，通过积分和推算方法获得运动载体的姿态角。高精度ins设备复杂、价格昂贵、维修困难，不利于农机导航系统的推广。基于卫星导航系统的一体双天线gnss板卡可同时测量农机的航向角和横滚角，并且可以完美地克服过去农机姿态测量方法的缺点，具有成本低廉、精度高、实时性好、空旷地区稳定性好等优点，非常适合空旷地区农田的大面积作业。

导航路径跟踪控制方法主要包括基于农机运动学或动力学模型的控制方法、以及模型无关的控制方法。由于农机本身的复杂性、土壤等农田环境以及农机具负载的不确定性的存在，导致农机导航控制系统为一个复杂的不确定性系统。为了避免建模不准确或者模型参数剧烈变化对农机路径跟踪控制性能所产生的负面影响，采用与模型无关的预瞄追踪方法进行农机路径跟踪控制，它不涉及复杂的控制理论知识，具有控制参数少、算法设计模拟人的驾驶行为具有预见性等特点，试验中发现预瞄追踪方法提高了农机上线的快速性、稳定性以及对复杂农田路况的适应性。

技术实现要素：

本发明的主要目的在于克服现有技术的缺点与不足，提供一种低成本、高精度的基于双天线gnss和预瞄追踪模型的农机自动导航控制方法，将gnss主从天线安装在农机合理的位置，实现双天线gnss板卡对农机的定位测姿。采用分段自适应预瞄追踪方法进行农机导航路径跟踪控制，提高农机自动导航控制精度及其稳定性。

本发明的目的通过以下的技术方案实现：

本发明公开了一种基于双天线gnss和预瞄追踪模型的农机自动导航控制方法，包括下述步骤：

s1、农机采用支持多星系统多频段载波相位差分测量技术进行高精度定位，定位方法为：基准站发送差分数据包到双天线gnss板卡，双天线gnss板卡作为移动站接收差分数据包，根据修正后的载波相位观测值进行农机的高精度坐标解算，从而获取农机在wgs-84坐标系下的原始纬度、经度和高程坐标；

s11、步骤s1所述的农机采用支持多星系统的载波相位差分测量技术进行高精度定位方法，其配套的基准站应支持多星系统多频段载波相位定位，基准站发送的差分数据包为包含多星系统差分信息的数据包；

s2、农机采用支持多星系统多频段载波相位差分技术的共时钟一体双天线gnss接收机进行测姿，测姿方法为：

s21、双天线gnss接收机根据载波相位差分原理求解出主天线指向从天线的基线矢量；

s22、将gnss主天线安装在农机驾驶室顶部的右端，从天线安装在农机驾驶室顶部的左端；

s23、农机的航向角与主从天线的基线矢量在水平面上的分量垂直，通过旋转计算公式即可得到农机的航向角；

s24、农机的横滚角即为主从天线基线矢量与水平面的夹角；

s3、采用分段自适应的与速度相关的前视距离作为预瞄追踪方法中的参数对农机进行路径跟踪控制。

作为优选的技术方案，步骤s22中，所述gnss主从天线具体安装方法为：

1)为农机建立车体坐标系，车体坐标系oxy定义为以车体质心位置为原点o，横轴x指向车体前进方向，纵轴y与横轴x垂直指向车体前进方向的左侧；

2)制作双天线固定支架，双天线固定支架为长度不低于1.3m的矩形钢管，安装在车体质心正上方的拖拉机驾驶室顶部，安装原则为钢管平行于车体坐标系y轴，关于原点o对称；

3)将gnss主天线安装在双天线固定支架的右端，从天线安装在固定支架的左端，主从天线关于车体坐标系的x轴对称。

作为优选的技术方案，步骤s24中，还包括对横滚角进行滤波的步骤，具体为：

采用卡尔曼滤波方法对横滚角进行滤波，获取农机横滚角的精确估计，此卡尔曼滤波器中状态向量为农业机械的横滚角和横滚角速度二维向量；测量量为双天线gnss接收机获取的横滚角一维标量。

作为优选的技术方案，步骤s3中，采用获取的航向角、农机横滚角的精确估计值将导航控制点从农机驾驶室顶部主天线的位置投影到农机质心底部，该步骤采用欧拉角的坐标转换矩阵实现，由于农机导航稳态时基本平行于规划行行驶，俯仰角对位置偏差的影响很小，故忽略俯仰角对导航控制点在投影过程中位置偏差的影响。

作为优选的技术方案，步骤s3具体为：

在农机前进方向的规划作业行直线上设定一个农机追踪点，称作预瞄点，导航控制点指向预瞄点的矢量方向作为农机的目标航向，农机的目标航向与当前航向的差值经限幅处理后作为当前农机轮角的决策值。

作为优选的技术方案，通过导航控制点作规划作业行的垂线，导航控制点至规划作业行直线的距离即为位置偏差，预瞄点至垂线的距离称作前视距离，其中位置偏差的精度是评价农机自动导航系统优劣的核心指标，前视距离的大小直接影响到农机路径跟踪控制效果，前视距离大，则位置偏差校正作用弱，控制响应时间长，但稳定性好不会产生大的控制震荡，前视距离小，则位置偏差校正作用强，控制响应时间短，但可能会产生较大的控制震荡。

作为优选的技术方案，动态确定前视距离的思路为：

①在稳定不震荡的前提条件下，前视距离以较小为易，以尽快校正位置偏差，有益于提高导航控制精度和上线速度；

②农机较高速度行驶时由于转向角执行滞后等原因造成系统震荡加大不稳定性增强，应适当增大前视距离。

作为优选的技术方案，在固定农机速度1m/s的条件下，以不同的前视距离参数对农机的直线跟踪控制效果进行测试，从中选取最优直线跟踪前视距离参数，以此前视距离作为直线跟踪前视距离的最小值；以0.2m/s递增速度，通过试验的方法寻找不同速度条件下最优前视距离，最优前视距离与速度的二分之三幂次方成正比，将前视距离写成a+v^3/2的形式，只需调整参数a，导航系统即对速度有很好的适应性。

作为优选的技术方案，由于上线时位置偏差很大，为增加导航系统上线时的稳定性，应适当增大上线时的前视距离，将位置偏差大于15cm时的状态视为上线状态，位置偏差小于15cm时视为直线跟踪状态，上线状态时，适当加大a值。

本发明与现有技术相比，具有如下优点和有益效果：

1.本发明支持多频多星系统载波相位差分技术的定位板卡能有效增加参与定位解算的卫星数量，提高农机导航定位的精度和稳定性，提高农机对复杂农田环境的适应性；

2.本发明基于卫星导航系统的双天线gnss板卡替代高精度惯导测姿提高了农机姿态测量的实时性，大幅降低了导航系统的成本；

3.本发明采用卡尔曼滤波方法对双天线gnss板卡输出的白噪声较大的横滚角进行滤波，可形成更连续和稳定的农机横滚角数据，避免滤波后信息滞后，获得农业机械横滚角的精确估计，使得农机的倾斜校正更加准确。

4.本发明采用分段自适应前视距离的预瞄追踪模型算法设计模拟人的驾驶行为具有预见性，路径跟踪控制效果好，提高了农机上线的快速性、稳定性以及对复杂农田路况的适应性；

5.本发明中前视距离与速度相关，提高了农机自动导航系统在相对高速作业时的控制精度和稳定性。

附图说明

图1是双天线与预瞄追踪模型的农机导航控制方法流程图

图2是农机gnss双天线安装示意图；

图3是双天线板卡输出的横滚角滤波前后对比图

图4是预瞄追踪模型几何表达示意图

图5是固定参数下直线跟踪的位置偏差曲线图

图6是固定参数下上线时的位置偏差曲线图

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

本实施例采用的农业机械为雷沃m904-d轮式拖拉机，其导航系统主要包括：基准站及数据链、司南k528双天线gnss板卡、双天线固定支架、嵌入式开发板和显示终端；如图2所示为本发明实施例gnss天线安装示意图，为轮式拖拉机1建立车体坐标系，车体坐标系oxy定义为以车体质心位置为原点o，横轴x指向车体前进方向，纵轴y与横轴垂直指向车体前进方向的左侧。双天线固定支架2为长度不低于1.3m的矩形钢管，安装在车体质心正上方的拖拉机驾驶室顶部，安装原则为钢管平行于车体坐标系y轴，关于原点o对称。gnss主天线3安装在双天线固定支架的右端(车体前进方向为前)，从天线4安装在固定支架的左端，主从天线关于车体坐标系的x轴对称。设置基准站发送rtcm3.2格式差分数据包，并通过数据链传送至k528板卡，设置k528双天线板卡以10hz的频率输出nmea-0183标准格式中$gngga和$gntra数据包，嵌入式开发板中的mcu以dma方式接收串口数据，并在超时中断程序中截取$gngga数据包中的纬度、经度、高程坐标信息，截取$gntra数据包中的航向角α和倾斜角β信息。

本实施例提供了一种基于双天线gnss和预瞄追踪模型的农机自动导航控制方法，包括下述步骤：

本实施例基于双天线gnss和预瞄追踪模型的农机自动导航控制方法，，包括下述步骤：

s1、农机采用支持多星系统的载波相位差分测量技术进行高精度定位，定位方法为：基准站发送差分数据包到双天线gnss板卡，双天线gnss板卡作为移动站接收差分数据包，根据修正后的载波相位观测值进行农机的高精度坐标解算，从而获取农机在wgs-84坐标系下的原始纬度、经度和高程坐标。

s11、步骤s1所述的农机采用支持多星系统的载波相位差分测量技术进行高精度定位方法，其配套的基准站应支持多星系统多频段载波相位定位，基准站发送的差分数据包为包含多星系统差分信息的数据包。

s2、农机采用支持多星系统多频段载波相位差分技术的共时钟一体双天线gnss接收机进行测姿，测姿方法为：

s21、双天线gnss接收机根据载波相位差分原理求解出主天线指向从天线的基线矢量；

s22、将gnss主天线安装在农机驾驶室顶部的右端，从天线安装在农机驾驶室顶部的左端；

所述gnss主从天线具体安装方法为：

1)为农机建立车体坐标系，车体坐标系oxy定义为以车体质心位置为原点o，横轴x指向车体前进方向，纵轴y与横轴x垂直指向车体前进方向的左侧；

2)制作双天线固定支架，双天线固定支架为长度不低于1.3m的矩形钢管，安装在车体质心正上方的拖拉机驾驶室顶部，安装原则为钢管平行于车体坐标系y轴，关于原点o对称；

3)将gnss主天线安装在双天线固定支架的右端，从天线安装在固定支架的左端，主从天线关于车体坐标系的x轴对称。

s23、农机的航向角与主从天线的基线矢量在水平面上的分量垂直，通过旋转计算公式即可得到农机的航向角，

s24、农机的横滚角即为主从天线基线矢量与水平面的夹角；

s3、采用分段自适应的与速度相关的前视距离作为预瞄追踪方法中的参数对农机进行路径跟踪控制。

2.将wgs-84坐标系下的原始纬度b、经度l坐标转换到enu导航平面坐标系下得到gnss主天线处平面坐标(x，y)，可以采用高斯投影变换或切平面法实现，本实施例采用高斯投影变换方法，具体转换公式可查阅相关资料。

步骤s14中，还包括对横滚角进行滤波的步骤，具体为：

采用卡尔曼滤波方法对横滚角进行滤波，获取农机横滚角的精确估计，此卡尔曼滤波器中状态向量为农业机械的横滚角和横滚角速度二维向量；测量量为双天线gnss接收机获取的横滚角一维标量。

具体为：采用卡尔曼滤波器对板卡输出的横滚角度β进行滤波处理，获得拖拉机横滚角度信息的精确估计该卡尔曼滤波器中，横滚角和横滚角速度存在导数关系，系统倾斜真实角度θ用来做状态向量一个分量，采用双天线gnss板卡测量得到的横滚角估计出角速度ω，以角速度ω作为状态向量的另一个分量，相应的状态方程和观测方程：

设定此卡尔曼滤波的初始化条件是：

图3是滤波前后横滚角对比图，线1是双天线gnss接收机输出的原始横滚角，线2是经滤波后输出的农机横滚角。

根据车体航向角ψ、横滚角与gnss主从天线基线矢量的关系，以及wgs-84坐标系与enu平面导航坐标系下航向角定义的不同：wgs-84坐标系下正北方向为0°，顺时针旋转为正，航向角取值范围为[0，360°)，enu平面导航坐标系下正东方向为0°，逆时针旋转为正，航向角取值范围为[0，360°)。可以得到

步骤s3中，采用获取的航向角ψ、农机横滚角的精确估计值将导航控制点从农机驾驶室顶部主天线的位置投影到农机质心底部，该步骤采用欧拉角的坐标转换矩阵实现，由于农机导航稳态时基本平行于规划行行驶，俯仰角对位置偏差的影响很小，故忽略俯仰角对导航控制点在投影过程中位置偏差的影响。

步骤s3具体为：如图4所示，在农机前进方向的规划作业行直线上设定一个农机追踪点pp，称作预瞄点，导航控制点指向预瞄点的矢量方向作为农机的目标航向th，农机的目标航向th与当前航向ch的差值经限幅处理后作为当前农机轮角的决策值。

通过导航控制点作规划作业行的垂线，导航控制点至规划作业行直线的距离即为位置偏差pe，预瞄点至垂线的距离称作前视距离pd，其中位置偏差pe的精度是评价农机自动导航系统优劣的核心指标，前视距离pd的大小直接影响到农机路径跟踪控制效果，前视距离pd大，则位置偏差校正作用弱，控制响应时间长，但稳定性好不会产生大的控制震荡，前视距离pd小，则位置偏差校正作用强，控制响应时间短，但可能会产生较大的控制震荡。

动态确定前视距离的方法时候：

①在稳定不震荡的前提条件下，前视距离pd以较小为易，以尽快校正位置偏差，有益于提高导航控制精度和上线速度；

②农机较高速度行驶时由于转向角执行滞后等原因造成系统震荡加大不稳定性增强，应适当增大前视距离。

在固定农机速度1m/s的条件下，以不同的前视距离参数对农机的直线跟踪控制效果进行测试，从中选取最优直线跟踪前视距离参数，以此前视距离作为直线跟踪中前视距离的最小值pd_min，避免低速时前视距离太小造成系统换向时液压系统压力太大。以0.2m/s递增速度，通过试验的方法寻找不同速度条件下最优前视距离。统计试验数据，最优前视距离与速度的二分之三幂次方成正比。将前视距离写成a+v^3/2的形式，只需调整参数a，导航系统即对速度有很好的适应性。图5是当pd_min＝1.5m，a＝0，速度为1-1.5m/s时，直线跟踪的位置偏差曲线图，在这组参数下直线跟踪位置偏差绝大多数在2cm之内。

由于上线时位置偏差很大，为增加导航系统上线时的稳定性，应适当增大上线时的前视距离。将位置偏差大于15cm时的状态视为上线状态，位置偏差小于15cm时视为直线跟踪状态。上线状态时，适当加大a值。图6是当pd_min＝2m，a＝0.5，速度为1-1.5m/s时，上线时的位置偏差曲线图，在这组参数下上线速度快，且没有振荡。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。