CN112327931A - 一种基于sdf地图的无人机三维路径快速规划方法 - Google Patents

Abstract

本发明提供一种基于SDF地图的无人机三维路径快速规划方法，具体步骤如下：将起始点加入待扩展列表中；判断待扩展列表是否为空；取出待扩展列表中评估函数f值最小的点p；以欧几里得符号距离场体素大小的单倍步长，判断点p与其父节点之间的路径是否存在碰撞若否则将当前起始点设置为点p的父节点，并将点p设置为新的起始点；以n倍步长对邻居节点进行扩展。本发明方法与传统的A星算法相比，在保证了逼近最优解的同时拥有更少的规划时间，加速效果明显，使得即使是全局路径规划算法也达到了实时的规划速度。

Description

一种基于SDF地图的无人机三维路径快速规划方法

技术领域

本发明涉及无人机三维路径快速规划方法技术领域，尤其涉及一种基于SDF地图的无人机三维路径快速规划方法。

背景技术

目前，三维路径规划有两种地图，一种是栅格地图，一种是欧几里德符号距离场地图(SDF地图)，而实际测试中，显示欧几里得符号距离场的相关库实现获取目标点到障碍物的距离从而判断是否与障碍物碰撞是比较耗时的(例如voxblox)，而且无论是栅格地图还是欧几里得符号距离场，都存在分辨率这样一个概念。要测试一段路径是否发生碰撞需要根据分辨率，将这条路径上的所有以分辨率作为划分依据的的点都进行碰撞检测，所以在实现传统的全局路径规划算法(例如A星算法)时，发现由于碰撞检测和分辨率之间密不可分的关系，使得传统的A星算法在这样的路径规划任务中运算速度较慢。

发明内容

本发明旨在解决现有技术的不足，而提供一种基于SDF地图的无人机三维路径快速规划方法。

本发明为实现上述目的，采用以下技术方案：

一种基于SDF地图的无人机三维路径快速规划方法，具体步骤如下：

步骤一，建立待扩展列表和已扩展列表，将起始点加入待扩展列表中；

步骤二，判断待扩展列表是否为空，若是则结束，若否则进入步骤三；

步骤三，取出待扩展列表中评估函数f值最小的点p，判断p点是否为目标点，若是则结束，若否则进入步骤四；

步骤四，以欧几里得符号距离场体素大小的单倍步长，判断点p与其父节点之间的路径是否存在碰撞，若是则将点p加入已扩展列表，并跳到步骤二，若否则将当前起始点设置为点p的父节点，并将点p设置为新的起始点，进入步骤五；

步骤五，以欧几里得符号距离场体素大小的n倍步长对当前起始点的全部未扩展的邻居节点进行扩展，n∈[2,10]，n为整数，对当前起始点周围未扩展的邻居节点进行碰撞检测，将未碰撞的邻居节点加入待扩展列表，之后跳到步骤二。

进一步的，从步骤四跳到步骤二之前，需判断点p的父节点集合是否为空，若是则跳到步骤二，若否则以父节点集合中剩余的父节点中评估函数f值最小的父节点为路径碰撞检测的参考点，再次进行碰撞检测，直到父节点集合为空。

进一步的，步骤五中的n＝4。

进一步的，评估函数f＝g+lambda*h，其中，g为从起始点开始到当前节点的实际代价，h为当前节点到目标点的启发值，lambda为权重系数，步骤三中lambda＝20。

进一步的，g＝g_value+distance1，其中g_value为当前节点的父节点的g值，distance1为当前节点的父节点到当前节点的欧几里得距离，h＝1.0001*distance2。distance2为当前节点到终点的欧几里得距离。

进一步的，步骤五进行之前，以欧几里得符号距离场体素大小的n倍步长对待扩展集合中的节点进行快速探索，快速探索的具体步骤如下：

步骤A，将点p拷贝一份命名为p2；

步骤B，找出p2的f值最小的邻居节点，公式中lambda＝1，以欧几里得符号距离场体素大小的单倍步长，判断点该邻居节点与p2之间的路径是否存在碰撞，若是则进入步骤五，若否则进入步骤C；

步骤C，判断该邻居节点是否在已扩展集合中，若是则进入步骤D，若否则将其加入待扩展列表并进入步骤G；

步骤D，判断该邻居节点的其他父节点是否全碰撞，若是则进入步骤E，若否则进入步骤F；

步骤E，将p2加入该邻居节点的父节点集合当中，以lambda＝20对该邻居节点的f值进行计算，并将该邻居节点加入待扩展列表中，进入步骤F；

步骤F，判断该邻居节点当前计算的g值是否小于其原有g值，若是则将该邻居节点的g值和f值更新为当前计算的g值和f值，并将其父节点更新为p2，之后进入步骤G，若否则直接进入步骤G；

步骤G，将该邻居节点作为新的p2，进入步骤B。

本发明的有益效果是：本发明方法与传统的A星算法相比，在保证了逼近最优解的同时拥有更少的规划时间，加速效果明显，使得即使是全局路径规划算法也达到了实时的规划速度。

附图说明

图1为本发明的流程图；

图2为本发明快速探索步骤的流程图；

图3为传统的A星算法规划的路径示意图；

图4为本发明算法规划的路径示意图；

以下将结合本发明的实施例参照附图进行详细叙述。

具体实施方式

下面结合实施例对本发明作进一步说明：

一种基于SDF地图的无人机三维路径快速规划方法，具体步骤如下：

步骤一，建立待扩展列表和已扩展列表，将起始点加入待扩展列表中；

步骤二，判断待扩展列表是否为空，若是则结束，若否则进入步骤三；

步骤三，取出待扩展列表中评估函数f值最小的点p，判断p点是否为目标点，若是则结束，若否则进入步骤四；

步骤四，以欧几里得符号距离场体素大小的单倍步长，判断点p与其父节点之间的路径是否存在碰撞，若是则将点p加入已扩展列表，并跳到步骤二，若否则将当前起始点设置为点p的父节点，并将点p设置为新的起始点，进入步骤五；

步骤五，以欧几里得符号距离场体素大小的n倍步长对当前起始点的全部未扩展的邻居节点进行扩展，n∈[2,10]，n为整数，对当前起始点周围未扩展的邻居节点进行碰撞检测，将未碰撞的邻居节点加入待扩展列表，之后跳到步骤二。

进一步的，从步骤四跳到步骤二之前，需判断点p的父节点集合是否为空，若是则跳到步骤二，若否则以父节点集合中剩余的父节点中评估函数f值最小的父节点为路径碰撞检测的参考点，再次进行碰撞检测，直到父节点集合为空。

进一步的，步骤五中的n＝4。

进一步的，步骤五中对当前起始点周围未扩展的邻居节点进行碰撞检测的具体方法为：判断当前起始点的未扩展邻居节点集合是否为空，若是则直接跳到步骤二，若否则取出一个未扩展邻居节点并对其进行碰撞检测，若通过检测则将其加入待扩展集合后重新判断未扩展邻居节点集合是否为空，若未通过碰撞检测则直接重新判断未扩展邻居节点集合是否为空。

进一步的，评估函数f＝g+lambda*h，其中，g为从起始点开始到当前节点的实际代价，h为当前节点到目标点的启发值，lambda为权重系数，步骤三中lambda＝20。

进一步的，g＝g_value+distance1，其中g_value为当前节点的父节点的g值，distance1为当前节点的父节点到当前节点的欧几里得距离，h＝1.0001*distance2。distance2为当前节点到终点的欧几里得距离。

进一步的，步骤五进行之前，以欧几里得符号距离场体素大小的n倍步长对待扩展集合中的节点进行快速探索，快速探索的具体步骤如下：

步骤A，将点p拷贝一份命名为p2；

步骤B，找出p2的f值最小的邻居节点，公式中lambda＝1，以欧几里得符号距离场体素大小的单倍步长，判断点该邻居节点与p2之间的路径是否存在碰撞，若是则进入步骤五，若否则进入步骤C；

步骤C，判断该邻居节点是否在已扩展集合中，若是则进入步骤D，若否则将其加入待扩展列表并进入步骤G；

步骤D，判断该邻居节点的其他父节点是否全碰撞，若是则进入步骤E，若否则进入步骤F；

步骤E，将p2加入该邻居节点的父节点集合当中，以lambda＝20对该邻居节点的f值进行计算，并将该邻居节点加入待扩展列表中，进入步骤F；

步骤F，判断该邻居节点当前计算的g值是否小于其原有g值，若是则将该邻居节点的g值和f值更新为当前计算的g值和f值，并将其父节点更新为p2，之后进入步骤G，若否则直接进入步骤G；

步骤G，将该邻居节点作为新的p2，进入步骤B。

本发明方法中，虽然不以单倍体素大小而是以多倍体素大小的步长进行扩展，但依然严格的进行单倍碰撞检测，这样做在保证能够得到了一个接近最优解的值的前提下，不仅减少了扩展节点的数量，而且规划所需时间却大大缩短。快速探索步骤的加入，使得算法能快速地找到起始点与目标点之间的路径，这样做即使是在原始步长的A星算法当中，仅仅依靠快速探索步骤也能使得规划速度得到相对大幅度的提升。将其与多倍步长的扩展方式相结合，能够实现更为快速的全局路径规划。

由于本发明方法采用以多倍体素大小的步长进行扩展的节点扩展方法，因而只进行端点的碰撞检测是不严谨的，但如果每次扩展邻居节点的时候都进行完整的碰撞检测的话，那么则规划的速度反而远远低于原始的基于分辨率来进行扩展的A星算法，所以这里在扩展的时候不进行完整的碰撞检测，而是只进行端点的碰撞检测作为初步的筛选，之后将端点从待扩展列表中取出时，再进行严谨的进行整条路径的碰撞检测。而仅仅是这样一个小的改变，却会导致很多问题，例如：扩展的节点的父节点有可能由于发生碰撞而被放入已扩展集合当中，而扩展的节点实际上真正的父节点可能是其他节点，如果将其丢弃则会导致原本有其他父节点可以通过但却无法被检测出来。因此，本发明提出通过快速探索的步骤解决该问题，使得扩展速度提升的同时能够解决只进行端点碰撞检测而引发的问题。显然，原始的A星算法是不具备这样的路径规划需求的，所以发明方法基于传统的A星算法进行了改进，在运算速度和最优解之间达到了有效的平衡。

表1 实施例路径规划耗时对比表

如表1所示，第一组测试当中，在24.7*34.6*12.8大小的地图大小内以(0,0.82,1.0)和(-14.05,13.22,0.62)作为起始点和目标点的情况下，原始A星算法规划所需时间为54.6462ms，本发明方法规划所需时间为11.8925ms，后两组测试数据与之类似，可以看出本发明方法可以比A星算法在逼近最优解的同时拥有更少的规划时间，加速效果明显，使得在这样的地图大小范围内即使是全局路径规划算法也达到了实时的规划速度。

上面结合具体实施例对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种改进，或未经改进直接应用于其它场合的，均在本发明的保护范围之内。

Claims (6)

1.一种基于SDF地图的无人机三维路径快速规划方法，其特征在于，具体步骤如下：

步骤一，建立待扩展列表和已扩展列表，将起始点加入待扩展列表中；

步骤二，判断待扩展列表是否为空，若是则结束，若否则进入步骤三；

步骤三，取出待扩展列表中评估函数f值最小的点p，判断p点是否为目标点，若是则结束，若否则进入步骤四；

步骤四，以欧几里得符号距离场体素大小的单倍步长，判断点p与其父节点之间的路径是否存在碰撞，若是则将点p加入已扩展列表，并跳到步骤二，若否则将当前起始点设置为点p的父节点，并将点p设置为新的起始点，进入步骤五；

步骤五，以欧几里得符号距离场体素大小的n倍步长对当前起始点的全部未扩展的邻居节点进行扩展，n∈[2,10]，n为整数，对当前起始点周围未扩展的邻居节点进行碰撞检测，将未碰撞的邻居节点加入待扩展列表，之后跳到步骤二。

2.根据权利要求1所述的基于SDF地图的无人机三维路径快速规划方法，其特征在于，从步骤四跳到步骤二之前，需判断点p的父节点集合是否为空，若是则跳到步骤二，若否则以父节点集合中剩余的父节点中评估函数f值最小的父节点为路径碰撞检测的参考点，再次进行碰撞检测，直到父节点集合为空。

3.根据权利要求1所述的基于SDF地图的无人机三维路径快速规划方法，其特征在于，步骤五中的n＝4。

4.根据权利要求1-3中任意一项所述的基于SDF地图的无人机三维路径快速规划方法，其特征在于，评估函数f＝g+lambda*h，其中，g为从起始点开始到当前节点的实际代价，h为当前节点到目标点的启发值，lambda为权重系数，步骤三中lambda＝20。

5.根据权利要求4所述的基于SDF地图的无人机三维路径快速规划方法，其特征在于，g＝g_value+distance1，其中g_value为当前节点的父节点的g值，distance1为当前节点的父节点到当前节点的欧几里得距离，h＝1.0001*distance2。distance2为当前节点到终点的欧几里得距离。

6.根据权利要求5所述的基于SDF地图的无人机三维路径快速规划方法，其特征在于，步骤五进行之前，以欧几里得符号距离场体素大小的n倍步长对待扩展集合中的节点进行快速探索，快速探索的具体步骤如下：

步骤A，将点p拷贝一份命名为p2；

步骤B，找出p2的f值最小的邻居节点，公式中lambda＝1，以欧几里得符号距离场体素大小的单倍步长，判断点该邻居节点与p2之间的路径是否存在碰撞，若是则进入步骤五，若否则进入步骤C；

步骤C，判断该邻居节点是否在已扩展集合中，若是则进入步骤D，若否则将其加入待扩展列表并进入步骤G；

步骤D，判断该邻居节点的其他父节点是否全碰撞，若是则进入步骤E，若否则进入步骤F；

步骤E，将p2加入该邻居节点的父节点集合当中，以lambda＝20对该邻居节点的f值进行计算，并将该邻居节点加入待扩展列表中，进入步骤F；

步骤F，判断该邻居节点当前计算的g值是否小于其原有g值，若是则将该邻居节点的g值和f值更新为当前计算的g值和f值，并将其父节点更新为p2，之后进入步骤G，若否则直接进入步骤G；

步骤G，将该邻居节点作为新的p2，进入步骤B。

Images