CN115752472A - 一种基于驾校场景下用于自动驾驶的全局路径规划方法 - Google Patents

Abstract

本发明公开了一种基于驾校场景下用于自动驾驶的全局路径规划方法，包括以下步骤：对整个驾校的道路关键道路节点进行采样，生成驾校的道路节点网图；根据道路节点网图，采用Dijkstra算法获得带权有向图；使用所述带权有向图作为A*算法的地图，进行路径规划获得路径点；采用三次样条曲线，根据所述路径点生成曲率连续的最终路径。该方法通过在驾校环境下，对关键道路节点采样，再基于采集到的道路关键节点，同时基于带权有向图，采用改进的双向A*算法对全局路径进行规划；极大的提升了搜索速率，实现了千位点位下的秒级搜索；有助于驾校车辆和接驳车的自动驾驶。

Description

一种基于驾校场景下用于自动驾驶的全局路径规划方法

技术领域

本发明涉及自动驾驶和路径规划技术领域，特别涉及一种基于驾校场景下用于自动驾驶的全局路径规划方法。

背景技术

近年来自动驾驶技术的不断发展,但商用乘用车落地仍然道阻且长。于是依托于各种细分场景下的特定自动驾驶技术应用落地逐步发展。当今自动驾驶技术已经逐渐走向成熟，但是开放道路的风险不可穷尽以及相关法律法规的不完善使得自动驾驶仍然处于试验场地和封闭道路场景下进行。

目前，很多驾校的场地较大，面积在百亩、千亩甚至万亩以上，驾校内部道路可理解为一个相对封闭的场景，其中，有各种训练教室和场地，学员需要在各个场地或教室之间穿梭，因此，就诞生了许多接驳车，来方便学员寻找到对应的场地或教室。

另外，现有技术中也存在对路径全局规划，但搜索效率较低；对于自动驾驶场景来说，搜索效率需要的时间越少越好，才能进一步保证安全性。

因此，如何对上述驾校场景下接驳车进行升级，进行高效率的全局路径规划，以便实现自动驾驶，同行从业人员亟待解决。

发明内容

本发明的目的在于提供一种基于驾校场景下用于自动驾驶的全局路径规划方法，解决了驾校场景下的全局路径规划效率较低的问题；该规划道路可以用于驾校车辆和接驳车的自动驾驶。

为实现上述目的，本发明采取的技术方案为：

本发明提供一种基于驾校场景下用于自动驾驶的全局路径规划方法，包括以下步骤：

S10、对整个驾校的道路关键道路节点进行采样，生成驾校的道路节点网图；

S20、根据道路节点网图，采用Dijkstra算法获得带权有向图；

S30、使用所述带权有向图作为A*算法的地图，进行路径规划获得路径点；

S40、采用三次样条曲线，根据所述路径点生成曲率连续的最终路径。

进一步地，所述步骤S10，包括：

对整个驾校内的每个道路的路口采用定位器进行点位采集，获取经纬度后计算相对于基站东北天坐标系下的坐标；

将点位之间权重设置为点和点的距离，当点位之间不可达则设置权重为无穷大，生成驾校的道路节点网图。

进一步地，所述步骤S30，包括：

将起点位置放入openList,将终点位置放入openList2,循环迭代朝向目标；

拓展节点，如果拓展的节点已经在openList中或closeList中，判断当前拓展节点的代价是否更小，若更小则用当前更小代价代替之前代价，选取该点作为拓展点，以该点作为下一次拓展的父节点，并将其放入closeList中，从openList中删除；每次拓展都将符合要求的点放入openList作为待选点，不断循环迭代直到目标节点；

当openList和openList2存在重叠点位时，搜索结束；

从重叠点位分别回溯到起点和终点，形成最终的搜索路径。

进一步地，当前拓展节点的代价函数如下：

f(n)＝g(n)+w(n)*h(n)

f(n)为初始节点由当前节点n到目标节点的估计代价；g(n)为初始节点到当前节点n的实际代价；h(n)为从当前节点n到目标节点的估计代价，使用坐标的欧式距离作为估计代价；w(n)为权重。

进一步地，所述步骤S40中，每个路径点的曲率满足小于1/R；R表示车辆最小转弯半径。

与现有技术相比，本发明具有如下有益效果：

本发明实施例提供的一种基于驾校场景下用于自动驾驶的全局路径规划方法，包括以下步骤：对整个驾校的道路关键道路节点进行采样，生成驾校的道路节点网图；根据道路节点网图，采用Dijkstra算法获得带权有向图；使用所述带权有向图作为A*算法的地图，进行路径规划获得路径点；采用三次样条曲线，根据所述路径点生成曲率连续的最终路径。该方法通过在驾校环境下，对关键道路节点采样，再基于采集到的道路关键节点，同时基于带权有向图，采用改进的双向A*算法对全局路径进行规划；极大的提升了搜索速率，实现了千位点位下的秒级搜索；有助于驾校车辆和接驳车的自动驾驶。

附图说明

图1为本发明实施例提供的基于驾校场景下用于自动驾驶的全局路径规划方法流程图；

图2为本发明实施例提供的改进后A*算法的搜索过程图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

在本发明的描述中，需要说明的是，术语“上”、“下”、“内”、“外”“前端”、“后端”、“两端”、“一端”、“另一端”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“设置有”、“连接”等，应做广义理解，例如“连接”，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

参照图1所示，本发明提供的一种基于驾校场景下用于自动驾驶的全局路径规划方法，包括以下步骤：

S10、对整个驾校的道路关键道路节点进行采样，生成驾校的道路节点网图；

S20、根据道路节点网图，采用Dijkstra算法获得带权有向图；

S30、使用所述带权有向图作为A*算法的地图，进行路径规划获得路径点；

S40、采用三次样条曲线，根据所述路径点生成曲率连续的最终路径。

该方法致力于驾校场景下的全局路径规划，规划道路可以用于驾校车辆和接驳车的自动驾驶。

下面分别对上述各个步骤进行详细的说明：

在步骤S10中，先对整个驾校道路的关键道路节点进行采样，在每个道路的路口采用定位器进行点位采集，获取经纬度后根据现有技术计算出相对于基站东北天坐标，并将点位之间权重设置为点和点的距离，点位之间不可达则设置权重为无穷大，这样即可生成驾校的道路节点网图。其中，定位器比如选用GPS卫星定位终端。

在步骤S20-S30中，根据上述关键道路节点建立的地图，天然适用于用于带权有向图的Dijkstra算法。

Dijkstra算法思想：

设G＝(V,E)是一个带权有向图，把图中顶点集合V分成两组，第一组为已求出最短路径的顶点集合(用S表示，初始时S中只有一个源点，以后每求得一条最短路径,就将加入到集合S中，直到全部顶点都加入到S中，算法就结束了)，第二组为其余未确定最短路径的顶点集合(用U表示)，按最短路径长度的递增次序依次把第二组的顶点加入S中。在加入的过程中，总保持从源点v到S中各顶点的最短路径长度不大于从源点v到U中任何顶点的最短路径长度。此外，每个顶点对应一个距离，S中的顶点的距离就是从v到此顶点的最短路径长度，U中的顶点的距离，是从v到此顶点只包括S中的顶点为中间顶点的当前最短路径长度。

Dijkstra算法作为一种全量搜索的算法，能够确保找到最优最短路径，但是缺点是，在较大的地图路径下，搜索时间过长，影响了效率。经测试在路网节点超过1000个的时候，在单颗2.5GhzCPU下，需要搜索超过15s秒钟。

基于此，采用改进的双向A*算法，用于对全局路径进行规划。

A*算法通常用于栅格地图作为环境地图，即对整个地图平面进行网格化，将整张地图栅格化成多个1*1或者2*2或者3*3的方格，最终基于方格中心确定最终的路径。这种方法在地图面积较小的情况下搜索较快，但是同样当地图面积过大时，若将栅格扩大可能导致栅格包裹了过多障碍物导致无法规划，栅格过小的时候时间复杂度骤升，导致规划速度极慢。

因此，在此处使用带权有向图作为A*算法的地图，对整张地图进行路径规划，能有效提升规划的速度。

A*算法是一种启发式算法，首先对每个搜索位置进行评估，得到最好的位置，再基于这个位置向目标进行搜索直达目标，相较于Dijkstra省略了大量的无价值搜索路径，极大提升了搜索效率。A*的代价函数为：

f(n)＝g(n)+h(n)

f(n)为初始节点由当前节点n到目标节点的估计代价；g(n)为初始节点到当前节点n的实际代价；h(n)为从当前节点n到目标节点的估计代价，在此处使用坐标的欧式距离作为估计代价。改进后的A*代价函数为：

f(n)＝g(n)+w(n)*h(n)

g(n)和h(n)同上，增加了权重w(n)，与采集点的类型相关，比如有直行点、拐角点等，可根据采集点类型赋予相应的权重。通过设定w(n)可以改变A*规划过程中h(n)对规划的影响，w(n)越大越趋近于广度优先算法，w(n)越小则趋近于Dijkstra算法。同时为了提高算法速度，采用双向搜索，一边从终点向起点进行搜索，另一边从起点向终点进行搜索提高搜索速度，该方法在1000个点位的情况下，相较于原始的Dijkstra算法提升约90％的速度。如图2所示，算法步骤基本如下：

1.将起点位置放入openList(第一开放列表)中,将终点位置放入openList2(第二开放列表)中,循环迭代朝向目标。

2.拓展节点，如果拓展的节点已经在openList中或closeList(封闭列表)中，判断当前拓展节点的代价是否更小，若更小则用当前更小代价代替之前代价，选取该点作为拓展点，以该点作为下一次拓展的父节点，并将其放入closeList中，从openList中删除。每次拓展都将符合要求的点放入openList作为待选点，不断循环迭代直到目标节点。

3.当双向的openList和openList2，存在重叠点位时，搜索结束。

4.从重叠点位分别回溯到起点和终点，形成最终的搜索路径。

在步骤S40中，采用三次样条曲线，根据路径点生成曲率连续的最终路径。实际的车辆行驶过程中，路径规划需要符合车辆运动学的特征和动力学约束。

因此，所规划的路径应当要满足路径平滑性的要求，即需要曲率连续的曲线而不是折线的方式。采用三次样条曲线，可以形成串联规划点的平滑曲线，可以满足车辆的运行轨迹。同时对每个点的曲率进行计算，应当要满足曲率小于1/R，R为车辆最小转弯半径；这样保证了车辆可以沿着当前路线进行前进，如果曲率大于1/R，则剔除当前点，选择下一个点继续采用三次样条拟合。比如当连续超过3个关键点拟合失败则认定当前规划路径失败，通常由于路口点位距离较远，且在同一道路上，因此基本不会发生曲率不符合的情况。

在实际实施时，可根据驾校的场景需求，灵活的使用少量的关键节点，该方法通过在驾校环境下，对关键道路节点采样，再基于采集到的道路关键节点，使用带权有向图代替了原始复杂的路径地图；同时基于带权有向图，采用改进的双向A*算法对全局路径进行规划；极大的提升了搜索速率，实现了千位点位下的秒级搜索；有助于驾校车辆和接驳车的自动驾驶。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (5)

1.一种基于驾校场景下用于自动驾驶的全局路径规划方法，其特征在于，包括以下步骤：

S10、对整个驾校的道路关键道路节点进行采样，生成驾校的道路节点网图；

S20、根据道路节点网图，采用Dijkstra算法获得带权有向图；

S30、使用所述带权有向图作为A*算法的地图，进行路径规划获得路径点；

S40、采用三次样条曲线，根据所述路径点生成曲率连续的最终路径。

2.根据权利要求1所述的一种基于驾校场景下用于自动驾驶的全局路径规划方法，其特征在于，所述步骤S10，包括：

对整个驾校内的每个道路的路口采用定位器进行点位采集，获取经纬度后计算相对于基站东北天坐标系下的坐标；

将点位之间权重设置为点和点的距离，当点位之间不可达则设置权重为无穷大，生成驾校的道路节点网图。

3.根据权利要求1所述的一种基于驾校场景下用于自动驾驶的全局路径规划方法，其特征在于，所述步骤S30，包括：

将起点位置放入openList,将终点位置放入openList2,循环迭代朝向目标；

拓展节点，如果拓展的节点已经在openList中或closeList中，判断当前拓展节点的代价是否更小，若更小则用当前更小代价代替之前代价，选取该点作为拓展点，以该点作为下一次拓展的父节点，并将其放入closeList中，从openList中删除；每次拓展都将符合要求的点放入openList作为待选点，不断循环迭代直到目标节点；

当openList和openList2存在重叠点位时，搜索结束；

从重叠点位分别回溯到起点和终点，形成最终的搜索路径。

4.根据权利要求3所述的一种基于驾校场景下用于自动驾驶的全局路径规划方法，其特征在于，当前拓展节点的代价函数如下：

f(n)＝g(n)+w(n)*h(n)

f(n)为初始节点由当前节点n到目标节点的估计代价；g(n)为初始节点到当前节点n的实际代价；h(n)为从当前节点n到目标节点的估计代价，使用坐标的欧式距离作为估计代价；w(n)为权重。

5.根据权利要求1所述的一种基于驾校场景下用于自动驾驶的全局路径规划方法，其特征在于，所述步骤S40中，每个路径点的曲率满足小于1/R；R表示车辆最小转弯半径。

Images