CN112799414B - 一种auv松弛轨迹规划方法 - Google Patents

一种auv松弛轨迹规划方法 Download PDF

Info

Publication number
CN112799414B
CN112799414B CN202110040918.9A CN202110040918A CN112799414B CN 112799414 B CN112799414 B CN 112799414B CN 202110040918 A CN202110040918 A CN 202110040918A CN 112799414 B CN112799414 B CN 112799414B
Authority
CN
China
Prior art keywords
planning
auv
point
path
particle
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
CN202110040918.9A
Other languages
English (en)
Other versions
CN112799414A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202110040918.9A priority Critical patent/CN112799414B/zh
Publication of CN112799414A publication Critical patent/CN112799414A/zh
Application granted granted Critical
Publication of CN112799414B publication Critical patent/CN112799414B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/04Control of altitude or depth
    • G05D1/06Rate of change of altitude or depth
    • G05D1/0692Rate of change of altitude or depth specially adapted for under-water vehicles

Landscapes

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

Abstract

本发明公开了一种AUV松弛轨迹规划方法,将全局规划航路按照设定路径长度划分为多个子航段;确定本次局部航迹规划的起点、终点,以及航迹规划空间范围;根据当前局部环境是否存在运动障碍物,确定执行路径规划或者轨迹规划;利用自适应差分进化粒子群优化算法完成局部航迹规划,得出中间路径点序列以及到达各个中间路径点期望航速;确定当前子目标点,解算AUV的期望航向、期望深度;输出航向指令、深度指令、航速指令,驱动AUV航行;执行方法至结束。本发明可以适应AUV周围局部环境障碍分布动态变化,且能够根据周围局部环境是否存在运动障碍物确定执行路径规划或者轨迹规划,从而权衡局部在线航迹规划的有效性要求和快速性要求。

Description

一种AUV松弛轨迹规划方法
技术领域
本发明涉及一种AUV松弛轨迹规划方法,特别是一种基于自适应差分进化粒子群优化的AUV松弛轨迹规划方法,属于自主潜航器智能控制技术领域。
背景技术
自主潜航器(Autonomous Underwater Vehicle,AUV)是一类自带能源并能够实现自主导航、自主决策与控制的水下无人工作平台,是辅助人类探索海洋、开发海洋的有效工具,在军事、民用领域均发挥着日益重要的作用,并具有广阔的发展前景。
AUV在执行具体任务时,以全局路径规划为基本导引信息航行至目标位置,然而,由于海洋环境的复杂性、未知性、不确定性和非结构性,仅凭全局路径规划无法确保航行安全,AUV在航行过程中还需要实时探测周围环境障碍物,并根据具体的局部环境信息进行局部运动规划,以规避先验知识以外、全局路径规划未预知的风险和障碍。在线局部运动规划是AUV完成测绘、搜探、侦察等其他水下任务的前提保障。一般而言,AUV须兼顾安全性与时效性,综合任务需求、静态障碍物分布情况、动态障碍物运动趋势以及AUV自身动力学特性等诸多因素实现水下三维运动规划。
专利ZL201610133445.6公开了一种智能向量场直方图避碰方法,能够解决AUV的水下实时障碍规避问题,但是该方法属于一种应急响应式避碰方法,不适用于障碍复杂分布情况下的慎思式运动规划。
专利CN201911307896.7公开了一种树下机器人路径规划方法,通过Dubins曲线和遗传算法的结合,实现了AUV的路径规划,但是该方法仅适用于二维路径规划,不适用于AUV在水下三维空间运动情况。
专利ZL201410121156.5公开了一种基于改进萤火虫算法的AUV三维航路规划方法,能够解决适用于AUV的水下三维空间路径规划问题,但是该方法并未考虑水下存在运动障碍物的情况。
AUV随着人类探索的脚步将在海洋中走向更深、更远,其自主性、智能性面临着更高的要求,在线局部运动规划是亟待解决的关键技术。
发明内容
针对上述现有技术,本发明的目的是为了解决自主潜航器在三维动态障碍环境下的安全航行问题,提供一种基于自适应差分进化粒子群优化(Adaptive DifferentialEvolution Particle Swarm Optimization,ADEPSO)的AUV松弛轨迹规划方法,可以适应AUV周围局部环境障碍分布动态变化,且能够根据周围局部环境是否存在运动障碍物确定执行路径规划或者轨迹规划,从而权衡局部在线航迹规划的有效性要求和快速性要求。
为解决上述技术问题,本发明的一种AUV松弛轨迹规划方法,包括以下步骤:
步骤1:将全局规划航路按照设定路径长度划分为多个子航段;
步骤2:根据AUV当前位置以及障碍分布情况,确定本次局部航迹规划的起点、终点,以及航迹规划空间范围;
步骤3:根据当前局部环境是否存在运动障碍物,确定本次优化过程是否对AUV速度指令进行优化,即确定执行路径规划或者轨迹规划,若仅有静态障碍则执行路径规划,若存在运动障碍则执行轨迹规划;
步骤4:利用自适应差分进化粒子群优化算法,完成局部航迹规划,得出中间路径点序列以及到达各个中间路径点的期望航速;
步骤5:根据AUV当前实际位置及规划结果,确定当前子目标点,进而利用空间几何原理解算AUV的期望航向、期望深度;
步骤6:输出航向指令、深度指令、航速指令,驱动AUV航行;
步骤7:判断AUV是否抵达当前全局路径规划终点,若是则转步骤8,若否则转步骤2,继续执行下一次优化过程;
步骤8:判断是否抵达全局规划最后一段航路终点,若否则转入下一全局规划航段,并转步骤2,若是则结束。
本发明还包括:
1.步骤2中根据AUV当前位置以及障碍分布情况,确定本次局部航迹规划的起点、终点,以及航迹规划空间范围具体为:
探测到障碍物后,以AUV当前位置作为局部规划起点,即子任务起点
Figure BDA0002895806740000021
Figure BDA0002895806740000022
分别代表北向、东向和深度方向坐标,以未遭受碰撞威胁的最近准子目标点作为局部规划的终点,即子任务终点
Figure BDA0002895806740000023
Figure BDA0002895806740000024
分别代表北向、东向和深度方向坐标;
根据子任务起点SS、子任务终点TS,以确定的北、东、深三个方向的正、负偏移距离,生成路径点搜索范围:
Figure BDA0002895806740000025
其中:Δn为北向负向偏移距离;
Figure BDA0002895806740000031
为北向正向偏移距离;Δe为东向负向偏移距离;
Figure BDA0002895806740000032
为东向正向偏移距离;Δd为深度负向偏移距离;
Figure BDA0002895806740000033
为深度正向偏移距离。
2.步骤4中利用自适应差分进化粒子群优化算法,完成局部航迹规划,得出中间路径点序列以及到达各个中间路径点的期望航速具体为:
步骤4.0:初始化,包括:
步骤4.0.1:设置算法参数;
步骤4.0.2:对粒子群进行初始化;
步骤4.0.3:对敏感粒子进行初始化;
步骤4.1:粒子适应度更新,包括:
步骤4.1.1:计算每个粒子的适应度;
步骤4.1.2:更新每个粒子的历史最优位置;
步骤4.1.3:更新全局最优位置;
步骤4.2:粒子状态更新,包括:
步骤4.2.1:更新每个粒子的速度;
步骤4.2.2:更新每个粒子的位置;
步骤4.3:敏感粒子适应度计算,包括:
步骤4.3.1:计算敏感粒子的适应度;
步骤4.3.2:计算敏感粒子适应度的变化量;
步骤4.4:执行差分进化操作,包括:
步骤4.4.1:根据敏感粒子适应度变化量确定粒子群的差分进化比率;
步骤4.4.2:完成变异、交叉和选择等差分进化操作;
步骤4.5:判定是否满足终止条件,若是则转步骤4.6,若否则转步骤4.1;
步骤4.6:把当前的全局最优位置作为优化结果输出,结束。
本发明的有益效果:与现有技术相比,本发明可以根据AUV周围局部环境是否存在运动障碍物而选择执行路径规划或轨迹规划,以权衡航迹规划问题的有效性要求与实时性要求;在粒子群优化算法中引入差分进化算法的变异、交叉、选择机制以增强种群多样性,并且,借鉴动态粒子群优化思想,根据敏感粒子适应度变化情况调整差分进化比率以适应约束条件的动态变化,从而提出ADEPSO算法,并应用于AUV的三维空间航迹规划,渐进规划出安全航迹,导引AUV在障碍环境下安全航行,直至到达既定目标点。
附图说明
图1为本发明提出的基于ADEPSO的AUV松弛轨迹规划方法流程图;
图2为ADEPSO算法的流程图;
图3为AUV局部松弛轨迹规划方法示意图;
图4显示了具体实施案例中的全局规划情况、障碍物分布情况、AUV的预定全局规划航路以及AUV实际航迹。
具体实施方式
下面结合附图对本发明具体实施方式做进一步说明。
结合图1和图2,本发明将全局规划航路划分为若干子航段,依次在各个子航段执行在线航迹规划,若仅有静态障碍则执行路径规划,若存在运动障碍则执行轨迹规划;提出一种ADEPSO算法,在基本粒子群优化算法中引入差分进化机制和动态优化思想,以增强种群多样性,提升全局优化能力,并且增强对动态约束条件的适应性,继而应用该优化算法解决AUV的松弛轨迹规划问题。
AUV在水下执行任务时,通常由水面监控系统设定一系列目标点以任务文本的方式下达给AUV,AUV需要依次通过这些全局规划目标点。
基于ADEPSO的AUV松弛轨迹规划方法,具体而言,包括以下步骤:
步骤1:将全局规划航路按照一定路径长度,划分为多个子航段;
步骤2:根据AUV当前位置以及障碍分布情况,确定本次局部航迹规划的起点、终点,以及航迹规划空间范围;
步骤3:根据当前局部环境是否存在运动障碍物,确定本次优化过程是否对AUV速度指令进行优化,即确定执行路径规划或者轨迹规划;
步骤4:利用ADEPSO算法,完成局部航迹规划,得出中间路径点序列以及到达各个中间路径点的期望航速;
步骤5:根据AUV当前实际位置及规划结果,确定当前子目标点,进而利用空间几何原理解算AUV的期望航向、期望深度;
步骤6:输出航向指令、深度指令、航速指令,驱动AUV航行;
步骤7:判断AUV是否抵达当前全局路径规划终点,若是则转步骤8,若否则转步骤2,继续执行下一次优化过程;
步骤8:判断是否抵达全局规划最后一段航路终点,若否则转入下一全局规划航段,并转步骤2,若是则结束。
本发明还包括:
(1)步骤1中:
将任务起点S与任务终点T的连线按照间距ld划分,得到NQ个准子目标点
Figure BDA0002895806740000051
i=1,2,…,NQ,其中,
Figure BDA0002895806740000052
点与终点Tt重合。
(2)步骤2中:
探测到障碍物后,以AUV当前位置作为局部规划起点(亦即子任务起点)
Figure BDA0002895806740000053
以未遭受碰撞威胁的最近准子目标点作为局部规划的终点(亦即子任务终点)
Figure BDA0002895806740000054
根据子任务起点SS、子任务终点TS,以确定的北、东、深三个方向的正、负偏移距离,生成路径点搜索范围:
Figure BDA0002895806740000055
其中:
Δn:北向负向偏移距离;
Figure BDA0002895806740000056
北向正向偏移距离;
Δe:东向负向偏移距离;
Figure BDA0002895806740000057
东向正向偏移距离;
Δd:深度负向偏移距离;
Figure BDA0002895806740000058
深度正向偏移距离。
(3)步骤3中:
由子任务起点SS到子任务终点TS的松弛轨迹规划问题描述为:
Figure BDA0002895806740000059
其中,pc表示候选粒子,包括子任务起点到子任务终点的规划路径点数目NP、规划路径点序列
Figure BDA00028958067400000510
和由子任务起点SS出发,经过规划路径点序列到达子任务终点TS过程中的速度指令vc,f是综合考虑安全性和快速性而设计的适应度函数。一般而言,AUV由任务起点S出发,经过多次局部松弛轨迹规划抵达任务终点T。各次局部规划的子任务起点、子任务终点分别为
Figure BDA00028958067400000511
Figure BDA00028958067400000512
i=1,2,…,NS,第i次局部松弛轨迹规划的结果是路径点数目
Figure BDA00028958067400000513
路径点序列
Figure BDA00028958067400000514
和航速指令
Figure BDA00028958067400000515
当任务环境中存在运动障碍物时,执行轨迹规划,即在路径规划的基础上进行航速规划以进一步降低与运动障碍物碰撞的风险;当任务环境中仅存在静态障碍物时,执行路径规划,航速指令与全局规划的航速指令保持一致,轨迹规划问题退化为路径规划问题。
(4)步骤4中:
(4.1)关于适应度函数说明
AUV松弛轨迹规划的适应度函数包括三部分,即针对静态障碍物相对距离的适应度fso、针对运动障碍物相对距离的适应度fdo和针对路径长度的适应度fpl
f=wfsofso+wfdofdo+wfplfpl
式中,wfso、wfdo和wfpl表示相应适应度的权重系数,满足:
wfso+wfdo+wfpl=1
fso和fdo反应了适应度的安全性,fpl反映了适应度的快速性。
适应度fso根据每段路径与各个障碍物之间的距离计算。
Figure BDA0002895806740000061
式中,dpokj表示第k段路径与第j个障碍物的距离,当至少有一段路径与某个障碍物距离为零时,表明对应的路段不安全,则粒子适应度取值为零。
适应度fdo根据每段路径上各个时刻AUV预测位置与各个运动障碍物预测位置之间的距离计算而得:
Figure BDA0002895806740000062
式中,Nbk表示根据候选粒子的速度值所预测的第k段路径经历的时间拍数,ddoklj表示第k段路径上第l时刻AUV与第j个运动障碍物的预测距离。
适应度fpl取为各段路径总长度的倒数,该数值越高,表明总路径越短,即AUV越能够快速到达任务终点:
Figure BDA0002895806740000063
(4.2)关于自适应差分进化机制
为了增强粒子群算法的种群多样性以提高其全局优化能力,并且能够适应动态环境以跟踪解空间中的动态极值,设计一种自适应差分进化粒子群优化算法(AdaptiveDifferential Evolution Particle Swarm Optimization,ADEPSO),该算法在基本粒子群算法的基础上,融入差分进化算法的变异、交叉、选择机制,并且,粒子群的差分进化比率根据敏感粒子适应度变化情况实时调整。
ADEPSO算法在经过粒子速度、位置更新后,对粒子群进行差分进化变异。
M∈N*:种群规模;
n∈N*:一个候选个体的状态维数;
x1:xM:全部候选个体;
Figure BDA0002895806740000071
第i个候选个体,i=1,2,…,M;
v1:vM:变异种群;
Figure BDA0002895806740000072
变异个体;
u1:uM:交叉种群;
Figure BDA0002895806740000073
交叉个体;
x*:最优个体;
g:代数。
变异操作是指对父代各自进行随机差分组合,生成新的个体,常见变异策略包括以下方式:
方式1:
vi(g)=xr1(g)+F[xr2(g)-xr3(g)]
方式2:
vi(g)=xbest(g)+F[xr1(g)-xr2(g)]
方式3:
vi(g)=xi(g)+F[xbest(g)-xi(g)]+F[xr1(g)-xr2(g)]
方式4:
vi(g)=xbest(g)+F[xr1(g)-xr2(g)]+F[xr3(g)-xr4(g)]
方式5:
vi(g)=xr1(g)+F[xr2(g)-xr3(g)]+F[xr4(g)-xr5(g)]
式中,vi(g)是变异个体,i=1,2,…,M,xbest(g)是当前的全局最优值,F∈[0,1]是缩放因子,r1,r2,r3,r4,r5∈{1,2,…,M},是互不相等的随机数。在以上变异方式中,方式1和5是完全随机变异,方式2和4是基于最优个体进行变异,方式3是利用个体与最优个体之间的差异进行变异。
父代个体发生变异的概率为Pv,变异个体与父代个体进行交叉得到候选子代个体:
Figure BDA0002895806740000081
式中,r是[0,1]区间内的均匀分布随机数,Pc∈[0,1]是交叉概率,
Figure BDA0002895806740000082
是候选子代个体ui(g)的第j个分量,i=1,2,…,M,j=1,2,…,n。
在选择操作中,候选子代个体与父代个体进行比较,优于父代个体者被选择作为子代个体:
Figure BDA0002895806740000083
借鉴基于敏感粒子的动态粒子群优化思想,并将这一思想与差分进化算法结合,即根据敏感粒子适应度的变化程度实时调整粒子群的差分进化比率,保证粒子个体之间的差异性与多样性,增强粒子群的全局优化能力。
按照随机选取或者特别指定的方式,选择若干粒子作为敏感粒子,以敏感粒子适应度变化情况作为约束条件变化程度的度量:
Figure BDA0002895806740000084
式中,Ns表示敏感粒子的数目,Δf(pcsi)/f(pcsi)表示敏感粒子pcsi自身适应度相对变化量,
Figure BDA0002895806740000085
是粒子群总体适应度变化量的综合度量,亦即约束条件变化程度的度量,进一步根据
Figure BDA0002895806740000086
确定粒子群的差分进化比率:
Figure BDA0002895806740000087
式中,
Figure BDA0002895806740000091
Figure BDA0002895806740000092
分别是差分进化比率的下界和上界,λde是差分进化比率调节系数。
(4.3)ADEPSO具体步骤
ADEPSO算法的步骤具体描述如下:
步骤4.0:初始化
步骤4.0.1:设置算法参数;
步骤4.0.2:对粒子群进行初始化;
步骤4.0.3:对敏感粒子进行初始化;
步骤4.1:粒子适应度更新
步骤4.1.1:计算每个粒子的适应度;
步骤4.1.2:更新每个粒子的历史最优位置;
步骤4.1.3:更新全局最优位置;
步骤4.2:粒子状态更新
步骤4.2.1:更新每个粒子的速度;
步骤4.2.2:更新每个粒子的位置;
步骤4.3:敏感粒子适应度计算
步骤4.3.1:计算敏感粒子的适应度;
步骤4.3.2:计算敏感粒子适应度的变化量;
步骤4.4:执行差分进化操作
步骤4.4.1:根据敏感粒子适应度变化量确定粒子群的差分进化比率;
步骤4.4.2:完成变异、交叉和选择等差分进化操作;
步骤4.5:判定是否满足终止条件,若是则转步骤4.6,若否则转步骤4.1;
步骤4.6:把当前的全局最优位置作为优化结果输出,结束。
结合参数给出实施例,考虑AUV的一般应用场景,设计并执行仿真案例,验证ADEPSO算法应用于三维动态障碍环境航迹规划的有效性。该实施例中,AUV的任务起点为(0,0,10)(米),任务终点为(50,400,50)(米),指令航速为2米/秒,任务区域内设置7个以有向长方体描述的静态障碍物和4个运动障碍物,静态障碍物参数如表1所示,运动障碍物参数如表2所示。
表1航迹规划仿真静态障碍参数
Figure BDA0002895806740000093
Figure BDA0002895806740000101
表2航迹规划仿真运动障碍物参数
Figure BDA0002895806740000102
静态障碍1和2位于全局航迹规划航线上,阻碍了AUV的航行,其他静态障碍分布在全局规划航路两侧。运动障碍1沿AUV预定航迹与AUV相向运动;运动障碍2、3的运动轨迹与AUV预定运动轨迹发生交叉;运动障碍4在AUV预定航路附近沿与预定航路近似平行的方向运动。以上障碍设置对AUV的安全航行构成威胁。
在任务初始时刻,AUV对环境信息是完全未知的,须在航行过程中利用前视声呐探测信息实时构建局部环境,在此基础上利用ADEPSO算法进行实时航路重规划,实现由任务起点到任务终点的安全航行。事实上,AUV执行任务的过程是一个“一边航行、一边探测、一边构建环境并实时规划安全航路”的动态过程。
通过以下步骤完成动态障碍环境下的AUV局部在线航迹规划:
步骤1:按照间距ld=100米,将全局规划航路划分为多个子航段;
步骤2:根据AUV当前位置以及障碍分布情况,确定本次局部航迹规划的起点、终点,进一步以北、东、深三个方向的正向、负向偏移距离Δn=-100、
Figure BDA0002895806740000103
Δe=-100、
Figure BDA0002895806740000104
Δd=-20、
Figure BDA0002895806740000105
确定局部规划空间范围;
步骤3:由于周围环境存在运动障碍物威胁AUV航行安全,故执行轨迹规划;
步骤4:利用ADEPSO算法,完成局部航迹规划,得出中间路径点序列以及到达各个中间路径点的期望航速,具体而言:
步骤4.0:初始化
步骤4.0.1:设置算法参数,种群规模M=100,差分进化比率的下界
Figure BDA0002895806740000106
和上界
Figure BDA0002895806740000107
差分进化比率调节系数λde=0.72,变异概率Pv=0.5,交叉概率Pc=0.5;
步骤4.0.2:对粒子群进行初始化;
步骤4.0.3:对敏感粒子进行初始化;
步骤4.1:粒子适应度更新
步骤4.1.1:计算每个粒子的适应度;
步骤4.1.2:更新每个粒子的历史最优位置;
步骤4.1.3:更新全局最优位置;
步骤4.2:粒子状态更新
步骤4.2.1:更新每个粒子的速度;
步骤4.2.2:更新每个粒子的位置;
步骤4.3:敏感粒子适应度计算
步骤4.3.1:计算敏感粒子的适应度;
步骤4.3.2:计算敏感粒子适应度的变化量,差分进化比率的下界
Figure BDA0002895806740000111
和上界
Figure BDA0002895806740000112
差分进化比率调节系数λde=1.5。
步骤4.4:执行差分进化操作
步骤4.4.1:根据敏感粒子适应度变化量确定粒子群的差分进化比率;
步骤4.4.2:完成变异、交叉和选择等差分进化操作。
步骤4.5:判定是否满足终止条件,若是则转步骤4.6,若否则转步骤4.1;
步骤4.6:把当前的全局最优位置作为优化结果输出,结束。
步骤5:根据AUV当前实际位置,确定当前子目标点,进而利用空间几何原理解算AUV的期望航向、期望深度;
步骤6:输出航向指令、深度指令、航速指令,驱动自主潜航器航行;
步骤7:判断AUV是否抵达当前全局路径规划终点,若是则转入步骤8,若否则转步骤2,继续执行下一次优化过程;
步骤8:判断是否已到达全局规划最后一段航路终点,若否则转入下一全局规划航段,并转步骤2,若是则结束。
AUV由任务起点出发后,首先沿直线朝向任务终点航行。在10秒时刻,探测到运动障碍1,航行安全受到威胁,利用ADEPSO算法执行航路重规划。由于AUV对环境信息的掌握并不全面,仅能够利用部分环境信息执行航迹规划,所以,其规划的结果就全局而言并非最优的,甚至不是绝对安全的,10秒时刻的规划结果事实上与静态障碍1发生碰撞,但是,沿该规划结果航行,足以保证AUV不致与运动障碍1发生碰撞,并且能够规避已探测的静态障碍。随后,AUV探测到运动障碍2,判断可能与之发生碰撞,并且,静态环境信息进一步完善,于32秒时刻执行第二次重规划,规避了与运动障碍2的碰撞威胁,虽然航迹规划的结果仍然不是绝对安全的,但是与静态障碍的碰撞危险进一步降低。后续航行过程中,AUV分别于52秒、72秒、82秒、92秒、202秒时刻执行6次航迹重规划,给出航路点序列和航速指令导引AUV航行,规划结果逐渐趋近于安全。AUV的全局路径规划长度为405.09米,计划航行时间203秒,实际路径总长度为771.02米,航行时间为340秒。AUV航行过程中与静态障碍物的最近距离为11.10米,与运动障碍物的最近距离为20.60米,没有发生碰撞,实现了安全航行,表明ADEPSO算法适用于AUV的三维动态航迹规划。
本领域内的技术人员将会认识到,本发明并不局限于上文所述并在图中示出的特定配置,为了简明起见这里省略了对已知方法的详细陈述,尽管参照前述实施例对本发明进行了详细的说明,以上所述的实施例是为了帮助读者理解本发明的技术方案。本发明可以有各种更改和变化,凡在本发明的精神和原则之内,所做出的任何修改、同等替换、改进等,均应包含在本发明的权利要求范围之内。

Claims (2)

1.一种AUV松弛轨迹规划方法,其特征在于,包括以下步骤:
步骤1:将全局规划航路按照设定路径长度划分为多个子航段;
步骤2:根据AUV当前位置以及障碍分布情况,确定本次局部航迹规划的起点、终点,以及航迹规划空间范围,具体为:
探测到障碍物后,以AUV当前位置作为局部规划起点,即子任务起点
Figure FDA0003902787180000011
Figure FDA0003902787180000012
分别代表北向、东向和深度方向坐标,以未遭受碰撞威胁的最近准子目标点作为局部规划的终点,即子任务终点
Figure FDA0003902787180000013
Figure FDA0003902787180000014
分别代表北向、东向和深度方向坐标;
根据子任务起点SS、子任务终点TS,以确定的北、东、深三个方向的正、负偏移距离,生成路径点搜索范围:
Figure FDA0003902787180000015
其中:Δn为北向负向偏移距离;
Figure FDA0003902787180000016
为北向正向偏移距离;Δe为东向负向偏移距离;
Figure FDA0003902787180000017
为东向正向偏移距离;Δd为深度负向偏移距离;
Figure FDA0003902787180000018
为深度正向偏移距离;
步骤3:根据当前局部环境是否存在运动障碍物,确定本次优化过程是否对AUV速度指令进行优化,即确定执行路径规划或者轨迹规划,若仅有静态障碍则执行路径规划,航速指令与全局规划的航速指令保持一致;若存在运动障碍则执行轨迹规划,即在路径规划的基础上进行航速规划以进一步降低与运动障碍物碰撞的风险;
步骤4:利用自适应差分进化粒子群优化算法,完成局部航迹规划,得出中间路径点序列以及到达各个中间路径点的期望航速;
步骤5:根据AUV当前实际位置及规划结果,确定当前子目标点,进而利用空间几何原理解算AUV的期望航向、期望深度;
步骤6:输出航向指令、深度指令、航速指令,驱动AUV航行;
步骤7:判断AUV是否抵达当前全局路径规划终点,若是则转步骤8,若否则转步骤2,继续执行下一次优化过程;
步骤8:判断是否抵达全局规划最后一段航路终点,若否则转入下一全局规划航段,并转步骤2,若是则结束。
2.根据权利要求1所述的一种AUV松弛轨迹规划方法,其特征在于:步骤4所述利用自适应差分进化粒子群优化算法,完成局部航迹规划,得出中间路径点序列以及到达各个中间路径点的期望航速具体为:
步骤4.0:初始化,包括:
步骤4.0.1:设置算法参数;
步骤4.0.2:对粒子群进行初始化;
步骤4.0.3:对敏感粒子进行初始化;
步骤4.1:粒子适应度更新,包括:
步骤4.1.1:计算每个粒子的适应度;
步骤4.1.2:更新每个粒子的历史最优位置;
步骤4.1.3:更新全局最优位置;
步骤4.2:粒子状态更新,包括:
步骤4.2.1:更新每个粒子的速度;
步骤4.2.2:更新每个粒子的位置;
步骤4.3:敏感粒子适应度计算,包括:
步骤4.3.1:计算敏感粒子的适应度;
步骤4.3.2:计算敏感粒子适应度的变化量;
步骤4.4:执行差分进化操作,包括:
步骤4.4.1:根据敏感粒子适应度变化量确定粒子群的差分进化比率;
步骤4.4.2:完成变异、交叉和选择等差分进化操作;
步骤4.5:判定是否满足终止条件,若是则转步骤4.6,若否则转步骤4.1;
步骤4.6:把当前的全局最优位置作为优化结果输出,结束。
CN202110040918.9A 2021-01-13 2021-01-13 一种auv松弛轨迹规划方法 Active CN112799414B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110040918.9A CN112799414B (zh) 2021-01-13 2021-01-13 一种auv松弛轨迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110040918.9A CN112799414B (zh) 2021-01-13 2021-01-13 一种auv松弛轨迹规划方法

Publications (2)

Publication Number Publication Date
CN112799414A CN112799414A (zh) 2021-05-14
CN112799414B true CN112799414B (zh) 2022-12-13

Family

ID=75810362

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110040918.9A Active CN112799414B (zh) 2021-01-13 2021-01-13 一种auv松弛轨迹规划方法

Country Status (1)

Country Link
CN (1) CN112799414B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113985899B (zh) * 2021-11-25 2023-09-22 江苏科技大学 基于区间多目标优化的水下机器人全局路径规划方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112091946A (zh) * 2020-09-16 2020-12-18 哈尔滨工业大学 一种上置式多自由度绳驱并联机器人
CN112148009A (zh) * 2020-09-23 2020-12-29 北京智远弘业机器人科技有限公司 Agv集群调度方法、装置、电子设备及计算机可读存储介质

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017019595A1 (en) * 2015-07-27 2017-02-02 Genghiscomm Holdings, LLC Airborne relays in cooperative-mimo systems
CN108473200A (zh) * 2015-12-22 2018-08-31 英特尔公司 用于在多旋翼无人机(uav)中强健的轨迹跟踪的通用控制器
CN107356254B (zh) * 2017-06-07 2021-01-12 东南大学 适用于地磁辅助导航航迹规划的粒子群优化方法
CN107289950B (zh) * 2017-07-28 2019-01-04 上海拓攻机器人有限公司 植保无人机作业航线规划方法及植保无人机
CN108919641B (zh) * 2018-06-21 2021-02-09 山东科技大学 一种基于改进樽海鞘算法的无人机航迹规划方法
CN109242162B (zh) * 2018-08-20 2020-11-24 国核电力规划设计研究院有限公司 风电场集电线路路径规划方法和装置
CN111307158B (zh) * 2020-03-19 2022-11-18 哈尔滨工程大学 一种auv三维航路规划方法
CN111562797B (zh) * 2020-07-06 2021-07-30 北京理工大学 确保收敛的无人机飞行时间最优实时轨迹优化方法
CN111930119B (zh) * 2020-07-31 2022-04-08 河海大学 流速自适应的无人船自主规划路径和运动精确跟踪方法
AU2020102302A4 (en) * 2020-09-16 2020-12-24 D, Shanthi DR Underwater robots design and control mechanism using particle swarm optimization algorithm

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112091946A (zh) * 2020-09-16 2020-12-18 哈尔滨工业大学 一种上置式多自由度绳驱并联机器人
CN112148009A (zh) * 2020-09-23 2020-12-29 北京智远弘业机器人科技有限公司 Agv集群调度方法、装置、电子设备及计算机可读存储介质

Also Published As

Publication number Publication date
CN112799414A (zh) 2021-05-14

Similar Documents

Publication Publication Date Title
Wang et al. Roboat: An autonomous surface vehicle for urban waterways
CN110333739B (zh) 一种基于强化学习的auv行为规划及动作控制方法
Polvara et al. Obstacle avoidance approaches for autonomous navigation of unmanned surface vehicles
Candeloro et al. A Voronoi-diagram-based dynamic path-planning system for underactuated marine vessels
Campbell et al. A review on improving the autonomy of unmanned surface vehicles through intelligent collision avoidance manoeuvres
Statheros et al. Autonomous ship collision avoidance navigation concepts, technologies and techniques
Hernández et al. Online motion planning for unexplored underwater environments using autonomous underwater vehicles
CN111399506A (zh) 一种基于动力学约束的全局-局部混合无人船路径规划方法
KR101990878B1 (ko) 시간상태 영역에서의 온라인 양방향 경로 계획 방법, 이를 구현하기 위한 프로그램이 저장된 기록매체 및 이를 구현하기 위해 매체에 저장된 컴퓨터프로그램
Guan et al. Autonomous collision avoidance of unmanned surface vehicles based on improved A-star and dynamic window approach algorithms
Majumder et al. Three dimensional D* algorithm for incremental path planning in uncooperative environment
Lin et al. Potential-field-based evolutionary route planner for the control of multiple unmanned aerial vehicles
Fu et al. Multi-behavior fusion based potential field method for path planning of unmanned surface vessel
CN112799414B (zh) 一种auv松弛轨迹规划方法
CN112947438B (zh) 基于全振荡型入侵野草优化算法的auv在线路径规划方法
Kuczkowski et al. Path planning algorithm for ship collisions avoidance in environment with changing strategy of dynamic obstacles
Hamad et al. Path Planning of Mobile Robot Based on Modification of Vector Field Histogram using Neuro-Fuzzy Algorithm.
Xu et al. Hybrid path planning method for USV using bidirectional A* and improved DWA considering the manoeuvrability and COLREGs
Lee et al. Nonlinear model predictive control with obstacle avoidance constraints for autonomous navigation in a canal environment
Tusseyeva et al. 3D global dynamic window approach for navigation of autonomous underwater vehicles
Raj et al. Dynamically feasible trajectory planning for anguilliform-inspired robots in the presence of steady ambient flow
Yu et al. A time dimension-added multiple obstacles avoidance approach for unmanned surface vehicles
Serigstad Hybrid collision avoidance for autonomous surface vessels
Algarín-Pinto et al. Adapted D* Lite to Improve Guidance, Navigation and Control of a Tail-Actuated Underwater Vehicle in Unknown Environments
Zhang et al. Q-learning Based Obstacle Avoidance Control of Autonomous Underwater Vehicle with Binocular Vision

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