水田环境下水稻直播机自动驾驶控制方法

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

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

0 引言

随着农业机械化和自动化的不断进步，精准农业也得到了显著的发展。无人农机的发展降低了农业生产成本、提高了质量并且减少了人工投入[1]。自主驾驶是无人农机核心技术之一。自主驾驶系统使得农机无需驾驶员控制方向盘，并且可以长时间工作，不会因为疲惫而降低作业质量[2]。因此，相对驾驶员，自主驾驶系统在长时间工作时可以更高精度地完成作业任务，提高作业质量，并且可以降低人力成本。随着计算机和传感器技术的进步，农机领域的自主驾驶系统获得了长足的发展且成为研究热点之一[3]。对于工作在水田等非结构化环境下的水稻直播机等农机装备而言，在非平坦且泥泞易滑的水田环境里实现高精度的路径跟踪和自动驾驶具有重要意义。

在农业自主驾驶系统上，常用的路径跟踪方法有PID控制[4-5]、模糊控制[6-8]、纯追踪算法[9-12]、最优控制[13-16]等。通常以转向角作为控制变量[17-18]。在无法获得准确的转向数学模型时，PID控制或模糊控制是农机转向控制最简单的控制方法[19]。文献[20-22]实现了基于PID控制方法的转向控制器，但是实验结果显示其算法具有较大的超调，且误差较大。WU等[23]设计了一种串联闭环PID控制器，实验结果显示这种控制器可以提高跟踪精度，但是控制器所需的转向角速度较难测量。文献[24-25]基于模糊控制算法实现了应用于农机的自适应自动驾驶算法。李逃昌[15]采用模糊控制调整前视距离的纯追踪模型方法在插秧机平台上进行了实验，但实验结果显示路径跟踪时存在大于0.1 m的超调，影响了路径跟踪的精度。ZHANG等[26]提出了一种综合拖拉机动态信息和跟踪误差的动态路径跟踪算法，具有较好的跟踪精度。HAN等[27]基于航向角偏差计算转向角实现了一种自动驾驶算法。JAVAD等[28]实现了一种高精度的非线性跟踪算法，仿真结果显示横向偏差均方根仅1.36 cm。李培新等[29]基于跟踪误差模型提出一种无人驾驶预测控制方法，仿真结果表明平均横向偏差小于0.1 m，航向角小于1°。但这些算法均未在水田类似的泥泞和非平坦路面测试，无法直接用于水稻直播机的自主驾驶。李革等[30]用基于速度、路径弯度等线性调整前视距离的改进纯追踪方法在插秧机平台上做了水田实验，实验结果显示作业段平均误差为0.058 m，最大跟踪误差达0.135 m，但实验直线段长度过短，不足20 m。

为实现水稻直播机在水田环境下较高精度的自动驾驶，提出一种在不平坦泥泞水田环境中的水稻直播机自动驾驶控制方法，以实现直线段的高精度路径跟踪和地头自动转向。

1 运动学模型

1.1 模型

在不考虑车胎与地面的相互作用，假设车辆不会发生侧滑俯仰等情况下，水稻直播机可简化为两轮车模型[31]，如图1所示。模型中所涉及的参数和说明如表1所示。

图1 车辆运动学模型Fig.1 Kinematic model of vehicle

参数含意δ前轮转角,也是控制变量ω转向角速度θ航向角θp期望路径P在点T处切线的航向角L水稻直播机轴距(x,y)车辆后轴中心点坐标,y轴正向为正北方向v车辆速度P车辆跟踪的期望路径d横向偏差φ航向角偏差R车辆转弯半径M(xm, ym)车辆后轴中点坐标T(xt, yt)路径P上距M最近的点坐标

1.2 农机运动学模型

(1)

(2)

由式(1)和式(2)可得

(3)

速度v在x轴方向的分量为

(4)

速度v在y轴方向的分量为

(5)

因此，水稻直播机的运动学模型为

(6)

水稻直播机的运动状态可以用四维向量描述为

s=(x,y,θ,δ)

(7)

2 自动驾驶控制方法

水稻直播机的自动驾驶控制方法包括在作业段的直线路径跟踪算法和地头处的自动转弯算法。在直线段应尽量提高跟踪精度，在地头转弯时应尽量提高速度。因为水田坑坑洼洼，不平坦的路面会导致车身倾斜，给农机定位带来一定的误差。为消除这种影响，需要根据车身的倾角对定位数据作出修正。

2.1 不平坦路面的定位修正

在水田环境等不平整路面上行驶时，车身往往会发生倾斜。当定位系统安装在有一定高度的车身上时，会给定位带来误差,如图2所示。定位系统测量车辆后轮轴中点M的位置作为车辆的位置。在车身横滚角和俯仰角为零时，定位天线测量点在车轴上的投影与M重合。在车身倾斜时，定位天线测量点在车辆后轴上的投影与M点并不重合，从而无法获得准确的M点位置信息。

图2 车身倾斜造成的误差Fig.2 Position errors caused by incline

车身倾斜造成的误差有2个，一个为横滚角导致的误差elat，一个为俯仰角导致的误差elon。设定位天线的安装高度为H，横滚角为α，俯仰角为ψ，则

elat=Hsinα

(8)

elon=Hsinψ

(9)

因此修正后的车辆位置M的坐标为

(10)

式中 (xm,ym)——修正后的M点坐标

2.2 直线段非线性路径跟踪控制算法

横向偏差d的计算方法为

(11)

设θp为期望路径P在点T处切线的航向角，则航向角偏差φ为

φ=θp-θ

(12)

横向偏差代表当前的跟踪偏差，而航向角偏差影响随后的跟踪偏差。因此车辆转向时既要考虑横向偏差也要考虑航向角偏差，可将两者组合起来计算转向角。其中消除航向角偏差φ的前轮转角控制量δφ等于航向角偏差φ，即

δφ=φ

(13)

消除横向偏差d的前轮转角控制量δd为一个非线性反馈控制，为

(14)

其中

dfront=d-Lsinθ

(15)

式中k2——可调参数

dfront——前轮的横向偏差

通过对控制算法添加对横向偏差和前进速度的非线性反馈环节，通过反正切变换对反馈输出限幅，同时在速度较快时减小前轮转角输出，可以有效减小超调量，提高稳定性。

结合式(13)～(15)，添加积分环节消除稳态误差，基于航向角偏差、横向偏差和车辆前进反馈的非线性控制路径跟踪算法为

(16)

其中Iint为减小静态误差的积分项，为横向偏差d在过去一段时间的积分；k1、k2和ki为转向算法中根据实际情况可调的参数，代表各项对转向角影响的比重。

2.3 地头自动调头算法

地头自动调头的关键在于识别是否到达地头。可以预先通过手持式定位仪标定作业地块的边界关键点，建立作业地块的数据库，从数据库获得的边界关键点计算出作业地块的边界位置信息。实时计算水稻直播机位置与地边界的距离，在小于阈值D时认为车辆到达地头，自动执行调头动作。

对于长江中下游平原地区的水田，地边界大部分可看作直线段，因此可通过计算点到线段的距离来获得车辆与地边的距离，判断是否到达地头，如图3所示。M为车辆位置，dis为车辆从M到地边AB的距离，C为M到AB所在直线的垂足。点到线段的距离可以通过计算向量点积获得。

图3 计算水稻直播机与地边距离的3种情况Fig.3 Three conditions for calculating distance between rice seeder and border of field

在控制算法识别车辆到达地头后，车辆自动执行调头动作。调头时的控制算法根据作业要求的作业线间距dgap得到转向半径R为

(17)

结合式(2)得前轮转角δ为

(18)

根据式(18)计算的车辆转角执行转向。转向过程中，当车辆相较于下一条作业线的横向偏差绝对值和航向角偏差绝对值小于预设的阈值时，认为车辆已完成调头，结束调头转向过程，重新执行式(16)的直线路径跟踪算法。

3 算法仿真

通过1.2节建立的农机运动学模型，建立如图4所示的算法仿真模型。

图4 算法仿真模型Fig.4 Algorithm simulation model

图中的控制算法模块内即为预设的路径跟踪算法，d为横向偏差，fi为航向角偏差，y为输出的前轮转角。分别对本文的路径跟踪算法、纯追踪算法和PD控制算法进行仿真。仿真时，初始横向偏差均设为0.5 m,车辆速度设为1.5 m/s。文中k1取1，k2取3，ki取0.05；纯追踪算法预瞄距离Ld取2 m；PD控制Kp取1.2，Kd取0.8。仿真结果如图5所示。

图5 仿真结果Fig.5 Simulation results

由仿真结果可见，本文算法能迅速响应消除横向偏差，超调小，稳定性好。相较于传统的纯追踪算法和PD控制算法，本文算法超调量减小30%，稳定性更好。

4 实验验证

4.1 实验平台

实验平台为上海世达尔公司的洋马VP6型水稻直播机，如图6所示。该平台搭载了由北斗GNSS定位系统、自动转向系统、油门控制系统、导航主控制器、惯性姿态检测模块、转向角检测模块等组成的自主驾驶控制系统。主控制器为一块800 MHz的ARM Cortex-A8控制器(AM335X型，美国德州仪器公司)，通过CAN总线和串口总线与其他传感器模块、执行模块通信；油门控制系统和转向控制系统分别采用STM8作为次级控制器，响应主控命令；用户通过基于Android的用户终端远程操控和监测水稻直播机。系统架构如图7所示。

图6 实验平台Fig.6 Experiment platform

图7 自动驾驶系统架构Fig.7 Framework of autonomous system

为测试本文提出的自动驾驶控制方法的性能，分别在水泥地面以及水田环境下对本文算法进行实验验证。

4.2 实验参数

车辆轴距L为1.05 m，天线安装高度H为2 m。经过参数整定，文中Iint取车辆过去20 s内的横向偏差的积分，k1取1，k2取2.26，ki取0.05。本文实验地头自动调头阈值D取2.9 m，重新上线横向偏差绝对值阈值取0.3 m,航向角偏差绝对值阈值取30°。

4.3 水泥地面实验

将水稻直播机的速度设定为0.8 m/s，在水泥路面上进行直线行走实验，跟踪直线的横向偏差如图8所示。水稻直播机在水泥路面上的直线跟踪横向偏差绝对值最大为0.026 3 m，平均绝对偏差为0.006 9 m，跟踪精度较高。

图8 水泥路面实验的横向偏差曲线Fig.8 Deviation curve of path tracking on cement road

4.4 水田实验

尽管水泥地实验表明本文的控制方法具有较高的精度，但是仍然需要水田实验来测试控制算法的有效性。

在实验前先用手持式定位仪将作业水田边界点的位置信息保存在数据库中，并加载到用户终端里。启动自动驾驶前，用户终端根据作业要求将作业路径规划成间隔为2.5 m的平行直线段。在水田实验中，车辆速度设为0.8 m/s，初始横向偏差和航向角偏差为零。实验水田并不平整，车身常有倾斜。经过实验，车辆实际行走轨迹如图9所示。图9中点划线为地边界，实线为车辆实际行走轨迹。右上角为车辆自动驾驶的起点，右下角为终点。图10为用户终端界面实时显示的行走轨迹。

图9 水田实验车辆行驶轨迹Fig.9 Path of rice seeder in paddy field

从图9和图10中车辆的行走轨迹可以看到，车辆可在地头自动调头进入下一条作业直线段。

图10 用户终端界面Fig.10 Interface of user terminal

水田实验的所有直线段横向偏差数据如图11所示。实际路径跟踪轨迹的效果如图12所示。各直线段定义为启动自主驾驶后或调头后第1次偏差小于0.05 m后到下一次调头前的这段路径。水田实验中所有直线段路径跟踪的偏差数据如表2所示。从表2可知，每条直线段的横向偏差均方根不超过0.041 m，总体横向偏差均方根为0.035 m。每条直线段的平均横向绝对偏差不超过0.032 m，总体平均横向绝对偏差为0.027 m。航向角平均绝对偏差为0.027°。

图11 所有直线段的横向偏差Fig.11 Lateral deviations of all straight paths

图12 车辆水田跟踪效果Fig.12 Paths tracking of rice seeder in paddy field

表2 水田实验中所有直线段路径跟踪偏差Tab.2 Summary of deviation of path tracking in paddy field

表3为横向偏差数据中小于0.05 m和0.10 m的横向偏差占所有数据的比例。从表3可知，每段直线的横向偏差绝对值都有不小于76.8%的比例在0.05 m以内，不小于98.4%的横向偏差绝对值在0.10 m内。在整个实验中，有85.8%的横向偏差绝对值在0.05 m以内，99.5%的横向偏差绝对值在0.10 m以内。其中，第1条、第5条、第6条和第8条直线段，路径跟踪的所有横向偏差绝对值小于0.10 m。图13为各段作业线横向偏差分布图，纵坐标为这段路径横向偏差所在各偏差范围的频率。水田实验结果表明，本文的自动驾驶方法在水田的直线跟踪上具有较好的跟踪精度，可以完成自动驾驶作业的路径跟踪要求。

表3 横向偏差小于0.05 m和0.1 m的数据比例Tab.3 Ratio of lateral deviation less than 0.05 m and 0.1 m %

图13 各段直线的横向偏差分布Fig.13 Distribution of lateral deviation of each path

5 结束语

将横向偏差和航向角偏差非线性组合作为反馈，结合积分环节控制方向盘转角实现自动驾驶转向。通过实时计算水稻直播机位置到地边界的距离实现地头自动调头切换作业线。为了提高定位精度，通过姿态传感器获取车身倾角对定位数据做了修正。水泥路面实验结果表明，本文算法在直线跟踪时横向偏差绝对值最大为0.026 3 m，平均绝对偏差仅0.006 9 m。在不平整水田的实验结果表明，本文算法跟踪路径的平均横向绝对偏差总体仅0.027 m，85.8%的横向偏差绝对值小于0.05 m，99.5%的横向偏差绝对值小于0.10 m。本文提出的方法在不平整泥泞水田环境中具有较好的鲁棒性，能实现对路径精准的跟踪和地头自动调头切换作业线，有效地改善了水稻直播机在不平整泥泞水田等恶劣环境下自动化作业的精度和效率。