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

权利要求书

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