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

技术特征：
1.一种基于高精度地图的矿山无人驾驶路网车辆排队方法，其特征在于，该方法包括：基于矿山的高精度地图中的作业区域和路口，构建拓扑地图g＝(v,e)；其中，节点集v表示矿山无人驾驶作业区域以及路口的集合；边集e表示连接两个节点的道路集合；构建拓扑地图g中各个边的控制点序列和对应的kd树；基于车辆定位坐标，确定车辆是否位于边上；基于kd树和控制点序列，确定位于边上的车辆的排队次序。2.如权利要求1所述的一种基于高精度地图的矿山无人驾驶路网车辆排队方法，其特征在于，所述构建拓扑地图g中各个边的控制点序列和对应的kd树，包括：对于节点i与j连接的边e
ij
，i,j∈v：获取边e
ij
对应的车道的中心线的所有控制点；按照所述控制点的先后顺序对控制点进行抽稀或插值，得到等间距排布的控制点序列；按照先后顺序，将控制点序列中的控制点依次加入kd树，得到边的三维kd树。3.如权利要求1所述的一种基于高精度地图的矿山无人驾驶路网车辆排队方法，其特征在于，所述基于车辆定位坐标，确定车辆是否位于边上，包括：基于高精度地图和车辆定位坐标，获取节点包围盒以及车辆包围盒；遍历所有节点，判定是否满足节点包围盒包含车辆包围盒或车辆包围盒与节点包围盒相交；若不满足，则车辆在边上。4.如权利要求1所述的一种基于高精度地图的矿山无人驾驶路网车辆排队方法，其特征在于，所述基于kd树和控制点序列，确定位于边上的车辆的排队次序，包括：对于任一车辆：遍历所有边的kd树，获取车辆与各个边上的最近控制点，得到最近控制点的索引以及车辆与最近控制点的距离；从中选择车辆与最近控制点的距离最小的控制点所在边作为车辆所在边；获取所有在车辆所在边上的其他车辆的索引，并按照索引大小排队，得到车辆所在边上的排队次序。5.如权利要求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)的位置。6.一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，该装置包括：地图拓扑模块，用于基于矿山的高精度地图中的作业区域和路口，构建拓扑地图g＝(v,e)；其中，节点集v表示矿山无人驾驶作业区域以及路口的集合；边集e表示连接两个节点的道路集合；kd树构建模块，用于构建拓扑地图g中各个边的控制点序列和对应的kd树；车辆位置判定模块，用于基于车辆定位坐标，确定车辆是否位于边上；排队次序确定模块，用于基于kd树和控制点序列，确定位于边上的车辆的排队次序。7.如权利要求6所述的一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，所述构建拓扑地图g中各个边的控制点序列和对应的kd树，包括：对于节点i与j连接的边e
ij
，i,j∈v：获取边e
ij
对应的车道的中心线的所有控制点；按照所述控制点的先后顺序对控制点进行抽稀或插值，得到等间距排布的控制点序列；按照先后顺序，将控制点序列中的控制点依次加入kd树，得到边的三维kd树。8.如权利要求6所述的一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，所述基于车辆定位坐标，确定车辆是否位于边上，包括：基于高精度地图和车辆定位坐标，获取节点包围盒以及车辆包围盒；遍历所有节点，判定是否满足节点包围盒包含车辆包围盒或车辆包围盒与节点包围盒相交；若不满足，则车辆在边上。9.如权利要求8所述的一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，所述基于kd树和控制点序列，确定位于边上的车辆的排队次序，包括：对于任一车辆：遍历所有边的kd树，获取车辆与各个边上的最近控制点，得到最近控制点的索引以及车辆与最近控制点的距离；从中选择车辆与最近控制点的距离最小的控制点所在边作为车辆所在边；获取所有在车辆所在边上的其他车辆的索引，并按照索引大小排队，得到车辆所在边上的排队次序。10.如权利要求8所述的一种基于高精度地图的矿山无人驾驶路网车辆排队装置，其特征在于，该装置还包括：排队位置确定模块，用于基于所述排队次序，确定边上的车辆的排队位置，其中：所述车辆的排队位置的计算方法为：其中，size(s
ij
)表示边e
ij
的控制点序列s
ij
中的控制点数量；d
stop
表示车辆在边终点处停车时距离边终点的停车距离；d
queue
表示车辆在边上排队时，车辆之间的间距；
n
queue
表示车辆的排队次序；d
interval
表示控制点序列的间距；s
ij
(n)表示控制点序列s
ij
中第n个控制点；表示控制点s
ij
(n)的位置。11.一种计算机可读存储介质，其特征在于，其存储用于矿山无人驾驶路网车辆排队的计算机程序，其中，所述计算机程序使得计算机执行如权利要求1
‑
5任一项所述的基于高精度地图的矿山无人驾驶路网车辆排队方法。12.一种电子设备，其特征在于，包括：一个或多个处理器；存储器；以及一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被配置成由所述一个或多个处理器执行，所述程序包括用于执行如权利要求1
‑
5任一项所述的基于高精度地图的矿山无人驾驶路网车辆排队方法。

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


技术研发人员：何玉东 张磊
受保护的技术使用者：北京易控智驾科技有限公司
技术研发日：2021.03.17
技术公布日：2021/10/18