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

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

An Optimal Path Planning and Obstacle Avoidance Design Method for Underwater Vehicles

The invention discloses an optimal path planning and obstacle avoidance design method for an underwater vehicle, which is based on the combination of binary reactor acceleration algorithm, 26-neighborhood NA* algorithm and arc-straight-arc turning strategy for underwater vehicle optimal path planning and obstacle avoidance. The invention first establishes the underwater environment model by discrete three-dimensional grid graph, which divides the underwater environment into several cells of the same size; then uses 26-neighborhood NA* algorithm to plan the path, eliminating the sawtooth effect of the path and avoiding partial collision; and then applies the \arc-straight line-arc\ turning strategy to avoid the influence of the body width and turning radius of the vehicle. The advantages of the present invention are that a 26-neighborhood NA* algorithm for three-dimensional scene is proposed and applied to autonomous navigation of underwater vehicle. Considering and utilizing the characteristics of large turning radius of underwater vehicle, collision caused by the width of the vehicle itself is effectively avoided, and the planned path is smoother with fewer turning points.

【技术实现步骤摘要】
一种水下航行器的最优路径规划与避障设计方法
本专利技术涉及一种最优路径规划方法，尤其涉及一种水下航行器的最优路径规划与避障设计方法。
技术介绍
传统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＝ar本文档来自技高网...

【技术保护点】
1.一种水下航行器的最优路径规划与避障设计方法，其特征在于，包括步骤：(1)将水下环境建立离散三维网格图；(2)基于三维场景利用NA*算法获取最佳路径；(3)利用“圆弧‑直线‑圆弧”转弯策略进行导航。

【技术特征摘要】
1.一种水下航行器的最优路径规划与避障设计方法，其特征在于，包括步骤：(1)将水下环境建立离散三维网格图；(2)基于三维场景利用NA*算法获取最佳路径；(3)利用“圆弧-直线-圆弧”转弯策略进行导航。2.根据权利要求1所述的水下航行器的最优路径规划与避障设计方法，其特征在于，所述步骤1中，采用离散网格图将水下环境划分为若干个相同大小的深色单元格和透明单元格；深色单元格表示障碍物，透明单元格表示无障碍物空间。3.根据权利要求2所述的水下航行器的最优路径规划与避障设计方法，其特征在于，所述步骤2中，NA*算法是通过选择合适的估价准则，综合判断所需搜索节点的代价值大小，并根据评估大小，选择权值最小节点进行下步操作，直至到达目标节点。4.根据权利要求3所述的水下航行器的最优路径规划与避障设计方法，其特征在于，所述步骤3中，水下航行器的每一次转弯路径都由固定的第一段圆弧、中间处的直线段和第二段圆弧共三段组成。5.根据权利要求1或4所述的水下航行器的最优路径规划与避障设计方法，其特征在于，所述水下航行器的最优路径规划与避障设计方法采用二叉堆加速算法。6.根据权利要求5所述的水下航行器的最优路径规划与避障设计方法，其...

【专利技术属性】
技术研发人员：黄浩乾，史仍笃，周军，王冰，刘志强，黄铁，伍光胜，
申请(专利权)人：河海大学，
类型：发明
国别省市：江苏,32