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

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

【技术实现步骤摘要】
一种基于SDF地图的无人机三维路径快速规划方法


[0001]本专利技术涉及无人机三维路径快速规划方法
，尤其涉及一种基于SDF地图的无人机三维路径快速规划方法。

技术介绍

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

技术实现思路

[0003]本专利技术旨在解决现有技术的不足，而提供一种基于SDF地图的无人机三维路径快速规划方法。
[0004]本专利技术为实现上述目的，采用以下技术方案：
[0005]一种基于SDF地图的无人机三维路径快速规划方法，具体步骤如下：
[0006]步骤一，建立待扩展列表和已扩展列表，将起始点加入待扩展列表中；
[0007]步骤二，判断待扩展列表是否为空，若是则结束，若否则进入步骤三；
[0008]步骤三，取出待扩展列表中评估函数f值最小的点p，判断p点是否为目标点，若是则结束，若否则进入步骤四；
[0009]步骤四，以欧几里得符号距离场体素大小的单倍步长，判断点p与其父节点之间的路径是否存在碰撞，若是则将点p加入已扩展列表，并跳到步骤二，若否则将当前起始点设置为点p的父节点，并将点p设置为新的起始点，进入步骤五；
[0010]步骤五，以欧几里得符号距离场体素大小的n倍步长对当前起始点的全部未扩展的邻居节点进行扩展，n∈[2,10]，n为整数，对当前起始点周围未扩展的邻居节点进行碰撞检测，将未碰撞的邻居节点加入待扩展列表，之后跳到步骤二。
[0011]进一步的，从步骤四跳到步骤二之前，需判断点p的父节点集合是否为空，若是则跳到步骤二，若否则以父节点集合中剩余的父节点中评估函数f值最小的父节点为路径碰撞检测的参考点，再次进行碰撞检测，直到父节点集合为空。
[0012]进一步的，步骤五中的n＝4。
[0013]进一步的，评估函数f＝g+lambda*h，其中，g为从起始点开始到当前节点的实际代价，h为当前节点到目标点的启发值，lambda为权重系数，步骤三中lambda＝20。
[0014]进一步的，g＝g_value+distance1，其中g_value为当前节点的父节点的g值，distance1为当前节点的父节点到当前节点的欧几里得距离，h＝1.0001*distance2。
distance2为当前节点到终点的欧几里得距离。
[0015]进一步的，步骤五进行之前，以欧几里得符号距离场体素大小的n倍步长对待扩展集合中的节点进行快速探索，快速探索的具体步骤如下：
[0016]步骤A，将点p拷贝一份命名为p2；
[0017]步骤B，找出p2的f值最小的邻居节点，公式中lambda＝1，以欧几里得符号距离场体素大小的单倍步长，判断点该邻居节点与p2之间的路径是否存在碰撞，若是则进入步骤五，若否则进入步骤C；
[0018]步骤C，判断该邻居节点是否在已扩展集合中，若是则进入步骤D，若否则将其加入待扩展列表并进入步骤G；
[0019]步骤D，判断该邻居节点的其他父节点是否全碰撞，若是则进入步骤E，若否则进入步骤F；
[0020]步骤E，将p2加入该邻居节点的父节点集合当中，以lambda＝20对该邻居节点的f值进行计算，并将该邻居节点加入待扩展列表中，进入步骤F；
[0021]步骤F，判断该邻居节点当前计算的g值是否小于其原有g值，若是则将该邻居节点的g值和f值更新为当前计算的g值和f值，并将其父节点更新为p2，之后进入步骤G，若否则直接进入步骤G；
[0022]步骤G，将该邻居节点作为新的p2，进入步骤B。
[0023]本专利技术的有益效果是：本专利技术方法与传统的A星算法相比，在保证了逼近最优解的同时拥有更少的规划时间，加速效果明显，使得即使是全局路径规划算法也达到了实时的规划速度。
附图说明
[0024]图1为本专利技术的流程图；
[0025]图2为本专利技术快速探索步骤的流程图；
[0026]图3为传统的A星算法规划的路径示意图；
[0027]图4为本专利技术算法规划的路径示意图；
[0028]以下将结合本专利技术的实施例参照附图进行详细叙述。
具体实施方式
[0029]下面结合实施例对本专利技术作进一步说明：
[0030]一种基于SDF地图的无人机三维路径快速规划方法，具体步骤如下：
[0031]步骤一，建立待扩展列表和已扩展列表，将起始点加入待扩展列表中；
[0032]步骤二，判断待扩展列表是否为空，若是则结束，若否则进入步骤三；
[0033]步骤三，取出待扩展列表中评估函数f值最小的点p，判断p点是否为目标点，若是则结束，若否则进入步骤四；
[0034]步骤四，以欧几里得符号距离场体素大小的单倍步长，判断点p与其父节点之间的路径是否存在碰撞，若是则将点p加入已扩展列表，并跳到步骤二，若否则将当前起始点设置为点p的父节点，并将点p设置为新的起始点，进入步骤五；
[0035]步骤五，以欧几里得符号距离场体素大小的n倍步长对当前起始点的全部未扩展
的邻居节点进行扩展，n∈[2,10]，n为整数，对当前起始点周围未扩展的邻居节点进行碰撞检测，将未碰撞的邻居节点加入待扩展列表，之后跳到步骤二。
[0036]进一步的，从步骤四跳到步骤二之前，需判断点p的父节点集合是否为空，若是则跳到步骤二，若否则以父节点集合中剩余的父节点中评估函数f值最小的父节点为路径碰撞检测的参考点，再次进行碰撞检测，直到父节点集合为空。
[0037]进一步的，步骤五中的n＝4。
[0038]进一步的，步骤五中对当前起始点周围未扩展的邻居节点进行碰撞检测的具体方法为：判断当前起始点的未扩展邻居节点集合是否为空，若是则直接跳到步骤二，若否则取出一个未扩展邻居节点并对其进行碰撞检测，若通过检测则将其加入待扩展集合后重新判断未扩展邻居节点集合是否为空，若未通过碰撞检测则直接重新判断未扩展邻居节点集合是否为空。
[0039]进一步的，评估函数f＝g+lambda*h，其中，g为从起始点开始到当前节点的实际代价，h为当前节点到目标点的启发值，lambda为权重系数，步骤三中lambda＝20。
[0040]进一步的，g＝g_value+distance1，其中g_value为当前节点的父节点的g值，distance1为当前节点的父节点到当前节点的欧几里得距离，h＝本文档来自技高网...

【技术保护点】

【技术特征摘要】
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为权重系数...

【专利技术属性】
技术研发人员：赵满超，王志敏，佘换林，
申请(专利权)人：天津基点科技有限公司，
类型：发明
国别省市：