实时避障路径规划方法、系统、设备及介质

1.本发明属于车辆无人驾驶技术领域，特别涉及一种实时避障路径规划方法、系统、设备及介质。


背景技术：

2.在无人驾驶矿用卡车自主运行时，由于矿区道路环境复杂，常常有山体落石发生；因此，对无人驾驶矿用卡车的实时避障路径规划已成为亟待解决的问题之一；目前，大多采用传统的快速搜索随机树算法(rapidly-exploring random tree，rrt)进行路径规划，其存在耗时较长，到达目标点的航向不可控，路径规划结果可执行性较差的问题。


技术实现要素：

3.针对现有技术中存在的技术问题，本发明提供了一种实时避障路径规划方法、系统、设备及介质，以解决现有的路径规划过程，耗时较长，到达目标点的航向不可控，路径规划结果可执行性较差的技术问题。
4.为达到上述目的，本发明采用的技术方案为：
5.本发明提供了一种实时避障路径规划方法，包括：
6.根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；
7.以规划起点初始化随机树a，以规划终点初始化随机树b；
8.在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a及所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇；
9.将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
10.进一步的，根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域的过程，具体如下：
11.获取规划起点和规划终点之间道路的两侧道沿坐标，并将所述规划起点的道沿坐标和所述规划终点的道沿坐标作为对角连线，构成矩形区域；
12.将所述矩形区域以预设固定长度划分为若干个栅格；
13.以每个栅格的四个角作为定位点，筛选出处于道路内的所有定位点；
14.将处于道路内的所有定位点形成的区域作为自动驾驶车辆的可行驶区域。
15.进一步的，以规划起点初始化随机树a，以规划终点初始化随机树b的过程，具体如下：
16.将规划起点作为随机树a的根节点，将规划终点作为随机树b的根节点。
17.进一步的，在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a及所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇的过程，具体如下：
18.(1)对所述随机树a进行搜索：
19.在所述自动驾驶车辆的可行驶区域内，随机生成采样点x
rand
；
20.根据预设代价函数在随机树a上搜索得到节点x
nearest
；其中，所述节点x
nearest
为使所述预设代价函数的值最小，且所述节点x
nearest
到所述采样点x
rand
的转弯角度小于最大允许转弯角度θ
max
；
21.沿节点x
nearest
与采样点x
rand
的连线方向上，拓展最大生长步长l
max
，得到节点x
new
；
22.对节点x
new
与节点x
nearest
之间进行障碍物检测，并对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测；
23.若节点x
new
与节点x
nearest
之间无障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则将加入节点x
new
到随机树a中，即节点x
new
为随机树a的新节点xa；
24.若节点x
new
与节点x
nearest
之间有障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则以预设的所述自动驾驶车辆的可行驶区域的栅格长度为递减量，逐步递减直至新的节点x
new
与节点x
nearest
之间没有障碍物，之后将新的节点x
new
将加入到随机树a中，即新的节点x
new
为随机树a的新节点xa；
25.(2)按照上述对所述随机树a进行搜索操作，对所述随机树b进行搜索，至随机树b生长至新节点xb；
26.(3)判断所述随机树a的新节点xa与随机树b的新节点xb是否满足预设连接要求，若满足，则将随机树a的新节点xa与随机树b的新节点xb连接，算法终止，即随机树a与随机树b相遇；否则，返回步骤(1)和步骤(2)，分别对随机树a和随机树b继续进行拓展，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求。
27.进一步的，所述预设代价函数为：
[0028][0029]
其中，df为采样点x
rand
到随机树a或随机树b上的节点的欧氏距离；为父节点到采样点x
rand
的航向变化量；dg为采样点x
rand
通过父节点后顺着树枝返回根节点的欧氏距离的总路程之和；α1是参数；a2是参数；β为生成路径总路程的权值。
[0030]
进一步的，对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测的过程，具体如下：
[0031]
根据随机数生成策略，生成自动驾驶车辆的左前轮坐标(x
left
,y
left
)；
[0032]
根据自动驾驶车辆的宽度w以及车辆的行进方向θj，计算自动驾驶车辆的右前轮坐标；其中，所述自动驾驶车辆的右前轮坐标(x
right
,y
right
)为：
[0033][0034]
获取与自动驾驶车辆的右前轮平行位置的道沿坐标；
[0035]
根据所述与自动驾驶车辆的右前轮平行位置的道沿坐标及所述自动驾驶车辆的右前轮坐标，判断自动驾驶车辆的右前轮与道沿是否发生碰撞，即得到自动驾驶车辆的两
个前轮与所述自动驾驶车辆的可行驶区域的边界的碰撞检测结果。
[0036]
进一步的，所述预设的连接要求具体如下：
[0037]
随机树a的新节点xa到随机树b的新节点xb的距离小于最大生长步长l
max
；
[0038]
随机树a的新节点xa的新节点xa的父节点的角度与随机树a的新节点xa到随机树b的新节点xb的角度差的绝对值小于最大允许转弯角θ
max
；
[0039]
随机树b的新节点xb到新节点xb的父节点的角度与随机树a的新节点xa到随机树b的新节点xb的角度差的绝对值小于最大允许转弯角θ
max
；
[0040]
随机树a的新节点xa到随机树b的新节点xb的路径上，自动驾驶车辆的左前轮和右前轮与所述自动驾驶车辆的可行驶区域的边界均无发生碰撞行为。
[0041]
本发明还提供了一种实时避障路径规划系统，包括：
[0042]
可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；
[0043]
随机树模块，用于以规划起点初始化随机树a，以规划终点初始化随机树b；
[0044]
搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a及所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇；
[0045]
路径模块，用于将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
[0046]
本发明还提供了一种实时避障路径规划的车辆，包括：
[0047]
存储器，用于存储计算机程序；
[0048]
处理器，用于执行所述计算机程序时实现所述的实时避障路径规划方法的步骤。
[0049]
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的实时避障路径规划方法的步骤。
[0050]
与现有技术相比，本发明的有益效果为：
[0051]
本发明提供了一种实时避障路径规划方法及系统，根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域，并构建随机树a和随机树b，通过双树搜索策略，为自动驾驶车辆规划出从规划起点到规划终点的路径，确保了到达目标点的航向的可控性，路径规划结果执行性较优；通过在双树搜索策略中引入代价函数策略、变步长搜索策略及主干停止生长策略，降低了双树路径规划的计算难度，在满足轨迹平滑性需求的基础上，提高规划速度。
附图说明
[0052]
图1为本发明所述的实时避障路径规划方法的流程图；
[0053]
图2为实施例中的自动驾驶车辆的可行驶区域示意图；
[0054]
图3为实施例中的双树快速搜索的结果示意图。
具体实施方式
[0055]
为了使本发明所解决的技术问题，技术方案及有益效果更加清楚明白，以下具体实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0056]
本发明提供了一种实时避障路径规划方法，包括以下步骤：
[0057]
步骤1、根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；其中，确定自动驾驶车辆的可行驶区域的过程，具体如下：
[0058]
步骤11、获取规划起点和规划终点之间道路的两侧道沿坐标，并将所述规划起点的道沿坐标和所述规划终点的道沿坐标作为对角连线，构成矩形区域。
[0059]
步骤12、将所述矩形区域以预设固定长度划分为若干个栅格。
[0060]
步骤13、以每个栅格的四个角作为定位点，筛选出处于道路内的所有定位点；其中，所述道路为所述规划起点与所述规划终点之间的道路。
[0061]
步骤14、将处于道路内的所有定位点形成的区域作为自动驾驶车辆的可行驶区域。
[0062]
步骤2、以规划起点初始化随机树a，以规划终点初始化随机树b；其中，将规划起点作为随机树a的根节点，将规划终点作为随机树b的根节点。
[0063]
步骤3、在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a和所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇。
[0064]
其中，利用预设的双向rrt算法，对所述随机树a和所述随机树b进行搜索的过程，具体如下：
[0065]
(1)对所述随机树a进行搜索：
[0066]
在所述自动驾驶车辆的可行驶区域内，随机生成采样点x
rand
。
[0067]
根据预设代价函数在随机树a上搜索得到节点x
nearest
；其中，所述节点x
nearest
为使所述预设代价函数的值最小，且所述节点x
nearest
到所述采样点x
rand
的转弯角度小于最大允许转弯角度θ
max
；其中，所述预设代价函数为：
[0068][0069]
其中，df为采样点x
rand
到随机树a或随机树b上的节点的欧氏距离；为父节点到采样点x
rand
的航向变化量；dg为采样点x
rand
通过父节点后顺着树枝返回根节点的欧氏距离的总路程之和；α1是参数，以使得在遍历寻找父节点的过程中排除掉该节点；a2是参数，a2的选择决定了生成路径的平滑程度和生成路径的速度，a2越大，生成的路径越平滑，但生成路径的速度越慢；β为生成路径总路程的权值。
[0070]
沿节点x
nearest
与采样点x
rand
的连线方向上，拓展最大生长步长l
max
，得到节点x
new
。
[0071]
对节点x
new
与节点x
nearest
之间进行障碍物检测，并对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测；其中，所述碰撞检测过程，具体如下：
[0072]
根据随机数生成策略，生成自动驾驶车辆的左前轮坐标(x
left
,y
left
)；
[0073]
根据自动驾驶车辆的宽度w以及车辆的行进方向θj，计算自动驾驶车辆的右前轮坐标；其中，所述自动驾驶车辆的右前轮坐标(x
right
,y
right
)为：
[0074][0075]
获取与自动驾驶车辆的右前轮平行位置的道沿坐标；
[0076]
根据所述与自动驾驶车辆的右前轮平行位置的道沿坐标及所述自动驾驶车辆的右前轮坐标，判断自动驾驶车辆的右前轮与道沿是否发生碰撞，即得到自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界的碰撞检测结果。
[0077]
若节点x
new
与节点x
nearest
之间无障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则将加入节点x
new
到随机树a中，即节点x
new
为随机树a的新节点xa；
[0078]
若节点x
new
与节点x
nearest
之间有障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则以预设的所述自动驾驶车辆的可行驶区域的栅格长度为递减量，逐步递减直至新的节点x
new
与节点x
nearest
之间没有障碍物，之后将新的节点x
new
将加入到随机树a中，即新的节点x
new
为随机树a的新节点xa；
[0079]
(2)按照上述对所述随机树a进行搜索操作，对所述随机树b进行搜索，至随机树b生长至新节点xb；
[0080]
(3)判断所述随机树a的新节点xa与随机树b的新节点xb是否满足预设连接要求，若满足，则将随机树a的新节点xa与随机树b的新节点xb连接，算法终止，即随机树a与随机树b相遇；否则，返回步骤(1)和步骤(2)，分别对随机树a和随机树b继续进行拓展，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求。
[0081]
其中，所述预设连接要求，具体如下：
[0082]
随机树a的新节点xa到随机树b的新节点xb的距离小于最大生长步长l
max
；
[0083]
随机树a的新节点xa的新节点xa的父节点的角度与随机树a的新节点xa到随机树b的新节点xb的角度差的绝对值小于最大允许转弯角θ
max
；
[0084]
随机树b的新节点xb到新节点xb的父节点的角度与随机树a的新节点xa到随机树b的新节点xb的角度差的绝对值小于最大允许转弯角θ
max
；
[0085]
随机树a的新节点xa到随机树b的新节点xb的路径上，自动驾驶车辆的左前轮和右前轮与所述自动驾驶车辆的可行驶区域的边界均无发生碰撞行为。
[0086]
步骤4、将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
[0087]
本发明所述的实时避障路径规划方法，根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域，并构建随机树a和随机树b，通过双树搜索策略，为自动驾驶车辆规划出从规划起点到规划终点的路径，确保了到达目标点的航向的可控性，路径规划结果执行性较优；通过在双树搜索策略中引入代价函数策略、变步长搜索策略及主干停止生长策略，降低了双树路径规划的计算难度，在满足轨迹平滑性需求的基础上，提高规划速度。
[0088]
本发明还提供了一种实时避障路径规划系统，包括可行域模块、随机树模块、搜索模块及路径模块；可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；随机树模块，用于以规划起点初始化随机树a，以规划终点初始化随机树b；搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a
及所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇；路径模块，用于将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
[0089]
本发明还提供了一种实时避障路径规划设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现实时避障路径规划方法的步骤。
[0090]
所述处理器执行所述计算机程序时实现上述实时避障路径规划方法的步骤，例如：根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；以规划起点初始化随机树a，以规划终点初始化随机树b；在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a及所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇；将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
[0091]
或者，所述处理器执行所述计算机程序时实现上述系统中各模块的功能，例如：可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；随机树模块，用于以规划起点初始化随机树a，以规划终点初始化随机树b；搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a及所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇；路径模块，用于将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
[0092]
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成预设功能的一系列计算机程序指令段，所述指令段用于描述所述计算机程序在所述实时避障路径规划设备中的执行过程。例如，所述计算机程序可以被分割成可行域模块、随机树模块、搜索模块及路径模块，各模块具体功能如下：可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；随机树模块，用于以规划起点初始化随机树a，以规划终点初始化随机树b；搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a及所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇；路径模块，用于将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
[0093]
所述实时避障路径规划设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述实时避障路径规划设备可包括，但不仅限于处理器、存储器。本领域技术人员可以理解，上述仅仅是实时避障路径规划设备的示例，并不构成对实时避障路径规划设备的限定，可以包括比上述更多的部件，或者组合某些部件，或者不同的部件，例如所述实时避障路径规划设备还可以包括输入输出设备、网络接入设备、总线等。
[0094]
所称处理器可以是中央处理单元(centralprocessingunit，cpu)，还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现成可编程门阵列(field-programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分
立硬件组件等。通用处理器可以是微处理器或者所述处理器也可以是任何常规的处理器等，所述处理器是所述实时避障路径规划设备的控制中心，利用各种接口和线路连接整个实时避障路径规划设备的各个部分。
[0095]
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述实时避障路径规划设备的各种功能。
[0096]
所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smartmediacard,smc)，安全数字(securedigital,sd)卡，闪存卡(flashcard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
[0097]
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种实时避障路径规划方法的步骤。
[0098]
所述实时避障路径规划系统集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。
[0099]
基于这样的理解，本发明实现上述实时避障路径规划方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，所述计算机程序在被处理器执行时，可实现上述实时避障路径规划方法的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或预设中间形式等。
[0100]
所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(read-onlymemory，rom)、随机存取存储器(randomaccessmemory，ram)、电载波信号、电信信号以及软件分发介质等。
[0101]
需要说明的是，所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括电载波信号和电信信号。
[0102]
实施例
[0103]
以自动驾驶车辆在某矿区道路上的避障路径规划过程为例。
[0104]
如附图1所示，本实施例1提供了一种实时避障路径规划方法，具体包括以下步骤：
[0105]
步骤1、根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；所述自动驾驶车辆的可行驶区域的确定过程，具体如下：
[0106]
步骤11、获取规划起点和规划终点之间道路的两侧道沿坐标，并将所述规划起点的道沿坐标和所述规划终点的道沿坐标作为对角连线，构成矩形区域；其中，所述道路两侧道沿坐标的获取过程，具体为：
[0107]
获得车辆到左侧道沿的距离，从而确定左侧道沿坐标分别为(x
0i
,y
0i
),(i＝0,1,
…
m)，右侧道沿的坐标分别为(x
1j
,y
1j
),(j＝0,1,
…
n)。
[0108]
所述矩形区域的构成过程为：道路两侧道沿的最小坐标(x
min
,y
min
)作为起始点，最
大坐标(x
max
,y
max
)作为终止点，将最小坐标(x
min
,y
min
)作为矩形其中一个角的坐标，最大坐标(x
max
,y
max
)作为最小坐标对角的坐标，从而得到矩形区域。
[0109]
步骤12、将所述矩形区域内以预设固定长度划分为若干栅格；其中，所述栅格为正方形，栅格的边长为s，且所述栅格的边长s大于等于测量长度a；其中，所述测量长度a为获取规划起点和规划终点之间道路的两侧道沿坐标时，对所述每隔测量长度a进行一次测量。
[0110]
步骤13、以每个栅格的四个角作为定位点，筛选出处于道路内的所有定位点；其中，所述道路为所述规划起点与所述规划终点之间的道路；其中，筛选出处于道路内的所有定位点的过程，具体如下：
[0111]
设定位点为ps(xs,ys)，找到左侧道沿的最近点p
0near
(x
0near
,y
0near
)和它在运动方向的下一个点的坐标p
0near 1
(x
0near 1
,y
0near 1
)，找到右侧道沿的最近点p
1near
(x
1near
,y
1near
)和它在运动方向的下一个点的坐标p
1near 1
(x
1near 1
,y
1near 1
)；
[0112]
根据向量计算方法η＝(x
near-xs)
×
(y
near 1-ys)-(y
near-ys)
×
(x
near 1-xs)；
[0113]
当η为正时，坐标ps位于向量p
near
→
p
near 1
的左边；当η为负时，坐标ps位于向量p
near
→
p
near 1
的右边。
[0114]
分别把找到的左右两侧道沿的最近点和下一点的坐标带入η中，得到η
left
和η
right
。当η
left
为负且η
right
为正时，表示该点位于两侧道沿之间，为道路内；
[0115]
其中x
near
,y
near
分别为：一侧道沿最近点的横坐标和纵坐标，x
near 1
,y
near 1
分别为：一侧道沿最近点运动方向的下一个点的坐标和纵坐标。
[0116]
步骤14、将处于道路内的所有定位点形成的区域作为自动驾驶车辆的可行驶区域；具体的，在筛选出处于道路内的所有定位点后，依次判断车辆宽度和安全距离与每个定位点到道沿最近距离的关系，d
min
≥w
t
/2 d
safe
，其中，d
min
为当前定位点到道沿的最近距离，w
t
为自动驾驶车辆的宽度，d
safe
为安全距离，剔除不满足上述关系的定位点；本实施例中，所述自动驾驶车辆的可行驶区域，如附图2所示。
[0117]
步骤2、以规划起点初始化随机树a，以规划终点初始化随机树b；其中，将规划起点作为随机树a的根节点，将规划终点作为随机树b的根节点。
[0118]
步骤3、在所述自动驾驶车辆的可行驶区域内，利用预设的双向rrt算法，分别对所述随机树a和所述随机树b进行搜索，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求，即随机树a与随机树b相遇。
[0119]
其中，利用预设的双向rrt算法，对所述随机树a和所述随机树b进行搜索的过程，具体如下：
[0120]
(1)对所述随机树a进行搜索：
[0121]
在所述自动驾驶车辆的可行驶区域内，随机生成采样点x
rand
。
[0122]
根据预设代价函数在随机树a上搜索得到节点x
nearest
；其中，所述节点x
nearest
为使所述预设代价函数的值最小，且所述节点x
nearest
到所述采样点x
rand
的转弯角度小于最大允许转弯角度θ
max
；其中，所述预设代价函数为：
[0123][0124]
其中，df为采样点x
rand
到随机树a或随机树b上的节点的欧氏距离；为父节点到采
样点x
rand
的航向变化量；dg为采样点x
rand
通过父节点后顺着树枝返回根节点的欧氏距离的总路程之和；α1是参数，以使得在遍历寻找父节点的过程中排除掉该节点；a2是参数，a2的选择决定了生成路径的平滑程度和生成路径的速度，a2越大，生成的路径越平滑，但生成路径的速度越慢；β为生成路径总路程的权值。
[0125]
本实施例中，所述随机树搜索代价函数最小节点的过程中，在树中寻找代价函数最小节点的过程中，并不是一定要从根节点开始的。在一开始的搜索过程中，我们规定当节点数小于m时，寻找代价函数最小的节点的过程是从根节点开始的，保障树的高速扩张性；当节点数大于m时，如果发生某一父节点j的子节点数达到预设最大值p，则取出该父节点j所对应其所在树中第nj个节点的属性，下一次寻找代价函数从nj开始寻找。
[0126]
沿节点x
nearest
与采样点x
rand
的连线方向上，拓展最大生长步长l
max
，得到节点x
new
。
[0127]
对节点x
new
与节点x
nearest
之间进行障碍物检测，并对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测；其中，所述碰撞检测过程，具体如下：
[0128]
根据随机数生成策略，生成自动驾驶车辆的左前轮坐标(x
left
,y
left
)；
[0129]
根据自动驾驶车辆的宽度w以及车辆的行进方向θj，计算自动驾驶车辆的右前轮坐标；其中，所述自动驾驶车辆的右前轮坐标(x
right
,y
right
)为：
[0130][0131]
获取与自动驾驶车辆的右前轮平行位置的道沿坐标；
[0132]
根据所述与自动驾驶车辆的右前轮平行位置的道沿坐标及所述自动驾驶车辆的右前轮坐标，判断自动驾驶车辆的右前轮与道沿是否发生碰撞，即得到自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界的碰撞检测结果。
[0133]
若节点x
new
与节点x
nearest
之间无障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则将加入节点x
new
到随机树a中，即节点x
new
为随机树a的新节点xa；
[0134]
若节点x
new
与节点x
nearest
之间有障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则以预设的所述自动驾驶车辆的可行驶区域的栅格长度为递减量，逐步递减直至新的节点x
new
与节点x
nearest
之间没有障碍物，之后将新的节点x
new
将加入到随机树a中，即新的节点x
new
为随机树a的新节点xa；
[0135]
(2)按照上述对所述随机树a进行搜索操作，对所述随机树b进行搜索，至随机树b生长至新节点xb；
[0136]
(3)判断所述随机树a的新节点xa与随机树b的新节点xb是否满足预设连接要求，若满足，则将随机树a的新节点xa与随机树b的新节点xb连接，算法终止，即随机树a与随机树b相遇；否则，返回步骤(1)和步骤(2)，分别对随机树a和随机树b继续进行拓展，直至随机树a的新节点xa与随机树b的新节点xb满足预设连接要求。
[0137]
其中，所述预设连接要求，具体如下：
[0138]
随机树a的新节点xa到随机树b的新节点xb的距离小于最大生长步长l
max
；
[0139]
随机树a的新节点xa的新节点xa的父节点的角度与随机树a的新节点xa到随机树b
的新节点xb的角度差的绝对值小于最大允许转弯角θ
max
；
[0140]
随机树b的新节点xb到新节点xb的父节点的角度与随机树a的新节点xa到随机树b的新节点xb的角度差的绝对值小于最大允许转弯角θ
max
；
[0141]
随机树a的新节点xa到随机树b的新节点xb的路径上，自动驾驶车辆的左前轮和右前轮与所述自动驾驶车辆的可行驶区域的边界均无发生碰撞行为。
[0142]
本实施例中，所述的随机树a和随机树b上的节点，具有如下属性：该节点的代价函数值val；左侧前车轮坐标(x
left
,y
left
)；右侧前车轮坐标(x
right
,y
right
)；到起始点的距离dg；该节点是父节点的第几个节点；从父节点到该节点的角度θj，逆时针为正；该节点是树生成的第nj个节点；该节点指向父节点的指针。
[0143]
步骤4、将随机树a的新节点xa与随机树b的新节点xb连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果，如附图3所示；由记录下停止生长时随机树a的终止节点xa和随机树b的终止节点xb，通过该节点指向父节点的指针获取父节点的信息，一直循环指到根节点为止，从而获取一棵树的路径。把从起始节点生长树得到的路径反向存储，再与从终止节点生长树得到的路径相合并，即可得到一条完整的路径。
[0144]
本发明所述的实时避障路径规划方法、系统、设备及介质，使用超快速双树rrt算法，通过双树的策略保证到达目标点的航向问题，并引入代价函数代替传统的距离搜索条件，在满足轨迹平滑性需求的基础上，提高规划速度，从而满足无人矿卡在避障路径规划中的实时需求。
[0145]
上述实施例仅仅是能够实现本发明技术方案的实施方式之一，本发明所要求保护的范围并不仅仅受本实施例的限制，还包括在本发明所公开的技术范围内，任何熟悉本技术领域的技术人员所容易想到的变化、替换及其他实施方式。