1.一种农业机械自动导航控制方法,其特征在于,该方法包括如下步骤:(1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据;
(2)采用预瞄控制方法,动态搜索目标点,将步骤(1)得到运动状态数据与预定义路径数据进行比对分析,推算出横向偏差和航向偏差;
(3)通过模糊控制算法将步骤(2)得到的横向偏差和航向偏差模糊化,然后将模糊量导入模糊控制器中,得到前轮转角;通过改进型纯追踪算法建立几何模型,从而由步骤(2)中得到的航向偏差推导求出前轮转角;将模糊控制算法和改进型纯追踪算法得到的前轮转角合成一个最终的前轮转角;
所述的改进型纯追踪算法为:
利用航向偏差建立几何模型,Pr为当前农业机械的位置,Pk+j为目标点,R为转弯半径,C为圆心点,qe为航向偏差,L为农业机械前后轮轴间距,d2为改进型纯追踪算法求得的前轮转角,那么在直角三角形BPrC中可以得出:由于qe为圆C的弦切角,因此可得:
—PrCPk+j=2qe
在等腰三角形PrCPk+j中,由正弦定理可得:
由上述三式消去R可得到前轮转角:
式中PrPk+j为农业机械当前位置与目标点的之间的距离;另外,将横向偏差作为控制器的一个观测量,再设定一个预设值dr,当观测到横向偏差d≠dr时,则将改进纯追型踪算法求得的前轮转角d2乘以一个比例系数kdr,由此可以增大前轮转角,使其快速到达目标点;
(4)最后根据前轮转角实时对农业机械进行控制,实现路径跟踪。
2.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述当前时刻农业机械的运动状态数据包括当前位置Pr(xr,yr)、当前速度V和当前航向角度q;所述预定义路径数据为n+1个位置点的集合,即{P0(x0,y0),P1(x1,y1),...,Pk(xk,yk),Pk+1(xk+1,yk+1),...,Pn(xn,yn)}。
3.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述预瞄控制方法,即动态搜索目标点,是通过动态计算前视距离,进而确定预定义路径上的预瞄点:(1)前视距离公式由下述公式计算得出:
Lf=L0+kvV+(Lb-kwW)
其中,Lf为前视距离;L0为前视距离基值;Lb为弯度的最大量化值;kv和kw分别是速度和弯度的比例系数;W为弯度,弯度的计算方法是:首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点Pk(xk,yk),由该点向后每隔i个点再取3个点,i为正整数,由此可得到4个点,分别是Pk、Pk+i、Pk+2i和Pk+3i,用直线连接这四个点,取直线PkPk+i和直线Pk+
2iPk+3i所夹的较小的角为弯度值,取值为0~180度;
(2)预瞄点Pm的确定:在农业机械当前纵向方向上,由当前位置点Pr向前截取前视距离Lf,得到P'm点,再通过快速查找算法得到预定义路径上与P'm点最近的点,该点即是预瞄点Pm。
4.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述的横向偏差和航向偏差的求解过程如下:(1)横向偏差d的确定:
首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点Pk(xk,yk),然后将农业机械当前位置点Pr向直线Pk(xk,yk)Pk+1(xk+1,yk+1)投影,得到点Pr',则横向偏差d=|PrPr'|;
(2)航向偏差qe的确定:
航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差:农业机械的当前位置点Pr与预瞄点Pm构成的向量为 设该向量沿逆时针方向旋转与y轴正向的夹角为b,那么航向偏差qe=b-q,q为当前航向角度。
5.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述的模糊控制方法具体为:(1)将步骤(2)得到的横向偏差和航向偏差模糊化,即采用基本连续论域,离散化采用均匀量化的方法,将连续论域中的值经量化因子变换后取整,具体为:q'e=int(kqqe)
d'=int(kdd)
其中q'e、d'均为连续数据取整后的离散量;kq为输入量化因子,qe为航向偏差;
横向偏差的连续论域离散化具体为:将横向偏差分为四个等级,当测得|d|∈(0,0.1]时,则kd=a1,当得|d|∈(0.1,0.3]时,则kd=a2,当|d|∈(0.3,05]时,则kd=a3,当d为其它值时,则kd=a4,其中a1、a2、a3和a4分别为横向偏差在四个等级下对应的取值;
(2)使用模糊控制表查询,打开规则观测器,输入不同的离散量d',q'e,得到对应的输出离散量d'1;
(3)输出去模糊控制,即模糊控制算法得出的前轮转角d1:从模糊控制量到实际输出控制量的转换公式如下:d1=kd1d'1
其中,输出的比例因子kd1的确定方法同输入量化因子。
6.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述步骤(3)具体为:通过改进型纯追踪算法和模糊控制算法分别得出前轮转角,然后将两个转角合成一个最终的转角,实现两种方法复合控制,其中合成公式如下:d=k1d1+k2d2
其中k1、k2分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角的折算因子,两者之和为1。
7.如权利要求3或4所述的农业机械自动导航控制方法,其特征在于,所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点的距离,距离最小的那个点即为最近点。