一种水下航行器的最优路径规划与避障设计方法与流程

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

背景技术：

传统a*算法是结合bfs算法(breadth-firstsearch算法)和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值。

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

进一步地，水下航行器路径包括两种情况：起始和到达的方向相同，都为顺时针方向/逆时针方向；起始和到达的方向是相对的，起始时是顺时针/逆时针方向，到达时是逆时针/顺时针方向。

有益效果：本发明与现有技术相比的优点在于：利用水下航行器转弯半径较大的特点，避免因为航行器自身宽度造成的碰撞；基于所提出的路径平滑原则及“圆弧－直线－圆弧”转弯策略，可以有效减少水下规划路径拐点；基于二叉堆的加速方法，计算量更小，用时更短。

附图说明

图1是本发明水下航行器的最优路径规划与避障设计方法原理示意图；

图2是离散三维网格图示意图；

图3是“圆弧-直线-圆弧”转弯策略几何关系示意图，(a)起始和到达的方向相同，(b)起始和到达的方向相反；

图4是基于二叉堆的加速方法示意图，(a)二叉树表示形式的二叉堆示意图，(b)一维数组表示形式的二叉堆示意图，(c)往堆中添加新元素示意图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

如图1所示，本发明所述的水下航行器的最优路径规划与避障设计方法，是由离散三维网格建模、na*算法、“圆弧-直线-圆弧”转弯策略、基于二叉堆的加速方法融合而成的路径规划方法，具体步骤包括：

(1)建立离散三维网格图；

采用离散三维网格图将水下环境划分为若干个相同大小的深色单元格和透明单元格，如图2所示，深色单元格(m、n、u、v)表示障碍物，透明单元格表示无障碍物空间。

离散三维网格图中的每个节点的位置由三维坐标n(xn,yn,zn)表示，则任意两节点a、b间的距离pab表示为：

(2)基于三维场景，提出na*算法，依据26邻域搜索节点，由估价准则来判断代价大小，最终获取最佳路径；

估价准则为：

f(n)＝g(n)+h(n)(2)

式(2)中，n代表离散三维网格图中的路径规划可选择节点，其三维坐标为n(xn,yn,zn)；g(n)为初始节点到计算节点的真实代价值，h(n)为计算节点到目标节点的最小路径的估计代价，f(n)为估价函数的估计代价。

选择欧几里得距离作为启发式函数：

式(3)中，d表示从一个节点移动到邻近节点的最小代价，h(a,b)表示a节点与b节点之间的欧几里得距离。

结合式(2)、式(3)综合判断所需搜索节点的代价值大小，并根据评估大小，选择代价值最小节点进行下步操作，直至到达目标点。

na*算法的基本思想是通过选择合适的估计准则，综合判断所需搜索节点的代价值大小，并根据评估大小，选择权值最小节点进行下步操作，直至到达目标节点。由于na*算法依据26邻域搜索节点，造成路径折线长、拐点多，导航耗时较长。

为获取最优路径，减少拐点，提出一种路径平滑方法：对当前节点，将当前节点与视野范围所能到达的最远节点直接连通并舍弃中间节点，依此继续分析直至扫描路径结束。减少节点的数量，加快计算速度。如果是一条线段的话，由四个点组成，则只取端点，省略中间两点。其次，对于任意一个节点，判断其是否可直接到达其视野范围所能到达的最远节点，如果可以，那么就移除中间点，将该节点与其视野范围所能到达的最远节点直接连通。

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

充分考虑水下航行器自身的宽度及其较大的转弯半径，本发明提出一种水下“圆弧-直线-圆弧”转弯策略，避免突然变向和碰撞。仅需已知起始点的方向、位置和航行器转弯半径以及到达目标点时的方向，水下航行器的每一次转弯路径都由固定的第一段圆弧、中间处的直线段和第二段圆弧一共三段组成。

算法需计算的变量为：圆弧转弯路径时，则计算转弯圆的圆心、圆弧所包含的弧度、圆弧开始的角度；直线转弯路径时，需计算角度、长度。一般，需考虑两种情况，如图3所示。

第一种是起始和到达的方向相同，都为顺时针方向。此时，

lp1p2＝lab(4)

kp1p2＝kab(5)

θarc1＝arctan(kab)(6)

θarc2＝θarc1(7)

式(4)中，lp1p2为点p1到p2的距离，lab为点a到点b的距离；式(5)中，kp1p2为直线p1p2的斜率，kab为直线ab的斜率；式(6)中，θarc1为离开第一段弧时的角度；式(7)中，θarc2为到达第二段弧时的角度。

第二种是起始和到达的方向是相对的，起始时是顺时针方向，到达时是逆时针方向。为了简化计算，相对目标圆作相同圆(圆心p3)与其正切与点b，此时第三圆与起始圆的关系就演变成第一种情况。此时，计算方案为：

lp2p3＝2r(8)

θp1p3p2＝arctan(lp1p2/2r)(9)

θab＝arctan(kp1p3)±(90°-θp1p3p2)(10)

式(8)中，lp2p3为点p2到p3的距离，r为圆弧半径，式(9)中θp1p3p2为角p1p3p2的度数，式(10)中，θab为直线ab的角度。至此，可以获取离开第一段弧和到达第二段弧的角度。

综上，己知位置和方向的起始点和目标点，运用“圆弧-直线-圆弧”转弯策略获取得到最短路径，并避免航行器本体宽度造成的碰撞，当前路径中任意时刻的位置和方向都是可求得的。

(4)为解决26邻域na*算法耗时较长的问题，采用基于二叉堆加速算法提高整体算法的计算效率。

基于二叉堆加速算法具体包括步骤：

(4.1)建立一个初始二叉堆并计算根节点f值，如图4(a)所示；

(4.2)将二叉堆简化成一维数组的形式，省去考虑二叉堆的指针问题，如图4(b)所示；

(4.3)往堆里增添新元素：根节点位置为1，其余元素的位置用m指代，则任何元素的两个子节点的位置可以表示为2m(当前元素的位置m乘以2)和2m+1(当前元素的位置m乘以2加1)，对于位置为2m或2m+1的子节点而言，位置为m的元素为父节点。添加新节点时将其置于数组末端，然后将新节点的f值和其父节点的f值比较，如果新节点的f值更低，则交换这两个元素的位置。然后重复上述步骤，直到该节点的f值不再低于该节点的父节点的f值，或这个元素已经到达一维数组的顶端，处于数组的位置1，如图4(c)所示。

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

本加速算法对于包含大量节点的地图加速效果更明显，可以有效提高路径规划算法计算效率。

本发明的水下航行器的最优路径规划与避障设计方法，克服了传统路径规划算法拐点多、易发生碰撞的特点，考虑并利用了水下航行器转弯半径较大的特点，可以有效减少所规划路径拐点，解决了水下未知环境最优路径规划与避障问题。