CN113516865B - 基于高精度地图的矿山无人驾驶路网车辆排队方法和装置 - Google Patents

Abstract

本发明提供了一种基于高精度地图的矿山无人驾驶路网车辆排队方法和装置，涉及路网控制技术领域。本发明首先将矿山无人驾驶高精地图构建成拓扑地图，采用节点和边的方式对拓扑地图进行结构描述。然后根据拓扑边上高精地图的车道中心线数据，进行拓扑边的KD树构建，并确定拓扑边的组成点序列。当车辆在地图内运行时，根据车辆的定位信息以及拓扑节点和拓扑边信息，获取车辆当前在拓扑地图的位置。如果车辆在拓扑边上，则根据拓扑边的KD树最终可以确定车辆在边上的排队次序。有助于后续在地图路网内运行过程对无人车辆进行有序的交通控制。

Description

基于高精度地图的矿山无人驾驶路网车辆排队方法和装置

技术领域

本发明涉及路网控制技术领域，具体涉及一种基于高精度地图的矿山无人驾驶路网车辆排队方法和装置。

背景技术

矿山无人驾驶是典型的封闭场景内的无人驾驶。由于矿山环境的多样性和可变性，矿山无人驾驶车辆行驶的路网也会随着作业的推进发生变化，这种情况下设置固定的交通控制设备(如红绿灯)并不合理。

高精度地图的引入给车辆在路网上的交通控制提供了很好的参考信息。随着环境的变化，高精度地图也会跟着发生变化，整个地图的路由信息、路口信息、作业区域信息、地图拓扑信息就会随之发生变化。现有的基于露天矿山的无人驾驶系统的路权分配及管控方法均是基于车辆的任务轨迹和定位信息来实现。

如果能够将高精度地图信息应用到交通控制中，可减少对专用交通控制设备的依赖，将极大的降低成本，同时也提高作业的灵活性。而利用高精度地图进行交通控制的关键在于如何实现车辆在边上的有序排队，现有的技术中并未涉及。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种基于高精度地图的矿山无人驾驶路网车辆排队方法和装置，解决了如何基于高精度地图实现车辆在边上的有序排队的问题。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：

第一方面，提供了一种基于高精度地图的矿山无人驾驶路网车辆排队方法，该方法包括：

基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

构建拓扑地图G中各个边的控制点序列和对应的KD树；

基于车辆定位坐标，确定车辆是否位于边上；

基于KD树和控制点序列，确定位于边上的车辆的排队次序。

进一步的，所述构建拓扑地图G中各个边的控制点序列和对应的KD树，包括：

对于节点i与j连接的边e_ij，i,j∈V：

获取边e_ij对应的车道的中心线的所有控制点；

按照所述控制点的先后顺序对控制点进行抽稀或插值，得到等间距排布的控制点序列；

按照先后顺序，将控制点序列中的控制点依次加入KD树，得到边的三维KD树。

进一步的，所述基于车辆定位坐标，确定车辆是否位于边上，包括：

基于高精度地图和车辆定位坐标，获取节点包围盒以及车辆包围盒；

遍历所有节点，判定是否满足节点包围盒包含车辆包围盒或车辆包围盒与节点包围盒相交；

若不满足，则车辆在边上。

进一步的，所述基于KD树和控制点序列，确定位于边上的车辆的排队次序，包括：

对于任一车辆：

遍历所有边的KD树，获取车辆与各个边上的最近控制点，得到最近控制点的索引以及车辆与最近控制点的距离；

从中选择车辆与最近控制点的距离最小的控制点所在边作为车辆所在边；

获取所有在车辆所在边上的其他车辆的索引，并按照索引大小排队，得到车辆所在边上的排队次序。

进一步的，该方法还包括：基于所述排队次序，确定边上的车辆的排队位置，其中：

所述车辆的排队位置的计算方法为：

其中，size(S_ij)表示边e_ij的控制点序列S_ij中的控制点数量；

d_stop表示车辆在边终点处停车时距离边终点的停车距离；

d_queue表示车辆在边上排队时，车辆之间的间距；

N_queue表示车辆的排队次序；

d_interval表示控制点序列的间距；

S_ij(n)表示控制点序列S_ij中第n个控制点；

表示控制点S_ij(n)的位置。

第二方面，提供了一种基于高精度地图的矿山无人驾驶路网车辆排队装置，该装置包括：

地图拓扑模块，用于基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

KD树构建模块，用于构建拓扑地图G中各个边的控制点序列和对应的KD树；

车辆位置判定模块，用于基于车辆定位坐标，确定车辆是否位于边上；

排队次序确定模块，用于基于KD树和控制点序列，确定位于边上的车辆的排队次序。

进一步的，所述构建拓扑地图G中各个边的控制点序列和对应的KD树，包括：

对于节点i与j连接的边e_ij，i,j∈V：

获取边e_ij对应的车道的中心线的所有控制点；

按照所述控制点的先后顺序对控制点进行抽稀或插值，得到等间距排布的控制点序列；

按照先后顺序，将控制点序列中的控制点依次加入KD树，得到边的三维KD树。

进一步的，所述基于车辆定位坐标，确定车辆是否位于边上，包括：

基于高精度地图和车辆定位坐标，获取节点包围盒以及车辆包围盒；

遍历所有节点，判定是否满足节点包围盒包含车辆包围盒或车辆包围盒与节点包围盒相交；

若不满足，则车辆在边上。

进一步的，所述基于KD树和控制点序列，确定位于边上的车辆的排队次序，包括：

对于任一车辆：

遍历所有边的KD树，获取车辆与各个边上的最近控制点，得到最近控制点的索引以及车辆与最近控制点的距离；

从中选择车辆与最近控制点的距离最小的控制点所在边作为车辆所在边；

获取所有在车辆所在边上的其他车辆的索引，并按照索引大小排队，得到车辆所在边上的排队次序。

进一步的，该装置还包括：

排队位置确定模块，用于基于所述排队次序，确定边上的车辆的排队位置，其中：

所述车辆的排队位置的计算方法为：

其中，size(S_ij)表示边e_ij的控制点序列S_ij中的控制点数量；

d_stop表示车辆在边终点处停车时距离边终点的停车距离；

d_queue表示车辆在边上排队时，车辆之间的间距；

N_queue表示车辆的排队次序；

d_interval表示控制点序列的间距；

S_ij(n)表示控制点序列S_ij中第n个控制点；

表示控制点S_ij(n)的位置。

第三方面，提供了一种计算机可读存储介质，其存储用于矿山无人驾驶路网车辆排队的计算机程序，其中，所述计算机程序使得计算机执行上述的基于高精度地图的矿山无人驾驶路网车辆排队方法。

第四方面，一种电子设备，包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行上述的基于高精度地图的矿山无人驾驶路网车辆排队方法。

(三)有益效果

本发明提供了一种基于高精度地图的矿山无人驾驶路网车辆排队方法和装置。与现有技术相比，具备以下有益效果：

本发明首先将矿山无人驾驶高精地图构建成拓扑地图，采用节点和边的方式对拓扑地图进行结构描述。然后根据拓扑边上高精地图的车道中心线数据，进行拓扑边的KD树构建，并确定拓扑边的组成点序列。当车辆在地图内运行时，根据车辆的定位信息以及拓扑节点和拓扑边信息，获取车辆当前在拓扑地图的位置。如果车辆在拓扑边上，则根据拓扑边的KD树最终可以确定车辆在边上的排队次序。有助于后续在地图路网内运行过程对无人车辆进行有序的交通控制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的流程图；

图2为本发明实施例构建的拓扑地图示意图；

图3为本发明实施例的等间距控制点示意图；

图4为本发明实施例的车辆包围盒与节点包围盒的关系；

图5为本发明实施例的边上的车辆的排队位置的示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本申请实施例通过提供一种基于高精度地图的矿山无人驾驶路网车辆排队方法和装置，解决了如何基于高精度地图实现车辆在边上的有序排队的问题。

本申请实施例中的技术方案为解决上述技术问题，总体思路如下：

首先将矿山无人驾驶高精地图构建成拓扑地图，采用节点和边的方式对拓扑地图进行结构描述。然后根据拓扑边上高精地图的车道中心线数据，进行拓扑边的KD树构建，并确定拓扑边的组成点序列。当车辆在地图内运行时，根据车辆的定位信息以及拓扑节点和拓扑边信息，获取车辆当前在拓扑地图的位置。如果车辆在拓扑边上，则根据拓扑边的KD树确定车辆在边上的排队次序。根据排队次序以及排队间距信息，对车辆在拓扑边上的停车位置进行控制，实现车辆有序排队。有助于后续在地图路网内运行过程对无人车辆进行有序的交通控制。

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

实施例1：

如图1所示，本发明提供了一种基于高精度地图的矿山无人驾驶路网车辆排队方法，该方法由计算机执行，该方法包括：

S1、基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

S2、构建拓扑地图G中各个边的控制点序列和对应的KD树；

S3、基于车辆定位坐标，确定车辆是否位于边上；

S4、基于KD树和控制点序列，确定位于边上的车辆的排队次序。

本实施例的有益效果为：

本发明实施例首先将矿山无人驾驶高精地图构建成拓扑地图，采用节点和边的方式对拓扑地图进行结构描述。然后根据拓扑边上高精地图的车道中心线数据，进行拓扑边的KD树构建，并确定拓扑边的组成点序列。当车辆在地图内运行时，根据车辆的定位信息以及拓扑节点和拓扑边信息，获取车辆当前在拓扑地图的位置。如果车辆在拓扑边上，则根据拓扑边的KD树最终可以确定车辆在边上的排队次序。有助于后续在地图路网内运行过程对无人车辆进行有序的交通控制。

下面对本发明实施例的实现过程进行详细说明：

S1、矿山无人驾驶是典型的封闭作业场景，其路网可以用加权有向图进行模拟。将矿山无人驾驶作业的区域(装载区、卸载区)以及路网交叉口(路口)作为有向图的顶点，由于高精度地图时以车道作为最小单位进行输出的，但是在进行交通控制时，车辆在一段没有路口的车道上行车时，车辆的运行安全(间距保持、跟车、超车、换车道等)是由车辆无人驾驶的决策规划来决定，因此在考虑所有车辆的交通控制时，是不需要考虑车道这一个层级的。但是，为了确保车辆能够依次进入路口、装载区、卸载区等节点，需要在节点之间的车道上对车辆进行排队，因此需要将连接作业区域以及路口的路网作为有向图的边，为了简化起见，考虑所有路段均为双向道路(即无向图)。

如图2所示，可以构建出整个矿山无人驾驶的路网图G＝(V,E)。

其中，顶点集合V＝v₁,v₂,...,v_m表示矿山无人驾驶作业区域以及路口的集合；例如图2中的v₁～v₇，v₁～v₃为装载区，v₄～v₅为路口，v₆～v₇为排土场；

边集合E＝{e_ij},i＝1,...,m,j＝1,...,m表示顶点i与j之间连接的边e_ij的集合，且

通过上述拓扑地图化后，整个矿区的高精度地图就变成只有节点和边的拓扑地图。下面，我们对拓扑地图的边进行KD树构建。

车辆在路网的边上主要考虑车辆的排队顺序问题，从而避免出现多车无规则的进入节点，导致发生拥堵和碰撞现象。

无人驾驶车辆在边上的排队依据为车辆所在的位置与路网的边的相对关系。车辆的位置可以通过车辆的GPS传感器等直接上传并转化为车辆在地图上的投影坐标。有了投影坐标后还需要知道该坐标与边的关系，才能确定不同车辆的先后顺序。

S2、构建拓扑地图G中各个边的控制点序列和对应的KD树；

对于任一节点i与j连接的边e_ij，i,j∈V：

S2.1、获取边对应的车道的中心线的所有控制点；

具体的，获取边e_ij的所有组成车道lane₁，....，lane_n，如果边上存在多个并行车道的情况，则只取多车道的其中一个车道。如图3所示，该边共包括四条车道lane₁，....，lane₄为例，获取其车道中心线控制点(即高精度地图中定义车道中心线的三维点列表，点间距不固定)。

S2.2、由于不同车道的控制点间距可能不一致，因此需要按照所述控制点的先后顺序对控制点进行抽稀或插值，得到如图3所示的等间距排布的控制点序列；

其中，控制点序列的间距d_interval可自定义，例如车道lane1为直线，因此两点间距为300m，而车道lane₂为曲线，两点间距为20m，而d_interval＝60m，则对于车道lane₁需要进行差值，得到间隔为60m的控制点，而车道lane₂则需要进行抽稀，得到间隔为60m的控制点。

S2.3、按照先后顺序，将控制点序列中的控制点依次加入KD树，得到边的三维KD树。

其中，KD树的KEY为三维点，KD树的Value为点在点序列中的索引。

得到所有边的KD树之后，就可以根据车辆的位置进行车辆路网边排序。

S3、基于车辆定位坐标，确定车辆是否位于边上，包括：

S3.1、基于高精度地图和车辆定位坐标，获取节点包围盒以及车辆包围盒；

S3.2、遍历所有节点，判定是否满足节点包围盒包含车辆包围盒或车辆包围盒与节点包围盒相交；具体的：

如图4所示，车辆包围盒与节点包围盒的关系(包含、相交、不相交)，

如果车辆包围盒与节点包围盒的关系为交叉或者包含关系，则认为车辆在节点内。

如果车辆不在节点内，则认为车辆在边上，因此需要确定车辆在哪一条边上，以及车辆在该边上的索引。

S4、基于KD树和控制点序列，确定位于边上的车辆的排队次序，包括：

具体的方法是对于任一车辆：

遍历所有的边，利用该拓扑边的KD树寻找车辆当前位置在边e_ij上的最近点，获取最近点的索引index_nij，并计算当前车辆坐标离最近点的距离d_nij。最后，取距离d_nij最小的值对应的边作为车辆当前所在的边e_ij，车辆在该边上的索引index即为当前车辆的拓扑边索引。即：

获取所有在同一条边上的车辆，并确定车辆在该边上的索引值，按照索引值从大到小的顺序进行排序后，即可以得到车辆在该条边上的排队次序N_queue。

S5、在获取排队次序后，还可进一步基于所述排队次序，确定如图5所示的边上的车辆的排队位置，

所述车辆的排队位置的计算方法为：

其中，size(S_ij)表示边e_ij的控制点序列S_ij中的控制点数量；

d_stop表示车辆在边终点处停车时距离边终点的停车距离，用于防止车辆控制误差导致车辆在未允许进入节点的情况下进入节点；

d_queue表示车辆在边上排队时，车辆之间的间距；

N_queue表示车辆的排队次序；

d_interval表示控制点序列的间距；

S_ij(n)表示控制点序列S_ij中第n个控制点；

表示控制点S_ij(n)的位置。

经过上述对车辆在拓扑边上的排序，就可以在多个车辆进入同一个节点时进行有序的交通控制。例如，当车辆靠近节点时，通过节点优先级、车辆排队次序、车辆放行优先级对车辆进行控制，实现交通节点内的有序放行。

举例说明交通控制的具体步骤：

(1)对每一个节点，设置两个服务队列，一个为当前服务队列，一个为期望服务队列，并将两个服务队列初始化为空。

(2)假设车辆从不同边进入同一个节点，判断所有边排在最前面的车与节点之间的距离，距离低于指定值时(如200米)，将车辆加入到期望服务队列。

(3)当有车辆靠近节点指定距离范围内(如100米)后，判断当前节点是否允许有车辆进入节点内部(如检查当前服务队列是否为空或者小于最大车辆数)。如果允许车辆进入，则判断期望服务队列内所有车辆的优先级，取优先级最高的车辆进行放行。如果节点不允许车辆进入，则车辆根据排队次序在各边依次等待。

(4)当被放行车辆完全进入节点后，将该车辆加入当前服务队列，并从期望服务队列中剔除。

(5)当车辆从节点内部驶出节点后，将车辆从当前服务队列中剔除。

循环上述操作，当各边上的车辆靠近节点后，会根据车辆的排队次序、放行优先级、节点的通行能力，对车辆进行有序的交通控制，实现交汇节点的安全有序运行。

实施例2：

一种基于高精度地图的矿山无人驾驶路网车辆排队装置，该装置包括：

地图拓扑模块，用于基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

KD树构建模块，用于构建拓扑地图G中各个边的控制点序列和对应的KD树；

车辆位置判定模块，用于基于车辆定位坐标，确定车辆是否位于边上；

排队次序确定模块，用于基于KD树和控制点序列，确定位于边上的车辆的排队次序。

可理解的是，本发明实施例提供的基于高精度地图的矿山无人驾驶路网车辆排队装置与上述基于高精度地图的矿山无人驾驶路网车辆排队方法相对应，其有关内容的解释、举例、有益效果等部分可以参考基于高精度地图的矿山无人驾驶路网车辆排队方法中的相应内容，此处不再赘述。

实施例3：

一种计算机可读存储介质，其存储用于矿山无人驾驶路网车辆排队的计算机程序，其中，所述计算机程序使得计算机执行如下步骤：

S1、基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

S2、构建拓扑地图G中各个边的控制点序列和对应的KD树；

S3、基于车辆定位坐标，确定车辆是否位于边上；

S4、基于KD树和控制点序列，确定位于边上的车辆的排队次序。

实施例4：

一种电子设备，包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行如下步骤：

S1、基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

S2、构建拓扑地图G中各个边的控制点序列和对应的KD树；

S3、基于车辆定位坐标，确定车辆是否位于边上；

S4、基于KD树和控制点序列，确定位于边上的车辆的排队次序。

综上所述，与现有技术相比，本发明具备以下有益效果：

本发明首先将矿山无人驾驶高精地图构建成拓扑地图，采用节点和边的方式对拓扑地图进行结构描述。然后根据拓扑边上高精地图的车道中心线数据，进行拓扑边的KD树构建，并确定拓扑边的组成点序列。当车辆在地图内运行时，根据车辆的定位信息以及拓扑节点和拓扑边信息，获取车辆当前在拓扑地图的位置。如果车辆在拓扑边上，则根据拓扑边的KD树确定车辆在边上的排队次序。根据排队次序以及排队间距信息，对车辆在拓扑边上的停车位置进行控制，实现车辆有序排队。有助于后续在地图路网内运行过程对无人车辆进行有序的交通控制。

需要说明的是，通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个......”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种基于高精度地图的矿山无人驾驶路网车辆排队方法，其特征在于，该方法包括：

基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

构建拓扑地图G中各个边的控制点序列和对应的KD树；所述控制点序列包括各个边对应的车道的中心线的等间距排布的控制点；且所述KD树的构建步骤包括：按照先后顺序，将控制点序列中的控制点依次加入KD树，得到边的三维KD树；

基于车辆定位坐标，确定车辆是否位于边上；且其步骤包括：遍历所有节点，判定是否满足节点包围盒包含车辆包围盒或车辆包围盒与节点包围盒相交；若不满足，则车辆在边上；

基于KD树和控制点序列，确定位于边上的车辆的排队次序；且其步骤包括：

对于任一车辆：

遍历所有边的KD树，获取车辆与各个边上的最近控制点，得到最近控制点的索引以及车辆与最近控制点的距离；

从中选择车辆与最近控制点的距离最小的控制点所在边作为车辆所在边；

获取所有在车辆所在边上的其他车辆的索引，并按照索引大小排队，得到车辆所在边上的排队次序。

2.如权利要求1所述的一种基于高精度地图的矿山无人驾驶路网车辆排队方法，其特征在于，所述构建拓扑地图G中各个边的控制点序列和对应的KD树，包括：

对于节点i与j连接的边e_ij，i,j∈V：

获取边e_ij对应的车道的中心线的所有控制点；

按照所述控制点的先后顺序对控制点进行抽稀或插值，得到等间距排布的控制点序列；

按照先后顺序，将控制点序列中的控制点依次加入KD树，得到边的三维KD树。

3.如权利要求1所述的一种基于高精度地图的矿山无人驾驶路网车辆排队方法，其特征在于，该方法还包括：基于所述排队次序，确定边上的车辆的排队位置，其中：

所述车辆的排队位置的计算方法为：

其中，size(S_ij)表示边e_ij的控制点序列S_ij中的控制点数量；

d_stop表示车辆在边终点处停车时距离边终点的停车距离；

d_queue表示车辆在边上排队时，车辆之间的间距；

N_queue表示车辆的排队次序；

d_interval表示控制点序列的间距；

S_ij(n)表示控制点序列S_ij中第n个控制点；

表示控制点S_ij(n)的位置。

4.一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，该装置包括：

地图拓扑模块，用于基于矿山的高精度地图中的作业区域和路口，构建拓扑地图G＝(V,E)；其中，节点集V表示矿山无人驾驶作业区域以及路口的集合；边集E表示连接两个节点的道路集合；

KD树构建模块，用于构建拓扑地图G中各个边的控制点序列和对应的KD树；所述控制点序列包括各个边对应的车道的中心线的等间距排布的控制点；且所述KD树的构建步骤包括：按照先后顺序，将控制点序列中的控制点依次加入KD树，得到边的三维KD树；

车辆位置判定模块，用于基于车辆定位坐标，确定车辆是否位于边上；且其步骤包括：遍历所有节点，判定是否满足节点包围盒包含车辆包围盒或车辆包围盒与节点包围盒相交；若不满足，则车辆在边上；

排队次序确定模块，用于基于KD树和控制点序列，确定位于边上的车辆的排队次序；且其步骤包括：

对于任一车辆：

遍历所有边的KD树，获取车辆与各个边上的最近控制点，得到最近控制点的索引以及车辆与最近控制点的距离；

从中选择车辆与最近控制点的距离最小的控制点所在边作为车辆所在边；

获取所有在车辆所在边上的其他车辆的索引，并按照索引大小排队，得到车辆所在边上的排队次序。

5.如权利要求4所述的一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，所述构建拓扑地图G中各个边的控制点序列和对应的KD树，包括：

对于节点i与j连接的边e_ij，i,j∈V：

获取边e_ij对应的车道的中心线的所有控制点；

按照所述控制点的先后顺序对控制点进行抽稀或插值，得到等间距排布的控制点序列；

按照先后顺序，将控制点序列中的控制点依次加入KD树，得到边的三维KD树。

6.如权利要求4所述的一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，该装置还包括：

排队位置确定模块，用于基于所述排队次序，确定边上的车辆的排队位置，其中：

所述车辆的排队位置的计算方法为：

其中，size(S_ij)表示边e_ij的控制点序列S_ij中的控制点数量；

d_stop表示车辆在边终点处停车时距离边终点的停车距离；

d_queue表示车辆在边上排队时，车辆之间的间距；

N_queue表示车辆的排队次序；

d_interval表示控制点序列的间距；

S_ij(n)表示控制点序列S_ij中第n个控制点；

表示控制点S_ij(n)的位置。

7.一种计算机可读存储介质，其特征在于，其存储用于矿山无人驾驶路网车辆排队的计算机程序，其中，所述计算机程序使得计算机执行如权利要求1-3任一项所述的基于高精度地图的矿山无人驾驶路网车辆排队方法。

8.一种电子设备，其特征在于，包括：

一个或多个处理器；

存储器；以及

一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行如权利要求1-3任一项所述的基于高精度地图的矿山无人驾驶路网车辆排队方法。

Images