CN114326744A - 一种基于全局地图更新的矿山卡车路径规划方法 - Google Patents

Abstract

本发明公开了一种基于全局地图更新的矿山卡车路径规划方法，包括基于历史数据的全局高精度地图和混合A‑Star算法进行第一层全局路径规划，并提取关键点；利用车辆激光雷达实时获取感知数据，基于DWA进行第二层局部路径规划；判断是否存在场景时变导致规划失效；若无影响，则车辆成功规划出路径并下发控制指令，驱动车辆到达目标点；若规划失效，则车辆利用当前的感知信息，将当前局部地图信息更新到全局地图对应的部分，完成全局地图局部更新并再重启第一层全局路径规划。本发明通过利用局部感知信息对全局地图进行更新的策略，结合双层规划路径算法，解决了矿卡路径规划因场景时变性导致的规划失效问题，满足了矿卡路径规划对地图实时性的需求。

Description

一种基于全局地图更新的矿山卡车路径规划方法

技术领域

本发明涉及矿山卡车路径规划技术领域，特别涉及一种基于全局地图更新的矿山卡车路径规划方法。

背景技术

路径规划技术作为矿山卡车无人驾驶的关键技术之一，是指无人驾驶矿卡在露天矿山的环境中，遵循车辆的非完整性约束，规划出一条从起始位置状态到目标位置状态无碰撞的最优路径或次优路径。首先，露天矿山生产的特点在于铲装机遍布在作业平台的各个角落，并且山体形状不规则，导致挖机和矿车之间经常会存在山体遮挡、石堆占道等情况，给自动驾驶矿山卡车的路径规划带来严峻的挑战。其次，挖机作业效率较高，在短时间内可轻易改变山体形状，这会导致同一挖机作业地点的相邻两次作业场景发生较大的变化；或者由于碎石滚落，使得某一区域发生堵塞，不再具备通行条件，这就给路径规划带来了极大的挑战。在这种具有高时变性的复杂场景下，无人矿卡往往很难规划出一条有效的路径达到挖机铲装点进行装料作业，从而导致铲装作业失败。

现有技术的不足之处在于：常用的路径规划会采用双层规划方案，但现有的双层规划算法并未考虑到因全局路径规划的不合理而导致局部规划DWA失效的问题。又比如，现有的路径规划技术依赖于高精度地图，针对地图场景不发生变动的情况进行路径规划，可以获得不错的规划结果，指导车辆行驶至目的地。但当规划完成后车辆进入到行驶阶段，若此时原路径中出现了新的障碍物，并且仅依靠局部路径规划不能躲避障碍物时，那么将会导致规划失效，造成这种结果的原因是所使用的全局地图没有考虑此处的障碍物。

发明内容

本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种基于全局地图更新的矿山卡车路径规划方法，以解决上述背景技术中提出的问题。

一种基于全局地图更新的矿山卡车路径规划方法，包括：

基于历史数据的全局高精度地图和混合A-Star算法进行第一层全局路径规划，并提取车辆位姿变化节点作为关键点；

利用车辆激光雷达实时获取感知数据，基于DWA进行第二层局部路径规划；

判断是否存在场景时变导致规划失效；

若无场景时变影响，则车辆成功规划出路径并下发控制指令，驱动车辆到达目标点；

若场景时变而导致规划失效，则车辆利用当前的感知信息，将当前局部地图信息更新到全局地图对应的部分，完成全局地图局部更新并再重启第一层全局路径规划。

作为本发明的进一步的方案：所述基于历史数据的全局高精度地图和混合A-Star算法进行第一层全局路径规划，并提取车辆位姿变化节点作为关键点的具体步骤包括：

获取历史数据中全局高精度地图；

根据得到的全局高精度地图和混合A-Star算法进行第一层全局路径规划；

将从起点到目标点车辆位姿变化的节点选为关键点进行提取，并作为第二层局部路径规划的输入。

作为本发明的进一步的方案：所述利用车辆激光雷达实时获取感知数据，基于DWA进行第二层局部路径规划的具体步骤包括：

利用车辆激光雷达实时获取感知数据，生成局部的栅格地图；

根据第一层全局路径规划的结果和局部的栅格地图，基于DWA进行第二层局部路径规划，同时输出控制指令控制车辆以预期速度和朝向从当前关键点行驶至下一关键点；

在车辆行驶过程中实时进行车辆碰撞检测。

作为本发明的进一步的方案：所述在车辆行驶过程中实时进行车辆碰撞检测的具体步骤包括：

根据车辆底层返回的位姿预测车辆下一时刻的轨迹点，并以预设比例对障碍物和车辆进行膨胀；

再通过与局部栅格地图的栅格占据情况进行比较，判断障碍物和车辆对应的栅格是否重叠。

作为本发明的进一步的方案：对历史数据的全局地图进行局部更新的具体步骤包括：

首先暂停局部路径规划，通过激光雷达获取车辆周围的点云信息，并利用预设坐标系的Z轴高程差建立局部栅格地图；

同时获取车辆的经纬度和朝向定位，并转换成全局东北天坐标系下的XY坐标和朝向；

最后通过全局地图更新算法，将局部栅格地图的占据值更新到全局地图中。

与现有技术相比，本发明存在以下技术效果：

通过采用上述的技术方案，利用双层规划方案，先基于上一次更新的全局高精度地图通过混合A-Star进行全局路径规划，并得到若干个关键点。再利用第二层局部规划算法DWA根据关键点计算车辆的期望前轮转角和速度，作为车辆的控制指令发送给车辆的控制模块，最终控制车辆到达目的地。通过地图局部更新结合到双层路径规划中，大大提高了规划的成功率，且无需重新绘制高精度地图。具有适应矿区场地高时变性的特点，降低成本、提高效率。避免因某次规划失效而需要人工驾驶车辆避障的情况，提高了自动化程度。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1为本申请公开的一些实施例的矿山卡车路径规划方法的步骤示意图；

图2为本申请公开的一些实施例的矿山卡车路径规划方法的流程框图；

图3为本申请公开的一些实施例的地图局部更新流程图；

图4为本申请公开的一些实施例的全局路径规划效果图；

图5为本申请公开的一些实施例的关键点未被占据和被占据时规划过程示意图；

图6为本申请公开的一些实施例的局部更新后全局路径规划的示意图；

图7为本申请公开的一些实施例的车辆坐标系的示意图；

图8为本申请公开的一些实施例的局部地图更新的示意图。

具体实施方式

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

DWA：动态窗口法(Dynamic Window Approach,缩写为DWA)；

请参考图1和图2，本发明实施例中，一种基于全局地图更新的矿山卡车路径规划方法，包括：

S1、基于历史数据的全局高精度地图和混合A-Star算法进行第一层全局路径规划，并提取车辆位姿变化节点作为关键点；

第一层全局路径规划的具体步骤包括：

获取历史数据中全局高精度地图，具体实施方式为获得全局高精度地图。若是首次作业，则地图事先由采图车在生产开始前进行采集，并加载至车辆本地。在下一次统一更新全局地图之前，不再需要从外部获取地图，而是对本地全局地图进行局部更新。

根据得到的全局高精度地图和混合A-Star算法进行第一层全局路径规划；

将从起点到目标点车辆位姿变化的节点选为关键点进行提取，并作为第二层局部路径规划的输入。

本实施例中，矿山卡车路径规划方法采用的是双层规划。首先通过混合A-Star进行全局路径规划，全局路径规划是基于上一次更新的全局高精度地图，规划出一条从起点到终点的全局路径，并设置若干个关键点。如图4所示，图示为全局路径规划效果图。图中箭头所指即为关键点。

相较于A-Star算法，本实施例采用混合A-Star算法考虑了车辆动力学的因素，使得规划出的路径更加合理，满足车辆的动力学要求。

S2、利用车辆激光雷达实时获取感知数据，基于DWA进行第二层局部路径规划；

本实施例中，第二层规划采用动态窗口法(Dynamic Window Approach,DWA)，第一层规划输出的轨迹点并不能作为车辆控制层的输入，需要通过动态窗口法根据关键点计算出车辆的期望前轮转角和速度，作为车辆的控制指令发送给车辆的控制模块，最终控制车辆到达目的地。

第二层局部路径规划的具体步骤包括：

利用车辆激光雷达实时获取感知数据，生成局部的栅格地图；

根据第一层全局路径规划的结果和局部的栅格地图，基于DWA进行第二层局部路径规划，同时，输出目标速度和期望转角等控制指令到控制层，控制车辆以预期速度和朝向从当前关键点行驶至下一关键点；

在车辆行驶过程中实时进行车辆碰撞检测，具体步骤包括：

根据车辆底层返回的位姿预测车辆下一时刻的轨迹点，并以预设比例对障碍物和车辆进行膨胀；

再通过与局部栅格地图的栅格占据情况进行比较，判断障碍物和车辆对应的栅格是否重叠。

具体实施方式中，根据车辆底层返回的位姿，预测车辆下一时刻的轨迹点，然后按照一定的参数比例将障碍物和车辆进行膨胀，再通过和局部地图的栅格占据情况进行比较，判断障碍物和车辆所对应的栅格是否发生重叠，从而判断该轨迹是否会发生碰撞，若DWA规划的所有轨迹都发生碰撞，则导致局部路径规划失效。

若局部路径规划失效，则判断在全局规划出的第k个关键点到第k+1个关键点之间(局部路径规划失效区间)的路况发生了较大变化，说明全局路径规划出的第k个关键点变得不合理，此时需将车辆感知信息融入到本地的全局地图中，在全局地图中更新出此关键点附近的障碍物信息。

S3、判断是否存在场景时变导致规划失效；

如图5所示，图示为关键点未被占据(左)、被占据时(右)规划过程示意图。

场景在全局路径规划的某个关键点处出现了与先验全局地图较大的区别时，比如山体落石占据了规划路径的某一关键点，导致局部路径规划算法DWA失败或规划效率严重降低。

若无场景时变影响，则车辆成功规划出路径并下发控制指令，驱动车辆到达目标点；

若场景时变而导致规划失效，则车辆利用当前的感知信息，将当前局部地图信息更新到全局地图对应的部分，完成全局地图局部更新并再重启第一层全局路径规划。

如图6所示，图示为利用局部更新的全局地图进行规划的结果示意图。

对历史数据的全局地图进行局部更新的具体步骤包括：

首先暂停局部路径规划，通过激光雷达获取车辆周围的点云信息，并利用预设坐标系的Z轴高程差建立局部栅格地图；

同时获取车辆的经纬度和朝向定位，并转换成全局东北天坐标系下的XY坐标和朝向；

最后通过全局地图更新算法，将局部栅格地图的占据值更新到全局地图中。

如图7和图8所示，图示为车辆坐标系和局部地图更新方法示意图。

更新地图后，设置重新规划的标志位为真。

重启路径规划的具体步骤为：

基于更新后的全局地图使用A-Star算法进行全局路径规划，从而避开此处的障碍路段。

基于DWA根据新的全局路径提供的关键点进行局部路劲规划，输出控制指令，使车辆到达预计目标点。

如图2所示，当矿卡到达起点位置后，将进入自动驾驶状态，启动路径规划模块，并基于首次加载的全局高精度地图为车辆规划出一条路径。然后依照规划好的路径控制车辆，在整个运行过程中会有两种情况：

1)车辆顺利按照规划路径到达目标点；

2)在运行过程中场景发生了变化导致车辆判断继续依照规划路径会发生碰撞即规划失效。若无场景时变影响则车辆总能成功规划出路径并发布速度和转角驱动车辆到达目标点；而当出现场景时变而导致规划失效时，车辆将利用当前的感知信息，通过算法将此处的局部地图信息更新到全局地图对应的部分，完成地图更新。完成全局地图的局部更新后，将基于更新后的全局地图再次启动全局路径规划器，即可针对车辆遇到的障碍进行绕行，使得车辆仍能完成本次的作业任务。

全局地图的局部更新的算法和流程：如图3所示，当局部规划判断无法通行时，将会发出重规划标志，检测到重规划标志后，车辆停下，利用激光雷达获取到的车辆周边的激光点云信息并通过Z轴高程差建立局部栅格地图，与此同时获取车辆的经纬度和朝向定位并转成全局东北天坐标系下的XY坐标和朝向，然后开始对全局地图中该坐标下的全局栅格地图进行遍历，将局部栅格地图的栅格值赋值给全局地图对应的坐标点，完成地图更新。此后，将新的全局地图发送给第一层规划器(混合A-Star规划器)进行全局规划。

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

Claims (5)

1.一种基于全局地图更新的矿山卡车路径规划方法，其特征在于，包括：

基于历史数据的全局高精度地图和混合A-Star算法进行第一层全局路径规划，并提取车辆位姿变化节点作为关键点；

利用车辆激光雷达实时获取感知数据，基于DWA进行第二层局部路径规划；

判断是否存在场景时变导致规划失效；

若无场景时变影响，则车辆成功规划出路径并下发控制指令，驱动车辆到达目标点；

若场景时变而导致规划失效，则车辆利用当前的感知信息，将当前局部地图信息更新到全局地图对应的部分，完成全局地图局部更新并再重启第一层全局路径规划。

2.根据权利要求1所述一种基于全局地图更新的矿山卡车路径规划方法，其特征在于，所述基于历史数据的全局高精度地图和混合A-Star算法进行第一层全局路径规划，并提取车辆位姿变化节点作为关键点的具体步骤包括：

获取历史数据中全局高精度地图；

根据得到的全局高精度地图和混合A-Star算法进行第一层全局路径规划；

将从起点到目标点车辆位姿变化的节点选为关键点进行提取，并作为第二层局部路径规划的输入。

3.根据权利要求1所述一种基于全局地图更新的矿山卡车路径规划方法，其特征在于，所述利用车辆激光雷达实时获取感知数据，基于DWA进行第二层局部路径规划的具体步骤包括：

利用车辆激光雷达实时获取感知数据，生成局部的栅格地图；

根据第一层全局路径规划的结果和局部的栅格地图，基于DWA进行第二层局部路径规划，同时输出控制指令控制车辆以预期速度和朝向从当前关键点行驶至下一关键点；

在车辆行驶过程中实时进行车辆碰撞检测。

4.根据权利要求3所述一种基于全局地图更新的矿山卡车路径规划方法，其特征在于，所述在车辆行驶过程中实时进行车辆碰撞检测的具体步骤包括：

根据车辆底层返回的位姿预测车辆下一时刻的轨迹点，并以预设比例对障碍物和车辆进行膨胀；

再通过与局部栅格地图的栅格占据情况进行比较，判断障碍物和车辆对应的栅格是否重叠。

5.根据权利要求1所述一种基于全局地图更新的矿山卡车路径规划方法，其特征在于，对历史数据的全局地图进行局部更新的具体步骤包括：

首先暂停局部路径规划，通过激光雷达获取车辆周围的点云信息，并利用预设坐标系的Z轴高程差建立局部栅格地图；

同时获取车辆的经纬度和朝向定位，并转换成全局东北天坐标系下的XY坐标和朝向；

最后通过全局地图更新算法，将局部栅格地图的占据值更新到全局地图中。

Images