CN109470249B - 一种水下航行器的最优路径规划与避障设计方法 - Google Patents

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

Info

Publication number
CN109470249B
CN109470249B CN201811316388.0A CN201811316388A CN109470249B CN 109470249 B CN109470249 B CN 109470249B CN 201811316388 A CN201811316388 A CN 201811316388A CN 109470249 B CN109470249 B CN 109470249B
Authority
CN
China
Prior art keywords
node
value
underwater vehicle
algorithm
path
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811316388.0A
Other languages
English (en)
Other versions
CN109470249A (zh
Inventor
黄浩乾
史仍笃
周军
王冰
刘志强
黄铁
伍光胜
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hohai University HHU
Original Assignee
Hohai University HHU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hohai University HHU filed Critical Hohai University HHU
Priority to CN201811316388.0A priority Critical patent/CN109470249B/zh
Publication of CN109470249A publication Critical patent/CN109470249A/zh
Application granted granted Critical
Publication of CN109470249B publication Critical patent/CN109470249B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

一种水下航行器的最优路径规划与避障设计方法
技术领域
本发明涉及一种最优路径规划方法，尤其涉及一种水下航行器的最优路径规划与避障设计方法。
背景技术
传统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值。
进一步地，减少拐点的路径平滑方法：对当前节点，将当前节点与视野范围所能到达的最远节点直接连通并舍弃中间节点，依此继续分析直至扫描路径结束。
进一步地，水下航行器路径包括两种情况：起始和到达的方向相同，都为顺时针方向/逆时针方向；起始和到达的方向是相对的，起始时是顺时针/逆时针方向，到达时是逆时针/顺时针方向。
有益效果：本发明与现有技术相比的优点在于：利用水下航行器转弯半径较大的特点，避免因为航行器自身宽度造成的碰撞；基于所提出的路径平滑原则及“圆弧－直线－圆弧”转弯策略，可以有效减少水下规划路径拐点；基于二叉堆的加速方法，计算量更小，用时更短。
附图说明
图1是本发明水下航行器的最优路径规划与避障设计方法原理示意图；
图2是离散三维网格图示意图；
图3是“圆弧-直线-圆弧”转弯策略几何关系示意图，(a)起始和到达的方向相同，(b)起始和到达的方向相反；
图4是基于二叉堆的加速方法示意图，(a)二叉树表示形式的二叉堆示意图，(b)一维数组表示形式的二叉堆示意图，(c)往堆中添加新元素示意图。
具体实施方式
下面结合附图和实施例对本发明的技术方案作进一步的说明。
如图1所示，本发明所述的水下航行器的最优路径规划与避障设计方法，是由离散三维网格建模、NA*算法、“圆弧-直线-圆弧”转弯策略、基于二叉堆的加速方法融合而成的路径规划方法，具体步骤包括：
(1)建立离散三维网格图；
采用离散三维网格图将水下环境划分为若干个相同大小的深色单元格和透明单元格，如图2所示，深色单元格(M、N、U、V)表示障碍物，透明单元格表示无障碍物空间。
离散三维网格图中的每个节点的位置由三维坐标n(xn,yn,zn)表示，则任意两节点a、b间的距离pab表示为：
Figure GDA0001923793450000031
(2)基于三维场景，提出NA*算法，依据26邻域搜索节点，由估价准则来判断代价大小，最终获取最佳路径；
估价准则为：
f(n)＝g(n)+h(n) (2)
式(2)中，n代表离散三维网格图中的路径规划可选择节点，其三维坐标为n(xn,yn,zn)；g(n)为初始节点到计算节点的真实代价值，h(n)为计算节点到目标节点的最小路径的估计代价，f(n)为估价函数的估计代价。
选择欧几里得距离作为启发式函数：
Figure GDA0001923793450000032
式(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值。
本加速算法对于包含大量节点的地图加速效果更明显，可以有效提高路径规划算法计算效率。
本发明的水下航行器的最优路径规划与避障设计方法，克服了传统路径规划算法拐点多、易发生碰撞的特点，考虑并利用了水下航行器转弯半径较大的特点，可以有效减少所规划路径拐点，解决了水下未知环境最优路径规划与避障问题。

Claims (3)

1.一种水下航行器的最优路径规划与避障设计方法，其特征在于，包括步骤：
(1)将水下环境建立离散三维网格图；
(2)基于三维场景利用NA*算法获取最优路径；
(3)利用“圆弧-直线-圆弧”转弯策略进行导航；
所述NA*算法是通过选择合适的估价准则，综合判断所需搜索节点的代价值大小，并根据评估大小，选择代价值最小节点进行下步操作，直至到达目标节点；
所述估价准则为：
f(n)＝g(n)+h(n) (2)
其中，n代表离散三维网格图中的路径规划可选择节点，其三维坐标为n(xn,yn,zn)；g(n)为初始节点到计算节点的真实代价值，h(n)为计算节点到目标节点的最小路径的估计代价，f(n)为估价函数的估计代价；选择欧几里得距离作为启发式函数：
Figure FDA0003096841320000011
其中，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所述的水下航行器的最优路径规划与避障设计方法，其特征在于，水下航行器路径包括两种情况：起始和到达的方向相同；起始和到达的方向是相对的。
CN201811316388.0A 2018-11-07 2018-11-07 一种水下航行器的最优路径规划与避障设计方法 Active CN109470249B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811316388.0A CN109470249B (zh) 2018-11-07 2018-11-07 一种水下航行器的最优路径规划与避障设计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811316388.0A CN109470249B (zh) 2018-11-07 2018-11-07 一种水下航行器的最优路径规划与避障设计方法

Publications (2)

Publication Number Publication Date
CN109470249A CN109470249A (zh) 2019-03-15
CN109470249B true CN109470249B (zh) 2021-07-27

Family

ID=65667025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811316388.0A Active CN109470249B (zh) 2018-11-07 2018-11-07 一种水下航行器的最优路径规划与避障设计方法

Country Status (1)

Country Link
CN (1) CN109470249B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111007518B (zh) * 2019-12-11 2023-05-26 南京工程学院 基于声纳图像处理的水下机器人水下定位和路径规划方法
CN111829528B (zh) * 2020-07-27 2022-02-08 中国科学院自动化研究所 仿生滑翔机器海豚的实时路径规划方法及系统
CN113486445B (zh) * 2021-07-05 2022-04-15 湖南国天电子科技有限公司 一种多个自主式水下航行器协同作业防碰撞方法及系统
CN116127611B (zh) * 2023-04-13 2023-06-20 中国人民解放军国防科技大学 一种水下航行器动态仿真方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106125764A (zh) * 2016-08-03 2016-11-16 西北工业大学 基于a*搜索的无人机路径动态规划方法
CN106441303A (zh) * 2016-09-30 2017-02-22 哈尔滨工程大学 一种基于可搜索连续邻域a*算法的路径规划方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104075717A (zh) * 2014-01-21 2014-10-01 武汉吉嘉伟业科技发展有限公司 一种基于改进a*算法的无人机航线规划算法
CN103900573B (zh) * 2014-03-27 2017-01-04 哈尔滨工程大学 一种基于s57标准电子海图的水下运载器多约束航路规划方法
CN104268420A (zh) * 2014-10-10 2015-01-07 重庆邮电大学 基于二叉堆节点排序的a星寻路方法及系统
CN104991895A (zh) * 2015-05-15 2015-10-21 南京航空航天大学 一种基于三维空域网格的低空救援航空器航迹规划方法
US10248925B2 (en) * 2016-12-06 2019-04-02 Walmart Apollo, Llc Systems and methods for compressing shortest path matrices for delivery route optimization
CN108205322A (zh) * 2017-12-29 2018-06-26 西安智加科技有限公司 一种不借助地图自动生成作业路径的农机作业方法及系统
CN108274465A (zh) * 2018-01-10 2018-07-13 上海理工大学 基于优化a*的人工势场机械臂三维避障路径规划方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106125764A (zh) * 2016-08-03 2016-11-16 西北工业大学 基于a*搜索的无人机路径动态规划方法
CN106441303A (zh) * 2016-09-30 2017-02-22 哈尔滨工程大学 一种基于可搜索连续邻域a*算法的路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
3D large grid route planner for the autonomous underwater vehicles;Hua Cao 等;《International Journal of Intelligent Computing and Cybernetics》;20091231;第2卷(第3期);第455-476页 *
优化A*算法在航迹规划上的应用;姚雨 等;《微电子学与计算机》;20170731;第34卷(第7期);第51-55页 *

Also Published As

Publication number Publication date
CN109470249A (zh) 2019-03-15

Similar Documents

Publication Publication Date Title
CN109470249B (zh) 一种水下航行器的最优路径规划与避障设计方法
CN110006430B (zh) 一种航迹规划算法的优化方法
Niu et al. Voronoi-visibility roadmap-based path planning algorithm for unmanned surface vehicles
CN111811514B (zh) 一种基于正六边形栅格跳点搜索算法的路径规划方法
Carsten et al. 3d field d: Improved path planning and replanning in three dimensions
CN111679692A (zh) 一种基于改进A-star算法的无人机路径规划方法
CN110196598B (zh) 一种无人艇的动态避碰方法
CN110908386B (zh) 一种无人车分层路径规划方法
CN109655063B (zh) 大型水陆两栖飞机海上搜索航路规划方法
CN109214596B (zh) 求取具有方向约束和障碍限制的栅格最短路径规划方法
CN111721296B (zh) 一种水下无人航行器数据驱动路径规划方法
CN115164907B (zh) 基于动态权重的a*算法的森林作业机器人路径规划方法
CN115167398A (zh) 一种基于改进a星算法的无人船路径规划方法
Chen et al. Research on ship meteorological route based on A-star algorithm
CN114281084B (zh) 一种基于改进a*算法的智能车全局路径规划方法
CN114705196B (zh) 一种用于机器人的自适应启发式全局路径规划方法与系统
CN115951682A (zh) 一种考虑洋流存在条件下的带约束四目标auv全局路径规划方法
CN115061468A (zh) 无人船编队队形分离与恢复方法
Li et al. Research on AUV path planning based on improved ant colony algorithm
CN114353814A (zh) 基于Angle-Propagation Theta*算法改进的JPS路径优化方法
CN113110530A (zh) 一种针对三维环境下的水下机器人路径规划方法
Chen et al. GVD-Exploration: An Efficient Autonomous Robot Exploration Framework Based on Fast Generalized Voronoi Diagram Extraction
Chang et al. On-road Trajectory Planning with Spatio-temporal Informed RRT
Liao et al. USV path planning based on sparse visibility graph
CN116578098B (zh) 一种自适应学习的无人艇控制方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant