CN101093396A - 一种农业机械的导航控制方法 - Google Patents

Abstract

本发明公开了一种农业机械的导航控制方法，包括通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据；采用基于预瞄控制的动态目标点搜索算法，推算出两个导航状态参数即横向跟踪误差和纵向航向偏差；再通过Fuzzy－PID控制算法推算出操纵控制量即前轮转向偏角；最后根据所述操纵控制量对农业机械进行实时控制，实现路径跟踪。本发明充分利用传统导航控制方法的优点，结合模糊控制方法，兼顾稳态跟踪精度和控制稳定性，提高了农业机械作业条件的导航跟踪精度、稳定性和安全性；不仅适用于旱地农业机械的作业需要，还适用于水田农业机械的作业需要，具有广阔的推广应用前景。

Description

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

技术领域

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

背景技术

农业机械导航控制的主要任务是根据导航定位结果，确定农业机械与预定路线的位置关系，进而结合农业机械的运动状态决策出合适的转向轮操纵角，以修正路径跟踪误差。国外常用的导航控制方法有三种，即线性模型控制方法、最优控制方法和模糊控制方法。东京大学利用机器视觉技术进行KUBOTA SPJ45A型久保田插秧机的导航控制技术研究，他将目标方向角度与车辆纵向方向角度进行对比，用Abe的线性转向控制模型计算得出转向轮转向角度。Qiu提出了一种基于预瞄的导航控制算法，该导航控制器包括一个前馈比例控制器和反馈PD控制器；前进速度决定前视距离，位置误差决定前馈增益；前馈控制器的输入是路径曲率，反馈控制器的输入是航向偏差，两控制器的输出合成为期望转向轮操纵角。东京大学采用模糊控制技术来进行导航控制研究，模糊控制器根据方向偏差和位置偏差确定转向角度和左转执行时间及右转执行时间。韩国的Lee等人利用机器视觉、DGPS、超声波传感器和模糊控制技术进行果树喷雾机的导航控制研究。O’Connor等人采用方向偏差、方向偏差变化率、转向角度、转向角度变化率、跟踪误差等五个状态变量建立车辆运动学方程，以转向角度变化率最小为优化指标，利用车辆线性运动学方程组建立了基于LQR的最优导航控制器。Kise等人开发了最优操纵控制器用于农用车辆导航，在曲线路径跟踪方面取得了一定的效果。周俊(2003)提出了视觉导航的图象处理技术，同时还利用线性状态反馈控制方法、二值控制方法建立了导航控制的内外环结构；在车辆纵向速度较高的状态下，提出了把横向偏差、航向偏差以及横向加速度作为输入量的三维横向模糊控制算法，以保证车辆横向运动的平稳性。毛恩荣等人(2005)研究了在铁牛654拖拉机上搭建DGPS自动驾驶系统的硬件组成及关键技术，根据最优控制理论，研究了横向偏差和航向角输出反馈控制系数的离线优化调节方法，为试验凑试最优控制参数提供依据。本发明人在2005年将GPS技术、计算机技术、传感器技术进行集成，研制了一种以蓄电池为电源、电动机为动力的农用智能移动作业平台，该样机采用前轮驱动、差速转向方式；并以DGPS、电子罗盘为主要导航传感器，在样机上建立了DGPS导航控制系统；还设计了基于预瞄跟随的导航控制算法，其核心是由航向偏差线性决定驱动轮速度差。由于该智能移动作业平台采用差速转向方式，路径跟踪的控制难度较大，导航精度难以提高。

常规线性模型控制方法，包括PID控制方法，可以获得高精度的路径跟踪效果，但是抗干扰能力弱。模糊控制方法具有良好的稳定性效果，但路径跟踪的稳态精度难以保证。最优控制器的建立主要依据农业机械运动学和动力学模型，其模型精确性对控制器性能有重要影响。但是，田间作业条件下农业机械运动学和动力学模型的建立是比较困难的。田间作业时，随着农业机械、作业机具与地面相互作用的变化，田间土壤特性的变化，农业机械运动特性必然是时变的，还有作业工况差引起的传感器测试干扰等，这些因素对常规线性模型控制方法和最优控制方法的影响都较大。

发明内容

本发明的目的在于克服现有技术中存在的缺点，提供一种导航跟踪精度高、稳定性好的农业机械的导航控制方法。该方法基于预瞄跟随理论，采用PID和模糊逻辑相结合的方法来设计路径跟踪的控制算法，充分利用PID路径跟踪的高精度和模糊控制的稳定性特点，建立稳定性好、精度高的路径跟踪控制算法。

本发明一种农业机械的导航控制方法，包括下述步骤：

(1)通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据；

(2)采用基于预瞄控制的动态目标点搜索算法，将所述位姿和运动状态数据与预定义路线数据进行对比分析，推算出两个导航状态参数即横向跟踪误差和纵向航向偏差；

(3)再通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角；

(4)最后根据所述操纵控制量对农业机械进行实时控制，实现路径跟踪。

所述的预定义路线P[N]由点数组表示，包含元素为：

{P₀，P₁，...，P_k-1，P_k，P_k+1，...，P_N}

其数学表达式为：

{(x₀，y₀)，(x₁，y₁)，...，(x_k-1，y_k-1)，(x_k，y_k)，(x_k+1，y_k+1)，...，(x_n，y_n)}

在GIS中，预定义路线由点数据集表示。P_k点的坐标(x_k，y_k)为WGS-84大地坐标系的高斯投影平面坐标。点与点之间的平均间距表示为Ls。本发明中所述坐标点均为WGS-84大地坐标系的高斯投影平面坐标。

所述导航传感器包括差分全球定位系统(DGPS)接收模块，机器视觉定位，超声波、激光等测距定位，以及陀螺仪、电子罗盘、速度计、里程计等航位推算定位(DR)设备。

所述的农业机械当前时刻的位姿和运动状态数据包括当前定位点、当前航向角度和当前速度，如图1所示，具体定义如下：

(1)当前定位点P_c：由DGPS接收机获得的当前时刻差分定位数据，以(x_c，y_c)表示；

(2)当前航向角度θ：由电子罗盘数据获得的当前时刻农业机械纵向方向的角度；以高斯投影平面坐标系xoy中x轴正向为0度，逆时针方向为正；

(3)当前速度V：由速度传感器获得的当前时刻农业机械纵向方向的前进速度。

所述的基于预瞄控制的动态目标点搜索算法，是通过动态计算前视距离，确定预定义路线上的预瞄点，具体步骤如下：

(1)前视距离L_f的确定：

如图1所述，前视距离L_f可解释为在农业机械当前纵向方向上的预瞄距离；考虑农业机械速度的影响，前视距离L_f可通过下述公式计算得到：

L_f＝L₀+K_lfvV+K_lfrR_df                                  (1)

其中，L₀为前视距离基值；K_lfv，K_lfr分别为前进速度、前视偏差变化率的比例系数，可在仿真或实验过程中通过整定获得，K_lfv与K_lfr均取正值，K_lfv与K_lfr的取值范围为0≤K_lfv≤1，0≤K_lfr≤1；R_df为前视偏差变化率，定义为：

$R_{\mathrm{df}}=\frac{\left|\stackrel{\→}{P_{k+2m}+P^{\′}k+2m}\right|-\left|\stackrel{\→}{P_k{P^{\′}}_k}\right|}{\left|\stackrel{\→}{P_{k+m}{P^{\′}}_{k+m}}\right|-\left|\stackrel{\→}{P_k{P^{\′}}_k}\right|}---\left(2\right)$

其中 $m=\mathrm{int}\left(\frac{{\mathrm{VT}}_s}{L_s}\right),$ P_k表示预定义路线上的第k坐标点，P′_k映射点为在直线P_kP_k-1上以P_k为垂足引垂线和农业机械当前纵向方向线的相交点，T_s为导航的采样和控制周期，Ls为预定义路线上点与点之间的平均间距；

(2)预瞄点P′_p的确定：在农业机械当前纵向方向上，由当前定位点P_c点起截取前视距离L_f，得到P_p点；再由P_p点向预定义路线投影，得到预瞄点P′_p。

所述的两个导航状态参数即横向跟踪误差和纵向航向偏差的推算过程如下：

(1)纵向航向偏差θ_e的确定：纵向航向偏差θ_e为目标方向θ_p与当前农业机械航向角度θ之间的差值；其中目标方向θ_p为矢量P_cP′_p的方向，以高斯投影平面坐标系xoy中x轴正向为0度，逆时针方向为正；

(2)横向跟踪误差XTE的确定：

如图1所示，设P_c到预定义路线的投影是P′_c，则XTE＝|P_cP′_c|，即

$\mathrm{XTE}=\sqrt{{(x_c-{x^{\′}}_c)}^2+{(y_c-{y^{\′}}_c)}^2}---\left(3\right)$

为了导航控制算法设计的方便，这里对XTE规定正负号，在农业机械沿预定义路线的前进方向上，若农业机械当前坐标点位于预定义路线的右侧，XTE为正；若当前坐标点位于预定义路线的左侧，XTE为负；判断XTE正负号的方法是：如图2所示，首先在预定义路线P[N]中，寻找到距离当前定位点P_c最近的节点P_k，沿预定义路线前进方向寻找下一节点P_k+1；规定向量

到 的夹角顺时针为负，逆时针为正；若向量

到

的夹角为正，表明当前坐标点位于预定义路线的左侧，XTE为负；若向量

到

的夹角为负，表明当前坐标点位于预定义路线的右侧，XTE为正。

所述通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角的具体方法步骤是：当位置偏差在区间[-a，a]且航向偏差在区间[-b，b]时，为PID控制方法的作用范围；其他区间为模糊逻辑方法Fuzzy的作用范围，实现两种决策算法的复合控制；其中，a和b的取值范围为0≤a≤0.5，0≤b≤45，其原理如图7所示。

其中，PID控制方法的算法为：该算法的实质是将位置偏差和航向偏差转换为合成误差ERROR，并将合成误差ERROR作为控制输入量输入到PID控制器，然后决策输出前轮转向偏角，达到在路径跟踪过程中既消除航向偏差又消除位置误差的目的；ERROR定义为：

ERROR＝α×XTE+β×θ_e                                    (4)

其中，α和β分别为横向跟踪误差、纵向航向偏差相对于合成误差的折算因子，α和β的确定主要根据试验效果进行现场整定，其中α和β的取值范围为0≤α≤10，0≤β≤1。

采用增量式PID控制算法来决策前轮转向偏角，具体如下：

$U_i=K[E_i-E_{i-1}+\frac{T_s}{T_i}{E}_i+\frac{T_d}{T_s}(E_i-2E_{i-1}+E_{i-2})]+U_{i-1}---\left(5\right)$

其中，U_i、U_i-1为第i、i-1时刻的转向轮期望偏角，E_i、E_i-1、E_i-2为第i、i-1、i-2时刻的转向轮期望偏角与实际偏角之差。T_s为采样周期，K为比例系数，T_d为微分时间常数，T_i为积分时间常数；K、T_d、T_i三参数根据试验效果进行现场整定，其取值范围为：0≤K≤10，0≤T_d≤1，0≤T_i≤0.5。

本发明将基于合成误差的PID决策算法用于农业机械导航，决策前轮转向偏角，对提高导航系统性能具有一定作用。PID控制器的比例调节系数K起偏角决策作用，而其中的微分环节可有效抑制导航控制系统中的振荡和不稳定，积分环节对于消除导航控制系统中的系统偏差将起到重要作用。PID决策算法不仅继承了传统线性决策模型中零稳态跟踪误差的重要特性，而且对于提高系统稳定性也具有重要作用。

其中，使用模糊逻辑Fuzzy方法来决策转向轮偏角的思路是将驾驶员的驾驶经验转化为模糊规则，建立模糊逻辑控制器，实现前轮转向偏角控制量决策。驾驶经验转化为模糊规则的步骤是：将农业机械与预定义路线之间的位置关系分为九种状态，如图3所示；根据驾驶员控制作业机械沿预定路线行驶的经验，建立了相应的转向控制策略，如表1所示；控制强度指转向轮偏转幅度，与期望转向轮偏角成正比；控制方向为前轮偏转方向，向左时期望偏角为正，向右时期望偏角为负；据此建立模糊控制规则表，如表2所示。

在Matlab环境下设计该模糊逻辑控制器的具体步骤如下：

(1)首先将输入变量模糊化：在Matlab环境下，建立模糊变量横向跟踪误差XTE，方位偏差θ_e的隶属函数曲线，如图4(a)、(b)所示；建立模糊变量转向控制量δ_p的隶属函数曲线，如图5所示；对应于转向模糊控制规则表，利用Matlab软件绘制出模糊控制表三维外观图，如图6所示。

其中，XTE、θ_e和δ_p作为模糊语言变量时，其模糊子集分别选为：F_xte＝{NB，NM，NS，NO，PO，PS，PM，PB}

$F_{{\mathrm{\θ}}_e}=\{\mathrm{NB},\mathrm{NM},\mathrm{NS},\mathrm{NO},\mathrm{PO},\mathrm{PS},\mathrm{PM},\mathrm{PB}\}$

$F_{{\mathrm{\δ}}_p}=\{\mathrm{NB},\mathrm{NM},\mathrm{NS},O,\mathrm{PS},\mathrm{PM},\mathrm{PB}\}$

语言变量XTE和θ_e组成模糊输入空间，δ_p为模糊输出空间，输入和输出空间的模糊分割分别为：F_xte分为8级，即负大、负中、负小、负零、正零、正小、正中和正大；F_θe分为8级，即负大、负中、负小、负零、正零、正小、正中和正大；F_δp分为7级，即负大、负中、负小、零、正小、正中和正大。

选取横向跟踪误差XTE、方位偏差θ_e和转向控制量δ_p的离散论域分别为：

XTE＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；

θ_e＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；

δ_p＝{-7，-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6，7}；

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

XTE′＝int(K_xteXTE)                                (6)

θ_e′＝int(K_θeθ_e)                                (7)

上两式中，输入量化因子K_xte、K_θe的确定方法是：根据试验测试结果，确定横向跟踪误差XTE、方位偏差θ_e的误差范围，由离散论域最大值除于相应误差范围最大值即可获得量化因子值。

(2)然后模糊控制表查询：打开规则观测器，输入不同的离散量XTE′，θ_e′，得到对应的清晰控制量δ_p，进而构成模糊控制表，如表3所示。

(3)最后输出去模糊控制量：从模糊控制量到实际输出控制量的转换算式如下：

${\mathrm{\δ}}_p^{\′}=K_{{\mathrm{\δ}}_p}{\mathrm{\δ}}_p---\left(8\right)$

上式中，输出比例因子K_δp的确定方法同输入量化因子。

由于PID控制方法有利于提高路径跟踪的稳态精度，但不能提高路径跟踪的稳定性，抗干扰能力较差；模糊逻辑控制方法有利于提高路径跟踪的稳态性，但不能显著提高路径跟踪的精度。PID控制方法和模糊逻辑控制方法在路径跟踪控制过程中存在互补性。因此，本发明采用PID和FUZZY的转向轮偏角复合决策算法来进行实际控制。实际应用中，可根据试验效果对两个偏差区间进行合理调节，以确定PID和FUZZY控制的相应作用范围。

本发明与现有技术相比具有如下优点和效果：

(1)本发明充分利用传统导航控制方法的优点，结合模糊控制方法，兼顾稳态跟踪精度和控制稳定性，提高了农业机械作业条件下的导航跟踪精度、稳定性和安全性。

(2)本发明可以控制农业机械沿着预定义路线或者地物标志线自动行走，路径跟踪误差小于30cm。

(3)本发明适用于智能农业机械装备的辅助或者自动导航、变量处方作业、农情信息采集的场合；不仅适用于旱地农业机械的作业需要，还适用于水田农业机械的作业需要，具有广阔的推广应用前景。

附图说明

图1为农业机械与预定义路线的位置关系解析原理图

图2为XTE正负号的确定方法

图3为农业机械与预定义路线之间的九种位置关系状态图

图4为模糊输入变量的隶属函数曲线

图5为模糊输出变量的隶属函数曲线

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

图7为转向轮偏角的FUZZY-PID决策算法原理图

图8为导航控制系统的原理结构框图

图9为转向控制系统框图

图10为转向传动机构的安装示意图

图11为基于DGPS的路径跟踪测试结果

图12为在有初始偏差条件下基于DGPS的路径跟踪测试结果

具体实施方式

下面结合实施例对本发明做进一步详细的描述，但本发明的实施方式不限于此。

本实施例采用的农业机械为久保田插秧机，在其上装载了导航传感器组合、转向控制系统和便携式计算机，便携计算机通过USB-RS232数据传输线与导航传感器组合、转向控制系统相连接，构成导航控制系统，原理结构如图8。

导航传感器组合包括伪距差分GPS、电子罗盘和微机械陀螺。伪距差分GPS可以采集插秧机在WGS-84坐标系下的定位坐标，作为初步定位信息；电子罗盘测量其航向角度信息(以正北方向为0，逆时针方向为正)；微机械陀螺测量其横向角速率(逆时针方向为正)。

转向控制系统包括转向操纵控制器、转向驱动机构和测试传感器三部分，图9是转向控制系统组成图。转向操纵控制器是车载计算机的下位机，主要完成转向控制过程中的转向轮偏角检测，根据便携计算机的转向信息指令，控制驱动机构正确的执行动作；转向驱动机构则完成驱动力传递，带动转向轮偏转，及时准确地实现转向动作。图10是转向驱动机构的安装示意图。

便携计算机实现传感器数据采集、数据预处理、导航控制算法以及控制指令输出等功能。动态数据采集频率是1Hz。数据采集、数据预处理和导航控制算法是采用Visual C++编程工具在Windows XP操作系统下开发完成的。

具体实施步骤如下：

1、预定路线设定

由熟练驾驶员驾驶插秧机沿预定作业路线匀速行走，由导航控制系统采集DGPS导航传感器数据并保存，构成预定义路线P[N]。其数学表达式为：

{(x₀，y₀)，(x₁，y₁)，...，(x_k-1，y_k-1)，(x_k，y_k)，(x_k+1，y_k+1)，...，(x_n，y_n)}

对于直线路径跟踪，可以在预定直线上选定两个坐标点P[1]、P[2](通过DGPS静态定位获得)，建立预定义路线的解析几何描述。

2、启动导航控制算法，具体运行流程是：

(1)插秧机位姿和运动状态参数采集

涉及数据有：当前定位点P_c坐标、当前航向角度θ(以高斯投影平面坐标系xoy中x轴正向为0度，逆时针方向为正)、当前速度v。

(2)动态目标点搜索

基于农业机械与预定跟踪路线的位置解析图1，本发明采用前述前视偏差变化率R_df确定当前前视距离L_f。R_df的解算采用公式(2)，前视距离L_f的解算采用公式(1)；并确立预瞄点P′_p。

(3)导航状态参数计算

所述的两个导航状态横向跟踪误差和纵向航向偏差的定义及计算过程如下：

1)目标方向θ_p计算：

在图1中，即为矢量P_cP′_p的方向，以高斯投影平面坐标系xoy中x轴正向为0度，逆时针方向为正。

2)纵向航向偏差θ_e：

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

3)横向跟踪误差XTE：

在图1中，设P_c到预定义路线的投影是P_c′，则XTE＝|P_cP′_c|，也即

$\mathrm{XTE}=\sqrt{{(x_c-{x^{\′}}_c)}^2+{(y_c-{y^{\′}}_c)}^2}$

判断XTE正负号的方法同前所述。

(4)转向轮偏角的PID决策算法

1)横向跟踪误差和纵向航向偏差合成

合成误差可用式(4)计算，α、β的经验值分别取10，1。

2)PID决策转向轮偏角

本发明使用增量式PID控制算法的算式决策转向轮偏角，如式(5)所示。根据试验测试结果，现场整定K、T_d、T_i三参数分别为：0.8，0.5，0.01。

(5)转向轮偏角的模糊逻辑决策算法

选取横向跟踪误差XTE与方位偏差θ_e和转向轮偏角控制量δ_p的离散论域分别为：

XTE＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；

θ_e＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；

δ_p＝{-7，-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6，7}；

1)输入变量模糊化

基本输入连续论域离散化采用均匀量化的方法，即将连续论域中的连续值经量化因子比例变换后四舍五入变为离散论域的整数值。具体算式如(6)、(7)。设定K_xte横向跟踪误差XTE与方位偏差θ_e的经验最大值分别为1m，90。，则K_xte＝0.6，K_θe＝0.06。

2)模糊控制表查询

输入不同的离散输入量XTE′，θ_e′，依据模糊控制表，得到对应的清晰控制量δ_p。

3)输出控制量去模糊

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

${\mathrm{\δ}}_p^{\′}=K_{{\mathrm{\δ}}_p}{\mathrm{\δ}}_p$

设定转向轮偏角控制量δ_p的经验最大值为45°，则 $K_{{\mathrm{\δ}}_p}=6.43.$

(6)基于PID和FUZZY的转向轮偏角复合决策

设定位置偏差区间[-0.3，0.3]、航向偏差区间[-30，30]为PID控制方法的作用范围(即a＝0.3m，b＝30°)，其他区间为模糊逻辑方法的作用范围，实现两种决策算法的复合控制，可用下式描述：

(7)输出控制参数

将决策的转向轮偏角按一定的串行通信格式打包后，发送给转向控制系统，实现转向轮偏转控制。串行通信格式如下：

$CON，a，b，c^*DD<CR><LF>

上述格式中，$CON表示转向轮偏角控制量，是标识字符串，也是字符串起始标志；a，b，c表示一个ACSII字符，实际代表一个十进制数据，分别表示转向轮偏角的百位，十位和个位。$与^*之间所有字符代码的校验和由^*后面的两位字符DD表示，校验和D为半Byte校验，^*后第1个D表示高4位校验和，第2个D表示低4位校验和，得到校验值后，再转换成ASCII字符得到；<CR><LF>是字符串结束标志。

导航控制算法的流程按照1Hz的频率运行，实现插秧机的路径跟踪控制。

直线路径跟踪的试验结果如图11、12：采用DGPS导航插秧机进行路径跟踪，以0.75m/s速度直线行走，最大横向跟踪误差0.136m，平均偏差0.044m。有初始航向偏差情况下，路径跟踪误差和航向偏差可以快速修正，路径跟踪过程平稳，稳态跟踪精度高，最大横向跟踪误差0.2m，平均偏差0.06m。

表1、驾驶员控制车辆的策略表

策略序号	方向偏差	位置偏差	控制强度	控制方向	位置关系
						1	+	-	弱	向左	(a)
2	+	0	中	向左	(b)
						3	+	+	强	向左	(c)
4	-	+	弱	向右	(d)
						5	-	0	中	向右	(e)
6	-	-	强	向右	(f)
						7	0	-	中	向右	(g)
8	0	0	无	无	(h)
						9	0	+	中	向左	(i)

表2、模糊控制规则表

θe	XTE
									NB	NM	NS	NO	PO	PS	PM	PB
	B	NB	NB	NB	NM	NM	NS	NS									O
NM									NB	NB	NM	NM	NS	NS	O	O
	NS	NB	NM	NS	NS	O	O	PS									PS
NO									NB	NM	NS	O	O	PS	PS	PM
	PO	NM	NS	NS	O	O	PS	PM									PB
PS									NS	NS	O	O	PS	PS	PM	PB
	PM	O	O	PS	PS	PM	PM	PB									PB
PB									O	PS	PS	PM	PM	PB	PB	PB

表3、路径跟踪的模糊控制表

θe ′	XTE′
														-6	-5	-4	-3	-2	-1	0	1	2	3	4	5	6
	-6	-6.25	-6.11	-5.92	-5.9	-5.8	-4.65	-4.12	-4.33	-2.52	-2.11	-2.12	-1.34														-0.18
-5														-6.11	-6.06	-5.45	-4.82	-4.82	-4.62	-3.42	-3.24	-2.51	-1.58	-0.94	-0.98	-0.2
	-4	-5.92	-5.45	-4.88	-4.26	4.17	-4.21	-2.6	-1.96	-1.87	-1.15	0	0.43														0.34
-3														-5.89	-4.82	4.26	-2.97	-2.91	-2.88	-2.22	-0.58	-0.57	0	1.19	1.62	1.75
	-2	-5.85	-4.82	-4.17	-2.91	-2.08	-2.08	-1.18	0.24	0.24	0.64	1.89	2.53														2.53
-1														-5.92	-4.82	-4.21	-2.88	-2.08	-0.31	-0.67	0.31	1.99	2.01	2.08	3.28	4.33
	0	-4.28	-3.59	-3.43	-3.38	-1.81	-0.68	0	0.68	1.83	3.43	3.43	3.59														4.36
1														-4.33	-3.24	-2.08	-2.01	-1.99	-0.31	0.67	0.31	2.08	2.95	4.22	4.83	5.97
	2	-2.53	-2.52	-1.89	-0.58	-0.24	-0.24	1.18	2.08	2.08	2.98	4.17	4.83														5.9
3														-1.75	-1.65	-1.19	0	0.58	0.58	2.22	2.88	2.91	3.03	4.26	4.83	5.94
	4	-0.34	-0.41	0	1.19	1.88	1.96	2.6	4.21	4.17	4.28	4.88	5.41														5.97
5														0.24	0.94	0.94	1.65	2.52	3.24	3.42	4.62	4.82	4.82	5.45	6.03	6.11
	6	0.22	1.38	2.12	2.11	2.53	4.33	4.12	4.65	5.8	5.9	5.93	6.09														6.25

Claims (7)

1、一种农业机械的导航控制方法，其特征在于包括下述步骤：

(1)通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据；

(2)采用基于预瞄控制的动态目标点搜索算法，将所述位姿和运动状态数据与预定义路线数据进行对比分析，推算出两个导航状态参数即横向跟踪误差和纵向航向偏差；

(3)再通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角；

(4)最后根据所述操纵控制量对农业机械进行实时控制，实现路径跟踪。

2、根据权利要求1所述的农业机械的导航控制方法，其特征在于：所述农业机械当前时刻的位姿和运动状态数据包括当前定位点P_c、当前航向角度θ和当前速度V。

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

(1)前视距离L_f通过下述公式计算得到：

L_f＝L₀+K_lfvV+K_lfrR_df

其中，L₀为前视距离基值；K_lfv，K_lfr分别为前进速度、前视偏差变化率的比例系数，可在仿真或实验过程中通过整定获得，K_lfv与K_lfr均取正值；R_df为前视偏差变化率，定义为：

$R_{\mathrm{df}}=\frac{\left|\stackrel{\&RightArrow;}{P_k+2m{P^{\&prime;}}_k+2m}\right|-|\stackrel{\&RightArrow;}{P_k{P^{\&prime;}}_k|}}{|\stackrel{\&RightArrow;}{P_k+m{P^{\&prime;}}_k+m|}-|\stackrel{\&RightArrow;}{P_k{P^{\&prime;}}_k}|}---\left(2\right)$

其中 $m=\mathrm{int}\left(\frac{{\mathrm{VT}}_s}{L_s}\right),$ P_k表示预定义路线上的第k坐标点，P′_k映射点为在直线P_kP_k-1上以P_k为垂足引垂线和农业机械当前纵向方向线的相交点，T_s为导航的采样和控制周期，Ls为预定义路线上点与点之间的平均间距；

(2)预瞄点P′_p的确定：在农业机械当前纵向方向上，由当前定位点P_c点起截取前视距离L_f得到P_p点；再由P_p点向预定义路线投影，得到预瞄点P′_p。

4、根据权利要求1所述的农业机械的导航控制方法，其特征在于：所述推算两个导航状态参数即横向跟踪误差和纵向航向偏差的过程如下：

(1)纵向航向偏差θ_e的确定：纵向航向偏差θ_e为目标方向θ_p与当前航向角度θ之间的差值；

(2)横向跟踪误差XTE的确定：

$\mathrm{XTE}=\sqrt{{(x_c-{x^{\&prime;}}_c)}^2+{(y_c-{y^{\&prime;}}_c)}^2}$

其中，P_c(x_c，y_c)为当前定位点；P′_c(x′_c，y′_c)为P_c到预定义路线的投影；并且若农业机械当前坐标点位于预定义路线的右侧，XTE为正；若当前坐标点位于预定义路线的左侧，XTE为负。

5、根据权利要求1所述的农业机械的导航控制方法，其特征在于：所述通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角是指，当位置偏差在区间[-a，a]且航向偏差在区间[-b，b]时，为PID控制方法的作用范围；其他区间为模糊逻辑方法Fuzzy的作用范围，实现两种决策算法的复合控制；其中，a和b的取值范围为0≤a≤0.5，0≤b≤45。

6、根据权利要求5所述的农业机械的导航控制方法，其特征在于：所述PID控制方法是指，将位置偏差和航向偏差转换为合成误差ERROR，并将合成误差ERROR作为控制输入量输入到PID控制器，然后决策输出前轮转向偏角；

(1)ERROR定义为：

ERROR＝α×XTE+β×θ_e

其中，α和β分别为横向跟踪误差、纵向航向偏差相对于合成误差的折算因子，α和β的确定主要根据试验效果进行现场整定；

(2)采用增量式PID控制算法来决策转向轮偏角：

$U_i=K[E_i-E_{i-1}+\frac{T_s}{T_i}{E}_i+\frac{T_d}{T_s}(E_i-2E_{i-1}+E_{i-2})]+U_{i-1}$

其中，U_i、U_i-1为第i、i-1时刻的转向轮期望偏角，E_i、E_i-1、E_i-2为第i、i-1、i-2时刻的转向轮期望偏角与实际偏角之差。T_s为采样周期，K为比例系数，T_d为微分时间常数，T_i为积分时间常数，K、T_d、T_i三参数根据试验效果进行现场整定。

7、根据权利要求5所述的农业机械的导航控制方法，其特征在于：所述模糊逻辑方法Fuzzy是指：

(1)首先将输入变量横向跟踪误差XTE与方位偏差θ_e模糊化，即采用基本连续论域离散化采用均匀量化的方法，将连续论域中的连续值经量化因子比例变换后四舍五入变为离散论域的整数值，算法为：

XTE′＝int(K_xteXTE)

θ_e′＝int(K_θeθ_e)

其中，输入量化因子K_xte、K_θe的确定方法是：根据试验测试结果，确定横向跟踪误差XTE、方位偏差θ_e的误差范围，由离散论域最大值除于相应误差范围最大值即可获得输入量化因子值；

(2)然后进行模糊控制表查询，即输入不同的离散输入量XTE′，θ_e′，依据模糊控制表，得到对应的清晰控制量即转向轮偏角δ_p；

(3)最后将输出控制量去模糊，转换公式为： ${{\mathrm{\&delta;}}_p}^{\&prime;}=K_{{\mathrm{\&delta;}}_p}{\mathrm{\&delta;}}_p,$ 其中输出比例因子K_δp的确定方法与上述输入量化因子的确定方法相同。

Images