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

权利要求书

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树，获取车辆与各个边上的最近控制点，得到最近控制点的索引以及车辆与最近控制点的距离；

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

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