一种防碰撞的动态智能网联车辆最优换道轨迹规划方法

本发明涉及车辆自动驾驶，具体为一种防碰撞的动态智能网联车辆最优换道轨迹规划方法。

背景技术：

1、随着全球经济的不断发展和城市化进程的加快，道路交通拥堵已成为一个普遍存在的问题，换道作为智能网联车辆最常见的驾驶行为之一，在很大程度上影响道路的流通性和安全性。安全、快速且平稳的换道行为可提高通行效率，减少交通事故的发生。合理的换道轨迹是智能网联车辆安全完成换道任务的前提，同时其性能也决定了换道过程的安全性、高效性和舒适性。

2、自动换道由决策模块、轨迹规划模块、执行模块组成，决策模块利用传感器、v2v通信获取智能网联车辆周围信息进行换道决策判断，轨迹规划模块针对换道决策规划无碰撞、满足车辆动力学约束的换道轨迹，执行模块依据换道轨迹进行换道。

3、现有方法的缺点：将换道看作静态的过程，没有考虑到换道过程中周围车辆的加速、减速，未考虑到实时碰撞的问题，容易造成车辆追尾，不能代表真实的驾驶场景。

技术实现思路

1、(一)解决的技术问题

2、针对现有技术的不足，本发明提供了一种防碰撞的动态智能网联车辆最优换道轨迹规划方法，具备换道舒适安全等优点，解决了上述技术问题。

3、(二)技术方案

4、为实现上述目的，本发明提供如下技术方案：一种防碰撞的动态智能网联车辆最优换道轨迹规划方法，包括以下步骤：

5、s1、建立智能网联车辆m基于五次多项式的换道轨迹规划方程；

6、s2、构建智能网联车辆m换道舒适度函数、换道效率函数以及总成本函数；

7、s3、构建智能网联车辆m换道的车辆动力学约束和交通规则约束；

8、s4、通过步骤s2建立的函数与步骤s1建立的轨迹规划方程进行配合，并在步骤s3的约束下，采用序列二次方法求解非线性规划问题，计算出最优换道轨迹；

9、s5、智能网联车辆m执行碰撞检测，判断下一时间步长是否存在碰撞风险；若下一时间步长不存在碰撞风险，则执行步骤s6；若下一时间步长存在碰撞风险，则执行步骤s7；

10、s6、智能网联车辆m以固定时间步长依据最优换道轨迹执行换道并判断换道是否完成，若换道完成，则智能网联车辆m完成车道变更；若换道未完成，则智能网联车辆m以所在位置为起点，执行步骤s1-s4，重新规划最优换道轨迹；

11、s7、智能网联车辆m以当前位置为起点，执行步骤s1-s4，计算智能网联车辆m返回原始车道的最优换道轨迹，依据返回原始车道的最优换道轨迹返回原始车道。

12、作为本发明的优选技术方案，所述步骤s1的轨迹方程建立过程具体步骤如下：

13、s1.1、换道运动解耦为两个正交方向：纵向(x方向)和横向(y方向)，利用五次多项式构建智能网联车辆m每个方向上的轨迹规划方程；

14、s1.2、建立智能网联车辆m在换道初始时刻ti和最终时刻tf的位移、速度和加速度方程；

15、s1.3、对相关变量进行初始化；

16、s1.4、对步骤s1中的轨迹规划方程系数进行计算。

17、作为本发明的优选技术方案，所述步骤s1.1中的轨迹规划方程表达式：

18、

19、其中，x(t)和y(t)分别表示车辆在t时刻的纵向位置和横向位置，ai(i＝1,2,3,4,5)和bi(i＝1,2,3,4,5)表示待确定系数，所述步骤s1.2中位移、速度和加速度方程表达式如下：

20、

21、

22、其中，x(ti)、分别表示x方向车辆换道初始时刻ti的位置、速度、加速度，x(tf)、分别表示x方向车辆换道最终时刻tf的位置、速度、加速度，y(ti)、分别表示y方向车辆换道初始时刻ti的位置、速度、加速度，y(tf)、分别表示y方向车辆换道最终时刻tf的位置、速度、加速度。

23、作为本发明的优选技术方案，所述步骤s1.3的初始化表达式如下：

24、

25、其中，xf为x方向上车辆的最终位置，wlane为车道宽度，vm、vc分别表示当前车辆和车辆目标车道前车的速度，所述步骤s1.4对ai(i＝1,2,3,4,5)和bi(i＝1,2,3,4,5)进行求解。

26、作为本发明的优选技术方案，所述步骤s2中的换道舒适度函数jcomfort的表达式如下：

27、

28、

29、其中，w1和w2为权重系数，jx(t)为t时刻x方向加加速度，jy(t)为t时刻y方向加加速度，jx,max、jy,max分别表示x方向、y方向的最大加加速度，ti、tf分别表示车辆换道初始时刻、换道最终时刻，ax,max、ay,max分别表示x方向、y方向的最大加速度，表示对内部函数进行积分，所述步骤s2中的换道效率函数jefficiency的表达式如下：

30、jefficiency＝w3(xf-xi)+w4(tf-ti)

31、其中，w3和w4为权重系数，xi、xf分别表示智能网联车辆m换道初始时刻的x方向位置、换道最终时刻的x方向位置，所述步骤s2中的总成本函数cost(xf，xi)的表达式如下：

32、cost(xf，xi)＝jefficiency+jcomfort

33、作为本发明的优选技术方案，所述步骤s3中的车辆动力学约束和交通规则约束的具体表达式如下：

34、

35、

36、其中，vx,max、vy,max分别为x方向、y方向的最大速度，ax,max、ay,max分别为x方向、y方向的最大加速度，jx,max、jy,max分别为x方向、y方向的最大加加速度，s.t.表示约束，表示对内部函数取最小值。

37、作为本发明的优选技术方案，所述步骤s5碰撞检测的过程具体如下：

38、s5.1、采用动态矩形对智能网联车辆m进行建模；矩形模型的中心作为智能网联车辆m的偏转中心，偏转中心坐标为(xm(t),ym(t))，智能网联车辆m偏转前的矩形顶点坐标表示为(xo(t),yo(t)),o＝1,2,3,4，偏转后的矩形顶点坐标表示为(xo'(t),yo'(t))，o＝1,2,3,4，其中o＝1对应的点表示智能网联车辆m矩形模型的右前方顶点，o＝2对应的点表示智能网联车辆m矩形模型的左前方顶点，o＝3对应的点表示智能网联车辆m矩形模型的左后方顶点，o＝4对应的点表示智能网联车辆m矩形模型的右后方顶点；

39、s5.2、对各个点的坐标进行计算；

40、s5.3、对智能网联车辆m矩形模型的边框定点进行设置并定义；

41、s5.4、对智能网联车辆m和智能网联车辆m原始车道前车a之间的安全约束进行计算；

42、s5.5、对智能网联车辆m和智能网联车辆m目标车道前车c之间的安全约束进行计算；

43、s5.6、对智能网联车辆m与智能网联车辆m目标车道后车b之间的安全约束进行计算。

44、作为本发明的优选技术方案，所述步骤s5.2的计算公式如下：

45、

46、

47、

48、其中，r表示偏转中心与每个矩形顶点之间的距离，l是智能网联车辆长度，w是智能网联车辆宽度，θ(t)表示智能网联车辆的偏离角，a为矩形模型(x1(t),y1(t))与水平方向之间夹角，cos(*)表示余弦函数，sin(*)表示正弦函数，所述步骤s5.3中矩形模型的参数设置如下：

49、偏转后智能网联车辆m矩形模型的左侧边框为l2，右侧边框为l5，前侧边框为l4，智能网联车辆m原始车道前车a矩形模型的后侧边框为l1，智能网联车辆m目标车道前车c矩形模型的左侧边框为l3，智能网联车辆m目标车道后车b矩形模型的前侧边框为l6，假设偏转后智能网联车辆m矩形模型的左前侧顶点为点1，右前侧顶点为点3，右后侧顶点为点6，偏转后智能网联车辆m原始车道前车a矩形模型的右后侧顶点为点2，偏转后智能网联车辆m目标车道前车c矩形模型的左后侧顶点为点4，偏转后智能网联车辆m目标车道后车b矩形模型的左前侧顶点为点5。

50、作为本发明的优选技术方案，所述步骤s5.4中的智能网联车辆m和智能网联车辆m原始车道前车a之间的安全约束表达式如下：

51、

52、

53、其中，xm(t)、ym(t)为智能网联车辆m的中心坐标，xa(t)、ya(t)为智能网联车辆m原始车道前车a的中心坐标，tanα表示对偏转角α的正切函数，la表示智能网联车辆m原始车道前车a长度，dm,safe1(t)为智能网联车辆m与智能网联车辆m原始车道前车a的最小安全距离，所述步骤s5.5智能网联车辆m和智能网联车辆m目标车道前车c之间的安全约束表达式如下：

54、

55、

56、其中，xc(t)、yc(t)为智能网联车辆m目标车道前车c的中心坐标，dm,safe2(t)表示智能网联车辆m与智能网联车辆m目标车道前车c的最小安全距离，表示对进行正切运算，所述步骤s5.6中的智能网联车辆m与智能网联车辆m目标车道后车b之间安全约束表达式如下：

57、

58、

59、其中xb(t)、yb(t)智能网联车辆m目标车道后车b的中心坐标，dm,safe3(t)为智能网联车辆m与智能网联车辆m目标车道后车b的最小安全距离。

60、作为本发明的优选技术方案，所述步骤s5中的碰撞检测存在不满足安全约束的情况下，智能网联车辆m将立即中止换道，并开始规划最优换道轨迹以返回原始车道。

61、与现有技术相比，本发明提供了一种防碰撞的动态智能网联车辆最优换道轨迹规划方法，具备以下有益效果：

62、1、本发明将最优换道轨迹规划和实时碰撞检测相结合，计算出每一时间步长的最优换道轨迹，并对每一时间步长内的潜在碰撞风险进行碰撞检测，若存在潜在碰撞，则计算返回原始车道的最优轨迹返回原始车道；可实现换道过程的实时避撞，有利于提升换道行为的安全性与舒适性。

63、2、本发明通过用五次多项式曲线建立换道轨迹，该曲线具有四阶平滑度，智能网联车辆的位置、速度、加速度和加加速度在整个换道过程中是连续的，换道轨迹具有很好的舒适性。