[发明专利]一种水下航行器的最优路径规划与避障设计方法

说明书

本发明公开了一种水下航行器的最优路径规划与避障设计方法，是基于二叉堆加速算法、26邻域NA*算法与“圆弧‑直线‑圆弧”转弯策略相融合进行的水下航行器最优路径规划与避障。本发明首先采用离散三维网格图的方式建立水下环境模型，网格图将水下环境划分为若干相同大小的单元格；然后采用26邻域NA*算法规划路径，消除路径锯齿效应并避免部分碰撞；再应用“圆弧‑直线‑圆弧”转弯策略，避免航行器本体宽度以及转弯半径的影响。本发明的优点在于提出了面向三维场景的26邻域NA*算法并将其应用于水下航行器自主导航，考虑并利用了水下航行器转弯半径较大的特点，有效避免因为航行器自身宽度造成的碰撞，所规划路径较平滑且拐点较少。

技术领域

本发明涉及一种最优路径规划方法，尤其涉及一种水下航行器的最优路径规划与避障设计方法。

背景技术

传统A*算法是结合BFS算法(Breadth-First Search算法)和Dijkstra算法(最短路径算法)的优点发展而来，依据8邻域搜索节点，是一种全局路径规划方法，针对于二维场景。但传统A*算法未考虑航行器本体宽度。同时，由于传统A*算法依据多邻域搜索节点，所以会产生锯齿效应，所规划路径折线长、拐点多。传统A*算法只针对二维环境，将A*算法扩展为可在水下三维环境中应用的NA*算法进行水下航行器最优路径规划，并且同时满足路径最优、拐点少、无碰撞的要求，在国内外鲜有报道。

发明内容

发明目的：针对以上问题，本发明提出一种水下航行器的最优路径规划与避障设计方法，该方法所规划路径满足平滑要求且能避免因为航行器自身宽度造成的碰撞，可以有效减少所规划路径拐点，实现水下环境最优路径规划与自主导航。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种水下航行器的最优路径规划与避障设计方法，包括步骤：

(1)将水下环境建立离散三维网格图；

(2)基于三维场景利用NA*算法获取最佳路径；

(3)利用“圆弧-直线-圆弧”转弯策略进行导航。

进一步地，所述步骤1中，采用离散网格图将水下环境划分为若干个相同大小的深色单元格和透明单元格；深色单元格表示障碍物，透明单元格表示无障碍物空间。

进一步地，所述步骤2中，NA*算法是通过选择合适的估价准则，综合判断所需搜索节点的代价值大小，并根据评估大小，选择权值最小节点进行下步操作，直至到达目标节点。

进一步地，所述步骤3中，水下航行器的每一次转弯路径都由固定的第一段圆弧、中间处的直线段和第二段圆弧共三段组成。

进一步地，所述水下航行器的最优路径规划与避障设计方法采用二叉堆加速算法。

进一步地，所述二叉堆加速算法包括步骤：

(4.1)建立一个初始二叉堆并计算根节点f值；

(4.2)将二叉堆简化成一维数组的形式；

(4.3)往堆里增添新元素：添加新节点时将其置于数组末端，然后将新节点的f值和其父节点的f值比较，如果新节点的f值更低，则交换这两个元素的位置；重复上述步骤，直到该节点的f值不再低于该节点的父节点的f值，或这个元素已经到达一维数组的顶端，处于数组的位置1；

(4.4)删除节点：删除位置1的元素，然后取一维数组尾端的一个元素，移动到位置1；比较该元素和该元素两个子节点的f值，如果该元素的f值比两个子节点的f值高，则把该元素的位置和f值较低的子节点位置交换；然后重复以上步骤，直到该元素到达一维数组的尾端或该元素的f不再高于两个子节点的f值。

进一步地，减少拐点的路径平滑方法：对当前节点，将当前节点与视野范围所能到达的最远节点直接连通并舍弃中间节点，依此继续分析直至扫描路径结束。