基于横纵向MPC的智能车换道控制算法_孙志伟.pdf

基金项目:国家自然科学基金(51505275)收稿日期:2021-05-18 修回日期:2021-05-24 第 40 卷 第 4 期计 算 机 仿 真2023 年 4 月 文章编号:1006-9348(2023)04-0461-08基于横纵向 MPC 的智能车换道控制算法孙志伟,李 聪(上海工程技术大学机械与汽车工程学院,上海 201620)摘要:为了保证智能车辆在不同车速以及极限工况下跟踪控制的精确性和稳定性,提出了一种结合横纵向控制的模型预测控制(MPC)的轨迹跟踪控制算法。首先建立基于车辆动力学模型的横向误差动力学模型以及车辆纵向控制模型,设计基于横纵向控制的 MPC 控制器,通过 MATLAB/Simulink 和 CarSim 进行仿真验证。仿真结果表明,控制器具有较好的跟踪精度和鲁棒性。将所设计的控制器与未添加纵向控制的 MPC 控制器进行对比分析,仿真结果表明,添加纵向控制后 MPC 控制器跟踪精度更优。关键词:智能车辆;横向控制;纵向控制;轨迹跟踪;模型预测控制中图分类号:U461 文献标识码:BLane Change Control Algorithm of Intelligent VehicleBased on Lateral and Longitudinal MPCSUN Zhi-wei,LI Cong(Shanghai University of Engineering Science,School of Mechanical and AutomotiveEngineering,Shanghai 201620,China)ABSTRACT:In order to ensure the accuracy and stability of tracking control of intelligent vehicles at differentspeeds and extreme conditions,a trajectory tracking control algorithm based on model predictive control(MPC)com-bined with lateral and longitudinal control was proposed.Firstly,the lateral error dynamics model based on vehicledynamics model and vehicle longitudinal control model were established,and the MPC controller based on lateral andlongitudinal control was designed,which was verified by Matlab/Simulink and CarSim simulation.Simulation resultsshow that the controller has better tracking accuracy and robustness.Then,on this basis,the designed controller wascompared with the MPC controller without longitudinal control,and the simulation results show that the MPC control-ler with longitudinal control has better tracking accuracy.KEYWORDS:Intelligent vehicle;Lateral control;Longitudinal control;Trajectory tracking;Model predictive control1 引言轨迹跟踪控制是智能车辆1研究领域的热点之一。轨迹跟踪是指通过控制车辆转向,使得车辆可以按照事先给定的参考轨迹的路线行驶2。在跟踪控制方面,其算法主要有滑模控制、模糊控制、鲁棒控制和模型预测控制等。文献3将增益调度、共享以及鲁棒性结合起来设计出一种控制器来进行轨迹的跟踪控制。文献4提出的滑膜综合控制器可以降低建模的不确定性和扰动时的鲁棒性。文献5利用神经网络的自学习和自调整特性,并结合模糊控制,设计了一种轨迹跟踪控制器。文献6针对实际交通环境下目标车道车速和加速度的动态变化,提出了基于轨迹预瞄的智能汽车变道动态跟踪控制策略。而 MPC 算法相比于其它控制算法来说,最大的优势是可以把多种约束添加到控制过程中,从而提高轨迹的跟踪精度。文献7利用 MPC 设计了一种主动转向控制器,使智能车辆在满足约束条件的情况下对参考的期望轨迹进行跟踪。文献8提出了一种轨迹规划和跟踪能在一个 MPC 优化问题中得到,实现了基于 MPC 算法的同步轨迹规划与跟踪。文献9提出了一种基于 MPC 的新型路径跟踪框架,是一个多层控制系统,包括三个具有固定速度的路径跟踪控制器和一个速度决策控制器,可以根据路径信息主动调整纵向速度,且有良好的跟踪精度。文献10提出一种基于自适应模164型预测控制的轨迹跟踪控制算法,能够提高低附着且变速条件下智能车辆轨迹跟踪控制精度和稳定性。文献11提出将侧偏角软约束与线性时变模型预测控制算法相结合,从而保证了车辆的稳定性,并且较好的实现了轨迹跟踪。文献12在线性变参数模型预测控制中综合考虑纵向和横向动力学,并且引入终端集概念,从而保证系统的稳定性。本文将五次多项式作为智能车辆的换道轨迹,以前轮转角和加速度补偿作为控制量,将车辆的横纵向控制与模型预测控制算法相结合,设计出一个整体的轨迹跟踪控制器用于跟踪参考的换道轨迹,并通过 Carsim 与 MATLAB/Simulink联合仿真对其进行验证,并且比较了加入纵向控制与不加纵向控制的跟踪控制精度的差异。2 模型的建立2.1 车辆动力学模型对于车辆动力学建模时的理想化假设:1)车辆在平坦路面上行驶,忽略车辆垂向运动;2)悬架系统及车辆是刚性的;3)只考虑纯侧偏轮胎特性,忽略轮胎力的纵横向耦合关系;4)用自行车模型来描述车辆运动,不考虑载荷的左右转移;5)假设车辆行驶速度变化缓慢,忽略前后轴的载荷转移;6)忽略纵向和横向空气动力学。基于上述假设,建立的车辆动力学模型如图 1。图 1 车辆动力学模型结合图 2 以及根据牛顿第二定律,分别沿 y 轴和 z 轴作受力分析,可得如下的车辆横向动力学模型may=m(y+vx?)=Fyf+FyrIz?=aFyf-bFyr(1)其中,Fxf,Fxr为地面给前轮胎和后轮胎施加的纵向力;Fyf,Fyr为地面给前轮胎和后轮施加的横向力;a 表示前轴长度;b 表示后轴长度;f表示前轮转角;m 为整车质量,Iz为车辆绕 z轴转动的转动惯量。考虑轮胎的侧偏特性Fyf=2CffFyr=2Crr(2)其中,f=f-a?+vyvx,r=b?-vyvx。将上式代入车辆动力学方程得y=-1m2(Cf+Cr)vx?y-(vx+2aCf-2bCrmvx)?+2Cfmf=-2aCf-2bCrIzvx?y-2a2Cf+2b2CrIzvx?+2aCfIzf(3)化为矩阵形式ddty?y?|=01000-2Cf+2Crmvx0-vx-2Cfa-2Crbmvx00010-2Cfa-2CrbIzvx0-2Cfa2+2Crb2Izvx|y?y?|+02Cfm02CfaIz|f(4)其状态变量和控制变量分别为X=y?y?|,U=f 各个状态量表示的含义分别为横向位置、横向位置变化率、横摆角以及横摆角变化率。根据式(4)就可以分析出在给定的前轮转角输入下,车辆的横向位移、横向速度、横摆角以及横摆角速度的响应,但是横向跟踪控制的目的是减小跟踪偏差,需要的状态方程是能够分析给定的前轮转角下车辆跟踪偏差的响应。因此,可以建立基于误差的动力学模型。假设 e1为横向误差,即车辆质心距车道线的距离;e2为航向误差,车辆纵向速度为 vx,车辆转弯半径为 R,结合图 1,可得:?des=vxR(5a)ay,des=v2xR=vx?des(5b)e1=ay-ay,des=y+vx(?-?des)(5c)?e1=?y+vx(-des)(5d)e2=-des(5e)其中,des为期望航向角,?des为期望转角速度,aydes为期望横向加速度,?e1为横向速度误差,e1横向加速度误差,e2为航向误差。将式(5)与式(3)结合,可得到车辆的横向误差动力学264模型:ddte1?e1e2?e2|=01000-2Cf+2Crmvx2Cf+2Crmvx-2Cfa-2Crbmvx00010-2Cfa-2CrbIzvx0-2Cfa2+2Crb2Izvx|e1?e1e2?e2|+02Cfm02CfaIz|f+0-vx-2Cfa-2Crbmvx0-2Cfa2+2Crb2Izvx|?des(6)2.2 纵向速度控制智能车辆在规划变道时,保持纵向方向速度恒定不变,那么在变道过程中,由于换道时会产生横向速度,此时智能车辆的合速度大于原来直行时的速度,这就会导致横向控制看起来能够跟踪期望轨迹,但是在时间上会落后期望轨迹,因此引入纵向控制13。纵向控制主要为速度控制,通过刹车、油门、挡位等实现对车速的控制。本文采用的车辆为自动挡,因此控制对象为刹车和油门。基于此本文搭建了基于MATLAB/Simulink 和 Carsim 联合仿真的纵向速度控制结构图,如图 2。图 2 纵向控制在纵向控制中,纵向误差的计算是其算法实现的核心。纵向误差包含两个状态量,即速度误差 ev和位置误差 es。原理图如图 3。图 3 纵向误差计算由图 3 可知,位置误差和速度误差分别为:es=-(dxcos des+dysin des)ev=Vxdes-Vxcos/k(7)3 轨迹跟踪控制3.1 轨迹跟踪控制框架轨迹跟踪控制器主要由两部分组成:MPC 控制器以及Carsim 车辆模型。MPC 控制器是实现轨迹跟踪控制的核心,具有模型预测、滚动优化和反馈矫正三个特点。所设计的MPC 控制器是将车辆的横向控制和纵向控制结合在一起,通过求解得到最优的控制量前轮转角 f和加速度 a,并将其输入到 Carsim 车辆模型中,得到车辆当前的状态,作为预测模型的输入,然后重复上述过程,最终实现轨迹跟踪。框架图如图 4。图 4 基于横纵向控制 MPC 轨迹跟踪控制器3.2 横纵向 MPC 控制器设计3.2.1 线性误差模型的建立由于无论是运动学模型还是动力学模型,均为非线性系统,而线性模型预测控制比非线性模型预测控制有更好的实时性,且更易于分析和计算,为此本文采用近似的线性化方法对非线性系统进行线性化。假设车辆参考系统在任意时刻的状态量和控制量满足如下方程Xr=f(Xr,ur)(8)但式(8)并不适合模型预测控制器的设计,需要对其进行线性化和离散化处理。线性时变和离散化处理后的线性化时变模型为:?X(k+1)=?A(k)?X(k)+?B(k)?u(k)(9)其中,?A(t)=I+TA(t),?B(t)=TB(t),I 为单位矩阵。结合横向误差动力学模型以及纵向控制模型对车辆的运动状态进行分析。并以横向误差(lateral_error)、横向误差率(lateral_error_rate)、航向角误差(heading_error)、航向角误差率(heading_error_rate)、位置误差(station_error)以及速度误差(speed_error)作为状态量。即:matrix_state=laterl_errorlaterl_error_rateheading_errorheading_error_ratestation_errorspeed_error|364 以前轮转角 f和加速度补偿 a 作为控制量control_matrix=fa|结合横向误差动力学模型(式(6)以及纵向误差控制模型(式(7),可得基于横纵向的 MPC 控制模型为:ddtlaterl_errorlaterl_error_rateheading_errorheading_error_ratestation_errorspeed_error|=0100000-2Cf+2Crmv2Cf+2Crm2Crb-2Cfamv0000010002Crb-2CfaIzv2Cfa-2CrbIz-2Crb2+2Cfa2Izv00000001000000|laterl_errorlaterl_error_rateheading_errorheading_error_ratestation_errorspeed_error|+002Cfm0002CfaIz0000-1|fa|+02Crb-2Cfamv-v0-2Crb2+2Cfa2Izv01|?(10)对应的状态矩阵、控制矩阵、扰动矩阵分别为A=0100000-2Cf+2Crmv2Cf+2Crm2Crb-2Cfamv0000010002Crb-2CfaIzv2Cfa-2CrbIz-2Crb2+2Cfa2Izv00000001000000|B=002Cfm0002CfaIz0000-1|,C=02Crb-2Cfamv-v0-2Crb2+2Cfa2Izv01|对应的线性化系数应为Ak,t=I+TA,Bk,t=TB,Ck,t=TC 最终得到的离散线性公式为?x(k+1)=Ak,t?x(k)+Bk,t?u(k)+Ck,t(11)系统的输出方程为y(k)=Dk,tx(k)(12)3.2.2 目标函数设计目标函数要能够保证车辆快速平稳地追踪上期望轨迹。因此,需要加入对系统状态变量的偏差和控制量的优化。在设计轨迹跟踪控制器时,采用如下形式的目标函数J(k)=Nj=1(?x(k+j|k)TQ?x(k+j|k)+?u(k+j-1|k)TR?u(k+j-1|k)(13)其中,N 为预测时域,Q,R 为权重矩阵,且满足 Q0,R0 的正定矩阵。为了避免被控系统控制量突变的现象,从而保证控制量的连续性,在式(13)中添加约束,将目标函数转化为如下形式:J(t),u(t-1),U(t)=Ni=1(t+i|t)-ref(t+i|t)2Q+N-1i=1U(t+i|t)2R(14)基于线性误差模型预测车辆未来时刻的输出,假设:(k|t)=?x(k|t)?u(k-1|t)|,代入式(11)得到新的状态空间方程:(k+1|t)=?Ak,t(k|t)+?Bk,tu(k|t)+?Ck,t(k|t)=?Dk,t(k|t)(15)其中?Ak,t=Ak,tBk,t0mnIm|,?Bk,t=Bk,tIm|,?Ck,t=Ck,t0|,?Dk,t=Dk,t0 将系统未来时刻的状态和输出以矩阵形式表达为X(t)=t(t|t)+tU(t|t)+tY(t)=?DtX(t)(16)其中464X(t)=(t+1|t)(t+2|t)(t+3|t)(t+Np|t)|,Y(t)=(t+1|t)(t+2|t)(t+3|t)(t+Np|t)|,U=u(t|t)u(t+1|t)u(t+2|t)u(t+Np-1|t)|,t=?C?A?C+?C?A2?C+?A?C+?CNp-1i=0?Ai?C|t=?At?A2t?A3t?ANpt|,t=?Bt000?At?Bt?Bt00?ANp-1t?Bt?ANp-2t?Bt?Bt|预测时域内输出量偏差表示为E(t)=t(t|t)-Yref(t)(17)其中,Yref=ref(t+1|t),ref(t+N|t)T。将式(16)、式(17)代入式(14),从而将目标函数调整为与控制量相关的函数J(t),u(t-1),U(t)=U(t)HtU(t)T+GtU(t)T+Pt(18)其中,Ht=TQe+Re,Gt=2E(t)TQe,Pt=E(t)TQeE(t),Pt为常量,Qe,Re为权重矩阵 Q,R 的扩充矩阵。3.2.3 约束条件设计本文主要考虑控制过程中的控制量极限约束和控制增量约束,控制量表达形式为Umin u(t-1)+Ni=1U(i)Umax(19)控制增量约束表达形式为Umin Ut(k)Umax(20)结合式(16)、(17)以及(18),优化问题求解可表达如下minUtU(t)HtU(t)T+GtU(t)TUmin u(t-1)+Ni=1U(i)UmaxUmin Ut(k)Umax|(21)在每一个控制周期内,根据式(21)完成优化求解后,得到控制时域内的一系列控制增量Ut=ut,ut+1,ut+N-1(22)将序列中的第一个控制量作为实际的控制输入增量作用于系统,则有u(t)=u(t-1)+ut(23)将 u(t)作为此时的控制量输入给系统,直到下一个控制时刻,系统根据新的状态信息预测下一时段内的输出,然后通过优化得到一组新的控制序列。如此反复,直至完成整个控制过程。4 仿真结果及分析仿真中的车辆模型采用 Carsim 中的车辆模型,具体参数见表 1。表 1 整车参数参数取值簧上质量/kg1270质心到前轴距离 a/mm1015质心到后轴距离 b/mm1795轮距/mm2080质心高度/mm540绕 z 轴转动惯量/(kgm2)1536.7前轮侧偏刚度/(Nrad-1)-148970后轮侧偏刚度/(Nrad-1)-82204 MPC 控制器中的部分参数设置见表 2。表 2 控制器参数参数数值控制时域10预测时域25umin-1,-0.47umax1,0.47aymin-0.4gaymax0.4gQdiag(30,2,30,1,1,1)Rdiag(1,1)4.1 不同车速情况下的仿真为了验证所提出的基于 MPC 的轨迹规划与跟踪控制器的性能,在摩擦系数为=0.85 的沥青路面上进行变道跟踪控制的模拟仿真。在这个场景中,智能车分别以 36km/h、72km/h 以及 108km/h 的恒定速度进行换道。仿真结果如图5,图 6 以及图 7。从图 5(a)、5(b)、6(a)、6(b)、7(a)和 7(b)可以看出在良好的路面附着下控制器轨迹跟踪效果良好。在速度为36km/h 的仿真中实际轨迹几乎与参考轨迹重合,控制器在低速良好路面的跟踪效果非常良好。在 108km/h 的仿真工况下,实际轨迹与参考轨迹偏差比 36km/h 和 72km/h 的偏差略大,这是因为高速下车辆的转向过于灵敏所致。在72km/h 的工况下仿真效果也比较良好。但三种工况的偏差都在控制器阈值范围内,由此可知在低中高三种车速仿真下,控制器均能精确的跟踪参考轨迹。从图 5(c)、5(d)、6(c)、6(d)、7(c)和 7(d)可以看出在良好的路面附着下本文所设计的控制器能够比较准确的跟564图 5 36 km/h 工况下的跟踪控制仿真图踪参考的横摆角。由于随着车速的增高,完成换道所需的最大优化方向盘转角越小,这是因为转向比随速度的增加而减小,所以横摆角跟踪误差的最大值随速度的增加而减小,且图 6 72 km/h 工况下的跟踪控制仿真图不同车速下的误差最大值均未超出控制器设置的阈值。4.2 雨雪路面车辆轨迹跟踪为了验证轨迹跟踪控制器的鲁棒性,本文在摩擦系数为664图 7 108 km/h 工况的跟踪控制仿真0.2 的冰雪路面以及 0.3 小雨路面条件下进行轨迹跟踪仿真测试。仿真情况如图 7 和图 8。如图 8 所示,当车速为 36km/h 时,车辆在雨雪路面均能图 8 36 km/h 工况低路面附着系数图 9 108 km/h 工况低路面附着系数实现轨迹跟踪控制,且跟踪效果良好,跟踪轨迹误差最大不超过 0.03m。由轮胎的侧偏特性图可知,此时车辆轮胎的前后侧偏角保持在线性区之内,从而保证了地面能给轮胎提供足够的侧向力,特别是在转弯时地面能够提供轮胎足够的向心力。如图 9 所示,当车辆以 108km/h 的速度进行换道时,车辆仍能对轨迹进行跟踪,但跟踪误差明显变大,这是由于速度过高,轮胎的前轮侧偏角处于轮胎侧偏特性图的饱和区,此时地面不能给轮胎提供转弯时所需的向心力,使车辆前轴发生侧偏,偏离参考轨迹。4.3 控制器的对比分析为了验证所设计的轨迹跟踪控制器的精确性,将其与未添加纵向控制的轨迹跟踪控制器在速度为 100km/h 和摩擦系数=0.85 的条件下进行比较分析,仿真结果如图 10。对最大偏差进行对比分析,见表 3。表 3 控制器对比控制器轨迹跟踪是否有效最大横向偏差(m)最大横摆角(deg)横纵向 MPC是0.0432.64横向 MPC是0.0652.66 由图 10 和表 3 可知,当车速为 100km/h 时,二者横摆角相差不大,都具有良好的行驶稳定性;横纵向 MPC 和横向764图 10 100km/h 工况下两种控制器的对比MPC 均能进行轨迹跟踪,但是横纵向 MPC 跟踪的最大横向偏差比横向 MPC 减小了 0.022m,说明横向跟踪精度有所提高,说明其有更好的跟踪精确性。5 结束语针对智能车辆换道安全控制问题,设计出横纵向 MPC的模型预测控制器对智能车辆的参考轨迹进行轨迹跟踪控制,使得智能车辆能够有效、安全的完成换道任务,并且通过MATLAB/Simulink 和 Carsim 的联合仿真,验证所设计的控制器的可行性与鲁棒性,并与未添加纵向控制的控制器进行对比分析,验证了该算法适用于对智能车辆换道行为跟踪控制,与未添加纵向控制的控制器相比其横向跟踪误差更小,跟踪精度更高。参考文献:1 李舜酩,沈峘,毛建国,辛江慧,缪小东.智能车辆发展及其关键技术研究现状J.传感器与微系统,2009,28(1):1-3,9.2 陈慧岩,陈舒平,龚建伟.智能汽车横向控制方法研究综述J.兵工学报,2017,38(6):1203-1214.3 Wang J,Zhang G,Wang R,et al.A Gain-Scheduling Driver As-sistance Trajectory-Following Algorithm Considering DifferentDriver SteeringCharacteristics J.IEEETransactionsonIntelligent Transportation Systems,2017,(5):1-12.4 Huang C,Naghdy F,Du H.Fault Tolerant Sliding ModePredictive Control for Uncertain Steer-by-Wire SystemJ.IEEETransactions on Cybernetics,2017:1-12.5 张炳力,李子龙,沈干,方涛,曹聪聪,郑平平.基于模糊神经网络的智能汽车轨迹跟踪研究J.汽车工程,2019,41(8):953-959.6 聂枝根,王万琼,赵伟强,黄震,宗长富.基于轨迹预瞄的智能汽车变道动态轨迹规划与跟踪控制J.交通运输工程学报,2020,20(2):147-160.7Falcone P,Borrelli F,Asgari J,et al.Predictive Active SteeringControl for Autonomous Vehicle SystemsJ.IEEE Transactions onControl Systems Technology,2007,15(3):566-580.8 Hongyan,Guo,Chen,et al.Simultaneous Trajectory Planning andTracking Using an MPC Method for Cyber-Physical Systems:ACase Study of Obstacle Avoidance for an Intelligent VehicleJ.IEEE Transactions on Industrial Informatics,2018.9 Bai G,Meng Y,Liu L,et al.A New Path Tracking Method Basedon Multilayer Model Predictive Control J.Applied Sciences,2019,9(13):2649.10 王银,张灏琦,孙前来,李小松,孙志毅.基于自适应 MPC 算法的轨迹跟踪控制研究J.计算机工程与应用,2021,57(14):251-258.11 孙银健.基于模型预测控制的无人驾驶车辆轨迹跟踪控制算法研究D.北京理工大学,2015.12 E Alcal,Puig V,J Quevedo.LPV-MPC Control forAutonomous VehiclesJ.IFAC-PapersOnLine,2019,52(28):106-113.13 RajeshRajamani.车辆动力学及控制 M.机械工业 出版社,2011.作者简介孙志伟(1997-),男(汉族),山东省青岛市人,硕士研究生,主要研究领域为智能车辆轨迹规划和跟踪控制。李 聪(1977-),女(汉族),山东省潍坊市人,副教授,硕士研究生导师,主要研究领域为汽车新能源技术。864