一种基于顶点风险评估的车辆分段路径规划方法

本发明属于车辆路径规划领域，涉及一种自动驾驶车辆路径规划方法。

背景技术：

1、近年来,智能汽车的安全、高效驾驶已经成为汽车产业未来的发展趋势，利用道路交通信息和车辆状态信息提升交通运输的安全性、改善汽车的出行效率和智能化水平的自动驾驶技术受到极大关注。自动驾驶车辆需要完成的任务主要包括环境感知和定位、决策制定与路径规划、运动控制。

2、作为自动驾驶车辆的核心功能模块之一，路径规划在上层感知和下层控制之间发挥着桥梁作用，具有极其重要的意义。结合全局路径和周围环境信息规划出的局部路径对驾驶质量有很大的影响。路径规划的目的是为自动驾驶车辆生成更安全、更舒适且符合驾乘人员期望的路径。

3、目前，现有技术已经对自动驾驶局部路径规划技术从不同的角度开展了研究，针对路径规划方法主要有以下几类：基于图搜索的方法、离散优化方法、人工势场方法和曲线插值方法等。例如，公开号为cn113029151b的中国发明，《一种智能车辆路径规划方法》，公开了利用场理论的混合视角实现了自动驾驶车辆的路径规划；公开号为cn115391977a的发明申请，《一种基于动态优化算法的动态车辆路径规划方法》，采用动态多目标优化问题解决了传统多目标优化问题的不足，实现了车辆的路径规划。

4、基于现有的路径规划方法相关的论文、专利、著作等文献进行分析，当前自动驾驶车辆的路径规划存在如下不足：1)道路风险势场和障碍车辆风险势场是基于主车和障碍车几何中心建立的，并不能正确地表征车辆最容易发生碰撞的四个顶点的风险大小；2)规划方法将单个曲线作为整个局部路径，这不能满足驾乘人员首先避免碰撞，然后舒适地行驶到路径中心线的期望；3)分段路径规划的中转点选择不能同时保证对道路空间的最大化利用和对驾乘人员期望的分阶段实现。

技术实现思路

1、有鉴于此，本发明的目的在于提供一种基于顶点风险评估的车辆分段路径规划方法，为自动驾驶车辆提供更安全、更舒适且符合驾乘人员期望的路径。

2、为达到上述目的，本发明提供如下技术方案：

3、本方案中的基于顶点风险评估的车辆分段路径规划方法，包括以下步骤：

4、s1、实时获取主车信息、障碍车辆信息和道路信息；根据所述主车信息、障碍车辆信息和道路信息，计算候选中转点航向角集和候选中转点集；

5、所述主车信息包括主车几何信息、主车运动信息和碰撞点信息；所述障碍车辆信息包括障碍车辆运动信息和障碍车辆几何信息；所述道路信息包括主车所在车道、障碍车辆所在车道、车道数、车道宽度和车道中心线位置；

6、s2、根据所述障碍车辆信息和所述自车信息，建立针对顶点的总风险势场分布描述函数；

7、s3、根据车辆当前位姿和候选中转点集，建立四次多项式系数求解矩阵，得到第一段候选路径集针对中的每条路径，依次判断主车顶点与障碍车包络框是否有重叠，以及主车顶点是否超出道路边界，若任意一项判断结果为是，则将中相应路径删除，得到第一段候选路径集

8、以第一段路径的平均曲率和平均势场值为属性参数，采用topsis方法(techniquefor order preference by similarity to an ideal solution，逼近理想解排序方法)从第一段候选路径集中计算得到第一段最优路径，同时从候选中转点集中取出相应的最优中转点；

9、s4、根据最优中转点和所述障碍车辆信息，采用沿道路方向等间隔遍历的方式在目标车道中心线上搜索可行的候选终点集，并建立五次多项式系数求解矩阵，得到第二段候选路径集针对中的每条路径，判断主车顶点是否超出道路边界，若结果为是，则将中相应路径删除，得到第二段候选路径集

10、以第二段路径的峰值曲率和路径长度为属性参数，采用topsis方法从第二段候选路径集中计算得到第二段最优路径，同时从候选终点集中取出相应的最优终点；

11、s5、组合所述第一段最优路径和所述第二段最优路径，得到本次规划的最优路径。

12、进一步，在所述s1中，所述碰撞信息包括碰撞点p0；

13、当障碍车辆静止时，p0的坐标为：

14、

15、其中，(xobs,yobs)为前方障碍车辆的质心坐标，lr_obs为障碍车的质心距离车后方的距离，wobs为障碍车的宽度，γ为碰撞点与前方障碍车辆最大概率发生碰撞一侧的后方顶点；

16、当障碍车辆运动时，p0的坐标为：

17、

18、其中，vobs为前方障碍车的速度，为保证碰撞安全的系数，，xini为主车质心的横轴坐标，lf为主车的质心距离车前方的距离，ve为主车在规划路径时刻的速度。

19、进一步，在所述s1中，候选中转点的航向角位于之间，且同时满足和

20、以为步长，则候选中转点航向角集为：

21、

22、根据候选中转点航向角集和碰撞点p0计算候选中转点集为：

23、

24、其中，pmid_set为候选中转点集，即在碰撞点p0下候选中转点航向角集对应的主车质心坐标集，lr为主车的质心距离车后方的距离，we为主车宽度。

25、本方案以几何或运动学模型选定碰撞点位置，没有复杂的运算，计算简单效率高，且可以最大限度地满足第一段路径的避撞要求。

26、进一步，在所述s2中，总风险势场分布描述函数ptotal的计算方法为：

27、ptotal＝proad+pobs   (5)

28、其中，proad表示道路风险势场，pobs表示前方障碍车辆风险势场；

29、proad的计算方法为：

30、

31、其中，sgn为符号函数，y为道路中某点的纵轴坐标，yl、yr为主车沿左右两条车道中心线行驶时车辆左右边缘的纵轴坐标，δyl、δyr表示yl、yr分别与左右道路边缘的距离，主车顶点超出道路边界时将proad设置为一个非常大的数值，该数值满足大于等于100；

32、pobs的计算方法为：

33、

34、其中，ve为自车速度，ε为用于防止分母为0的非常小的数字，满足ε小于等于0.01,且ε＞0，x为道路中某点的横轴坐标，δx、δy分别为横轴方向和纵轴方向的标准偏差系数，yel_l、yel_r分别为以障碍车的长度和宽度为矩形包络框的左右边纵轴坐标，主车顶点与障碍车包络框有重叠时将pobs设置为100。

35、进一步，在所述s3中，四次多项式系数求解方法为

36、

37、其中，xe,ye,分别为自车在规划路径时刻的纵坐标、横坐标和航向角，xmid、ymid来自于候选中转点集pmid_set，来自于候选中转点航向角集p1为某次基于四次多项式系数求解矩阵得到的候选路径系数；计算过程中，每次在候选中转点集和候选中转点航向角集中取出一组对应的参数带入式(8)计算p1，直到取出所有参数后停止遍历，得到第一段候选路径集该集合由每次计算得到的p1组成；

38、根据主车几何信息计算每条候选路径任意点对应的四个顶点坐标：

39、

40、其中：xg,yg,分别为自车质心在任意点的横坐标、纵坐标和航向角；

41、针对第一段候选路径集中的所有曲线，以xs1_step为步长在横轴上进行采样，则平均曲率和平均势场值的计算方法为：

42、

43、

44、其中，n为每条候选路径的采样点个数，ki为第i个采样点处路径的曲率值，ri为第i个采样点处车辆四个顶点的势场值。

45、本方案在第一段路径采用平均曲率和平均势场值为属性进行决策，满足驾乘人员希望主车能够避免与障碍车发生碰撞的同时，最大限度地考虑路径的平滑度；采用topsis多属性决策方法，避免了寻找固定加权系数的问题。

46、进一步，在所述s4中，五次多项式系数求解方法为

47、

48、其中：xfin,yfin,分别为规划的第二段路径终点的横坐标、纵坐标和航向角；p2为某次基于五次多项式系数求解矩阵得到的候选路径系数，该系数完整表示了该候选路径；计算过程中，每次在候选终点集中取出一组参数带入上式计算p2，直到取出所有参数后停止遍历，得到第二段候选路径集该集合由每次计算得到的p2组成；

49、针对第二段候选路径集中的所有曲线，以xs2_step为步长在横轴上进行采样，则峰值曲率kmax和和路径长度l的计算方法为：

50、kmax＝max(ki)   (13)

51、

52、其中，ki为第i个采样点处路径的曲率值，p2为每条曲线的多项式系数，计算方法如前所述。

53、本方案在第二段路径采用峰值曲率和和路径长度为属性进行决策，满足驾乘人员希望主车可以又快又稳地行驶到目标车道的目标。

54、进一步，判断主车顶点与障碍车包络框是否有重叠的方法为：

55、计算主车顶点所在位置的障碍车辆风险势场值，若任意顶点该值大于2，则判断二者重叠；

56、判断主车顶点是否超出道路边界的方法为：

57、计算主车顶点所在位置的道路风险势场值，若任意顶点该值大于2，则判断超出了道路边界。

58、本发明的有益效果在于：

59、本方案基于主车的四个顶点建立了道路风险势场函数，可以更好地表征车辆最危险位置的风险大小，也可以定量评估车辆在行驶过程中由航向角引发的风险变化，避免了传统风险势场仅仅评估车辆质心位置风险和不能表征航向角对风险的影响等缺陷；基于障碍车辆轮廓建立了障碍车辆风险势场函数，可以更好地表征主车顶点在接近障碍车过程中的风险变化趋势；基于车辆顶点的风险评估方式更加贴近真实的驾驶场景，因为单一的基于质心位置的风险评估不能同时表征车辆最容易发生碰撞的四个顶点的风险状况；采用两段式分段路径规划方法，可以保证每段路径规划可以与驾乘人员的期望保持一致——驾乘人员首先希望主车能够避免与障碍车发生碰撞，当主车行驶到中转点后，与障碍车已不可能发生碰撞，此时驾乘人员希望主车可以又快又稳地行驶到目标车道；所述的中转点寻找方法和候选路径筛选方法，既可以实现对道路空间的最大化利用，又可以实现对驾乘人员期望的分阶段实现。

60、本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。