CN107966153A - 水下航行器路径规划算法 - Google Patents
水下航行器路径规划算法 Download PDFInfo
- Publication number
- CN107966153A CN107966153A CN201711193718.7A CN201711193718A CN107966153A CN 107966153 A CN107966153 A CN 107966153A CN 201711193718 A CN201711193718 A CN 201711193718A CN 107966153 A CN107966153 A CN 107966153A
- Authority
- CN
- China
- Prior art keywords
- mrow
- obstacle
- flow field
- msub
- mtd
- 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.)
- Granted
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 24
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 28
- 238000012937 correction Methods 0.000 claims description 27
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000006243 chemical reaction Methods 0.000 claims description 16
- 230000003068 static effect Effects 0.000 claims description 11
- 238000013139 quantization Methods 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 230000001419 dependent effect Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 239000004576 sand Substances 0.000 claims description 3
- 238000000034 method Methods 0.000 abstract description 25
- 230000004888 barrier function Effects 0.000 description 9
- 238000005457 optimization Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 241000251468 Actinopterygii Species 0.000 description 1
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 239000008239 natural water Substances 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000001308 synthesis method Methods 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
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
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
水下航行器路径规划算法，用于规划水下航行器的航行路径。该方法将水下障碍均近似等效为几何凸面体，建立以水下航行器航行目标点为中心的汇流场，构建自由环境下航行目标点的初始流场并引入水下障碍物的影响对初始流场进行校正，获得修正流场，并基于修正力促行获得水下航行器的路径点，从而获得航行路径。本发明规划的路径质量较高，水下航行器总能安全避开障碍物并最终到达目标点。为水下航行器自主控制系统搭载该路径规划算法，可进一步提高水下自主航行器的自主感知和决策控制能力，提高自主航行能力。
Description
技术领域
本发明涉及航行器导航与控制技术领域，具体为一种水下航行器路径规划算法。
背景技术
自主式水下航行器(Autonomous Underwater Vehicle，以下简称AUV)是一种具备自主导航与规划能力、可代替人类执行水下作业任务的无人运载平台，相比于载人潜水器或有缆型无人航行器，具有隐蔽性好、机动性高、使用灵活、活动范围广等独特优势，代表了水下航行器的未来发展方向，并已广泛应用于各类军用与民用领域。然而，随着AUV应用领域与应用需求的不断扩大，AUV的任务场景与任务要求等也日益复杂，一些与新应用不相适应的技术问题也日渐暴露，成为制约其发展的瓶颈。
AUV作为一类典型的自主无人系统，智能化与自主化是其本质特征与未来趋势。然而，现有AUV的自主能力受限，某些情况下还需由岸上或船上控制站全时监控并发送控制指令，任务执行效率低且难以保证安全性。因此，如何进一步提高AUV自主航行能力，实现AUV自主感知、决策与控制，是保证AUV满足复杂任务应用需求的关键。结合近年来AUV领域的发展趋势可知，路径规划技术是提高其自主能力和航行安全的关键技术之一。
AUV路径规划是指依据已知环境信息或从传感器(如前视声呐、高频雷达等)实时探测的环境信息(如障碍信息、洋流信息等)，在安全躲避各类障碍威胁的前提下，规划一条从起始点到目标点、满足AUV性能约束、某任务代价指标下最优(如能量消耗最少、航行时间最短等)的路径。该问题的核心思路主要包括环境建模与优化搜索两部分：首先，按照某些规则将规划空间划分，从而将路径规划问题建模为特定空间下的优化搜索问题；然后，采用合适的优化搜索算法，寻找在某指标下的可行最优路径。国内外学者对AUV路径规划问题从不同角度进行了大量的研究，从规划空间建模的角度出发，主要包括基于图形的方法、空间分解法、随机规划法、数学规划法、人工势场法。
AUV路径规划技术虽然已取得了大量研究成果，但大多仅适用于无障碍或稀疏障碍环境下，而复杂海洋环境下的AUV路径规划问题一直未得到有效的解决。该问题的难点主要体现在海洋环境具有复杂性，具体包括三维搜索空间、环境非结构化(例如包含多种类型的密集水下障碍物、存在非凸区域等)、环境动态性(例如存在动态洋流、运动威胁等)、环境不确定性(规划空间信息部分或完全未知)等，而现有方法存在较大的局限性，难以兼顾路径最优性、可行性、实时性、复杂环境约束、AUV性能约束等要求，因此AUV在复杂海洋环境下的路径规划能力仍有很大的提升空间。
发明内容
本发明的目的在于结合水下的环境特点，提出一种适用于水下航行器的路径规划算法，使水下航行器自动适应各类静态障碍、动态威胁的复杂水下环境。
为了实现以上目的，本发明提供如下的技术方案：
水下航行器路径规划算法，用于规划水下航行器的航行路径，包括以下步骤：
S1：将水下障碍均近似等效为几何凸面体，采用统一的凸面体方程进行描述：
其中，a,b,c,p,q,r为决定几何凸面体大小和形状的常数，(x0,y0,z0)表示几何凸面体的中心，Γ(P)＝1为障碍物表面方程；
S2：建立以水下航行器航行目标点为中心的汇流场：
定义该汇流场的速度为其中V0为汇流场的速率，d(P,Pd)表示水下航行器当前位置与航行目标位置的欧式距离；
S3：构建自由环境下航行目标点的初始流场：
假设水流流场为恒速流场，水流流速为uc(P)，将水流流速与汇流场流速按权重叠加，u(P)＝λsus(P)+λcuc(P)，其中u(P)为初始流场的流速，λs和λc分别表示汇流场和水流流场的权重系数；
S4：定义水下障碍物的修正矩阵，以定义水下障碍物对航行目标点初始流场的扰动影响：
其中，K为障碍物的总数，ρk、σk分别表示第k个障碍物的排斥反应系数和切向反应系数，nk(P)表示第k个障碍物的径向法向量，tk(P)表示第k个障碍物的水平切向量：其中，
S5：采用修正矩阵对初始流场进行修正，得到静态障碍环境下的修正流场：
S6：获得水下航行器的下一路径点：其中ΔT为时间步长；
S7：将各路径点依此连接，获得水下航行器的规划路径。
作为优选：所述路径规划算法进一步包括以下步骤：在步骤S4中，量化定义水下障碍物对初始流场的扰动影响，定义表示第k个障碍物的比例系数，取决于水下航行器与第k个障碍物表面之间的距离：
其中，K为障碍物的总数，Γi(P)为水下障碍物等效凸面体方程；水下航行器距离水下障碍物表面的距离越大，越小，以使障碍物对初始流场的修正作用越小；对进行归一化处理：
其中
对修正矩阵进行量化处理，得到：
在步骤S5中，采用量化处理后的修正矩阵获得修正流场：
作为优选：在步骤S5中，若静态环境存在运动威胁时，引入运动威胁来修正流场：
计算威胁的相对水下航行器的参考运动速度其中为威胁的实际运动速度；
计算所有威胁的参考速度
构建威胁运动下的修正流场，其中为修正流场流速；
在步骤S6中，采用威胁运动下的修正流场构建得到静态障碍环境下的修正流场
作为优选：对障碍物的排斥反应系数和切向反应系数、汇流流场权重系数、水流流场权重系数、进行迭代调整，以获得最优的水下航行器路径。
本发明的有益效果为：
(1)本发明提供了一种不同水下障碍物的统一化处理方法，将不同的障碍物按规则等效为圆球、圆柱、圆锥等标准几何体，可处理不同形状的障碍物并进行统一描述，大大减少了问题复杂度；
(2)通过对障碍物的等效处理，模拟了自然界水流避障现象，原理简单、计算量少，尤其适应于复杂海洋环境下的路径规划；
(3)引入了威胁运动对水下航行器运动的影响，从而可提高航迹路径规划的精度；
(4)本发明规划的路径质量较高，水下航行器总能安全避开障碍物并最终到达目标点。为水下航行器自主控制系统搭载该路径规划算法，可进一步提高水下自主航行器的自主感知和决策控制能力，提高自主航行能力。
附图说明
图1为本发明的方法流程图；
图2a为水下障碍物等效圆球标准凸面体示意图；
图2b为水下障碍物等效圆柱标准凸面体示意图；
图2c为水下障碍物等效圆锥标准凸面体示意图；
图2d为水下障碍物等效长方体标准凸面体示意图；
图3为初始流场示意图；
图4为修正流场示意图；
图5为在同样水下环境下，本发明方法水下航行器规划路径与现有技术水下航行器规划路径对比图。
具体实施方式
以下将结合附图对本发明的具体实施方式进行清楚完整地描述。显然，具体实施方式所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
本发明提供了一种水下航行器航行路径的规划算法，该算法考虑水下复杂环境所形成的障碍物环境、以及各种动态运动威胁对航行路径的影响，研究了一种可合理避障、高效行进的航行路径规划算法。
水下航行器路径规划算法，用于规划水下航行器的航行路径，此处所述的水下航行器是指自主式水下航行器，是一种具备自主导航能力与规划能力的航行器，可在水下自主由起始位置运动到目标位置。为自主式水下航行器控制器搭载该控制算法，可提高自主式水下航行器的自主航行能力。
水下航行器路径规划的基本原理参考水流避障的原理。分析水流避障自然现象与水下航行器路径规划问题的宏观相似之处，揭示绕流流线与避障路径的共性特征。自然界的水流具有如下宏观特征：在无障碍的自由环境下，水流从高处到低处沿直线流动并到达终点；在障碍环境下，水流会平滑地绕过障碍并最终到达终点；而当地形条件非常复杂时，水流总能适应地形，即总能找到合适的避障路线。
上述自然现象与水下航行器路径规划具有相似之处，因此可忽略流体力学的细节而提取水流现象的宏观规律：将笔直的水流看作初始流场，初始流场的流线即为无障碍环境下的AUV初始路径；绕过障碍的水流可等效为修正流场，修正流场的流线即为障碍环境下的AUV避障路径。
实施例1
水下航行器的路径规划算法包括以下步骤：
S1：将水下障碍均近似等效为几何凸面体，采用统一的凸面体方程进行描述：
其中，a,b,c,p,q,r为决定几何凸面体大小和形状的常数，(x0,y0,z0)表示几何凸面体的中心，Γ(P)＝1为障碍物表面方程。
水下障碍物为水下航行器运动的主要威胁，水下障碍物主要包括：海底地形、湍急旋涡、大型鱼群、水下机器人等。然而，自然环境中的各种障碍物形状是不规则的，为了简化计算，统一各种障碍物的表示，将水下环境中的各种障碍用标准的几何凸面体进行等效。等效的规则为：获得障碍物的精确点云数据，则可利用某种估算技术如球体-圆环体模块包络算法来获得具有C1平滑度(即一阶可导)的物体包络；在仅知物体的粗略轮廓数据或规划时间受限的情况下，则可直接从包络数据库中寻找合适的包络类型并进行扩展。障碍物建立虚拟的集合凸面体包络，包括柱体、锥形或球形包络。以柱体模型为例，可建立具有规则形状的障碍物边界线(如圆、矩形等)，然后通过沿边界线垂直方向将边界线拉伸的方式建立规则柱体，从而实现对障碍物的完全包络。然后，将圆球、圆柱、圆锥、长方体等标准的凸面体用统一方程进行描述。图2a至图2d给出了各种等效几何体的包络示意图。此外，考虑到水下航行器的机动能力限制、障碍物预处理与建模误差等，也可以引入膨胀因子λ来适当扩大障碍物范围，加入膨胀因子后，凸面体方程可表示为：
凸面体等效坐标系的建立规则为，以凸面体底面所在的平面为x-y平面，来建立坐标系。其中几何凸面体的中心做如下理解：对于球体、长(正)方体等，中心为凸面体的几何中心；对于圆锥体的中心，中心为凸面体下底面的中心。
S2：为了使水下航行器快速达到目标点，建立以水下航行器航行目标点为中心的汇流场：
定义该汇流场的速度为其中V0为汇流场的速率，d(P,Pd)表示水下航行器当前位置与航行目标位置的欧式距离；
S3：参考图3，构建自由环境下航行目标点的初始流场，将目标位置汇流场与水流流场按一定比例进行叠加：
假设水流流场为恒速流场，水流流速为uc(P)，将水流流速与汇流场流速按权重叠加，u(P)＝λsus(P)+λcuc(P)，其中u(P)为初始流场的流速，λs和λc分别表示汇流场和水流流畅的权重系数；两者的取值范围为0～1且满足λs+λc＝1，具体的取值需迭代优化，其中u(P)可近似表达自然条件下的初始流场；
S4：定义水下障碍物的修正矩阵，以定义水下障碍物对航行目标点初始流场的扰动影响：
其中，K为障碍物的总数，k(1≤k≤K)表示第k个障碍物，Mk(P)表示第k个障碍物的修正矩阵，ρk、σk分别表示第k个障碍物的排斥反应系数和切向反应系数，两者的取值范围通常为0.1～1.0，具体规划计算时，将根据迭代优化选取合适的取值，nk(P)表示第k个障碍物的径向法向量，tk(P)表示第k个障碍物的水平切向量：其中，
此处需要说明的是，如果惯性坐标系o-xyz下障碍物的三条轴线与x,y,z轴不平行，首先需将惯性坐标系旋转为参考坐标系o-x'y'z'(坐标旋转矩阵为Q)，然后采用上述公式计算修正矩阵M'k(P')，最后通过坐标反变换得到惯性坐标系o-xyz下的修正矩阵即Mk(P)＝Q-1M'k(P')。
S5：采用修正矩阵对初始流场进行修正，得到静态障碍环境下的修正流场：
S6：获得水下航行器的下一路径点：其中ΔT为时间步长；
S7：从起始点开始，将各路径点依此连接，获得水下航行器的规划路径，水下航行器运行过程中，按此规划路径行走。
更进一步的，为了获得一个最优的规划路径，对障碍物的排斥反应系数和切向反应系数、汇流流场权重系数、水流流场权重系数、进行迭代调整，选取合适的反应系数和权重系数。
障碍物排斥或吸引反应系数对流线形状起决定性作用：反应系数越大，流线避障的时机越早且幅度越大。此外，汇流流场与实际洋流场的权重系数也会影响流线形状。因此需对上述参数迭代调整，直至获得最优路径。首先，依据水下航行器期望航行方向(由规划路径确定)、水下航行器相对于洋流场的速率(假设为恒定值)、洋流场速度矢量等信息，采用速度矢量合成方法求得AUV沿规划路径航行的实际速率，从而计算航行时间。然后，以最小化航行时间作为优化目标，对障碍物反应系数与各流场权重系数进行优化调整。由于上述系数的细微变化对流线形状影响不大，因此可直接选取一系列离散值(如0.1,0.2,…)，进而得到多条规划路径，并从中筛选出最优路径。
至此，为一个完整的水下航行器的路径规划算法。
实施例2
为了进一步提高路径规划算法的规划精度，进一步提供如下的一种引入水下障碍物对初始流场影响，以及威胁运动物对航行规划影响的路径规划算法。
步骤S1至步骤S3的过程参考实施例1，均与实施例1相同。
S4：定义水下障碍物的修正矩阵，以定义水下障碍物对航行目标点初始流场的扰动影响：
其中，K为障碍物的总数，k(1≤k≤K)表示第k个障碍物，Mk(P)表示第k个障碍物的修正矩阵，ρk、σk分别表示第k个障碍物的排斥或吸引反应，nk(P)表示第k个障碍物的径向法向量，tk(P)表示第k个障碍物的水平切向量：其中，
为了提高路径规划算法的精确性，进一步量化水下障碍物对初始流场的扰动影响，定义表示第k个障碍物的比例系数，取决于水下航行器与第k个障碍物表面之间的距离：
其中，K为障碍物的总数，Γi(P)为水下障碍物等效凸面体方程；水下航行器距离水下障碍物表面的距离越大，越小，以使障碍物对初始流场的修正作用越小；对进行归一化处理：
其中
对修正矩阵进行量化处理，得到：
S5：参考图4，采用修正矩阵对初始流场进行修正，得到静态障碍环境下的修正流场：当空间中仅有静态障碍物时，采用这种修正方式；
而当水流空间中有运动的障碍物(运动威胁)时，进一步考虑空间中运动威胁对水下自主航行器的运动影响。
计算威胁的相对水下航行器的参考运动速度其中为威胁的实际运动速度；
当存在多个运动威胁时，需要计算所有威胁的参考速度：
然后构建流速为u(P)-v(P)的相对初始流场、以及流速为的相对修正流场。由于相对流场内的移动威胁可看作静止的，因此满足如下公式：
基于此，构建威胁运动下的修正流场，其中为修正流场流速。
S6：采用威胁运动下的修正流场构建得到静态障碍环境下的修正流场，获得水下航行器的下一路径点：其中ΔT为时间步长；
S7：从起始点开始，将各路径点依此连接，获得水下航行器的规划路径，水下航行器运行过程中，按此规划路径行走。
为了获得一个最优的规划路径，对障碍物的排斥反应系数和切向反应系数、汇流流场权重系数、水流流场权重系数、进行迭代调整，选取合适的反应系数和权重系数。
至此，为一个完整的水下航行器的路径规划算法。
在同样的水下环境下，采用本方法进行的水下航行器的规划路径与传统的人工势场法进行的水下航行器的路径规划如图5所示。从图5可见，采用本方法进行水下航行器的规划路径与人工势场法规划的路径相比，可减少路径长度，更合理的规避水下障碍物。
Claims (4)
1.水下航行器路径规划算法，用于规划水下航行器的航行路径，其特征在于，包括以下步骤：
S1：将水下障碍均近似等效为几何凸面体，采用统一的凸面体方程进行描述：
其中，a,b,c,p,q,r为决定几何凸面体大小和形状的常数，(x0,y0,z0)表示几何凸面体的中心，Γ(P)＝1为障碍物表面方程；
S2：建立以水下航行器航行目标点为中心的汇流场：
定义该汇流场的速度为其中V0为汇流场的速率，d(P,Pd)表示水下航行器当前位置与航行目标位置的欧式距离；
S3：构建自由环境下航行目标点的初始流场：
假设水流流场为恒速流场，水流流速为uc(P)，将水流流速与汇流场流速按权重叠加，u(P)＝λsus(P)+λcuc(P)，其中u(P)为初始流场的流速，λs和λc分别表示汇流场和水流流场权重系数；
S4：定义水下障碍物的修正矩阵，以定义水下障碍物对航行目标点初始流场的扰动影响：
其中，K为障碍物的总数，ρk、σk分别表示第k个障碍物的排斥反应系数和切向反应系数，nk(P)表示第k个障碍物的径向法向量，tk(P)表示第k个障碍物的水平切向量：其中，
<mrow>
<msub>
<mi>n</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&Gamma;</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<mi>x</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&Gamma;</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<mi>y</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&Gamma;</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<mi>z</mi>
</mrow>
</mfrac>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>,</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&Gamma;</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<mi>y</mi>
</mrow>
</mfrac>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mrow>
<mo>&part;</mo>
<msub>
<mi>&Gamma;</mi>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&part;</mo>
<mi>x</mi>
</mrow>
</mfrac>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
S5：采用修正矩阵对初始流场进行修正，得到静态障碍环境下的修正流场：
<mrow>
<mover>
<mi>u</mi>
<mo>&OverBar;</mo>
</mover>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>M</mi>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mi>u</mi>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
S6：获得水下航行器的下一路径点：其中ΔT为时间步长；
S7：将各路径点依此连接，获得水下航行器的规划路径。
2.如权利要求1所述的水下航行器路径规划算法，其特征在于，所述路径规划算法进一步包括以下步骤：在步骤S4中，量化定义水下障碍物对初始流场的扰动影响，定义表示第k个障碍物的比例系数，取决于水下航行器与第k个障碍物表面之间的距离：
<mrow>
<msub>
<mover>
<mi>&omega;</mi>
<mo>&OverBar;</mo>
</mover>
<mi>k</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mi>K</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<munderover>
<mo>&Pi;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mi>i</mi>
<mo>&NotEqual;</mo>
<mi>k</mi>
</mrow>
<mi>K</mi>
</munderover>
<mfrac>
<mrow>
<mo>(</mo>
<msub>
<mi>&Gamma;</mi>
<mi>i</mi>
</msub>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>&Gamma;</mi>
<mi>i</mi>
</msub>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
<mo>+</mo>
<mo>(</mo>
<msub>
<mi>&Gamma;</mi>
<mi>k</mi>
</msub>
<mo>(</mo>
<mi>P</mi>
<mo>)</mo>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mfrac>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>K</mi>
<mo>></mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
其中，K为障碍物的总数，Γi(P)为水下障碍物等效凸面体方程；水下航行器距离水下障碍物表面的距离越大，越小，以使障碍物对初始流场的修正作用越小；对进行归一化处理：
其中
对修正矩阵进行量化处理，得到：
在步骤S5中，采用量化处理后的修正矩阵获得修正流场：
3.如权利要求1所述的水下航行器路径规划算法，其特征在于，在步骤S5中，若静态环境存在运动威胁时，引入运动威胁来修正流场：
计算威胁的相对水下航行器的参考运动速度其中为威胁的实际运动速度；
计算所有威胁的参考速度
构建威胁运动下的修正流场，其中为修正流场流速；
在步骤S6中，采用威胁运动下的修正流场构建得到静态障碍环境下的修正流场。
4.如权利要求1所述的水下航行器路径规划算法，其特征在于，对障碍物的排斥反应系数和切向反应系数、汇流流场权重系数、水流流场权重系数、进行迭代调整，以获得最优的水下航行器路径。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711193718.7A CN107966153B (zh) | 2017-11-24 | 2017-11-24 | 水下航行器路径规划算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711193718.7A CN107966153B (zh) | 2017-11-24 | 2017-11-24 | 水下航行器路径规划算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107966153A true CN107966153A (zh) | 2018-04-27 |
| CN107966153B CN107966153B (zh) | 2020-02-18 |
Family
ID=61997728
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711193718.7A Active CN107966153B (zh) | 2017-11-24 | 2017-11-24 | 水下航行器路径规划算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107966153B (zh) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109240288A (zh) * | 2018-08-31 | 2019-01-18 | 武汉理工大学 | 一种基于轨迹单元的障碍物情况下无人艇避碰路径规划方法 |
| CN109657834A (zh) * | 2018-10-31 | 2019-04-19 | 广东工业大学 | 一种基于自适应基因长度差分进化算法的海底路径规划方法 |
| CN111200823A (zh) * | 2020-01-08 | 2020-05-26 | 中国电子科技集团公司电子科学研究院 | 一种通信补足方法、装置、auv和存储介质 |
| CN111307158A (zh) * | 2020-03-19 | 2020-06-19 | 哈尔滨工程大学 | 一种auv三维航路规划方法 |
| CN113074730A (zh) * | 2021-03-17 | 2021-07-06 | 华中科技大学 | 一种水下路径规划方法和系统 |
| CN113190037A (zh) * | 2021-04-08 | 2021-07-30 | 上海吞山智能科技有限公司 | 基于改进流体扰动和麻雀算法的无人机最优路径搜索方法 |
| CN113282105A (zh) * | 2021-06-16 | 2021-08-20 | 北京北航天宇长鹰无人机科技有限公司 | 无人机路径避障调整的方法、计算机装置和可读存储介质 |
| CN113433969A (zh) * | 2021-06-24 | 2021-09-24 | 中国电子科技集团公司电子科学研究院 | 基于组合向量场法的无人机集群自主避障方法及装置 |
| CN113703438A (zh) * | 2021-07-14 | 2021-11-26 | 深圳市水务工程检测有限公司 | 用于输水隧洞巡检的auv自主导航路径规划方法 |
| CN114398719A (zh) * | 2021-12-24 | 2022-04-26 | 华中科技大学 | 一种水下航行器动态安全领域模型的构建方法及应用 |
| CN114459485A (zh) * | 2021-11-29 | 2022-05-10 | 湖北中南鹏力海洋探测系统工程有限公司 | 一种基于微弱动力的海面表层漂流浮标自主导航方法 |
| CN115143970A (zh) * | 2022-09-01 | 2022-10-04 | 安徽大学 | 一种基于威胁度评估的水下航行器避障方法和系统 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2012203682A (ja) * | 2011-03-25 | 2012-10-22 | Ihi Corp | 水中航走体用船上管制装置の表示方法 |
| CN102749080A (zh) * | 2012-06-18 | 2012-10-24 | 北京航空航天大学 | 一种基于流体力学的无人机三维航路生成方法 |
| CN103760907A (zh) * | 2013-12-30 | 2014-04-30 | 哈尔滨工程大学 | 一种基于布谷鸟搜索算法的水下潜器三维路径规划方法 |
| JP2015110374A (ja) * | 2013-12-06 | 2015-06-18 | 三菱重工業株式会社 | 自律式移動体の経路作成支援装置及びその方法並びにプログラム |
-
2017
- 2017-11-24 CN CN201711193718.7A patent/CN107966153B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2012203682A (ja) * | 2011-03-25 | 2012-10-22 | Ihi Corp | 水中航走体用船上管制装置の表示方法 |
| CN102749080A (zh) * | 2012-06-18 | 2012-10-24 | 北京航空航天大学 | 一种基于流体力学的无人机三维航路生成方法 |
| JP2015110374A (ja) * | 2013-12-06 | 2015-06-18 | 三菱重工業株式会社 | 自律式移動体の経路作成支援装置及びその方法並びにプログラム |
| CN103760907A (zh) * | 2013-12-30 | 2014-04-30 | 哈尔滨工程大学 | 一种基于布谷鸟搜索算法的水下潜器三维路径规划方法 |
Non-Patent Citations (2)
| Title |
|---|
| 姚鹏等: "基于动态流体扰动原理的三维滚动航路规划", 《北京航空航天大学学报》 * |
| 王宏伦等: "基于流水避石原理的无人机三维航路规划", 《电光与控制》 * |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109240288B (zh) * | 2018-08-31 | 2021-08-10 | 武汉理工大学 | 一种基于轨迹单元的障碍物情况下无人艇避碰路径规划方法 |
| CN109240288A (zh) * | 2018-08-31 | 2019-01-18 | 武汉理工大学 | 一种基于轨迹单元的障碍物情况下无人艇避碰路径规划方法 |
| CN109657834A (zh) * | 2018-10-31 | 2019-04-19 | 广东工业大学 | 一种基于自适应基因长度差分进化算法的海底路径规划方法 |
| CN109657834B (zh) * | 2018-10-31 | 2022-10-04 | 广东工业大学 | 一种基于自适应基因长度差分进化算法的海底路径规划方法 |
| CN111200823A (zh) * | 2020-01-08 | 2020-05-26 | 中国电子科技集团公司电子科学研究院 | 一种通信补足方法、装置、auv和存储介质 |
| CN111307158A (zh) * | 2020-03-19 | 2020-06-19 | 哈尔滨工程大学 | 一种auv三维航路规划方法 |
| CN113074730A (zh) * | 2021-03-17 | 2021-07-06 | 华中科技大学 | 一种水下路径规划方法和系统 |
| CN113190037A (zh) * | 2021-04-08 | 2021-07-30 | 上海吞山智能科技有限公司 | 基于改进流体扰动和麻雀算法的无人机最优路径搜索方法 |
| CN113282105A (zh) * | 2021-06-16 | 2021-08-20 | 北京北航天宇长鹰无人机科技有限公司 | 无人机路径避障调整的方法、计算机装置和可读存储介质 |
| CN113433969A (zh) * | 2021-06-24 | 2021-09-24 | 中国电子科技集团公司电子科学研究院 | 基于组合向量场法的无人机集群自主避障方法及装置 |
| CN113433969B (zh) * | 2021-06-24 | 2023-02-17 | 中国电子科技集团公司电子科学研究院 | 基于组合向量场法的无人机集群自主避障方法及装置 |
| CN113703438A (zh) * | 2021-07-14 | 2021-11-26 | 深圳市水务工程检测有限公司 | 用于输水隧洞巡检的auv自主导航路径规划方法 |
| CN114459485A (zh) * | 2021-11-29 | 2022-05-10 | 湖北中南鹏力海洋探测系统工程有限公司 | 一种基于微弱动力的海面表层漂流浮标自主导航方法 |
| CN114398719A (zh) * | 2021-12-24 | 2022-04-26 | 华中科技大学 | 一种水下航行器动态安全领域模型的构建方法及应用 |
| CN114398719B (zh) * | 2021-12-24 | 2024-07-26 | 华中科技大学 | 一种水下航行器动态安全领域模型的构建方法及应用 |
| CN115143970A (zh) * | 2022-09-01 | 2022-10-04 | 安徽大学 | 一种基于威胁度评估的水下航行器避障方法和系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107966153B (zh) | 2020-02-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107966153B (zh) | 水下航行器路径规划算法 | |
| Wu et al. | Bi-directional adaptive A* algorithm toward optimal path planning for large-scale UAV under multi-constraints | |
| Li et al. | Path planning technologies for autonomous underwater vehicles-a review | |
| Chen et al. | Optimal time-consuming path planning for autonomous underwater vehicles based on a dynamic neural network model in ocean current environments | |
| Guo et al. | Research progress of path planning methods for autonomous underwater vehicle | |
| Yao et al. | Three-dimensional path planning for AUV based on interfered fluid dynamical system under ocean current (June 2018) | |
| CN104020770B (zh) | 基于多项式的uuv空间轨迹规划方法 | |
| Wang et al. | Improved quantum particle swarm optimization algorithm for offline path planning in AUVs | |
| Cao et al. | Toward optimal rendezvous of multiple underwater gliders: 3D path planning with combined sawtooth and spiral motion | |
| Wang et al. | 3-D path planning with multiple motions for a gliding robotic dolphin | |
| Lan et al. | Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents | |
| Yao et al. | A hierarchical architecture using biased min-consensus for USV path planning | |
| CN108762280A (zh) | 一种考虑海洋环流影响的uuv基于能量消耗优化的远程航海路径规划方法 | |
| Li et al. | Three-dimensional path planning for AUVs in ocean currents environment based on an improved compression factor particle swarm optimization algorithm | |
| Fan et al. | Formation control of multiple unmanned surface vehicles using the adaptive null-space-based behavioral method | |
| Kim | Control laws to avoid collision with three dimensional obstacles using sensors | |
| Nie et al. | Planar path-following tracking control for an autonomous underwater vehicle in the horizontal plane | |
| CN110320907B (zh) | 一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法 | |
| Yan et al. | A novel path planning for AUV based on objects’ motion parameters predication | |
| Zheng et al. | Electric sense based pose estimation and localization for small underwater robots | |
| Kim et al. | Any-angle path planning with limit-cycle circle set for marine surface vehicle | |
| Zhao et al. | Global path planning of unmanned vehicle based on fusion of A∗ algorithm and Voronoi field | |
| Zeng et al. | Path planning for rendezvous of multiple AUVs operating in a variable ocean | |
| Li et al. | Real time path planning for a class of torpedo-type AUVs in unknown environment | |
| Wang et al. | Stepwise cooperative trajectory planning for multiple BUVs based on temporal–spatial Bezier curves |
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 |