CN111947678A - 一种针对结构化道路的自动驾驶导航方法及系统 - Google Patents

Abstract

本发明涉及自动驾驶导航引擎领域，特别涉及一种针对结构化道路的自动驾驶导航方法及系统，所述方法包括：基于高精度地图构建道路拓扑关系有向图，并根据道路拓扑关系有向图获取规划道路群；判断车辆能否从当前道路到达规划道路群中规划的下级道路，若可以则在获取规划道路群的基础上，基于高精度地图构建车道拓扑关系，并根据车道拓扑关系获取规划车道群，令当前车辆在规划道路群规划的路径上行驶并根据规划车道群的信息进行变道；本发明基于apollo格式高精地图和自动驾驶系统定位信息，合理规划道路级行车路径、车道级行车路径，为自动驾驶车辆决策规划提供充足的全局路径信息。

Description

一种针对结构化道路的自动驾驶导航方法及系统

技术领域

本发明涉及自动驾驶导航引擎领域，特别涉及一种针对结构化道路的自动驾驶导航方法及系统。

背景技术

随着自动驾驶技术的发展，为实现乘客自定义乘车点和目的地，合理规划行车路线，业内对导航引擎系统的研究愈发重视，区别于当前导航软件的地图引擎和导航引擎系统，自动驾驶导航引擎需针对自动驾驶需求进行开发和适配，对道路级、车道级进行规划，并区分车道优先级，便于自动驾驶系统做出合理正确的决策。

当前车端应用的导航引擎系统多应用于车基端，为驾驶员提供导航信息，无法应用于自动驾驶系统，无法为自动驾驶系统中“驾驶脑”决策规划提供充足的信息；高精地图是自动驾驶系统中重要的一环，目前常用高精地图格式包括opendrive格式和apollo格式，可为自动驾驶系统提供精确地道路信息、车道信息、交通标志、交通标识等信息，但如何将高精地图和导航引擎系统紧密结合，开发适用于自动驾驶系统的导航引擎系统，在公开技术中暂属于空白阶段。

目前地图软件中的导航引擎系统较为完善，但其特点和产品定位与自动驾驶系统需求匹配性较差；自动驾驶系统中高精地图定义较为完善，但目前多应用于定位功能，与导航引擎系统功能定义有一定的差距。同时，部分导航引擎系统混淆导航功能和局部路径规划功能，导航引擎是单纯根据高精地图信息(拓扑关系、车道边界限制等)和初始定位信息，规划处一条基于某种筛选规则(例如最短路径)且可以到达终点的道路级路径，并给出每条道路下推荐行走的车道和禁行车道，而具体是否按照推荐车道行驶、何时换道至推荐车道、局部车道中心线曲率是否满足车辆行走条件、车辆换道时虚实线位置限制等是局部路径规划功能考虑的，二者独立，不可混为一谈。

发明内容

为了解决上述问题，本发明提出一种一种针对结构化道路的自动驾驶导航方法及系统，所述方法具体包括以下步骤：

S1、基于高精度地图构建道路拓扑关系有向图，并根据道路拓扑关系有向图获取规划道路群；

S2、判断车辆能否根据规划道路群规划的路径前进并在规划道路群的路径上逐次变道，若可以则进行步骤S3，否则返回步骤S1重新规划；

S3、在获取规划道路群的基础上，基于高精度地图构建车道拓扑关系，并根据车道拓扑关系获取规划车道群，令当前车辆在规划道路群规划的路径上行驶并根据规划车道群的信息进行变道。

进一步的，获取规划道路群的过程包括：

利用高精地图中的道路信息，构建道路拓扑关系有向图，将道路长度信息作为有向图中各定点的权重，连接关系作为边的权重；

将当前车辆的定位信息投影到高精地图中，找到当前车辆所属道路，作为道路信息规划的起始道路；

将站点信息投影到高精地图中，找到各站点所属道路，作为道路信息规划的必经道路，将最后一个站点作为终点，其所在的道路作为终点道路；

基于路径规划搜索算法，根据道路拓扑关系有向图和起始道路、必经道路、终点道路信息获取规划道路群，表示为{起始道路编号，必经道路1编号，必经道路2编号，...，必经道路m编号，终点道路编号}。进一步的，判断车辆能否根据规划道路群规划的路径前进并在规划车道群上逐次变道的具体过程包括：

确定车道的上级车道群、下级车道群、同级车道群，并确定车道边界线类型；

判断车辆当前起始车道是否为目标车道，若是则规划成功；

否则判断车道线是否为虚线且换道距离充足，若是则可以成功规划并进行换道；

否则判断起始车道的下级车道群是否为空，若为空则规划失败，若不为空则将该车道的下级车道群的每一条车道及其对应的道路作为新的起始车道与起始道路，重复道路规划步骤，选取长度最短的作为更新结果。

更进一步的，本发明中的上级车道群指当前车道的前续车道合集，即地图拓扑关系中可以直接到达当前车道的所有车道合集；本发明中的同级车道群指当前车道的平行车道合集，与当前车道同属于同一条道路；下级车道群指当前车道的后继车道合集，即地图拓扑关系中当前车道可以直接到达的所有车道合集。

进一步的，获取规划车道群的过程包括：

利用高精地图中的道路信息，构建车道拓扑关系，确定车道的上级车道群、下级车道群、同级车道群，并确定车道边界线类型；

将当前车辆的定位信息投影到高精地图中，找到当前车辆所属车道，作为车道信息规划的起始车道；

将站点信息投影到高精地图中，找到各站点所属车道，作为车道信息规划的必经车道，将最后一个站点作为终点，其所在的车道作为终点车道；

定义规划道路群中所有道路下属的车道为可选车道群，将每条车道作为一个节点，依据车道的上级车道群、下级车道群、同级车道群以及车道边界线类型，构建车道拓扑关系有向图；

基于路径规划搜索算法，根据构建的可选车道群的拓扑关系有向图、起始车道、必经车道、终点车道信息获取规划车道群，表示为{起始推荐车道编号，必经车道1编号，必经车道2编号...终点车道编号}；

根据规划车道群，规定与规划道路群中上级道路内车道直接相连的车道推荐值为0，其同级车道群中车道与推荐值为0的车道之间的车道数为其平行车道群中车道的推荐值；

车辆在当前道路上行驶时，若当前行驶的车道不是当前道路的必经车道，则根据推荐值判断向左侧还是右侧换道，直到行驶到当前道路的必经车道上。

进一步的，构建车道拓扑关系有向图的过程中，将一条道路上的一个车道视为一个节点，两个节点连通时，可进行驾驶操作，其中：

任意两个不直接相连的节点间代价值为MAX，MAX为一个极大值，为当前车道不可到达车道的代价值；

节点到达其自身的代价值为0；

每个节点到达其上级车道群中每条车道节点的代价值为0；

每个节点到达其同级车道群中每条车道节点的代价值为需要换道的数量；

若节点在高精地图中左右边界为实线无法换道，则节点i的相邻左车道代价值为MAX；

当前车道为公交车道等禁行车道时，节点到任何车道的代价值均为MAX。

本发明还提出一种针对结构化道路的自动驾驶导航系统，所述系统包括导航服务器以及与导航服务器连接的道路规划模块、车道规划模块以及规划检测模块，其中：

导航服务器，用于通过网络获取高精度地图信息、将规划检测模块检测合格的导航规划对应的道路规划群、车道规划群及其对应的车道推荐值下发给车辆；

道路规划模块，用于根据导航服务器获取的高精度地图信息获取当前车辆的道路规划群；

车道规划模块，用于根据导航服务器获取的高精度地图信息获取当前车辆的车道规划群；

规划检测模块，用于根据道路规划群和车道规划群中的规划信息判断车辆能否将该规划信息下发到车辆，若可以则将道路规划群和车道规划群发送给导航服务器。

本发明基于apollo格式高精地图和自动驾驶系统定位信息，根据外部输入(乘客用户等)的车辆预约到达起点、终点信息，合理规划道路级行车路径、车道级行车路径，为自动驾驶车辆决策规划提供充足的全局路径信息。

附图说明

图1为本发明的整体流程图；

图2为本发明的道路信息规划功能流程图；

图3为本发明的道路信息规划功能示意图；

图4为本发明车道信息规划功能流程图；

图5为本发明重规划检测流程图；

图6为本发明重规划检测功能示意图；

图7为本发明一种针对结构化道路的自动驾驶导航系统结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供一种针对结构化道路的自动驾驶导航方法，如图1，具体包括以下步骤：

S1、基于高精度地图构建道路拓扑关系有向图，并根据道路拓扑关系有向图获取规划道路群；

S2、判断车辆能否根据规划道路群规划的路径前进并在规划道路群的路径上逐次变道，若可以则进行步骤S3，否则返回步骤S1重新规划；

S3、在获取规划道路群的基础上，基于高精度地图构建车道拓扑关系，并根据车道拓扑关系获取规划车道群，令当前车辆在规划道路群规划的路径上行驶并根据规划车道群的信息进行变道。

实施例1

基于高精地图中对道路、车道等名称的定义，本发明的技术方案在于：输入信息包括高精地图、自动驾驶车辆定位信息(简称定位信息)、车辆预约到达的站点信息(简称站点信息)；输出信息为规划道路群、规划车道群，包括到达终点所经过的道路信息、车道信息。依靠道路信息规划功能、车道信息规划功能和重规划检测功能实现，顺序执行上述功能后，即可通过输入信息计算出输出信息，整体流程如图1所示，下面分别对其进行介绍。

(1)道路信息规划功能

道路信息规划流程如图2所示。

首先，根据高精地图中包含的道路信息，构建道路拓扑关系有向图，将道路长度信息作为有向图中各定点的权重，连接关系作为边的权重。其次，将定位信息投影到高精地图中，找到当前车辆所属道路，作为道路信息规划的起始道路，将站点信息投影到高精地图中，找到各站点所属道路，作为道路信息规划的必经道路，定义最后一个站点为终点，定义最后一条必经道路为终点道路。构建示例如图3所示。

根据上述构建的道路拓扑关系有向图和起始道路、必经道路、终点道路信息，基于路径规划搜索方法，例如Dijkstra算法、A*算法，搜索出最短的、连接起始道路、必经道路、终点道路的行驶路线，并将所有经过的道路编号存储，形成规划道路群。规划道路群示例：{起始道路编号，必经道路1编号，必经道路2编号...终点道路编号}。

(2)重规划检测功能

对生成的规划道路群进行检查，主要防止规划后路线无法实际行驶。例如，起始道路内，起始车道并非目标车道且无法通过换道到达同级车道群下的目标车道，例如车道线为白实线/黄实线或距离实线区的纵向距离不满足换道时车辆动力学最短纵向距离要求，此时显然规划道路群和规划车道群无法满足实际行车需求，因此需重新规划。

重规划检测功能流程如图5所示，具体包括以下步骤：

判断车辆当前起始车道是否为目标车道，若是则规划成功；

否则判断车道线是否为虚线且换道距离充足，若是则可以成功规划并进行换道；

否则判断起始车道的下级车道群是否为空，若为空则规划失败，若不为空则将该车道的下级车道群的每一条车道及其对应的道路作为新的起始车道与起始道路，重复道路规划步骤，选取长度最短的作为更新结果。

重规划检验只要是对起始道路与其下级道路的检验，防止出现虽然存在拓扑关系，但由于车辆距离道路终点太近导致无法换道到达下级道路的情况发生，例如图6中由于当前车在右转专用车道上，虽然道路连接关系可左转、直行，但是对于车道来说是不可以的，因此需要重新规划。图6中车辆所在车道无法直行直接到达站点A，只能右转后掉头再前往站点A，当车辆已经位于图中所在的车道时，不能直接通过直行的道路到达站点A所在的道路，只能先右转到达，再在右转之后的道路上调头，在调头之后右转到达站点A所在的道路。

(3)车道信息规划功能

车道信息规划流程如图4所示。

首先，定义规划道路群中所有道路下属的车道为可选车道群，将每条车道作为一个节点，依据车道的上级车道群、下级车道群、同级车道群以及车道边界线类型，构建车道拓扑关系有向图。

其次，基于路径规划搜索算法，根据构建的可选车道群的拓扑关系有向图、起始车道、必经车道、终点车道信息获取规划车道群，表示为{起始推荐车道编号，必经车道1编号，必经车道2编号...终点车道编号}，即车辆位于起始道路的起始车道起步，在起始道路上换道到必经车道1，因此必经道路1是与车辆与起始道路直接相连的车道，在必经车道1上行驶直到到达必经道路1，以此类推，直到到达终点道路的终点车道，必经车道是连接规划路径中两条相邻道路的车道。

根据规划车道群，规定与规划道路群中上级道路内车道直接相连的车道推荐值为0，即必经车道的推荐值为0，其同级车道群中车道与推荐值为0的车道之间的车道数为其平行车道群中车道的推荐值。

例如，从终点车道向前开始迭代，即当前车道为终点车道，若当前道路上没有站点，则需要从当前车道换道到与下一道路直接连接的车道上，从当前车道换道到与下一道路直接连接的车道过程中可能有一个或多个换道过程，将与终点车道相邻的且与必经车道最近的车道作为目标车道n，即选择相邻车道中推荐值最小的车道作为目标车道，到达目标车道n之后，按以上方法继续选择并换道到目标车道n-1，直到到达起点车道；特别地，本发明同一条道路上车辆的必经车道包括站点所在的车道和与下一道路直接连接的车道，若当前道路没有站点，则只包括与下一道路直接连接的车道，当同一道路上存在多个站点时可以将必经车道的推荐值都设置为0，并且在未规划的路段中距离当前车道越近其规划时的优先级越高，即需要依次驶入同一道路上的需要停靠站点所在的车道，并在该道路结束之前驶入与下一道路直接连接的车道。

在本发明中必经车道与规划道路群存在对应关系，当前道路的最后一个必经车道必定是与下一道路直接相连的任一车道，当行驶到下一道路的车道后再判断是否到达下一道路的必经车道，如果是则无需换道，如图3，若当前在道路5，道路5的上级车道是道路4、下级道路是道路6，在道路4中可以直接到达道路5的车道是4_1_-1，因此将4_1_-1作为道路4必经车道，即在道路4中车道4_1_-1的推荐值为0；

在当车辆成功换道到与车道5_1_-1直接相连的车道6_1_-1上后，发现车道不能从该车道直接到达道路7上的车道，即车道6_1_-1不是道路6的必经车道，此时需要车辆进行换道，计算当前所在车道与必经车道之间的车道数，即当前车道的推荐值，车辆从当前车道换道到推荐值较小的车道上，直到换道到必经车道，即推荐值为0的车道。优选的，当前道路上存在多条与下一道路直接相连的车道时，选择换道数最少的车道作为必经车道。

本实施例在构建车道的拓扑关系有向图时，将道路上的一条车道作为一个节点，仅连通的节点之间可以进行驾驶，规定每个节点之间的连通关系为：

任意两个不直接相连(无法直接到达)的节点间代价值为MAX，例如节点i与其不相连节点j的代价值cost(i,j)＝MAX，MAX为一个极大值，为当前车道不可到达车道的代价值；

节点到达其自身的代价值为0，例如节点i到达节点i的cost(i,i)＝0；

每个节点到达其上级车道群中每条车道节点的代价值为0，例如节点i上级车道为节点j，则cost(i,j)＝0；

每个节点到达其平行车道群中每条车道节点的代价值为需要换道的数量，例如节点j所在车道为节点i所在车道的相邻左车道，则达到节点j所在车道的代价值cost(i,j)＝1，到达位于节点i左2车道节点k所在车道的代价值cost(i,k)＝2；

节点i在高精地图中左右边界为实线无法换道，则节点i的相邻左车道j的cost(i,j)＝MAX；

节点i为公交车道等禁行车道时，节点i到任何车道的cost值均为MAX。

如图3，当前道路方向中最左边的车道，即图中虚线所指的车道为当前道路中尾号为“-1”的车道，车辆位于起始车道1_1_-2，即车道规划中的目标车道n，在已知目标车道n-1的情况下，计算从目标车道n-1到达目标车道n的车道数即为推荐值cost，即车辆从上一道路的必经车道行驶到当前道路，通过推荐值确定上一道路的必经车道与当前道路的必经车道的关系，若为同一车道，即推荐值为0。

最终形成规划车道群，包括目标车道群、车道推荐值和推荐车道三类信息。流程如图4所示。

实施例2

本实施例提出一种针对结构化道路的自动驾驶导航系统，如图7，所述系统包括导航服务器以及与导航服务器连接的道路规划模块、车道规划模块以及规划检测模块，其中：

导航服务器，用于通过网络获取高精度地图信息、将规划检测模块检测合格的导航规划对应的道路规划群、车道规划群及其对应的车道推荐值下发给车辆；

道路规划模块，用于根据导航服务器获取的高精度地图信息获取当前车辆的道路规划群；

车道规划模块，用于根据导航服务器获取的高精度地图信息获取当前车辆的车道规划群；

规划检测模块，用于根据道路规划群和高精度地图信息进行道路规划，判断车辆能否根据规划道路群规划的路径前进并在规划道路群的路径上逐次变道，若可以则给车道规划模块下发命令，否则令道路规划模块重新进行规划。

车道规划群中的规划信息判断车辆能否将该规划信息下发到车辆，若可以则将道路规划群和车道规划群发送给导航服务器。

道路规划模块、车道规划模块以及规划检测模块的工作原理已在方法中详细描述，此处不再赘述。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (10)

1.一种针对结构化道路的自动驾驶导航方法，其特征在于，具体包括以下步骤：

S1、基于高精度地图构建道路拓扑关系有向图，并根据道路拓扑关系有向图获取规划道路群；

S2、判断车辆能否从当前道路到达规划道路群中规划的下级道路，若可以则进行步骤S3，否则返回步骤S1重新规划；

S3、在获取规划道路群的基础上，基于高精度地图构建车道拓扑关系，并根据车道拓扑关系获取规划车道群，令当前车辆在规划道路群规划的路径上行驶并根据规划车道群的信息进行变道。

2.根据权利要求1所述的一种针对结构化道路的自动驾驶导航方法，其特征在于，获取规划道路群的过程包括：

利用高精地图中的道路信息，构建道路拓扑关系有向图，将道路长度信息作为有向图中各定点的权重，连接关系作为边的权重；

将当前车辆的定位信息投影到高精地图中，找到当前车辆所属道路，作为道路信息规划的起始道路；

将站点信息投影到高精地图中，找到各站点所属道路，作为道路信息规划的必经道路，将最后一个站点作为终点，其所在的道路作为终点道路；

基于路径规划搜索算法，根据道路拓扑关系有向图和起始道路、必经道路、终点道路信息获取规划道路群，表示为{起始道路编号，必经道路1编号，必经道路2编号，...，必经道路n-1编号，终点道路n编号}。

3.根据权利要求1所述的一种针对结构化道路的自动驾驶导航方法，其特征在于，判断车辆能否从当前道路到达规划道路群中规划的下级道路的具体过程包括：

确定车道的下级车道群、同级车道群，并确定车道边界线类型；

判断车辆当前起始车道是否通过拓扑关系可直接到达规划道路群中的必经道路，若是则规划成功；

否则判断车道线是否为虚线且换道距离充足，若是则可以成功规划；

否则判断起始车道的下级车道群是否为空，若为空则规划失败，若不为空，则将该车道的下级车道群中属于必经道路的每一条车道及其对应的道路作为新的起始车道与起始道路，重复道路规划步骤，选取换道次数最少或者路径最短的作为更新结果。

4.根据权利要求1所述的一种针对结构化道路的自动驾驶导航方法，其特征在于，获取规划车道群的过程包括：

将当前车辆的定位信息投影到高精地图中，找到当前车辆所属车道，作为车道信息规划的起始车道；

定义规划道路群中所有道路下属的车道为可选车道群，将每条车道作为一个节点，依据车道的上级车道群、下级车道群、同级车道群以及车道边界线类型，构建车道拓扑关系有向图；

基于路径规划搜索算法，根据构建的可选车道群的拓扑关系有向图、起始车道、必经车道、终点车道信息获取规划车道群，表示为{起始推荐车道编号，必经车道1编号，必经车道2编号...终点车道编号}；

根据规划车道群，规定与规划道路群中上级道路内车道直接相连的车道推荐值为0，其同级车道群中车道与推荐值为0的车道之间的车道数为该车道的推荐值；

车辆在当前道路上行驶时，若当前行驶的车道不是当前道路的必经车道，则根据推荐值判断向左侧还是右侧换道，直到行驶到当前道路的必经车道上。

5.根据权利要求4所述的一种针对结构化道路的自动驾驶导航方法，其特征在于，构建车道拓扑关系过程中，将一条道路上的一个车道视为一个节点，两个节点连通时，可进行驾驶操作，其中：

任意两个不直接相连的节点间代价值为MAX，MAX为一个极大值，为当前车道不可到达车道的代价值；

节点到达其自身的代价值为0；

每个节点到达其上级车道群中每条车道节点的代价值为0；

每个节点到达其同级车道群中每条车道节点的代价值为需要换道的数量；

若节点在高精地图中左右边界为实线无法换道，则节点i的相邻左车道代价值为MAX；

当前车道为公交车道等禁行车道时，节点到任何车道的代价值均为MAX。

6.一种针对结构化道路的自动驾驶导航系统，其特征在于，所述系统包括导航服务器以及与导航服务器连接的道路规划模块、车道规划模块以及规划检测模块，其中：

导航服务器，用于通过网络获取高精度地图信息、将规划检测模块检测合格的导航规划对应的道路规划群、车道规划群及其对应的车道推荐值下发给车辆；

道路规划模块，用于根据导航服务器获取的高精度地图信息获取当前车辆的道路规划群；

车道规划模块，用于根据导航服务器获取的高精度地图信息获取当前车辆的车道规划群；

规划检测模块，用于根据道路规划群和高精度地图信息进行道路规划，判断车辆能否根据规划道路群规划的路径前进并在规划道路群的路径上逐次变道，若可以则给车道规划模块下发命令，否则令道路规划模块重新进行规划。

车道规划群中的规划信息判断车辆能否将该规划信息下发到车辆，若可以则将道路规划群和车道规划群发送给导航服务器。

7.根据权利要求6所述的一种针对结构化道路的自动驾驶导航系统，其特征在于，道路规划模块进行的操作包括：

利用高精地图中的道路信息，构建道路拓扑关系有向图，将道路长度信息作为有向图中各定点的权重，连接关系作为边的权重；

将当前车辆的定位信息投影到高精地图中，找到当前车辆所属道路，作为道路信息规划的起始道路；

将站点信息投影到高精地图中，找到各站点所属道路，作为道路信息规划的必经道路，将最后一个站点作为终点，其所在的道路作为终点道路；

基于路径规划搜索算法，根据道路拓扑关系有向图和起始道路、必经道路、终点道路信息获取规划道路群，表示为{起始道路编号，必经道路1编号，必经道路2编号，...，必经道路n-1编号，终点道路n编号}。

8.根据权利要求6所述的一种针对结构化道路的自动驾驶导航系统，其特征在于，车道规划模块进行的操作包括：

将当前车辆的定位信息投影到高精地图中，找到当前车辆所属车道，作为车道信息规划的起始车道；

定义规划道路群中所有道路下属的车道为可选车道群，将每条车道作为一个节点，依据车道的上级车道群、下级车道群、同级车道群以及车道边界线类型，构建车道拓扑关系有向图；

基于路径规划搜索算法，根据构建的可选车道群的拓扑关系有向图、起始车道、必经车道、终点车道信息获取规划车道群，表示为{起始推荐车道编号，必经车道1编号，必经车道2编号...终点车道编号}；

根据规划车道群，规定与规划道路群中上级道路内车道直接相连的车道推荐值为0，其同级车道群中车道与推荐值为0的车道之间的车道数为其同级车道群中车道的推荐值；

车辆在当前道路上行驶时，若当前行驶的车道不是当前道路的必经车道，则根据推荐值判断向左侧还是右侧换道，直到行驶到当前道路的必经车道上。

9.根据权利要求8所述的一种针对结构化道路的自动驾驶导航系统，其特征在于，构建车道拓扑关系过程中，将一条道路上的一个车道视为一个节点，两个节点连通时，可进行驾驶操作，其中：

任意两个不直接相连的节点间代价值为MAX，MAX为一个极大值，为当前车道不可到达车道的代价值；

节点到达其自身的代价值为0；

每个节点到达其上级车道群中每条车道节点的代价值为0；

每个节点到达其同级车道群中每条车道节点的代价值为需要换道的数量；

若节点在高精地图中左右边界为实线无法换道，则节点i的相邻左车道代价值为MAX；

当前车道为公交车道等禁行车道时，节点到任何车道的代价值均为MAX。

10.根据权利要求6所述的一种针对结构化道路的自动驾驶导航系统，其特征在于，规划检测模块包括：

判断车辆当前起始车道是否为目标车道，若是则规划成功；

否则判断车道线是否为虚线且换道距离充足，若是则可以成功规划并进行换道；

否则判断起始车道的下级车道群是否为空，若为空则规划失败，若不为空则将该车道的下级车道群的每一条车道及其对应的道路作为新的起始车道与起始道路，重复道路规划步骤，选取换道次数最少或者路径最短的路径作为更新结果。

Images