CN111829526B

CN111829526B - 一种基于防撞半径的距离地图重构与跳点路径规划方法

Info

Publication number: CN111829526B
Application number: CN202010717373.6A
Authority: CN
Inventors: 秦龙; 唐见兵; 杨妹; 尹全军; 张琪; 曾俊杰; 曾云秀; 谢旭; 王鹏
Original assignee: National University of Defense Technology
Current assignee: National University of Defense Technology
Priority date: 2020-07-23
Filing date: 2020-07-23
Publication date: 2022-05-10
Anticipated expiration: 2040-07-23
Also published as: CN111829526A

Abstract

本发明公开一种基于防撞半径的距离地图重构与跳点路径规划方法，包括：在二维平面八连通网格路径规划地图上，构建基于防撞半径的距离图并对其进行初始化；将距离图的距离矩阵作为对规划主体进行路径规划的搜索空间，当搜索空间中的网格状态变化时，根据网格状态变化的波阵面来动态更新距离图；使用基于跳点搜索算法在搜索空间进行网格的扩展和跳跃点的识别时，根据距离图所承载的障碍物距离信息和规划主体的防撞半径进行碰撞检测。用于解决由于过渡更新冗余区域导致计算资源耗费及由于JPS方法自身规划导致在与DM融合过程中任务地图动态重构和路径规划效率较低等问题，实现任务地图的高效动态重构和提升路径规划的效率。

Description

一种基于防撞半径的距离地图重构与跳点路径规划方法

技术领域

本发明属于人工智能技术领域，尤其涉及一种基于防撞半径的距离地图重构与跳点路径规划方法。

背景技术

在人工智能领域，机器人技术和计算机游戏智能被视为建模人类在复杂环境下的感知、规划、决策和行动能力的综合领域。近年来，各类型机器人在生产生活中广泛使用，以及学界与工业界对计算机游戏智能的重视程度逐渐提高，典型的例子包括DeepMind团队开发的StarCraft游戏AI和OpenAI公司开发的Dota游戏AI等。作为机器人和游戏非玩家角色(NPC,Non-Player Characters)各方面技术的基础，基于均匀网格(uniform grids)地图的最短路径规划技术随着GPPC(Grid-based Path Planning Competitions)竞赛的举行和工业界对典型地图测试集的开放，在近十年也取得了显著的进步。作为一个基础服务，很多场景中有限的计算资源需要同时应答大量智能体的路径规划请求，这一请求一般关注三个方面的技术实现约束，首先，期望规划的时间复杂度尽可能的降低，以确保请求响应的实时性；其次，规划的结果应是可执行的，即满足请求主体的动力学约束和物理防撞半径；第三，路径规划的服务反馈需要动态的反应任务空间的地形变化(如障碍物的动态添加和删除)情况。因此，路径规划技术的快速响应能力、物理可达性、以及动态适应性受到相关领域的关注。

现有快速路径规划技术

在快速响应方面，其中最为著名的一类算法被称为跳点搜索(JPS,Jump PointSearch)算法，主要由澳大利亚莫纳什大学的Daniel Harabor博士提出的“Harabor D,Grastien A.Online Graph Pruning for Pathfinding on Grid Maps[C]//NationalConference on Artificial Intelligence.2011:1114–1119.”及“Harabor D,GrastienA.Improving Jump Point Search[C]//International Conference on AutomatedPlanning and Scheduling.2014.”。当智能体在网格地图上移动时，JPS在相同起点和终点、相等长度的所谓对称路径中，将对角方向移动优先路径确定为主要偏序(diagonal-first canonical ordering)，将不满足该原则的其他所有路径剔除在搜索空间之外。理论证明，任何可通行的两个位置之间都存在至少一条满足该规则的最短路径。如图1(a)、图1(b)、图1(c)所示，被该偏序规则剔除的子节点(灰色所示)通常可以由当前节点的父节点从更短的路径或长度相同但对角移动出现更早的路径到达。通常，这样的剪枝递归使用可以使某一节点的子节点仅剩1个或0个，不仅降低了正在被扩展节点的分支因子，也减小了子节点的分支因子。另外，JPS应用A*算法进行路径规划时，并不将中间节点加入到用于扩展后续路径节点的Open表中，除非遇到图1(c)中需要将前进方向向其他分支扩展的节点时，才将这样的节点加入Open表中以待扩展。中间节点都将被跳过，因此图1(c)中的特殊节点(位于中心的网格)称为“跳点”，JPS的名称由此得来。

JPS算法的优点是仅需在线处理，不需要对地图进行预处理，但是与基本的A*算法相比，速度提高了至少一个数量级。其主要缺点是扩展节点时(也就是寻找“跳点”进行扩展的过程)需要进行大量的逐行逐列的中间节点扫描和判断，因此与A*相比虽然减少了OPEN表的长度，但是对中间节点的check在很大程度上抵消了OPEN表长度缩减所带来的性能提升。

基于此，新的JPS PLUS(JPS+)算法，以及后续的JPS+with intermediate pruning(JPS+P)、JPS+with Bounding Box(JPS+BB)等算法，新增了离线阶段的预处理步骤，具体作法是：为每一可通行节点的每一可通行方向计算和存储与通过该方向可到达的第一个跳点或障碍物的距离，在线运行时通过这一预存信息实现真正意义上的“跳跃”，而无需检测跳点之间的中间节点，同时通过中转剪枝、包围盒测试等技术，进一步提升算法效率。然而，JPS+以后的算法改进都需要对地图进行预处理，且预处理算法无法提供有效的局部修复机制，使得在线运行时地图发生动态变化后，无法快速的给出基于修复和更新后的地图进行服务响应的有效方案。

现有动态距离地图修复技术，在机器人和游戏人工智能领域，距离图(DistanceMaps，以下简称DM)已被广泛用作对任务空间进行离散化表示的一种模型。在分布有障碍物的均匀网格环境中，可以构造相应的距离图，基于距离图向每个网格提供与其最近的障碍物的最大距离信息，与基础的占据网格地图(Grid Maps)相比，DM能够基于其存储的最大距离信息帮助规划主体(例如游戏中的NPCs或真实世界工作的机器人等具有一定防撞半径约束的对象)在路径规划时完成碰撞检测，剔除不合理网格，从而有效地规划出无碰撞路径，图2显示了在室内环境中构建的DM。黑色区域网格表示障碍物，对于每个未被障碍物占据的网格，距其最近的障碍物的距离越大，其灰度值越高。

现有的DM动态生成算法主要是以Kalra,Nidhi等人提出的Dynamic Brushfire算法为基础“Kalra,Nidhi,D.Ferguson,and A.Stentz."Incremental reconstruction ofgeneralized Voronoi diagrams on grids."Robotics and Autonomous Systems 57.2(2009):123-128.”，通过改进生成拓扑边界的精细程度、提升局部修复效率、进一步地图重要的拓扑关键节点等方式，产生了大量基于Dynamic Brushfire的变体“Burgard,Wolfram,B.Lau,and C.Sprunk."Efficient grid-based spatial representations for robotnavigation in dynamic environments."Robotics&Autonomous Systems61.10(2013):1116-1130.”及“Long Qin,et.al.“Dynamic Detection of Topological Informationfrom Grid-Based Generalized Voronoi Diagrams.”Mathematical Problems inEngineering.2013”。

DM的生成的一般原理如图3和图4所示，DM的动态生成主要采用两种产生自网格状态变化(如新的障碍物的生成，或者现有障碍物的删除)源头的所谓“波阵面”来逐步更新DM，这两种波阵面分别命名为“Lower Wavfront”和“Raise Wavefront”。DM生成算法保留一个优先级队列OPEN来对波阵面当前探索的网格进行排序。OPEN表内存储的待扩展的波阵面元素的优先级由该元素与波阵面源头的距离值的大小确定，距离越小，优先级越高，越早被处理。当一个元素从OPEN表取出进行扩展时，该元素所存储的距离加上新增的变化将传播到其相邻的网格，并且这些相邻网格中仍然受该波阵面影响的将再次被加入OPEN表，以便为下一轮传播做准备。

如图3(a)所示，当一个障碍网格(中间的点表示为s)被新插入网格地图的中心时，它首先将自身与最近障碍物之间的距离存储值dist设为0，然后启动一个Lower Wavefront以将距离变化传播到其相邻的单元(如图3(b)所示)。这种传播来自障碍物网格s，即变化的来源，当距离变化不再对网格产生影响时终止，如图3(c)和图3(d)所示，Wavefront遇到了其他障碍物生成的波阵面，在保持相同距离的网格处停了下来，无法继续传播，从而形成了DM的新边界，如图3(e)所示。

另一方面，当移除障碍物网格时如图4(a)所示，我们再次从地图中移除中心障碍物s)，此前被s所引发的wavefront影响到的区域中的所有网格的dist值变为无效，为了修改这些网格的状态，需要启动一个Raise Wavefront以重置这些无效单元，该波阵面的更新过程如图4(b)所示。Raise Wavefront终止于s与其他障碍占主导的相邻区域之间的DM边界(如图4(c)所示)，此后，边界另一侧的Lower Wavefront将继续传播直到无效区域，重新浸没该区域并生成新的DM边界(如图4(d)和图4(e)所示)。

对于现有的DM生成算法，其波阵面传播只是简单地生成待扩展网格的所有相邻网格(即8方向的扩展)。这种不加以区分的完全扩展模式会增加OPEN表的规模，使其成为动态更新DM的一个效率瓶颈。为了减少需要按优先级队列排序的元素数量，QinLong等人采用基于主要偏序(Cardinal Ordering)的剪枝策略来选择波阵面传播的树状扩展路径，提出了Cardinal Ordering Dynamic Brushfire(简称CODB)算法。图5显示了CODB如何从源网格(表示为白色瓦片)传播波阵面的，这种传播方式仅访问每个受影响的网格一次(其中灰色箭头表示波前的传播方向)。也就是说，该算法只考虑箭头方向上的单元的后继，而忽略其他的，从而可以获得显著的运行时节省。然而，现有的DM生成算法不会在更新时考虑规划主体的防撞半径，更新过程会一直覆盖该波阵面所能管辖的最大区域，这在障碍物占比小，障碍物空间分布稀疏的大地形空间中导致无法接受的运算开销，而且对于满足一定防撞半径的无碰路径规划任务而言，这种DM更新方式存在大量的冗余检测，是造成DM在实际应用中鲁棒性和实时性在特定任务空间中无法满足需求的主要原因。

发明内容

本发明要解决的技术问题是不需要对地图进行预处理，即可实时快速有效地进行路径规划，提出了一种基于防撞半径的距离地图重构与跳点路径规划方法。

为解决该问题，本发明所采用的技术方案是：

一种基于防撞半径的距离地图重构与跳点路径规划方法，包括以下步骤：

步骤1：在规划主体所处的二维平面八连通网格路径规划地图上，构建基于防撞半径的距离图DM并对其进行初始化，所述距离图DM的数据结构包括与所述八连通网格路径规划地图相同维度大小的障碍物位置矩阵obst和距离矩阵dist，所述障碍物位置矩阵obst中的元素obst_s存储的是与网格s距离最近的障碍物所在网格n的坐标，所述距离矩阵dist中的元素dist_s存储的是网格s距离最近的障碍物所在网格n的欧式距离；

步骤2：将所述距离图DM的距离矩阵dist作为对规划主体进行路径规划的搜索空间，当搜索空间中的网格状态变化时，根据网格状态变化的波阵面来动态更新受波阵面影响区域的距离图DM；

步骤3：使用基于跳点搜索算法JPS在所述搜索空间进行网格的扩展和跳跃点的识别时，根据所述距离图所承载的障碍物距离信息和规划主体的防撞半径进行碰撞检测，完成对规划主体的路径规划。

进一步地，所述波阵面影响区域为规划主体的防撞半径所覆盖的区域。

进一步地，步骤3的具体方法是：

步骤3.1：在以距离图DM中的距离矩阵dist作为对规划主体进行路径规划的搜索空间，基于主要偏序的剪枝策略进行网格扩展，剪除冗余邻接网格；

步骤3.2：在扩展网格时，仅选择传播方向上的网格的后继者作为下一轮传播的候选者，所述传播方向是指从源网格s到网格n的方向；

步骤3.3：判断所需扩展的网格s其在距离矩阵dist中的值dist_s是否大于规划主体的防撞半径，如果小于防撞半径，则继续扩展该网格，否则，不扩展该网格。

进一步地，所述基于主要偏序的剪枝策略是指：在相同起点和终点、相等长度的所谓对称路径中，将对角方向移动优先路径确定为主要偏序，将不满足该原则的其他所有路径剔除在搜索空间之外。

进一步地，所述网格状态变化是指有新增的障碍物网格或者障碍物网格被移除。

进一步地，对距离图DM进行的初始化是指：使优先级队列OPEN为空，将所有网格设置为无占据状态，对于一个网格S，初始化时，使dist_s＝∞，obst_s＝φ，所述OPEN表内存储的是待扩展的波阵面元素的优先级，所述优先级由波阵面元素与波阵面源头的距离值d的大小确定，距离越小，优先级越高；

当有新增的障碍物网格o时，更新障碍物位置矩阵obst和距离矩阵dist，将障碍物网格o所在的网格标记为障碍，使obst_o＝o，使dist_o＝0，表示障碍物网格o本身为最近的障碍单元，并将障碍物网格o以障碍物源头网格的身份插入波阵面的优先级队列OPEN表等待扩展；

当有移除的障碍物网格o时，对障碍物位置矩阵obst和距离矩阵dist中的网格数值进行重置，使dist_o＝∞，obst_o＝φ，将所移除的障碍物网格o以移除障碍物源头网格的身份插入波阵面优先级队列OPEN表等待扩展，并设置其优先级。

进一步地，步骤2中所述动态更新受波阵面影响区域的距离图DM的方法是：

从优先级队列OPEN中取出下一个待处理网格，直到OPEN为空；

如果待处理的网格s是被移除的障碍物网格，则将该网格s作为源网格进行波阵面的传播，并对传播中受波阵面影响的网格重置其dist值，当波阵面触及了被重置且不在其他障碍物波阵面波及区域的网格，则停止源于网格o的波阵面传播，或当波阵面到达一个被其他障碍物的波阵面影响的邻接网格n时，停止源于网格o的波阵面传播，同时将由n发起的，源于obst_n的障碍物网格波阵面Lower Wavefront记录到OPEN表；

如果待处理的网格s为有效的最近障碍单元格，则将该网格s作为源网格进行波阵面的传播，并对传播中受波阵面影响的网格更新其distt和obst值，当传播扩展的网格n与网格s的距离d不再保持距离缩减的趋势或者距离d大于所述规划主体本身的防撞半径时，则终止波阵面的传播；

如果待处理的网格为扩展网格s，则将该元素所存储的距离加上新增的距离变化值传播到其相邻的网格，并且这些相邻网格中仍然受该波阵面影响的网格将再次被加入OPEN表，以便为下一轮传播做准备，如果相邻网格在距离矩阵dist中的元素值大于所述规划主体本身的防撞半径，则终止传播，或者相邻网格在障碍物位置矩阵obst中的元素值不是由传播到该相邻网格的源网格坐标而是其他障碍物影响的邻接网格n时，则停止传播，并将由n发起的，源于obst_n的波阵面Lower Wavefront记录到OPEN表。

本发明还提供了一种基于防撞半径的距离地图重构与跳点路径规划系统，包括存储器和处理器，所述存储器存储有基于防撞半径的距离地图重构与跳点路径规划程序，所述处理器在运行所述基于防撞半径的距离地图重构与跳点路径规划程序时执行前面所述路径规划方法的步骤。

与现有技术相比，本发明所取得的有益效果是：

本发明一种基于防撞半径的距离地图重构与跳点路径规划方法，仅在以规划主体为中心以规划主体的防撞半径所覆盖的区域内对障碍物临近区域网格进行波阵面扩展，更新障碍物位置和障碍物距离；在基于主要偏序的距离图DM修复过程中将规划主体的防撞半径纳入考虑，只对影响到规划主体无碰路径规划的障碍物临近区域网格进行波阵面扩展，以防止现有算法过渡的更新冗余区域，避免其所带来的充分但非必要的计算耗费；其次是对现有跳点搜索算法JPS进行改进，在算法进行网格的扩展和跳跃点的识别时，在其检测规则上融合距离图DM所承载的障碍物距离信息，以确保JPS+DM的路径规划组合能够在保持算法完备性和最优性的同时，在动态场景中具备与传统的、不需要预处理的算法(如A*，D*等)平均至少一个数量级(10倍)的性能提升。因此，本发明将从充分利用规划主体防撞半径剪枝DM更新区域，以及将JPS进行改进以适应DM搜索空间等技术突击来解决上述两个问题，实现任务地图的高效动态重构和提升路径规划的效率。

附图说明

图1(a)为现有JPS技术中非跳点情景下对角移动时的剪枝示意图；

图1(b)为现有JPS技术中非跳点情景下轴向移动时的剪枝示意图；

图1(c)为现有JPS技术中在跳点位置处的剪枝示意图；

图2为室内环境中构建的距离图；

图3为障碍物网格波阵面Lower Wavfront传播示意图；

图4为被移除的网格波阵面Raise Wavefront传播示意图；

图5为CODB算法基于主要偏序算法扩展波阵面的原理图；

图6为主要偏序算法的更新流程示意图；

图7为JPS+DM算法的执行流程示意图；

图8为Dijestla算法架构的性能瓶颈示意图；

图9为JPS+DM运行效率性能对比图；

图10为路径规划效果图。

具体实施方式

图6至图10示出了本发明一种基于防撞半径的距离地图重构与跳点路径规划方法的一种具体实施例，包括

本实施例中，使用距离图DM能够基于其存储的距离信息帮助规划主体在路径规划时完成碰撞检测，剔除不合理网格，从而有效地规划出无碰撞路径。

本实施例中，对距离图DM进行的初始化是指：

使优先级队列OPEN为空，将所有网格设置为无占据状态，对于一个网格S，初始化时，使dist_s＝∞，obst_s＝φ，所述OPEN表内存储的是待扩展的波阵面元素的优先级，所述优先级由波阵面元素与波阵面源头的距离值d的大小确定，距离越小，优先级越高；

表1 DM的初始化伪代码

表1给出了距离图DM初始化的伪代码，本实施例中使用了三个函数完成初始化，即1)initialize函数，用于对描述DM所需要保存的数据结构进行初始化；在初始化时，所有网格被设置为无占据状态(第1行到第6行)。使优先级队列OPEN为空，对于地图上的每一个网格s，赋予距离矩阵dist中相对应网格的元素值dist_s＝∞，障碍物网格矩阵obst中，并使obst_s＝φ。

2)对于新增的障碍物网格o，使用setObstacleCell函数更新dist和obst矩阵对应的值，并将网格o以一个Lower Wavefront源网格的身份插入波阵面的优先级队列OPEN表等待进行扩展；当通过调用setObstacleCell函数将o标记为障碍时，它将dist_o设置为0并将其自身称为最近的障碍单元，即obst_o＝o(第7和8行)。

3)对于需要移除的障碍物网格o，则按照相反的步骤，使用removeObstacleCell函数，重置收到被移除网格影响的邻接网格，对所涉及的dist和obst网格数值进行重置，而后将被移除的障碍物网格o作为Raise Wavefornt的源网格压入OPEN表。当通过调用removeObstacleCell释放o时，函数resetCell(o)将网格o重置为初始值，即dist_o＝∞和

(第10行)，并将raise_o设置为true(第11行)。同时使用函数insert将单元格o插入优先级队列OPEN中，优先级值为d，或者如果o已经处于OPEN中，则更新优先级。

本实施例中波阵面影响区域为规划主体的防撞半径所覆盖的区域，网格状态变化是指有新增的障碍物网格或者障碍物网格被移除。当有新增的障碍物网格或者障碍物网格被移除的状态变化时，距离图根据网格状态变化情况及时更新受新增的障碍物网格的波阵面影响，更新距离图中的两个矩阵信息，或在被移除的障碍物网格状态变化时，重置受被移除的障碍物网格影响的距离图信息。具体动态更新的方法为：

从优先级队列OPEN中取出优先级最高的下一个待处理网格，直到OPEN为空；待处理的网格分3种情况：

1)如果待处理的网格s是被移除的障碍物网格，则将该网格s作为源网格进行波阵面的传播，并对传播中受波阵面影响的网格重置其dist和obst值，当波阵面触及了被重置且不在其他障碍物波阵面波及区域的网格，则停止源于网格o的波阵面传播，或当波阵面到达一个被其他障碍物的波阵面影响的邻接网格n时，停止源于网格o的波阵面传播，同时将由n发起的，源于obst_n的障碍物网格波阵面Lower Wavefront记录到OPEN表；

2)如果待处理的网格s为有效的最近障碍单元格，则将该网格s作为源网格进行波阵面的传播，并对传播中受波阵面影响的网格更新其dist和obst值，当传播扩展的网格n与网格s的距离d不再保持距离缩减的趋势或者距离d大于所述规划主体本身的防撞半径时，则终止波阵面的传播；

3)如果待处理的网格为扩展网格s，则将该元素所存储的距离加上新增的距离变化值传播到其相邻的网格，并且这些相邻网格中仍然受该波阵面影响的网格将再次被加入OPEN表，如果相邻网格在距离矩阵dist中的元素值大于所述规划主体本身的防撞半径，则终止传播，或者相邻网格在障碍物位置矩阵obst中的元素值不是由传播到该相邻网格的源网格坐标而是其他障碍物影响的邻接网格n时，则停止传播，并将由n发起的，源于obst_n的波阵面Lower Wavefront记录到OPEN表。

表格2给出了动态更新距离图DM的伪代码。函数update弹出具有最小dist_s值(即优先级最高)的下一个未处理网格s，直到OPEN队列为空(第13行和第14行)。

如果待处理的网格s是有效的最近障碍单元格，则以新插入的障碍物网格则调用函数lower，继续新插入的障碍物网格波阵面Lower Wavefront的传播(第17行到第19行)。新插入的障碍物网格调用函数lower来传播波阵面Lower Wavefornt，以传播对受影响网格的dist的更新(第26至30行)。当扩展的网格n的新计算的距离(在第23行中表示为d)不再保持距离缩减的趋势，或者d大于行为主体本身的防撞半径R时(第24行)时，这一起源于新插入障碍物网格s的Lower Wavefront将终止。

如果待处理网格s已经被清除(reset)，则以障碍物移除而新释放的网格调用函数raise(第16和17行)来启动波阵面Raise Wavefront的传播，重置受到被移除网格影响的邻接网格(第34到37行)。新释放的网格波阵面Raise Wavefront传播的终止条件有两种：第一种情况，是到达一个被其他障碍物的波阵面影响的邻接网格(即，扩展的网格n的最近障碍物不是当前被移除的s，即obst_n！＝s且obst_n是有效的)，在这种情况下，由n发起的，源于obst_n的波阵面Lower Wavefront将会被记录到OPEN表(第38和39行)，以确保该波阵面能够有可能在后续扩展过程中占据被重置的区域；第二种情况，Raise Wavefront的波阵面触及了被重置且不在其他障碍物波阵面波及区域的网格，在这种情况下只单纯停止传播，不会将“无主”的网格压入OPEN表(即第33行判断为false，算法不做任何操作)。

表格2 DM动态更新的算法伪代码

步骤3：使用基于跳点搜索算法JPS在所述搜索空间进行网格的扩展和跳跃点的识别时，融合距离图DM所承载的障碍物距离和防撞半径进行碰撞检测，完成对规划主体的路径规划。

步骤3.1：在以距离图DM中的距离矩阵dist作为对规划主体进行路径规划的搜索空间，基于主要偏序的剪枝策略进行网格扩展，剪除冗余邻接网格；基于主要偏序的剪枝策略是指：在相同起点和终点、相等长度的所谓对称路径中，将对角方向移动优先路径确定为主要偏序，将不满足该原则的其他所有路径剔除在搜索空间之外，如图6所示，。

本实施例中引入函数findSuccessors来过滤网格s当前需要扩展的邻接网格，这一过程通过使用主要偏序剪枝来实现。如图5所示，仅选择箭头方向上的单元的后继者作为下一轮传播的候选者(表2中的第20和31行)，而忽略其他的。函数direction(s，n)确定从s到n的方向并因此更新dir_n的值(表2中的第29和37行)。dir_n表示网格n下一步波阵面继续波阵面传播wavefront时所沿方向，findSuccessors函数执行时需要查询dir_s矩阵来过滤网格s需要扩展的临接网格；

步骤3.3：判断所需扩展的网格s其在距离矩阵dist中的值dist_s是否大于规划主体的防撞半径，如果小于防撞半径，则认为无碰撞并继续扩展该网格，否则，认为检测到碰撞且不扩展该网格。

如图7所示为JPS+DM的算法执行流程示意图，在生成DM之后，JPS将以DM中的dist矩阵作为其规划路径的搜索空间，在基于主要偏序进行网格扩展时，除了基础的JPS算法本身需要剪除的冗余邻接网格之外，判断一个网格是否能被扩展的必要条件增加了对该网格dist值的比较，DM的dist矩阵为每个网格存储了与该网格距离最近的障碍物的最大间隙，因此碰撞测试的操作可以简化为有效的查表操作。例如，对于一个防撞半径为R的规划主体agent，其所在网格标识为s，则碰撞测试的结果由如公式1所示的防撞检测等式确定其中1表示检测到冲突，0表示无冲突，C(R,s)为碰撞测试的结果，dist_s为生成的距离图矩阵元素，当网格s本身的半径R大于或等于距离s最近的障碍物，C(R,s)认为是检测到碰撞的，反之为不碰撞，1-C(R，s)为一个返回值是布尔变量的碰撞检测函数(Collision testfunction)。

如图8所示，JPS搜索算法的基本框架沿用了如图所示的Dijestla算法架构，其性能瓶颈主要体现在第3行OPEN表的优先级排序操作和第7行扩展当前被检测网格的后继网格的数量两个方面。

JPS搜索算法现有的性能提升是通过如图所示“剪枝”和“跳跃”取得的，前者依据“主要偏序扩展”的思想，避免了将邻接8网格全部作为后继网格进行考察；后者沿着后继网格的方向开展树状遍历操作，只将“跳点”压入JPS搜索算法的OPEN表作为后续扩展用，借此大幅压缩OPEN表规模，提升效率。

本发明在JPS已有的效率提升基础之上，进一步改进“剪枝”和“跳跃”过程，对于JPS搜索算法中的prune函数和successor函数中需要检测的所有网格，增加基于DM所提供的dist矩阵的碰撞检测查询，在进一步压缩后继网格的同时，确保所有的待扩展跳点以及跳点之间所形成的主序路径严格保证是安全的，即满足公式(1)所给出的检测条件。

本发明所要解决的技术问题主要有两个方面，首先是如何在基于主要偏序的DM修复过程中将行为主体的防撞半径纳入考虑，只对影响到主体无碰路径规划的障碍物临近区域网格进行波阵面扩展，以防止现有算法过渡的更新冗余区域，避免其所带来的充分但非必要的计算耗费；其次是如何对现有JPS规划算法进行改进，在算法进行网格的扩展和跳跃点的识别时，在其检测规则上融合DM所承载的障碍物距离信息，以确保JPS+DM的路径规划组合能够在保持算法完备性和最优性的同时，在动态场景中具备与传统的、不需要预处理的算法(如A*，D*等)平均至少一个数量级(10倍)的性能提升。

因此，本发明将从充分利用规划主体防撞半径剪枝DM更新区域，以及将JPS进行改进以适应DM搜索空间等技术突击来解决上述两个问题，实现任务地图的高效动态重构和提升路径规划的效率。

如图9所示，在相同规模网格地图上，在本发明所提供的DM地图之上使用改进之后的JPS+完成途径规划任务，较之于经典Dijestla和A*算法，具有至少一个数量级的效率提升，且可以根据规划主体本身的物理半径R获取满足实际物理约束的路径结果。以1平方公里，分辨率为1米/网格的地图上，选择100对最优路径平均长度超过800米的起点-终点对，分别采用经典A*算法(曲线系列1)和本算法库所提供算法(曲线系列2)执行路径规划任务所得到的效率对比，单位为毫秒(ms)，计算效率平均提升14到40倍。

如图10所示为本发明基于特定地形场景的具体实施方式示意图，为了检验基于DM的JPS路径规划在针对有防撞半径的智能体的路径规划应用中的有效性，设计了一个200×200的地形场景。

在DM的构建阶段，黑色网格为障碍物，在此基础上利用本发明给出的基于半径R＝10所生成的DM图，根据每个发生dist值更新的网格，其值的大小被以0-255区间的灰度可视化的进行了表示，距离障碍物越近，灰度值越低，dist大于R的网格为安全区，以灰度255的高亮级别表示。

在基于DM的JPS路径规划阶段，为突出比较，本案例模拟了X和Y两个智能体在其中进行路径规划X使用A*算法进行规划。而Y则使用JPS对DM进行搜索。可以看出，智能体X需要在规划过程中大量扩展检测网格(白色部分)，其OPEN表规模巨大，由此带来巨大的时间开销。

而对于智能体Y，有效利用DM所给出的防撞信息，并基于自身的跳跃扩展能力，其路径规划过程因为剪枝和跳点选择而具备很高的时间效率，而且由于本发明中DM的更新考虑了Y的防撞半径，其在障碍物动态变化时具备很好的实时规划响应能力。

本发明实施例还提供一种基于防撞半径的距离地图重构与跳点路径规划系统，包括存储器和处理器，所述存储器存储有基于防撞半径的距离地图重构与跳点路径规划程序，所述处理器在运行所述基于防撞半径的距离地图重构与跳点路径规划程序时执行上述实施例所述方法的步骤。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims

1.一种基于防撞半径的距离地图重构与跳点路径规划方法，其特征在于，包括以下步骤：

步骤1：在规划主体所处的二维平面八连通网格路径规划地图上，构建基于防撞半径的距离地图并对其进行初始化，所述距离地图的数据结构包括与所述八连通网格路径规划地图相同维度大小的障碍物位置矩阵obst和距离矩阵dist，所述障碍物位置矩阵obst中的元素obst_s存储的是与网格s距离最近的障碍物所在网格n的坐标，所述距离矩阵dist中的元素dist_s存储的是与网格s距离最近的障碍物所在网格n的欧式距离；

步骤2：将所述距离地图的距离矩阵dist作为对规划主体进行路径规划的搜索空间，当搜索空间中的网格状态变化时，根据网格状态变化的波阵面来动态更新受波阵面影响区域的距离地图；

步骤3：使用基于跳点搜索算法JPS在所述搜索空间进行网格的扩展和跳跃点的识别时，根据所述距离地图所承载的障碍物距离信息和规划主体的防撞半径进行碰撞检测，完成对规划主体的路径规划；步骤3的具体方法是：

步骤3.1：在以距离地图中的距离矩阵dist作为对规划主体进行路径规划的搜索空间时，基于主要偏序的剪枝策略进行网格扩展，剪除冗余邻接网格；

2.根据权利要求1所述的规划方法，其特征在于，所述波阵面影响区域为规划主体的防撞半径所覆盖的区域。

3.根据权利要求2所述的规划方法，其特征在于，所述基于主要偏序的剪枝策略是指：在相同起点和终点、相等长度的所谓对称路径中，将对角方向移动优先路径确定为主要偏序，将不满足对角方向作为优先移动路径原则的其他所有路径剔除在搜索空间之外。

4.根据权利要求1所述的方法，其特征在于，所述网格状态变化是指有新增的障碍物网格或者障碍物网格被移除。

5.根据权利要求1所述的方法，其特征在于，对距离地图进行的初始化是指：

使优先级队列OPEN表为空，将所有网格设置为无占据状态，对于一个网格S，初始化时，使dist_s＝∞，obst_s＝φ，所述优先级队列OPEN表内存储的是待扩展的波阵面元素的优先级，所述优先级由波阵面元素与波阵面源头的距离值d的大小确定，距离越小，优先级越高；

6.根据权利要求4所述的方法，其特征在于，步骤2中所述动态更新受波阵面影响区域的距离地图的方法是：

从优先级队列OPEN表中取出优先级最高的下一个待处理网格，直到优先级队列OPEN表为空；

如果待处理的网格s是被移除的障碍物网格，则将该网格s作为源网格进行波阵面的传播，并对传播中受波阵面影响的网格重置其dist和obst值，当波阵面触及了被重置且不在其他障碍物波阵面波及区域的网格，则停止源于网格o的波阵面传播，或当波阵面到达一个被其他障碍物的波阵面影响的邻接网格n时，停止源于网格o的波阵面传播，同时将由n发起的，源于obst_n的障碍物网格波阵面Lower Wavefront记录到优先级队列OPEN表；

如果待处理的网格s为有效的最近障碍单元格，则将该网格s作为源网格进行波阵面的传播，并对传播中受波阵面影响的网格更新其dist和obst值，当传播扩展的网格n与网格s的距离d不再保持距离缩减的趋势或者距离d大于所述规划主体本身的防撞半径时，则终止波阵面的传播；

如果待处理的网格为扩展网格s，则将该元素所存储的距离加上新增的距离变化值传播到其相邻的网格，并且这些相邻网格中仍然受该波阵面影响的网格将再次被加入优先级队列OPEN表，如果相邻网格在距离矩阵dist中的元素值大于所述规划主体本身的防撞半径，则终止传播，或者相邻网格在障碍物位置矩阵obst中的元素值不是由传播到该相邻网格的源网格坐标而是其他障碍物影响的邻接网格n时，则停止传播，并将由n发起的，源于obst_n的波阵面Lower Wavefront记录到优先级队列OPEN表。

7.一种基于防撞半径的距离地图重构与跳点路径规划系统，其特征在于，包括存储器和处理器，所述存储器存储有基于防撞半径的距离地图重构与跳点路径规划程序，所述处理器在运行所述基于防撞半径的距离地图重构与跳点路径规划程序时执行权利要求1～6任一项所述方法的步骤。