一种自动驾驶路径规划方法、系统及计算机可读存储介质与流程

1.本发明属于自动驾驶领域，特别涉及了一种自动驾驶路径规划方法。


背景技术：

2.轨迹规划(trajectories planning)主要指考虑实际临时或者移动障碍物，考虑速度、动力学约束的情况下，尽量按照规划路径进行轨迹规划。轨迹规划的核心就是要解决车辆该怎么走的问题。轨迹规划的输入包括拓扑地图，障碍物及障碍物的预测轨迹，交通信号灯的状态，还有定位导航、车辆状态等其他信息。而轨迹规划的输出就是一个轨迹，轨迹是一个时间到位置的函数，就是在特定的时刻车辆在特定的位置上。轨迹规划的目标是计算出安全、舒适的轨迹供无人驾驶车辆完成预定的行驶任务。
3.快速搜索随机树(rrt
‑
rapidly
‑
exploringrandom trees)，是一种常见的用于路径(运动)规划的方法，它本质上是一种随机生成的数据结构—树，这种思想自从lavalle提出以后已经得到了极大的发展，到现在依然有改进的rrt不断地被提出来。
4.经过实际应用，发现现有的rrt技术存在如下问题：
5.(1)随机的生成路径，路径具有偏差性；
6.(2)随机树在搜索过程中无导向性；
7.(3)收敛速度迟缓，搜索效率低。


技术实现要素：

8.为了解决上述背景技术提到的技术问题，本发明提出了一种自动驾驶路径规划方法、系统及计算机可读存储介质，将人工势场法中的目标引力思想和障碍物斥力思想入到rrt算法中，改善基于rrt算法路径规划的性能。
9.为了实现上述技术目的，本发明的技术方案为：
10.一种自动驾驶路径规划方法，包括以下步骤：
11.确定车辆周围的障碍物位置，在障碍物周围均匀生成若干根节点xp(i)，i＝1,2,
…
，以xp(i)为根节点构建局部随机树ti；
12.在障碍物斥力影响范围内，增加障碍物的斥力分量，引导局部随机树ti朝着无障碍物的方向扩展；
13.分别以起点和终点为根节点，建立双向随机树ta和tb；增加终点产生的引力分量，引导随机树ta朝着终点方向生长；增加起点产生的引力分量，引导随机树tb朝着起点方向生长；
14.随机树ta或tb在扩展过程中，若随机树ta或tb的新节点与其他随机树的节点距离小于设定的阈值，则合并两棵树，若起点与终点在同一棵树上，结束搜索，返回搜索路径，否则继续搜索。
15.进一步地，所述增加引力分量的具体过程如下：
16.对双向随机树中的每个节点n都增加一个目标引力函数g(n)，此处的节点n表示由
起点xinit或终点xgoal向外扩展的第n个新节点xnew，表示为：
17.f(n)＝r(n)+g(n)
18.其中，f(n)表示从节点n到终点xgoal的生长指导函数，r(n)表示从起点到节点n的随机生长函数；所述目标引力函数g(n)：
19.g(n)＝ρ
·
k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖)
20.其中，ρ为步长，k
p
为引力场系数，xnear为距离随机点xrand最接近的节点，‖*‖表示几何距离；
21.根据r(n)＝ρ
·
(xrand
‑
xnear)/(‖xrand
‑
xnear‖)，得：
22.f(n)＝ρ
·
(xrand
‑
xnear)/(‖xrand
‑
xnear‖)+ρ
·
k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖)进而得到增加引力分量后新节点xnew的生成式：
23.xnew＝xnear+ρ
·
((xrand
‑
xnear)/(‖xrand
‑
xnear‖)+k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖))
24.双向随机树的各节点的生长指导函数均为f(n)，使双向随机树在引力分量的作用下在自由空间内朝目标方向搜索生长。
25.进一步地，所述增加斥力分量的具体过程如下：
26.在对障碍物周围局部随机树中的每个节点n都引入一个障碍物斥力函数t(n)，此处的节点n表示由起点xinit向外扩展的第n个新节点xnew，表示为：
27.f(n)＝r(n)+t(n)
28.其中，f(n)表示从节点n到终点的生长指导函数，r(n)为从起点到节点n的随机生长函数，所述障碍物斥力函数t(n)：
[0029][0030]
其中，ρ为步长，k
rep
为斥力场系数，p(x)表示节点到障碍物的最短距离，p0表示障碍物对节点的影响距离，x
obstacle
表示障碍物位置，xnear为距离随机点xrand最接近的节点；
[0031]
根据得：
[0032][0033]
其中，‖*‖表示几何距离；
[0034]
进而得到增加斥力分量后新节点xnew的生成式：
[0035][0036]
以障碍物周围若干节点为根节点的随机树的各节点的生长指导函数均为f(n)，使多向随机树在斥力分量的作用下在自由空间内向远离障碍物的方向搜索生长。
[0037]
进一步地，设定搜索循环次数，如果在循环次数内搜索树无法达到终点或目标区域，则路径搜索失败。
[0038]
一种自动驾驶路径规划系统，包括处理器和存储器，所述存储器存储有所述处理器的执行指令，所述处理器配置为执行所述执行指令来实现上述自动驾驶路径规划方法。
[0039]
一种计算机可读存储介质，用于存储程序，执行所述程序以实现上述自动驾驶路径规划方法。
[0040]
采用上述技术方案带来的有益效果：
[0041]
(1)本发明采用双向随机树和多棵局部随机树的探索与合并，增加引力分量，使双向随机树朝着各自目标方向生长，减少了现有rrt技术的随机性；
[0042]
(2)本发明在障碍物周围均匀生成若干根节点，对根节点增加斥力分量，生成多棵局部随机树，快速寻找可通行的路径，减少扩展过程中对障碍物的检测时间，加快rrt算法的收敛速度，改善了rrt算法的偏差性；
[0043]
(3)本发明通过在障碍物周围均匀生成若干个随机树根节点，以这若干个根节点，向障碍物外搜索扩展建立多棵局部随机树，从而减少了对障碍物的检测时间，减少了迭代次数。
附图说明
[0044]
图1是本发明的方法流程图；
[0045]
图2是本发明中引入斥力分量的示意图；
[0046]
图3是本发明中移入引力分量的示意图；
[0047]
图4是实施例中采用现有rrt算法的结果示意图；
[0048]
图5是实施例中采用本发明技术方案的结果示意图。
具体实施方式
[0049]
以下将结合附图，对本发明的技术方案进行详细说明。
[0050]
本发明设计了一种自动驾驶路径规划方法，如图1所示，包括以下步骤：
[0051]
确定车辆周围的障碍物位置，在障碍物周围均匀生成若干根节点xp(i)，i＝1,2,
…
，以xp(i)为根节点构建局部随机树ti；
[0052]
在障碍物斥力影响范围内，增加障碍物的斥力分量，引导局部随机树ti朝着无障碍物的方向扩展；
[0053]
分别以起点和终点为根节点，建立双向随机树ta和tb；增加终点产生的引力分量，引导随机树ta朝着终点方向生长；增加起点产生的引力分量，引导随机树tb朝着起点方向
生长；
[0054]
随机树ta或tb在扩展过程中，若随机树ta或tb的新节点与其他随机树的节点距离小于设定的阈值，则合并两棵树，若起点与终点在同一棵树上，结束搜索，返回搜索路径，否则继续搜索。
[0055]
rrt算法在自由空间搜索生成新节点的过程中需要对障碍物进行检测，如果检测到障碍物则放弃此次迭代，重新生成新节点，并重新进行检测，直到不与障碍物发生碰撞时，将生成的新节点加入到随机树中。随机树在扩展过程中，对障碍物检测需要消耗大量的时间，迭代次数多，从而降低了随机树的搜索效率。本发明在障碍物周围均匀生成若干个随机树根节点，以这若干个根节点，向障碍物外搜索扩展建立多棵局部随机树，减少了对障碍物的检测时间，减少了迭代次数。
[0056]
在本实施例中，将人工势场法中的障碍物斥力思想引入到rrt算法中，引导局部随机树朝着远离障碍物的方向生长，如图2所示，具体过程如下：
[0057]
对双向随机树中的每个节点n都增加一个目标引力函数g(n)，此处的节点n表示由起点xinit或终点xgoal向外扩展的第n个新节点xnew，表示为：
[0058]
f(n)＝r(n)+g(n)
[0059]
其中，f(n)表示从节点n到终点xgoal的生长指导函数，r(n)表示从起点到节点n的随机生长函数；所述目标引力函数g(n)：
[0060]
g(n)＝ρ
·
k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖)
[0061]
其中，ρ为步长，k
p
为引力场系数，xnear为距离随机点xrand最接近的节点，‖*‖表示几何距离；
[0062]
根据r(n)＝ρ
·
(xrand
‑
xnear)/(‖xrand
‑
xnear‖)，得：
[0063]
f(n)＝ρ
·
(xrand
‑
xnear)/(‖xrand
‑
xnear‖)+ρ
·
k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖)进而得到增加引力分量后新节点xnew的生成式：
[0064]
xnew＝xnear+ρ
·
((xrand
‑
xnear)/(‖xrand
‑
xnear‖)+k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖))
[0065]
双向随机树的各节点的生长指导函数均为f(n)，使双向随机树在引力分量的作用下在自由空间内朝目标方向搜索生长。
[0066]
在本实施例中，将人工势场法中的目标引力思想引入rrt算法，引导双向随机树朝着各自的目标方向生长，如图3所示，具体过程如下：
[0067]
对双向随机树中的每个节点n都增加一个目标引力函数g(n)，此处的节点n表示由起点xinit或终点xgoal向外扩展的第n个新节点xnew，表示为：
[0068]
f(n)＝r(n)+g(n)
[0069]
其中，f(n)表示从节点n到终点xgoal的生长指导函数，r(n)表示从起点到节点n的随机生长函数；所述目标引力函数g(n)：
[0070]
g(n)＝ρ
·
k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖)
[0071]
其中，ρ为步长，k
p
为引力场系数，xnear为距离随机点xrand最接近的节点，‖*‖表示几何距离；
[0072]
根据r(n)＝ρ
·
(xrand
‑
xnear)/(‖xrand
‑
xnear‖)，得：
[0073]
f(n)＝ρ
·
(xrand
‑
xnear)/(‖xrand
‑
xnear‖)+ρ
·
k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖)进而得到增加引力分量后新节点xnew的生成式：
[0074]
xnew＝xnear+ρ
·
((xrand
‑
xnear)/(‖xrand
‑
xnear‖)+k
p
·
(xgoal
‑
xnear)/(‖xgoal
‑
xnear‖))
[0075]
双向随机树的各节点的生长指导函数均为f(n)，使双向随机树在引力分量的作用下在自由空间内朝目标方向搜索生长。
[0076]
在本实施例中，设定搜索循环次数，如果在循环次数内搜索树无法达到终点或目标区域，则路径搜索失败。
[0077]
本发明还要保护一种自动驾驶路径规划系统，包括处理器和存储器，所述存储器存储有所述处理器的执行指令，所述处理器配置为执行所述执行指令来实现上述自动驾驶路径规划方法。
[0078]
本发明还要保护一种计算机可读存储介质，用于存储程序，执行所述程序以实现上述自动驾驶路径规划方法。
[0079]
通过仿真实验进行现有rrt算法与本发明技术方案的对比实验，实验结果如图4和5所示，从图中可以看出，相比现有rrt算法，本发明的节点数量减少，偏差性降低，导向性更强，收敛速度更快。
[0080]
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。