一种基于车车协同下无人驾驶车辆的换道轨迹规划方法与流程

本发明属于汽车主动安全与辅助驾驶领域，特别是涉及一种基于车车协同下无人驾驶车辆的换道轨迹规划方法。

背景技术：

在无人驾驶环境下，车辆对周围交通状态的感知和车辆实时轨迹控制是无人驾驶车辆安全高效运行的重要因素。其中轨迹控制就是通过轨迹规划得到平滑连续的曲率轨迹，使之满足车辆的运动特性和安全要求。现有的轨迹规划是对机器人研究领域的路径规划方法的拓展，一类是全局轨迹方法，提出寻找连接起点到目标点的全局轨迹，由于该类方法需要对行驶环境变化进行全面分析，耗时很长且不利于处理突发情况，因此难以在无人驾驶车辆的轨迹规划中实现。另一类局部轨迹方法是在全局轨迹的引导下，依靠环境感知信息生成实时轨迹，可广泛应用于无人驾驶车辆的轨迹规划中。

无人驾驶车辆的轨迹规划包括车辆跟驰、换道和超车几种情况。其中跟驰轨迹规划方法的研究在传统成熟的跟驰模型下已蓬勃发展，部分相关技术已得到应用，超车行为可以看作两次换道行为，因此车辆换道轨迹规划是无人驾驶车辆轨迹规划研究的重点内容之一。现常用于无人驾驶车辆的换道轨迹规划方法有贝塞尔曲线、样条曲线和多项式曲线。贝塞尔曲线生成的换道轨迹具有连续的曲率半径，但是需要选取控制点，只适用于静态规划，难以实现换道中的实时避碰行为。样条曲线可以对圆弧换道轨迹、正弦换道轨迹进行再规划，克服原有曲线曲率突变、不连续等缺点，但会导致无法实现实时控制。多项式曲线计算速度快，曲率连续便于实时控制，通过三次微分可得出舒适度方程。

在无人驾驶环境下，车辆大多处于高速行驶状态，且车速稳定，车辆之间的跟驰距离很小，给无人驾驶车辆的换道带来困难。虽然超车换道等非转向换道会减少甚至避免，但道路出入口的强制换道和城市路段的转向换道是不可避免的，所以本发明立足于车车协同的思想，依靠车辆之间的信息交流，简化并实现无人驾驶环境下车辆安全舒适换道。

技术实现要素：

针对现有技术的不足，本发明提供了一种基于车车协同下无人驾驶车辆的换道轨迹规划方法，以换道协同策略和五次多项式的轨迹规划方法为基础，以车辆运动学和舒适度为控制条件，建立目标车道后车不同协同程度下的主车换道轨迹优化模型和换道轨迹评价费用模型，可以规划设计出无人驾驶环境下车辆安全换道的最优轨迹。

为了实现上述技术目的，本发明采用如下技术方案：

一种基于车车协同下无人驾驶车辆的换道轨迹规划方法，其特征在于，具体包括以下步骤：

步骤一：根据安全避碰原则建立车车协同换道准则：

主车换道前发出换道请求，周边车辆接收到主车发出的换道请求信号后是否接受换道请求的准则为：

(101)当前车道后车、目标车道前车若保持当前车速匀速行驶不会与主车相碰撞，则接受主车的换道请求，并保持当前车速行驶，直至主车换道成功，若会与主车相撞，拒绝主车的换道请求；

(102)当前车道前车、目标车道后车若保持当前车速匀速行驶不会与主车相碰撞，则接受主车的换道请求，并保持当前速度匀速行驶，直至主车换道成功；若会与主车相碰撞，判断与其后车的位置关系和协同程度的要求，在可接受的协同程度下加速或减速，并判断协同行为后是否与主车发生碰撞，若无碰撞，接受换道请求，并在预定协同程度下加速或减速协同，直至主车换道成功，若协同行为后仍与主车碰撞，拒绝主车的换道请求；

步骤二：建立主车、当前车道前车、目标车道后车的行驶轨迹模型：

以主车换道前所在的位置为原点，换道前车辆行驶的方向为x轴，垂直于行驶方向为y轴，建立平面直角坐标系，建立主车、目标车道前后车的行驶轨迹模型；主车换道运动轨迹方程，运动轨迹方程包括：

(201)采用五次多项式建立主车m换道的行驶轨迹：

其中，xm(t)和ym(t)分别表示m的纵向位移和横向位移；ai,i∈{0,1,2,3,4,5}和bi,i∈{0,1,2,3,4,5}分别是是m纵向位移和横向位移的参数；t表示时间。根据换道起始状态和换道结束状态求解五次多项式；

(202)当前车道前车lo的行驶轨迹：

(203)目标车道后车fd的行驶轨迹：

其中，分别表示m与lo、fd之间换道前的车头间距；分别表示lo、fd的初始行驶速度；w表示车辆fd到m的横向距离；表示fd的减速度；表示fd通过减速达到可接受速度的时间，当fd匀速行驶时，

步骤三：选取矩形车辆模型，根据安全避碰原则建立车车协同安全换道条件的计算模型：

(301)主车m与当前车道前车lo的安全换道的条件：

(302)主车m与目标车道后车fd的安全换道的条件：

其中，分别表示lo的车长与车宽，与分别表示fd的车长与车宽；是车辆沿质心旋转的半径；α为车辆的偏转角，为主车m的矩形模型顶点与水平方向的夹角；

步骤四：根据车辆动力学和乘客舒适度两方面的因素对主车安全换道模型中的参数进行约束，给定测试参数，计算得到换道轨迹集，为车车协同下无人驾驶车辆的换道轨迹：

(401)动力学约束条件：

转向角：δα为车辆单位时间δt内变化的转向角，δαmax为车辆设置的在不影响驾驶安全性能下，单位时间δt内变化的最大转向角；为m在t时刻的横向速度，为m在t时刻的纵向速度；

曲率：k为曲率，r为转弯半径，zm为主车的轴距，kmax为不影响驾驶安全性能的最大曲率；

速度：0<vx,m(t)<vx,max，vx,m(t)为m纵向的行驶速度，vx,max为路段上限制的最大纵向行驶速度；

位置：0<ym(t)<w，ym(t)为m的横向位移，w为车道的宽度；

(402)乘客舒适度约束条件，采用横纵向的加速度和加加速度来判断：

其中，ax和jx分别为纵向的加速度和加加速度；ay和jy分别为横向的加速度和加加速度；ax,max和jx,max为考虑舒适度而允许的最大纵向加速度和加加速度；ay,max和jy,max为考虑舒适度而允许的最大横向加速度和加加速度；

步骤五：综合人的舒适程度、换道的纵向位移和换道时间三个因素建立评价换道轨迹影响程度的费用函数模型，计算得到最优的换道轨迹，为车车协同下无人驾驶车辆的换道最优轨迹：

(501)目标车道后车fd匀速协同时的费用函数：

其中，w1、w2、w3是权重系数，其和为1；max(j)、max(xc)和max(tc)分别为可行换道轨迹集中的舒适度函数、纵向换道距离、换道时间的最大值；其中人的舒适度计算函数为

(502)目标车道后车fd减速协同时的费用函数

其中，λ1、λ2、λ3、λ4是权重系数，其和为1；xfd,slow是换道时间tc内fd减速协同的距离，xfd,con是换道时间tc内如果fd保持匀速行驶的距离；

步骤六：由费用函数计算得出的主车最优换道轨迹的最终状态，根据主车换道前的初始状态和最终状态求解公式(1)得出最优换道轨迹方程。

本发明的有益效果在于：

本发明提出的一种基于车车协同下无人驾驶车辆的换道轨迹规划方法；首先由五次多项式生成无人驾驶车辆换道轨迹，根据安全避碰原则，建立矩形车辆模型条件下的安全换道的边界条件模型，然后通过满足车辆动力学和乘客舒适度的要求作为控制条件，求解得到无人驾驶车辆的实际换道轨迹范围，最后建立轨迹评价费用函数，得到最优换道轨迹，运用本发明方法能够完成无人驾驶环境下的车辆安全换道，同时满足车辆换道舒适性以及动力学的要求。

附图说明

图1为本发明一种基于车车协同下无人驾驶车辆的换道轨迹规划方法中主车与周围其他车辆的相对位置示意图。

图2为本发明一种基于车车协同下无人驾驶车辆的换道轨迹规划方法中车车协同的过程示意图。

图3(a)为本发明一种基于车车协同下无人驾驶车辆的换道轨迹规划方法中车辆m与lo的潜在碰撞形式示意图。

图3(b)为本发明一种基于车车协同下无人驾驶车辆的换道轨迹规划方法中车辆m与fd的潜在碰撞形式示意图。

图4为本发明一种基于车车协同下无人驾驶车辆的换道轨迹规划方法中4种车辆模型。

图5为本发明一种基于车车协同下无人驾驶车辆的换道轨迹规划方法中车辆碰撞关系示意图。

具体实施方式

如图1所示，主车m在换道过程中，周围车辆主要有当前车道前车lo，当前车道后车fo，目标车道前车ld和目标车道后车lo。一种基于车车协同下无人驾驶车辆的换道轨迹规划方法包括以下步骤：

第一步，主车换道前发出换道请求，建立周边车辆接收到主车发出的换道请求信号后是否接受换道请求的准则：

当前车道后车fo、目标车道前车ld若保持当前车速匀速行驶不会与主车相碰撞，则接受主车的换道请求，并保持当前车速行驶，直至主车换道成功，若会与主车相撞，拒绝主车的换道请求。

当前车道前车lo、目标车道后车fd若保持当前车速匀速行驶不会与主车相碰撞，则接受主车的换道请求，并保持当前速度匀速行驶，直至主车换道成功；若会与主车相碰撞，需判断与其后车的位置关系和协同程度的要求，在可接受的协同程度下加速或减速，并判断协同行为后是否与主车发生碰撞，若无碰撞，接受换道请求，并在预定协同程度下加速或减速协同，直至主车换道成功，若协同行为后仍与主车碰撞，拒绝主车的换道请求。当前车道前车、目标车道后车与主车的车车协同过程见图2。

根据车车协同策略，换道车辆之间的避碰发生在主车与当前车道前车和目标车道后车之间。

第二步，建立主车m、当前车道前车lo、目标车道后车lo的行驶轨迹模型。以m换道前所在的位置为原点，换道前车辆行驶的方向为x轴，垂直于行驶方向为y轴，建立平面直角坐标系。

考虑到多项式曲线计算速度快，曲率连续便于实时控制，通过三次微分可得出舒适度方程，采用五次多项式建立主车m换道的行驶轨迹：

其中，xm(t)和ym(t)分别表示m的纵向位移和横向位移；ai,i∈{0,1,2,3,4,5}和bi,i∈{0,1,2,3,4,5}分别是是m纵向位移和横向位移的参数；t表示时间。根据换道起始状态和换道结束状态求解五次多项式。

起始状态：

结束状态：

其中，x0和y0表示m在起始状态的横向和纵向位移，vx，0和vy，0表示m在起始状态的横向和纵向速度，ax，0和ay，0表示m在起始状态的横向和纵向加速度；xc和yc表示换道结束时的横向和纵向位移，vx，c和vy，c表示m在换道结束时的横向和纵向速度，αx，c和ay，c表示m在换道结束时的横向和纵向加速度，tc为换道时间。

当前车道前车lo的行驶轨迹：

目标车道后车fd的行驶轨迹:

其中，分别表示m与lo、fd之间换道前的车头间距；分别表示lo、fd的初始行驶速度；w表示车辆fd到m的横向距离，文中记为车道的宽度；表示fd的减速度；表示fd通过减速达到可接受速度的时间。

本发明给出车辆碰撞形式，如图3(a)、图3(b)。考虑到各种车辆模型的优缺点，为实现车辆在换道过程中避碰的要求，采用了矩形作为车辆模型，如图4；给定的车辆碰撞关系如图5。

第三步，建立车辆成功安全换道条件如下：

若为当前车道前车协同换道，选用lo与m安全换道的条件进行轨迹计算：

若为目标车道后车协同换道，选用fd与m安全换道的条件进行轨迹计算：

其中，分别表示lo的车长与车宽，与分别表示fd的车长与车宽；是车辆沿质心旋转的角度；α为车辆的偏转角，为主车m的矩形模型顶点与水平方向的夹角；

第四步，根据车辆动力学和乘客舒适度两方面的因素对主车安全换道模型中的参数进行约束，给定测试参数，计算得到换道轨迹集：

运动学要求：

转向角：δα为车辆单位时间δt内变化的转向角，δαmax为车辆设置的在不影响驾驶安全性能下，单位时间δt内变化的最大转向角；为m在t时刻的横向速度，为m在t时刻的纵向速度；

曲率：k为曲率，r为转弯半径，zm为主车的轴距，kmax为不影响驾驶安全性能的最大曲率；

速度：0<vx,m(t)<vx,max，vx,m(t)为m纵向的行驶速度，vx,max为路段上限制的最大纵向行驶速度；

位置：0<ym(t)<w，ym(t)为m的横向位移，w为车道的宽度；

舒适度要求：

其中，ax和jx分别为纵向的加速度和加加速度；ay和jy分别为横向的加速度和加加速度；ax,max和jx,max为考虑舒适度而允许的最大纵向加速度和加加速度；ay,max和jy,max为考虑舒适度而允许的最大横向加速度和加加速度；

第五步，综合人的舒适程度、换道的纵向位移和换道时间三个因素建立评价换道轨迹影响程度的费用函数模型，计算得到最优的换道轨迹，为车车协同下无人驾驶车辆的换道最优轨迹：

目标车道后车fd匀速协同时的费用函数：

其中，w1、w2、w3是权重系数，其和为1；max(j)、max(xc)和max(tc)分别为可行换道轨迹集中的舒适度函数、纵向换道距离、换道时间的最大值。其中人的舒适度计算函数为

目标车道后车fd减速协同时的费用函数：

其中，λ1、λ2、λ3、λ4是权重系数，其和为1；xfd,slow是换道时间tc内fd减速协同的距离，xfd,con是换道时间tc内如果fd保持匀速行驶的距离。

由费用函数计算得出的主车最优换道轨迹的最终状态，根据主车换道前的初始状态和最终状态求解公式(1)得出最优换道轨迹方程。

本实施例选取后车匀速协同换道和减速协同换道两种情况进行分析。给定测试参数如下，所有车辆车长均为4.5m，车宽均为1.8m，车道宽度均为3.5m；m的起始速度vx，0为30km/h，换道到目标车道的速度vx，c为40km/h，lo的速度30km/h；m与lo的车头间距为20m；换道时间设为1-12s；车辆动力学和舒适的约束条件：转向角δα∈(0，20°)；曲率k∈(0，1)；vx，m(t)∈(0，60)；ym(t)∈(0，3.5)；ax，ay的绝对值均小于10；jx，jy的绝对值均小于10。测试以下几种情况下主车换道的安全轨迹：

目标车道后车匀速协同换道时：给定fd的速度40km/h，fd与m的车头间距为20m。带入公式(3)得到fd的行驶轨迹，再将其带入公式(5)，根据公式(5)及约束条件可计算生成符合要求的换道轨迹，见表1。

表1目标车道后车匀速协同换道时满足约束条件的主车安全轨迹集

再根据公式(6)，得到xc＝68.05m，yc＝3.51m，tc＝7.0s时对应的轨迹为最优轨迹，带入到公式(1)的起始、最终状态中，拟合求解最优换道轨迹方程为：

对该轨迹进行动态仿真，主车可以实现无碰安全换道，且换道的曲线较为平缓，满足无人驾驶环境下乘客的舒适度要求。

目标车道后车减速协同换道时：给定测试参数fd协同前速度为48km/h，协同速度为40km/h，fd与m的车头间距为20m。带入公式(3)得到fd的行驶轨迹，再将其带入公式(5)，根据公式(5)及约束条件可计算生成符合要求的换道轨迹，见表2。

表2目标车道后车减速协同换道时满足约束条件的主车安全轨迹集

再根据公式(7)，得到xc＝59.31m，yc＝3.50m，tc＝6.10s时对应的轨迹为最优轨迹，带入到公式(1)的起始、最终状态中，拟合求解最优换道轨迹方程为：

对该轨迹进行动态仿真，主车可以实现无碰安全换道，且换道的曲线较为平缓，满足无人驾驶环境下乘客的舒适度要求。