CN109798899A - 一种面向海底未知地形搜索的树扩散启发式路径规划方法 - Google Patents

Abstract

本发明公开了一种面向海底未知地形搜索的树扩散启发式路径规划方法，包括下述步骤：步骤一，对地图空间进行建模；在二维空间中生成O‑XYZ坐标系，O为原点，X、Y、Z分别对应X、Y、Z轴，并对空间进行离散化处理，在坐标系中生成density*density*density个像素点，其中density表示密度，每一个像素点代表一个位置点；同时对位置点划分为可达点和不可达点，在算法中将可达点标记为0，将不可达点标记为1；本发明在航行器逐步探测海底信息的过程中，可实时运算当前搜索到的路径信息，而不需要待全部海底信息探测完成后再进行运算，节省了在探索过程中浪费的时间。

Description

一种面向海底未知地形搜索的树扩散启发式路径规划方法

技术领域

本发明涉及路径规划技术领域，具体涉及一种面向海底未知地形搜索的树扩散启发式路径规划方法。

背景技术

海洋是个巨大的资源宝库，除了含有丰富的水资源以外，也蕴含着丰富的生物资源、石油资源、可燃冰资源和矿产资源，其数量非常庞大。同时，海洋面积约占地球表面积的70％，是人类未来重要的活动空间。多个世纪以来，人类不断加大对陆地资源的开发，使得陆地资源日趋枯竭，稀缺的陆地资源不足以支撑人类社会快速发展的需要，对人类起着巨大的制约影响。对此，世界各国开始对海洋资源进行探测与开发。在新世纪下，各国在对陆地资源开发的同时对海洋进行探索，能拓宽人类的生存空间，促进各国的合作交流，其具有巨大的开发潜力和战略意义。

目前，各国对海洋竞相探索与开发，但对海洋只探索了5％，还有95％的大海海底是未知的。根据已知的海底信息可推断，未知的海洋环境往往是复杂和危险的，所以加强对海洋探测设备的研究具有重要的价值。随着科技的发展，很多国家均研发出了自主式水下机器人(AUV)，与载人潜水器相比，AUV的体积非常小，相对比较灵活，而且所需能源也少，可大批量生产。各个研究机构为AUV搭配了各种探测设备，如传感器，使之能够探测未知区域；且每个AUV均内置计算机***，能够感知复杂和危险的水底环境并进行决策与判断，具有强自主学习的智能性，能为使用者提供高效的环境分析。

在AUV探索过程中，需要路径规划技术来引导航行器的探索，为航行器在复杂的海底环境空间中搜索出一条从起始点到终止点的可行路径，并要求该路径具有不触碰海底障碍物、路径长度尽可能短的特性；针对这些要求，国内外已有大量相关的路径规划研究。其中，有基于虚拟势场和导航函数搜索的方法，如人工势场法、谐波函数势场、流场等等；有基于数学最优化的方法，如滚动优化、线性规划、水平集、支持向量机等等；有基于生物智能的算法，如蚁群算法、粒子群算法、遗传与进化算法、神经网络法等等；，但是，以上方法均是基于地图信息全已知的前提下进行的，属于全局信息搜索，而实际情况中往往很难获得地形的全部信息，属于局部信息搜索的情况，需要航行器逐步探测来获取。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种面向海底未知地形搜索的树扩散启发式路径规划方法，该方法能够实时运算当前搜索到的路径信息，节省了成本，大大提高工作效率。

本发明的目的通过下述技术方案实现：

一种面向海底未知地形搜索的树扩散启发式路径规划方法，包括下述步骤：

步骤一，对地图空间进行建模；在二维空间中生成O-XYZ坐标系，O为原点，X、Y、Z分别对应X、Y、Z轴，并对空间进行离散化处理，在坐标系中生成density*density*density个像素点，其中density表示密度，每一个像素点代表一个位置点；同时对位置点划分为可达点和不可达点，在算法中将可达点标记为0，将不可达点标记为1；将两点之间的连线划分为可行路径和不可行路径，可行路径是指航行器可通行的路径，不可行路径是指该路径会触碰障碍物；在分析障碍物时，若地形空间的像素点被障碍物囊括，则将该像素点标记为障碍点；对于可达点，对其划分为普通可达点、边缘可达点和潜力可达点，而潜力可达点是边缘可达点的子集；

步骤二，面对未知的海底空间，航行器需要逐步探测地形环境，并评估可行路径；航行器基于起始点进行第一层的探测，即扩散操作，将探测得到的障碍点记录到障碍点集合，以及将探测得到的潜力点记录到潜力点集合；

其中，所述扩散操作具体为：基于一个扩散点，遍历当前已获得的所有像素点信息，判断该像素点是否满足潜力点定义，若满足则将该潜力点设置为扩散点的孩子节点，将扩散点设置为潜力点的父亲节点，并计算当前潜力点到根节点的可行路径距离；

步骤三，进入探测循环，直至当前层的潜力点集合为空时才退出循环；在循环中，遍历当前层的所有潜力点，将这些潜力点分别作为扩散点进行扩散操作，扩散得到的新潜力点设为对应扩散点的孩子节点，并标记为当前层的下一层，与之相对应的，将扩散点作为新潜力点的父亲节点；由于不同的扩散点可能扩散得到相同的潜力点，对于同一个潜力点有不同的到达起始点的路径的情况，实际上只有最短的一条路径才有意义，所以此处利用对树结构的修正操作以达到任意一个被探测到的潜力点均只有一个父亲节点；当遍历完当前层的潜力点后，接着遍历下一层的潜力点，直至当前层的潜力点集合为空时，跳出循环；

其中，所述修正操作具体为：扩散点执行扩散操作时，若搜索到的潜力可达点已有父亲节点，且将该潜力可达点沿着原父亲节点到根节点的可行路径距离记为oldDist，沿着当前扩散点到根节点的可行路径距离记为newDist，则对比oldDist和newDist：若oldDist要短，则该潜力可达点的父子关系不变；若newDist要短，则取消原父子关系，将当前扩散点设置为该潜力可达点的父亲，将该潜力可达点设为当前扩散点的孩子。

步骤四，最终得到一棵完整的树，其中根节点为起始点，叶子节点为能够与终止点直连的潜力点，然后遍历每一个叶子节点，利用树的孩子与父亲的连接关系，计算出每一条路径的长度，并从中找出最短路径，结束算法。

优选地，在步骤三的循环中，如果当前扩散点到根节点的距离大于目前已到达终止点的完整路径的距离bestLen，意味着扩散后得到的路径必定长于bestLen，则该扩散点不执行扩散操作。

优选地，在扩散操作中，潜力可达点的邻域的步长可设为1l、2l、3l，邻域指的是以当前点为中心点，以步长的距离沿着纵横两方向构成的区域所覆盖的像素点，邻域的不同步长设置影响探测到的潜力边缘点的数目，最终影响到算法得出的最短路径。

优选地，判断空间中任意两点是否能够直连的具体步骤如下：

取点i和点j的坐标，分别找到x、y、z坐标的最小值lower和最大值upper，然后以步长1，分别对x、y、z从lower_x、lower_y和lower_z进行递增，每一次递增所影响到的另外两个元素由下式求得：

然后对递增后得出的坐标(x₀,y₀,z₀)归约到最近的位置点并进行障碍判断，若该顶点是非可行点，则判定点i和点j之间的线段在点(x₀,y₀,z₀)处触碰到了障碍物，点i和点j不能直连；若该连线在x、y、z三个方向的递增障碍判断中均无触碰到障碍物，则点i和点j能够直连。

本发明与现有技术相比具有以下的有益效果：

(1)本发明在航行器逐步探测海底信息的过程中，可实时运算当前搜索到的路径信息，而不需要待全部海底信息探测完成后再进行运算，节省了在探索过程中浪费的时间；

(2)本发明只记录障碍物的边缘信息，可大大减少存储的成本，提高运算的效率。

(3)本发明不是每次只能探测到当前位置邻域的信息，而是很好地贴合航行器的声纳技术，在当前位置能尽可能远地收集到地形信息并加以分析处理出下一个位置点的情况，提高了探索地形环境的效率；

(4)本发明能得出一个树形结构的搜索结果，层次结构清晰，使用者除了能搜索出最短路径，还能从中分析出地形的特点。

附图说明

图1为本发明的整体流程图；

图2为本发明潜力可达点判定示意图；

图3为本发明圆形障碍物与边缘点的二维俯视图；

图4为本发明判断两点之间是否直连示意图；

图5为本发明扩散操作探测示意图；

图6为本发明邻域范围设置示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

本发明首先对地图空间进行建模，然后基于航行器当前点通过声纳探测地形的潜力可达点，之后每一次基于潜力可达点再进行探测，直到所有潜力可达点探测完毕为止，最后通过探测到的潜力可达点计算出最短路径。

具体来说，如图1～6所示，一种面向海底未知地形搜索的树扩散启发式路径规划方法，包括下述步骤：

(1)对地图空间进行建模；在二维空间中生成O-XYZ坐标系，O为原点，X、Y、Z分别对应X、Y、Z轴，并对空间进行离散化处理，在坐标系中生成density*density*density个像素点，其中density表示密度，每一个像素点代表一个位置点。同时对位置点划分为可达点、不可达点，在算法中将可达点标记为0，将不可达点标记为1。另外，将两点之间的连线划分为可行路径和不可行路径，可行路径是指航行器可通行的路径，不可行路径是指该路径会触碰障碍物。在分析障碍物时，若地形空间的像素点被障碍物囊括，则将该像素点标记为障碍点。对于可达点，对其划分为普通可达点、边缘可达点和潜力可达点，而潜力可达点是边缘可达点的子集。其中边缘可达点的定义是：该可达点的邻域H范围内至少存在一个障碍点。潜力可达点的定义是：该可达点是边缘可达点，并且其邻域H范围内存在至少一个不能与扩散点直连的边缘可达点。如图2所示，当前的扩散点为点A，待判断点B是否为潜力可达点，其中当前邻域H的点集为C、D、E、F、G、H、I、J，发现点G与点A的连线触碰到了障碍物，所以点G不能与点A直连，点B满足潜力可达点的条件。

(2)面对未知的海底空间，航行器需要逐步探测地形环境，并评估可行路径。首先航行器基于起始点进行第一层的探测，即扩散操作，将探测得到的障碍点记录到障碍点集合以及将探测得到的潜力点记录到潜力点集合当中。其中，扩散操作是指：基于一个扩散点，遍历当前已获得的所有像素点信息，判断该像素点是否满足潜力点定义，若满足则将该潜力点设置为扩散点的孩子节点，将扩散点设置为潜力点的父亲节点，并计算当前潜力点到根节点的可行路径距离。

(3)此处进入探测循环，直到当前层的潜力点集合为空时才退出。在循环当中，遍历当前层的所有潜力点，将这些潜力点分别作为扩散点进行扩散操作，扩散得到的新潜力点设为对应扩散点的孩子节点，并标记为当前层的下一层，与之相对应的，将扩散点作为新潜力点的父亲节点。由于不同的扩散点可能扩散得到相同的潜力点，对于同一个潜力点有不同的到达起始点的路径的情况，实际上只有最短的一条路径才有意义，所以此处利用对树结构的修正操作以达到任意一个被探测到的潜力点均只有一个父亲节点。当遍历完当前层的潜力点后，接着遍历下一层的潜力点。直到当前层的潜力点集合为空时，跳出循环。另外在循环当中，如果当前扩散点到根节点的距离大于目前已到达终止点的完整路径的距离bestLen，意味着扩散后得到的路径必定长于bestLen，则该扩散点不执行扩散操作。而循环里的每一次扩散操作当中，均要执行修正操作：扩散点执行扩散操作时，若搜索到的潜力可达点已有父亲节点，且将该潜力可达点沿着原父亲节点到根节点的可行路径距离记为oldDist，沿着当前扩散点到根节点的可行路径距离记为newDist，则对比oldDist和newDist：若oldDist要短，则该潜力可达点的父子关系不变；若newDist要短，则取消原父子关系，将当前扩散点设置为该潜力可达点的父亲，将该潜力可达点设为当前扩散点的孩子。

(4)最终得到一棵完整的树，其中根节点为起始点，叶子节点为能够与终止点直连的潜力点。然后遍历每一个叶子节点，利用树的孩子与父亲的连接关系，计算出每一条路径的长度，并从中找出最短路径，结束算法。

在算法步骤(2)和(3)的扩散操作中，潜力可达点的邻域的步长可设为1l、2l、3l，邻域指的是以当前点为中心点，以步长的距离沿着纵横两方向构成的区域所覆盖的像素点，邻域的不同步长设置影响探测到的潜力边缘点的数目，最终影响到算法得出的最短路径。

在算法步骤(2)、(3)和(4)当中，均要用到空间中任意两点i和j是否直连的判断：取点i和点j的坐标，分别找到x、y、z坐标的最小值lower和最大值upper，然后以步长1，分别对x、y、z从lower_x、lower_y和lower_z进行递增，每一次递增所影响到的另外两个元素由下式求得：

然后对递增后得出的坐标(x₀,y₀,z₀)归约到最近的位置点并进行障碍判断，若该顶点是非可行点，则判定点i和点j之间的线段在点(x₀,y₀,z₀)处触碰到了障碍物，点i和点j不能直连；若该连线在x、y、z三个方向的递增障碍判断中均无触碰到障碍物，则点i和点j能够直连。

下述为本发明的一个具体实施方式：

(1)地图空间建模；

在二维空间中生成O-XYZ坐标系，O为原点，X、Y、Z分别对应X、Y、Z轴，并以步长为1生成分辨率为(density*density*density)个三维像素点，x、y、z坐标的取值范围均是[0,density-1]，并都初始化为普通可达点，之后随着算法的进行逐步划分为普通可达点和不可达点，其中可达点又划分为普通可达点、边缘可达点、潜力可达点，用于分析和搜索出最短路径。以density＝10为例，假定有3个半径长度相同或不同的圆形障碍物，障碍物分布如图3所示，障碍点为圆形内的实心原点，上三角形为起始点，下三角形为终止点，小正方形为潜力可达点。四幅图依次是从起始点开始扩散逐步得到的空间信息。

由于描述一个像素点的位置需要x、y、z坐标，要占用很大的空间而且操作不方便，针对这个问题，本算法将每一个像素点分配一个唯一的空间编号i，其中i∈Z∧i∈[0,density*density*density-1]，之后将像素点归类于不同集合时，直接存储其空间编号即可。其x、y坐标转换成编号i的公式如下：

i＝x+y*density+z*density*density

其编号i转换成x、y坐标的公式如下：

x＝i％density

y＝(i％(density²))/density

z＝i/(density²)

地图构建完毕后，直连的标准定义为两点连线是否不穿过障碍物，若不穿过则表示两点之间可以直连，否则不直连。由于地图是以离散的像素点进行建模的，而两点间的线段是连续的，所以要对此线段作一个等距点的抽样，在尽可能提升判断准确度的同时控制时间运行成本的增加，使两者达到一个良好的平衡。如图4所示，为一个像素点空间，相邻点之间的距离为1，M₂为面ABCD上的点，M₃为面ADHE上的点，M₄为面JEHI上的点，假设要判断M₁和M₅是否能直连，在线段M₁M₅上，对M₁分别向x、y、z方向进行步长为1的递增，判断沿途经过的点是否触碰障碍。先从x轴方向递增开始，依次判断M₁M₂M₄M₅是否触碰障碍；再对y轴方向进行递增，依次判断M₁M₅；最后对z轴方向进行递增，依次判断M₁M₃M₅。标记当中判断过的点，以免下次重复访问。在判断一个点是否触碰障碍物时，将其归约到离自身最近的位置点，若该位置点为障碍点，则判定当前点触碰障碍物。

(2)树扩散启发式；

如图5所示，航行器欲从起始点S到达终止点E，首先通过侦察发现∠BSA方向有障碍物，且对点S进行扩散操作后发现，点A和点B为潜力点，并把这两点设定为扩散点S的孩子节点，将这两点的父亲设置为点S，并计算各自到根节点S的可行距离，分别为0.3。接着点A继续扩散，搜索到点D为潜力点并计算点D到根节点S的可行路径SAD的距离，其长度为0.8。之后点B执行扩散操作，搜索到点C和点D为潜力点，其中点D是已被探测过的潜力点，其原有的到根节点的距离为0.8，而当前探测到的到根节点的可行路径SBD的距离长度为0.7，比SAD短，所以此处执行修正操作，将边AD剪断，即把点D的父亲设为点B，且更新点D到根节点的可行路径长度为0.7，然后将点A的孩子节点D删除。最后对点C和点D执行扩散操作时，发现这两点均可直连终止点E，则停止这两点的扩散。对应的树形数据结构中，点S为根节点，点C和点D均是树的叶子节点，终止点E不在树形结构当中；点S的层数为1，点A和点B的层数为2，点C和点D的层数为3。

邻域范围设置示意图如图6所示，假设当前的扩散点为A，欲判定点B是否为潜力可达点。在1H的领域设置下，点B的邻域有8个点；在2H的邻域设置下，点B的邻域有24个点。可见在潜力可达点的判定中，步长越大，判定越宽松，最终将判定点判定为潜力可达点的机会越大，对地形信息的把握也越大，最终搜索得到的最短路径也更准确。

(3)潜力可达点扩散操作；

待地图构建完后，首先对起始点进行扩散，得到第一批潜力可达点，此时这些潜力可达点均位于树的第二层，而第一层仅有起始点。然后进入潜力可达点的扩散循环，其中，潜力可达点随着扩散操作的进行不断增多，直到不能再探索到新的潜力点为止。而每一次扩散操作得到的潜力可达点所在树的层数均是扩散点的下一层，当当前层所有潜力可达点扩散完毕后，进入下一层继续扩散，直到当前层没有新的潜力可达点时跳出循环，然后计算所有叶子节点对应可行路径的长度并输出最短路径信息。

对于每一次的扩散操作，首先标记当前扩散点已扩散，然后判断扩散点是否能与终止点直连，若能则标记扩散点能与终止点直连，然后计算该完整可行路径的长度，如果该长度小于历史最优可行路径的长度，则把历史最优可行路径的长度设为该完整可行路径的长度，最后退出函数；若扩散点不能直连终止点，则继续执行函数。下面判断当前扩散点到根节点的可行路径长度是否长于历史最优可行路径，若比历史最优可行路径长，则意味着之后构建的完整可行路径必定比历史最优可行路径长，扩散没有意义，退出函数；若比历史最优可行路径短，则遍历当前已获取的位置点，把满足潜力点条件的点加入到候选集。最后把候选集中未访问过的潜力可达点设为扩散点的孩子，并把扩散点设为自身的父亲；对于访问过的潜力可达点，先判断该点到根节点的可行路径距离newDis是否小于该点原来到根节点的可行路径距离oldDis，若是，则执行修正操作，并对该点执行扩散操作；若否，不作任何操作。

(4)计算最短路径长度；

待所有潜力可达点扩散完毕后，通过树的所有叶子节点计算出对应可行路径的长度，然后对比每一条可行路径，找出长度最短的路径作为最短路径输出。

本发明公开的基于树扩散路径优化技术，该技术分为两个阶段：空间建模和路径搜索。在空间建模阶段里，首先对地图空间进行离散化处理，将每一个点定义为像素点，将两点之间所构成的边的关系划分为可连通和不可连通，每一个可行路径均由可连通边构成；然后基于该离散化空间构建O-XYZ坐标系，根据障碍物的形状特性，把相应的像素点标记为障碍点，且确保障碍点围成的面积覆盖原障碍物的面积，保证航行器的行驶安全；待空间建模完成以后，应用树扩散方法进行路径搜索；首先基于起始点进行扩散，得到第一批潜力可达点，然后进入扩散循环，对潜力可达点集合里的点逐一进行扩散，待对应树形结构里的当前层的潜力可达点扩散完后，对下一层的点进行扩散，直到当前层的潜力可达点集合为空时，停止扩散；最后通过树的叶子节点计算出对应的完整可行路径长度，对比各条路径后，输出最短路径，结束算法。本发明提出的方法与用于求解路径规划问题的已有算法相比，具有高效、快速等优点，本发明在航行器逐步探测海底信息的过程中，可实时运算当前搜索到的路径信息，而不需要待全部海底信息探测完成后再进行运算，节省了在探索过程中浪费的时间；只记录障碍物的边缘信息，可大大减少存储的成本，提高运算的效率；不是每次只能探测到当前位置邻域的信息，而是很好地贴合航行器的声纳技术，在当前位置能尽可能远地收集到地形信息并加以分析处理出下一个位置点的情况，提高了探索地形环境的效率；能得出一个树形结构的搜索结果，层次结构清晰，使用者除了能搜索出最短路径，还能从中分析出地形的特点。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.一种面向海底未知地形搜索的树扩散启发式路径规划方法，其特征在于，包括下述步骤：

步骤一，对地图空间进行建模；在二维空间中生成O-XYZ坐标系，O为原点，X、Y、Z分别对应X、Y、Z轴，并对空间进行离散化处理，在坐标系中生成density*density*density个像素点，其中density表示密度，每一个像素点代表一个位置点；同时对位置点划分为可达点和不可达点，在算法中将可达点标记为0，将不可达点标记为1；将两点之间的连线划分为可行路径和不可行路径，可行路径是指航行器可通行的路径，不可行路径是指该路径会触碰障碍物；在分析障碍物时，若地形空间的像素点被障碍物囊括，则将该像素点标记为障碍点；对于可达点，对其划分为普通可达点、边缘可达点和潜力可达点，而潜力可达点是边缘可达点的子集；

步骤二，面对未知的海底空间，航行器需要逐步探测地形环境，并评估可行路径；航行器基于起始点进行第一层的探测，即扩散操作，将探测得到的障碍点记录到障碍点集合，以及将探测得到的潜力点记录到潜力点集合；

其中，所述扩散操作具体为：基于一个扩散点，遍历当前已获得的所有像素点信息，判断该像素点是否满足潜力点定义，若满足则将该潜力点设置为扩散点的孩子节点，将扩散点设置为潜力点的父亲节点，并计算当前潜力点到根节点的可行路径距离；

步骤三，进入探测循环，直至当前层的潜力点集合为空时才退出循环；在循环中，遍历当前层的所有潜力点，将这些潜力点分别作为扩散点进行扩散操作，扩散得到的新潜力点设为对应扩散点的孩子节点，并标记为当前层的下一层，与之相对应的，将扩散点作为新潜力点的父亲节点；由于不同的扩散点可能扩散得到相同的潜力点，对于同一个潜力点有不同的到达起始点的路径的情况，实际上只有最短的一条路径才有意义，所以此处利用对树结构的修正操作以达到任意一个被探测到的潜力点均只有一个父亲节点；当遍历完当前层的潜力点后，接着遍历下一层的潜力点，直至当前层的潜力点集合为空时，跳出循环；

其中，所述修正操作具体为：扩散点执行扩散操作时，若搜索到的潜力可达点已有父亲节点，且将该潜力可达点沿着原父亲节点到根节点的可行路径距离记为oldDist，沿着当前扩散点到根节点的可行路径距离记为newDist，则对比oldDist和newDist：若oldDist要短，则该潜力可达点的父子关系不变；若newDist要短，则取消原父子关系，将当前扩散点设置为该潜力可达点的父亲，将该潜力可达点设为当前扩散点的孩子；

步骤四，最终得到一棵完整的树，其中根节点为起始点，叶子节点为能够与终止点直连的潜力点，然后遍历每一个叶子节点，利用树的孩子与父亲的连接关系，计算出每一条路径的长度，并从中找出最短路径，结束算法。

2.根据权利要求1所述的面向海底未知地形搜索的树扩散启发式路径规划方法，其特征在于，在步骤三的循环中，如果当前扩散点到根节点的距离大于目前已到达终止点的完整路径的距离bestLen，意味着扩散后得到的路径必定长于bestLen，则该扩散点不执行扩散操作。

3.根据权利要求1所述的面向海底未知地形搜索的树扩散启发式路径规划方法，其特征在于，在扩散操作中，潜力可达点的邻域的步长可设为1l、2l、3l，邻域指的是以当前点为中心点，以步长的距离沿着纵横两方向构成的区域所覆盖的像素点，邻域的不同步长设置影响探测到的潜力边缘点的数目，最终影响到算法得出的最短路径。

4.根据权利要求1所述的面向海底未知地形搜索的树扩散启发式路径规划方法，其特征在于，判断空间中任意两点是否能够直连的具体步骤如下：

取点i和点j的坐标，分别找到x、y、z坐标的最小值lower和最大值upper，然后以步长1，分别对x、y、z从lower_x、lower_y和lower_z进行递增，每一次递增所影响到的另外两个元素由下式求得：

然后对递增后得出的坐标(x₀,y₀,z₀)归约到最近的位置点并进行障碍判断，若该顶点是非可行点，则判定点i和点j之间的线段在点(x₀,y₀,z₀)处触碰到了障碍物，点i和点j不能直连；若该连线在x、y、z三个方向的递增障碍判断中均无触碰到障碍物，则点i和点j能够直连。