水稻穴直播机自主驾驶控制系统设计

李志腾，李彦明，唐小涛，刘成良

(上海交通大学 机械与动力工程学院，上海 200240)

0 引言

自动导航与自主驾驶是智能农机装备发展的一个关键技术组成要素，自动转向控制和导航控制是智能农机自动导航系统研究的关键技术，直接影响自动导航控制系统的稳定性、响应的快速性和精度。

自动转向控制有电液控制和电机控制两种方式。对于大型农机，转向直接由油缸驱动，可采用电液转向控制方法。伊利诺伊大学厄巴纳-香槟分校的H. Qiu[1]，中国农业大学的陈文良[2-3]、何卿[4]，华南农业大学的罗锡文[5]、吴晓鹏[6]等，都对液压转向农机自动驾驶技术进行了研究，表明液压自动转向控制系统具有输出扭矩大、响应速度快及节省能量等优点。目前，液压转向自动驾驶在新疆、黑龙江等地被广泛运用在大型农业机械上。

插秧机、穴直播机等小型农业机械因其油路封闭而不易实现电液控制，必须采用电机转向控制方法。拓普康及Autofarm公司均推出了电动转向控制产品，但由于其价格、系统不开放或结构尺寸固定等原因，不适合国内穴直播机使用。伊利诺伊大学厄巴纳-香槟分校的Yoshisada Nagasaka[7]等，华南农业大学张智刚、胡炼、罗锡文[8-10]等，中国农业机械化科学院伟利国[11-12]等，沈阳自动化研究所的李逃昌[13]等分别以插秧机为实验平台，进行了电控农机的转向控制系统及导航控制系统的相关研究。相对于液压转向控制、电动转向控制传动链长，存在传动间隙等非线性因素，其控制较液压转向更为复杂，尤其是水田恶劣复杂环境下的高精度控制仍是现在研究的重点。

本文针对南方水田水稻穴直播机自动驾驶需求，以洋马VP6型穴直播机为试验平台，自主设计了CAN总线化的穴直播机电动转向控制、行进速度控制及挡位控制，分析了运动及控制模型，并开发了双闭环轨迹跟踪控制算法，实现了穴直播水田自主驾驶，水田直线跟踪上线稳定精度可达8cm。

1 自主驾驶系统改造及控制系统组成

1.1 自主驾驶机电改造

自主驾驶系统的机电改造包括两部分，即转向机构的改造和变速机构的改造。其中，转向机构的改造如图1所示。

减速步进电机的扭矩通过同步齿形带带动同步带轮传递到与方向盘的转轴上，并通过转动转换机构传递到下面的转向柱上；液压转向助力结构协助方向盘进行左右转向，实现了穴直播机的转向控制。

穴直播机的左右前轮和前桥上均不易安装角度传感器，且前轮转角的变化可以由中间转向轴通过平行四杆机构传递给安装在后轮轴上的机构。因此，采用角度传感器安装在后轮轴上的方法对前轮转角的变化进行检测。由于前轮转动时，前轮的左侧偏角与右侧偏角不对称，故可以将前轮左右转角变化的大小等效到中间转向轴转角的变化，即

δm=(δl+δr)/2

其中，δm为中间转向轴转角；δl为左侧前轮转角；δr为右侧前轮转角。

中间转向轴的转角变化通过平行四杆传递到后轮轴上，经试验确定，角度传感器测量的角度可以等效于穴直播机的前轮转角。

变速机构的改造包括主变速手柄的改造和变速踏板的改造，如图2所示。主变速手柄控制穴直播机的前进、后退和停止，变速踏板的升降改变穴直播机的行驶速度。电动推杆的伸缩带动连杆，使齿轮变速箱中的齿轮啮合在前进与后退的齿轮上，主变速手柄位置被移动到相应的挡位，变速踏板的升降则是由电动推杆的伸缩通过拉线的拉伸来实现的，从而实现了穴直播机行驶速度的自动控制。

1.方向盘 2.转动转换机构 3.同步带轮 4.减速步进电机 5.液压转向助力器 6.平行四杆机构 7.角度传感器

图2 变速机构改造

1.2 自主驾驶控制系统组成

自主驾驶控制系统的组成如图3所示。

图3 控制系统总体结构图

基于安卓系统的小米Pad规划管理系统通过蓝牙将设定路径传输给自主驾驶控制器；自主驾驶控制器接收到北斗定位系统定位的穴直播机的位置信息和航向信息，并根据读取姿态传感器的数据，与期望路径相比较，得出期望转角与行进速度，通过CAN总线对基于STM8的变速踏板控制器、主变速手柄控制器及驱动步进电机的转向系统控制器进行控制。变速踏板控制器和主变速手柄控制器都是通过CAN总线控制电动推杆，实现穴直播机的变速踏板的升降控制和挡位位置控制。姿态传感器将穴直播机的姿态信息以20Hz的输出频率传输给自主驾驶控制器。

2 自主驾驶系统控制算法设计

自主驾驶系统原理图如图4所示。自主驾驶控制系统是一个双闭环的控制系统，由外环路径跟踪控制系统和内环转向角跟踪控制系统组成。其中，外环控制系统根据北斗定位系统接收到的的定位数据和位姿传感器的姿态数据，与期望的规划路径进行比较，得出前轮的期望转角δ；内环控制系统则根据期望转角δ与实际转角的差值通过转向控制器驱动步进电机控制转向机构，使穴直播机前轮达到期望的转角。

图4 自主驾驶系统原理图

2.1 自主外环轨迹跟踪控制器设计

根据农机的运动学模型[13]得

(1)

其中，d′为横向偏差的导数；θe为航向偏差；L为轴距；v为行驶速度；δ为期望转角。

由于穴直播机行驶速度很小，且直线跟踪时速度和变化也很小，可以认为不变，因此得出

(2)

由非线性农机相对运动学模型[13]得

(3)

其中，ρ为农机即时转弯曲率；c(s)为路径曲率；s为曲线路径弧长参数。

可以推导出

(4)

根据文献[13]采用的反馈线性化理论和线性系统理论，将模型通过状态变换和输入变换得到最优控制律为

sec3θ3[1-dc(s)]2ρ+[dc(s)-1]×

c(s)(tan2θe+sec2θe)

=-kpd-kd[1-dc(s)]tanθe

(5)

其中，kp、kd为控制参数。

穴直播机跟踪直线时，可以合理地假设θe非常小，c(s)恒为0，则可以得到

ρ=cos3θe(-kdtanθe-kpd)

(6)

将式(6)代入式(4)，得穴直播机跟踪直线时所需要的期望转角为

δ=arctan[Lcos3θe(-kdtanθe-kpd)]

(7)

2.2 自主内环转向控制系统设计

图4内环穴直播机转向系统传递函数为

(8)

其中，δ为期望转角；θm为实际反馈转角；T为电机转向系统时间常数；Km为电机转向系统综合放大系数；Kp为控制器比例系数。

由此可以看出：控制器比例系数越大，响应变快；但超调量变大，振荡次数增加。经过试验反复测试，内环转向控制比例系数选为Kp=4.7。

在实际控制中，考虑步进电机频率转矩特性限制，采用了限幅比例控制，即控制器模型为

其中，u为控制器输出频率；±um分别为控制器输出频率正负限值；±ε分别为转角偏差阈值，取ε=35°。

图5为转向跟踪控制验证曲线。以方波作为转向控制器导向输入信号，采样频率为10Hz，角度幅值为15°。

试验结果表明：方波信号跟踪上升时间为4s，超调量为不大于5%，自动转向系统的跟踪效果可以满足要求。

图5 转向角跟踪方波信号曲线

3 试验测试及结果

以VP6型穴直播机为试验平台，如图6所示。

图6 VP6型穴直播机平台

首先在松江农业推广站试验田附近的水泥地面上对穴直播机测试了穴直播机直线跟踪的效果，而后进一步测试其在水田环境的直线跟踪效果。

经过反复试验确定了效果较为理想的控制器参数范围为kp=(0.8,1.2)、kd=(1,3.5)，试验时采用kp=1、kd=3.5。

在水泥地面试验时，穴直播机挂载播种器。首先，设定跟踪的AB直线，并设定穴直播机所在直线离AB直线的初始横向偏差为0.55m，主变速手柄在前进挡位，由变速踏板控制器设定行走速度恒为0.8m/s。试验结果如图7所示。

图7 水泥地面直线跟踪的试验结果

图7中，上线距离约为5m，稳定后的最大横向偏差不大于0.05m，标准方差为0.015m。转向角跟踪平均滞后时间不大于0.2s。这说明，自主驾驶控制系统能够快速回到预定路径，满足穴直播机的作业要求。

水田环境试验时，穴直播机也挂载播种器。首先，设定AB直线，并设定穴直播机距离AB直线的初始距离为0.5m，主变速手柄在前进挡位，由变速踏板控制器设定行走速度恒为0.8m/s。试验结果如图8所示。

图8 水田直线跟踪的试验结果

图8中，穴直播机上线距离约为6m，不发生侧滑的情况下稳定后的最大横向偏差不大于0.08m，标准差为0.027 4m，转向角跟踪平均延时不大于0.3s，自主驾驶控制系统能够满足水田的作业需求。

4 结论

1)为满足水田水稻穴直播机自主作业和无人驾驶需求，根据洋马VP6型穴直播机的结构特点，进行了穴直播机机电改造，完成了CAN总线化转向、油门及挡位的控制系统设计，满足自动驾驶功能要求。

2)针对穴直播机自动驾驶的高性能要求，进行了运动学及控制模型分析，分别建立了外环轨迹跟踪和内环转向角度跟踪双闭环控制算法策略，并经过试验确定了合适参数，实现了水田干扰下自动驾驶±8cm跟踪。

3)实现了穴直播机自动驾驶的完全自主化设计。