本发明属于车辆导航追踪领域，具体涉及一种适用于农业机械直线导航的侧滑角度估计方法及估计器。
背景技术：
随着农业机械自动化水平的提高，农业机械自动导航技术越来越受重视，尤其是在我国东北及新疆地区的旱田作业中应用日趋成熟与完善。根据农作物种植的特点，农业机械导航系统对于直线路径追踪的精度要求远高于其他类型导航车辆的要求。但是不同于旱田农业机械自动导航技术的日趋完善，受水田作业环境硬底层不平、车辆侧滑等因素的影响，直线路径追踪精度差成为水田农业机械自动导航技术存在并亟需解决的主要问题。
国内农业机械自动驾驶起步较晚，对侧滑问题的研究还未开展，提高水田农业机械自动导航系统直线路径追踪精度是目前农业机械自动导航系统的主要研究问题之一。农业机械自动导航路径追踪算法多依赖于车辆动力学模型，其中侧滑角度是车辆动力学模型中的参数之一，由于侧滑产生在轮胎与地块接触面缺少直接测量的传感器，导致侧滑角度难以获取，而当前农业机械自动导航路径跟踪算法大多忽略掉了侧滑角度的影响，从而导致水田作业机械过程中路径追踪精度差。
为此，本发明基于状态观测理论，设计一种农业机械侧滑角度估计器，为农业机械自动导航路径直线追踪算法提供参数参考，进而为提高侧滑条件下农业机械自动导航的路径跟踪精度提供支持。
技术实现要素：
本发明针对采用前轮转向的农业机械车辆在直线路径追踪过程中存在的侧滑问题，基于观测器理论，提出一种适用于农业机械直线导航的侧滑角度估计方法及估计器。
本发明是采用以下的技术方案实现的：
一种适用于农业机械直线导航的侧滑角度估计方法，包括以下步骤：
步骤s1、在农业机械行进过程中，采集农业机械的前轮转向角信息、以及农业机械的前进速度信息、天线定位信息和当前姿态信息，并对其进行对应的分析处理；
步骤s2、构建农业机械的动力学方程并将其作为系统状态方程，基于状态观测器理论实现对直线导航路径追踪过程中的侧滑角度的估计，具体的：
(1)根据步骤s1所采集的农业机械的天线定位信息和当前姿态信息，分析并获得j时刻综合误差信号ε(j)：
其中，y(j)表示j时刻的位置偏差测量值，位置偏差记为导航点坐标距离路径规划线上最近点之间的距离，
(2)根据得到的综合误差信号ε(j)，分析并获得j时刻侧滑角度估计值
其中，
(3)根据采集的前轮转角信息、前进速度信息以及综合误差信号ε(j)和侧滑角度估计值
其中，
(4)根据采集的农业机械前进速度信息以及分析获得的航向偏差估计值、侧滑角度估计值和综合误差信号，对航向的位置偏差进行估计，得到位置偏差估计值：
其中，
进一步的，所述步骤s1中，对采集的数据进行分析处理时，具体采用以下方式：
(1)对采集的前轮转向角信息进行a/d转换并进行滤波处理，得到j时刻前轮转向角数字值δ(j)；
(2)对采集的农业机械的前进速度信息进行滤波得到j时刻的当前前进速度v(j)；
(3)对采集的天线定位信息和当前车辆姿态信息经过坐标变换及分析，得到导航点坐标信息与路径规划线之间的位置偏差量测值y(j)与航向偏差量测值
进一步的，所述步骤s2中，所构建的农业机械的动力学方程为：
其中，δ表示前轮轮角角度，l为农业机械车身长度，v表示车辆前进速度，β表示侧滑角度，
本发明另外还提出一种适用于农业机械直线导航的侧滑角度估计器，所述农业机械的自动导航系统包括车辆前轮轮角传感器和gnss定位定向装置，所述侧滑角度估计器包括综合误差计算器、第一估计器、第二估计器和第三估计器；
所述前轮轮角传感器用以采集前轮转向角信息，前轮转向角信息经过处理后传输至第二估计器的输入端；所述gnss定位定向装置用以采集农业机械的前进速度信息、天线定位信息和当前姿态信息，所采集的前进速度信息经过滤波处理后同样传输至第二估计器的输入端；所采集的天线定位信息和当前车辆姿态信息经过分析计算，得到导航点坐标信息与路径规划线之间的位置偏差量测值y(j)与航向偏差量测值
所述综合误差计算器的输出端分别与第一估计器、第二估计器和第三估计器的输入端相连，第一估计器的输出端分别与第二估计器和第三估计器的输入端相连，第二估计器的输出端分别与综合误差计算器和第三估计器的输入端相连，第三估计器的输出端与综合误差计算器的输入端相连；
所述综合误差计算器用以分析获得j时刻综合误差信号ε(j)，即：
其中，
所述第一估计器实现对j时刻侧滑角度估计值
其中，k1为系数，ts表示系统控制周期；
所述第二估计器用以对j时刻航向偏差进行估计，得到航向偏差估计值，即：
其中，v(j)为车辆当前速度，l为车身长度，δ(j)为当前前轮转向角度，k2为系数；
所述第三估计器实现对航向的位置偏差的估计，得到位置偏差估计值，即：
其中，k3为系数。
进一步的，所述前轮轮角传感器的输出端依次通过a/d转换器和第一数字滤波器与第二估计器的输入端相连，第一数字滤波器用以实现对经过a/d转换器转换后的前轮转向角信号的滤波处理。
与现有技术相比，本发明的优点和积极效果在于：
本方案基于状态观测理论实现对侧滑角度的估计，不需要增加额外的硬件，计算量低，方便单片机、arm等低成本嵌入式系统实现；采用三个状态观测器实现对车体航向偏差、位置偏差、侧滑角度信息的估计，分析过程利用积分代替微分，避免了微分运算对误差的放大作用；而且，在获取侧滑角度的同时，完成对航向偏差、位置偏差估计，本身具备滤波功能，改善了由于定位信息更新延迟所造成的航向偏差、位置偏差获取误差偏差较大等问题。
附图说明
图1为本发明实施例所述侧滑角度估计器的原理框图；
图2为本发明实施例所述直线导航示意图；
图3为本发明实施例所述综合误差计算器原理框图；
图4为本发明所述第一估计器原理框图；
图5为本发明所述第二估计器原理框图；
图6为本发明所述第三估计器原理框图；
图7为基于传统方法对侧滑角度估计直线路径追踪的精度数据图；
图8为本发明侧滑角度估计试验数据图；
图9为采用本发明侧滑角度补偿直线导航路径追踪精度数据图。
具体实施方式
为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。
本发明利用观测器理论，基于轮角测量信息、车辆前进速度、车辆动力学模型设计一种适用于农业机械直线导航的侧滑角度估计方法及估计器。
实施例1，一种适用于农业机械直线导航的侧滑角度估计方法，具体包括以下步骤：
步骤s1、在农业机械行进过程中，采集农业机械的前轮转向角信息、以及农业机械的前进速度信息、天线定位信息和当前姿态信息，并对其进行对应的分析处理；
步骤s2、构建农业机械的动力学方程并将其作为系统状态方程，基于状态观测器理论实现对直线导航路径追踪过程中的侧滑角度的估计，具体的：
(1)根据步骤s1所采集的农业机械的天线定位信息和当前姿态信息，分析并获得j时刻综合误差信号ε(j)：
其中，y(j)表示j时刻的位置偏差测量值，位置偏差记为导航点坐标距离路径规划线上最近点之间的距离，
(2)根据得到的综合误差信号ε(j)，分析并获得j时刻侧滑角度估计值
其中，
(3)根据采集的前轮转角信息、前进速度信息以及综合误差信号ε(j)和侧滑角度估计值
其中，
(4)根据采集的农业机械前进速度信息以及分析获得的航向偏差估计值、侧滑角度估计值和综合误差信号，对航向的位置偏差进行估计，得到位置偏差估计值：
其中，
其中，所述步骤s1中，通过安装在农业机械自动导航系统上的车辆前轮轮角传感器和gnss定位定向装置进行数据采集，对采集的数据进行分析处理时，具体采用以下方式：
(1)对采集的前轮转向角信息进行a/d转换并进行滤波处理，得到j时刻前轮转向角数字值δ(j)；这里采用均值滤波，定义均值滤波的滤波点数为n，a/d转换的采样间隔为δt，系统控制周期为ts，则均值滤波点数n满足关系式：
n＜0.5ts/δt；
(2)对采集的农业机械的前进速度信息进行二次滤波得到j时刻的当前前进速度v(j)；
(3)对采集的天线定位信息和当前车辆姿态信息经过坐标变换及分析：
坐标变换由两步完成：
1.高斯-克吕格投影，由大地参考坐标系经纬度高程转换为地心-地固坐标系坐标；
2.欧拉坐标变换模块根据定位天线在车体坐标系的坐标信息以及车辆姿态信息计算得到车辆中心点在导航坐标系下的坐标信息(以下简称导航点坐标)；
通过坐标变化得到导航点坐标信息，并分析得到导航点坐标信息与路径规划线之间的位置偏差量测值y(j)与航向偏差量测值
根据现代控制理论知识，准确的动力学模型是实现导航的精确路径追踪的前提与基础，在直线路径追踪过程中，步骤s2中所构建的农业机械的动力学方程为：
其中，δ表示前轮轮角角度，l为农业机械车身长度，v表示车辆前进速度，β表示侧滑角度，
在实际应用中，侧滑产生在地块与车轮接触面，难以采用加装传感器侧滑信息。本方案利于状态观测器理论，将公式(2)作为本方案设计过程中的系统状态方程；农业机械直线路径追踪过程中，存在两个偏差信息：位置偏差和航向偏差，并且两个偏差信息均可测；为使量测信息中充分包含车体当前坐标信息，本方案量测方程采用位置偏差和航向偏差线性的组合；根据系统状态方程和量测方程，实现对侧滑信息的估计。
实施例2，基于实施例1所提出的估计方法，本实施例提出一种适用于农业机械直线导航的侧滑角度估计器，农业机械的自动导航系统安装有车辆前轮轮角传感器1和gnss定位定向装置2，如图1所示，前轮轮角传感器1输出的前轮转向角模拟值经a/d转换器3和第一数字滤波器4后输出j时刻前轮转向轮角数字值δ(j)；第一数字滤波器4实现对轮角传感器a/d转换后信号的滤波，为均值滤波。定义均值滤波的滤波点数为n，ad转换的采样间隔为δt，系统控制周期为ts，为保证系统的正常运行，均值滤波点数n满足关系式：
n＜0.5ts/δt。
gnss定位/定向装置2用以采集农业机械的前进速度信息、天线定位信息和当前姿态信息：其输出的的前进速度信息v经过第二数字滤波器5完成对gnss输出的车辆移动速度的滤波，得到j时刻的前进速度v(j)，第二数字滤波器5为二阶低通滤波器；其输出的天线定位信息和当前车辆姿态信息经过坐标变换模块6和追踪误差计算器7的分析计算，得到导航点坐标信息与路径规划c之间的位置偏差的量测值y(j)与航向偏差的量测值
坐标变换模块6由两步完成：
1.高斯-克吕格投影，由大地参考坐标系经纬度高程转换为地心-地固坐标系坐标；
2.欧拉坐标变换模块根据定位天线在车体坐标系的坐标信息以及车辆姿态信息计算得到车辆中心点在导航坐标系下的坐标信息(以下简称导航点坐标)；
基于所采集的大地参考坐标系下的天线定位信息(包括经度、纬度和高程)与当前姿态信息(航向、横滚和俯仰)，坐标变换模块6包括高斯-克吕格投影变换和欧拉坐标变换，目的在于根据车辆姿态信息与天线定位信息获取车辆中心点在导航坐标系下的投影点坐标信息。根据导航坐标系的定义，gnss定位/定向装置2输出定位天线在大地参考坐标系下的经度、纬度和高程定位信息；采用高斯-克吕格投影，将定位天线在大地参考坐标系下的经度、纬度、高程转换为地心-地固坐标系下的x、y、z坐标信息，记为(px,py,pz)；选取地心-地固坐标系为导航坐标系，该坐标系采用常规的东北天坐标系，即x轴为东方向，y轴为北方向，z轴垂直于xy平面指向天空方向；定义车辆中心点为车体坐标系坐标原点o’,车头方向为车体坐标系纵轴y’，垂直于车头由坐标原点o’指向车身右侧方向为车体坐标系横轴x’，根据右手法则，由坐标原点指向垂直于车身的天空方向为车体坐标系z’；gnss定位天线安装在车体坐标系下的坐标是已知的,记为(vx,vy,vz)；如前所述，车体的姿态信息，包括横滚、俯仰、航向,记为(roll,pitch,yaw)；根据欧拉变换基本原理，可利用欧拉变换获取导航坐标系下，车辆中心点的坐标信息(x,y,z)(以下简称导航点坐标)，考虑到该坐标变换技术比较成熟，在此不做详述。
坐标变换完成后，通过追踪误差计算器7计算导航点坐标与路径规划线c之间的位置偏差y(j)与航向偏差
需要说明的是农业机械为一刚体，水田作业机械尤其是喷雾机、插秧机，相比较于旱田作业机械，尺寸较小，通常作业速度小于8km/h，本方案针对农业机械工作于直线追踪，车辆转向角度小，因此近似假定前后轮侧滑同时产生且侧滑角度相同。
继续参考图1，侧滑角度估计器包括综合误差计算器8，第一估计器9，第二估计器10和第三估计器11，四者相互作用获得航向偏差估计值、位置偏差估计值、侧滑角度估计值。
根据现代控制理论知识，准确的动力学模型是实现导航的精确路径追踪的前提与基础。在假定前后轮侧滑角度相同的前提下，定义农业机械的曲率半径为c(s)，前轮轮角角度为δ，农业机械的动力学方程可描述为：
在直线路径追踪过程中，可近似认为曲率半径c(s)＝0，公式1简化为：
但是在实际应用中，侧滑产生在地块与车轮接触面，难以采用加装传感器侧滑信息。本方案利于状态观测器理论，设计一种适用于农业机械直线路径追踪过程的侧滑信息估计器，公式(2)作为本方案设计过程中的系统状态方程；农业机械直线路径追踪过程中，存在两个偏差信息：位置偏差和航向偏差，并且两个偏差信息均可测；为使量测信息中充分包含车体当前坐标信息，本方案量测方程采用位置偏差和航向偏差线性的组合；根据系统状态方程和量测方程，设计观测器对侧滑信息进行估计，具体的：
如图3所示，综合误差计算器8包括第一加法器81、第二加法器82、第一乘法器83、第二乘法器84和第三加法器85；综合误差计算器8根据位置与航向偏差的量测值和位置与航向偏差的估计值计算得到j时刻综合误差信号ε(j)，即：
其中，，
如图4所示，第一估计器9包括第四乘法器91、第四加法器92和第一状态存储器93，第一状态存储器93记载前一时刻侧滑角度估计值为
ts表示系统控制周期；
如图5所示，第二估计器10包括第一除法器101、第一余弦计算器102、第一正切计算器103、第五加法器104、第二正切计算器105、第六加法器106、第五乘法器107、第六乘法器108、第七加法器109、第七乘法器1010、第八加法器1011和第二状态存储器1012；第二状态存储器1012记载前一时刻航向偏差的估计值
如图6所示，第三估计器11包括第九加法器111、正弦计算器112、第八乘法器113、第九乘法器114、第十加法器115、第十乘法器116、第十一加法器117、第三状态存储器118，第三状态存储器118记载前一时刻位置偏差的估计值
综上所述，本方案基于状态观测理论实现对侧滑角度的估计，不需要增加额外的硬件，计算量低，方便单片机、arm等低成本嵌入式系统实现；采用三个状态观测器实现对车体航向偏差、位置偏差、侧滑角度信息的估计，分析过程利用积分代替微分，避免了微分运算对误差的放大作用；而且，在获取侧滑角度的同时，完成对航向偏差、位置偏差估计，本身具备滤波功能，改善了由于定位信息更新延迟所造成的航向偏差、位置偏差获取误差偏差较大等问题。
试验验证：
为验证本方案的效果，进行了实物试验：试验地点为广州华南农业大学增城试验基地，试验地块为水田，经前期人工驾驶车辆试验，部分区域存在可感受到的侧滑现象，试验车辆为雷沃四轮驱动水旱两用高地隙喷雾机zp9500，采用霍尔传感器测量轮角角度，传感器型号为德国novotechnik公司生产的rf4000-120，直线路径追踪算法是基于车辆链式非线性模型基础上所设计的反馈控制率，控制率的输出为用数学公式描述为：
式中，
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。
1.一种适用于农业机械直线导航的侧滑角度估计方法，其特征在于，包括以下步骤：
步骤s1、在农业机械行进过程中，采集农业机械的前轮转向角信息、以及农业机械的前进速度信息、天线定位信息和当前姿态信息，并对其进行对应的分析处理；
步骤s2、构建农业机械的动力学方程并将其作为系统状态方程，基于状态观测器理论实现对直线导航路径追踪过程中的侧滑角度的估计，具体的：
(1)根据步骤s1所采集的农业机械的天线定位信息和当前姿态信息，分析并获得j时刻综合误差信号ε(j)：
其中，y(j)表示j时刻的位置偏差测量值，位置偏差记为导航点坐标距离路径规划线上最近点之间的距离，
(2)根据得到的综合误差信号ε(j)，分析并获得j时刻侧滑角度估计值
其中，
(3)根据采集的前轮转角信息、前进速度信息以及综合误差信号ε(j)和侧滑角度估计值
其中，
(4)根据采集的农业机械前进速度信息以及分析获得的航向偏差估计值、侧滑角度估计值和综合误差信号，对航向的位置偏差进行估计，得到位置偏差估计值：
其中，
2.根据权利要求1所述的适用于农业机械直线导航的侧滑角度估计方法，其特征在于：所述步骤s1中，对采集的数据进行分析处理时，具体采用以下方式：
(1)对采集的前轮转向角信息进行a/d转换并进行滤波处理，得到j时刻前轮转向角数字值δ(j)；
(2)对采集的农业机械的前进速度信息进行滤波得到j时刻的当前前进速度v(j)；
(3)对采集的天线定位信息和当前车辆姿态信息经过坐标变换及分析，得到导航点坐标信息与路径规划线之间的位置偏差量测值y(j)与航向偏差量测值
3.根据权利要求1所述的适用于农业机械直线导航的侧滑角度估计方法，其特征在于：所述步骤s2中，所构建的农业机械的动力学方程为：
其中，δ表示前轮轮角角度，l为农业机械车身长度，v表示车辆前进速度，β表示侧滑角度，
4.根据权利要求2所述的适用于农业机械直线导航的侧滑角度估计方法，其特征在于：在对采集的前轮转向角信息进行滤波处理时，采用均值滤波，定义均值滤波的滤波点数为n，a/d转换的采样间隔为δt，系统控制周期为ts，则均值滤波点数n满足关系式：
n＜0.5ts/δt。
5.一种适用于农业机械直线导航的侧滑角度估计器，所述农业机械的自动导航系统包括车辆前轮轮角传感器(1)和gnss定位定向装置(2)，其特征在于，所述侧滑角度估计器包括综合误差计算器(8)、第一估计器(9)、第二估计器(10)和第三估计器(11)；
所述前轮轮角传感器(1)用以采集前轮转向角信息，前轮转向角信息经过处理后传输至第二估计器(10)的输入端；所述gnss定位定向装置(2)用以采集农业机械的前进速度信息、天线定位信息和当前姿态信息，所采集的前进速度信息经过滤波处理后同样传输至第二估计器(10)的输入端；所采集的天线定位信息和当前车辆姿态信息经过分析计算，得到导航点坐标信息与路径规划线之间的位置偏差量测值y(j)与航向偏差量测值
所述综合误差计算器(8)的输出端分别与第一估计器(9)、第二估计器(10)和第三估计器(11)的输入端相连，第一估计器(9)的输出端分别与第二估计器(10)和第三估计器(11)的输入端相连，第二估计器(10)的输出端分别与综合误差计算器(8)和第三估计器(11)的输入端相连，第三估计器(11)的输出端与综合误差计算器(8)的输入端相连；
所述综合误差计算器(8)用以分析获得j时刻综合误差信号ε(j)，即：
其中，
所述第一估计器(9)实现对j时刻侧滑角度估计值
其中，k1为系数，ts表示系统控制周期；
所述第二估计器(10)用以对j时刻航向偏差进行估计，得到航向偏差估计值，即：
其中，v(j)为车辆当前速度，l为车身长度，δ(j)为当前前轮转向角度，k2为系数；
所述第三估计器(11)实现对航向的位置偏差的估计，得到位置偏差估计值，即：
其中，k3为系数。
6.根据权利要求5所述的适用于农业机械直线导航的侧滑角度估计器，其特征在于：所述前轮轮角传感器(1)的输出端依次通过a/d转换器(3)和第一数字滤波器(4)与第二估计器(10)的输入端相连，第一数字滤波器(4)用以实现对经过a/d转换器(3)转换后的前轮转向角信号的滤波处理。
7.根据权利要求6所述的适用于农业机械直线导航的侧滑角度估计器，其特征在于：设第一数字滤波器(4)滤波点数为n，a/d转换器(3)的采样间隔为δt，自动导航系统的控制周期为ts，则滤波点数n满足关系式：
n＜0.5ts/δt。
8.根据权利要求5所述的适用于农业机械直线导航的侧滑角度估计器，其特征在于：所述gnss定位定向装置(2)的输出端的一端通过第二数字滤波器(5)与第二估计器(10)的输入端相连，所述第二数字滤波器(5)实现对所采集的前进速度信息的滤波处理；
gnss定位定向装置(2)的输出端的另一端依次经过坐标变换模块(6)和追踪误差计算器(7)与综合误差计算器(8)的输入端相连，所述坐标变换模块(6)通过对采集的信息进行坐标变换并得到导航点坐标信息，所述追踪误差计算器(7)用以计算导航点坐标信息与路径规划线之间的位置偏差量测值y(j)与航向偏差量测值
