水稻直播机自动驾驶模糊自适应控制方法

张 雁 李彦明 刘翔鹏 陶建峰 刘成良 李瑞川

(1.上海交通大学机械与动力工程学院， 上海 200240； 2.山东五征集团， 日照 262399)

0 引言

路径跟踪控制方法是农机导航系统中的核心技术，是众多学者研究的重点和热点。

根据模型特点，路径跟踪控制算法可以分为3种[1]：基于运动学模型的路径跟踪控制方法、基于动力学模型的路径跟踪控制方法以及与模型无关的路径跟踪控制方法。不同的方法应用场景不同，同时，3种方法也各有交汇和融合。基于运动学模型的路径跟踪控制方法是在简化的二轮车模型的基础上提出的，通常采用最优控制、鲁棒控制等设计导航控制器[2-5]；基于动力学模型的路径跟踪控制方法是建立在牛顿第二定律的基础上设计导航控制器的[6-7]；与模型无关的路径跟踪控制方法是根据模糊控制[8-10]、神经网络[11-13]或PID控制[14-15]等设计导航控制器。此外，纯追踪模型也受到了很多学者的重点关注，其只有一个控制参数——前视距离，很多学者提出自适应调整该参数的方法来提高导航控制系统的性能[16-19]。

YOSHISADA等[20]在插秧机平台上设计了控制器，最大误差不超过12 cm。伟利国[21]采用滑模自校正控制方法在插秧机上进行了现场实验，直线路径跟踪精度不大于10 cm。李逃昌[4]采用模糊控制调整前视距离的纯追踪模型方法在插秧机平台上进行了实验，结果表明，系统具有一定的快速性，但存在一定程度的超调。

为提高水田环境作业的水稻直播机导航控制系统的稳定性和快速性，提出一种利用模糊自适应调整参数Kd的控制方法。

1 水稻直播机运动学模型

1.1 模型说明

在不考虑车胎与地面的相互作用下，假设车辆不发生侧滑俯仰等情况，把水稻直播机看作是两轮车模型[22]。

水稻直播机追踪期望路径的示意图如图1所示，图中C为设定跟踪的期望路径，M为后轮轴中心，即车体的控制点，P点为车体离目标路径最近点；假设P点是唯一的，当水稻直播机始终靠近P点时，假设是可以满足的；s是沿着C方向在M点处的曲线坐标，C(s)表示期望路径C在这一点的曲率；de为相对于期望路径的横向位置误差。本文规定当水稻直播机在期望路径的右侧时横向位置误差为正(de>0)，当水稻直播机在期望路径左侧时横向偏差为负(de<0)；θe为相对于期望路径的航向偏差；δ为前轮期望转角；L为车轮轴距；V是行进速度。

图1 水稻直播机与期望路径的位置关系Fig.1 Position relationship between rice planting machinery and expected path

1.2 状态空间模型

为了便于计算，现提出如下假设：控制变量为农机车速和前轮期望转角；直播机为刚性体； 1-deC(s)≠0；θe≠π/2。1-deC(s)≠0表明此时后轮轴中心与曲线坐标的曲率中心重合，直播机靠近路径行驶时，这种情况不会发生。

水稻直播机模型的状态方程[22-24]为

(1)

1.3 模型的链式形式

根据两输入系统的链式形式[25]，有

(2)

设A=[a1a2a3]T和M=[m1m2]T分别为状态变量和控制变量。为了更好地说明式(2)是线性的，设

(3)

则式(2)可写为

(4)

则式(4)是一个线性系统。由于控制律的参数独立于水稻直播机速度，驱使线性系统变化的变量a1在水稻直播机行驶的路径上是平均分布的；因此，可以设定

a1=s

(5)

为了简化计算，通过式(5)可以将非线性农机模型(式(1))转换成链式形式(式(2)和式(4))。为了与链式模型(式(2))保持一致，控制变量m1被定义为

(6)

为了便于计算，设

a2=de

(7)

(8)

a3=(1-deC(s))tanθe

(9)

最后，可以得到控制变量m2的表达式为

(10)

1.4 控制律设计

由于式(4)是一个线性系统，则根据线性系统控制理论，可以得到控制律为

m3=-Kda3-Kpa2(Kp、Kd∈R+)

(11)

式中Kp、Kd——系数

将式(11)代入式(4)中得

(12)

式(12)表示a2和a3均收敛于零，即横向偏差和航向偏差都收敛于零。则将式(11)代入式(3)、(6)、(10)，可以得到控制变量

(13)

水稻直播机跟踪直线时有C(s)=0，式(13)便可以简化为

δ(de,θe)=arctan(Lcos3θe(-Kdtanθe-Kpde))

(14)

式中，δ(de,θe)为控制律输出的期望前轮转角。Kp和Kd的比值决定了自主导航控制系统的稳定性和快速性，比值越大快速性越好，稳定性越差；相反，比值越小，稳定性越好，快速性变差；当两者的比例相同时，Kp和Kd的值越大系统的控制越灵敏，但是由于机构限制和控制周期的限制，机构不一定能快速响应，同时还会引起超调。因此，Kp和Kd的值有一个合适的取值范围。

1.5 PD最优模型仿真

由式(14)可以建立基于状态控制的最优控制仿真模型，如图2所示，其中f(u)即为式(14)所示的控制律。Saturation模块表示机构的限制，即前轮转角的值限制为车辆转向机构允许的范围。

图2 最优控制仿真模型Fig.2 Optimal control simulation model

根据图2的仿真模型，设定初始横向偏差为0.5 m、航向初始偏差为0°时，水稻直播机轴距为1.06 m，直播机行驶速度为1 m/s，可得如图3、4所示的横向偏差曲线。

图3 不同比例系数的横向偏差曲线Fig.3 Lateral deviation curves of different coefficient ratios

图4 相同比例系数的横向偏差曲线Fig.4 Lateral deviation curves of the same coefficient ratio

从图3可以看出，当Kp/Kd=1(此时Kp=1.2，Kd=1.2)时，自主导航系统超调较小，并且可以快速稳定地跟踪期望路径。当Kp和Kd比值增大(Kp=1.2，Kd=0.8)时，自主导航控制系统的快速性变好，超调增大；当Kp和Kd比值减小(Kp=1.2，Kd=1.6)时，系统超调减小，快速性变差。当Kp和Kd比值进一步减小(Kp=1.2，Kd=2.0)时，系统基本没有超调；如图4所示，在相同比例系数下，Kp和Kd的取值越大，系统的超调减小，而且快速性也好。因此，在现场实验时应该找一组较好的控制参数，并且能够自适应调整。

2 模糊自适应的PD控制器

根据1.4节和1.5节的结论，当Kp和Kd比值越大快速性越好，稳定性越差；相反，比值越小，稳定性越好，快速性变差。则Kp固定为1.2，然后利用模糊方法的自适应调整控制律参数Kd，这样可以提高自主导航控制系统的稳定性和快速性，为此提出了一种模糊自适应的调整控制律参数的控制方法。

2.1 模糊控制器设计

2.1.1输入输出变量模糊化

以横向偏差de和航向偏差θe为模糊控制器的输入，控制律参数Kd为其输出；首先对输入输出变量进行模糊化。

(1) 横向偏差为de，基本论域为[-0.5 m,0.5 m]，量化等级为{-5,-2.5,0,2.5,5}={NB,NM,Z,PM,PB}，量化因子取10。

(2) 航向偏差为θe，基本论域为[-30°,30°]，量化等级为{-5,-2.5,0,2.5,5}={NB,NM,Z,PM,PB}，量化因子取1/6。

(3) 控制律参数Kd。基本论域为[1,3.5]，量化等级为{1,1.63,2.25,2.83,3.5}={Z,S,M,L,VL}，比例因子取1。

2.1.2模糊规则设计原则

横向偏差和航向偏差越小，参数Kd越大，则Kp/Kd的比值越小，增加系统的稳定性。

横向偏差越大，或者航向偏差越大，参数Kd越小，则Kp/Kd的比值越大，增加系统的响应速度。

2.1.3隶属度函数

横向偏差、航向偏差以及输出变量控制参数Kd的隶属度函数均为高斯函数，横向偏差的隶属度函数如图5所示。

图5 隶属度函数Fig.5 Membership function

根据量化等级和模糊规则可以设计模糊控制规则表，如表1和图6所示。

表1 模糊控制规则Tab.1 Fuzzy control regulation

图6 模糊规则曲面Fig.6 Surface of fuzzy regulation

2.2 模糊自适应控制器仿真

另外，在相同的条件下，进行了模糊自适应调整控制参数Kp和Kd的比值和固定Kp与Kd比值(Kp=1.2，Kd=1.2)的仿真实验，结果如图7所示。

图7 不同方法的横向偏差对比Fig.7 Comparison of lateral deviation of different methods

从仿真结果可以得到，自适应调整控制参数的方法不仅使系统具有较好的响应速度，而且没有超调。

3 实验验证

3.1 实验平台

以上海世达尔公司的洋马VP6型水稻直播机为实验平台，如图8所示，经机电一体化改造后，搭载由北斗导航GNSS定位系统、自动转向系统、导航主控制器、转向角检测模块组成的自主导航控制系统。分别在上海松江农业推广站试验田附近的水泥地面上和水田环境下进行了验证实验。

图8 实验平台Fig.8 Experiment platform

3.2 确定参数实验

参考仿真模型的数据即Kp=1.2，Kd=1.8，首先在水泥地面上对控制参数进行了整定。设定期望跟踪路径为直线AB，并设定水稻直播机所在位置距离直线AB的初始横向偏差为0.50 m，航向偏差为0°；主变速手柄被设定在前进挡位，由变速踏板控制器设定行走速度恒为0.8 m/s，更改Kp与Kd比值，导航控制系统的横向偏差曲线如图9所示。

图9 不同比例系数的横向偏差曲线Fig.9 Transverse deviation curves of different ratios of Kp/Kd

图11 水泥地面路径跟踪的实验结果Fig.11 Track curve and lateral deviation curve of cement experiment

从图9可以看出，在一定范围内，Kp/Kd的比值越大，导航系统的响应时间越短，响应速度越快，上线距离越短；同时，超调也越大；Kp/Kd的比值越小，导航控制系统的响应速度越慢，上线距离越长；但超调小，甚至没有超调(Kp/Kd=1/3)时，系统的稳定性较好，精度较高。这与2.2节仿真的结论是一致的。

进一步处理上面的数据，可以得到导航控制系统稳定后(误差见图10)不同Kp/Kd比值的跟踪效果如表2所示。

图10 系统稳定后的横向偏差曲线Fig.10 Transverse deviation curves after system stability

Kp/Kd平均偏差平均绝对偏差最大偏差标准差1/3-0.02840.02880.06920.01561-0.02630.02860.11470.02552-0.02620.02000.09420.0246

从图10和表2可得，导航控制系统存在一定的稳态误差；Kp/Kd=1/3时，标准差为0.015 6 m，小于其他两种比值的标准差(0.025 5 m和0.024 6 m)。Kp/Kd的取值较小时，导航控制系统的稳定性相对较好。

最后，经过反复测试，确定了参数Kp和Kd的取值范围分别为[0.8,2.1]和[1,3.5]。根据第2节设计的模糊自适应控制器，分别进行了水泥地面和水田环境的路径跟踪实验。

3.3 水泥地面实验

首先，在水泥地面实验时，直播机挂载播种器，设定期望跟踪路径为直线AB，并设定直播机所在位置距离直线AB的初始横向偏差为0.50 m，航向偏差为0°；主变速手柄被设定在前进挡位，由变速踏板控制器设定行走速度为0.8 m/s。

如图11所示，导航控制系统上线距离为4.3 m，无超调，稳定后最大横向偏差为0.039 8 m。对导航控制系统稳定后(偏差见图12)的数据进行分析，其结果如表3所示。

将该组数据与3.2节固定Kp/Kd的数据进行对比，结果如图13所示。由表2、3和图13可见，模糊自适应PD控制器直线跟踪的平均偏差和最大偏差等性能指标相对于未使用模糊调整的PD控制器均较好。模糊自适应PD控制器直线跟踪的平均绝对偏差比前两组控制器(Kp=1,Kd=3和Kp=1,Kd=1)小，与Kp/Kd=2的控制器的平均绝对偏差基本相当。可见，自适应调整控制律参数的控制方法较未使用模糊调整的固定Kp/Kd比值的控制方法超调较小，响应速度更快，跟踪精度和稳定性也更高。

图12 系统稳定后的横向偏差曲线(水泥地面)Fig.12 Lateral error after system stability (cement pavement)

参数平均偏差平均绝对偏差最大偏差标准差数值-0.01810.02090.03980.0275

图13 模糊自适应的横向偏差曲线Fig.13 Lateral deviation curves of different Kp/Kd

3.4 水田环境实验

按照3.2节的实验设定和实验步骤，对在水田环境下导航系统的路径跟踪效果进行了实验。为了对比模糊自适应控制方法与传统方法的控制效果，在水田里还进行了固定PD参数的PD控制方法和纯追踪控制方法的实验。其中PD控制实验中取Kp为1.2，Kd为2.2；纯追踪控制实验中取前视距离为1.2 m。

如图14所示，导航控制系统上线距离约为6 m，超调为0.04 m，说明导航控制系统具有较好的快速性。

图14 水田环境路径跟踪的实验结果Fig.14 Experimental results of environmental path tracking in paddy field

图15 系统稳定后的横向偏差曲线(水田环境)Fig.15 Lateral error after system stability (paddy field)

导航控制系统稳定后(偏差见图15和表4)，模糊自适应PD控制的最大偏差为0.208 8 m，比固定参数的PD控制器和纯追踪控制器的最大偏差都要小。同时模糊自适应控制的平均绝对偏差为0.039 0 m，比固定参数的PD控制器和纯追踪控制器的偏差都要小，总体精度更高。模糊自适应PD控制的标准差为0.073 6 m，也比固定参数的PD控制和纯追踪控制小，路径跟踪更为稳定。从图14还可以看到，模糊自适应PD算法可在复杂环境下自动调整Kd，相较于其他两种算法，超调较小，响应较快。这与理论分析和仿真的结果是一致的。

表4 水田直线路径跟踪偏差Tab.4 Linear path tracking error m

根据以上对比实验，可以得出：

(1) 从图9、11、14、15可以看出，基于模糊自适应PD控制器的自主导航系统具有较好的响应速度、跟踪精度和稳定性。

(2) 从表2～4可知，导航系统存在稳态误差，这是由于车体坐标系与导航坐标系以及前轮角位移传感器的对称零点误差导致的。

(3) 在水田环境作业时，由于导航控制系统没有添加姿态补偿，导致车体发生严重侧滑侧偏时，定位误差变大，严重影响了导航系统的跟踪精度和稳定性，大大降低了导航精度。可以通过惯性传感器测量车身姿态改善这一问题。

4 结论

(1)在建立水稻直播机运动学模型的基础上，基于链式空间状态模型设计了PD控制器。

(2)建立了水稻直播机的仿真模型，通过仿真得出了Kp和Kd的值，以及两者比值对系统稳定性和响应速度的影响，基于仿真结论提出了一种模糊自适应控制方法，并通过仿真验证了该模糊自适应控制方法的快速性、准确性和稳定性。

(3)通过现场实验确定了控制参数的取值范围。为了验证方法的有效性，在水泥地面和水田环境进行了实验，实验结果表明，所提出的方法在水田环境下平均绝对偏差为0.039 0 m，最大偏差为0.208 8 m，相较于固定参数的PD控制器和纯追踪控制方法，具有更小的偏差和超调，响应更快，鲁棒性更好，有效提高了水稻直播机在水田环境下导航控制系统的稳定性和快速性。