CN105867377B - 一种农业机械自动导航控制方法 - Google Patents

Abstract

本发明公开了一种农业机械自动导航控制方法，该方法包括如下步骤：(1)GPS接收机实时测得当前时刻农业机械的运动状态数据；(2)采用预瞄控制方法，将运动状态数据与预定义路径数据进行比对分析，推算出横向偏差和航向偏差；(3)通过模糊控制算法进行模糊化，然后将模糊量导入模糊控制器中，得到前轮转角；通过改进型纯追踪算法建立几何模型，从而导求出前轮转角；将两种算法得到的前轮转角合成一个前轮转角；(4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。本发明由两种导航方法结合在一起实现自动控制，是稳定性良好，抗干扰能力强，能够提高系统的鲁棒性。两种方法的结合，优势互补，使其满足旱地和水田的作业需要。

Description

一种农业机械自动导航控制方法

技术领域

本发明涉及农业机械导航控制领域，特别涉及一种农业机械自动导航控制算法。

背景技术

农业机械导航控制的主要任务是根据各个传感器得到农业机械的导航位置，确定该位置与预定义路径的位置关系，综合农业机械的运动状态以及合适的控制算法，决策出相应的前轮转角，以修正路径跟踪误差。

导航控制方法常用的有线性模型控制方法、模糊控制方法和最优控制方法。日本东京大学利用机器视觉技术，根据线性转向控制模型，将目标方向角和农业机械纵向角融合，计算出前轮转角，实现了转弯控制。随后东京大学又采用模糊控制技术来进行导航研究，模糊控制器输入的是方向偏差和位置偏差，输出的是前轮的转向角度和左右执行的时间，从而完成导航控制。O’Connor等人采用方向偏差、方向偏差变化率、转向角度、转向角度变化率和跟踪位置误差这五个状态变量建立农业机械运动学方程，以角度变化率最小为优化目标，利用农业机械线性运动学方程组建立了基于LQR的最优控制器，从而实现了导航控制。Benson等人应用PID控制方法设计了PID控制器，该算法不依赖精确的数学模型，避免了繁琐的建模过程，只需要一些对象的响应特征来组合控制，对算法的比例参数、积分参数和微分参数进行合理的调节，即可实现良好的路径跟踪效果。专利ZL200710029018.4中应用模糊控制算法和PID算法设计控制器，当横向偏差较小时，应用PID控制；当横向偏差较大时，采用模糊控制。该方法在一定范围内保证了稳态跟踪精度和控制的稳定性，但由于在实际导航中的每一时刻，只采用了其中一种控制算法，因此很难发挥两种控制算法的互补优势。

通过以上分析可知，导航控制的难点和重点是提高路径跟踪的精度和转向控制的稳定性。常规的线性模型控制方法，包括PID控制方法，能够获得较高精度的路径跟踪效果，但是抗干扰能力较弱；模糊控制算法具有良好的稳定性，但是路径跟踪的稳态精度难以保证；最优控制器的建立要依赖精确的农业机械运动学和动力学模型，但是在田间作业条件下，土壤特性的变化、农业机械与地面相互作用的变化，导致农业机械运动学和动力学模型的建立比较困难，

发明内容

本发明的目的在于改进和完善导航控制技术中的不足，提供一种路径跟踪精度相对较高、稳定性较好的导航控制方法。该方法基于预瞄跟随理论，采用改进型纯追踪算法和模糊控制相结合的方法来设计控制器，建立一种稳定性好、精度高的路径跟踪算法。

为了达到上述目的，本发明所采用的技术方案如下：一种农业机械自动导航控制方法，该方法包括如下步骤：

(1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据；

(2)采用预瞄控制方法，动态搜索目标点，将步骤1得到运动状态数据与预定义路径数据进行比对分析，推算出横向偏差和航向偏差；

(3)通过模糊控制算法将步骤2得到的横向偏差和航向偏差模糊化，然后将模糊量导入模糊控制器中，得到前轮转角；通过改进型纯追踪算法建立几何模型，从而由步骤2中得到的航向偏差推导求出前轮转角；将模糊控制算法和改进型纯追踪算法得到的前轮转角合成一个最终的前轮转角；

(4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。

进一步的，所述当前时刻农业机械的运动状态数据包括当前位置P_r(x_r,y_r)、当前速度V和当前航向角度q；所述预定义路径数据为n+1个位置点的集合，即 {P₀(x₀,y₀),P₁(x₁,y₁),...,P_k(x_k,y_k),P_k+1(x_k+1,y_k+1),...,P_n(x_n,y_n)}。

进一步的，所述预瞄控制方法，即动态搜索目标点，是通过动态计算前视距离，进而确定预定义路径上的预瞄点：

(1)前视距离公式由下述公式计算得出：

L_f＝L₀+k_vV+(L_b-k_wW)

其中，L_f为前视距离；L₀为前视距离基值；L_b为弯度的最大量化值；k_v和k_w分别是速度和弯度的比例系数，这两个系数可在仿真或试验中通过整定获得；W为弯度，弯度的计算方法是：首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点P_k(x_k,y_k)，由该点向后每隔i个点再取3个点，i为正整数，由此可得到4个点，分别是P_k、P_k+i、P_k+2i和P_k+3i，用直线连接这四个点，取直线P_kP_k+i和直线P_k+2iP_k+3i所夹的较小的角为弯度值，取值为0～180：

(2)预瞄点P_m的确定：在农业机械当前纵向方向上，由当前位置点P_r向前截取前视距离L_f，得到P′_m点，再通过快速查找算法得到预定义路径上与P′_m点最近的点，该点即是预瞄点P_m。

进一步的，所述的横向偏差和航向偏差的求解过程如下：

(1)横向偏差d的确定：

首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点P_k(x_k,y_k)，然后将农业机械当前位置点P_r向直线P_k(x_k,y_k)P_k+1(x_k+1,y_k+1)投影，得到点P′_r，则横向偏差d＝|P_rP′_r|；

(2)航向偏差q_e的确定：

航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差：农业机械的当前位置点P_r与预瞄点P_m构成的向量为设该向量沿逆时针方向旋转与y轴正向的夹角为那么航向偏差

进一步的，所述的模糊控制方法具体为：

(1)将步骤2得到的横向偏差和航向偏差模糊化，即采用基本连续论域，离散化采用均匀量化的方法，将连续论域中的值经量化因子变换后取整，具体为：

q′_e＝int(k_qq_e)

d'＝int(k_dd)

其中q′_e、d'均为连续数据取整后的离散数据；输入量化因子k_q的确定方法是：根据仿真和试验测试结果，确定航向偏差的误差范围，由离散论域最大值除以相应误差范围最大值即可获得量化因子，获得之后再通过试验进行修正即可；

横向偏差的连续论域离散化具体为：将横向偏差分为四个等级，当测得|d|∈[0,0.1]时，则k_d＝a₁，当得|d|∈(0.1,0.3]时，则k_d＝a₂，当|d|∈(0.3,05]时，则k_d＝a₃，当d为其它值时，则k_d＝a₄，其中a₁、a₂、a₃和a₄分别为横向偏差在四个等级下对应的取值；

(2)使用模糊控制表查询，打开规则观测器，输入不同的离散量d'，q′_e，得到对应的输出离散量

(3)输出去模糊控制，即模糊控制算法得出的前轮转角从模糊控制量到实际输出控制量的转换公式如下：

其中，输出的比例因子的确定方法同输入量化因子。

进一步的，所述的改进型纯追踪算法为：

利用航向偏差建立几何模型，求解农业机械到达目标点所需要的转弯半径，然后由转弯半径再求解出前轮转角，同时把横向偏差作为观测量，如果横向偏差超过设定值，那么给前轮转角一个补偿量以增大前轮转角，使农业机械快速转弯到达目标点；

P_r为当前农业机械的位置，P_k+j为目标点，R为转弯半径，C为圆心点，q_e为 航向偏差，L为农业机械前后轮轴间距，为改进型纯追踪算法求得的前轮转角，那么在直角三角形BP_rC中可以得出：

由于q_e为圆C的弦切角，因此可得：

-P_rCP_k+j＝2q_e

在等腰三角形P_rCP_k+j中，由正弦定理可得：

由上述三式消去R可得到前轮转角：

式中P_rP_k+j为农业机械当前位置与目标点的之间的距离；另外，将横向偏差作为控制器的一个观测量，再设定一个预设值d_r，当观测到横向偏差d≠d_r时，则将改进纯追型踪算法求得的前轮转角乘以一个比例系数k_dr，由此可以增大前轮转角，使其快速到达目标点；其中d_r和k_dr的大小通过仿真或试验整定得出。

进一步的，所述步骤3具体为：通过改进型纯追踪算法和模糊控制算法分别得出前轮转角，然后将两个转角合成一个最终的转角，实现两种方法复合控制，其中合成公式如下：

其中k₁、k₂分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角的折算因子，两者之和为1，两者取值由仿真和试验整定得出。

进一步的，所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点的距离，距离最小的那个点即为最近点。

本发明与现有技术相比有如下特点：

(1)本发明对传统纯追踪算法做了改进，使其不仅适应直线行驶还能满足曲线转弯，同时结合模糊控制算法，在实际导航中充分发挥了上述两种导航算 法各自的优势并达到互补的效果，因此极大地提高了控制的精度和稳定性。

(2)本发明在导航农业机械沿预定义路径行驶时，直线段行驶的跟踪误差小于5cm，曲线转弯段行驶的跟踪误差小于20cm。

(3)本发明适合于农业机械自动化装备，能够满足旱地的作业需要，同时也可满足水田作业要求。

附图说明

图1为弯度求解图；

图2为横向偏差求解图；

图3为横向偏差正负号求解图；

图4为航向偏差求解图；

图5为模糊变量的隶属函数图；

图6为模糊控制输出的三维外观图；

图7为改进型纯追踪算法原理图；

图8为导航控制原理框图。

具体实施方式

为使本发明更明显易懂，兹以一实施例作进一步详细描述，但不用来限制本发明的范围。

一种农业机械自动导航控制方法，该方法包括如下步骤：

(1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据；

(2)采用预瞄控制方法，动态搜索目标点，将步骤1得到运动状态数据与预定义路径数据进行比对分析，推算出横向偏差和航向偏差；

(3)通过模糊控制算法将步骤2得到的横向偏差和航向偏差模糊化，然后将模糊量导入模糊控制器中，得到前轮转角；通过改进型纯追踪算法建立几何模 型，从而由步骤2中得到的航向偏差推导求出前轮转角；将模糊控制算法和改进型纯追踪算法得到的前轮转角合成一个最终的前轮转角；

(4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。

进一步的，所述当前时刻农业机械的运动状态数据包括当前位置P_r(x_r,y_r)、当前速度V和当前航向角度q，如图1所示；具体定义如下：

(1)当前定位点P_r(x_r,y_r)，由GPS接收机获得当前的差分定位数据；

(2)当前速度V,由速度传感器测得当前农业机械纵向方向的前进速度；

(3)当前航向角度q，由电子罗盘获得当前农业机械纵向方向的角度，在高斯投影平面坐标系中，表示为与y轴顺时针旋转的夹角，范围为0～360。

所述的预定义路径是由人工驾驶车辆，GPS接收机记录的路径数据，是一个点数据，其数学表达式为：

{P₀(x₀,y₀),P₁(x₁,y₁),...,P_k-1(x_k-1,y_k-1),P_k(x_k,y_k)}

需要说明的是，预定义路径由点数据集表示，P_k为第k个点，(x_k,y_k)为第k个点的坐标，所有点的坐标均在WGS-84大地坐标系中，表示为高斯投影平面坐标。

所述预瞄控制方法，即动态搜索目标点，是通过动态计算前视距离，进而确定预定义路径上的预瞄点：

(1)前视距离公式由下述公式计算得出：

L_f＝L₀+k_vV+(L_b-k_wW)

其中，L_f为前视距离；L₀为前视距离基值；L_b为弯度的最大量化值；k_v和k_w分别是速度和弯度的比例系数，这两个系数可在仿真或试验中通过整定获得；W为弯度，如图1所示，弯度的计算方法是：首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点P_k(x_k,y_k)，由该点向后每隔i个点再取3个点，i为正整数，由此可得到4个点，分别是P_k、P_k+i、P_k+2i和P_k+3i，用直线连接 这四个点，取直线P_kP_k+i和直线P_k+2iP_k+3i所夹的较小的角为弯度值，取值为0～180：

(2)预瞄点P_m的确定：在农业机械当前纵向方向上，由当前位置点P_r向前截取前视距离L_f，得到P′_m点，再通过快速查找算法得到预定义路径上与P′_m点最近的点，该点即是预瞄点P_m。

所述的横向偏差和航向偏差的求解过程如下：

(1)横向偏差d的确定：

如图2所示，首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点P_k(x_k,y_k)，然后将农业机械当前位置点P_r向直线P_k(x_k,y_k)P_k+1(x_k+1,y_k+1)投影，得到点P′_r，则横向偏差d＝|P_rP′r|；

为了导航控制算法设计的方便，这里对横向偏差正负号，在车辆沿预定义路径前进的方向上，如果车辆位于预定义路径右侧，则d为正，如果在左侧，则d为负；判断正负的方法是：如图3所示，首先在预定义路径中找到距离车辆当前位置点最近的点P_k，然后沿着路径前进方向寻找下一个点P_k+1，于是构成一个向量设该向量逆时针旋转与y轴正向的夹角为a₁。由车辆当前位置点P_r与点P_k+1，又可组成一个向量设该向量逆时针旋转与y轴正向的夹角为a₂。如果a₁-a₂为正，则车辆在右侧，那么横向偏差为正，否则为负。

(2)航向偏差q_e的确定：

航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差：如图4所示，农业机械的当前位置点P_r与预瞄点P_m构成的向量为设该向量沿逆时针方向旋转与y轴正向的夹角为那么航向偏差角度的取值范围为-90～90，如果为正，则表示车辆当前的航向在目标方向的左边，否则在右边。

所述的模糊控制方法具体为：

模糊控制是将驾驶员的驾驶经验转换为模糊规则，进而决策出前轮转角。 基本思路是当横向偏差和航向偏差越大，那么控制的程度越强，即前轮转角越大；当横向偏差和航向偏差越小，那么控制的程度越弱，即前轮转角越小；当两者一个较大一个较小，那么控制的程度为中。规定前轮右转时为正，左转时为负。驾驶经验转换为模糊规则的方法是：将农业机械相对于预定义路径的状态分为七种，即严重偏左、偏左、稍微偏左、正对着路径、稍微偏右、偏右、严重偏右，根据这七种状态，驾驶员会做出相应的转向控制策略，对应的也分为七种，即大幅度右转、右转、稍微右转、不转向、稍微左转、左转、大幅度左转。据此建立模糊控制规则表，如表1所示。具体设计步骤是在Matlab环境中设计的，如下所示：

(1)首先将两个输入变量d、q_e和一个输出变量进行模糊化，其模糊子集分别选为：

F_d＝{NL,NM,NS,ZO,PS,PM,PL}

三个变量的模糊分割均为7级，即负大、负中、负小、零、正小、正中和正大。选取横向偏差、航向偏差和输出转角的离散论域分别为：

d＝{-6,-5,-4,-3，-2，-1,0,1,2,3,4,5,6}

q_e＝{-6,-5,-4,-3，-2，-1,0,1,2,3,4,5,6}

模糊推理系统的基本属性确定为：“与”运算采用极小运算，“或”运算采用极大运算，模糊蕴涵采用极小运算，模糊规则综合采用极大运算，去模糊化采用重心法。航向偏差的基本连续论域离散化采用均匀量化的方法，即将连续论域中的连续值经量化因子比例变换后四舍五入变为离散论域的整数值，具体求解公式为：

q′_e＝int(k_qq_e)

d'＝int(k_dd)

其中q′_e、d'均为连续数据取整后的离散数据；输入量化因子k_q的确定方法是：根据仿真和试验测试结果，确定航向偏差的误差范围，由离散论域最大值除以相应误差范围最大值即可获得量化因子，获得之后再通过试验进行修正即可；

横向偏差的连续论域离散化具体为：将横向偏差分为四个等级，当测得|d|∈[0,0.1]时，则k_d＝a₁，当得|d|∈(0.1,0.3]时，则k_d＝a₂，当|d|∈(0.3,05]时，则k_d＝a₃， 当d为其它值时，则k_d＝a₄，其中a₁、a₂、a₃和a₄分别为横向偏差在四个等级下对应的取值，其数值通过仿真或试验整定得出。

在Matlab环境中，建立模糊变量横向偏差、航向偏差和输出转角的隶属函数曲线，如图5(a)、(b)和(c)所示。对应的模糊控制规则表，利用Matlab软件绘出模糊控制表的三维外观图，如图6所示。

(2)使用模糊控制表查询，模糊控制表如表2所示，打开规则观测器，输入不同的离散量d'，q′_e，得到对应的输出离散量

(3)输出去模糊控制，即模糊控制算法得出的前轮转角从模糊控制量到实际输出控制量的转换公式如下：

其中，输出的模糊控制算法得出的前轮转角的比例因子的确定方法同输入量化因子。

进一步的，所述的改进型纯追踪算法为：

利用航向偏差建立几何模型，求解农业机械到达目标点所需要的转弯半径，然后由转弯半径再求解出前轮转角，同时把横向偏差作为观测量，如果横向偏差超过设定值，那么给前轮转角一个补偿量以增大前轮转角，使农业机械快速转弯到达目标点；

在图6中，一组点为预定义路径点，P_r为当前农业机械的位置，P_k+j为目标点，R为转弯半径，C为圆心点，q_e为航向偏差，L为农业机械前后轮轴间距， 为改进型纯追踪算法求得的前轮转角，那么在直角三角形BP_rC中可以得出：

由于q_e为圆C的弦切角，因此可得：

—P_rCP_k+j＝2q_e

在等腰三角形P_rCP_k+j中，由正弦定理可得：

由上述三式消去R可得到前轮转角：

式中P_rP_k+j为农业机械当前位置与目标点的之间的距离；另外，将横向偏差作为控制器的一个观测量，再设定一个预设值d_r，当观测到横向偏差d≠d_r时，则将改进纯追型踪算法求得的前轮转角乘以一个比例系数k_dr，由此可以增大前轮转角，使其快速到达目标点；其中d_r和k_dr的大小通过仿真或试验整定得出。

进一步的，所述步骤3具体为：通过改进型纯追踪算法和模糊控制算法分别得出前轮转角，然后将两个转角合成一个最终的转角，实现两种方法复合控制，其中合成公式如下：

其中k₁、k₂分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角的折算因子，两者之和为1，两者取值由仿真和试验整定得出。

进一步的，所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点的距离，距离最小的那个点即为最近点。

上述两种导航控制方法的结合，既能提高路径跟踪的稳态精度，又能提高路径跟踪的稳定性。其中改进型纯追踪算法是从几何的角度推算的，由航向偏差经过几何运算得出前轮转角，运算过程未进行近似处理，因此得出的是精确解。该解对于消除导航控制系统中的偏差和稳态误差起到重要作用，有利于提高导航精度。

模糊控制方法是一种近似求解方法，该方法的特点是不需要确切的理论模型，仅根据人的驾驶经验来制定模糊规则，建立模糊控制器。因此抗干扰能力较强，不会因模型变化而改变控制效果，能够消除导航系统的振荡误差。

本实施例使用的农业机械为井关插秧机，在其上装载了导航传感器组合、转向控制系统和车载计算机，各部分之间通信使用总线，由此构成导航控制系统，其原理结构如图8所示。导航传感器组合包括差分GPS、电子罗盘、速度传感器和微机械陀螺仪。转向控制系统包括操纵控制器、转向驱动机构和测试传感器。车载计算机主要实现数据采集、数据预处理、导航控制算法和控制指令输出等功能，动态数据采集频率为5Hz。

具体实施的步骤如下：

1、预定义路径数据获取

首先由驾驶员驾驶插秧机在水田中正常作业行走，由GPS接收机实时获取定位信息并以总线数据的形式输出，该数据构成预定义路径，路径包括直线段和曲线转弯段，其数学表达式为P|n]＝{(x₀,y₀),(x₁,y₁),...,(x_k,y_k),...,(x_n,y_n)}

2、导航数据获取

(1)插秧机位姿和速度参数的获取

涉及数据包括有：插秧机当前定位点坐标、当前航向角度和当前插秧机速度。

(2)前视距离的求解

前视距离的定义如图2所示，求解方法采用前述公式(1)进行求解，利用速度和路径的弯度来确定合适的前视距离，进而可以确定目标点。各参数取值如下：

L₀＝0.15，L_b＝0.62，k_v＝0.31，k_w＝0.1

(3)导航偏差的求解

所述的导航偏差分为横向偏差和航向偏差，定义及计算过程如下所示：

1)目标方向的计算

在图2中，即为矢量为的方向，以高斯投影平面坐标系xoy中y轴正向为0度，顺时针旋转到矢量的角度即为该角度取值范围为0～360。

2)航向偏差q_e的计算

在图2中，即为目标方向角与插秧机当前航向角度之间的差值。

3)横向偏差d的计算

在图2中，设点P′_r为农业机械当前位置点向预定义路径上投影的点，则d＝|P_rP′_r|，判断正负的方法同前所述。

(4)前轮转角模糊控制算法

将横向误差和航向误差以及前轮转角的离散论域分别选取为：

d＝{-6,-5,-4,-3，-2，-1,0,1,2,3,4,5,6}

q_e＝{-6,-5,-4,-3，-2，-1,0,1,2,3,4,5,6}

1)输入变量模糊化

将输入连续论域中的连续值经量化因子比例转换为离散量，然后四舍五入取值。其中航向偏差与横向偏差分别按上述方法进行量化，其中各参数取值如下所示：

k_q＝0.25，a₁＝12、a₂＝2、a₃＝0.2、a₄＝0

2)模糊控制表查询

输入不同的离散输入量d'和q′_e，依据路径跟踪模糊控制表，如表2所示，即可得到对应的输出离散量

3)输出控制量清晰化

模糊量到实际输出控制量的转换公式如下所示：

其中参数

(5)前轮转角改进型纯追踪算法

首先根据插秧机当前的航向偏差计算出插秧机到达目标点所需的转弯半径，进而求出前轮转角，计算公式如公式(9)所示，然后利用横向偏差作为观测量，如果偏差过大，那么将前轮转角乘以一个系数，将其增大，这样插秧机能够快速到达目标点，各参数分别为：

d_r＝0.02、k_dr＝1.08

(6)模糊控制和改进型纯追踪控制复合决策

上述两种算法得出两个前轮转角，利用如下公式进行综合：

其中整定出的两个参数数值分别为：

k₁＝0.2、k₂＝0.8

(7)输出控制参数

将决策出的前轮转角按一定的总线通信格式打包，发送给转向控制系统，即可实现前轮转角的控制。

表1、模糊控制规则表

表2、路径跟踪模糊控制表

Claims (7)

1.一种农业机械自动导航控制方法，其特征在于，该方法包括如下步骤：

(1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据；

(2)采用预瞄控制方法，动态搜索目标点，将步骤(1)得到运动状态数据与预定义路径数据进行比对分析，推算出横向偏差和航向偏差；

(3)通过模糊控制算法将步骤(2)得到的横向偏差和航向偏差模糊化，然后将模糊量导入模糊控制器中，得到前轮转角；通过改进型纯追踪算法建立几何模型，从而由步骤(2)中得到的航向偏差推导求出前轮转角；将模糊控制算法和改进型纯追踪算法得到的前轮转角合成一个最终的前轮转角；

所述的改进型纯追踪算法为：

利用航向偏差建立几何模型，P_r为当前农业机械的位置，P_k+j为目标点，R为转弯半径，C为圆心点，q_e为航向偏差，L为农业机械前后轮轴间距，d₂为改进型纯追踪算法求得的前轮转角，那么在直角三角形BP_rC中可以得出：

由于q_e为圆C的弦切角，因此可得：

—P_rCP_k+j＝2q_e

在等腰三角形P_rCP_k+j中，由正弦定理可得：

由上述三式消去R可得到前轮转角：

式中P_rP_k+j为农业机械当前位置与目标点的之间的距离；另外，将横向偏差作为控制器的一个观测量，再设定一个预设值d_r，当观测到横向偏差d≠d_r时，则将改进纯追型踪算法求得的前轮转角d₂乘以一个比例系数k_dr，由此可以增大前轮转角，使其快速到达目标点；

(4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。

2.如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述当前时刻农业机械的运动状态数据包括当前位置P_r(x_r,y_r)、当前速度V和当前航向角度q；所述预定义路径数据为n+1个位置点的集合，即{P₀(x₀,y₀),P₁(x₁,y₁),...,P_k(x_k,y_k),P_k+1(x_k+1,y_k+1),...,P_n(x_n,y_n)}。

3.如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述预瞄控制方法，即动态搜索目标点，是通过动态计算前视距离，进而确定预定义路径上的预瞄点：

(1)前视距离公式由下述公式计算得出：

L_f＝L₀+k_vV+(L_b-k_wW)

其中，L_f为前视距离；L₀为前视距离基值；L_b为弯度的最大量化值；k_v和k_w分别是速度和弯度的比例系数；W为弯度，弯度的计算方法是：首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点P_k(x_k,y_k)，由该点向后每隔i个点再取3个点，i为正整数，由此可得到4个点，分别是P_k、P_k+i、P_k+2i和P_k+3i，用直线连接这四个点，取直线P_kP_k+i和直线P_{k+ 2i}P_k+3i所夹的较小的角为弯度值，取值为0～180度；

(2)预瞄点P_m的确定：在农业机械当前纵向方向上，由当前位置点P_r向前截取前视距离L_f，得到P'_m点，再通过快速查找算法得到预定义路径上与P'_m点最近的点，该点即是预瞄点P_m。

4.如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述的横向偏差和航向偏差的求解过程如下：

(1)横向偏差d的确定：

首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点P_k(x_k,y_k)，然后将农业机械当前位置点P_r向直线P_k(x_k,y_k)P_k+1(x_k+1,y_k+1)投影，得到点P_r'，则横向偏差d＝|P_rP_r'|；

(2)航向偏差q_e的确定：

航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差：农业机械的当前位置点P_r与预瞄点P_m构成的向量为设该向量沿逆时针方向旋转与y轴正向的夹角为b，那么航向偏差q_e＝b-q，q为当前航向角度。

5.如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述的模糊控制方法具体为：

(1)将步骤(2)得到的横向偏差和航向偏差模糊化，即采用基本连续论域，离散化采用均匀量化的方法，将连续论域中的值经量化因子变换后取整，具体为：

q'_e＝int(k_qq_e)

d'＝int(k_dd)

其中q'_e、d'均为连续数据取整后的离散量；k_q为输入量化因子，q_e为航向偏差；

横向偏差的连续论域离散化具体为：将横向偏差分为四个等级，当测得|d|∈(0，0.1]时，则k_d＝a₁，当得|d|∈(0.1，0.3]时，则k_d＝a₂，当|d|∈(0.3，05]时，则k_d＝a₃，当d为其它值时，则k_d＝a₄，其中a₁、a₂、a₃和a₄分别为横向偏差在四个等级下对应的取值；

(2)使用模糊控制表查询，打开规则观测器，输入不同的离散量d'，q'_e，得到对应的输出离散量d'₁；

(3)输出去模糊控制，即模糊控制算法得出的前轮转角d₁：从模糊控制量到实际输出控制量的转换公式如下：

d₁＝k_d1d'₁

其中，输出的比例因子k_d1的确定方法同输入量化因子。

6.如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述步骤(3)具体为：通过改进型纯追踪算法和模糊控制算法分别得出前轮转角，然后将两个转角合成一个最终的转角，实现两种方法复合控制，其中合成公式如下：

d＝k₁d₁+k₂d₂

其中k₁、k₂分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角的折算因子，两者之和为1。

7.如权利要求3或4所述的农业机械自动导航控制方法，其特征在于，所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点的距离，距离最小的那个点即为最近点。