基于非线性模型的农用车路径跟踪控制器设计与试验

刘正铎 张万枝 吕钊钦 郑文秀 穆桂脂

(1.山东农业大学机械与电子工程学院，泰安 271018； 2.山东省园艺机械与装备重点实验室，泰安 271018)

0 引言

农用车辆的路径跟踪是实现农业机械自动化与智能化的关键技术，其路径跟踪的难点在于田间作业环境复杂，农用车辆很难根据不同环境做出正确判断，保证车辆的正常行驶。农用车辆的路径跟踪性能依赖其控制器所用的控制算法，其中主要的控制算法包括LQR控制算法、Lyapunov法、Backstepping法和滑膜控制算法等[1-4]。

文献[5]提出一种基于LQR算法的轨迹控制方法，在滚动时域的每一采样时刻将参考轨迹上的轨迹跟踪误差线性化，从而得到线性模型并利用该模型设计了路径跟踪控制器。文献[6-7]基于非切换解析模型预测控制理论，结合非线性模型预测控制，设计出一种船舶定位控制系统，使船舶可以快速移动到参考位置。文献[8-9]通过推导无人驾驶车辆在转弯时的速度、轮胎侧偏角、横摆角速度等参数，应用滑膜控制方法在仿真条件下可以很好地控制车辆漂移时的行驶状态。文献[10]利用粒子群算法优化模糊控制中的控制参数，该方法可以减小横向误差，当车速为0.8 m/s时曲线路径跟踪最大横向偏差为5.9 cm。文献[11]采用了基于触须原理的局部路径规划，利用车辆在行进过程中多组不同半径的圆弧段来描述路径，在跟踪直线路径时得到了良好的效果，但是它忽略了车辆在转弯时曲率变化的速度，从而造成在速度较高时无法成功跟踪圆形路径。

近年来模型预测控制越来越受到学者们的青睐，模型控制的优点是预测与控制的效果良好，缺点是控制器的控制作用依赖于数学模型的精度，当模型精度低时直接影响到路径跟踪的精度。张纯刚等[12]结合预测控制理论进行自动驾驶的局部路径规划，提出了基于滚动窗口的路径跟踪方法，该方法可以充分利用已知的局部信息，通过反馈进行局部路径规划，得到了很好的收敛性。文献[13-15]采用线性模型预测进行农用车的路径跟踪，通过求解带约束的目标函数、在线滚动优化、反馈校正、修正预测模型等使路径跟踪系统的误差最小，但是由于建模过程涉及到线性化与离散化,所以在跟踪圆形路径时会造成跟踪误差较大。3 m/s时直线跟踪最大横向偏差为1.706 cm，圆形跟踪最大横向偏差为15.245 cm。本文从非线性模型预测控制算法角度出发，通过合理简化模型提高建模精度，在降低计算量的同时保证路径跟踪的效果。

1 路径跟踪系统组成

本文设计的田间路径跟踪试验小车如图1所示，具体结构参数如表1所示。

图1 试验小车Fig.1 Test car1.前轮转角传感器 2.速度传感器 3.差速驱动装置 4.前轮转向驱动器 5.信号转换装置 6.电瓶 7.后轮驱动器 8.前轮转向装置

表1 试验小车主要技术指标Tab.1 Main technical indexes of vehicle

路径信息采集模块：转角传感器为E6B2-CCWZ3E型编码器，测量误差为±0.3°；速度传感器为JK5002D型霍尔接近开关，测速误差±0.05 m/s。控制执行模块：转向驱动器为ASMT-01series型大功率直线舵机控制器；转向执行装置为42BYGH47型步进电动机驱动涡轮蜗杆式转向装置,其行程为80 mm,速度最快可达29 mm/s；后轮驱动器为CM2010型有刷电动机控制器；后轮驱动装置为DMW86型无刷直流电动机，额定转速为3 200 r/min，额定功率为1 000 W。路径信息处理模块：导航控制器下位机为STM32F103ZET6型单片机，其主要任务是采集试验小车转速传感器与前轮转角传感器的模拟信号并转换成数字信号，通过WiFi与USART通信传输给导航控制器上位机(DELL计算机)中的Matlab软件，从而进行算法处理。路径跟踪系统工作流程图如图2所示。

图2 路径跟踪系统工作流程图Fig.2 Flow chart of path tracking system processing

2 路径跟踪控制器设计

2.1 农用车运动学模型

对农用车进行运动学分析，在平面坐标系下建立如图3所示的模型。该模型前轮转向，后轮驱动。在工作过程中，转向轮与驱动轮通过调节电压来调节转角与转速。在整个运动学分析过程中，将农用车视为刚体，运行在水平面上。为确定农用车在整个运行轨迹中的位置和姿态，建立导航坐标系。选取农用车质心点为参考点并定义农用车的位姿信息为(x,y,φ)。在不考虑农用车侧倾、侧滑、俯仰等情况下，可将低速行驶的农用车辆简化为二自由度运动学模型[16-19]。

图3 农用车的运动学模型Fig.3 Kinematics model of agricultural vehicle

农用车运动学方程[20-21]为

(1)

式中l——轴距

φ——航向角

δ——前轮偏角

x、y——质心坐标值

v——前、后轴中心速度

将式(1)表示为状态空间方程形式

(2)

其中

ξ=(x,y,φ)u=(v,δ)

式中u——系统控制量

f(·)——状态方程

ξ——系统状态量

η——系统输出量

2.2 非线性模型预测控制器的设计

2.2.1控制器的预测模型

在非线性模型预测控制中，传统算法存在计算量大、收敛速度慢等问题。本文利用欧拉公式对农用车运动学模型进行离散化求解找到一个显式的迭代方程[22-23]，减少了计算量，加快了系统的收敛速度。通过非线性模型、当前状态量和控制时域内的控制量对预测时域内的控制量进行预测，得

(3)

式中k——离散变量

T——采样周期

根据这一模型只需知道被控系统在某一时刻的初始位姿信息(x(k),y(k),φ(k))与两个控制步长内的控制输入序列(v1,δ1)、(v2,δ2)便可以预测出系统在预测时域内的控制输出序列。将式(3)整理为输出函数的形式

ηp(t+j|t)=Fj(ξ(t|t),u(t|t),…,u(t+Nc-1|t))(j=1,2,…,Np)

(4)

(5)

(6)

式中Np——预测时域

Nc——控制时域

为减少非线性模型控制的计算量、增强系统的实时性，在本文控制器中Nc=2，从而得到非线性模型预测控制器的预测模型为

(7)

2.2.2非线性模型的闭环校正

为了更好地描述系统及克服试验时的不确定因素，在控制输出的基础上通过误差预测和实时反馈实现对预测模型的闭环校正。

e(t+i-1|t)=Γr(t+i-1|t)-Γp(t+i-1|t)(i=1,2,…,Np)

(8)

其中

Γr(t+i-1|t)=

式中e(t+i-1|t)——t时刻系统在预测时域Np内的预测误差

Γr(t+i-1|t)——t时刻系统根据实时反馈推出的t+i-1时刻的系统输出量

Γp(t+i-1|t)——t时刻系统根据运动学模型推出的t+i-1时刻的系统输出量

Xr(t)、Yr(t)、φr(t)——系统通过实时反馈得到的农用车实际位姿信息

vc、δc——控制时域内的控制量

i=1时下角c为1，i>1时下角c为2。

设定f(0,0)=0为系统的一个稳定点，同时也是系统的控制目标。对于任意预测时域Np,考虑如下形式的目标函数

(9)

其中

式中 ΔΓ——预测误差序列

Δu——控制增量序列

Q——权重矩阵

R——权重系数

目标函数第1项表征了期望输出的跟踪能力，第2项表征终端约束。

通过设置状态量与控制量的取值范围将路径跟踪问题转换为解决非线性二次型的约束问题。由于目标函数与约束函数在求解算法中的梯度都是连续的，所以目标函数可以根据所给的初始值进行梯度步长的收敛与求解。从而将式(9)转换为求解基于递推序列二次规划法[24-25]的响应问题，在此基础上进行梯度计算解决非线性约束的边界优化

(10)

其中

Γmin≤Γ(i|t)≤Γmax

ΔΓmin≤ΔΓ(i|t)≤ΔΓmaxumin≤u(i|t)≤umax(i=0,1,…,Nc-1)Δumin≤Δu(i|t)≤Δumax(i=0,1,…,Nc-1)

式中Γmin、Γmax——状态变量约束最小值、最大值

ΔΓmin、ΔΓmax——状态变量增量约束最小值、最大值

umin、umax——控制量约束最小值、最大值

Δumin、Δumax——控制增量约束最小值、最大值

由式(10)可以看出，本文所述控制器是根据预测时域与控制时域内的误差信息对路径跟踪进行预测控制，在动态响应阶段，预测时域与控制时域内的信息不完善，系统处于不稳定状态。由于此时控制系统启动时间较短，各个参考速度下的农用车实际速度相差不大，在同样时间内参考速度越大的运动学模型会预测出更多的路径信息，使系统更快地趋于稳定；当农用车在行驶过程中(即系统稳定阶段),此时农用车如果受到外界干扰,农用车的速度越大，偏差就会越大，因为此时不同速度下的农用车均已稳定，控制器需要重新预测路径，相同时间下，速度越快的农用车，偏差越大。

2.2.3非线性模型的滚动优化

本文采用内点法[26-27]进行求解。将t时刻控制输入量的第1个元素作为实际控制量作用于控制系统，在t+1时刻重复以上控制过程实现最优控制。

3 控制器仿真与分析

3.1 仿真路径

图4 试验参考路径Fig.4 Experiment reference path

如图4所示，农用车辆从O点进入山东农业大学试验基地，K点为目标试验田，其中O、A、B、C、D、E、F、G、H、J、K为路径方程改变的节点，在一定程度上它们的各项参数反映出控制器对该段路径的跟踪性能。其中定义A点为动态响应阶段最大超调量坐标点。基于Matlab平台进行模型预测控制器设计，从速度v、采样周期T、预测时域Np3方面进行仿真分析。

3.2 速度对路径跟踪的影响

设定控制器参数为：Np=30、Nc=2、T=0.03 s，v分别为1、2、3 m/s时，在导航坐标系下路径跟踪结果如图5所示。 其中，Δx表示横向跟踪偏差，Δy表示纵向跟踪偏差。

图5 速度对路径跟踪的影响Fig.5 Effect of speed on path tracking

图6 采样周期对路径跟踪的影响Fig.6 Effect of sampling period on path tracking

从图5可看出，农用车可以有效跟踪参考路经。从图5b、5c可以看出，在动态响应阶段，v=1 m/s时超调量最大且调节时间最长,横向最大跟踪偏差为-9.24 cm,纵向最大跟踪偏差为-10 cm,调节时间与超调量随速度的增加而减小。图5c在节点E、G处出现了较大的纵向跟踪偏差，此时农用车刚经历了AC段的直线路径，随后进入CE段圆形路径，由于两段曲线方程不一样，致使控制器需要重新预测控制量以适应新的参考路径，从而造成较大偏差。图5c、5d反映出路径跟踪时农用车的行驶稳定性，可以看出，在仿真条件下，速度的增大对跟踪精度影响不大，但是对于动态响应阶段的稳定性有较大影响。

3.3 采样周期对路径跟踪的影响

设定控制器参数为：Np=30，Nc=2，v=3 m/s，T分别为0.01、0.02、0.03 s时，在导航坐标系下路径跟踪结果如图6所示。

控制器的控制精度取决于预测模型的精度，而本文对于预测模型求解采用的是欧拉法，所以预测模型的精度与采样周期T直接相关。由图6可以看出，随着采样周期的变长，路径跟踪动态响应阶段的横向跟踪偏差变小，纵向跟踪偏差变大；在路径跟踪过程中，跟踪偏差则会随着采样周期的变长而变大。由此看出，在仿真条件下采样时间的增大会降低路径跟踪的精度，同时会提高路径跟踪动态响应阶段的稳定性。

3.4 预测时域对路径跟踪的影响

设定控制器参数为：Nc=2，v=3 m/s，T=0.03 s，Np分别为15、30、45时，在导航坐标系下路径跟踪结果如图7所示。

由图7可以看出，预测时域越大横向跟踪偏差和前轮偏角变化幅度越小，这主要是因为预测时域较大时，控制器能够很好地预测系统未来输出，及时根据当前误差修正系统控制信号，但同时会增加相应的计算量。

图7 预测时域对路径跟踪的影响Fig.7 Effect of prediction horizon on path tracking

4 试验与结果分析

4.1 非线性模型预测控制器路径跟踪试验

试验场地为山东农业大学试验基地，试验现场如图8所示。

图9 试验结果Fig.9 Experiment result

图8 路径跟踪试验现场Fig.8 Tracking experiment scene

试验初始时，试验小车的质心位置与参考路径起点重合。设定控制器参数为Np=30、Nc=2、T=0.03 s，试验小车分别以1、2、3 m/s的速度沿参考路径行驶。试验分3组(每组进行1个速度的试验)，每组进行3次试验，取3次试验的平均值进行绘图。

由图9可以看出，虽然路径跟踪的横向偏差与纵向偏差有波动，但是试验小车依然可以有效地跟踪参考路径。路径跟踪的具体数据如表2所示。

由试验结果可以看出，试验结果并没有完全与仿真结果一致，偏差主要出现在路径方程改变的位置与圆形路径位置，如节点D、E、G、H处，由表2可以看出，其最大横向偏差为-4.28 cm，最大纵向偏差为-6.61 cm。出现较大偏差的原因一方面是受到试验条件的制约：小车各个机构之间的链接间隙、转向与速度的延迟、数据传输与处理的延迟、上位机(计算机)与下位机(STM32单片机) 之间的耦合等；另一方面是由于路径方程改变的位置大都处于直线路径的末端。此时小车在行驶过程中已经有了较大的累计偏差，此时路径方程的突然改变相当于把一个较大的横向与纵向偏差作用到小车上从而致使控制器无法有效跟踪参考路径。从中同样可以看出该控制器对于直线路段有着较好的控制作用，直线路段的最大横向偏差为1.40 cm，最大纵向偏差为-3.05 cm。

表2 节点处的具体参数值Tab.2 Specific parameter values at node cm

4.2 与线性模型预测控制器的对比试验

为了更好地验证本文提出的路径跟踪算法，将本文方法与线性模型预测控制方法进行对比，分别进行直线与圆形路径跟踪试验。设置主要控制器参数：Nc=2，T=0.01 s，Np=30，v为1、2、3 m/s时路径跟踪结果如图10所示。

对比试验表明，在速度不大于2 m/s的情况下，两种控制器在跟踪精度上没有明显差别；在v=3 m/s跟踪圆形路径时，线性模型预测控制器下的试验小车出现了较大偏差，横向跟踪偏差均值为12.09 cm;纵向跟踪偏差均值为38.08 cm。这是由于此时小车的非线性特性突出，控制器依然采用线性模型作为预测方程预测小车下一时刻运动就会导致较大跟踪偏差。在本文所述控制器下小车横向跟踪偏差均值降低了36.8%，纵向跟踪偏差均值降低了32.98%。由此可见，在速度较高时，本文所述控制器在跟踪精度上有较大优势。

5 结论

(1)针对农用车线性化建模控制精度较低，在系统非线性特性突出时预测效果较差等问题，提出非线性模型预测控制。利用欧拉法建立预测模型；通过反馈矫正、滚动优化完成控制器的闭环设计。

图10 对比试验结果Fig.10 Contrast experiment results

并对其进行了仿真与试验验证，结果表明该方法具有计算简单、精度高的特点。

(2)从仿真结果可以看出，在参考路径的轨迹方程改变时，农用车会产生一个不利于行驶稳定性的较大偏差与波动；在行驶速度不大于3 m/s时，速度的增大对跟踪精度影响不大,但是对于行驶稳定性有较大影响。采样时间的增大会降低路径跟踪的精度，同时会提高动态响应阶段的稳定性；预测时域的增加可以提高跟踪精度。

(3)在场地试验中，试验小车的最大横向跟踪偏差-4.28 cm发生在速度为2 m/s时，最大纵向跟踪偏差-6.61 cm发生在速度为3 m/s时。从试验结果中可以看出，该控制器无论是从路径跟踪的精度还是稳定性上都可以较好地满足农用车辆路径跟踪的实际要求。