[发明专利]基于双天线GNSS和预瞄追踪模型的农机自动导航控制方法

权利要求书

1.基于双天线GNSS和预瞄追踪模型的农机自动导航控制方法，其特征在于，包括下述步骤：

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

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

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

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

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

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

S24、农机的横滚角即为主从天线基线矢量与水平面的夹角，采用卡尔曼滤波方法对横滚角进行滤波，获取农机横滚角的精确估计，此卡尔曼滤波器中状态向量为农机的横滚角和横滚角速度二维向量；测量量为双天线GNSS接收机获取的横滚角一维标量，具体为：

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

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

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

S3、采用分段自适应的与速度相关的前视距离作为预瞄追踪方法中的参数对农机进行路径跟踪控制，即在农机前进方向的规划作业行直线上设定一个农机追踪点，称作预瞄点，导航控制点指向预瞄点的矢量方向作为农机的目标航向，农机的目标航向与当前航向的差值经限幅处理后作为当前农机轮角的决策值；

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

动态确定前视距离的思路为：

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

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

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