基于车车通信技术的自动换道算法研究与仿真＊

王乐 王文格 彭景阳 胡小双 郭恒宇

（湖南大学，长沙 410082）

1 前言

自动驾驶技术能够提高车辆的安全性和交通效率[1]，自动驾驶车辆的车道跟踪与换道方面均有较多研究成果[2-3]。换道导致的事故占美国所有交通事故的比例约为4%～10%，同时造成了10%的交通延迟[4]，所以对自动驾驶车辆换道的研究具有重要意义。

You 等人采用六次多项式进行纵向轨迹规划避开前方障碍车辆，通过车车通信技术获得更加准确的环境信息[5]。Yang 等人采用与时间无关的多项式方程表征车道变换轨迹曲线，可避免现有模型对速度和加速度的不切实际的假设[6]。Luo等人将车道变换轨迹描述为参数化五次多项式，通过求解非线性规划（Non-Linear Programming，NLP）问题计算多项式参数[3]。张荣辉等人使用六次多项式进行纵向轨迹规划[7]。在轨迹跟踪方面，用于移动机器人的Backsteppin[8]在自动驾驶车辆的轨迹跟踪中得到了应用[9]。叶涛等人在Backstepping 的基础上设计了滑模控制方法进行轨迹跟踪[3,10]，Farnaz等人[11]、姜立标等人[12]分别采用反馈误差学习的方法和新的趋近律对其进行了改进。

目前，自动驾驶车辆的换道研究主要集中于轨迹规划与轨迹跟踪，且存在以下不足：大多数研究仅考虑起始车道前方车辆或典型换道场景，考虑因素较少；多数研究只在换道开始时考虑避撞安全需求；轨迹跟踪中滑模跟踪算法适应性较差。

本文基于车车通信技术，考虑新的换道场景，提出并行换道轨迹规划方法。针对换道过程中的碰撞安全问题，提出碰撞检测与并行轨迹重规划方法，并改进滑模轨迹跟踪算法，提高其适用性。

2 自动换道整体策略

典型的向左加速换道场景如图1 所示，其中M 为换道车辆，Ld、Fd分别为目的车道前、后方车辆，Lo、Fo为原车道前、后方车辆。此场景下，只允许换道至Fd和Ld之间。

图1 典型向左加速换道场景

新的换道场景如图2和图3所示，Fd-F为目标车道后方车辆的后方车辆，Ld-L为目标车道前方车辆的前方车辆。图2 中，允许换道至Fd和Ld之间或Fd和Fd-F之间。图3中，允许换道至Fd和Ld之间或Ld和Ld-L之间。当Fd-F不存在时，图2与图1场景相同。

图2 向左加速换道场景

图3 向右减速换道场景

自动换道策略如图4所示，其中换道车辆通过车车通信技术获得周围车辆信息，通过GPS、加速度传感器、速度传感器等获得状态信息。根据收集到的信息，换道决策器和换道轨迹规划器分别进行决策和轨迹规划，轨迹跟踪控制器对已规划的轨迹进行跟踪，碰撞检测器不断进行检测，轨迹重规划器在检测到碰撞的情况下进行轨迹重规划。

图4 自动换道策略

3 换道轨迹规划

3.1 基于优化模型的多项式换道轨迹

3.1.1 多项式换道轨迹

选择五次多项式作为换道轨迹，其具有连续三阶导数和曲率平滑的优点[3]：

式中，a0～a5、b0～b5为未知系数；x(t)、y(t)分别为t时刻纵向、横向坐标；t为时间。

根据车辆换道开始和结束时刻的车辆状态可得：

式中，x0、y0分别为初始时刻的纵向和横向坐标；vx,0、vy,0分别为初始时刻的纵向和横向速度；ax,0、ay,0分别为初始时刻的纵向和横向加速度；xf为换道结束时刻的纵向位置；yf为换道结束时刻的横向位置，等于车道宽度；vx,f为换道结束时刻的目标速度；vy,f为换道结束时刻的横向速度，等于零；ax,f、ay,f分别为换道结束时刻的纵向和横向加速度，均为零；tf为换道结束时刻。

式（1）有12个未知参数，式（2）有12个状态方程，可直接解出参数a0～a5和b0～b5。将tf和xf设为变量，将换道轨迹规划问题转化为带约束的优化问题，本文以向左换道为例进行说明。

3.1.2 约束条件

3.1.2.1 位置约束

换道过程中，换道车辆的横向位置应在目标车道和本车道中心线之间：

式中，w为车道宽度。

换道终止时刻，其纵向位置应满足：

式中，xFd-F(tf)、xM(tf)、xLd(tf)、xFd(tf)分别为Fd-F、M、Ld、Fd在换道结束时刻的纵向位移；d为车辆巡航时应保持的最小纵向距离；L为车长。

3.1.2.2 速度约束

换道过程中，换道车辆的纵向速度应小于最大车速，且为正值：

式中，vx(t)为换道车辆在t时刻的纵向车速；vx,max为允许的最大车速。

3.1.2.3 动力学约束

换道车辆的加速度、加加速度应满足：

式中，ax(t)和ay(t)分别t时刻纵向和横向的加速度；jx(t)和jy(t)分别t时刻纵向和横向的加加速度；ax,max和ay,max分别为纵向和横向最大加速度；jx,max和jy,max分别为纵向和横向最大加加速度。

3.1.2.4 安全约束

采用动态圆方法逼近车辆二维形状[5]，如图5所示。

图5 车辆形状示意

由图5可得动态圆圆心的一系列坐标为：

式中，μ∈[0,1]；xb、xf分别为车辆后端圆和前端圆的纵向坐标；yb、yf分别为车辆后端圆和前端圆的横向坐标。

换道过程中换道车辆的外轮廓与其他车辆的外轮廓不重合即为安全。因此，安全约束为：

式中，xM、xo分别为换道车辆和障碍车辆（Ld、Fd、Lo、Fo、Fd-F）纵向位移；yM、yo分别为换道车辆和障碍车辆横向位移；RM、Ro分别为换道车辆和障碍车辆动态圆的半径，其应略大于车辆宽度的1/2。

将式（8）代入式（9）可得：

式中，μM,μo∈[0,1]；xM,b(t)、xo,b(t)分别为t时刻换道车辆和障碍车辆后端圆的纵向坐标；xM,f(t)、xo,f(t)分别为t时刻换道车辆和障碍车辆前端圆的纵向坐标；yM,b(t)、yo,b(t)分别为t时刻换道车辆和障碍车辆后端圆的横向坐标；yM,f(t)、yo,f(t)分别为t时刻换道车辆和障碍车辆前端圆的横向坐标。

在轨迹规划时刻，假设各障碍车辆匀速直线行驶。

3.1.3 优化目标

考虑车辆换道过程的舒适性和对交通流的影响，设定优化目标为[3]：

式中，λ1、λ2、λ3为权重系数；ξ为常数。

该优化模型对于前述其他场景同样适用，只需去除未出现车辆对应的约束条件。

3.2 并行换道轨迹规划

假设车道后方车辆速度不大于前方车辆速度，分别选取Ld、Fd和Fd-F车速为vx,f，采用优化模型作并行计算，结果可能为0条、1条或多条轨迹：若是0条轨迹，按照第3.1 节处理；若是1 条轨迹，则进行换道轨迹跟踪；若是多条轨迹，则根据驾驶风格选择一条轨迹进行轨迹跟踪。

3.3 碰撞检测与并行轨迹重规划

针对换道过程中障碍车辆车速等状态发生非预期改变的情况，本文研究了碰撞检测与并行轨迹重规划方法。

在换道过程中收集周围车辆信息和自身状态信息后，采用式（10）不断进行碰撞检测。如将要发生碰撞则进行并行轨迹重规划，同时进行换道轨迹和回到起始车道轨迹规划，进行换道轨迹规划的方法同3.1节，回到起始车道轨迹规划时，式（5）改为：

对重规划轨迹进行选择时，首先选择换道轨迹，无换道轨迹时，选择回到起始车道轨迹。

4 换道轨迹跟踪

4.1 车辆位姿误差模型

车辆实际位姿与参考位姿误差模型如图6 所示。Mc与Mr分别为车辆当前时刻实际位置和参考位置，Pc=(xc,yc,φc)T和Pr=(xr,yr,φr)T分别为当前车辆的实际位姿和参考位姿，qc=(vc,ωc)T代表当前时刻实际速度和横摆角速度，qr=(vr,ωr)T代表当前时刻的参考速度和横摆角速度。

图6 车辆位姿误差模型

当前车辆坐标系中，根据坐标变换公式可得位姿误差方程为：

式中，xe、ye、φe分别为纵向、横向、方向角误差；φ为方向角。

进一步，可得位姿误差微分方程为：

式中，ω为横摆角速度。

4.2 轨迹跟踪控制器设计

车辆轨迹跟踪是对任意时刻初始误差Pe=(xe,ye,φe)T，寻找控制输入qc=(vc,ωc)T，使其在该控制作用下有界，且

本文在Backstepping 方法的基础上，设计滑模控制器切换函数为：

使s1→0、s2→0，然后使得xe→0、φe→-arctan(vrye)，因此ye→0，φe→0。

选择等速趋近率：

式中，k为常数向量；sgn 为符号函数。

为减小抖动，采用连续函数取代式（16）中的符号函数：

式中，ki为向量k中的变量；δi为正的小量。

令α=arctan(vrye)，将式（17）代入式（15）可得：

最后可得到控制律为：

5 仿真分析

搭建CarSim-Prescan-MATLAB 联合仿真平台开展仿真分析，车长度为4.2 m，宽度为1.6 m，仿真工况为城市环境道路。

5.1 并行换道轨迹规划仿真分析

表1 和表2 描述了换道开始时刻的车辆状态，其中起始位置是指以换道车辆为坐标原点，周围车辆与换道车辆的纵向位置关系，换道车辆前方为正，后方为负。

表1 向左换道场景

表2 向右换道场景

并行换道轨迹规划方法可自主决策出换道结束时刻换道车辆与目标车道车辆的相对位置关系。其中工况1和工况3最终状态为Ld、Fd之间，工况2最终状态为Fd、Fd-F之间，工况4 最终状态为Ld、Ld-L之间。该方法可提高换道可能性及轨迹合理性。对于工况2，Fd虽然在后方，但距离较近且速度较快，在典型换道情景下要加速换道至Fd前方，将无法换道。对于工况4，虽然Ld在前方，但距离较近且速度较慢，典型换道情景与本文方法分别认为应减速换道于Ld之后和换道于Ld之前，所得参数如表3所示，后者的|ax,max|、|ay,max|、|jx,max|、|jy,max|和J均小于前者，更加合理。

表3 工况对比

5.2 碰撞检测与并行轨迹重规划仿真分析

在换道开始后，障碍车辆状态可能发生非预期改变，从而引起碰撞。因此，为保证换道安全，需要验证本算法在该情况下的有效性。换道开始时刻的场景如表4所示。

表4 轨迹重规划场景

轨迹重规划仿真结果如图7、图8所示，并将换道开始后的时刻标记于图上相应位置。在工况5 与工况6中，换道开始后Fd速度分别变为15 m/s 和22 m/s，按原轨迹行驶将会发生碰撞，如图7a 和图8a 所示。本文提出的碰撞检测方法检测到碰撞后，并行轨迹重规划器将发送新轨迹到轨迹跟踪器，以避开碰撞，如图7b和图8b所示。

图7 向左换道，重规划向目标车道

图8 向左换道，重规划向起始车道

仿真结果表明，提出的碰撞检测与并行轨迹重规划算法可在换道过程中进行碰撞检测，并重新规划轨迹避开潜在碰撞情况，保证了换道过程的安全。

5.3 轨迹跟踪仿真分析

为研究改进轨迹跟踪算法与文献[3]中原轨迹跟踪算法的区别，设立的仿真工况参数如表5 所示，结果如图9、图10所示。

表5 换道轨迹参数

图9 低速换道轨迹跟踪误差

比较图9 与图10 可知，在低速换道轨迹跟踪中，初始误差在两种算法的作用下都可在较短时间内收敛，但在高速换道轨迹跟踪中，横向误差在原算法作用下收敛较慢，而在改进算法中会逐渐收敛到0，所以改进的轨迹跟踪方法适用性更强。

图10 高速换道轨迹跟踪误差

6 结束语

本文研究了基于车车通信的换道轨迹规划和换道轨迹跟踪算法，提出并行换道轨迹规划方法，和碰撞检测与并行轨迹重规划方法，并对换道轨迹跟踪方法进行改进，增加了换道的可能性和轨迹合理性，提高了换道安全性。