一种农业机械自动导航控制方法
【专利摘要】本发明公开了一种农业机械自动导航控制方法，该方法包括如下步骤：(1)GPS接收机实时测得当前时刻农业机械的运动状态数据；(2)采用预瞄控制方法，将运动状态数据与预定义路径数据进行比对分析，推算出横向偏差和航向偏差；(3)通过模糊控制算法进行模糊化，然后将模糊量导入模糊控制器中，得到前轮转角；通过改进型纯追踪算法建立几何模型，从而导求出前轮转角；将两种算法得到的前轮转角合成一个前轮转角；(4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。本发明由两种导航方法结合在一起实现自动控制，是稳定性良好，抗干扰能力强，能够提高系统的鲁棒性。两种方法的结合，优势互补，使其满足旱地和水田的作业需要。
【专利说明】
一种农业机械自动导航控制方法
技术领域
[0001] 本发明涉及农业机械导航控制领域，特别涉及一种农业机械自动导航控制算法。
【背景技术】
[0002] 农业机械导航控制的主要任务是根据各个传感器得到农业机械的导航位置，确定 该位置与预定义路径的位置关系，综合农业机械的运动状态以及合适的控制算法，决策出 相应的前轮转角，以修正路径跟踪误差。
[0003] 导航控制方法常用的有线性模型控制方法、模糊控制方法和最优控制方法。日本 东京大学利用机器视觉技术，根据线性转向控制模型，将目标方向角和农业机械纵向角融 合，计算出前轮转角，实现了转弯控制。随后东京大学又采用模糊控制技术来进行导航研 究，模糊控制器输入的是方向偏差和位置偏差，输出的是前轮的转向角度和左右执行的时 间，从而完成导航控制。O'Connor等人采用方向偏差、方向偏差变化率、转向角度、转向角度 变化率和跟踪位置误差这五个状态变量建立农业机械运动学方程，以角度变化率最小为优 化目标，利用农业机械线性运动学方程组建立了基于LQR的最优控制器，从而实现了导航控 制。Benson等人应用PID控制方法设计了 PID控制器，该算法不依赖精确的数学模型，避免了 繁琐的建模过程，只需要一些对象的响应特征来组合控制，对算法的比例参数、积分参数和 微分参数进行合理的调节，即可实现良好的路径跟踪效果。专利ZL200710029018.4中应用 模糊控制算法和PID算法设计控制器，当横向偏差较小时，应用PID控制；当横向偏差较大 时，采用模糊控制。该方法在一定范围内保证了稳态跟踪精度和控制的稳定性，但由于在实 际导航中的每一时刻，只采用了其中一种控制算法，因此很难发挥两种控制算法的互补优 势。
[0004] 通过以上分析可知，导航控制的难点和重点是提高路径跟踪的精度和转向控制的 稳定性。常规的线性模型控制方法，包括PID控制方法，能够获得较高精度的路径跟踪效果， 但是抗干扰能力较弱；模糊控制算法具有良好的稳定性，但是路径跟踪的稳态精度难以保 证;最优控制器的建立要依赖精确的农业机械运动学和动力学模型，但是在田间作业条件 下，土壤特性的变化、农业机械与地面相互作用的变化，导致农业机械运动学和动力学模型 的建立比较困难，
【发明内容】
[0005] 本发明的目的在于改进和完善导航控制技术中的不足，提供一种路径跟踪精度相 对较高、稳定性较好的导航控制方法。该方法基于预瞄跟随理论，采用改进型纯追踪算法和 模糊控制相结合的方法来设计控制器，建立一种稳定性好、精度高的路径跟踪算法。
[0006] 为了达到上述目的，本发明所采用的技术方案如下：一种农业机械自动导航控制 方法，该方法包括如下步骤：
[0007] (1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据；
[0008] (2)采用预瞄控制方法，动态搜索目标点，将步骤1得到运动状态数据与预定义路 径数据进行比对分析，推算出横向偏差和航向偏差；
[0009] (3)通过模糊控制算法将步骤2得到的横向偏差和航向偏差模糊化，然后将模糊量 导入模糊控制器中，得到前轮转角;通过改进型纯追踪算法建立几何模型，从而由步骤2中 得到的航向偏差推导求出前轮转角;将模糊控制算法和改进型纯追踪算法得到的前轮转角 合成一个最终的前轮转角；
[0010] (4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。
[0011] 进一步的，所述当前时刻农业机械的运动状态数据包括当前位置pr(xr，yr)、当前 速度V和当前航向角度q;所述预定义路径数据为n+1个位置点的集合，即{Po(xo，y())，Ρι(χι, yi)，· · ·，Pk(xk，yk)，Pk+i(xk+i，yk+i)，· · ·，Pn(xn，yn)} 〇
[0012] 进一步的，所述预瞄控制方法，即动态搜索目标点，是通过动态计算前视距离，进 而确定预定义路径上的预目苗点：
[0013] (1)前视距离公式由下述公式计算得出：
[0014] Lf = L〇+kvV+(Lb-kwff)
[0015]其中，Lf为前视距离;Lo为前视距离基值;Lb为弯度的最大量化值;kv和1^分别是速 度和弯度的比例系数，这两个系数可在仿真或试验中通过整定获得;W为弯度，弯度的计算 方法是：首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点P k(Xk， yk)，由该点向后每隔i个点再取3个点，i为正整数，由此可得到4个点，分别是Pk、Pk+1、P k+2_ Pk+3i，用直线连接这四个点，取直线PkPk+i和直线Pk+2iPk+3i所夹的较小的角为弯度值，取值为 0 ~180:
[0016] (2)预瞄点Pm的确定:在农业机械当前纵向方向上，由当前位置点Pr向前截取前视 距离Lf，得到P'"点，再通过快速查找算法得到预定义路径上与P'm点最近的点，该点即是预 瞄点Pm。
[0017] 进一步的，所述的横向偏差和航向偏差的求解过程如下：
[0018] (1)横向偏差(1的确定：
[0019] 首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点Pk(Xk， yk)，然后将农业机械当前位置点Pr向直线Pk(Xk，yk)P k+1(Xk+1，yk+1)投影，得到点Ρ\，则横向 偏差d= |PrP\| ;
[0020] (2)航向偏差qe的确定：
[0021] 航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差:农 业机械的当前位置点Pr与预目苗点Pm构成的向量为，设该向量沿逆时针方向旋转与y轴正 向的夹角为A，那么航向偏差兔=力-分&
[0022] 进一步的，所述的模糊控制方法具体为：
[0023] (1)将步骤2得到的横向偏差和航向偏差模糊化，即采用基本连续论域，离散化采 用均匀量化的方法，将连续论域中的值经量化因子变换后取整，具体为：
[0024] q7 e = int(kqqe)
[0025] d'=int(kdd)
[0026] 其中q'hd'均为连续数据取整后的离散数据;输入量化因子kq的确定方法是:根据 仿真和试验测试结果，确定航向偏差的误差范围，由离散论域最大值除以相应误差范围最 大值即可获得量化因子，获得之后再通过试验进行修正即可；
[0027] 横向偏差的连续论域离散化具体为:将横向偏差分为四个等级，当测得I d|e[0， 0 · 1]时，贝1Jkd = ai，当得 I d I e (ο · 1，〇 · 3]时，贝ljkd = a2,当 I d I e (〇· 3,05]时，贝ljkd = a3,当d为 其它值时，则kd = a4，其中ai、a2、a3和a4分别为横向偏差在四个等级下对应的取值；
[0028] (2)使用模糊控制表查询，打开规则观测器，输入不同的离散量dW e，得到对应 的输出尚散量4 ;
[0029] (3)输出去模糊控制，即模糊控制算法得出的前轮转角A :从模糊控制量到实际输 出控制量的转换公式如下：
[0030] dx =
[0031] 其中，输出的比例因子Μ勺确定方法同输入量化因子。
[0032] 进一步的，所述的改进型纯追踪算法为：
[0033] 利用航向偏差建立几何模型，求解农业机械到达目标点所需要的转弯半径，然后 由转弯半径再求解出前轮转角，同时把横向偏差作为观测量，如果横向偏差超过设定值，那 么给前轮转角一个补偿量以增大前轮转角，使农业机械快速转弯到达目标点；
[0034] Pr为当前农业机械的位置，Pk+j为目标点，R为转弯半径，C为圆心点，qe为航向偏 差，L为农业机械前后轮轴间距，.4为改进型纯追踪算法求得的前轮转角，那么在直角三角 形BP rC中可以得出：
[0036]由于qe为圆C的弦切角，因此可得：
[0037] -PrCPk+j = 2qe
[0038] 在等腰三角形PrCPk+j中，由正弦定理可得：
[0040]由上述三式消去R可得到前轮转角：
[0042]式中PrPk+j为农业机械当前位置与目标点的之间的距离；另外，将横向偏差作为控 制器的一个观测量，再设定一个预设值dr，当观测到横向偏差d矣dr时，则将改进纯追型踪算 法求得的前轮转角4乘以一个比例系数kdr，由此可以增大前轮转角，使其快速到达目标点； 其中dr和kdr的大小通过仿真或试验整定得出。
[0043] 进一步的，所述步骤3具体为:通过改进型纯追踪算法和模糊控制算法分别得出前 轮转角，然后将两个转角合成一个最终的转角，实现两种方法复合控制，其中合成公式如 下：
[0044] d = kjdy +k2d,
[0045] 其中lu、k2分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮 转角的折算因子，两者之和为1，两者取值由仿真和试验整定得出。
[0046] 进一步的，所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点 的距离，距离最小的那个点即为最近点。
[0047] 本发明与现有技术相比有如下特点：
[0048] (1)本发明对传统纯追踪算法做了改进，使其不仅适应直线行驶还能满足曲线转 弯，同时结合模糊控制算法，在实际导航中充分发挥了上述两种导航算法各自的优势并达 到互补的效果，因此极大地提高了控制的精度和稳定性。
[0049] (2)本发明在导航农业机械沿预定义路径行驶时，直线段行驶的跟踪误差小于 5cm，曲线转弯段行驶的跟踪误差小于20cm〇
[0050] (3)本发明适合于农业机械自动化装备，能够满足旱地的作业需要，同时也可满足 水田作业要求。
【附图说明】
[0051] 图1为弯度求解图；
[0052]图2为横向偏差求解图；
[0053]图3为横向偏差正负号求解图；
[0054]图4为航向偏差求解图；
[0055]图5为模糊变量的隶属函数图；
[0056]图6为模糊控制输出的三维外观图；
[0057]图7为改进型纯追踪算法原理图；
[0058]图8为导航控制原理框图。
【具体实施方式】
[0059] 为使本发明更明显易懂，兹以一实施例作进一步详细描述，但不用来限制本发明 的范围。
[0060] 一种农业机械自动导航控制方法，该方法包括如下步骤：
[0061 ] (1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据；
[0062] (2)采用预瞄控制方法，动态搜索目标点，将步骤1得到运动状态数据与预定义路 径数据进行比对分析，推算出横向偏差和航向偏差；
[0063] (3)通过模糊控制算法将步骤2得到的横向偏差和航向偏差模糊化，然后将模糊量 导入模糊控制器中，得到前轮转角;通过改进型纯追踪算法建立几何模型，从而由步骤2中 得到的航向偏差推导求出前轮转角;将模糊控制算法和改进型纯追踪算法得到的前轮转角 合成一个最终的前轮转角；
[0064] (4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。
[0065] 进一步的，所述当前时刻农业机械的运动状态数据包括当前位置Pr(xr，yr)、当前 速度V和当前航向角度q，如图1所示;具体定义如下：
[0066] (1)当前定位点Pr(xr，yr)，由GPS接收机获得当前的差分定位数据；
[0067] (2)当前速度V，由速度传感器测得当前农业机械纵向方向的前进速度；
[0068] (3)当前航向角度q，由电子罗盘获得当前农业机械纵向方向的角度，在高斯投影 平面坐标系中，表示为与y轴顺时针旋转的夹角，范围为0~360。
[0069] 所述的预定义路径是由人工驾驶车辆，GPS接收机记录的路径数据，是一个点数 据，其数学表达式为：
[0070] {P〇(x〇，y〇)，Pi(xi，yi)，·· ·，Pk-i(Xk-i，yk-1)，Pk(Xk，yk)}
[0071] 需要说明的是，预定义路径由点数据集表示，Pk为第k个点，（Xk，yk)为第k个点的坐 标，所有点的坐标均在WGS-84大地坐标系中，表示为高斯投影平面坐标。
[0072] 所述预瞄控制方法，即动态搜索目标点，是通过动态计算前视距离，进而确定预定 义路径上的预瞄点：
[0073] (1)前视距离公式由下述公式计算得出：
[0074] Lf = Lo+kvV+ (Lb-kwff)
[0075]其中，Lf为前视距离;Lo为前视距离基值;Lb为弯度的最大量化值;kv和1^分别是速 度和弯度的比例系数，这两个系数可在仿真或试验中通过整定获得;W为弯度，如图1所示， 弯度的计算方法是:首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的 点Pk(x k，yk)，由该点向后每隔i个点再取3个点，i为正整数，由此可得到4个点，分别是Pk、 Pk+i、Pk+2_Pk+3i，用直线连接这四个点，取直线PkPk+i和直线Pk+2iPk +3i所夹的较小的角为弯 度值，取值为〇~180:
[0076] (2)预瞄点Pm的确定:在农业机械当前纵向方向上，由当前位置点Pr向前截取前视 距离L f，得到P'"点，再通过快速查找算法得到预定义路径上与P'm点最近的点，该点即是预 瞄点K。
[0077] 所述的横向偏差和航向偏差的求解过程如下：
[0078] (1)横向偏差(1的确定：
[0079] 如图2所示，首先通过快速查找算法得到预定义路径上与当前农业机械位置最近 的点Pk(Xk，yk)，然后将农业机械当前位置点Pr向直线Pk(Xk，yk)Pk+1 (Xk+i，yk+1)投影，得到点 P'r，则横向偏差d= |P^r| ;
[0080] 为了导航控制算法设计的方便，这里对横向偏差正负号，在车辆沿预定义路径前 进的方向上，如果车辆位于预定义路径右侧，则d为正，如果在左侧，则d为负;判断正负的方 法是：如图3所示，首先在预定义路径中找到距离车辆当前位置点最近的点P k，然后沿着路 径前进方向寻找下一个点Pk+i，于是构成一个向量^^，设该向量逆时针旋转与y轴正向的 夹角为ai。由车辆当前位置点Pr与点Pk+i，又可组成一个向量:设该向量逆时针旋转与 y轴正向的夹角为a2。如果ai_a2为正，则车辆在右侧，那么横向偏差为正，否则为负。
[0081 ] (2)航向偏差qe的确定：
[0082]航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差:如 图4所示，农业机械的当前位置点Pr与预瞄APm构成的向量为7^，设该向量沿逆时针方向 旋转与y轴正向的夹角为i?，那么航向偏差孓=力-g *角度的取值范围为-90~90,如果为 正，则表示车辆当前的航向在目标方向的左边，否则在右边。
[0083]所述的模糊控制方法具体为：
[0084]模糊控制是将驾驶员的驾驶经验转换为模糊规则，进而决策出前轮转角。基本思 路是当横向偏差和航向偏差越大，那么控制的程度越强，即前轮转角越大；当横向偏差和航 向偏差越小，那么控制的程度越弱，即前轮转角越小；当两者一个较大一个较小，那么控制 的程度为中。规定前轮右转时为正，左转时为负。驾驶经验转换为模糊规则的方法是:将农 业机械相对于预定义路径的状态分为七种，即严重偏左、偏左、稍微偏左、正对着路径、稍微 偏右、偏右、严重偏右，根据这七种状态，驾驶员会做出相应的转向控制策略，对应的也分为 七种，即大幅度右转、右转、稍微右转、不转向、稍微左转、左转、大幅度左转。据此建立模糊 控制规则表，如表1所示。具体设计步骤是在Matlab环境中设计的，如下所示：
[0085] (1)首先将两个输入变量d、qe和一个输出变量4进行模糊化，其模糊子集分别选 为：
[0086] Fd={NL，NM，NS，ZO，PS，PM，PL}
[0087] = J NL NM, ；V5, ZO, PS, PM, PL\
[0088] = ? NL, NM, NS, ZO, PS, PM " PL ^
[0089] 三个变量的模糊分割均为7级，即负大、负中、负小、零、正小、正中和正大。选取横 向偏差、航向偏差和输出转角的离散论域分别为：
[0090] d={-6，-5，-4，-3，-2，-l，0，l，2,3,4,5,6}
[0091] qe={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}
[0092] ： -6,-5,-4, -3? ~2> -1,(),1,2,3,4,5,-7}
[0093]模糊推理系统的基本属性确定为："与"运算采用极小运算，"或"运算采用极大运 算，模糊蕴涵米用极小运算，模糊规则综合米用极大运算，去模糊化米用重心法。航向偏差 的基本连续论域离散化采用均匀量化的方法，即将连续论域中的连续值经量化因子比例变 换后四舍五入变为离散论域的整数值，具体求解公式为：
[0094] q7 e = int(kqqe)
[0095] d'=int(kdd)
[0096] 其中q'hd'均为连续数据取整后的离散数据;输入量化因子kq的确定方法是:根 据仿真和试验测试结果，确定航向偏差的误差范围，由离散论域最大值除以相应误差范围 最大值即可获得量化因子，获得之后再通过试验进行修正即可；
[0097] 横向偏差的连续论域离散化具体为:将横向偏差分为四个等级，当测得|d|e[0， 0· 1]时，贝1Jkd = ai，当得 I d I e (ο · 1，〇· 3]时，贝ljkd = a2,当 | d I e (〇· 3,05]时，贝ljkd = a3，当d为 其它值时，则kd = a4，其中ai、a2、a3和a4分别为横向偏差在四个等级下对应的取值，其数值通 过仿真或试验整定得出。
[0098]在Mat 1 ab环境中，建立模糊变量横向偏差、航向偏差和输出转角的隶属函数曲线， 如图5(a)、(b)和(c)所示。对应的模糊控制规则表，利用Matlab软件绘出模糊控制表的三维 外观图，如图6所示。
[0099] (2)使用模糊控制表查询，模糊控制表如表2所示，打开规则观测器，输入不同的离 散量d '，q' e，得到对应的输出离散量
[0100] (3)输出去模糊控制，即模糊控制算法得出的前轮转角吟?从模糊控制量到实际输 出控制量的转换公式如下：
[01 01 ] cfj = k^jdj
[0102 ]其中，输出的模糊控制算法得出的前轮转角的比例因子的确定方法同输入量化 因子。
[0103] 进一步的，所述的改进型纯追踪算法为：
[0104] 利用航向偏差建立几何模型，求解农业机械到达目标点所需要的转弯半径，然后 由转弯半径再求解出前轮转角，同时把横向偏差作为观测量，如果横向偏差超过设定值，那 么给前轮转角一个补偿量以增大前轮转角，使农业机械快速转弯到达目标点；
[0105]在图6中，一组点为预定义路径点，Pr为当前农业机械的位置，Pk+j为目标点，R为转 弯半径，C为圆心点，qe为航向偏差，L为农业机械前后轮轴间距，锋为改进型纯追踪算法求 得的前轮转角，那么在直角三角形BP rC中可以得出：
[0107]由于qe为圆C的弦切角，因此可得：
[0108] -PrCPk+j = 2qe
[0109] 在等腰三角形PrCPk+j中，由正弦定理可得：
由上述三式消去R可得到前轮转角：
[0113] 式中PrPk+j为农业机械当前位置与目标点的之间的距离；另外，将横向偏差作为控 制器的一个观测量，再设定一个预设值dr，当观测到横向偏差d矣dr时，则将改进纯追型踪算 法求得的前轮转角4乘以一个比例系数kdr，由此可以增大前轮转角，使其快速到达目标点； 其中dr和kdr的大小通过仿真或试验整定得出。
[0114] 进一步的，所述步骤3具体为:通过改进型纯追踪算法和模糊控制算法分别得出前 轮转角，然后将两个转角合成一个最终的转角，实现两种方法复合控制，其中合成公式如 下：
[0115] +k2d,
[0116] 其中ki、k2分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮 转角的折算因子，两者之和为1，两者取值由仿真和试验整定得出。
[0117] 进一步的，所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点 的距离，距离最小的那个点即为最近点。
[0118] 上述两种导航控制方法的结合，既能提高路径跟踪的稳态精度，又能提高路径跟 踪的稳定性。其中改进型纯追踪算法是从几何的角度推算的，由航向偏差经过几何运算得 出前轮转角，运算过程未进行近似处理，因此得出的是精确解。该解对于消除导航控制系统 中的偏差和稳态误差起到重要作用，有利于提高导航精度。
[0119] 模糊控制方法是一种近似求解方法，该方法的特点是不需要确切的理论模型，仅 根据人的驾驶经验来制定模糊规则，建立模糊控制器。因此抗干扰能力较强，不会因模型变 化而改变控制效果，能够消除导航系统的振荡误差。
[0120] 本实施例使用的农业机械为井关插秧机，在其上装载了导航传感器组合、转向控 制系统和车载计算机，各部分之间通信使用总线，由此构成导航控制系统，其原理结构如图 8所示。导航传感器组合包括差分GPS、电子罗盘、速度传感器和微机械陀螺仪。转向控制系 统包括操纵控制器、转向驱动机构和测试传感器。车载计算机主要实现数据采集、数据预处 理、导航控制算法和控制指令输出等功能，动态数据采集频率为5Hz。
[0121] 具体实施的步骤如下：
[0122] 1、预定义路径数据获取
[0123] 首先由驾驶员驾驶插秧机在水田中正常作业行走，由GPS接收机实时获取定位信 息并以总线数据的形式输出，该数据构成预定义路径，路径包括直线段和曲线转弯段，其数 学表达式为卩|11] = {(叉()，7())，（叉1，71)，...，（叉1<，71〇，...，（叉11，711)}
[0124] 2、导航数据获取
[0125] (1)插秧机位姿和速度参数的获取
[0126] 涉及数据包括有:插秧机当前定位点坐标、当前航向角度和当前插秧机速度。
[0127] (2)前视距离的求解
[0128] 前视距离的定义如图2所示，求解方法采用前述公式（1)进行求解，利用速度和路 径的弯度来确定合适的前视距离，进而可以确定目标点。各参数取值如下：
[0129] L〇 = 0.15，Lb = 0.62，kv = 0.31，kw=0.1
[0130] (3)导航偏差的求解
[0131] 所述的导航偏差分为横向偏差和航向偏差，定义及计算过程如下所示：
[0132] 1)目标方向i?的计算
[0133] 在图2中，即为矢量为的方向，以高斯投影平面坐标系xoy中y轴正向为0度，顺 时针旋转到矢量7^的角度即为力，该角度取值范围为0~360。
[0134] 2)航向偏差qe的计算
[0135] 在图2中，即为目标方向角与插秧机当前航向角度之间的差值。
[0136] 3)横向偏差d的计算
[0137] 在图2中，设点为农业机械当前位置点向预定义路径上投影的点，则dilPrP'r ，判断正负的方法同前所述。
[0138] (4)前轮转角模糊控制算法
[0139] 将横向误差和航向误差以及前轮转角的离散论域分别选取为：
[0140] d={-6，-5，-4，-3，-2，-l，0，l，2,3,4,5,6}
[0141] qe={-6，-5，-4，-3，-2，_l，0，l，2,3,4,5,6}
[0142] 4 = ! -7, - 6,-5,-4,-3, - 2, - 1,0,1,2,3,4,5,6, - 7|·
[0143] 1)输入变量模糊化
[0144] 将输入连续论域中的连续值经量化因子比例转换为离散量，然后四舍五入取值。 其中航向偏差与横向偏差分别按上述方法进行量化，其中各参数取值如下所示：
[0145] kq = 0 · 25，ai = 12、a2 = 2、a3 = 0 · 2、a4 = 0
[0146] 2)模糊控制表查询
[0147] 输入不同的离散输入量d '和Y e，依据路径跟踪模糊控制表，如表2所示，即可得到 对应的输出尚散量。
[0148] 3)输出控制量清晰化
[0149] 模糊量到实际输出控制量的转换公式如下所示：
[0150] c?! =kd,4
[0151] 其中参数=-21.429。
[0152] (5)前轮转角改进型纯追踪算法
[0153] 首先根据插秧机当前的航向偏差计算出插秧机到达目标点所需的转弯半径，进而 求出前轮转角，计算公式如公式(9)所示，然后利用横向偏差作为观测量，如果偏差过大，那 么将前轮转角乘以一个系数，将其增大，这样插秧机能够快速到达目标点，各参数分别为：
[0154] dr = 0.02、kdr = l .08
[0155] (6)模糊控制和改进型纯追踪控制复合决策
[0156] 上述两种算法得出两个前轮转角，利用如下公式进行综合：
[0157] d = kj^ + kjd,
[0158] 其中整定出的两个参数数值分别为：
[0159] ki = 0 · 2、k2 = 0 · 8
[0160] (7)输出控制参数
[0161]将决策出的前轮转角按一定的总线通信格式打包，发送给转向控制系统，即可实 现前轮转角的控制。
[0162]表1、模糊控制规则表
[0164] 表2、路径跟踪模糊控制表
【主权项】
1. 一种农业机械自动导航控制方法，其特征在于，该方法包括如下步骤： (1) 通过GPS接收机实时测得当前时刻农业机械的运动状态数据； (2) 采用预瞄控制方法，动态搜索目标点，将步骤1得到运动状态数据与预定义路径数 据进行比对分析，推算出横向偏差和航向偏差； (3) 通过模糊控制算法将步骤2得到的横向偏差和航向偏差模糊化，然后将模糊量导入 模糊控制器中，得到前轮转角;通过改进型纯追踪算法建立几何模型，从而由步骤2中得到 的航向偏差推导求出前轮转角;将模糊控制算法和改进型纯追踪算法得到的前轮转角合成 一个最终的前轮转角； (4) 最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。2. 如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述当前时刻农业机 械的运动状态数据包括当前位置Pr( Xr，yr)、当前速度V和当前航向角度q;所述预定义路径 数据为n+1个位置点的集合，gp{Po(xo，yo)，Pi(xi，yi)，...，Pk(xk，yk)，Pk+i(xk+i，yk+i)，...，P n (Χη,Υη) } 〇3. 如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述预瞄控制方法， 即动态搜索目标点，是通过动态计算前视距离，进而确定预定义路径上的预瞄点： (1) 前视距离公式由下述公式计算得出： Lf = L〇+kvV + (Lb_kwff) 其中，Lf为前视距离;L〇为前视距离基值;Lb为弯度的最大量化值;k4Pkw分别是速度和 弯度的比例系数;W为弯度，弯度的计算方法是:首先通过快速查找算法确定预定义路径上 与当前农业机械位置最近的点Pk(x k，yk)，由该点向后每隔i个点再取3个点，i为正整数，由 此可得到4个点，分别是Pk、Pk+i、Pk +2dPPk+3i，用直线连接这四个点，取直线PkPk+i和直线Pk+ 2iPk+3i所夹的较小的角为弯度值，取值为0~180° ; (2) 预瞄点Pm的确定：在农业机械当前纵向方向上，由当前位置点Pr向前截取前视距离 Lf，得到，再通过快速查找算法得到预定义路径上与P、点最近的点，该点即是预瞄点 Pm〇4. 如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述的横向偏差和航 向偏差的求解过程如下： (1) 横向偏差(1的确定： 首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点Pk(Xk，yk)，然 后将农业机械当前位置点P:向直线Pk(Xk，yk)Pk+1(x k+1，yk+1)投影，得到点P\，则横向偏差d = PrP7 rI ； (2) 航向偏差的确定： 航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差:农业机 械的当前位置点Pr与预目苗点Pm构成的向量为，设该向量沿逆时针方向旋转与y轴正向 的夹角为b，那么航向偏差qe = b-q。 5 .如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述的模糊控制方法 具体为： (1)将步骤2得到的横向偏差和航向偏差模糊化，即采用基本连续论域，离散化采用均 匀量化的方法，将连续论域中的值经量化因子变换后取整，具体为： Q e=illt(kqqe) d' = int(kdd) 其中q%、d'均为连续数据取整后的离散量;kq为输入量化因子； 横向偏差的连续论域离散化具体为:将横向偏差分为四个等级，当测得|d|e [〇,〇.1] 时，贝收〇1 = 31，当得|(1|￡(〇.1，〇.3]时，贝1|1^ = 32，当|(1|￡(〇.3,05]时，贝1|1^ = 33，当(1为其它 值时，则kd = a4，其中ai、a2、a3和a4分别为横向偏差在四个等级下对应的取值； (2) 使用模糊控制表查询，打开规则观测器，输入不同的离散量d'，q%，得到对应的输出 呙散量d'i; (3) 输出去模糊控制，即模糊控制算法得出的前轮转角δ::从模糊控制量到实际输出控 制量的转换公式如下： di = kdid'i 其中，输出的比例因子1^的确定方法同输入量化因子。6. 如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述的改进型纯追踪 算法为： 利用航向偏差建立几何模型，Pr为当前农业机械的位置，Pk+j为目标点，R为转弯半径，C 为圆心点，qe为航向偏差，L为农业机械前后轮轴间距，d2为改进型纯追踪算法求得的前轮转 角，那么在直角三角形BP rC中可以得出：由于qe为圆C的弦切角，因此可得： -PrCPk+j = 2qe 在等腰三角形PrCPk+j中，由正弦定理可得：由上述三式消去R可得到前轮转角：式中PrPk+j为农业机械当前位置与目标点的之间的距离;另外，将横向偏差作为控制器 的一个观测量，再设定一个预设值dr，当观测到横向偏差d矣dr时，则将改进纯追型踪算法求 得的前轮转角d2乘以一个比例系数kdr，由此可以增大前轮转角，使其快速到达目标点。7. 如权利要求1所述的农业机械自动导航控制方法，其特征在于，所述步骤3具体为:通 过改进型纯追踪算法和模糊控制算法分别得出前轮转角，然后将两个转角合成一个最终的 转角，实现两种方法复合控制，其中合成公式如下： d = kidi+k2d2 其中lu、k2分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角 的折算因子，两者之和为1。8. 如权利要求3和4所述的农业机械自动导航控制方法，其特征在于，所述快速查找算 法是计算预定义路径的每个点与当前农业机械坐标点的距离，距离最小的那个点即为最近
【文档编号】G05D1/02GK105867377SQ201610228933
【公开日】2016年8月17日
【申请日】2016年4月13日
【发明人】李革, 王宇, 吴苗苗, 章铁成, 王嘉鹏, 郭刘粉
【申请人】浙江理工大学