CN118068837A - 一种装载区多车多铲协同装载调度及轨迹规划方法与装置 - Google Patents

Abstract

本发明公开了一种装载区多车多铲协同装载调度及轨迹规划方法与装置，装载区多车多铲协同装载调度及轨迹规划方法包括：步骤1，根据车辆的入场时间、出场时间、以及空/重载情况，确定车辆的优先级顺序；步骤2，根据优先级顺序，生成调度指令，并规划每一辆矿卡的行驶轨迹；步骤3，根据规划好的每一辆矿卡的行驶轨迹，预估各矿卡在每个时刻所处位置，进行多车动态冲突检测，按照优先级顺序使优先级低的矿卡实时避让优先级高的矿卡。本发明用于解决露天矿区装载作业面内存在多辆挖掘机与多辆无人驾驶矿卡的协同装载问题。

Description

一种装载区多车多铲协同装载调度及轨迹规划方法与装置

技术领域

本发明涉及矿山自动驾驶技术领域，特别是涉及一种装载区多车多铲协同装载调度及轨迹规划方法与装置。

背景技术

露天矿山的开采过程主要包括穿孔、***、采装、运输、排土等。其中，矿石装载作业是矿山生产的重要环节之一，需要车辆与车辆、车辆与挖掘机之间密切协同配合。随着露天矿山无人驾驶技术的发展，矿石装载作业也慢慢走向无人化道路。对于装载作业面较广的矿山，特别是沙石骨料矿山，通常会存在多辆挖掘机同时进行装载作业的情况。因此，装载区多车与多铲的协同装载调度及车辆行驶轨迹规划技术是实现智慧矿山的关键技术之一。

然而，目前的技术大多只针对一个装载作业面只存在一台挖掘机的作业场景，重点放在装载区单车与单铲(单挖掘机)的协同装载作业流程的智能化研究上，而少有针对装载区多车与多铲协同装载调度及轨迹规划方法的研究。

例如，现有技术一提出一种基于无人驾驶矿卡在装载区智能循铲的交互方法，通过分别在无人驾驶矿卡和铲车上安装协同设备，由铲车司机根据自身习惯确定矿卡停靠角度，同时监督矿卡的自动化作业流程，实现矿卡自动协同配合铲车作业。但是，该方法只考虑了无人驾驶矿卡与单铲车的协同装载作业，未考虑存在多辆无人驾驶车辆在装载区内的碰撞冲突问题，也未考虑装载区内存在多辆铲车时无人驾驶车辆的协同调度及轨迹规划方案。

还例如：现有技术二考虑一个挖掘机同时存在两个装载位，并实时更新装载位数据和装载位状态，当收到无人驾驶矿卡入场请求后，分配空闲装载位及对应路径和路权给对应车辆，车辆自动行驶至指定装载位。实时判断车辆是否满足出场条件，满足后根据碰撞检测结果控制卡车驶出装载作业区，完成装载协同作业调度及路径规划与控制。技术二相较于技术一进一步考虑了多装载位状态分配和装载路径规划问题，但所考虑的两个装载位状态均由一台挖掘机所控制，仍未考虑装载作业面内多辆分散的挖掘机同时进行装载作业场景，因此也未考虑装载区多车多铲协同调度及轨迹规划方案。

发明内容

本发明的目的在于提供一种高效、安全，并且尽可能避免停车等待的装载区多车多铲协同装载调度及轨迹规划方法与装置，以解决露天矿区装载作业面内存在多辆挖掘机与多辆无人驾驶矿卡的协同装载问题。

为实现上述目的，本发明提供一种装载区多车多铲协同装载调度及轨迹规划方法，其包括：

步骤1，根据车辆的入场时间、出场时间、以及空/重载情况，确定车辆的优先级顺序；

步骤2，根据优先级顺序，生成调度指令，并规划每一辆矿卡的行驶轨迹；

步骤3，根据规划好的每一辆矿卡的行驶轨迹，预估各矿卡在每个时刻所处位置，进行多车动态冲突检测，按照优先级顺序使优先级低的矿卡实时避让优先级高的矿卡。

进一步地，步骤1中的“车辆的优先级顺序”的确定方法如下：

对于驶入装载区的矿卡：将距装载入口点的沿轨迹距离小于设定距离阈值的车辆加入优先级队列，并且，越早驶入装载区的矿卡的优先级越高；

对于准备驶出装载区的矿卡：重载矿卡的优先级高于空载矿卡，并且，越早开始准备驶出装载区的矿卡的优先级越高；

对于已经驶出装载区的矿卡：从优先级队列中予以删除。

进一步地，步骤2具体包括：

在接收到即将到达装载入口点的车辆发送的入场请求后，判断是否有装载等待点处于非占用状态；若有，则调度矿卡前往指定的装载等待点处进行等待，并返回步骤1更新优先级队列，然后根据更新后的优先级顺序，规划矿卡从装载入口点到指定的装载等待点之间的轨迹，同时将指定的装载等待点置为占用状态；

在矿卡驶向指定的装载等待点的过程中，实时监控装载位是否处于空闲状态；若是，则生成矿卡前往指定的装载位的调度指令，并对矿卡从装载等待点到指定的装载位之间的轨迹进行规划，将矿卡驶离的装载等待点重新置为非占用状态；

当矿卡处于装载状态时，实时判断是否收到装载完成指令；若是，则调度矿卡驶出装载区，并返回步骤1更新优先级队列，再根据更新后的优先级顺序，对车辆从装载位到装载出口点之间的轨迹进行规划，在矿卡驶离装载出口点时，返回步骤1更新优先级队列。

进一步地，步骤2中行驶轨迹的规划方法具体包括：

路径规划：对同一装载挖掘机处的驶入装载区的矿卡的路径规划时，将对应准备驶出装载区的矿卡路径轮廓视作障碍物，障碍物构型空间C_obs描述为下式：

C_obs＝{p_i|i＝1,2,…,N}∪{o_j|j＝1,2_,…,M}∪{c_k,m|k＝1,2,…,Q,m＝1,2,…,P}

式中，k为路径点索引，Q为同一装载挖掘机处反向路径上路径点的总数，m为矿卡包络点索引，P表示矿卡包络点的总数，p_i表示装载区地图中第i个边界点坐标，N为装载区地图中边界点的总数，o_j为装载区内除矿卡外的其他第j个静态障碍物点坐标，M为静态障碍物点的总数，c_k,m为同一装载挖掘机处反向路径的第k个路径点位置处第m个矿卡包络点坐标。

进一步地，步骤3具体包括：

步骤31，实时计算任意两辆矿卡之间的距离d，再判断该距离是否小于设定阈值D，若是，则执行步骤32；

步骤32，判断两辆矿卡是否已经进行过冲突处理，若是，则跳转至步骤34，否则执行步骤33；

步骤33，在优先级队列中，查询两辆矿卡的优先级，优先级高的矿卡无需考虑停车，按照原来规划的既定轨迹行驶，优先级低的矿卡进行碰撞检测；碰撞检测的方法具体包括：

步骤331，从车辆当前位置开始，遍历优先级低的矿卡即将行驶的一段局部轨迹，并依次根据每个轨迹点时间信息，估计优先级高的矿卡在对应时刻的位置，进行动态碰撞检测；其中，优先级低的矿卡进行碰撞检测的局部轨迹的沿轨迹长度为设定阈值D；若该段局部轨迹与优先级高的矿卡存在碰撞风险，则转至步骤332，两辆矿卡按照最终行驶轨迹正常行驶，当前碰撞检测完毕；

步骤332，计算优先级低的矿卡的避让停车点；避让停车点的计算方式为：从车辆当前位置开始，不考虑时间信息，在优先级低的矿卡的最终行驶轨迹上找到第一个与优先级高的矿卡的最终行驶轨迹相干涉的轨迹点；然后，从第一个相干涉的轨迹点开始，然后，在优先级低车辆局部轨迹上计算得到的干涉轨迹点之前，找到与干涉轨迹点间隔安全距离d_safe的对应轨迹点，作为备选停车点；接着，计算该点的位置与优先级低的矿卡当前位置对应沿轨迹距离是否不小于车辆安全停车距离，若是，则确认备选的停车点为车辆的避让停车点，并跳转至步骤35，否则执行步骤34；

步骤34，让原先优先级高的矿卡停车避让原先优先级低的矿卡，跳转至步骤35；

步骤35，调度优先级低的矿卡在避让停车点前停车，待优先级高的矿卡驶过后，再调度优先级低的矿卡重新沿停车避让之前规划的行驶轨迹行驶。

进一步地，步骤31中的阈值D的确定方式为：

式中，v₁、v₂分别为两辆矿卡的当前车速；a₁、a₂分别为两辆矿卡的舒适减速度；如果以后轴中心为基准，d_f1、d_f2分别为两辆矿卡后轴中心到车头距离；如果是以车辆集合中心为基准，d_f1、d_f2分别为矿卡集合中心分别到两辆矿卡车头的距离；d_safe1、d_safe2分别为综合考虑车辆响应时延和停车间距下的两车安全距离。

本发明还提供一种装载区多车多铲协同装载调度及轨迹规划装置，其包括：

任务调度模块，根据车辆的入场时间、出场时间、以及空/重载情况，确定车辆的优先级顺序；

轨迹规划模块，根据优先级顺序，生成调度指令，并规划每一辆矿卡的行驶轨迹；

冲突检测模块，根据规划好的每一辆矿卡的行驶轨迹，预估各矿卡在每个时刻所处位置，进行多车动态冲突检测，按照优先级顺序使优先级低的矿卡实时避让优先级高的矿卡。

进一步地，轨迹规划模块具体包括：

调度指令生成单元，其用于在接收到即将到达装载入口点的车辆发送的入场请求后，判断是否有装载等待点处于非占用状态；若有，则调度矿卡前往指定的装载等待点处进行等待，并更新优先级队列，然后根据更新后的优先级顺序，规划矿卡从装载入口点到指定的装载等待点之间的轨迹，同时将指定的装载等待点置为占用状态；在矿卡驶向指定的装载等待点的过程中，实时监控装载位是否处于空闲状态；若是，则生成矿卡前往指定的装载位的调度指令，并对矿卡从装载等待点到指定的装载位之间的轨迹进行规划，将矿卡驶离的装载等待点重新置为非占用状态；当矿卡处于装载状态时，实时判断是否收到装载完成指令；若是，则调度矿卡驶出装载区，并更新优先级队列，再根据更新后的优先级顺序，对车辆从装载位到装载出口点之间的轨迹进行规划，在矿卡驶离装载出口点时，更新优先级队列。

进一步地，轨迹规划模块具体包括：

行驶轨迹规划单元，其用于路径规划：对同一装载挖掘机处的驶入装载区的矿卡的路径规划时，将对应准备驶出装载区的矿卡路径轮廓视作障碍物，障碍物构型空间C_obs描述为下式：

C_obs＝{p_i|i＝1,2,…,N}∪{o_j|j＝1,2,…,M}∪{c_k,m|k＝1,2,…,Q,m＝1,2,…,P}

式中，k为路径点索引，Q为同一装载挖掘机处反向路径上路径点的总数，m为矿卡包络点索引，P表示矿卡包络点的总数，p_i表示装载区地图中第i个边界点坐标，N为装载区地图中边界点的总数，o_j为装载区内除矿卡外的其他第j个静态障碍物点坐标，M为静态障碍物点的总数，c_k,m为同一装载挖掘机处反向路径的第k个路径点位置处第m个矿卡包络点坐标。

进一步地，冲突检测模块具体包括：

矿卡距离检测单元，其用于实时计算任意两辆矿卡之间的距离d，再判断该距离是否小于设定阈值D；

冲突处理检测单元，其用于在距离d小于设定阈值D的情形下，判断两辆矿卡是否已经进行过冲突处理，若是，则让原先优先级高的矿卡停车避让原先优先级低的矿卡；

碰撞检测单元，其用于在冲突处理检测单元判定两辆矿卡未进行过冲突处理的情形下，在优先级队列中，查询两辆矿卡的优先级，优先级高的矿卡无需考虑停车，按照原来规划的既定轨迹行驶，优先级低的矿卡进行碰撞检测；碰撞检测的方法具体包括：

步骤331，从车辆当前位置开始，遍历优先级低的矿卡即将行驶的一段局部轨迹，并依次根据每个轨迹点时间信息，估计优先级高的矿卡在对应时刻的位置，进行动态碰撞检测；其中，优先级低的矿卡进行碰撞检测的局部轨迹的沿轨迹长度为设定阈值D；若该段局部轨迹与优先级高的矿卡存在碰撞风险，则转至步骤332，两辆矿卡按照最终行驶轨迹正常行驶，当前碰撞检测完毕；

步骤332，计算优先级低的矿卡的避让停车点；避让停车点的计算方式为：从车辆当前位置开始，不考虑时间信息，在优先级低的矿卡的最终行驶轨迹上找到第一个与优先级高的矿卡的最终行驶轨迹相干涉的轨迹点；然后，从第一个相干涉的轨迹点开始，然后，在优先级低车辆局部轨迹上计算得到的干涉轨迹点之前，找到与干涉轨迹点间隔安全距离d_safe的对应轨迹点，作为备选停车点；接着，计算该点的位置与优先级低的矿卡当前位置对应沿轨迹距离是否不小于车辆安全停车距离，若是，则确认备选的停车点为车辆的避让停车点，并调度优先级低的矿卡在避让停车点前停车，待优先级高的矿卡驶过后，再调度优先级低的矿卡重新沿停车避让之前规划的行驶轨迹行驶，否则执行让原先优先级高的矿卡停车避让原先优先级低的矿卡。

由于本发明首先根据多辆挖掘机对应装载位状态，确定车辆前往合适的装载位；然后，根据车辆入场时间、车辆出场时间、车辆空/重载情况综合确定车辆行驶优先级；接着，基于优先级顺序规划矿卡前往装载等待点、前往装载位或驶出装载区的行驶轨迹；最后，在车辆行驶过程中实时进行动态冲突检测，并根据优先级顺序调整矿卡行驶轨迹。

本发明考虑了装载区内存在多辆挖掘机与多辆无人驾驶矿卡协同装载作业场景，通过矿卡、挖掘机、中央控制平台三者紧密配合，实现装载区多车多铲协同装载调度及轨迹规划，提高装载作业效率。

附图说明

图1为本发明实施例的装载区多车多铲调度及轨迹规划***的架构图。

图2为本发明实施例的装载区多车多铲协同装载调度及轨迹规划的示意图。

图3为本发明实施例的装载区多车多铲作业场景下车辆入场协同调度流程图。

图4为本发明实施例的装载区多车多铲作业场景下车辆出场协同调度流程图。

图5为本发明实施例的装载区多车多铲作业场景下车辆入场轨迹规划示意图。

图6为本发明实施例的装载区多车多铲作业场景下车辆出场轨迹规划示意图。

图7为本发明实施例的动态冲突检测示意图。

图8为本发明实施例的优先级高的矿卡驶过的判断方法示意图。

具体实施方式

在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。

在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。

如图1所示，本发明实施例提供的装载区多车多铲协同装载调度及轨迹规划装置用于实现多车多铲协同装载作业，包括中央控制平台、无人驾驶矿卡和挖掘机。其中，中央控制平台可读取地图坐标信息和静态障碍物信息，能实时获取所有无人驾驶矿卡的当前任务信息、轨迹信息和车辆状态信息，以及挖掘机的装载状态信息和装载位信息，完成装载区多车多铲协同装载任务调度及轨迹规划。

具体地，中央控制平台包括任务调度模块、轨迹规划模块、冲突检测模块和第一通信模块。其中，任务调度模块用于先车辆的优先级顺序，并生成相应的调度指令，最后通过第一通信模块下发给无人驾驶矿卡。其中，调度指令包括入场、装载和出场指令。轨迹规划模块用于根据车辆优先级顺序和调度指令，规划每一辆矿卡的行驶轨迹，行驶轨迹再通过第一通信模块下发给相应的矿卡。冲突检测模块用于在车辆行驶过程中实时进行动态冲突检测，并将检测结果转换为调度指令，通过第一通信模块下发至相应的矿卡。第一通信模块用于与矿卡和挖掘机进行实时信息交互。

矿卡包括第二通信模块和无人驾驶***。其中：第二通信模块用于与中央控制平台的第一通信模块进行实时信息交互。无人驾驶***用于执行中央控制平台下发的调度指令和行驶轨迹，并通过第二通信模块上报车辆行驶状态及任务执行状态。

挖掘机包括第三通信模块和协同作业***。其中：第三通信模块用于与中央控制平台的第一通信模块进行实时信息交互。协同作业***用于装载位状态管理与人机交互，并通过第三通信模块上报挖掘机作业状态和装载位状态。

露天矿山装载区是指露天矿山中的一个特定区域，用于装载和运输采矿所得的矿石或其他矿物。装载区通常配备有专门的装载设备，如装载机或挖掘机，用于将矿石从堆场或开采坑口装入运输设备中。为了加快物料装载及运输效率，通常会在一个装载区内调配多辆矿用运输车辆，并部署多辆装载机或挖掘机。

如图2所示，装载区内有进行装载作业的挖掘机1、挖掘机2和挖掘机3，图中出现的车辆均指矿卡，分别为即将驶入装载区的矿卡a，在等待装载完成驶离装载区的车辆b、车辆c和车辆d。矿卡a在进入装载区前需要被调度前往合适的装载等待点，而矿卡b、c、d需要等待装载完成后被调度驶出装载区。四辆矿卡均需要进行相应的轨迹规划，在轨迹规划时需要考虑避免与其他车辆的碰撞冲突。所规划的轨迹信息包含：大地坐标系下的x、y值，航向角θ，曲率k，速度v，时刻t和距起点距离s等信息。

本发明实施例的装载区多车多铲协同装载调度及轨迹规划方法包括：

步骤1，根据车辆的入场时间、出场时间、以及空/重载情况，确定车辆的优先级顺序。

对于所有在装载区内作业的矿卡，均需要确定其优先级。优先级的确定规则如下：

对于驶入装载区的矿卡：将距装载入口点的沿轨迹距离小于设定距离阈值的车辆加入优先级队列，并且，越早驶入装载区的矿卡的优先级越高。其中的设定距离阈值的具体数值确保车辆能够在图2所示的装载入口点平稳停车。

当车辆完成装载准备驶出装载区时，车辆优先级升级，即所有重载矿卡的优先级均高于空载矿卡优先级，对于越早开始准备装载区的车辆，其优先级越高。

对于已经驶出装载区的矿卡：从优先级队列中予以删除。

基于上述优先级确定规则，实时更新装载区内车辆优先级队列。

步骤2，根据优先级顺序，生成调度指令，并规划每一辆矿卡的行驶轨迹。

生成调度指令，以对单车进行调度，具体包括：a.根据装载区内装载等待点占用和装载位情况，分别调度矿卡入场前往装载点、驶入装载位或在装载入口点等待；b.根据车辆装载完成情况，调度矿卡驶出前往装载点。

在一个实施例中，如图3所示，步骤2具体包括：

当矿卡即将到达装载入口点时，向中央调度平台发送入场请求指令。中央调度平台在接收到即将到达装载入口点的车辆发送的入场请求后，判断是否有装载等待点处于非占用状态；若无，则调度矿卡在装载入口点处等待，直到存在非占用状态的装载等待点；若有，则调度矿卡前往指定的装载等待点处进行等待，并返回步骤1更新优先级队列，然后根据更新后的优先级顺序，规划矿卡从装载入口点到指定的装载等待点之间的轨迹，同时将指定的装载等待点置为占用状态。

在矿卡驶向指定的装载等待点的过程中，矿卡行驶过程中，中央调度平台实时监控装载位是否处于空闲状态；若是，则生成矿卡前往指定的装载位的调度指令，并对矿卡从装载等待点到指定的装载位之间的轨迹进行规划，将矿卡驶离的装载等待点重新置为非占用状态；若否，矿卡在装载等待点处等待，直到存在非占用状态的装载位。

上述装载位的位置和状态均由挖掘机协同作业***确定，并通过图1所示的第三通信模块实时上报至中央调度平台。

在一个实施例中，装载等待点与单挖掘机唯一绑定，挖掘机与矿卡进行协同装载，挖掘机上安装有协同终端，用于装载位的确定以及装载驶入驶出信号的发出。当挖掘机处于未就绪状态时，装载等待点则默认为占用状态。装载等待点由中央调度平台操作人员根据装载作业面范围和挖掘机位置手动确定。

在一个实施例中，为了保证车辆能够从装载等待点驶入装载位的过程中具有足够空间调整矿卡位置，装载等待点的位置与装载位的位置距离应大于2.5倍车辆的最小转弯半径，且小于4倍车辆的最小转弯半径。

在一个实施例中，结合图3，步骤2具体还包括：当矿卡处于装载状态时，实时判断是否收到装载完成指令。若否，则继续停车等待装载完成；若是，则调度矿卡驶出装载区，此时并返回步骤1更新优先级队列，再根据更新后的优先级顺序，然后根据优先级顺序，对车辆从装载位到装载出口点之间的轨迹进行规划。装载完成指令由挖掘机协同作业***通过第三通信模块上报至中央调度平台，然后再由第一通信模块转发至车辆。车辆沿规划轨迹驶出装载区，同时在矿卡驶离装载出口点时，返回步骤1更新优先级队列。

当确定车辆的优先级和需要到达的终点状态时，规划矿卡从指定起点到指定终点的行驶轨迹。对于调度任务的不同，轨迹规划的起点和终点又算不同。对于车辆入场调度任务，规划起点为装载入口点，规划终点为装载等待点；对于车辆前往装载调度任务，规划起点为装载等待点，规划终点为装载点；对于车辆出场调度任务，规划起点为装载点，规划终点为装载点。

步骤2中的“规划每一辆矿卡的行驶轨迹”采用路径速度解耦的方法，具体如下：

(一)对于路径规划：对于同一个装载挖掘机处的入场和准备驶出装载区的矿卡，其规划轨迹可能会存在大范围相反方向的重叠，从而导致“死锁”现象发生。为了解决这一问题，本发明实施例提出了车辆可通行区域划分方法，该方法具体包括：

对于同一装载挖掘机处的驶入装载区的矿卡的路径规划，将对应准备驶出装载区的矿卡路径轮廓视作障碍物，即保证前往同一挖掘机处的入场和准备驶出装载区的矿卡行驶路径不存在空间干涉。以图5所示工况为例，驶入装载区的矿卡a在规划到达装载等待点1的路径时，将准备驶出装载区的矿卡d的行驶轨迹轮廓视为不可跨越区域，即矿卡a在进行路径搜索时将矿卡d的行驶轨迹轮廓视为障碍物。同理，对于同一装载挖掘机处的准备驶出装载区的矿卡的路径规划，将对应的驶入装载区的矿卡路径轮廓视作障碍物。如图6所示，准备驶出装载区的矿卡c在规划到达装载出口点的路径时，将驶入装载区的矿卡a的行驶轨迹轮廓视为不可跨越区域，即矿卡c在进行路径搜索时将矿卡a的行驶轨迹轮廓视为障碍物。也就是说，对同一装载挖掘机处的驶入装载区的矿卡的路径规划时，将对应准备驶出装载区的矿卡路径轮廓视作障碍物，障碍物构型空间C_obs描述为下式：

C_obs＝{p_i|i＝1,2,…,N}∪{o_j|j＝1,2,…,M}∪{c_k,m|k＝1,2,…,Q,m＝1,2,…,P}

式中，k为路径点索引，Q为同一装载挖掘机处反向路径上路径点的总数，m为矿卡包络点索引，P表示矿卡包络点的总数，p_i表示装载区地图中第i个边界点坐标，装载区地图可通过手持设备采集或车载激光雷达采集，N为装载区地图中边界点的总数，o_j为装载区内除矿卡外的其他第j个静态障碍物点坐标，静态障碍物是指凹坑、石头等障碍物，不包括是视作为的障碍物，M为静态障碍物点的总数，c_k,m为同一装载挖掘机处反向路径的第k个路径点位置处第m个矿卡包络点坐标。

(二)速度规划：规划得到车辆行驶路径后，根据最新的优先级顺序建立车辆的ST图，将比自车优先级高的矿卡视作障碍物车辆，通过轨迹点碰撞检测，得到优先级高的矿卡占据自车的ST图区间范围，采用动态规划算法在所建立的ST图中生成一条粗糙的速度曲线，然后采用二次规划算法对速度曲线进行平滑，以进行全局速度规划，得到矿卡最终行驶轨迹，并进行存储。

在一个实施例中，路径搜索方法均采用传统的Hybrid A*算法。对于矿卡入场和出场的路径规划，由于不涉及到车辆倒车，Hybrid A*算法只进行前向拓展；对于矿卡驶入装载位的路径规划，Hybrid A*算法需要进行前向和后向拓展。

步骤3，当装载区内矿卡沿规划轨迹行驶时，虽然在轨迹规划时初步考虑了多车行驶轨迹相互避让问题，但由于矿卡执行对应规划轨迹时存在误差，各车之间仍可能存在时空干涉。针对潜在车辆冲突问题，根据规划好的每一辆矿卡的行驶轨迹，预估各矿卡在每个时刻所处位置，进行多车动态冲突检测，按照优先级顺序使优先级低的矿卡实时避让优先级高的矿卡，从而保证车辆运输效率与行车安全。

在一个实施例中，如图7所示，步骤3具体包括：

步骤31，实时计算任意两辆矿卡之间的距离d，然后判断距离d是否小于设定阈值D，若是，执行步骤32，否则认为两车不存在碰撞风险，两车正常继续沿轨迹行驶。其中，距离d可以理解为矿卡后轴中心位置之间的欧式距离。

在一个实施例中，设定阈值D的确定方式为：

式中，v₁、v₂分别为两辆矿卡当前车速；a₁、a₂分别为两辆矿卡的舒适减速度；如果以后轴中心为基准，d_f1、d_f2分别为两辆矿卡后轴中心到车头距离；如果是以车辆集合中心为基准，d_f1、d_f2分别为矿卡集合中心分别到两辆矿卡车头的距离；d_safe1、d_safe2分别为为综合考虑车辆响应时延和停车间距下的两车安全距离。

需要说明的是，设定阈值D选取的意义在于至少保证其中一辆矿卡能够在两车干涉位置前安全停车，这样既减少了动态冲突检测的计算量，同时能够有效保证车辆行车安全。阈值D的设定还可以采用梯形减速方法或其他更符合车辆减速特性的方法计算获得。

步骤32，判断两辆矿卡是否已经进行过冲突处理，即一辆车正在避让另一辆车，若是，则跳转至步骤36，若否则执行步骤33。

步骤33，在优先级队列中，查询两辆矿卡的优先级，优先级高的矿卡无需考虑停车，优先级低的矿卡进行碰撞检测。如图2所示，车辆a的优先级高于车辆b，则车辆a无需考虑车辆b，而车辆b需要针对车辆a做碰撞检测。碰撞检测的的方法具体包括：

步骤331，从车辆当前位置开始，遍历优先级低的矿卡即将行驶的一段局部轨迹，并依次根据每个轨迹点时间信息，估计优先级高的矿卡在对应时刻的位置，进行动态碰撞检测。其中，优先级低的矿卡进行碰撞检测的局部轨迹的沿轨迹长度为设定阈值D，若该段局部轨迹与优先级高的矿卡存在碰撞风险，则转至步骤332，否则认定车辆通过动态冲突检测，两辆矿卡按照最终行驶轨迹正常行驶，当前碰撞检测完毕。值得注意的时，为了保正碰撞检测结果与车辆实际行驶结果一致，在进行碰撞检测时，应保证两车都考虑了足够的安全距离。

步骤332，计算优先级低的矿卡的避让停车点；避让停车点的计算方式为：从车辆当前位置开始，不考虑时间信息，在优先级低的矿卡的最终行驶轨迹上找到第一个与优先级高的矿卡的最终行驶轨迹相干涉的轨迹点；然后，从第一个相干涉的轨迹点开始，然后，在优先级低车辆局部轨迹上计算得到的干涉轨迹点之前，找到与干涉轨迹点间隔安全距离d_safe的对应轨迹点，作为备选停车点。如图7所示，通过碰撞检测得到车辆b行驶轨迹上的干涉轨迹点，然后在考虑安全距离d_safe情况下找到备选停车点，再计算车辆当前位置到备选停车点的距离是否大于车辆安全停车距离，若是则该点作为车辆b的停车避让点。接着，计算该点的位置与优先级低的矿卡当前位置对应沿轨迹距离是否不小于车辆安全停车距离，若是，则确认备选的停车点为车辆的避让停车点，并跳转至步骤35，否则执行步骤34。其中，车辆安全停车距离是指车辆从当前速度开始，以舒适减速度减速至车辆静止所需的距离。

步骤34，让原先优先级高的矿卡停车避让原先优先级低的矿卡，跳转至步骤35。由于先前确定的优先级顺序无法保证优先级低的矿卡安全停车避让优先级高的矿卡，因此此时应调换两车优先级顺序，即让原先优先级高的矿卡停车避让原先优先级低的矿卡。以根据图7举例说明，开始时车辆a的优先级高于车辆b，若通过步骤332后发现车辆b无法避让车辆a，此时让车辆a来避让车辆b。

步骤35，调度优先级低的矿卡在避让停车点前停车，待优先级高的矿卡驶过后，再调度优先级低的矿卡重新沿停车避让之前规划的行驶轨迹行驶。

其中，判断优先级高的矿卡驶过的方法为：实时对优先级低的矿卡的局部轨迹做碰撞检测，若检测到与优先级高的矿卡的轮廓产生干涉后干涉再消失，表明优先级高的矿卡已经驶过干涉点，此时车辆开始恢复行驶。如图8所示，当优先级低的矿卡b在避让停车点处停下时，优先级高的矿卡a从车辆b轨迹左侧行驶至右侧。车辆a从左侧开始与车辆b的行驶轨迹产生干涉，随后干涉一直存在，当车辆a行驶至车辆b轨迹右侧后干涉消失，此时车辆b可恢复行驶。

当一辆车产生避让停车动作时，可能使得在装载区内已经在行驶的且比其优先级更低的车辆增加避让的概率。但值得注意的是，装载区通常区域较大，同时，在进行全局轨迹规划时考虑了车辆冲突避让，因此这种两车发生干涉的几率通常较小，通过实时判断任意两车是否需要进行动态冲突检测，可保证车辆行驶安全，最坏的情况为装载区内已经在行驶的且比其优先级更低的车辆均需要停车避让。

目前针对露天矿山装载作业区多车多铲场景下的车辆装载调度及轨迹规划方法研究较少，本发明设计了一套完整的装载区多车多铲协同装载调度及轨迹规划方法体系。所提出的装载区多车多铲场景下车辆优先级分配原则，能够有效保证车辆行驶安全，并节省能源消耗；所提出的车辆可通行区域划分方法，在轨迹规划时可有效防止“死锁”现象发生；所提出的动态冲突检测与处理方法，可有效避免多车之间的潜在冲突，保证车辆行驶效率的同时，进一步保障车辆行驶安全完备性。

本发明实施例还提供一种装载区多车多铲协同装载调度及轨迹规划装置，其包括任务调度模块、轨迹规划模块和冲突检测模块。

任务调度模块用于根据车辆的入场时间、出场时间、以及空/重载情况，确定车辆的优先级顺序。

轨迹规划模块用于根据优先级顺序，生成调度指令，并规划每一辆矿卡的行驶轨迹。

冲突检测模块用于根据规划好的每一辆矿卡的行驶轨迹，预估各矿卡在每个时刻所处位置，进行多车动态冲突检测，按照优先级顺序使优先级低的矿卡实时避让优先级高的矿卡。

在一个实施例中，轨迹规划模块具体包括调度指令生成单元，度指令生成单元用于在接收到即将到达装载入口点的车辆发送的入场请求后，判断是否有装载等待点处于非占用状态；若有，则调度矿卡前往指定的装载等待点处进行等待，并更新优先级队列，然后根据更新后的优先级顺序，规划矿卡从装载入口点到指定的装载等待点之间的轨迹，同时将指定的装载等待点置为占用状态；在矿卡驶向指定的装载等待点的过程中，实时监控装载位是否处于空闲状态；若是，则生成矿卡前往指定的装载位的调度指令，并对矿卡从装载等待点到指定的装载位之间的轨迹进行规划，将矿卡驶离的装载等待点重新置为非占用状态；当矿卡处于装载状态时，实时判断是否收到装载完成指令；若是，则调度矿卡驶出装载区，并更新优先级队列，再根据更新后的优先级顺序，对车辆从装载位到装载出口点之间的轨迹进行规划，在矿卡驶离装载出口点时，更新优先级队列。

在一个实施例中，轨迹规划模块具体包括行驶轨迹规划单元，行驶轨迹规划单元用于路径规划：对同一装载挖掘机处的驶入装载区的矿卡的路径规划时，将对应准备驶出装载区的矿卡路径轮廓视作障碍物，障碍物构型空间C_obs描述为下式：

C_obs＝{p_i|i＝1,2,…,N}∪{o_j|j＝1,2,…,M}∪{c_k,m|k＝1,2,…,Q,m＝1,2,…,P}

式中，k为路径点索引，Q为同一装载挖掘机处反向路径上路径点的总数，m为矿卡包络点索引，P表示矿卡包络点的总数，p_i表示装载区地图中第i个边界点坐标，N为装载区地图中边界点的总数，o_j为装载区内除矿卡外的其他第j个静态障碍物点坐标，M为静态障碍物点的总数，c_k,m为同一装载挖掘机处反向路径的第k个路径点位置处第m个矿卡包络点坐标。

在一个实施例中，冲突检测模块具体包括矿卡距离检测单元、冲突处理检测单元和碰撞检测单元。其中：

矿卡距离检测单元用于实时计算任意两辆矿卡之间的距离d，再判断该距离是否小于设定阈值D。

冲突处理检测单元用于在距离d小于设定阈值D的情形下，判断两辆矿卡是否已经进行过冲突处理，若是，则让原先优先级高的矿卡停车避让原先优先级低的矿卡。

碰撞检测单元用于在冲突处理检测单元判定两辆矿卡未进行过冲突处理的情形下，在优先级队列中，查询两辆矿卡的优先级，优先级高的矿卡无需考虑停车，按照原来规划的既定轨迹行驶，优先级低的矿卡进行碰撞检测；碰撞检测的方法具体包括：

步骤331，从车辆当前位置开始，遍历优先级低的矿卡即将行驶的一段局部轨迹，并依次根据每个轨迹点时间信息，估计优先级高的矿卡在对应时刻的位置，进行动态碰撞检测；其中，优先级低的矿卡进行碰撞检测的局部轨迹的沿轨迹长度为设定阈值D；若该段局部轨迹与优先级高的矿卡存在碰撞风险，则转至步骤332，两辆矿卡按照最终行驶轨迹正常行驶，当前碰撞检测完毕；

步骤332，计算优先级低的矿卡的避让停车点；避让停车点的计算方式为：从车辆当前位置开始，不考虑时间信息，在优先级低的矿卡的最终行驶轨迹上找到第一个与优先级高的矿卡的最终行驶轨迹相干涉的轨迹点；然后，从第一个相干涉的轨迹点开始，然后，在优先级低车辆局部轨迹上计算得到的干涉轨迹点之前，找到与干涉轨迹点间隔安全距离d_safe的对应轨迹点，作为备选停车点；接着，计算该点的位置与优先级低的矿卡当前位置对应沿轨迹距离是否不小于车辆安全停车距离，若是，则确认备选的停车点为车辆的避让停车点，并调度优先级低的矿卡在避让停车点前停车，待优先级高的矿卡驶过后，再调度优先级低的矿卡重新沿停车避让之前规划的行驶轨迹行驶，否则执行让原先优先级高的矿卡停车避让原先优先级低的矿卡。

本发明在原理上也是可以适用于卸载的，把挖掘机要素更改为推土机即可，但有部分细节有所不同。

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

Claims (10)

1.一种装载区多车多铲协同装载调度及轨迹规划方法，其特征在于，包括：

步骤1，根据车辆的入场时间、出场时间、以及空/重载情况，确定车辆的优先级顺序；

步骤2，根据优先级顺序，生成调度指令，并规划每一辆矿卡的行驶轨迹；

步骤3，根据规划好的每一辆矿卡的行驶轨迹，预估各矿卡在每个时刻所处位置，进行多车动态冲突检测，按照优先级顺序使优先级低的矿卡实时避让优先级高的矿卡。

2.如权利要求1装载区多车多铲协同装载调度及轨迹规划方法，其特征在于，步骤1中的“车辆的优先级顺序”的确定方法如下：

对于驶入装载区的矿卡：将距装载入口点的沿轨迹距离小于设定距离阈值的车辆加入优先级队列，并且，越早驶入装载区的矿卡的优先级越高；

对于准备驶出装载区的矿卡：重载矿卡的优先级高于空载矿卡，并且，越早开始准备驶出装载区的矿卡的优先级越高；

对于已经驶出装载区的矿卡：从优先级队列中予以删除。

3.如权利要求1装载区多车多铲协同装载调度及轨迹规划方法，其特征在于，步骤2具体包括：

在接收到即将到达装载入口点的车辆发送的入场请求后，判断是否有装载等待点处于非占用状态；若有，则调度矿卡前往指定的装载等待点处进行等待，并返回步骤1更新优先级队列，然后根据更新后的优先级顺序，规划矿卡从装载入口点到指定的装载等待点之间的轨迹，同时将指定的装载等待点置为占用状态；

在矿卡驶向指定的装载等待点的过程中，实时监控装载位是否处于空闲状态；若是，则生成矿卡前往指定的装载位的调度指令，并对矿卡从装载等待点到指定的装载位之间的轨迹进行规划，将矿卡驶离的装载等待点重新置为非占用状态；

当矿卡处于装载状态时，实时判断是否收到装载完成指令；若是，则调度矿卡驶出装载区，并返回步骤1更新优先级队列，再根据更新后的优先级顺序，对车辆从装载位到装载出口点之间的轨迹进行规划，在矿卡驶离装载出口点时，返回步骤1更新优先级队列。

4.如权利要求3装载区多车多铲协同装载调度及轨迹规划方法，其特征在于，步骤2中行驶轨迹的规划方法具体包括：

路径规划：对同一装载挖掘机处的驶入装载区的矿卡的路径规划时，将对应准备驶出装载区的矿卡路径轮廓视作障碍物，障碍物构型空间C_obs描述为下式：

C_obs＝{p_i|i＝1,2,…,N}∪{o_j|j＝1,2,…,M}∪{c_k,m|k＝1,2,…,Q,m＝1,2,…,P}

式中，k为路径点索引，Q为同一装载挖掘机处反向路径上路径点的总数，m为矿卡包络点索引，P表示矿卡包络点的总数，p_i表示装载区地图中第i个边界点坐标，N为装载区地图中边界点的总数，o_j为装载区内除矿卡外的其他第j个静态障碍物点坐标，M为静态障碍物点的总数，c_k,m为同一装载挖掘机处反向路径的第k个路径点位置处第m个矿卡包络点坐标。

5.如权利要求4装载区多车多铲协同装载调度及轨迹规划方法，其特征在于，步骤3具体包括：

步骤31，实时计算任意两辆矿卡之间的距离d，再判断该距离是否小于设定阈值D，若是，则执行步骤32；

步骤32，判断两辆矿卡是否已经进行过冲突处理，若是，则跳转至步骤34，否则执行步骤33；

步骤33，在优先级队列中，查询两辆矿卡的优先级，优先级高的矿卡无需考虑停车，按照原来规划的既定轨迹行驶，优先级低的矿卡进行碰撞检测；碰撞检测的方法具体包括：

步骤331，从车辆当前位置开始，遍历优先级低的矿卡即将行驶的一段局部轨迹，并依次根据每个轨迹点时间信息，估计优先级高的矿卡在对应时刻的位置，进行动态碰撞检测；其中，优先级低的矿卡进行碰撞检测的局部轨迹的沿轨迹长度为设定阈值D；若该段局部轨迹与优先级高的矿卡存在碰撞风险，则转至步骤332，两辆矿卡按照最终行驶轨迹正常行驶，当前碰撞检测完毕；

步骤332，计算优先级低的矿卡的避让停车点；避让停车点的计算方式为：从车辆当前位置开始，不考虑时间信息，在优先级低的矿卡的最终行驶轨迹上找到第一个与优先级高的矿卡的最终行驶轨迹相干涉的轨迹点；然后，从第一个相干涉的轨迹点开始，然后，在优先级低车辆局部轨迹上计算得到的干涉轨迹点之前，找到与干涉轨迹点间隔安全距离d_safe的对应轨迹点，作为备选停车点；接着，计算该点的位置与优先级低的矿卡当前位置对应沿轨迹距离是否不小于车辆安全停车距离，若是，则确认备选的停车点为车辆的避让停车点，并跳转至步骤35，否则执行步骤34；

步骤34，让原先优先级高的矿卡停车避让原先优先级低的矿卡，跳转至步骤35；

步骤35，调度优先级低的矿卡在避让停车点前停车，待优先级高的矿卡驶过后，再调度优先级低的矿卡重新沿停车避让之前规划的行驶轨迹行驶。

6.如权利要求5装载区多车多铲协同装载调度及轨迹规划方法，其特征在于，步骤31中的阈值D的确定方式为：

式中，v₁、v₂分别为两辆矿卡的当前车速；a₁、a₂分别为两辆矿卡的舒适减速度；如果以后轴中心为基准，d_f1、d_f2分别为两辆矿卡后轴中心到车头距离；如果是以车辆集合中心为基准，d_f1、d_f2分别为矿卡集合中心分别到两辆矿卡车头的距离；d_safe1、d_safe2分别为综合考虑车辆响应时延和停车间距下的两车安全距离。

7.一种装载区多车多铲协同装载调度及轨迹规划装置，其特征在于，包括：

任务调度模块，根据车辆的入场时间、出场时间、以及空/重载情况，确定车辆的优先级顺序；

轨迹规划模块，根据优先级顺序，生成调度指令，并规划每一辆矿卡的行驶轨迹；

冲突检测模块，根据规划好的每一辆矿卡的行驶轨迹，预估各矿卡在每个时刻所处位置，进行多车动态冲突检测，按照优先级顺序使优先级低的矿卡实时避让优先级高的矿卡。

8.如权利要7装载区多车多铲协同装载调度及轨迹规划装置，其特征在于，轨迹规划模块具体包括：

调度指令生成单元，其用于在接收到即将到达装载入口点的车辆发送的入场请求后，判断是否有装载等待点处于非占用状态；若有，则调度矿卡前往指定的装载等待点处进行等待，并更新优先级队列，然后根据更新后的优先级顺序，规划矿卡从装载入口点到指定的装载等待点之间的轨迹，同时将指定的装载等待点置为占用状态；在矿卡驶向指定的装载等待点的过程中，实时监控装载位是否处于空闲状态；若是，则生成矿卡前往指定的装载位的调度指令，并对矿卡从装载等待点到指定的装载位之间的轨迹进行规划，将矿卡驶离的装载等待点重新置为非占用状态；当矿卡处于装载状态时，实时判断是否收到装载完成指令；若是，则调度矿卡驶出装载区，并更新优先级队列，再根据更新后的优先级顺序，对车辆从装载位到装载出口点之间的轨迹进行规划，在矿卡驶离装载出口点时，更新优先级队列。

9.如权利要求8装载区多车多铲协同装载调度及轨迹规划装置，其特征在于，轨迹规划模块具体包括：

行驶轨迹规划单元，其用于路径规划：对同一装载挖掘机处的驶入装载区的矿卡的路径规划时，将对应准备驶出装载区的矿卡路径轮廓视作障碍物，障碍物构型空间C_obs描述为下式：

C_obs＝{p_i|i＝1,2,…,N}∪{o_j|j＝1,2,…,M}∪{c_k,m|k＝1,2,…,Q,m＝1,2,…,P}

式中，k为路径点索引，Q为同一装载挖掘机处反向路径上路径点的总数，m为矿卡包络点索引，P表示矿卡包络点的总数，p_i表示装载区地图中第i个边界点坐标，N为装载区地图中边界点的总数，o_j为装载区内除矿卡外的其他第j个静态障碍物点坐标，M为静态障碍物点的总数，c_k,m为同一装载挖掘机处反向路径的第k个路径点位置处第m个矿卡包络点坐标。

10.如权利要求4装载区多车多铲协同装载调度及轨迹规划装置，其特征在于，冲突检测模块具体包括：

矿卡距离检测单元，其用于实时计算任意两辆矿卡之间的距离d，再判断该距离是否小于设定阈值D；

冲突处理检测单元，其用于在距离d小于设定阈值D的情形下，判断两辆矿卡是否已经进行过冲突处理，若是，则让原先优先级高的矿卡停车避让原先优先级低的矿卡；

碰撞检测单元，其用于在冲突处理检测单元判定两辆矿卡未进行过冲突处理的情形下，在优先级队列中，查询两辆矿卡的优先级，优先级高的矿卡无需考虑停车，按照原来规划的既定轨迹行驶，优先级低的矿卡进行碰撞检测；碰撞检测的方法具体包括：

步骤331，从车辆当前位置开始，遍历优先级低的矿卡即将行驶的一段局部轨迹，并依次根据每个轨迹点时间信息，估计优先级高的矿卡在对应时刻的位置，进行动态碰撞检测；其中，优先级低的矿卡进行碰撞检测的局部轨迹的沿轨迹长度为设定阈值D；若该段局部轨迹与优先级高的矿卡存在碰撞风险，则转至步骤332，两辆矿卡按照最终行驶轨迹正常行驶，当前碰撞检测完毕；

步骤332，计算优先级低的矿卡的避让停车点；避让停车点的计算方式为：从车辆当前位置开始，不考虑时间信息，在优先级低的矿卡的最终行驶轨迹上找到第一个与优先级高的矿卡的最终行驶轨迹相干涉的轨迹点；然后，从第一个相干涉的轨迹点开始，然后，在优先级低车辆局部轨迹上计算得到的干涉轨迹点之前，找到与干涉轨迹点间隔安全距离d_safe的对应轨迹点，作为备选停车点；接着，计算该点的位置与优先级低的矿卡当前位置对应沿轨迹距离是否不小于车辆安全停车距离，若是，则确认备选的停车点为车辆的避让停车点，并调度优先级低的矿卡在避让停车点前停车，待优先级高的矿卡驶过后，再调度优先级低的矿卡重新沿停车避让之前规划的行驶轨迹行驶，否则执行让原先优先级高的矿卡停车避让原先优先级低的矿卡。