1.一种水下航行器的最优路径规划与避障设计方法,其特征在于,包括步骤:(1)将水下环境建立离散三维网格图;
(2)基于三维场景利用NA*算法获取最优路径;
(3)利用“圆弧‑直线‑圆弧”转弯策略进行导航;
所述NA*算法是通过选择合适的估价准则,综合判断所需搜索节点的代价值大小,并根据评估大小,选择代价值最小节点进行下步操作,直至到达目标节点;
所述估价准则为:
f(n)=g(n)+h(n) (2)其中,n代表离散三维网格图中的路径规划可选择节点,其三维坐标为n(xn,yn,zn);g(n)为初始节点到计算节点的真实代价值,h(n)为计算节点到目标节点的最小路径的估计代价,f(n)为估价函数的估计代价;选择欧几里得距离作为启发式函数:其中,D表示从一个节点移动到邻近节点的最小代价,h(a,b)表示a节点与b节点之间的欧几里得距离;
所述获取最优路径即减少拐点,对当前节点与视野范围所能到达的最远节点直接连通并舍弃中间节点,依此继续分析直至扫描路径结束;
所述转弯策略为水下航行器的每一次转弯路径都由固定的第一段圆弧、中间处的直线段和第二段圆弧共三段组成;
所述水下航行器的最优路径规划与避障设计方法采用二叉堆加速算法;
所述二叉堆加速算法包括以下步骤:
1)建立一个初始二叉堆并计算根节点f值;
2)将二叉堆简化成一维数组的形式;
3)往堆里增添新元素:添加新节点时将其置于数组末端,然后将新节点的f值和其父节点的f值比较,如果新节点的f值更低,则交换这两个元素的位置;重复上述步骤,直到该节点的f值不再低于该节点的父节点的f值,或这个元素已经到达一维数组的顶端,处于数组的位置1;
4)删除节点:删除位置1的元素,然后取一维数组尾端的一个元素,移动到位置1;比较该元素和该元素两个子节点的f值,如果该元素的f值比两个子节点的f值高,则把该元素的位置和f值较低的子节点位置交换;然后重复以上步骤,直到该元素到达一维数组的尾端或该元素的f不再高于两个子节点的f值。
2.根据权利要求1所述的水下航行器的最优路径规划与避障设计方法,其特征在于,所述步骤(1)中,采用离散网格图将水下环境划分为若干个相同大小的深色单元格和透明单元格;深色单元格表示障碍物,透明单元格表示无障碍物空间。
3.根据权利要求1所述的水下航行器的最优路径规划与避障设计方法,其特征在于,水下航行器路径包括两种情况:起始和到达的方向相同;起始和到达的方向是相对的。