许小伟,崔金融,严运兵,马霁旻,王亚玮
(1.武汉科技大学汽车与交通工程学院,湖北 武汉,430065; 2.智新科技股份有限公司,湖北 武汉,430056;3.华中科技大学电气与电子工程学院,湖北 武汉,430074)
智能车换道超车是一种常见的驾驶行为,其中换道时刻的选择和高速换道行驶过程中的安全性需要重点关注,必须解决换道决策、路径规划以及轨迹跟踪等多个关键技术问题[1]。
换道行为涉及到多个车道,由于人工势场理论简单且直观,因此被广泛用于量化道路的动态环境。张朋[2]利用人工势场法对道路的动态环境复杂度进行建模,并对提出的量化模型进行实验标定。李林恒等[3]为了直观地表达智能车在换道过程中面临的风险,选用人工势场法建立换道模型,并利用国内道路环境进行模型标定。朱乃宣等[4]借助人工势场理论量化前车产生的风险场场强,依据驾驶风格来设计不同的规划阈值而达到个性化换道的目的,但是换道触发时刻仅考虑前车的影响,未考虑换道对相邻车道正常行驶车辆造成的影响。
轨迹跟踪的目的是使车辆按照规划的路径行驶。目前智能车轨迹跟踪方法主要有PID控制、模型预测控制和LQR控制等,其中LQR控制能考虑到多项性能指标且实时性好,因此得到广泛应用。陈亮等[5]构建了加入前馈控制的LQR控制器,有效减少了跟踪误差。胡杰等[6]根据车速变化对LQR权重系数进行模糊调节,并验证了控制器在实车环境中的控制性能。高琳琳等[7]对LQR控制器的权重矩阵进行了自适应调整。然而,模糊调节和自适应调整权重都是依靠经验设计,并不能保证输出最优的控制量。
针对上述问题,本文基于人工势场理论量化道路风险场,建立换道规则,并借助最小纵向安全距离来判断换道的安全性,同时通过改进的LQR控制器对换道轨迹进行跟踪。本文最后利用Carsim和Simulink搭建联合仿真平台,来验证所提出的换道超车决策的有效性以及轨迹跟踪控制器的效果。
本文基于人工势场理论计算车辆正常行驶过程中遇到的风险水平,并建立道路风险场,包括障碍车辆风险场和引导风险场。障碍车辆风险场考虑了障碍车辆的自身属性,引导风险场考虑了主车与周围车辆的相对位置和相对速度。
障碍车辆风险场涉及换道车辆与周围车辆的相对运动状态。由实际交通流可知,车辆换道过程中,在纵向上,换道车辆接近周围车辆时,风险场会不断增强,且车辆相对距离越近,场强的增长幅度越大;在横向上,允许换道车辆以较小的换道距离超越前车,且前车的风险场应尽可能地不影响相邻车道。综上,选用二次高斯函数模型[8]来表示周围车辆的场强,建立障碍车辆势场Uobst。
Uobst=
(1)
∂x,obst=kxLobst
(2)
∂y,obst=kyWobst
(3)
式中:x、y分别为换道车辆质心的纵、横向坐标;xobst、yobst分别为障碍车辆质心的纵、横向坐标;Aobst为障碍车辆的场强幅值,本文取值为1;∂x,obst、∂y,obst分别为障碍车辆产生的势场在纵向和横向上的影响范围;Lobst、Wobst分别为障碍车辆的长度和宽度;kx、ky分别为障碍车辆在x、y方向上的缩放系数,考虑到实际超车过程中障碍车辆只对同车道后车产生影响,而对同向相邻的车道影响很小甚至没有影响,故取kx=1,ky=0.5。
构建图1所示的换道超车场景,道路环境为同向行驶的三车道,规定向右行驶为正方向,文中均以A车为主车构建行驶风险场,其余车辆视为周围的障碍车辆,各车辆行驶状态参数见表1。
表1 车辆状态参数
图1 智能车换道超车场景示意图
依据图1所示场景形成的势场如图2所示,可以看出,障碍车辆风险场在纵向上影响范围较大,而在横向上影响范围较小;其次,随着主车与障碍车相对距离的减小,主车所受到的风险也越大。由此可知,使用二次高斯函数建立障碍车辆势场满足换道场景的要求。
(a)障碍车辆势场3D图
为了避免两车发生碰撞事故,增加一个额外的引导势场,更好地引导在慢速行驶的障碍车后方的车辆提前换道行驶。选用的引导势场函数表达式为
Uext=Aobst+Fx(x-xobst)-Fy(y-yobst)2,
∀x∈[-S,0]
(4)
式中:S为引导势场在纵向上的影响范围;Fx、Fy分别为引导势场在纵向和横向上的影响系数,计算方式如下:
(5)
(6)
引导势场只有在车辆靠近障碍车辆的时候才会产生实际意义,故引导势场的强度必然不小于零,小于零的势场值用零代替。影响范围公式为:
S=0.5Lobst+Smin+tavrel
(7)
式中:Smin为引导势场产生影响的最短距离;vrel为两车之间的相对速度;ta为相对速度减小时的接近时距。
图1所示换道场景的引导势场见图3,可以看出,引导势场影响范围随着两车相对速度绝对值的增加而扩大,且距离障碍车辆质心越近,引导势场值越大。
(a)引导势场3D图
综上所述,车辆总的道路风险场为障碍车辆势场与引导势场的整合:
Utotal=Uobst+Uext
(8)
智能车的换道动机是为了追求更高的通行效率以及更优的行驶状态,即智能车换至目标车道可获得足够的行驶空间以加速到期望车速。首先,比较主车当前所处位置与相邻车道中心线位置的风险场,若是相邻车道中心线位置的风险场强度低于主车所处位置的场强,则认为主车有向相邻车道的换道动机,相反则认为本车保持当前车道行驶更佳。但是,在实际情况下,若两处的风险场场强的差值很小,一旦车辆状态发生微小变化,就会引起场强变化,导致车辆频繁产生换道意图,反而影响通行效率。因此,本文定义一个规划阈值P,只有当主车所处位置与相邻车道中心线位置的场强差值超过预设的规划阈值,才认为换道后可获取更优行驶状态,即满足式(9)时,车辆产生向相邻车道的换道超车意图。
Utotal(x,yi)>Utotal(x,yj)+P
(9)
式中:x为主车所处位置纵向坐标,yi为主车所处位置横向坐标,yj为相邻车道中心线横向坐标。
换道意图产生后,为了保证换道安全性,本文以最小纵向安全距离作为判据,其中包括本车道内最小纵向安全距离以及目标车道前、后车最小纵向安全距离。由文献[9]统计分析可知,车辆切入场景的纵向速度变化很小,因此本文假设换道过程中主车与周围车辆都保持恒定的纵向速度行驶。
(1)本车道最小纵向安全距离
在换道过程中,智能车可通过传感器设备获得与主车相同车道的前车的速度以及二者之间的纵向车距,保持合理的车距进行换道。与前车的最小纵向安全距离MSS1计算如下:
(10)
式中:v为主车当前车速,vf为前车速度,tC为换道临界时间,dmin为最小安全距离的临界值。
(2)目标车道前、后车最小纵向安全距离
在更为复杂的交通环境中,车辆换道至有正常行驶车辆的目标车道,因此要保证换道车辆与目标车道上车辆之间的安全性。换道起始时刻,智能车获取目标车道车辆的位置与速度信息,进行分析计算,保证换道结束时满足安全车距的要求。换道车辆与目标车道前、后车之间的最小纵向安全距离MSS2、MSS3分别为:
(11)
(12)
式中:vi,f、vi,r分别代表目标车道前、后车的纵向速度。
在车辆产生换道意图并判断换道安全后,决策系统发出触发换道指令,换道行为触发流程如图4所示。
图4 换道行为触发流程
本文利用LQR控制器对智能车规划好的轨迹进行跟踪。首先建立一个适用于控制器设计的二自由度动力学跟踪误差模型[10],如图5所示。
图5 二自由度动力学跟踪误差模型
图5中:δ为前轮转角;lf、lr分别为车辆质心到前、后轴的距离;ed为车辆质心到参考轨迹的最短距离,即位置偏差;φ为车辆航向角,φdes为参考航向角;ω为横摆角速度;vx、vy分别为车辆纵、横向速度。
根据动力学模型得到二自由度微分方程为:
(13)
式中:m为车辆质量,Iz为转动惯量,Cf、Cr分别为车辆前、后轮侧偏刚度。
定义航向误差为
eφ=φ-φdes
(14)
设参考轨迹的半径为Rdes,可得到理想的横向加速度为
(15)
智能车产生的横向加速度为
(16)
(17)
(18)
综上所述,得出如下公式:
(19)
(20)
将式(19)和式(20)转换为状态空间表达形式,则为
(21)
轨迹跟踪控制器是基于误差模型计算出前轮转角的输入,存在一定的滞后性,故在本文方法中对车辆状态进行一定采样时间ts的预瞄。设车辆在预瞄时间内匀速直线行驶,预瞄后车辆的航向角为φpre、坐标为(Xpre,Ypre),有
(22)
式中:(X0,Y0)为轨迹规划点的坐标。
本文利用LQR[11-12]对智能车换道轨迹进行跟踪控制。首先明确线性系统的性能指标函数J为
(23)
式中:Q和R为控制器的权重矩阵。
为了使性能指标J达到最优,采用变分法进行求解。计算得到反馈矩阵为
K=(R+BTPB)-1BTPA
(24)
其中P为如下Riccati方程的正定解:
P=Q+ATPA-ATPB(R+BTPB)-1BTPA
(25)
最终得到LQR控制器的最优控制律为
U(t)=-KX(t)
(26)
式中:K=[k1,k2,k3,k4]为LQR控制器增益。
将式(26)带入式(21)可得
(27)
由此可见,系统存在稳态误差,即存在无法消除的位置偏差和航向偏差,为此设计前馈控制器消除该误差。设前馈控制输出的前轮转角为δf,即控制量输入U(t)=-KX(t)+δf。
根据终值定理可得系统的稳态误差为
e=-(A-BK)-1(Bδf+B1C)
(28)
为保证横向偏差稳态值趋于零,设计前馈控制律为
(29)
式中:kref为道路参考曲率。
将前馈控制律与LQR反馈控制律相结合,得到跟踪算法的前轮转角输出量。
式(23)中的权重矩阵Q和R在车辆跟踪控制中起着非常重要的作用。寻找最优的Q和R是一项复杂的工作,往往需要大量试错才能得出可行解。本文利用遗传算法来优化LQR控制器参数[12-13]。
鉴于二自由度动力学误差模型有且仅有前轮转角一个控制量,因此确定控制权重矩阵R=1。Q中的对角线参数代表了各项误差的权值,在车辆轨迹跟踪过程中更加关注位置误差和航向误差,因此将Q矩阵设置为
(30)
下面利用遗传算法对矩阵Q中参数a、b进行优化。为了提高跟踪的精准性,同时考虑位置误差和航向误差,因此设目标适应度函数为
(31)
根据目标适应度函数最小化原则,LQR控制器参数优化流程如图6所示。具体步骤如下:
图6 采用遗传算法优化LQR 控制器的流程
步骤1在0~10的约束范围内产生N个初始个体。
步骤2将初始种群中的个体赋值给权重矩阵Q中的a、b,得到反馈增益矩阵K,求出最优控制律,并输入给车辆仿真模型,运行模型,计算适应度函数值。
步骤3选取适应度数值小的个体进行迭代。
步骤4通过保优、交叉、变异等遗传操作,保留父代和产生的子代,组成新的种群。
步骤5重复步骤2~4,直至满足终止条件。
为了验证本文方法的有效性,利用Simulink和Carsim搭建联合仿真平台,针对车辆在快速路上自主换道行为进行仿真测试,车辆的部分结构参数如表2所示。
表2 车辆结构参数
在带有前馈的LQR控制器基础之上,分别采用经验法和遗传算法确定控制器参数,在相同的车速和路况条件下进行双移线仿真测试,验证轨迹跟踪控制效果。
在双移线测试中,路面附着系数设为0.85,车速恒定为60 km/h,双移线的轨迹方程[7]为
(32)
方案一 按照经验法选取控制效果最好的一组控制参数:a=5,b=5。
方案二 采用遗传算法对控制器参数优化,遗传算法的终止代数设为20,初始种群个体数量为100,交叉和变异的概率分别取为0.75和0.15。多次运行程序,虽然每次结果不同,但是非常接近,最后得出a=9.9608,b=0.1233。
两个方案的测试结果如图7所示。采用经验法确定参数的LQR控制器可以将轨迹跟踪的位置误差控制在0.9 m以内,航向误差控制在0.12 rad以内;利用遗传算法优化后的LQR控制器可以将位置误差控制在0.6 m以内,航向误差控制在0.1 rad以内。可以看出,通过遗传算法优化LQR控制器矩阵参数能减少轨迹跟踪系统的超调量,提升跟踪精度。
(a)侧向位置和位置误差
本文主要利用两类场景、四种工况来验证换道决策方法的有效性和跟踪的准确性。第一类为简单的换道场景,仅本车道前面存在低速障碍车辆;第二类为复杂的换道场景,存在多车道车辆影响。
(1)第一类场景
此类场景设计了两种工况,通过设定不同的主车速度来验证换道超车触发时刻的合理性。
工况一仿真场景如图8所示,主车A行驶在快速路中间车道上,其前方有车辆B,而相邻车道无任何车辆,该工况下各车辆的状态参数如表3所示。
表3 工况一的仿真参数设定
图8 工况一场景
车辆换道超车过程如图9所示。结合表3以及图8~图9可知,本车道上车辆B的行驶速度低于主车A,且主车A的期望速度高于其当前速度,为了追求更好的通行效率,主车A不断靠近前车B,当主车所处位置的势场值与相邻道路中心势场值之差大于阈值时,在t1时刻主车A产生换道意图,由于左侧车道没有障碍车,选择该车道为目标车道。下一步进行安全性分析,左侧没有障碍车且主车A与前车B的相对距离为41 m,大于本车道最小纵向安全距离MSS1=17 m的要求,故满足换道的安全性要求。然后规划出换道轨迹,t1时刻开始触发换道指令,进行超车,t2时刻主车A驶入目标车道,换道结束。
图9 工况一的换道过程
图10所示为工况一的仿真结果,包括换道过程中车辆实际轨迹与规划出的目标轨迹的对比、轨迹跟踪误差以及主车A的侧向加速度和横摆角。从图10可以看出,整个换道过程相对平稳,轨迹跟踪误差小于0.3 m,侧向加速度和横摆角的峰值均在约束范围之内。
(a)侧向位置和位置误差
工况二仿真场景与图8相同。相较于工况一,主车A的速度有所提升,车辆的具体位置与速度如表4所示。
表4 工况二的仿真参数设定
工况二的车辆换道超车过程如图11所示。与工况一相比,主车A的速度有所提升,即主车与障碍车的速度差增加了,导致引导势场的增强,两车产生追尾的可能性也提高了,因此换道触发时刻有所提前。在t1时刻车辆A产生换道意图,且此时前、后车距离为53 m,满足本车道最小纵向安全距离MSS1=26 m的要求。车辆A在t1时刻触发换道指令进行换道,于t2时刻完成换道。
图11 工况二的换道过程
工况二的仿真结果如图12所示。与工况一相比,在车辆A行驶速度增加后,只有侧向加速度与跟踪误差的峰值有所增加,且增幅很小,同时,换道过程中横摆角的峰值几乎没有变化,这体现出本文提出的轨迹跟踪控制方法具有良好的适应性。
(a)侧向位置和位置误差
(2)第二类场景
下面通过两种工况来验证本文换道决策方法在复杂道路场景中的适用性。
工况三仿真场景参照图1,主车A行驶在快速路中间车道上,其前方有车辆B,且左右相邻车道上均有车辆。该工况下各车辆的状态参数如表5所示。
表5 工况三的仿真参数设定
工况三的换道过程如图13所示。车辆A和B的位置及速度参数与工况二一致,依据工况二测试结果可知,A车在与前车B的距离为53 m处产生换道意图,且C车所在车道速度更快,故被定为目标车道。此刻A车与C车相距36 m,满足与目标车道后车之间的最小纵向安全距离MSS3=17 mm的要求,t1时刻触发换道指令,t2时刻A车驶入目标车道完成换道。
图13 工况三的换道过程
工况四仿真场景与工况三相同,只是车辆C的速度由100 km/h提升到120 km/h,各车辆的状态参数见表6。
表6 工况四的仿真参数设定
图14所示为工况四的换道超车过程。相较于工况三,车辆C速度提升后,主车A受环境中其他车辆的位置和行驶速度的影响,换道触发时刻有明显的延后,这是车辆综合考虑换道意图和换道安全性的结果。首先主车A在中间车道行驶,不断靠近障碍车辆B,由工况三可知,A车在距B车53 m时产生换道意图,此时A车与C车的纵向距离为21 m,低于最小安全换道距离MSS3=33 mm的要求,因此等待C车超越A车后,并且两车的相对距离大于前车最小纵向安全距离MSS2=8 mm的条件时,A车开始换道。由工况四的仿真结果可知,本文换道超车控制方法充分考虑了复杂道路环境下的换道安全性。
图14 工况四的换道过程
为了保障智能车自主换道超车的安全性,本文首先根据人工势场理论量化车辆换道风险,建立了基于道路风险场的换道触发决策模型,然后采用改进的LQR控制器对轨迹进行跟踪,其中LQR控制参数通过遗传算法进行优化。本文最后通过Simulink和Carsim联合仿真验证了改进后控制器的精度提升,并且通过不同道路环境下的换道超车仿真试验,证明了所提出的换道触发决策模型的有效性和安全性。然而,本研究未考虑换道过程中车辆纵向速度的改变,且目前还都只是基于联合仿真测试,下一步工作将针对本文方法进行实车验证。