刘正铎 张万枝 吕钊钦 郑文秀 穆桂脂 程向勋
(1.山东农业大学机械与电子工程学院, 泰安 271018; 2.山东省园艺机械与装备重点实验室, 泰安 271018)
自动驾驶技术在农业机械研究中的应用[1-8]越来越受到学者们的重视。农用运输车辆需要快速、准确地跟踪到期望路径。现阶段农用运输车主要是人工驾驶,不仅工作效率低,而且还加大了劳动人员的工作强度。文献[8]利用GPS与电子罗盘实现了拖拉机的自动导航,结果表明,平均偏差率为14.38%,最大偏差率为31.10%,GPS信号接收效果及定位精度均不理想。这是由于果树的枝叶遮挡GPS接收机,使其不能稳定接收卫星信号而造成的。为提高定位精度,RTK-DGPS高精度自动导航系统在实际农业生产中得到广泛应用[9-12],文献[13]采用 RTK-DGPS定位技术,设计了东方红X-804型拖拉机的自动导航系统,通过田间试验验证了控制系统的精确性和可靠性。文献[14]通过改进纯追踪模型的控制方法,在仿真条件下得到了较好的跟踪结果,但是其控制器的鲁棒稳定性较差,没有一套完善的参数调节标准,难以应用于实际生产。文献[15]基于最小二乘法实现了车辆的参数辨识,基于GNSS设计了路径跟踪模糊控制器,但是在实际应用过程中跟踪偏差较大。文献[16]利用模糊控制调整纯追踪模型的前视距离,设计了水稻插秧机的路径跟踪控制器,试验证明,该方法在速度0.3 m/s与1 m/s的情况下具有很好的适应性。
由此可以看出,由于田间的作业环境复杂(树木的枝叶遮挡信号、高温、沙尘、地面不平等),农用车辆的路径跟踪问题难点主要在于跟踪精度与跟踪稳定性上[17-19]。
模型预测控制被广泛应用于车辆自动导航[20]、主动前轮转向[21]等方面,它能及时弥补模型失配、时变、干扰等因素引起的不确定性。文献[22-24]从理论角度详细阐述了将线性模型预测控制器应用到汽车导航的可能性并进行了相关的仿真试验,但其中并未涉及控制器的鲁棒性分析及证明,更没有通过实物验证。针对上述问题,本文将模型预测控制与农用运输车辆相结合,设计导航控制器,实现农用运输车辆的路径跟踪。本文所述导航控制器能够通过内部积分器实时计算农用车辆的位姿信息,实现对所设计路径的有效跟踪,通过滚动优化实时反馈,增强控制系统稳定性,减少对外部环境的依赖。
对农用运输车进行运动学分析,建立如图1所示的模型。该模型前轮转向,后轮驱动。在整个运动学分析过程中,将农用运输车视为刚体,运行在水平面上。为了确定车辆在整个运行轨迹中的位姿,建立导航坐标系。选取农用运输车后轴中心点为参考点并定义小车的位姿信息(x,y,φ),其中(x,y)为农用运输车后轴中心坐标,φ为农用运输车航向角。在不考虑车辆侧倾、俯仰、侧滑等运动的情况下,可将低速行驶的农用运输车简化为二自由度的运动学模型[7]。其运动学方程为
(1)
式中l——轴距δ——前轮转角
v——后轴中心速度
图1 农用运输车的运动学模型Fig.1 Kinematic model of agricultural transport vehicle
为进一步提高定位精度,减少定位过程中的外界干扰,通过控制器中的内置积分器来处理传感器收集到的速度与转角信息,通过对速度与转角的不间断积分得到农用运输车的实时位姿为
(2)
式中Xt、Yt、φt——t时刻农用车的实际位姿
x0、y0、φ0——农用车的初始位姿
用r表示目标轨迹,xr、yr、φr为目标位姿,vr、δr为目标速度和目标前轮转角,农用运输车的线性误差模型可以表示为
(3)
其中,控制增量为
Δv=v-vrΔδ=δ-δr
状态偏差为
xe=x-xrye=y-yrφe=φ-φr
为将该模型应用到控制器,对式(3)进行离散化处理并写成状态空间方程形式
(4)
其中
ξt=(x,y,φ)ut=(v,δ)
式中ut(k)——t时刻递推k次系统控制量
ξt(k)——t时刻递推k次系统状态量
Ak,t——t时刻递推k次系统矩阵
Bk,t——t时刻递推k次输入矩阵
ηt(k)——t时刻递推k次系统输出量
k——离散变量T——采样周期
t——采样时刻
农用运输车辆在田间行驶时会受到各种各样的扰动,为使农用运输车稳定性达到使用要求,对式(3)进行鲁棒性分析。
定义新的状态变量为
(5)
于是将证明式(3)的收敛性转换为证明式(5)的收敛性。定义李雅普诺夫函数为
(6)
求导得
(7)
其中Δv、Δδ的取值与控制器参数采样周期T有关,为了便于证明,令
(8)
将式(8)代入式(7)得
(9)
根据李雅普诺夫第二法可以得出当T>0时,V、φe收敛,于是可以得到xe、ye、φe收敛且收敛于0。
农用运输车辆在田间行驶过程中的各种扰动,最终都可以归结为沿x方向的偏差dx与沿y方向的偏差dy,将dx、dy代入式(3)得
(10)
将式(8)、(10)代入式(7)得
(11)
用杨氏不等式对式(11)右侧进行处理得
由以上分析可知当采样周期T>0时,农用运输车在没有外界干扰情况下即可实现路径跟踪,当采样周期0
为得到系统的预测模型,构建状态变量为
λ(k|t)=[ξ(k|t)u(k-1|t)]
(12)
将式(12)代入式(4)得到新的状态空间表达式
(13)
式中n——状态量维度m——控制量维度
ΔU——控制增量
Im——m维单位矩阵
O——零矩阵
从而得到输出方程即系统的预测模型为
Γ(t)=ψξ(t|t)+ΘΔU(t)
(14)
其中
式中 Δu——系统控制量增量
Nc——系统控制时域
Np——系统预测时域
为将路径跟踪问题转换为二次规划问题,构造目标函数[25-26]。通过权重系数将系统状态量偏差与控制量偏差关联起来,设置状态量与控制量的约束条件,通过有效集法把每步迭代中有效约束作为等式约束进行求解,重复此过程寻得最优解,同时引入松弛因子防止系统出现无可行解现象。目标函数为
(15)
其中 ΔΓ=Γ-ΓrΓmin≤Γ(t)≤Γmax
ΔΓmin≤ΔΓ(t)≤ΔΓmax
Γr(k)=[ηr(t+1|t) …ηr(t+Np|t)]T
umin≤u(t+k)≤umax
Δumin≤Δu(t+k)≤Δumax
式中ρ——权值系数ε——松弛因子
Q——权重矩阵R——权重系数
J——目标函数
Γmin、Γmax——状态变量约束最值
ΔΓmin、ΔΓmax——状态变量增量约束最值
umin、umax——控制量约束最值
Δumin、Δumax——控制增量约束最值
文中采用内点法进行求解,将t时刻控制输入量的第1个元素作为实际控制量。在t+1时刻,重复以上控制过程实现最优控制。
图2为农用无人驾驶车辆从O点到G点的参考轨迹。农用车辆从O点进入,对其进行喷药或果实运输,G点为终点。其中A、B、C、D、E、F、G、H、J、K、L为路径方程改变的节点,在一定程度上其各项参数可以反映出控制器对该路段的路径跟踪性能。其中定义A点为路径跟踪动态响应阶段最大超调量的坐标点。定义F点为扰动测试点,在此位置加入扰动以测试控制器的鲁棒稳定性。基于Matlab平台进行模型预测控制器设计,从采样周期T、横向偏差扰动Yr、前轮转角扰动δfr等3方面进行仿真分析。
图2 参考路径Fig.2 Reference path
根据路径信息(参考路径的坐标范围与小车转向时前轮转角的取值范围设定目标函数的极值约束)与目标速度设置控制器参数如下:l=1;Q=diag(100,100,100),R=100;umin=[-3.2 m/s -0.5 rad]T,umax=[3.2 m/s 0.5 rad]T;Δumin=[-0.05 m/s -0.47 rad]T, Δumax=[0.05 m/s 0.47 rad]T;Γmin=[-5 -1 -0.5]T,Γmax=[5 1 0.5]T;ΔΓmin=[-0.05 -0.05 -0.008 2]T,ΔΓmax=[0.05 0.05 0.008 2]T。
设定控制器参数为:Nc=30,Np=60,v=2 m/s,T为0.05、0.10、0.15 s。在导航坐标系下,路径跟踪结果如图3所示。
图3 采样周期对路径跟踪的影响Fig.3 Effect of sampling period on path tracking
可以看出,3种不同采样周期下的控制器路径跟踪效果均较好。图3b农用运输车的纵向跟踪偏差,反映农用运输车路径跟踪的实时性,在AC、EG、JL直线段出现稳态纵向跟踪偏差且偏差随采样周期T的增大而增大,这说明车辆在跟踪圆形路径时相对于仿真路径产生了延迟,且延迟随采样周期的增大而增大。图3c为农用运输车的横向跟踪偏差,反映了农用运输车的跟踪精度,可以看出节点E、J处横向偏差出现较大波动,这是由于路径方程改变引起的,此时控制器需要重新预测控制量以适应新的参考路径。图3d反映出路径跟踪时农用运输车的行驶稳定性,可以看出,在仿真条件下农用运输车可以稳定地跟踪参考路径。
设定控制器参数为:Np=60,Nc=30,v=2 m/s,T=0.05 s,Yr为0.5、1.0、1.5 m。在导航坐标系下,路径跟踪结果如图4所示。
图4 横向扰动对路径跟踪的影响Fig.4 Effect of lateral disturbance on path tracking
当农用运输车在果园运输时,车辆会进行人为的横向移动,将此移动作为扰动加入路径分析。在节点F处加入横向偏差扰动,可以看出,在3种不同横向偏差的扰动作用下控制器的鲁棒性均较好,随着扰动的增大,跟踪偏差也在增大;当扰动出现时,控制器会快速地做出反应,及时调节前轮转角与速度,使车辆尽快回到参考路径上行驶。
设定控制器参数为:Np=60,Nc=30,v=2 m/s,T=0.03 s,δfr为5°、10°、15°。在导航坐标系下,路径跟踪结果如图5所示。
图5 前轮转角扰动对路径跟踪的影响Fig.5 Effect of front wheel angle on path tracking
当农用运输车在果园间行驶时,由于地面不平等因素会造成车辆在行驶过程中前轮转角的改变,在F点加入前轮转角扰动。对路径跟踪情况进行分析。从图5可以看出,跟踪偏差随着前轮偏角扰动的增大而增大,此时控制器会快速地做出反应,及时调节前轮转角,使车辆尽快地回到参考路径上行驶。
自主设计的田间路径跟踪试验小车如图6所示,具体结构参数如表1所示。
路径信息采集模块:转角传感器为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型单片机,其主要任务是采集试验小车转速传感器与前轮转角传感器收集来的信息并转换成数字信号,通过Wi-Fi传输至导航控制器上位机戴尔Vostro计算机中的Matlab软件,从而进行算法处理。
图6 试验小车Fig.6 Test car1.前轮转角传感器 2.速度传感器 3.差速驱动装置 4.前轮转向驱动器 5.信号转换装置 6.蓄电池 7.后轮驱动器 8.前轮转向装置
参数数值/方式外形尺寸(长×宽×高)/(cm×cm×cm)120×63×33轴距/cm100最大速度/(m·s-1)3.2 质量/kg50控制方式PWM轮胎半径/cm15
试验场地为山东农业大学本部紫叶李树林,如图7所示。场地尺寸及树木位置与仿真时(图2)一致。
图7 山东农业大学紫叶李树林Fig.7 Prunus cerasifera forest in Shandong Agricultural University
初始时,车辆中心位置与路径起点重合。设定控制器参数为v=2 m/s、Np=60、Nc=30、T=0.05 s。为了更好地验证本文所述控制器的鲁棒性,向节点F处添加前轮转角扰动δfr为5°、10°、15°。试验结果如图8所示。
由图8可以看出,场地试验中小车在跟踪参考路径时,直线路段偏差较小,圆形路段与加入前轮转角扰动时偏差较大,试验小车可以有效地跟踪参考路径。其具体数据如表2所示。
由试验结果可以看出,不同转角扰动下的最大横向偏差主要出现在节点C处与添加扰动节点F处,分别为19.56、20.97、23.89 cm,出现较大偏差一方面是因为受到试验条件的制约:小车各个机构之间的链接间隙、轮胎打滑、数据传输与处理的延迟、上位机 (计算机上的Matlab)与下位机(STM32单片机) 之间的耦合等;另一方面是因为路径方程改变的位置大都处于直线路径的末端,此时小车在行驶过程中已经有了较大的累积偏差,再加上路径方程的突然改变,致使控制器无法在短时间内解出最优控制量。
图8 场地试验中前轮转角扰动对路径跟踪的影响Fig.8 Effect of front wheel angle disturbance on path tracking in field test
cm
通过试验可以看出,该控制器对于直线路段有着较好的控制作用,在控制精度上,v=2 m/s时直线段最大横向偏差为10.57 cm,均值保持在8.49 cm以内;在跟踪实时性上,最大纵向偏差为25.77 cm,均值保持在18.47 cm以内。试验小车跟踪圆弧段的偏差较大,最大横向偏差为22.83 cm,最大纵向偏差为39.42 cm,这主要是由于控制器中采用线性模型作为预测方程造成的,在跟踪圆形路径时,受轮胎侧偏、车辆质心变化等因素的影响,小车非线性特性突出,此时控制器依然采用线性模型预测小车下一时刻运动,与实际情况差别较大,造成了较大的跟踪偏差。
在添加前轮转角扰动的节点F处,横向偏差随扰动的增大而增大,当δfr=15°时,横向偏差最大为23.89 cm,纵向偏差最大为62.53 cm,说明试验小车在跟踪精度上偏离参考轨迹23.89 cm,在跟踪实时性上,落后仿真路径62.53 cm。由图8可以看出,此时控制器可以迅速起到调节作用,使小车回到参考轨迹继续行驶。
由此可以看出本文所述控制器无论是在行驶稳定性上,还是在跟踪精度上,都可以满足农用运输车辆对路径跟踪的要求。
与现有研究进行对比,文献[27-28]在未使用外部定位的情况下实现了拖拉机的自动导航,减少了控制器对外部环境的依赖。其中文献[27]结合最优控制理论,设计了基于速度和转向角的双参数最优控制算法,横向偏差最大值为12 cm;文献[28]设计一种基于指数趋近律的滑模变结构控制器,速度为2 m/s时,横向偏差最大值为11 cm。分析可知:本文采用的线性模型预测控制方法,在跟踪精度上要优于文献[27-28],同时利用农用车辆的纵向跟踪偏差分析了路径跟踪的实时性,并对控制器的鲁棒性做了详细研究,通过仿真与试验验证了农用车路径跟踪在添加了外部扰动情况下的行驶稳定性。
(1)针对农用运输车辆作业环境复杂,路径跟踪稳定性较差等问题,结合线性模型预测控制理论设计了农机自动导航控制器。该控制器通过内部积分器实现了农用车的定位,通过引入松弛因子解决了运算过程中出现非可行性解的问题,从而实现农机设备的自动导航。
(2)通过构建车辆误差模型的李雅普诺夫函数,得到系统采样周期T与车辆鲁棒稳定性的关系,并通过仿真与试验验证了本文所述控制器在添加外界扰动的情况下依然可以有效跟踪参考路线。
(3)在仿真中,分析了采样周期T、横向扰动Yr、前轮转角扰动δfr对路径跟踪稳定性的影响;在场地试验中,v=2 m/s无扰动情况下试验小车最大横向偏差为22.83 cm,均值为8.49 cm,基本实现了农用车辆的自动导航;前轮转角δfr扰动下的最大横向跟踪偏差为23.89 cm,均值为8.49 cm,偏差随扰动的增大而增大,控制器可以迅速起到调节作用,使小车回到参考轨迹继续行驶。