CN109582027B - 一种基于改进粒子群优化算法的usv集群避碰规划方法 - Google Patents
一种基于改进粒子群优化算法的usv集群避碰规划方法 Download PDFInfo
- Publication number
- CN109582027B CN109582027B CN201910032577.3A CN201910032577A CN109582027B CN 109582027 B CN109582027 B CN 109582027B CN 201910032577 A CN201910032577 A CN 201910032577A CN 109582027 B CN109582027 B CN 109582027B
- Authority
- CN
- China
- Prior art keywords
- usv
- coordinate system
- sensor
- points
- puffing
- 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
Links
- 239000002245 particle Substances 0.000 title claims abstract description 66
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 46
- 238000005457 optimization Methods 0.000 title claims abstract description 32
- 238000000034 method Methods 0.000 title claims abstract description 25
- 230000001007 puffing effect Effects 0.000 claims description 23
- 230000004888 barrier function Effects 0.000 claims description 14
- 230000006870 function Effects 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000004458 analytical method Methods 0.000 claims description 6
- 230000008447 perception Effects 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 2
- 238000003672 processing method Methods 0.000 claims description 2
- 238000005096 rolling process Methods 0.000 abstract description 5
- 230000007547 defect Effects 0.000 abstract description 2
- 238000002121 ultrasonic speckle velocimetry Methods 0.000 description 57
- 230000008569 process Effects 0.000 description 11
- 230000000007 visual effect Effects 0.000 description 7
- 238000013461 design Methods 0.000 description 6
- 238000004088 simulation Methods 0.000 description 5
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000002068 genetic effect Effects 0.000 description 2
- 230000008961 swelling Effects 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000035939 shock Effects 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
Images
Classifications
-
- 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/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Biophysics (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Artificial Intelligence (AREA)
- Biomedical Technology (AREA)
- Life Sciences & Earth Sciences (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明属于无人水面艇避障领域,具体涉及一种基于改进粒子群优化算法的USV集群避碰规划方法。本发明主要包括以下步骤:根据导航雷达和光电传感器的参数特性建立USV综合视域模型;构建坐标系;构建环境模型;设计用于USV集群避碰规划的滚动优化策略和改进粒子群优化算法;将综合传感器探测到的信息及目标点的信息输入到改进粒子群优化算法中,获得USV下一时刻的航行和航速的调整指令。本发明不仅克服了标准粒子群优化算法易于陷入局部最优的缺点,而且结合了USV当前环境信息,提高了USV避碰规划的实时性,在适应度函数中加入了USV的转角优化,在获得最优路径的同时也提高了路径的平滑性。
Description
技术领域
本发明属于无人水面艇避障领域,具体涉及一种基于改进粒子群优化算法的USV集群避碰规划方法。
背景技术
近几年来,越来越多的国家注重海上力量的发展,无人水面艇以其体积小、灵活性高和作战能力强的优势成为了研究的热点。无人水面艇不仅可以自主航行和自主规划,还能完成目标跟踪和港湾巡逻等任务,所以无论在军事领域还是民用领域,无人水面艇都发挥着越来越重要的作用。然而,面对未知的工作环境和多样化的任务使命时,单一USV显得势单力薄并不能高效的完成任务,而由多艘USV联合起来构成的集群***,鲁棒性和机动性强、作业效率高且作业范围也比较广,USV集群***的这些优势受到了许多国家的青睐,在未来的一段时间内,无人水面艇的智能化、体系化、标准化将是各国发展的重点,无人水面艇集群在与其它武器的综合协同作战中,也将扮演更加重要的角色,而无人水面艇集群***对未来海上战场的整体面貌也将会产生革命性的影响。海洋环境复杂多变,无论是岛屿、礁石还是一些其他的动态物体都可能会对无人艇集群得航行构成一定的威胁,而集群中的各USV之间也必须保持一定的安全距离,这就要求无人水面艇集群在完成各项任务过程中必须具备一定的自主避碰能力,无人水面艇集群自主避碰的能力是保障其高效、安全地完成各项使命任务的关键技术之一。
人工势场法、可视图法、滚动窗口法等传统方法以及遗传算法、蚁群算法、粒子群算法、神经网络算法等智能算法是目前常用的避碰规划算法。大部分避碰规划算法都不能在短时间内为USV规划出一条无避碰最优路径,传统方法如人工势场法,虽然具有简单的原理、较小的计算量、较快运算速度,但容易找不到全局最优解,甚至在避碰过程中出现震荡现象,群智能算法如遗传算法虽然可以避免陷入局部极值点,但是原理复杂,计算量大,且规划时间较长,因此,为USV设计一种简单、快速、实时在线的避碰规划算法是至关重要的。采用基于滚动优化策略和粒子群优化算法相结合的方法可以实现USV集群的自主避碰,该算法利用传感器实时感知获得的局部环境信息来进行运算,使用粒子群优化算法快速为集群中的每个USV在综合视域内规划出一条无避碰最优路径。
发明内容
本发明的目的在于提供一种基于改进粒子群优化算法的USV集群避碰规划方法。
一种基于改进粒子群优化算法的USV集群避碰规划方法,主要包括以下步骤:
步骤1:根据导航雷达和光电传感器的参数特性建立USV综合视域模型;
步骤2:构建坐标系;
步骤3:构建环境模型;
步骤4:设计用于USV集群避碰规划的滚动优化策略和改进粒子群优化算法;
步骤5:将综合传感器探测到的信息及目标点的信息输入到改进粒子群优化算法中,获得USV下一时刻的航行和航速的调整指令。
步骤2中建立三个坐标系:全局环境坐标系XGOGYG、基于USV的局部坐标系XUOUYU和基于传感器综合感知信息的局部坐标系XRORYR;
全局坐标系XGOGYG采用北东坐标系,左下角为坐标原点,X轴指向正北方向,Y轴指向正东方向;
局部坐标系分为船体坐标系和传感器坐标系,船体坐标系的原点位于USV的重心处,导航雷达和光电传感器安装在距离船体中心a处,其X轴和Y轴平行于USV局部坐标系的X轴和Y轴。
步骤3中构建环境模型具体包括以下步骤:
(3.1)数据采集:导航雷达和光电传感器的感知范围为360°,假设每隔1°的角度上传感器获取且只能获取一个距离信息,按逆时针方向角度从0°变化至360°,即传感器每扫描一次便获得361组感知数据点,这些数据点包含两个参数,第一个参数是点与传感器之间的角度,第二个参数点到传感器的距离;
(3.2)将感知数据点提取为线段特征:首先,通过区域划分的方式将点集些划分为多个区域,然后再将每个区域内数据点集进行线性拟合,进而提取为线段特征;
(3.3)静态障碍物膨化:对静态障碍物的膨化只是对其部分进行膨化,膨化方式主要分为以下三种情况:单线段的膨化、相邻线段的膨化和曲线的膨化;
(3.4)动态障碍物的轨迹预测:首先将动态障碍物膨化为一个圆形,然后根据自回归模型预测出动态障碍物在全局坐标系下的位置和方向。
步骤4具体包括:USV在运动过程中,每次以综合视域内的子目标点规划局部路径,然后使用改进粒子群优化算法优化每个综合视域内的窗口,粒子群优化算法的改进包括惯性权重和学习因子的设计、适应度函数的选择。
USV在运动过程中,每次以综合视域内的子目标点规划局部路径,运动过程中将执行以下步骤:
(4.1)USV通过自身携带的传感器获取综合视域内的环境信息;
(4.2)从起点到子目标点利用粒子群算法规划出一条无避碰最优路经;
(4.3)USV将沿着规划出的路径从起点运动到子目标点;
(4.4)以此子目标为起点,返回步骤1,直到USV行驶到全局的终点。
采取基于正切函数的惯性调整策略改进设计的惯性权重ω的值,公式为:
式中,α=π/4,m为控制因子,k为当前迭代次数,kmax为最大迭代次数;
采取基于线性调整策略改进设计学习因子c1和c2的值,公式为:
式中k为当前迭代次数,kmax为最大迭代次数,c1s=2.5,c2s=1.0,c1e=1.0,c2e=2.5;
适应度函数的选择:
已知USV的起始点为(x0,y0),目标点为(xn,yn),子路径点为(xi,yi),其中i=0,1,2…n-1,n,则适应度函数定义为:
根据USV的每次转角定义的适应度函数为:
结合上述公式,得出USV在避碰规划过程中总的适应度函数:
f=λ1f1+λ2f2。
粒子群优化算法的具体包括以下步骤:
Step 1:初始化粒子群算法中的参数:ωs、ωe、c1s、c1e、c2s、c2e,种群数量PNUM,最大迭代次数kmax,令k=0;
Step 2:初始化粒子种群,即粒子的速度和位置,将粒子个体的历史最优值设为pbest,群体最优值设置为gbest;
Step 3:在当代的进化中,计算每个粒子的适应度函数值f;
Step 4:更新粒子的pbest和gbest;
Step 5:更新粒子的速度和位置;
Step 6:若满足终止条件,输出gbest并结束,否则转Step 5。
粒子的更新为以下公式:
其中,c1、c2为学习因子,通常情况下c1+c2≤4,r1、r2为[0,1]之间的随机数,ω为惯性权重,pid为个体最优值,gid为全局最优值,下标“i”、“d”分别表示第i个粒子和第i个粒子维数。
本发明的有益效果在于:
本发明通过在集群中每个USV的综合视域内产生同心圆进行建模,利用导航雷达和光电传感器实时测得的综合环境信息与改进后粒子群优化算法相结合以滚动的方式来规划每个USV综合视域内的无避碰路径,不仅克服了标准粒子群优化算法易于陷入局部最优的缺点,而且结合了USV当前环境信息,提高了USV避碰规划的实时性,在适应度函数中加入了USV的转角优化,在获得最优路径的同时也提高了路径的平滑性。
附图说明
图1是本发明的综合视域模拟图;
图2是本发明的坐标示意图;
图3(a)是单线段的膨化图;图3(b)是相邻线段的膨化图;
图4(a)是凸曲线的膨化图;图4(b)是凹曲线的膨化图;
图5(a)是障碍物膨化后的可视切线图;图5(b)是当USV行驶一段距离后障碍物膨化后的可视切线图;
图6是本发明的算法流程图;
图7是本发明的仿真效果图。
具体实施方式
下面结合附图对本发明进一步详细说明。
根据导航雷达和光电传感器的参数特性建立USV综合视域模型;
导航雷达和光电传感器的参数特性如表1所示:
表1 USV常用传感器性能参数表
构建全局坐标系和局部坐标系
如图2所示,USV的局部坐标系XUOUYU的原点位于USV的重心处,以当前USV的前进方向为X轴正方向,垂直于X轴且指向USV右舷侧的方向为Y轴正方向。导航雷达和光电传感器安装在距离船体中心a处,其X轴和Y轴平行于USV局部坐标系的X轴和Y轴,Qi为传感器感知到的障碍,它与传感器的距离为d,角度为θ,USV在全局环境坐标系的坐标为(xu,yu),αu是USV的艏向与全局环境坐标轴XG(北向)的夹角。
将传感器数据的坐标(xi,yi)转化为全局坐标系中的坐标的(xg,yg)的公式为:
障碍物的膨化
1.单线段的膨化。如图3(a)所示假定某线段的两个端点分别为a1、a2,USV与障碍物的安全航行距离为ds,则将a1延长ds长度到a11,a2延长ds长度到a22,过a11做a11a22的垂线,在垂线上取两点b1和b2,b1和b2与a11的距离均为ds,同理,过a22做a11a22的垂线,b3与b4便是垂线上距离a22均为ds的两点,连接b1b2b4b3便是该线段膨化后的多边形。
2.相邻线段的膨化。如图3(b)所示,对于相邻线段组成的折线,首先,按照单线段的处理方法将每条线段膨化为一个四边形,然后,合并相邻的四边形,对于两个四边形相交的部分,则将它们的交点e4作为合并后新多边形顶点,舍弃部分其它的交点和原有的部分顶点,如果一个多边形的顶点出现在另一个多边形内,那么这个顶点便要舍弃,最后,两个四边形合称为一个多边形,将相邻的两个线段膨化后,通过合并便形成了一个含有六个顶点e1e3e2e4e5e6的多边形。
3.凸曲线的膨化。凸曲线的膨化如图4(a)所示,首先,连接凸曲线的两个端点h1、h2,分别延长ds长度到h11和h22,过h11做h11h22的垂线,取垂线上一点h12,h12到h11的距离为ds,同理,过h22做h11h22的垂线,取垂线上一点h21,h21到h22的距离为ds,曲线连接h12和h21,h12h21就为曲线h11h22膨化后的曲线。
4.凹曲线的膨化。凹曲线的膨化如图4(b)所示,首先,连接凸曲线的两个端点g1、g2,过g1做g1g2的垂线,取垂线上一点g11,g11到g1的距离为ds,同理,过g2做g1g2的垂线,取垂线上一点g22,g22到g2的距离为ds,曲线连接g11和g22,g11g22就为曲线g1g2膨化后的曲线。
如图5(a)所示,在USV的综合视域内导航雷达和光电传感器只能检测到障碍物1、障碍物2和障碍物3的一部分,即实线表示的部分,而且障碍物3有一部分被障碍物1遮挡,障碍物1的可视部分经过膨化后的边界点为l1和l2,障碍物2的可视部分经过膨化后的边界点为l3和l4,障碍物3的可视部分边界经过膨化后的边界点为l5和l6,连接Ol1、Ol2、Ol3、Ol4、Ol5和Ol6,其中,l1、l2、l3和l4称为膨化可视切点,l5和l6称为膨化伪可视切点,Ol1、Ol2、Ol3和Ol4称为膨化可视切线,Ol5和Ol6称为膨化伪可视切线。当USV行驶一段距离后,如图5(b)所示,视点位于O1处时,l7和l8便是视点O1的膨化可视切点,O1l7和O1l8是其膨化可视切线。
障碍物位置预测的自回归模型
根据上述分析可知,USV通过传感器可以获得t时刻动态障碍物的速度和位置信息,而动态障碍物下一时刻的位置和方向则可以通过自回归模型预测出,假设第k个动态障碍物在t时刻的位置为Lk(t)=(xob,yob),障碍物的位置序列用{Lk(t),t=1,2…}表示,则可以用公式表示第k个动态障碍物的位置的n阶自回归模型:
其中,e(t)为预测误差,系数αi为2×2的矩阵。
通过将取样的时间缩短可以使动态障碍物的加速度变化缓慢,因此可以采用一阶自回归模型对动态障碍物的加速度{ak(t),t=1,2…}进行建模:
ak(t)=βk,tak(t-1)+w(t)
其中,w(t)为预测误差,βk,t为加速度模型系数。
假设t时刻动态障碍物的速度为vk(t),则其加速度和位置的关系如下所示:
ak(t)=vk(t)-vk(t-1)
=|Lk(t)-Lk(t-1)|-|Lk(t-1)-Lk(t-2)|
=Lk(t)-2Lk(t-1)+Lk(t-2)
通过上述分析可预测出第k个动态障碍物在t+1时刻的位置为:
Lk(t)=(2+βk,t)Lk(t-1)+(1+2βk,t)Lk(t-2)+βk,tLk(t-3)+w(t)
Lk(t+1)=Lk(t)+vk(t)+βkak(t)
USV在运动过程中,每次以综合视域内的子目标点规划局部路径,运动过程中将执行以下步骤:
(4.1)USV通过自身携带的传感器获取综合视域内的环境信息;
(4.2)从起点到子目标点利用粒子群算法规划出一条无避碰最优路经;
(4.3)USV将沿着规划出的路径从起点运动到子目标点;
粒子群优化算法的自我更新:
(4.4)以此子目标为起点,返回步骤1,直到USV行驶到全局的终点。
在每次迭代中,粒子通过两个“极值”来评价和更新自己:第一个就是粒子本身在历史搜索过程中找到的无避碰最优子路径点,称之为个体极值,另一个是整个种群找到的无避碰最优子路径点,称为全局极值。粒子的自我更新如公式:
其中,c1、c2为学习因子,通常情况下c1+c2≤4,r1、r2为[0,1]之间的随机数,ω为惯性权重,pid为个体最优值,gid为全局最优值,下标“i”、“d”分别表示第i个粒子和第i个粒子维数,其中粒子的维数d的值为2。
粒子群优化算法的参数设计
ω的值越大,粒子的全局搜索能力就越强,同时也加快了算法的收敛速度,但是问题的最优解不易获得,ω的值变小,局部搜索能力变强,优化问题的最优解也容易获得,但是算法的收敛速度将变慢,甚至陷入停滞状态。在粒子群优化算法迭代前期,为了尽可能的发现更大的解空间,粒子需要有较强的全局搜索能力,而到后期的时候,粒子需要加强局部搜索能力,尽可能的搜寻最优解。因此,随着迭代次数的增加,惯性权重ω应不断减小。采取基于正切函数的惯性调整策略改进设计ω的值,如公式:
式中,α=π/4,ωs=0.9,ωe=0.4,m为控制因子,其值为0.5,k为当前迭代次数,kmax为最大迭代次数,ωk随着迭代次数的增加从0.9线性递减到0.4。
在算法初期,粒子的全局搜索能力要强,以便尽可能的探测全局空间,到了算法后期,粒子局部搜索将是寻找全局最优解的关键,因此要加强粒子的局部搜索能力,因此,在迭代过程中c1应逐渐减小,c2应逐渐增大。采取基于线性调整策略改进设计学习因子的调节,如公式:
式中k为当前迭代次数,kmax为最大迭代次数,c1s=2.5,c2s=1.0,c1e=1.0,c2e=2.5,c1随着迭代次数的增加从2.5非线性递减到1.0,c2从1.0非线性增加到2.5。
种群规模PNUM的大小为80。
已知USV的起始点为(x0,y0),目标点为(xn,yn),子路径点为(xi,yi),其中i=0,1,2…n-1,n,则适应度函数定义为:
根据USV的每次转角定义的适应度函数为:
结合以上分析,得出USV在避碰规划过程中总的适应度函数:
f=λ1f1+λ2f2
其中λ1=0.8,λ2=20。
根据以上分析可得出粒子群优化算法的具体步骤如下:
Step 1:初始化粒子群算法中的参数:ωs、ωe、c1s、c1e、c2s、c2e,种群数量PNUM,最大迭代次数kmax,令k=0;
Step 2:初始化粒子种群,即粒子的速度和位置,将粒子个体的历史最优值设为pbest,群体最优值设置为gbest;
Step 3:在当代的进化中,计算每个粒子的适应度函数值f;
Step 4:更新粒子的pbest和gbest;
Step 5:更新粒子的速度和位置;
Step 6:若满足终止条件,输出gbest并结束,否则转Step 5。
本次发明以Qt为开发平台搭建仿真实验环境,并根据导航雷达与光电传感器的参数特性模拟USV综合探测周围环境信息,在仿真实验中,模拟环境的大小为130×110,单位为千米,导航雷达与光电传感器只能探测到障碍物的边界信息,其中导航雷达的探测半径为10千米,光电传感器的探测半径为15千米。本次仿真实验共有3艘USV,其初始速度大小均为40节,为了方便分析,3艘USV的起始点和终点是确定的,环境中的静态障碍物包括岛屿,如圆型障碍物和不规则障碍,人工放置的多边形障碍,如三角形、四边形和五边形,动态障碍物为一个小型的等边三角形,粒子群优化算法的最大迭代次数为200次,种群规模为80,算法的计算时间为292ms。3艘USV在整个环境下的避碰规划过程如图7所示。
Claims (1)
1.一种基于改进粒子群优化算法的USV集群避碰规划方法,其特征在于,主要包括以下步骤:
步骤1:根据导航雷达和光电传感器的参数特性建立USV综合视域模型;
步骤1.1:构建坐标系;
建立三个坐标系:全局环境坐标系XGOGYG、基于USV的局部坐标系XUOUYU和基于传感器综合感知信息的局部坐标系XRORYR;
全局坐标系XGOGYG采用北东坐标系,左下角为坐标原点,X轴指向正北方向,Y轴指向正东方向;
基于USV的局部坐标系XUOUYU的原点位于USV的重心处,以当前USV的前进方向为X轴正方向,垂直于X轴且指向USV右舷侧的方向为Y轴正方向,导航雷达和光电传感器安装在距离船体中心a处,基于传感器综合感知信息的局部坐标系XRORYR的X轴和Y轴平行于USV局部坐标系的X轴和Y轴,Qi为传感器感知到的障碍,它与传感器的距离为d,角度为θ;USV在全局环境坐标系的坐标为(xu,yu),αu是USV的艏向与全局环境坐标轴XG的夹角;
将传感器数据的坐标(xi,yi)转化为全局坐标系中的坐标的(xg,yg)的公式为:
步骤1.2:构建环境模型;
步骤1.2.1:数据采集;
导航雷达和光电传感器的感知范围为360°,每隔1°的角度上传感器只能获取一个距离信息,按逆时针方向角度从0°变化至360°,即传感器每扫描一次便获得361组感知数据点,这些感知数据点包含两个参数,第一个参数为感知数据点与传感器之间的角度,第二个参数为感知数据点到传感器的距离;
步骤1.2.2:将感知数据点提取为线段特征;通过区域划分的方式将感知数据点划分为多个区域,然后再将每个区域内感知数据点集进行线性拟合,进而提取为线段特征;
步骤1.2.3:静态障碍物膨化,膨化方式包括单线段的膨化、相邻线段的膨化、凸曲线的膨化、凹曲线的膨化;
单线段的膨化:某线段的两个端点分别为a1、a2,USV与障碍物的安全航行距离为ds,则将a1延长ds长度到a11,a2延长ds长度到a22,过a11做a11a22的垂线,在垂线上取两点b1和b2,b1和b2与a11的距离均为ds;同理,过a22做a11a22的垂线,b3与b4便是垂线上距离a22均为ds的两点,连接b1b2b4b3便是该线段膨化后的多边形;
相邻线段的膨化:对于相邻线段组成的折线,首先按照单线段的处理方法将每条线段膨化为一个四边形,然后,合并相邻的四边形,对于两个四边形相交的部分,则将它们的交点e4作为合并后新多边形顶点,舍弃部分其它的交点和原有的部分顶点;如果一个多边形的顶点出现在另一个多边形内,那么这个顶点便要舍弃,最后,两个四边形合称为一个多边形,将相邻的两个线段膨化后,通过合并便形成了一个含有六个顶点e1e3e2e4e5e6的多边形;
凸曲线的膨化:连接凸曲线的两个端点h1、h2,分别延长ds长度到h11和h22,过h11做h11h22的垂线,取垂线上一点h12,h12到h11的距离为ds;同理,过h22做h11 h22的垂线,取垂线上一点h21,h21到h22的距离为ds,曲线连接h12和h21,h12 h21就为曲线h11h22膨化后的曲线;
凹曲线的膨化,连接凸曲线的两个端点g1、g2,过g1做g1g2的垂线,取垂线上一点g11,g11到g1的距离为ds;同理,过g2做g1g2的垂线,取垂线上一点g22,g22到g2的距离为ds,曲线连接g11和g22,g11g22就为曲线g1g2膨化后的曲线;
步骤1.2.4:动态障碍物的轨迹预测;将动态障碍物膨化为一个圆形,根据自回归模型预测出动态障碍物在全局坐标系下的位置和方向;
假设第k个动态障碍物在t时刻的位置为Lk(t)=(xob,yob),障碍物的位置序列用{Lk(t),t=1,2…}表示,则第k个动态障碍物的位置的n阶自回归模型为:
其中,e(t)为预测误差,系数αi为2×2的矩阵;
采用一阶自回归模型对动态障碍物的加速度{ak(t),t=1,2…}进行建模:
ak(t)=βk,tak(t-1)+w(t)
其中,w(t)为预测误差,βk,t为加速度模型系数;
假设t时刻动态障碍物的速度为vk(t),则其加速度和位置的关系如下所示:
ak(t)=vk(t)-vk(t-1)
=|Lk(t)-Lk(t-1)|-|Lk(t-1)-Lk(t-2)|
=Lk(t)-2Lk(t-1)+Lk(t-2)
通过分析可预测出第k个动态障碍物在t+1时刻的位置为:
Lk(t)=(2+βk,t)Lk(t-1)+(1+2βk,t)Lk(t-2)+βk,tLk(t-3)+w(t)
Lk(t+1)=Lk(t)+vk(t)+βk,tak(t);
步骤2:以综合视域内的子目标点规划局部路径,从起点到子目标点利用粒子群算法规划出一条无避碰最优路经;
步骤2.1:初始化粒子群算法中的参数:ωs、ωe、c1s、c1e、c2s、c2e,种群数量PNUM,最大迭代次数kmax,令当前迭代次数k=0;已知USV的起始点为(x0,y0),目标点为(xn,yn),子路径点为(xi,yi),i=0,1,2…n-1,n;
步骤2.2:初始化粒子种群,即粒子的速度和位置,将粒子个体的历史最优值设为pbest,群体最优值设置为gbest;
步骤2.3:在当代的进化中,计算每个粒子的适应度函数值f;
f=λ1f1+λ2f2
其中,λ1、λ2为设定的常数;
步骤2.4:更新粒子的pbest和gbest;
步骤2.5:更新粒子的速度和位置;
其中,c1、c2为学习因子;r1、r2为[0,1]之间的随机数;ω为惯性权重;pid为个体最优值;gid为全局最优值;下标i表示第i个粒子;下标d表示粒子维数,d=2;α=π/4,m为控制因子;
步骤2.6:若满足终止条件,输出gbest并结束,否则,转步骤2.5;
步骤3:USV将沿着规划出的路径从起点运动到子目标点;
步骤4:若USV行驶到全局的终点,则停止;否则,以此子目标为起点,返回步骤1。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910032577.3A CN109582027B (zh) | 2019-01-14 | 2019-01-14 | 一种基于改进粒子群优化算法的usv集群避碰规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910032577.3A CN109582027B (zh) | 2019-01-14 | 2019-01-14 | 一种基于改进粒子群优化算法的usv集群避碰规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109582027A CN109582027A (zh) | 2019-04-05 |
CN109582027B true CN109582027B (zh) | 2022-02-22 |
Family
ID=65915068
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910032577.3A Active CN109582027B (zh) | 2019-01-14 | 2019-01-14 | 一种基于改进粒子群优化算法的usv集群避碰规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109582027B (zh) |
Families Citing this family (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110632941B (zh) * | 2019-09-25 | 2020-12-15 | 北京理工大学 | 一种复杂环境下无人机目标跟踪的轨迹生成方法 |
CN111124762B (zh) * | 2019-12-30 | 2023-11-14 | 航天科工网络信息发展有限公司 | 一种基于改进粒子群算法的动态副本放置方法 |
CN111142532B (zh) * | 2020-01-03 | 2023-04-28 | 三一汽车制造有限公司 | 基于5g网络通讯的无人驾驶压路机群避障方法和*** |
CN111413966B (zh) * | 2020-03-12 | 2022-04-29 | 天津大学 | 一种递进式模型预测无人驾驶规划跟踪协同控制方法 |
CN111563657B (zh) * | 2020-04-10 | 2022-11-15 | 福建电子口岸股份有限公司 | 一种通过蚁群算法结合多维度策略解决港口拖轮调度的方法 |
CN111505935B (zh) * | 2020-04-30 | 2022-10-18 | 内蒙古工业大学 | 一种自动引导车控制方法和*** |
CN112231793B (zh) * | 2020-09-14 | 2023-04-18 | 重庆大学产业技术研究院 | 一种基于二进制粒子群优化算法的钢筋混凝土构件交叉区域钢筋自动避障排布方法 |
CN112213948B (zh) * | 2020-10-23 | 2022-06-10 | 武汉理工大学 | 一种基于mpc的船舶航速协同控制方法 |
CN112405547B (zh) * | 2020-11-30 | 2022-03-08 | 湖南科技大学 | 未知环境下的群机器人多目标搜索方法 |
CN112434378B (zh) * | 2020-12-09 | 2024-02-02 | 扬州中远海运重工有限公司 | 一种造船企业线表绘制交互控件设计方法 |
CN112947447B (zh) * | 2021-02-10 | 2023-11-24 | 大连理工大学 | 一种基于同步规划-控制策略的水面无人舰艇自主导航方法 |
CN113625780B (zh) * | 2021-10-12 | 2022-01-28 | 北京大学 | 一种避免死锁的分布式无人集群协同运动路径规划方法 |
CN114326702B (zh) * | 2021-11-25 | 2023-09-22 | 江苏科技大学 | 一种基于模型预测控制的内河无人船避碰方法 |
CN114237254B (zh) * | 2021-12-20 | 2024-01-09 | 中国海洋大学 | 基于Smith预估器和粒子群算法的无人船舵向PID控制设计方法 |
CN114844403B (zh) * | 2022-03-11 | 2023-06-23 | 西北工业大学 | 基于改进粒子群算法的电机驱动器pid参数自整定方法 |
CN116448115B (zh) * | 2023-04-07 | 2024-03-19 | 连云港杰瑞科创园管理有限公司 | 基于导航雷达和光电的无人艇概率距离地图构建方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8285655B1 (en) * | 2004-02-03 | 2012-10-09 | Hrl Laboratories, Llc | Method for object recongnition using multi-layered swarm sweep algorithms |
CN103336526A (zh) * | 2013-06-20 | 2013-10-02 | 苏州经贸职业技术学院 | 基于协同进化粒子群滚动优化的机器人路径规划方法 |
CN105717929A (zh) * | 2016-04-29 | 2016-06-29 | 中国人民解放军国防科学技术大学 | 一种多分辨率障碍物环境下移动机器人混合路径规划方法 |
-
2019
- 2019-01-14 CN CN201910032577.3A patent/CN109582027B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8285655B1 (en) * | 2004-02-03 | 2012-10-09 | Hrl Laboratories, Llc | Method for object recongnition using multi-layered swarm sweep algorithms |
CN103336526A (zh) * | 2013-06-20 | 2013-10-02 | 苏州经贸职业技术学院 | 基于协同进化粒子群滚动优化的机器人路径规划方法 |
CN105717929A (zh) * | 2016-04-29 | 2016-06-29 | 中国人民解放军国防科学技术大学 | 一种多分辨率障碍物环境下移动机器人混合路径规划方法 |
Non-Patent Citations (4)
Title |
---|
A hierarchical global path planning approach for mobile robots based on multi-objective particle swarm optimization;Mac,Thi Thoa等;《Applied Soft Computing》;20171231;第59卷;第68-76页第1-8节 * |
基于粒子群算法的路径规划问题研究;吴高超;《中国优秀硕士学位论文全文数据库 信息科技辑》;20170115;第I140-50页第2.3章 * |
基于自回归模型的移动机器人路径规划;槐创锋 等;《计算机测量与控制》;20100831;第18卷(第8期);第1902-1905页第0-5节 * |
采用粒子群优化和B样条曲线的改进可视图路径规划算法;吕太之 等;《华侨大学学报(自然科学版)》;20180228;第39卷(第1期);第104-108页第1-3节 * |
Also Published As
Publication number | Publication date |
---|---|
CN109582027A (zh) | 2019-04-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109582027B (zh) | 一种基于改进粒子群优化算法的usv集群避碰规划方法 | |
CN110632931B (zh) | 动态环境下基于深度强化学习的移动机器人避碰规划方法 | |
CN109241552B (zh) | 一种基于多约束目标的水下机器人运动规划方法 | |
CN110456823B (zh) | 一种针对无人机计算与存储能力受限的双层路径规划方法 | |
CN108319293B (zh) | 一种基于lstm网络的uuv实时避碰规划方法 | |
CN109765929B (zh) | 一种基于改进rnn的uuv实时避障规划方法 | |
CN114625151B (zh) | 一种基于强化学习的水下机器人避障路径规划方法 | |
CN113033119B (zh) | 一种基于双评论家强化学习技术的水下航行器目标区域上浮控制方法 | |
CN108334677B (zh) | 一种基于gru网络的uuv实时避碰规划方法 | |
CN109144102A (zh) | 一种基于改进蝙蝠算法的无人机航路规划方法 | |
CN109784201B (zh) | 基于四维风险评估的auv动态避障方法 | |
CN109871031B (zh) | 一种固定翼无人机的轨迹规划方法 | |
CN110488835B (zh) | 一种基于双反向传播神经网络的无人***智能局部路径规划方法 | |
CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制*** | |
CN110906935A (zh) | 一种无人艇路径规划方法 | |
Sun et al. | A novel fuzzy control algorithm for three-dimensional AUV path planning based on sonar model | |
CN105372653B (zh) | 面向岸基空管雷达***中一种高效转弯机动目标跟踪方法 | |
Yan et al. | A novel path planning for AUV based on objects’ motion parameters predication | |
CN115201799A (zh) | 一种用于声呐的时变卡尔曼滤波跟踪方法 | |
CN115373415A (zh) | 一种基于深度强化学习的无人机智能导航方法 | |
Zhang et al. | Intelligent vector field histogram based collision avoidance method for auv | |
CN112990549B (zh) | 一种空间非合作目标抵近绕飞观测轨迹优化方法 | |
Li et al. | An efficient deep reinforcement learning algorithm for Mapless navigation with gap-guided switching strategy | |
CN114609925B (zh) | 水下探索策略模型的训练方法及仿生机器鱼水下探索方法 | |
CN114167856A (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 |