一种用于自动驾驶车辆的避障路径规划方法

1.本发明涉及路径规划技术领域，特别涉及一种用于自动驾驶车辆的避障路径规划方法。


背景技术：

2.随着自动驾驶技术的不断发展，路径规划已经成为众多学者研究的热点问题，其主要思路是根据能耗、路程、时间等性能指标，在已知车辆的起始状态、目标状态、和环境中障碍物的前提下，规划出一条由起始位置到目标位置的满足车辆动力学约束的无碰撞最优路径。
3.路径规划又可以分为两大类，分别是全局路径规划和局部路径规划。在环境已知情况下搜索全局最优路径是全局路径规划，在环境未知情况下的避障路径规划是局部路径规划。目前常用的路径规划算法有：a*算法(a-star算法)、神经网络、遗传算法、粒子群算法、蚁群算法、人工势场法等。人工势场法是由khatib于1986年提出的，其思想是通过障碍物的斥力场和目标的引力场共同作用形成虚拟的势场，通过势函数的叠加搜索一条避开障碍物的路径。但是人工势场法仍然存在局部最优、路径振荡以及无法到达目标点等问题。因此针对人工势场法在遇到大型障碍物时路径振荡和规划失败的问题，利用局部栅格法结合a-star算法完成大型障碍物的避障，相比a-star算法花费时间更少，但是上述算法规划的路径较长，并不是最优的路径。


技术实现要素：

4.本发明的目的在于，提供一种用于自动驾驶车辆的避障路径规划方法。本发明可以在复杂环境中更快的规划出一条无碰撞且平滑的自动驾驶车辆路径，具有路径生成时间更短，效率更高的优点。
5.本发明的技术方案：一种用于自动驾驶车辆的避障路径规划方法,包括a-star算法和人工势场法，按如下步骤进行：
6.步骤a、对a-star算法进行改进形成二次a-star算法，用于缩短路径长度，提高路径规划安全性；
7.步骤b、采用正反向交替搜索机制对人工势场法加以改进，形成双向人工势场法，通过双向人工势场法完成双向的路径规划，提高搜索效率；
8.步骤c、规划路径时，利用二次a-star算法和双向人工势场法结合共同进行，同时对二次a-star算法和双向人工势场法在切换时的连接处进行优化，最后利用贝塞尔曲线优化二次a-star算法和双向人工势场法共同规划的路径，使其符合车辆的行驶轨迹。
9.上述的自动驾驶车辆的避障路径规划方法，步骤a中，所述的二次a-star算法是在a-star算法得到路径以及父节点邻域中子节点的代价值后，选取与路径起点不相邻、代价值最小的子节点作为新的父节点，若路径起点与新的父节点之间不存在障碍物同时此时代价值也是最小，则更新路径剔除中间的父节点，并进入下一次搜索；反之，则不更新路径；通
过不断循环搜索，直至找到一条从起点到目标点的路径。
10.前述的自动驾驶车辆的避障路径规划方法，步骤b中，双向人工势场法是增设从终点出发的虚拟车辆，与起点出发的车辆相向而行，避免了出现在目标点附近的障碍物所导致的目标位置无法到达问题，从而完成由起点至终点的无碰撞路径规划。
11.前述的自动驾驶车辆的避障路径规划方法，所述双向人工势场法路径规划的终止条件包括如下：
12.条件(a)、在正向和反向寻路过程中，均实现到达当前目标点；
13.条件(b)、正反向行驶的车辆均受力平衡，位置已经不再发生变化，且未抵达当前目标点；
14.条件(c)、正反向寻路过程中遇到大型障碍物，随受力尚未平衡但在相同的位置反复振荡，且未抵达当前目标点；
15.当满足上述条件(a)(b)或(c)的任意一个时，双向人工势场法运行终止，用于缩短搜索时间。
16.前述的自动驾驶车辆的避障路径规划方法，所述双向人工势场法进行路径规划是分别把路径的起点a和终点b作为正反向搜索的初始位置；正向搜索开始时，以a为起始点，b为目标点，获得下一路径点a1；然后开始反向搜索，以b为起始点，以刚刚获得的正向路径点a1为目标点进行路径规划，此时获得下一路径点b1，以此完成一次交替搜索，如此循环交替前进，直至正反向搜索到相同的路径点，相遇点与两端点之间的路径即最终所求的双向人工势场路径。
17.前述的自动驾驶车辆的避障路径规划方法，步骤s3中，所述的连接处优化是根据二次a-star算法中栅格，将一个栅格分为四个完全相同的小方格，然后根据双向人工势场法路径的前进方向以及终止点在四个小方格中所处的位置确定二次a-star算法的起始点以及目标点，确保二次a-star算法和双向人工势场法的连接处路径规划合理。
18.前述的自动驾驶车辆的避障路径规划方法，步骤s3中，所述的连接处优化是在双向人工势场法由于出现路径点反复振荡现象而终止后，选取包括终止点在内的最后三个路径点，并将他们从路径点中删除，计算得到以三点的坐标为顶点的三角形的外心坐标，并根据该点位置选取二次a-star算法的起始点和目标点，从而避免影响运行结果。
19.前述的自动驾驶车辆的避障路径规划方法，步骤s3中，所述的连接处优化是针对优先陷入局部极小值陷阱的车辆，不再更新其路径，若一段时间内它能脱离该位置，则继续更新其路径。
20.前述的自动驾驶车辆的避障路径规划方法，步骤s3中，所述的连接处优化是在双向人工势场法进行路径规划时，不断调整参数，充分考虑车辆的宽度；当二次a-star算法后，对各个障碍物之间的距离进行计算，若距离小于车辆的宽度，则在障碍物之间增加虚拟障碍物，避免二次a-star算法进行错误的路径规划。
21.与现有技术相比，本发明通过先对传统a-star算法和人工势场法进行改进，形成二次a-star算法和双向人工势场法；在规划路径时，利用二次a-star算法和双向人工势场法结合共同进行，同时对二次a-star算法和双向人工势场法在切换时的连接处进行优化，最后利用贝塞尔曲线优化二次a-star算法和双向人工势场法共同规划的路径，使其符合车辆的行驶轨迹。通过仿真结果表明，本发明可以在复杂环境中更快的规划出一条无碰撞且
平滑自动驾驶车辆路径，具有路径生成时间更短，效率更高的优点。本发明由于人工势场法存在无法避免的陷入局部极小值陷阱的问题，同时路径也会出现振荡现象，导致无法生成完整的路径，因此本发明将二次a-star算法和双向人工势场法进行结合，在自动驾驶车辆陷入局部极小值陷阱后，利用二次a-star算法完成路径规划。通过仿真结果表明，相比人工势场法，本发明能适应各种各样的环境，更有利于车辆在复杂环境中的路径规划。此外，本发明在二次a-star算法的设计中，增加了对车宽的考虑，设计了虚拟障碍物，避免二次a-star算法进行错误的路径规划，从而使所生成的路径的曲率能够满足车辆的转向要求。
附图说明
22.图1为本发明的二次a-star算法流程图；
23.图2是双向人工势场法的搜索示意图；
24.图3是目标点在障碍物附近的算法对比图；
25.图4是特殊三点共线的算法对比图；
26.图5是受力平衡的算法对比图；
27.图6是本发明的具体流程图；
28.图7是传统a-star算法的路径规划示意图；
29.图8是二次a-star算法的路径规划示意图；
30.图9是简单地图的路径规划图；
31.图10是一般地图的路径规划图；
32.图11是复杂地图的路径规划图；
33.图12是复杂地图下贝塞尔曲线优化后的规划路径图。
具体实施方式
34.下面结合附图和实施例对本发明作进一步的说明，但并不作为对本发明限制的依据。
35.实施例：一种用于自动驾驶车辆的避障路径规划方法,包括a-star算法(a-star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法)和人工势场法(人工势场法路径规划是由khatib提出的一种虚拟力法(oussama khatib，real-time obstacle avoidance for manipulators and mobile robots.proc of the 1985ieee.)。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动)，按如下步骤进行：
36.步骤a、对a-star算法进行改进形成二次a-star算法，用于缩短路径长度，提高路径规划安全性；如图1所示，所述的二次a-star算法是在a-star算法得到路径以及父节点邻域中子节点的代价值后，选取与路径起点不相邻、代价值最小的子节点作为新的父节点，若路径起点与新的父节点之间不存在障碍物同时此时代价值也是最小，则更新路径剔除中间的父节点，并进入下一次搜索；反之，则不更新路径；通过不断循环搜索，直至找到一条从起点到目标点的路径。
37.步骤b、采用正反向交替搜索机制对人工势场法加以改进，形成双向人工势场法，
通过双向人工势场法完成双向的路径规划，提高搜索效率；本实施例中，双向人工势场法是增设从终点出发的虚拟车辆，与起点出发的车辆相向而行，避免了出现在目标点附近的障碍物所导致的目标位置无法到达问题，从而完成由起点至终点的无碰撞路径规划。改进过程中需要考虑的问题是双向搜索的自动驾驶车辆最终能否在抵达目标的过程中相遇，理想的情况是相遇的位置在起始点与目标点连线的中点附近，这能最大限度的缩短搜索时间。因此在本实施例中，设置所述双向人工势场法路径规划的终止条件包括如下：
38.条件(a)、在正向和反向寻路过程中，均实现到达当前目标点；
39.条件(b)、正反向行驶的车辆均受力平衡，位置已经不再发生变化，且未抵达当前目标点；
40.条件(c)、正反向寻路过程中遇到大型障碍物，随受力尚未平衡但在相同的位置反复振荡，且未抵达当前目标点；
41.当满足上述条件(a)(b)或(c)的任意一个时，双向人工势场法运行终止，用于缩短搜索时间。
42.本实施例中，采用正反向交替搜索机制对人工势场法加以改进，获得的双向人工势场法在进行路径规划运行时，如图2所示，分别把路径的起点a和终点b作为正反向搜索的初始位置；正向搜索开始时，以a为起始点，b为目标点，获得下一路径点a1；然后开始反向搜索，以b为起始点，以刚刚获得的正向路径点a1为目标点进行路径规划，此时获得下一路径点b1，以此完成一次交替搜索，如此循环交替前进，直至正反向搜索到相同的路径点，相遇点与两端点之间的路径即最终所求的双向人工势场路径。
43.经过双向人工势场法所获得的规划路径如图3所示，对比图中两种算法规划的路径可知，在特定的环境下，本发明采用的双向人工势场法能够解决传统人工势场法存在的障碍物在目标点附近时无法抵达目标位置的问题。
44.步骤c、规划路径时，如图6所示，利用二次a-star算法和双向人工势场法结合共同进行。在两个算法的结合进行路径规划时中，二次a-star算法和双向人工势场法在切换时的连接处有一些需要解决的问题。(1)首先是由于两种算法的运行环境不同造成的问题，二次a-star算法建模环境一般为栅格地图，而双向人工势场法建模环境为任意二维环境，这就造成了a*算法当前位置以及路径点的坐标均为整数，而人工势场法所采用的坐标均为小数。(2)其次是双向人工势场法陷入局部极小值陷阱后，采用二次a-star算法时起始点以及目标点的选择问题，这是由于人工势场法终止可能是遇到大型障碍物而反复振荡造成，此时终止点的选择将会影响后续的算法的运行结果。(3)然后是由于正反向陷入局部极小值陷阱的时间可能不同，这样会导致先陷入陷阱的自动驾驶车辆会在陷阱附近振荡，造成路径不连续很复杂。(4)最后二次a-star算法应考虑障碍物之间的距离进行规划的问题，这是由于本发明考虑到车辆的宽度，而二次a-star算法规划路径时并没有考虑到障碍物之间的距离是否能通过一定宽度的车辆。
45.针对上述问题，本发明分别提出对应的解决方案，具体方法如下。
46.针对问题(1)，本发明根据二次a-star算法中栅格，将一个栅格分为四个完全相同的小方格，然后根据双向人工势场法路径的前进方向以及终止点在四个小方格中所处的位置确定二次a-star算法的起始点以及目标点，确保二次a-star算法和双向人工势场法的连接处路径规划合理。
47.针对问题(2)，本发明在双向人工势场法由于出现路径点反复振荡现象而终止后，选取包括终止点在内的最后三个路径点，并将他们从路径点中删除，计算得到以三点的坐标为顶点的三角形的外心坐标，并根据该点位置选取二次a-star算法的起始点和目标点，从而避免影响运行结果。
48.针对问题(3)，本发明针对优先陷入局部极小值陷阱的车辆，不再更新其路径，若一段时间内它能脱离该位置，则继续更新其路径。
49.针对问题(4)，本发明在双向人工势场法进行路径规划时，不断调整参数，充分考虑车辆的宽度；当二次a-star算法后，对各个障碍物之间的距离进行计算，若距离小于车辆的宽度，则在障碍物之间增加虚拟障碍物，避免二次a-star算法进行错误的路径规划。
50.基于上述改进后，进一步考虑路径的合理性，将该路径经过贝塞尔曲线拟合，得到最终完整路径。
51.为了进一步验证本发明的科学性，利用matlab软件在15*15的栅格地图下进行仿真，传统a-star算法和二次a-star算法在相同的环境下进行仿真，结果如图7和图8所示。由图7和图8中可以发现，利用两种算法都能实现从起点到终点的路径规划，但是采用二次a-star算法路径更短，转弯角度减小，路径更加平滑。
[0052][0053]
表1不同a*算法的仿真数据对比
[0054]
由表1可知，二次a-star算法的路径长度相比传统a-star算法缩短了2.13％，平均转弯角度减少了56.25％，有效缩短了路径，提高了路径的安全性。
[0055]
在简单地图下对比双向人工势场法与传统人工势场法，仿真过程中参数的选取如下：车宽w＝2，引力常数eta
att
＝0.05，斥力常数eta
rep
＝100，障碍物影响范围d0＝20m，步长len
step
＝0.8m，仿真结果如图3、图4、图5所示。
[0056]
图3中，目标点与障碍物之间的距离较近。传统算法在a处受到的引力小，斥力大，出现无法靠近目标点的现象；改进的算法(即本发明实施例的算法)由起始点和目标点依次出发，正反向路径的临时目标点不断发生改变，最后在图中某一处相遇从而完成路径规划。
[0057]
图4为三点共线问题，自动驾驶车辆、障碍物以及目标点在一条直线上。传统算法在规划路径时，在b出现斥力与引力的大小相等但方向相反的情况，导致自动驾驶车辆陷入局部极小值；改进算法进行反向路径规划时，在a受到障碍物的影响，反向路径发生变化，导致车辆、障碍物、目标点不在一条直线上，从而可以完成路径规划。
[0058]
图5为受力平衡问题，传统人工势场法从起点出发到达a后，由于障碍物的合斥力与引力等大反向，陷入局部最优，无法完成路径规划；双向人工势场法从起始点和目标点同时出发，到达a、b后同样陷入局部最优，此时确定a、b为二次a-star算法的起始点和目标点，利用二次a-star算法完成路径规划，连接a、b两点，完成从原始起始点到目标点的路径规划。
[0059]
图9为障碍物较少情况下的路径规划，传统算法的路径在前半段与双向人工势场法的路径基本一致，但是目标点附近存在障碍物，导致传统算法在到达a后无法前进，陷入局部最优，改进算法则顺利完成路径规划。
[0060][0061]
表2相同环境下不同算法仿真数据对比
[0062]
仿真数据对比结果如表2所示，结果表明双向人工势场法结合二次a*算法确实有效解决了自动驾驶车辆无法到达目标、陷入局部最优、受力平衡等问题。
[0063]
在环境较为复杂的情况下，对本文算法以及传统人工势场法进行多次仿真分析。仿真结果如图10、图11以及图12所示。
[0064]
在图10中，障碍物较多环境较为复杂，传统算法完成了路径规划，表明人工势场法在复杂环境中仍有可能到达目标点，完成路径规划，具有一定的适应性；双向人工势场法在到达a、b后由于受力平衡，无法继续进行路径规划，此时确定a、b为二次a-star算法的起始点和目标点，同时考虑自动驾驶车辆的宽度，避免与障碍物发生碰撞，完成路径规划。改进后的算法路径更短，效率更高，适应性也更强。
[0065]
图11中，在障碍物极多、地图及其复杂的环境下进行仿真，传统算法到达c后陷入局部最优陷阱，而且在a附近出现了路径振荡现象，没有完成路径规划；双向人工势场法在极其复杂的环境下更快地停止了算法，转换成二次a-star算法，确定a、b为起始点和目标点，顺利完成避障路径规划，证明了改进算法适应性更强，可靠性更好，而且确实有效。
[0066]
图12中，对改进算法生成的路径利用贝塞尔曲线进行优化得到最终路径，对比传统算法生成的路径。不难发现，优化后的路径没有发生与障碍物碰撞的现象，路径较短，而且十分平滑能满足自动驾驶车辆的行驶路径要求。
[0067][0068]
表3复杂地图下不同算法仿真数据对比
[0069]
从表3的仿真结果数据对比中可以发现，双向人工势场法相较于传统人工势场法在复杂环境下的路径规划能力更强，更可靠，路径平均缩短了6.31％。
[0070]
综上所述，本发明通过先对传统a-star算法和人工势场法进行改进，形成二次a-star算法和双向人工势场法；在规划路径时，利用二次a-star算法和双向人工势场法结合共同进行，同时对二次a-star算法和双向人工势场法在切换时的连接处进行优化，最后利用贝塞尔曲线优化二次a-star算法和双向人工势场法共同规划的路径，使其符合车辆的行驶轨迹。通过仿真结果表明，本发明可以在复杂环境中更快的规划出一条无碰撞且平滑自动驾驶规划路径，具有路径生成时间更短，效率更高的优点。