CN101221447A - 一种机械自动转向控制方法 - Google Patents

Abstract

本发明涉及一种机械自动转向控制方法，该方法包括步骤：确定其位置偏差和航向偏差；根据农业机械实际前轮转角的变化，通过参数自整定PID控制算法在线整定PID参数，推算出下一时刻期望前轮转角，从而实现对农业机械的自动转向控制。本发明在常规PID导航控制方法的基础上，充分利用模糊控制方法，以农业机械实际前轮转角的变化为依据，满足其在不同误差、误差变化率下对PID控制参数的不同要求，在线整定PID参数。既具有模糊控制灵活性和适应性优点，又具有PID控制精度较高的特点，能够提高农业机械自动转向控制的稳定性和精确度以及控制系统的鲁棒性。

Description

一种机械自动转向控制方法

技术领域

本发明涉及农业机械导航控制领域，具体涉及一种机械自动转向的控制方法。

背景技术

农业机械导航控制的主要目的是根据各传感器得到的相对精确的导航定位结果，确定农业机械本身与预定义路径的位置关系，结合农业机械的运动状态及相应控制算法，决策出合适的前轮转角，以修正路径跟踪误差，使农业机械的当前航向与目标航向快速重合。导航控制分为纵向控制和横向控制，其中横向控制主要指转向控制，纵向控制主要指速度的调节。农业机械自动转向控制方法，是实现其精确的航向跟踪和自动导航控制的基础。

常用的导航控制方法包括线性模型控制方法、最优控制方法和模糊控制方法。美国伊利诺伊大学对农用机械的自动控制和多传感器的信息融合做了深入广泛的研究，成功开发了能实现多种耕作作业的拖拉机，并利用电液控制系统执行转向动作，实现了农用拖拉机沿行间隙行走的无人驾驶。日本东京大学利用机器视觉技术进行自动导航系统研究，根据线性转向控制模型，将目标方向角度和车辆纵向角度融合，计算出转向轮偏转角度，完成转弯控制。美国的Noguchi N等人应用神经网络和遗传算法，建立了具有自学习能力的农用车辆控制系统，实验结果表明，该模型对在平坦路面行驶的车辆具有很好的控制效果，但不适合于路面倾斜的情况。Benson、Dong ZL等人应用PID(比例-积分-微分proportion-integral-derivative)控制方法设计了PID控制器，该算法不依赖于精确的数学模型，避免了繁琐的建模过程，只需要一些对象的响应特征来组合控制，对算法的比例参数、积分参数和微分参数进行合理的调节。实验结果表明该方法具有良好的路径跟踪效果。国内的周俊与刘成良等人基于卡尔曼滤波的思想，融合了各传感器的观测值，给出了农业机器人导航的预测跟踪控制方法，避免了以视觉系统为主的计算耗时而导致状态反馈滞后而产生的不利影响，对导航控制的鲁棒性和精度有一定改善。华南农业大学集成GPS技术、计算机技术和多传感器技术，开发了以电动机为动力的农用智能移动作业平台，经实验证明，该平台路径跟踪的控制难度较大。

通过分析可知，导航控制的重点和难点是提高转向控制的稳定性和路径跟踪精度。常规PID控制方法可以获得较高精度的路径跟踪效果，且具有一定的鲁棒性和可靠性，但该方法的抗干扰能力较弱。

模糊控制方法是近年来发展起来的新型控制方法，其优点是不需要掌握受控对象的精确数学模型，根据人工控制规则，组织控制决策决定控制量的大小，可以获得良好的动态特性，但其静态特性比较差。在田间作业条件下，建立农业机械的运动学和动力学模型比较困难，随着作业环境和作业条件的变化，其运动特性随时间变化，各种干扰因素对常规控制方法的影响也比较大。

发明内容

本发明的目的在于改进和完善现有的农业机械自动转向控制技术中存在的不足，提供一种导航跟踪精度相对较高、稳定性较好的机械自动转向控制方法。

为实现上述目的，本发明采用如下技术方案：

一种机械自动转向控制方法，该机械的存储装置中存储有PID控制器的比例常数、微分常数和积分常数，该方法包括以下步骤：

(1)根据机械转向要到达的目标点确定其位置偏差和航向偏差；

(2)根据所述位置偏差和航向偏差确定当前期望前轮转角，并控制转向机构根据所述当前期望前轮转角来控制前轮转向，同时通过角度传感器采集当前实际前轮转角；

(3)计算当前期望前轮转角与当前实际前轮转角的误差及误差的变化率，PID控制器根据所述误差及误差的变化率确定PID比例常数的增量、微分常数的增量和积分常数的增量，得到调整后的比例常数、微分常数和积分常数；

(4)利用调整后的比例常数、微分常数和积分常数求出当前期望前轮转角的增量，得到调整后的期望前轮转角，并控制转向机构根据所述调整后的期望前轮转角来控制前轮转向。

其中，所述PID控制器与转向机构之间连接有步进电机，所述PID确定当前期望前轮转角后向所述步进电机发送控制指令，所述步进电机根据所述控制指令控制转向机构使其根据所述当前期望前轮转角来控制前轮转向。

其中，在步骤(1)中，根据所述机械当前时刻的行驶速度确定所述机械的最佳前视距离，依据所述最佳前视距离确定目标航向角，由所述目标航向角和最佳前视距离确定机械的目标点坐标。

其中，根据所述机械当前时刻的行驶速度确定所述机械的最佳前视距离的方法为：

$L=\left\{\begin{array}{cc}2& v\≤1.5m/s\\ v+0.75& v>1.5m/s\end{array}\right.$

其中，L为机械当前时刻最佳前视距离，v为行进速度。

其中，由所述目标航向角和最佳前视距离确定机械的目标点坐标：

x_pre＝x+L·cos(θ_p)

y_pre＝y+L·sin(θ_p)

其中，(x_pre，y_pre)表示目标点坐标，(x，y)表示机械当前位置点的坐标，L表示最佳前视距离，θ_p表示机械的目标航向角。

其中，所述当前实际前轮转角与当前期望前轮转角之间的误差变化率EC由当前实际前轮转角与当前期望前轮转角之间的误差E的微分求得。

其中，在步骤(4)中，利用调整后的积分常数、微分常数和积分常数求出前轮转角的增量的方法为：

Δu_i＝K_P(e_i-e_i-1)+K_Ie_i+K_D(e_i-2e_i-1+e_i-2)]

其中，e_i，e_i-1，e_i-2分别为当前时刻i，第一时刻i-1，第二时刻i-2期望前轮转角与当前实际前轮转角的偏差，其中，第一时刻i-1为当前时刻i的前一时刻，第二时刻i-2为第一时刻i-1的前一时刻，K_P为比例常数，K_I＝K_P*T/T_I，K_D＝K_P*T_D/T，T为采样周期，T_I为积分常数，T_D为微分常数，其中所述采样周期为1秒。

其中，所存储的所述比例常数K_P的取值范围为，0≤K_P≤1000，所述积分常数T_I的取值范围为0≤T_I≤0.5，所述微分常数T_D的取值范围为0≤T_D≤10。

其中，所存储的所述比例常数为K_P为75、积分常数为T_I为0.01、微分常数T_D为8。

本发明的基于参数自整定PID控制器的机械自动转向控制方法，既具有模糊控制灵活性和适应性的优点，又具有PID控制精度较高的特点。与当前的农用机械自动转向控制方法相比，具有以下优点：

(1)在现有导航控制方法的基础上，充分利用模糊控制方法，提高了农业机械自动转向控制的稳定性和精确度。

(2)依据农业机械前轮转角的变化，满足其在不同误差、误差变化率状态下对PID控制参数的不同要求，利用基于参数自整定PID控制器的转向控制方法，实现农业机械的自动转向控制。该方法优于常规PID控制方法，可提高控制系统的鲁棒性。

附图说明

图1为本发明中机械与预定义路径的位置关系解析图；

图2为本发明机械自动转向控制方法的系统框图；

图3为本发明机械自动转向控制方法的原理图；

图4为本发明机械自动转向控制方法的模糊推理部分原理图；

图5为本发明机械自动转向控制方法的工作流程图；

图6为本发明中机械自动转向控制部分的组成示意图；

具体实施方式

以下实施例用于说明本发明，但不用来限制本发明的范围。

本实施例中的农业机械根据现有技术具有DGPS定位技术，目前GPS系统提供的定位精度是优于10米，而为得到更高的定位精度，通常采用差分GPS技术即DGPS定位技术：将一台GPS接收机安置在基准站上进行观测。根据基准站已知精密坐标，计算出基准站到卫星的距离改正数，并由基准站实时将这一数据发送出去。用户接收机在进行GPS观测的同时，也接收到基准站发出的改正数，并对其定位结果进行改正，从而提高定位精度，利用DGPS定位技术采集农业机械的位置坐标，为航位推算或定位算法提供原始数据。

本实施例中农业机械根据现有技术还包括电子罗盘、角度传感器和加速度计等。其中电子罗盘测量农业机械的航向角度；角度传感器获得农业机械的前轮转角；加速度计测量农业机械的加速度，从而得到农业机械当前时刻的行进速度。

如图6所示，本实施例中农业机械的自动转向控制部分主要涉及下位机和上位机两部分，下位机角度传感器获得农业机械的前轮转角，上位机采用ARM芯片，与加速度计连接实现传感器数据的采集与处理、控制算法的实现及控制指令的输出等，ARM芯片与电机驱动器连接，该电机驱动器中的控制驱动机构根据上位机的转向控制指令，及时准确地控制转向步进电机执行动作，上位机还可以通过RS232串口连接笔迹本等信息处理终端，与其相互传输数据。

本实施例中的对农业机械的自动转向控制方法具体如下：

1.预定义路径设定

本实施例中的农业机械的预定义路径采用数据集{P₀，P₁，...，P_k-1，P_k，，P_k+1，...，P_n}表示，其中的P加注不同的下标表示预定义路径上不同的位置点，在分析时用不同的坐标表示，其中每一个位置点的坐标为高斯投影平面坐标，分别表示为{(x₀，y₀)，(x₁，y₁)，...，(x_k-1，y_k-1)，(x_k，y_k)，(x_k+1，y_k+1)，...，(x_n，y_n)}，其中x表示横坐标，y表示纵坐标。

利用DGPS定位技术采集预定义路径各个位置点坐标，即{(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_A和终点P_B，利用DGPS定位技术获取定位数据，通过坐标转换得到该两点的高斯平面坐标，分别表示为(x_A，y_A)和(x_B，y_B)。

预先采用DGPS定位技术采集了预定义路径上各个点的坐标，在农业机械具体行驶过程中，对其自动转向控制的方法如下所述。

2.自动转向控制方法

(1)获取农业机械当前位置点P_c、航向角θ_c及行进速度v

如图1所示为本发明中机械与预定义路径的位置关系解析图，农业机械在行驶过程中，当前时刻农业机械的位置点P_c、航向角θ_c和行进速度v的数据获取方法如下：

①农业机械当前位置点P_c：本实施例中通过DGPS定位技术首先获得的农业机械当前时刻的位置点坐标，再经过航位推算或定位校正得到农业机械当前位置点P_c；

②农业机械当前航向角度θ_c：本实施例采用电子罗盘获得，并转换为高斯平面下的角度值；

③农业机械当前行驶速度v：首先由加速度计获得农业机械当前的加速度值，再经过积分计算得到当前的行进速度。

(2)根据预定义路径确定农业机械的目标点P′_p

本实施例中采用采用动态路径搜索算法，推算出农业机械最佳前视距离L及目标点坐标，进而确定目标点P′_p。

所述的动态路径搜索算法是基于预瞄跟随理论，通过动态计算农业机械当前时刻的最佳前视距离L，确定其在预定义路径上的目标点坐标，具体步骤如下：

(21)确定农业机械的最佳前视距离L：本发明主要考虑农业机械行进速度对其前视距离的影响，通过实验确定出行进速度和前视距离的关系，表示为：

$L=\left\{\begin{array}{cc}2& v\≤1.5m/s\\ v+0.75& v>1.5m/s\end{array}\right.---\left(1\right)$

式中L为农业机械当前时刻最佳前视距离，v为行进速度；

(22)确定农业机械的目标点P′_p的坐标：根据决策出的最佳前视距离L，农业机械的目标点P′_p的坐标计算公式可表示为：

x_pre＝x+L·cos(θ_p)  (2)

y_pre＝y+L·sin(θ_p)  (3)

式中，(x_pre，y_pre)表示目标点P′_p的坐标，(x，y)表示农业机械当前位置点P_c的坐标，L表示最佳前视距离，θ_p表示农业机械目标航向。

如图1所示，其中：

农业机械目标航向θ_p的确定方法为： ${\mathrm{\θ}}_p=\stackrel{\→}{P_c{P^{\′}}_p},$ 即θ_p为农业机械当前位置点P_c指向目标点P′_p的矢量；

(3)确定农业机械的当前位置偏差D_e和航向偏差θ_e

参见图1，其中农业机械的当前位置偏差D_e和航向偏差θ_e的定义如下：

农业机械当前的航向偏差θ_e：定义为农业机械目标航向θ_p与当前航向角θ_c的差值，表示为θ_e＝θ_p-θ_c；

农业机械位置偏差D_e：定义为农业机械当前位置点P_c及其在预定义路径上的投影点P′_c间的距离，表示为 $D_e=\left|\stackrel{\→}{P_c{P}_c^{\′}}\right|,$ 其中当前位置点P_c在预定义路径上的投影点P′_c通过求互相垂直的两直线的交点获得；

本实施例中规定当农业机械沿预定义路径方向行进时，若农业机械当前位置点P_c位于预定义路径的右侧，则位置偏差D_e为正；若其当前位置点P_c位于预定义路径的左侧，则位置偏差D_e为负。

因此位置偏差D_e和航向偏差θ_e方向的判断方法如下：

位置偏差D_e方向判定方法：若航向偏差θ_e为正，则位置偏差D_e为正，反之则为负。

航向偏差θ_e方向判定方法：若航向偏差θ_e为正，则说明农业机械在预定义路径的右边，反之则在左边。

(4)PID控制器首先根据农业机械的位置偏差和航向偏差确定农业机械的初始期望前轮转角，并根据该初始期望前轮转角向步进电机发送指令，步进电机根据该指令控制转向机构使其控制前轮按初始期望前轮转角转向，同时，通过角度传感器采集实际的前轮转角，根据农业机械实际前轮转角的变化，通过参数自整定PID控制算法，在线整定PID参数，推算出下一时刻期望前轮转角，并向步进电机发送指令，步进电机根据该指令控制转向机构使其控制前轮按期望前轮转角转向，重复以上过程实现期望前轮转角的不断在线调整，完成自动转向过程。

本实施例中农业机械自动转向控制步骤是基于参数自整定PID控制器的，参数自整定PID控制器在线整定PID参数K_P、T_I、T_D，推算出下一时刻期望前轮转角方法包括为：

(41)初始化前轮转角的PID控制参数K_P、T_I、T_D和采样周期T；

本实施例采用增量式PID控制算法，根据仿真试验结果，设置PID参数K_P、T_I、T_D的初始值，分别为75、0.01和8，选取采样周期T为1秒，增量式PID控制方法只与最近的两次前轮转角偏差有关系，当存在误差或计算精度不足时，对控制量计算的影响程度较小，增量式PID控制方法的目的在于对当前时刻i农业机械的期望前轮转角进行整定，使下一时刻按整定后的期望前轮转角控制转向，具体根据下式获取：

Δu_i＝u_i-u_i-1＝K_P(e_i-e_i-1)+K_Ie_i+K_D(e_i-2e_i-1+e_i-2)](4)

其中，u_i，u_i-1分别为当前时刻i，第一时刻i-1农业机械的期望前轮转角，e_i，e_i-1，e_i-2分别为当前时刻i、第一时刻i-1、第二时刻i-2期望前轮转角与实际前轮转角的偏差，其中，第一时刻i-1为在当前时刻i的前一时刻，第二时刻i-2为在第一时刻i-1的前一时刻，K_P为比例系数，K_I＝K_P*T/T_I，K_D＝K_P*T_D/T，其中，T为采样周期，优选的初始数值范围为0.1～1s，T_I为积分时间常数，T_D为微分时间常数，0≤K_P≤1000，0≤T_I≤0.5。

(42)确定PID控制器的前轮转角控制量的模糊逻辑规则表和模糊逻辑控制表；

本实施例中PID控制器采用自适应模糊PID控制器，实现PID三个参数K_P、T_I、T_D的在线整定，进而决策农业机械的前轮转角，以提高农业机械自动转向控制的精度和稳定性。

(421)确定PID控制器的前轮转角控制量的模糊逻辑规则表；

自适应模糊PID控制器设计的核心是总结驾驶人员的技术知识和实际操作经验，转化为模糊控制规则，建立模糊逻辑决策表，实现PID参数变化量的决策。考虑到误差、误差变化率以及PID控制器三个参数K_P、K_P、K_P的变化量Δk_p、Δk_i、Δk_d变量的正负特性，将每一种变量的取值范围分为7种，分别为：正大(PB)、正中(PM)、正小(PS)、零(ZO)、负小(NS)、负中(NM)和负大(NB)，最终得到模糊误差E、模糊误差变化率EC及PID控制器三个参数的变化量Δk_p、Δk_i、Δk_d的模糊控制表(如表1、表2、表3所示)，表1、表2、表3中对应于模糊误差E的正大PB1、正中PM1、正小PS1、负小NS1、负中NM1和负大NB1共7个等级，对应于模糊误差变化率EC的正大PB2、正中PM2、正小PS2、负小NS2、负中NM2和负大NB2共7个等级，对应于变化量Δk_p的正大PB3、正中PM3、正小PS3、负小NS3、负中NM3和负大NB3共7个等级，对应于变化量Δkp的正大PB4、正中PM4、正小PS4、负小NS4、负中NM4和负大NB4共7个等级，对应于变化量Δk_p的正大PB5、正中PM5、正小PS5、负小NS5、负中NM5和负大NB5共7个等级。

本实施例中不限于将上述中的每个变化分为7个数值范围，也可以进一步细化为更多的数值范围，但符合下面表中所述的模糊规则。

表1ΔK_P的模糊规则表

表2ΔK_I的模糊规则表

表3ΔK_D的模糊规则表

本实施例中参数整定原则为：模糊决策的输出量是PID参数的变化量ΔK_P、ΔK_I、ΔK_D。根据不同的模糊误差E的绝对值|E|和模糊误差变化率EC的绝对值|EC|对K_P、K_I、K_D进行整定，原则如下：

★当模糊误差|E|较大时，如3≤|E|≤5，为使系统具有较好的跟踪性能，加快系统的响应速度，应取较大的K_P，如500≤K_P≤1000；同时为避免系统在初始时，由于误差的瞬时增大可能出现的微分饱和而使控制作用超出允许范围，此时应取较小的K_D，如0≤K_D≤3；同时为避免系统响应出现较大超调，产生积分饱和，应对积分作用加以限制，通常取K_I＝0。

★当模糊误差|E|适中，如1≤|E|≤3，为使系统响应具有较小超调，应取稍小的K_P，如0≤K_P≤100；此时K_D的取值对系统响应的影响较大，要大小适中，如3≤K_D≤8.以保证系统的响应速度；同时可增加一些积分对控制的作用，但若K_I太大，易造成积分饱和，太小则不能加快系统响应速度，所以K_I的取值要适当，如0≤K_I≤0.3。

★当模糊误差|E|较小时，如0≤|E|≤1，为使系统具有较好的稳态性，应取较大的K_P与K_I，如K_P如500≤K_P≤1000，如0.3≤K_I≤0.5；同时为避免系统在设定值附近出现振荡，K_D值的选择非常重要，一般可根据|EC|来确定：当模糊误差变化率|EC|值较小时，如0≤|EC|≤2，K_D可取大些，如5≤K_D≤10；当|EC|值较大时，如2≤|EC|≤5，K_D可取小些，如0≤K_D≤5，通常K_D为中等大小。

(422)确定PID控制器的前轮转角控制量的模糊逻辑控制表；

在实验仿真中，将误差e及误差变化率e’的精确值模糊化成模糊量误差E、模糊量误差变化率EC，即分别以误差e、误差变化率e’的精确值为中心值取一段数值范围，得到模糊量误差E、模糊量误差变化率EC，对模糊量误差E选取模糊子集{NB1，NM1，NS1，ZO1，PS1，PM1，PB1}，对模糊量误差变化率EC选取模糊子集{NB2，NM2，NS2，ZO2，PS2，PM2，PB2}。选取PID控制参数K_P的变化量ΔK_P的模糊子集{NB3，NM3，NS3，ZO3，PS3，PM3，PB3}，选取PID控制参数K_I的变化量ΔK_I的模糊子集{NB4，NM4，NS4，ZO4，PS4，PM4，PB4}，选取PID控制参数K_D的变化量ΔK_D的模糊子集{NB5，NM5，NS5，ZO5，PS5，PM5，PB5}，取模糊量误差E和模糊量误差变化率EC变化范围，将该范围定义为模糊集上的论域，表示为E’＝{-15，-12，-9，-6，-3，0，3，6，9，12，15}，EC’＝{-5，-4，-3，-2，-1，0，1，2，3，4，5}，其中E’中每一个整数都对应于模糊误差E中的一段数值范围，EC’中每一个整数都对应于模糊误差变化率中的一段数值范围。

基于专家知识和现场操作人员的经验制定模糊规则，打开模糊规则器，输入不同的离散量E’、EC’，得到PID参数变化量ΔK_P、ΔK_I、ΔK_D的对应值，构成模糊控制表(如表4、表5、表6所示)。

表4ΔK_P的模糊控制表

表5ΔK_I的模糊控制表

表6ΔK_D的模糊控制表

本实施例中首先初始PID控制器的三个参数K_P、K_I、K_P，K_P为比例系数，K_I＝K_P*T/T_I，K_D＝K_P*T_D/T，其中，T为采样周期，T_I为积分时间常数，T_D为微分时间常数，本实施例根据仿真试验结果，设置PID参数K_P、T_I、T_D的初始值分别为75、0.01和8，选取采样周期T为1秒，确定了PID控制器的前轮转角控制量的模糊逻辑规则表和模糊逻辑控制表后，如图5所示，结合前面所述的内容详述在农业机械行进过程中自动转向控制过程：

a：取当前采样值；

b：根据前面所述的方法确定农业机械当前的位置点P_c和目标点P′_p的坐标，确定当前农业机械的位置偏差D_e和航向偏差θ_e；

c：由当前时刻农业机械的位置偏差D_e和航向偏差θ_e确定农业机械的当前期望前轮转角u_i，并根据该当前期望前轮转角u_i向步进电机发送指令，步进电机根据该指令控制转向机构使其控制前轮按当前期望前轮转角u_i转向，同时，通过角度传感器采集实际的前轮转角，该实际前轮转角与当前期望前轮转角的误差为e_i，对当前期望前轮转角的误差e_i求微分(de/dt)，得到误差变化率e’；

d：对当前期望前轮转角的误差e_i和误差变化率e’作为输入量输入到PID控制器，PID控制器将误差e_i的精确值转换为模糊误差E，将误差变化率e’的精确值转换为模糊误差变化率EC

将糊化后的输入量E、EC做为模糊推理部分的输入，再由E、EC和总的控制规则R，根据推理合成规则进行模糊推理得到模糊控制量U为： $U={(E\×\mathrm{EC})}^{T_1}\·R$

其中，T₁代表一定的控制规则，U为相应的控制量(在实施例中分别代表Δk_p、Δk_i、Δk_d)。

e.逆模糊化：经模糊控制算法计算后得到的控制量，为控制量语言变量的论域中的值，不能直接控制对象，须将其转换为控制量基本论域中的值。控制量的比例因子定义如下：

k_u＝u_max/l

其中，l为控制量在0～u_max范围内量化后分成的档数，采用重心法对输出模糊控制量U进行逆模糊化。

本实施例中当系统某时刻的误差e、误差变化率e′已知，即可通过确定参数变化量的值，在线整定PID控制器当前时刻的参数K_P、K_I、K_D。；

f：对当前的K_P、K_I、K_D进行整定；K_P＝K_P′+ΔK_P、K_I＝K_I′+ΔK_I、K_D＝K_D′+ΔK_D，其中，K′_P、K′_I、K′_D为上一时刻的PID参数；

g：PID控制器根据下式获取控制参数Δu_i：

Δu_i＝K_P(e_i-e_i-1)+K_Ie_i+K_D(e_i-2e_i-1+e_i-2)](4)

得到整定后的期望前轮转角Δu_i+u_i，根据该整定后的期望前轮转角向步进电机发送指令，步进电机根据该指令控制转向机构使其控制前轮按整定后的期望前轮转角u_i转向，完成一步控制；

h：然后等待下一次采样，重复执行步骤b～g，如此循环，当农业机械的位置偏差和航向偏差在设定的误差范围内时，结束该过程，即可完成农业机械的自动转向控制

如图3所示，本实施例中自适应模糊PID控制器以农业机械的当前时刻前轮转角与期望前轮转角之间的误差e和误差e的变化率e’作为输入量，在农业机械行进过程中通过不断检测e和e’，根据模糊控制原理确定的模糊逻辑决策表来对控制器的3个参数K_P、T_I、T_D进行在线修改，使其具有良好的动态和静态性能。

下面以求K_P为例说明推理方法：

(一)根据表1，可将每条K_P调整规律写出，例如，第一条可写为：R₁：if E＝NB and EC＝NB then K_P＝PS，该规则隶属度的计算方法为：

${\mathrm{\μ}}_{K_{p1}}\left(C_P\right)={\mathrm{\μ}}_{\mathrm{NB},E}\left(E\right)^{\mathrm{\μ}}_{\mathrm{NB},\mathrm{EC}}\left(\mathrm{EC}\right)---\left(5\right)$

同理，可求出关于K_P的所有规则的隶属度μ_Kpi(c_p)(i＝1，2，...，n)，其中，n为关于K_P的所有规则的条数，c_Pi为第i条规则中所取模糊集合的中心值，μ_NB，E(E)代表当E取NB时的隶属度，μ_NB，EC(EC)代表当EC取NB时的隶属度。

(二)当系统在某时刻误差E、误差变化率EC已知，K_P的计算公式为：

$K_P=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}({\mathrm{\μ}}_{K_{\mathrm{pi}}}\left(c_p\right)\×c_{\mathrm{pi}})}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\μ}}_{K_{\mathrm{pi}}}\left(c_p\right)}---\left(6\right)$

同理，可以得到K_I、K_D的计算公式，如(7)，(8)所示。其中，m、l分别为关于K_I、K_D的所有规则的条数。

$K_I=\frac{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}({\mathrm{\μ}}_{K_{\mathrm{ii}}}\left(d_i\right)\×d_{\mathrm{ii}})}{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\μ}}_{K_{\mathrm{ii}}}\left(d_i\right)}---\left(7\right)$

$K_D=\frac{\underset{i=1}{\overset{1}{\mathrm{\Σ}}}({\mathrm{\μ}}_{K_{\mathrm{di}}}\left(g_d\right)\×g_{\mathrm{di}})}{\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{\mathrm{\μ}}_{K_{\mathrm{dii}}}\left(g_d\right)}---\left(8\right)$

从式(5)～(8)可以看出，K_P、K_I、K_D与模糊误差E和模糊误差变化率EC之间建立了一种函数关系，可以满足系统在不同模糊误差E、模糊误差变化率EC状态下对PID控制参数的不同要求，所以该控制器优于常规PID控制器。

ΔK_P、ΔK_I、ΔK_D的模糊规则表建立后，即可进行K_P、K_I、K_D的在线整定。设模糊误差E，模糊误差变化率EC和ΔK_P、ΔK_I、ΔK_D均服从正态分布，可得出各模糊子集的隶属度，根据各模糊子集的隶属度赋值表和各参数模糊控制模型，应用模糊合成推理设计PID参数变化量的模糊决策表，查出修正参数代入下式计算：

K_P＝K′_P+ΔK_P，其中，K′_P为上一时刻的PID参数，ΔK_P可由模糊规则表1查得；

K_I＝K′_I+ΔK_I，其中，K′_I为上一时刻的PID参数，ΔK_P可由模糊规则表2查得；

K_D＝K′_D+ΔK_D，其中，K′_D为上一时刻的PID参数，ΔK_P可由模糊规则表3查得。

将系统误差E和误差变化率EC变化范围定义为模糊集上的离散论域，表示为：E’＝{-15，-12，-9，-6，-3，0，3，6，9，12，15}，EC’＝{-5，-4，-3，-2，-1，0，1，2，3，4，5}，打开模糊规则器，输入不同的离散量E、EC，得到对应的ΔK_P、ΔK_I、ΔK_D，构成模糊控制表(如表4、表5、表6所示)。

本发明提供的机械自动转向控制系统框图如图2所示，该系统是基于参数自整定PID控制器的，本实施例中参数自整定PID控制部分主要由以下几部分组成：

①模糊控制器：为模糊控制系统的核心，是采用基于模糊控制知识表示和规则推理的语言型模糊控制器，主要包括输入量的模糊化、模糊推理和逆模糊化三部分。根据被控对象的不同以及对系统静态、动态特性要求的不同，模糊控制器的规则也有所不同，从而控制算法各异；

②执行机构：包括直流电机和步进电机等；

③被控对象：可以是一种设备或装置以及它们的群体，在一定的约束条件下工作；

④传感器：需设置传感器将被控对象或各种过程的被控制量转换为电信号；

⑤D/A转换器实现数/模转换和模/数转换。

虽然本发明是集体结合以上优选实施例示出和说明的，但是熟悉该技术领域的人员可以理解，其中无论在形式上还是在细节上都可以做出各种改变，这并不背离本发明的精神实在和专利保护范围。

Claims (10)

1.一种机械自动转向控制方法，该机械的存储装置中存储有PID控制器的比例常数、微分常数和积分常数，其特征在于，该方法包括以下步骤：

(1)根据机械转向要到达的目标点确定其位置偏差和航向偏差；

(2)PID控制器根据所述位置偏差和航向偏差确定当前期望前轮转角，并控制转向机构根据所述当前期望前轮转角来控制前轮转向，同时通过角度传感器采集当前实际前轮转角；

(3)计算当前期望前轮转角与当前实际前轮转角的误差及误差的变化率，PID控制器根据所述误差及误差的变化率确定PID比例常数的增量、微分常数的增量和积分常数的增量，得到调整后的比例常数、微分常数和积分常数；

(4)PID控制器利用调整后的比例常数、微分常数和积分常数求出当前期望前轮转角的增量，得到调整后的期望前轮转角，并控制转向机构根据所述调整后的期望前轮转角来控制前轮转向。

2.如权利要求1所述的机械自动转向控制方法，其特征在于，所述PID控制器与转向机构之间连接有步进电机，所述PID确定当前期望前轮转角后向所述步进电机发送控制指令，所述步进电机根据所述控制指令控制转向机构使其根据所述当前期望前轮转角来控制前轮转向。

3.如权利要求1所述的机械自动转向控制方法，其特征在于，在步骤(1)中，根据所述机械当前时刻的行驶速度确定所述机械的最佳前视距离，依据所述最佳前视距离确定目标航向角，由所述目标航向角和最佳前视距离确定机械的目标点坐标。

4.如权利要求3所述的机械自动转向控制方法，其特征在于，根据所述机械当前时刻的行驶速度确定所述机械的最佳前视距离的方法为：

$L=\left\{\begin{array}{cc}2& v\≤1.5m/s\\ v+0.75& v>1.5m/s\end{array}\right.$

其中，L为机械当前时刻最佳前视距离，v为行进速度。

5.如权利要求4所述的机械自动转向控制方法，其特征在于，由所述目标航向角和最佳前视距离确定机械的目标点坐标：

x_pre＝x+L·cos(θ_p)

y_pre＝y+L·sin(θ_p)

其中，(x_pre，y_pre)表示目标点坐标，(x，y)表示机械当前位置点的坐标，L表示最佳前视距离，θ_p表示机械的目标航向角。

6.如权利要求1所述的机械自动转向控制方法，其特征在于，所述当前实际前轮转角与当前期望前轮转角之间的误差变化率EC由当前实际前轮转角与当前期望前轮转角之间的误差E的微分求得。

7.如权利要求1所述的机械自动转向控制方法，其特征在于，在步骤(4)中，利用调整后的积分常数、微分常数和积分常数求出前轮转角的增量的方法为：

Δu_i＝K_P(e_i-e_i-1)+K_Ie_i+K_D(e_i-2e_i-1+e_i-2)]

其中，e_i，e_i-1，e_i-2分别为当前时刻i，第一时刻i-1，第二时刻i-2期望前轮转角与当前实际前轮转角的偏差，其中，第一时刻i-1为当前时刻i的前一时刻，第二时刻i-2为第一时刻i-1的前一时刻，K_P为比例常数，K_I＝K_P*T/T_I，K_D＝K_P*T_D/T，T为采样周期，T_I为积分常数，T_D为微分常数。

8.如权利要求7所述的机械自动转向控制方法，其特征在于，所述采样周期T为1秒。

9.如权利要求1所述的机械自动转向控制方法，其特征在于，所存储的所述比例常数K_P的取值范围为，0≤K_P≤1000，所述积分常数T_I的取值范围为0≤T_I≤0.5，所述微分常数T_D的取值范围为0≤T_D≤10。

10.如权利要求1所述的机械自动转向控制方法，其特征在于，所存储的所述比例常数为K_P为75、积分常数为T_I为0.01、微分常数T_D为8。

Images