CN112665603B

CN112665603B - 一种基于带时间窗a*改进的多车路径规划方法

Info

Publication number: CN112665603B
Application number: CN202011487316.XA
Authority: CN
Inventors: 王欣然
Original assignee: Dilu Technology Co Ltd
Current assignee: Dilu Technology Co Ltd
Priority date: 2020-12-16
Filing date: 2020-12-16
Publication date: 2022-03-25
Anticipated expiration: 2040-12-16
Also published as: CN112665603A

Abstract

本发明公开了一种基于带时间窗A*改进的多车路径规划方法，具体为：获取路网结构信息及该路网上所有车辆的位置，生成动态拓扑图；结合动态拓扑图及已规划的其它车辆路径的时间窗信息，采用带时间窗的A*算法规划当前车辆的全局路径，并基于车辆动力学模型计算车辆遵循全局路径行驶时的时间窗；对于当前车辆的全局路径在其经过的路口处进行分割，分割为多个路径段，将分割后的路径段逐段下发给当前车辆执行，即当前车辆执行完前一个路径段后到达该路径段的出口停止等待，直到下一个路径段下发后再执行下一个路径段，直至所有路径段都执行完；根据当前车辆到达前一个路径段的出口时，实际时刻和理想时刻之间的差异，对下一个路径段的时间窗进行调整。

Description

一种基于带时间窗A*改进的多车路径规划方法

技术领域

本发明涉及一种基于带时间窗A*改进的多车路径规划方法，属于自动驾驶-路径规划领域。

背景技术

近几年来，自动驾驶技术取得了突破性的发展，也使得越来越多的机构将大量的人力物力投入到了自动驾驶的研发工作中。从软件层面上来看，自动驾驶大体分为定位、感知、决策、规划和控制。作为一个承上启下的模块，规划从感知、定位和决策获取数据，并指引控制模块实际控制车辆运动，使其担任着至关重要的角色。

路径规划算法和相关技术的提出也是层出不穷，例如：带时间窗的A*算法、RRT(快速扩展随机数算法)和人工势场法等。带时间窗的A*算法以其复杂度低、易实现以及实用性强的特点在自动驾驶领域备受青睐。传统的带时间窗的A*算法能有效地解决单体(机器人或者无人车辆)的路径规划问题。但当该算法扩展到多体规划时，由于缺乏考虑应用场景的实际情况，致使算法实用性和效率降低。主要有两个问题：1)带时间窗A*算法通常是为所有个体同时规划路径以求得所有个体的全局最优结果。但在很多场景中(例如：码头和物流园区)，车辆通常是先后完成任务。所以，同时规划的方法明显不适用这些场景；2)车辆实际运动与理想情况有偏差，无法按照规划好的时间窗依次经过指定区域。若用重规划的方式来修正这些偏差，会导致算法复杂度的提升以及大量计算资源的浪费。

发明内容

本发明所要解决的技术问题是：提供一种基于带时间窗A*改进的多车路径规划方法，对传统的带时间窗的A*算法进行改进，可以有效地解决车辆实际运动状态与理想状态之间的偏差问题，使其适用于无人驾驶车辆的实际运作场景。

本发明为解决上述技术问题采用以下技术方案：

一种基于带时间窗A*改进的多车路径规划方法，包括如下步骤：

步骤1，获取路网结构信息以及该路网上所有车辆的位置，生成动态拓扑图；

步骤2，结合动态拓扑图以及已规划的其它车辆路径的时间窗信息，采用带时间窗的A*算法规划当前车辆从起始节点到目标节点的全局路径，并基于车辆动力学模型计算车辆遵循全局路径行驶时的时间窗；

步骤3，对于当前车辆的全局路径在其经过的路口处进行分割，将全局路径分割为多个路径段，将分割后的路径段逐段下发给当前车辆执行，即当前车辆执行完前一个路径段后到达该路径段的出口停止等待，直到下一个路径段下发后再执行下一个路径段，直至所有路径段都执行完；

步骤4，根据当前车辆到达前一个路径段的出口时，实际时刻和理想时刻之间的差异，对下一个路径段的时间窗进行调整，所述理想时刻即步骤2计算得到的车辆离开路径段的时刻，具体如下：

1)若实际时刻与理想时刻差值的绝对值小于等于5秒，则下一个路径段直接下发不做时间窗调整；

2)若实际时刻早于理想时刻Δt秒，Δt为设定值，并且当前车辆到达前一个路径段的出口时无其他车辆等待或者当前车辆的优先级最高，则将下一个路径段的时间窗都减去Δt秒；

3)若实际时刻早于理想时刻Δt秒，但在前一个路径段的出口处有其他车辆并且其他车辆的优先级高于当前车辆，若等待优先级比当前车辆高的其他车辆通过的时间为k秒，则将下一个路径段的时间窗都减去(Δt-k)秒。

作为本发明的一种优选方案，所述步骤1的具体过程如下：

步骤1.1，获取路网结构信息，将路网上的道路段映射为拓扑图上的节点，道路段之间的连接以及驶入驶出关系映射为拓扑图上的边，生成初始的动态拓扑图；

步骤1.2，计算所有边的权值，具体为：当边前后两侧的道路段均为直行道时，边的权值为0；当边左右两侧的道路段均为直行道时，边的权值为50；当前后两侧的道路段一个为直行道另一个为弯道时，边的权值为20；

步骤1.3，计算所有节点的权值，具体为：当节点上未停靠车辆时，节点的权值为该节点对应的道路段的长度；当节点上停靠车辆时，节点的权值为Weight＝Length*(1+2^K)，其中，Weight表示节点的权值，Length表示节点对应的道路段的长度，K表示节点上停靠的车辆的数目，K≥1；

步骤1.4，将边的权值、节点的权值与初始的动态拓扑图结合起来，得到动态拓扑图。

作为本发明的一种优选方案，步骤2所述采用带时间窗的A*算法规划当前车辆从起始节点到目标节点的全局路径中，将全局路径视为依次连接的若干节点，在第n个节点的离开时刻规划第n+1个节点时增加时间维度的约束条件：

条件1，对于当前车辆而言，选择第n个节点的邻接节点作为第n+1个节点，且第n个节点的邻接节点中不包括第n-1个节点；

条件2，对于当前车辆而言，其在第n+1个节点的进入时刻至离开时刻的时间窗内未被其他已规划好的车辆的全局路径占用。

作为本发明的一种优选方案，步骤3所述路口包括十字路口和T型路口。

作为本发明的一种优选方案，步骤4所述优先级最高指的是车辆的理想时刻最早。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明将传统算法中的多车同时规划改为多车先后规划，并引入动态拓扑图的方法来实现多车规划时的相关联性。

2、本发明将规划好的整体路径拆分成多段，根据车辆的实际运动情况，每段先做相应时间窗信息的调整，再下发给车辆执行。

3、本发明方法可以有效地应用到自动驾驶的不同场景中，加强了可执行性和鲁棒性。

附图说明

图1是本发明基于带时间窗A*改进的多车路径规划方法的流程图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，本发明提出了一种基于带时间窗A*改进的多车路径规划方法，主要步骤如下：

步骤1：根据当前所有车辆在道路上的位置，将路网映射成具有特定权值的拓扑图。

步骤2：根据当前拓扑图以及其它已规划好了的车辆的路径信息，规划当前车辆路径。

步骤3：将该车规划好的路径分段，拆分处为该路径经过的在路网上的所有路口(十字或T字路口)。

步骤4：根据车辆实际到达前一段路径终点的时刻，调整下一段路径中的时间窗信息再下发到车辆执行。

具体内容如下：

步骤1：生成动态拓扑图：

本发明中车辆运动和规划所依据的道路网络格式(俗称“路网”)为基于Opendrive格式的高精地图。传统的带时间窗的A*算法用的拓扑图由节点和有向边组成。该拓扑图与高精地图存在对应关系。节点对应路网上的道路(lane)。有向边对应道路段之间的连接以及驶入驶出关系。节点和有向边都有相应的权值。节点的权值为对应的道路段的长度，有向边的权值为车辆换道的惩罚值，可根据特定场景设置。本算法的输入为车辆定位信息和高精地图，所以首先根据所有车辆当前的位置信息以及高精地图的路网信息提取出一个拓扑图(点线图)。生成步骤如下：

1)将lane映射成节点(Vertex)，lane与lane之间的连接关系映射成边(Edge)。生成一个初始的有向图(点线图)。

2)计算所有边的开销，若边两端的道路都为直行道(即直走不用变道，前后连接)，则开销为0，若两端的道路是通过变道相连(左右连接)的，则开销为50，连接U形弯道路的边开销加20。

3)计算所有节点的开销，每个节点初始的开销为该节点对应的lane的长度。若该lane上停靠了其他车辆，则该lane的开销会相应增加。而节点权值Weight的计算方法按照下列公式：

Weight＝Length*(1+2^K)

其中，Length表示该道路段的长度，K表示当前时刻在这个道路段上出现的车辆的数目。

步骤2：基于A*算法生成每辆车的全局路径：

本发明所使用的初始的全局路径生成算法为A*算法。传统的A*算法的步骤与逻辑本发明不再赘述，但本发明对于A*算法基于最优节点搜索邻接点的步骤增加了限制条件：

N{Adj}_t+1＝N{Adj}_t+N_t+1

N表示搜索使用节点，Adj表示邻接点集合，t表示某一特定时刻，N{Adj}_t+1表示在t+1时刻节点N的相邻节点集合，N{Adj}_t表示在t时刻节点N的相邻节点集合(即N能直接到达的节点)，N_t+1表示对于t时刻节点来说，在t+1时刻能够到达的节点，除了t-1时刻所在的节点。上述公式表示当前节点的下一个节点只能在t+1时刻的邻接点集合中搜索，从而保证了时间维度上的合理性。同时，算法会参考车辆的模拟运动学模型(该模型由控制模块提供，用于模拟车辆在路网上不同位置不同时刻的速度和位移变化)，计算出车辆到达各个道路段的进入和离开时刻(即时间窗)。所以当t时刻时，当前车辆在t+1时刻能够选择的邻接点还必须是在t+1时刻未被其它已规划好的路径占用的节点。根据传统的A*算法基于步骤1生成的动态拓扑图搜寻得到每辆车的全局路径后，参照车辆简化的动力学模型生成车辆遵循全局路径行驶时的参考速度和时间窗(即车辆进入和离开每个lane的时刻，进入和离开一个lane的时刻构成该lane的时间窗。)

步骤3：基于时间窗的全局路径分割：

对于传统的带时间窗的A*算法而言，车辆的实际运动状态与理想运动状态的差异在直道上影响并不到，主要影响的路网上的区域是路口。因为当多个车辆前后行驶时，车辆本身有主动避障功能，不管车辆实际运动状态与理想状态差异多大，当前车辆都得与前后车辆保持一定的安全距离。但该差异确实会影响车辆依次通过路口的顺序。

所以将每辆车的全局路径在其经过的路口处(即高精地图上的junction元素处)进行分割，再将分割后的路径逐段下发给车辆执行，即车辆执行完前一段轨迹的信息到达路口停止等待，直到接收到下一段轨迹下发了再行驶到下一个路口，以此方式走完所有分割后的路径段。

步骤4：分割后的轨迹段时间窗的调整：

根据车辆到达指定路口处的实际时刻和理想时刻间的差异，对下一段分割后的轨迹段进行时间窗的调整。具体可能情况如下：

1)若实际时刻与理想时刻差异不大(5s内)，则下一段路径直接下发不做时间窗调整。

2)若实际时刻早于理想时刻Δt s到达，并且路口无车辆可直接同行或者路口处本车辆优先级最高(即按照理想时刻本车应该最早到达路口)，则将下一段路径的时间窗都减去Δt s。

3)若实际时刻早于理想时刻Δt s到达，但路口有其他车辆并且其优先级高于本车(即其他车辆理想状态下应该先到达该路口)，优先级高的车辆花了k s通过路口，k表示等待优先级比本车高的其他车辆通过路口的时间，则将下一段路径的时间窗都减去(Δt-k)s，Δt可正可负。

当最后一段路径执行完毕后车辆全局路径整体执行完毕，车辆原地等待，直道有新的全局路径下发执行。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims

1.一种基于带时间窗A*改进的多车路径规划方法，其特征在于，包括如下步骤：

3)若实际时刻早于理想时刻Δt秒，但在前一个路径段的出口处有其他车辆并且其他车辆的优先级高于当前车辆，若等待优先级比当前车辆高的其他车辆通过的时间为k秒，则将下一个路径段的时间窗都减去Δt-k秒。

2.根据权利要求1所述基于带时间窗A*改进的多车路径规划方法，其特征在于，所述步骤1的具体过程如下：

3.根据权利要求1所述基于带时间窗A*改进的多车路径规划方法，其特征在于，步骤2所述采用带时间窗的A*算法规划当前车辆从起始节点到目标节点的全局路径中，将全局路径视为依次连接的若干节点，在第n个节点的离开时刻规划第n+1个节点时增加时间维度的约束条件：

4.根据权利要求1所述基于带时间窗A*改进的多车路径规划方法，其特征在于，步骤3所述路口包括十字路口和T型路口。

5.根据权利要求1所述基于带时间窗A*改进的多车路径规划方法，其特征在于，步骤4所述优先级最高指的是车辆的理想时刻最早。