0 引言
随着科学技术的不断进步和武器装备的日益现代化,出现了一些建立在新科学理论和技术基础上的新概念武器。无人弹药补给车作为具有侦查、运输等功能的执行军事任务的机器人系统,是无人作战平台家族的重要成员[1]。无人弹药补给车辆的自动驾驶系统通常由四部分组成:环境感知、定位导航、路径规划和控制决策,其中车辆控制算法是整个自动驾驶系统中最为基础关键的部分之一,决定了无人弹药补给车执行作战任务时的可靠性和准确性。
目前有很多车辆控制算法在无人车辆上得到应用,如纯跟踪算法、线性二次调节器(LQR)、PID控制算法、Stanley控制和模型预测控制(MPC)等[2]。纯跟踪算法大多数运用的是无人车的运动学几何模型,由于几个模型的反应速度会比较慢,因此这种算法大多是在低速的状态下才能得到比较好的路径跟踪控制效果[3]。LQR在使用的过程中没有考虑无人车的动力学因素和约束条件,不能达到非常理想的控制效果[4]。而模型预测控制是带约束优化滚动求解问题的控制方法,该方法是通过建立控制系统的动力学模型进而预测未来一段时间内系统的输出,通过求解带约束的最优控制问题使得系统的跟踪误差最小[5]。这种方法能减小系统时滞所带来的误差,具有提高系统性能的优点。梁政焘等[6]考虑了控制约束和状态约束,提出了基于车辆二自由度运动学模型的轨迹跟踪控制方法。高洪波等[7]提出了智能车辆开环最优控制的离散模型预测控制横向轨迹跟踪的方法。江浩斌等[8]提出了一种智能汽车非时间参考的螺旋弯曲坡道路径跟踪算法。上述模型预测控制方法在控制过程中通常都假定简单道路地形,忽略了道路曲率的影响,在大曲率情况下跟踪偏差较大。
本文基于改进的动力学约束模型预测控制,建立魔术公式轮胎模型和车辆动力学模型,对大曲率工况进行了横向稳定性仿真和试验验证。
1 无人弹药补给车运动控制方案
车辆路径跟踪控制方案流程如图1所示。首先通过环境感知技术,获得车辆当前的姿态信息(包括车辆的横向位置、航向角等),结合路径规划系统给出的目标路径信息,基于车辆当前的状态信息,通过模型预测控制算法计算出前轮理想转向角,通过编写的上位机软件,将转向角发送到转向执行机构,控制车辆前轮转动相应的角度,同时在车辆运动过程中实时反馈的状态信息对车辆自身位置不断调整,从而完成路径跟踪控制[9-10]。
图1 车辆路径跟踪控制流程
Fig.1 Vehicle path tracking control flowchart
2 模型预测控制器设计
模型预测控制包括预测模型、滚动优化、反馈校正3个基本部分[11]。在此基础上设计了基于动力学约束的模型预测路径跟踪控制器。
2.1 预测模型
2.1.1 轮胎模型及其线性化
车辆在运动过程中,轮胎所受的纵向力、侧向力、垂直力及回正力矩对车辆稳定性起着至关重要的作用。由于车辆轮胎结构复杂,动力学性能呈非线性,选取符合实际的轮胎模型至关重要。采用以魔术公式为基础的半经验轮胎模型[12]。魔术公式的一般表达式为
Yx=Dsin{Cartan[Bx-E(Bx-arctan(Bx))]}
(1)
式(1)中: 系数B为刚度因子,C为形状因子,D为峰值因子,E为曲率因子,由轮胎的垂向载荷和外倾角确定;Y为输出变量,可以是纵向力或侧向力或者回正力矩;x为输入变量,在不同情况下分别表示为轮胎的侧偏角或纵向滑移率。魔术轮胎的输入量与输出量之间的关系如图2所示。
图2 输入量与输出量关系
Fig.2 Relationship between input and output
由于纵向力对轮胎影响较小,可忽略不计,主要关注轮胎的侧向力,利用魔术公式计算轮胎侧向力的公式如下:
Fy=Dsin{Carctan[Bx-E(Bx-arctan(Bx))]}+SV
(2)
2.1.2 车辆动力学模型的建立
车辆动力学模型在车辆高速行驶下有更高的可靠性,并且需要考虑轮胎的受力情况和侧偏特性[13]。因此车辆动力学模型更加复杂但同时精确度也更高,由于模型过为复杂会使计算效率低下,对车辆模型进行适当简化,并做出以下合理假设:
1) 假设车辆在平坦路面行驶,忽略车辆的垂向运动。
2) 假设车辆及悬挂系统是刚性的,忽略悬架运动及其对耦合关系的影响。
3) 假设车辆为前轮转向且两轮转角相同。
4) 不考虑轮胎的横向载荷转移。
5) 忽略空气动力学影响[14]。
基于上述假设建立建立如图3所示三自由度车辆动力学模型。XOY为大地坐标系,xoy为车身坐标系。其中为前轮转角,为后轮转角且恒为0。分别表示车辆前后轮受到x轴方向的力;分别表示车辆前后轮受到y轴方向的力;分别表示前后轮受到的纵向力;分别表示车辆前后轮受到的侧向力。
图3 三自由度车辆动力学模型
Fig.3 Three degree of freedom vehicle dynamics model
分别得到沿x轴、y轴和z轴的力平衡方程:
在x轴上
(3)
在y轴上
(4)
在z轴上
(5)
式(3)中: (a,b)分别为前后轴到质心距离,m为整车整备质量,Iz为车辆绕z轴的转动惯量。
轮胎在x轴方向和y轴方向受到的合力与纵、侧向力关系如下:
Fxf=F1fcosδf-Fcfsinδf
(6)
Fxr=F1rcosδf-Fσsinδf
(7)
Fyf=F1fsinδf-Fdcosδf
(8)
Fyr=F1rsinδf-Fcrcosδf
(9)
轮胎的纵向力和侧向力可以表示为轮胎侧偏角、滑移率、路面摩擦系数和垂直载荷等参数的复杂函数:
F1=f1(α,s, μ,Fz)
(10)
Fc=fc(α,s, μ,Fz)
(11)
其中: α为轮胎侧偏角,s为滑移率,Fz为轮胎所收到垂直载荷,为路面摩擦系数。
轮胎的侧偏角可由几何关系计算得到:
(12)
式(12)中, vc和v1为侧向和纵向速度,可用坐标系方向的速度vx和vy表示:
v1=vysinδ+vxcosδ
(13)
vc=vycosδ+vxsinδ
(14)
其中, δ为轮胎转角。轮胎的速度一般可以通过车辆速度计算得到:
(15)
(16)
轮胎在地面上的滑移率s可以由以下算式计算
(17)
式(17)中: wt为车辆轮胎角速度,r为车轮半径。
可以通过以下算式得到车辆前后轮胎所受到的垂直载荷为
(18)
(19)
最后,车辆自身坐标系和大地坐标系存在转换关系,作如下变换:
(20)
(21)
结合以上式,得到车辆的非线性动力学模型。将系统描述为以下的状态空间表达式:
(22)
η=hdyn(ξdyn)
(23)
在该系统中,状态量选为
控制量选为udyn,输出量选为ηdyn=[φ,Y]T。该模型即为预测模型的基础,通过动力学模型建立的状态空间系统是一个非线性系统,需要对其进行线性化处理,得到:
(24)
式(24)中,Δξ=ξ-ξr, Δu=u-ur
(25)
其中
(26)
(27)
然后对其进行离散化处理,得到:
ξ(k+1)=A(k)ξ(k)+B(k)u(k)
(28)
式(28)中,A(K)=I+TA(t), B(K)=I+TB(t),T为一个采样周期。
2.2 滚动优化
2.2.1 目标函数设计
车辆动力学模型复杂程度较高,且加入了更多的约束条件,因此在控制器实际执行过程中,很可能出现在规定的计算时间内无法得到最优解的情况[15]。因此有必要在目标函数中加入松弛因子,其表达式如下:
(29)
综合以上目标函数和约束条件,基于动力学模型的路径跟踪控制器在每个控制周期内要解决如下优化问题:
(30)
其中: yhc为硬约束输出,ysc为软约束输出,yhc,min和yhc,max为硬约束极限值,ysc,min和ysc,max为软约束极限值。
2.2.2 约束条件设计
为使得模型更加精准,本文对车辆动力学模型控制量和控制增量进行相关约束,同时还对质心侧偏角、车辆附着条件和轮胎侧偏角进行约束。
1) 质心侧偏角约束质心侧偏角对车辆的稳定性具有较大影响,故必须将质心侧偏角限定在合理的范围内。在附着良好的干燥沥青路面。车辆稳定行驶的质心侧偏角极限可达±12°;而在附着系数低的冰雪路面,极限值近似±2°[16]。因此将质心侧偏角的约束条件设置为
(31)
2) 车辆附着系数约束
汽车动态性能不仅受到驱动力的制约,还受到轮胎与地面附着条件的限制。因此有必要添加对车辆附着系数条件的约束[17]。当车辆纵向匀速行驶时,侧向加速度ay和附着系数μ满足以下关系:
|ay|≤μg
(32)
约束条件过为宽松会导致侧向加速度太大,影响车辆的稳定性;约束条件限定太窄会出现运算过程求解失败的现象。故将该约束条件设定为软约束条件:
-ε≤ay≤+ε
(33)
式(33)中,ay,min、ay,max为车辆加速度极限约束。
3) 轮胎侧偏角约束
根据轮胎侧偏特性可知,当轮胎侧偏角小于5°时,侧偏角和侧偏力成线性关系,故需对侧偏角进行约束,根据小角度假设,做出如下约束:
-2.5°<α<2.5°
(33)
2.2.3 反馈校正
在每个控制周期内完成式(10)的求解后,可以得到控制时域内的一系列控制输入增量和松弛因子:
(34)
将该控制序列中的第一个元素作为实际的控制输入增量作用于系统,即
(35)
进入下一个控制周期后,重复上述过程,实现对目标路径的跟踪控制。
3 仿真与实车验证
3.1 模拟仿真
在Carsim中完成车辆的相关动力学参数、输入以及输出接口设置,在S函数中编写模型预测控制算法,在Simulink仿真中搭建的动力学仿真控制模型如图4所示。
图4 动力学仿真模型
Fig.4 Dynamic simulation model
仿真车辆具体参数与试验车辆实际参数一致,如表1所示。
表1 整车参数
Table 1 Vehicle parameters
参数数值车身尺寸/mm3 000×1 900×760车辆质量m/kg1 617.6轴距/mm2 287前后轮中心到质心的距离/mm1 117质心高度/mm664.2绕x轴转动惯量/(kg·m2)2 027.9绕y轴转动惯量/(kg·m2)1 332.4绕z轴转动惯量/(kg·m2)1 585.1前后轮滑移率0.2前轮轮胎侧偏刚度/(N·rad-1)67 800后轮轮胎侧偏刚度/(N·rad-1)63 900
为了验证车辆在大曲率路面的跟踪能力,在高速情况下进行联合仿真实验,分别在40、60、80 km/h的速度下进行了路径跟踪测试。跟踪轨迹选用具有大曲率连续转弯的双移线轨迹。双移线轨迹是一种经典工况,在测试车辆稳定性和无人驾驶轨迹跟踪仿真分析中应用广泛[18]。双移线轨迹的曲率变化如图5所示。基于传统的模型预测控制算法路径跟踪效果和改进的模型预测控制算法在速度40、60、80 km/h的路径跟踪效果对比如图6所示。改进与未改进算法的误差对比效果如图7所示。
图5 双移线轨迹曲率变化
Fig.5 The change of curvature of double track
图6 仿真路径对比
Fig.6 Comparison of simulation paths
图7 仿真误差对比
Fig.7 Comparison of simulation errors
由图6可知,车辆进入第1个弯道时(曲率为-0.011 2 rad/m),2种算法都表现出很好的跟踪效果,随着曲率逐渐增大,跟踪误差在2个曲率峰值点(-0.03 46 rad/m,0.050 1 rad/m)附近达到最大,并且随着车辆速度的提高,车辆的横向误差也在逐渐增大,此时加入动力学约束的跟踪控制器偏差明显小于未加入动力学约束的跟踪控制器。由图7可见,传统的模型预测控制器在40、60、80 km/h时的横向误差分别为0.28、0.44、0.80 m;改进后的模型预测控制器在40、60、80 km/h时的横向误差分别为0.15、0.18、0.22 m。跟踪横向误差分别下降了46.4%、59.1%和72.5%。通过分析可知,改进后的模型预测控制器跟踪能力更好,稳定性更强。并且随着速度的增大,表现出的纠偏能力随之增大。
3.2 跟踪控制试验
采用前轮转向,四轮驱动车辆,车辆定位系统由GPS-惯性组合导航与无线终端设备(DTU)组成,对车辆的定位精度可达0.05 m,车辆转弯半径为6 m。车辆底盘如图8所示,车辆定位系统如图9所示。由于客观条件限制,试验只能在有限的校园环境中进行,跟踪轨迹工况很大程度受到影响。因此试验中车辆速度控制在30 km/h左右。
图8 无人弹药补给车底盘
Fig.8 Unmanned ammunition supply vehicle chassis
图9 定位系统
Fig.9 Positioning system
试验对大曲率路径包括多弯道路径和圆弧路径进行了路径跟踪实验验证。实验过程中,首先利用高精度定位系统对期望轨迹进行原始的数据采集,再经过数据处理实现坐标转化以及轨迹平滑,将车辆跟踪轨迹与期望轨迹进行对比,分析改进的模型预测控制器的跟踪精度及可靠性。
车辆跟踪多弯道路径的参考路径和实际路径对比如图10所示,车辆在曲率较小处跟踪效果良好,在曲率较大处,产生的横向误差逐渐增大。从图11横向跟踪误差可以看出,车辆进入弯道前,横向跟踪误差最大为0.18 m;车辆进入弯道后在15～20 m处跟踪误差达到最大,为0.47 m,跟踪效果良好。车辆跟踪圆弧路径的路径对比如图12所示,误差如图13所示。可以看出,由于圆弧路径曲率变化较小,横向跟踪误差没有明显突变,稳定在0.2 m左右,最大误差控制在0.35 m以内,具有较高的跟踪精度。
图10 多弯道路径跟踪
Fig.10 Multi-curve path tracking
图11 多弯道路径跟踪误差
Fig.11 Multi-curve path tracking error
图12 圆弧路径跟踪
Fig.12 Arc path tracking
图13 圆弧路径跟踪误差
Fig.13 Arc path tracking error
根据2种工况下的试验情况表明:加入动力学约束的模型预测控制器能够对多弯道路径、圆弧路径进行较好的跟踪,跟踪过程中产生的横向跟踪误差都在理想范围内。
综上所述,无人弹药补给车辆路径跟踪控制系统运行基本良好,试验车在一定的误差范围内能保持平稳运行,且改进后的模型预测控制器相比未改进的模型预测控制器在大曲率处的跟踪误差更小。提高了车辆在复杂道路环境下的跟踪精度。充分证明本文设计的模型预测控制算法能够满足复杂路径跟踪控制的稳定性、实时性以及准确性要求。
4 结论
本文针对无人弹药补给车辆路径跟踪问题,首先建立了车辆运动学模型,然后又结合魔术公式轮胎模型建立了考虑动力学特性的车辆动力学模型,接着采用模型预测控制理论设计了基于动力学的改进的模型预测控制器,并通过联合仿真平台进行验证分析,最后以无人弹药补给车为试验车,通过定位模块对控制系统进行试验调试,最后结果表明,无人弹药补给车可以满足在大曲率复杂路面上的行驶要求。
[1]谢磊.某军用无人车辆全向底盘设计与性能仿真研究[D].南京:南京理工大学,2018.XIE Lei.Research on design and performance simulation of all-wheel independent suspension for military unmanned vehicles[D].Nanjing:Nanjing University of Science and Technology,2018.
[2]余世全.无人驾驶汽车的发展现状和相关建议[J].专用汽车,2023(1):11-14.YU Shiquan.Development status and relevant suggestions for autonomous vehicles[J].Special Purpose Vehicles,2023(1):11-14.
[3]LIRAN S,KANGWEN S,XIAO G,et al.Prescribed-time error-constrained moving path following control for a stratospheric airship with disturbances[J].Acta Astronautica,2023,212(8):307-315.
[4]宋晓华.复杂工况下自动驾驶车辆轨迹跟踪控制研究[D].重庆:重庆交通大学,2020.SONG Xiaohua.Research on trajectory tracking control of autonomous vehicles under complex operating conditions[D].Chongqing:Chongqing Jiaotong University,2020.
[5]陈慧岩,陈舒平,龚建伟.智能汽车横向控制方法研究综述[J].兵工学报,2017,38(6):1203-1214.CHEN Huiyan,CHEN Shuping,GONG Jianwei.Overview of research on lateral control methods for intelligent vehicles[j].Acta Armamentarii,2017,38(6):1203-1214.
[6]梁政焘,赵克刚,裴锋,等.基于MPC的智能车轨迹跟踪算法[J].机械与电子,2019,37(1):66-70.LIANG Zhengtao,ZHAO Kegang,PEI Feng,et al.Intelligent vehicle trajectory tracking algorithm based on MPC[J].Mechanical and Electronic,2019,37(1):66-70.
[7]GAO H B,CHENG B,WANG J Q,et al.Object classification using CNN-based fusion of vision and LIDAR in autonomous vehicle environment[J].IEEE Transactions on Industrial Informatrics.2018,14(9):4224-4231.
[8]江浩斌,张旭培,马世典.智能汽车非时间参考的螺旋弯曲坡道路径跟踪控制[J].重庆理工大学学报(自然科学),2018,32(12):1-6.JIANG Haobing,ZHANG Xupei,MA Shidian.Non-time-referenced path tracking control of spiral curved ramp for intelligent vehicle[J].Journal of Chongqing University(Technology Science),2018,32(12):1-6.
[9]YANG Y,YANKAI L,XING W,et al.An optimal goal point determination algorithm for automatic navigation of agricultural machinery:Improving the tracking accuracy of the Pure Pursuit algorithm[J].Computers and Electronics in Agriculture,2022(194):12-21.
[10]WANG S,GUO J,MAO Y,et al.Research on the model predictive trajectory tracking control of unmanned ground tracked vehicles[J].Drones,2023,7(8):23-31.
[11]LIN B Y,SHAOFENG N,LINGXI L,et al.A comparison study of kinematic and dynamic models for trajectory tracking of autonomous vehicles using model predictive control[J].International Journal of Control,Automation and Systems,2023,21(9):3006-3021.
[12]李广南.基于模型预测控制的无人驾驶车辆轨迹跟踪[D].柳州:广西科技大学,2022.LI Guangnan.Unmanned vehicle trajectory tracking based on model predictive control[D].Liuzhou:Guangxi University of Science and Technology,2022.
[13]ZHEJUN H,HUIYUN L,WENFEI L,et al.A new trajectory tracking algorithm for autonomous vehicles based on model predictive control[J].Sensors,2021,21(21):7165-7165.
[14]肖世德,江海锋,杜金兰,等.基于两阶段纯追踪模型的农用车辆路径跟踪算法研究[J].农业机械学报,2023,54(4):439-446,458.XIAO Shide,JIANG Haifeng,DU Jinlan,et al.Research on agricultural vehicle path tracking algorithm based on two stage pure tracking model[J].Transactions of the Chinese Society for Agricultural Machinery,2023,54(4):439-446,458.
[15]柴善鹏,姚立健,徐丽君,等.基于动态前视距离纯追踪模型的温室农机路径跟踪研究[J].中国农机化学报,2021,42(11):58-64,79.CHAI Shanpeng,YAO Lijian,XU Lijun,et al.Research on path tracking of greenhouse agricultural machinery based on dynamic forward range pure tracking model[J].China Journal of Agricultural Machinery Chemistry2021,42(11):58-64,79.
[16]STEVE M,SHRIJIT S,MARTIN FRANCISCO,et al.Regulated pure pursuit for robot path tracking[J].Autonomous robots,2023(6):47-55.
[17]FUE K,PORTER W,BARNES E,et al.Autonomous navigation of a center-articulated and hydrostatic transmission rover using a modified pure pursuit algorithm in a cotton field[J].Sensors,2020,20(16):21-30.
[18]陈龙,罗杰,杨旭,等.基于模糊纯追踪控制的自动泊车算法研究[J].武汉理工大学学报(信息与管理工程版),2019,41(3):316-321.CHEN Long,LUO Jie,YANG Xu,et al.Research on automatic parking algorithm based on fuzzy pure tracking control[J].Journal of Wuhan University of Technology (Information and Management Engineering Edition),2019,41(3):316-321.