CN106444835B - 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 - Google Patents
基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 Download PDFInfo
- Publication number
- CN106444835B CN106444835B CN201610887891.6A CN201610887891A CN106444835B CN 106444835 B CN106444835 B CN 106444835B CN 201610887891 A CN201610887891 A CN 201610887891A CN 106444835 B CN106444835 B CN 106444835B
- Authority
- CN
- China
- Prior art keywords
- node
- path
- value
- point
- 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
Links
- 239000002245 particle Substances 0.000 title claims abstract description 77
- 238000000034 method Methods 0.000 title claims abstract description 20
- 238000005457 optimization Methods 0.000 claims abstract description 6
- 230000004888 barrier function Effects 0.000 claims description 31
- 230000003044 adaptive effect Effects 0.000 claims description 25
- 238000011156 evaluation Methods 0.000 claims description 8
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 5
- 238000003780 insertion Methods 0.000 claims description 4
- 230000037431 insertion Effects 0.000 claims description 4
- 230000006978 adaptation Effects 0.000 claims description 3
- 230000001174 ascending effect Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 230000000644 propagated effect Effects 0.000 claims description 2
- 238000007689 inspection Methods 0.000 claims 1
- 241001479919 Ardeola grayii Species 0.000 description 1
- 241000254158 Lampyridae Species 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000004744 fabric Substances 0.000 description 1
- 230000002028 premature Effects 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
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/10—Simultaneous control of position or course in three dimensions
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)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明提供的是一种基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法。步骤1.航行空间的建模;步骤2.建立Lazy Theta星算法代价函数;步骤3.以z=zsafemin水平面为Lazy Theta星算法二维路径规划平面,以路径起点S的X、Y轴坐标(xs,ys)和路径终点D的X、Y轴坐标(xd,yd)为二维路径的起点和终点,在水平面内进行二维路径规划;步骤4.根据三维路径规划寻找一条无碰长度最短路径的优化目标,设计深度规划评价函数;步骤5.利用粒子群算法进行深度规划;步骤6.输出最优三维路径。本发明通过对三维问题的简化,结合两种不同算法的优点,降低算法的计算复杂度,提高三维路径规划的快速性和可靠性。
Description
技术领域
本发明涉及的是一种水下潜器路径规划方法。具体地说是一种水下潜器的三维路径规划方法。
背景技术
三维航路规划是水下潜器等具有空间移动能力的智能机器执行水下任务的关键技术之一,越来越受到人们的重视。现有的三维路径规划算法大部分由二维路径规划算法发展而来,在二维路径规划中,A星算法应用广泛。例如申请号为201110172301.9的专利文件中,提出了一种游戏路径搜索的简化方法;申请号为201410531309.3的专利文件中,他提出了一种基于二叉堆节点排序的A星寻路方法系统;申请号为201410010003.3的专利文件中,提出了一种基于自适应A星算法的飞行器最优路径确定方法。上述三个技术方案中利用A星算法,能够快速搜索得到二维空间中一条无碰最短路径。然而A星算法搜索得到的路径有两个明显的缺点,一是路径长度比实际最短路径长,二是存在许多折角需要进行后期加工平滑。Lazy Theta星算法是解决A星算法不足的一种变体。Lazy Theta星算法不仅路径不受地图网格边缘限制,而且路径节点少,路径长度短,但是在三维问题求解上适应性不高,高维大范围问题的求解计算复杂度仍很大。
三维环境相比于二维环境无论在地形障碍物建模或是路径搜索上都更为复杂。目前备受关注的三维路径规划的算法是启发式智能优化算法,如粒子群算法、蚁群算法、萤火虫算法等。例如申请号为201210178003.5的专利文件中,公开了一种基于粒子群算法的自适应三维空间路径规划方法;申请号为201310744400.9的专利文件中,公开了一种基于布谷鸟搜索算法的水下潜器三维路径规划方法;申请号为201310235123.9的专利文件中,公开了一种基于高程图和蚁群寻食的三维路径规划方法。这些方法都在三维路径规划问题的求解上有较强的智能性和适应性,然而在大范围路径搜索上依然存在计算复杂度大、盲目性大等问题。
发明内容
本发明的目的在于提供一种能降低计算复杂度,提高三维路径规划的快速性和可靠性的基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法。
本发明的目的是这样实现的:
步骤1.以起点S和终点D为基准建立航行空间,以水下潜器的最小安全潜行深度zsafemin形成的水平面z为基准,建立Lazy Theta星算法二维障碍物栅格地图,z=zsafemin;
步骤2.建立Lazy Theta星算法代价函数;
步骤3.以z=zsafemin水平面为Lazy Theta星算法二维路径规划平面,以路径起点S的X、Y轴坐标(xs,ys)和路径终点D的X、Y轴坐标(xd,yd)为二维路径的起点和终点,在水平面内进行二维路径规划;
步骤4.根据三维路径规划寻找一条无碰长度最短路径的优化目标,设计深度规划评价函数;
步骤5.利用粒子群算法进行深度规划;
步骤6.输出最优三维路径。
本发明还可以包括:
1、步骤3具体包括:栅格地图中的网格点总数为n,具体步骤如下,
步骤3.1.定义OPEN和CLOSED两个表;
步骤3.2.初始化OPEN表和CLOSED表;
步骤3.3.判断OPEN表是否为空,为空则搜索路径失败,若不为空,执行步骤3.4;
步骤3.4.把OPEN表中表头网格点Pi移入CLOSED表中,i<n;
步骤3.5.判断节点Pi是否是终点D,若是,则路径搜索成功,转至步骤3.9,若不是,执行步骤3.6;
步骤3.6.对节点Pi和其父节点进行障碍物检测,若Pi和连线之间没有障碍物,保留为Pi的父节点,否则,更新节点Pi的适应值和父节点;
步骤3.7.扩展Pi的东、西、南、北、东南、东北、西南、西北八个方向地理位置相邻的子节点Pj,j<n,j≠i;
步骤3.8.对OPEN表中各个网格点按照适应值的大小进行升序排序,返回步骤3.3;
步骤3.9.算法终止,输出包含路径起点和终点的二维路径序列P(P1,P2,...Pk),k为最终的路径点的个数。
2、所述扩展Pi的东、西、南、北、东南、东北、西南、西北八个方向地理位置相邻的子节点Pj,具体包括以下几种情况:
步骤3.7.1.如果Pj不在OPEN表或者CLOSED表中,将其存放在OPEN表中,设Pi的父节点为设置Pj父节点为
步骤3.7.2.如果Pj在OPEN表中,重新计算在OPEN表中的适应值F(Pj),若新适应值小于原有适应值,更新节点Pj的适应值和父节点,若新适应值大于原有适应值,保持原有适应值和父节点的设置;
步骤3.7.3.如果Pj已经存在于CLODED表中或者是障碍物点,忽略此节点。
3、所述利用粒子群算法进行深度规划的具体步骤为:
步骤5.1.初始化粒子群算法参数;
步骤5.2.随机产生一组初始解;
步骤5.3.根据步骤4确定的深度规划路径评价函数计算各个粒子的适应度值,更新粒子的个体最优值和种群最优值;
步骤5.4.计算当前迭代中粒子群的惯性权重和学习因子;
步骤5.5.根据粒子更新公式更新粒子;
步骤5.6.重复步骤5.3~步骤5.5直到迭代次数达到最大迭代次数N;
步骤5.7.输出最优路径节点深度坐标,粒子群算法路径规划结束,得到一条从起点到终点的最优三维路径。
本发明充分利用Lazy Theta星算法和粒子群算法的优点,提出了一种基于LazyTheta星和粒子群混合算法的水下潜器三维路径规划方法。本发明克服了传统路径规划算法在三维路径规划问题的求解上适应性不强的缺陷。利用降维的思想,将三维问题转化为一个二维平面路径规划和一个深度规划,二维路径规划问题利用Lazy Theta星算法进行求解,得到具有二维信息的路径节点。在得到的二维路径节点的基础上,利用粒子群算法对二维路径点进行深度规划,得到路径点的深度信息。通过对三维问题的简化,结合两种不同算法的优点,降低算法的计算复杂度,提高三维路径规划的快速性和可靠性。
附图说明
图1:本发明中基于Lazy Theta星和粒子群混合算法的三维路径规划的流程图;
图2:本发明中基于Lazy Theta星算法的二维路径规划流程图;
图3:本发明中基于粒子群算法的深度规划流程图。
具体实施方式
本发明先用Lazy Theta星进行二维路径规划,再利用粒子群算法对路径在深度方向上进行深度规划,得到水下潜器的三维路径,具体包括以下几个步骤:
步骤1.航行空间的建模
步骤1.1.航行空间的建立
在水下潜器三维路径规划范围内建立全局坐标系Oxyz,以坐标为(xs,ys,zs)的起点S和坐标为(xd,yd,zd)的终点D建立航行空间。
步骤1.2.建立Lazy Theta星算法二维地图
在步骤1.1建立的航行空间内,以水下潜器潜行的最小安全潜行深度zsafemin为标准作水平面,形成z=zsafemin的水平面。以z=zsafemin水平面上的障碍物为基准,建立障碍物栅格地图。沿X、Y方向,即沿着经度、纬度方向分别以10′为单位划分,形成10′×10′的栅格区域。检测每个网格的中心点的X、Y坐标位置是否处于障碍物内,处于障碍物内的网格值置为1,处于障碍物外的网格值置为0。遍历所有的网格点,得到具有障碍物信息的0-1栅格地图。
步骤2.Lazy Theta星算法代价函数建立
为了达到Lazy Theta星算法搜索一条无碰的最短二维路径的目的,设计代价函数F(P)为:
其中,F(P)是适应值函数,G(P)是自起始节点S到节点P所走过的实际路径长度,其值等于起始节点S到节点P的父节点Pparent走过的实际路径长度加上父节点Pparent到节点P的欧式距离H(P)是路径的启发值,是节点P到终点D的欧式距离。二维欧式距离计算公式如下:
其中,P.x,P.y为节点P的X、Y轴坐标,Pparent.x,Pparent.y为节点Pparent的X、Y轴坐标,D.x,D.y为终点D的X、Y轴坐标。
步骤3.利用Lazy Theta星算法在水平面内进行二维路径规划
以z=zsafemin水平面为Lazy Theta星算法二维路径规划平面,以路径起点S的X、Y轴坐标(xs,ys)和路径终点D的X、Y轴坐标(xd,yd)为二维路径的起点和终点,进行路径规划。设栅格地图中一共有n个网格点,Lazy Theta星算法二维路径规划流程如图2,具体步骤如下:
步骤3.1.定义OPEN和CLOSED两个表,OPEN表用于存放未检测过的网格点,CLOSED表存放已检测过的网格点。
步骤3.2.将起始点S放入OPEN表中,初始化起始点S的父节点为其自身,适应值F(S)=0。CLOSED表初始化为空,第i个节点定义为Pi,其中i为网格点的索引(i<n)。
步骤3.3.判断OPEN表是否为空,为空则搜索路径失败,若不为空,转至步骤3.4。
步骤3.4.把OPEN表中表头网格点Pi移入CLOSED表中。
步骤3.5.判断节点Pi是否是目标终点D,若是,则路径搜索成功,转至步骤3.9,若不是,转至步骤3.6。
步骤3.6.对节点Pi和其父节点进行障碍物检测,等距离取节点Pi和节点连线上的q个点,定位这q个点坐标是否有在障碍内的点,q的值可根据栅格地图分辨率大小而定,在本发明中,取q=10。若Pi和连线之间没有障碍物,保留为Pi的父节点,若Pi和连线之间存在障碍物,计算起始点S到节点Pi已经扩展过的所有邻居节点Pc(此节点在CLOSED表中)的路径长度G(Pc),并计算节点Pi到节点Pc的直线距离选取邻居节点中使得值最小的节点Pc'作为节点Pi的父节点,更新节点Pi的适应值和父节点。
步骤3.7.扩展Pi的八个方向(东,西,南,北,东南,东北,西南,西北)地理位置相邻的子节点Pj(j<n,j≠i),考虑以下几种情况:
(1)如果Pj不在OPEN表或者CLOSED表中,将其存放在OPEN表中,设Pi的父节点为直接设置Pj父节点为计算节点Pj的适应值F(Pj)=G(Pj)+H(Pj),G(Pj)是自起始节点S到节点Pj所走过的实际路径长度,其值等于起始节点S到节点Pj的父节点Piparent走过的实际路径长度加上父节点Piparent到节点Pj的欧式距离H(Pj)是路径的启发值, 是节点Pj到终点D的欧式距离。
(2)如果Pj在OPEN表中,重新计算在OPEN表中的适应值F(Pj)。若新适应值小于原有适应值,更新节点Pj的适应值和父节点,若新适应值大于原有适应值,保持原有适应值和父节点的设置。
(3)如果Pj已经存在于CLODED表中或者是障碍物点,忽略此节点,返回步骤3.7。
步骤3.8.对OPEN表中各个网格点按照适应值的大小进行升序排序,返回3.3步骤。
步骤3.9.算法终止,输出包含路径起始点和终点二维路径点序列P(P1,P2,...Pk),k为最终的路径点的个数。
步骤4.深度规划评价函数的建立
Lazy Theta星算法规划出二维路径点后,三维路径规划中路径点的X、Y轴坐标确定。问题转化为:路径P(P1,P2,...Pk)的每个路径点Pi(i<k)所在X、Y方向上的坐标位置(Pi.x,Pi.y)确定(在步骤3得到),利用粒子群算法(PSO)优化路径点Z轴方向上的坐标位置Pi.z,得到完整的三维路径,k为二维路径点的个数。
粒子群中每个粒子代表一条路径,粒子的维数即路径的节点数。粒子各维的值是路径节点的深度,因此只要以航程最短为目标,搜索合适的节点深度组合就可以达到三维规划的目的。根据三维路径规划寻找一条无碰长度最短路径的优化目标,设路径P(P1,P2,...Pk)为路径上的节点集,其中k为粒子维数,其值等于步骤3得到的路径节点数目,节点之间直线连接即成为一条无碰路径,评价函数f(P)设置为:
其中,ΔLi是第i段路径的直线欧式距离。Pi.x,Pi.y,Pi.z为第i个路径节点的X、Y、Z轴方向上的坐标,Pi-1.x,Pi-1.y,Pi-1.z为第i-1个路径节点的X、Y、Z轴方向上的坐标,其中i≤k。m1和m2分别是进入障碍物的节点数和中间节点数,M是路径进入障碍物的惩罚值,取值为起点S与终点D欧式距离的20%~50%。
本发明的障碍物检测中,除了检测路径节点是否进入障碍物外,还考虑连接节点的直线段是否进入障碍物。具体做法是,在每个节点直线段中间均匀插入若干点,插入点的数量q可以根据规划难度而定,本发明中设置q=10。定位路径节点和路径节点间的点的X、Y坐标所在点的障碍物所在深度,若路径点的Z坐标位置在障碍物深度以下,视为路径点进入障碍物,在障碍物深度以上,则为没有进入障碍物。
步骤5.利用粒子群算法进行深度规划
粒子群深度规划流程如图3所示,具体步骤如下:
步骤5.1.初始化粒子群算法的各类参数,包括粒子数目n、迭代次数N、粒子维数k、更新速度。
步骤5.2.随机产生一组初始解。
步骤5.3.根据步骤4确定的路径评价函数计算各个粒子的适应值。更新粒子的个体最优值和种群最优值。
若当前迭代的粒子个体适应度优于历史记录的个体最优值,则以当前粒子的适应度作为该粒子的个体最优值。若种群中出现某个体的最优值优于种群最优值,则以此个体的最优值作为种群最优值。
步骤5.4.计算当前迭代中粒子群的惯性权重和学习因子
(1)惯性权重ω的计算方法
惯性权重ω代表了粒子惯性的大小,ω较大时具有较强的全局收敛能力,ω较小时具有较强的局部搜索能力。在实际问题中,一般希望算法的初始阶段有较强的全局搜索能力,到算法的后期则希望算法有较强的局部搜索能力。因此,惯性权重ω随迭代次数的增加设计成:
式中,ωmax为最大惯性权重,取值范围为0.75~1.05,本发明中取值为0.9,ωmin为最小惯性权重系数,取值范围为0.3~0.55,本发明取值为0.4,i为当前迭代次数,N为最大迭代次数。
(2)学习因子的计算方法
两个学习因子中,c1越大粒子种群多样性越好。c2越大则粒子的收敛速度越快。为了使算法不要过早收敛,提高种群的多样性,在优化初期取较大的c1使得种群多样性丰富,在算法的后期提高c2加快算法收敛。根据这两个规则设计c1、c2随迭代次数变化的计算公式为:
其中,c1f,c1c,c2f,c2c为常数,c1f,c2c取值范围为0.3~0.7,c1c,c2f取值范围为2.2~2.8,本发明取c1f=0.5,c1c=2.5,c2f=2.5,c2c=0.5,i为当前迭代次数,N为最大迭代次数。
步骤5.5.根据粒子更新公式更新粒子
设粒子群的种群粒子数目为n,k维空间中搜索最优解,k的值等于步骤3得到的路径节点数目。第j个粒子位置记为Pj=(Pj1,Pj2,...,Pjk),j=1,2,...,n,粒子速度记为Vj=(Vj1,Vj2,...,Vjk),(j=1,2,...,n)。第j个粒子至今搜索到的个体最优解记为Pjbest,群体至今搜索到的最优解记为Pgbest。第j个粒子的位置和速度根据以下公式进行更新:
V′j=ωVj+c1rand()(Pjbest-Pj)+c2rand()(Pgbest-Pj)
P′j=Pj+P′j
式中,Vj'是第j个粒子更新后的运动速度,Pj'是第j个粒子更新后的粒子位置,在本发明中为粒子各路径点的深度信息。ω是惯性权重系数,c1,c2是学习因子,ω,c1,c2的值由步骤5.4求得。c1,c2调节的是粒子学习种群最优解和个体最优解的程度。rand()为在0到1之间取的随机数。
步骤5.6.重复步骤5.3~步骤5.5直到迭代次数达到N。
步骤5.7.输出路径节点深度坐标,粒子群算法路径规划结束,至此得到一条从起点S到终点D的最优三维路径。
步骤6.输出最优三维路径。
Claims (1)
1.一种基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法,其特征是包括如下步骤:
步骤1.航行空间的建模
步骤1.1.航行空间的建立
在水下潜器三维路径规划范围内建立全局坐标系Oxyz,以坐标为(xs,ys,zs)的起点S和坐标为(xd,yd,zd)的终点D建立航行空间;
步骤1.2.建立Lazy Theta星算法二维地图
在步骤1.1建立的航行空间内,以水下潜器潜行的最小安全潜行深度zsafemin为标准作水平面,形成z=zsafemin的水平面;以z=zsafemin水平面上的障碍物为基准,建立障碍物栅格地图,沿X、Y方向,即沿着经度、纬度方向分别以10′为单位划分,形成10′×10′的栅格区域,检测每个网格的中心点的X、Y坐标位置是否处于障碍物内,处于障碍物内的网格值置为1,处于障碍物外的网格值置为0,遍历所有的网格点,得到具有障碍物信息的0-1栅格地图;
步骤2.Lazy Theta星算法代价函数建立
设计代价函数F(P)为:
其中,F(P)是适应值函数,G(P)是自起始节点S到节点P所走过的实际路径长度、其值等于起始节点S到节点P的父节点Pparent走过的实际路径长度加上父节点Pparent到节点P的欧式距离H(P)是路径的启发值,是节点P到终点D的欧式距离,二维欧式距离计算公式如下:
其中,P.x,P.y为节点P的X、Y轴坐标,Pparent.x,Pparent.y为节点Pparent的X、Y轴坐标,D.x,D.y为终点D的X、Y轴坐标;
步骤3.利用Lazy Theta星算法在水平面内进行二维路径规划
以z=zsafemin水平面为Lazy Theta星算法二维路径规划平面,以路径起点S的X、Y轴坐标(xs,ys)和路径终点D的X、Y轴坐标(xd,yd)为二维路径的起点和终点,进行路径规划,设栅格地图中一共有n个网格点,具体步骤如下:
步骤3.1.定义OPEN和CLOSED两个表,OPEN表用于存放未检测过的网格点,CLOSED表存放已检测过的网格点;
步骤3.2.将起始点S放入OPEN表中,初始化起始点S的父节点为其自身,适应值F(S)=0,CLOSED表初始化为空,第i个节点定义为节点Pi,其中i为节点点的索引,i<n;
步骤3.3.判断OPEN表是否为空,为空则搜索路径失败,若不为空,转至步骤3.4;
步骤3.4.把OPEN表中表头节点Pi移入CLOSED表中;
步骤3.5.判断节点Pi是否是目标终点D,若是,则路径搜索成功,转至步骤3.9,若不是,转至步骤3.6;
步骤3.6.对节点Pi和其父节点Piparent进行障碍物检测,等距离取节点Pi和其父节点Piparent连线上的q个点,定位这q个点坐标是否有在障碍内的点,q的值根据栅格地图分辨率大小而定,取q=10,若节点Pi和其父节点Piparent连线之间没有障碍物,保留Piparent为Pi的父节点,若节点Pi和其父节点Piparent连线之间存在障碍物,计算起始点S到节点Pi已经扩展过的所有邻居节点Pc的路径长度G(Pc),并计算节点Pi到节点Pc的直线距离选取邻居节点中使得值最小的节点Pc'作为节点Pi的父节点,更新节点Pi的适应值和父节点;
步骤3.7.扩展节点Pi的东、西、南、北、东南、东北、西南、西北八个方向地理位置相邻的子节点Pj,j<n,j≠i,考虑以下几种情况:
(1)如果节点Pj不在OPEN表或者CLOSED表中,将其存放在OPEN表中,设节点Pi的父节点为Piparent,直接设置节点Pj父节点为Piparent,计算节点Pj的适应值F(Pj)=G(Pj)+H(Pj),G(Pj)是自起始节点S到节点Pj所走过的实际路径长度、其值等于起始节点S到节点Pj的父节点Piparent走过的实际路径长度加上父节点Piparent到节点Pj的欧式距离H(Pj)是路径的启发值, 是节点Pj到终点D的欧式距离;
(2)如果节点Pj在OPEN表中,重新计算在OPEN表中的适应值F(Pj),若新适应值小于原有适应值,更新节点Pj的适应值和父节点,若新适应值大于原有适应值,保持原有适应值和父节点的设置;
(3)如果节点Pj已经存在于CLODED表中或者是障碍物点,忽略此节点,返回步骤3.7;
步骤3.8.对OPEN表中各个网格点按照适应值的大小进行升序排序,返回3.3步骤;
步骤3.9.算法终止,输出包含路径起始点和终点二维路径点序列P(P1,P2,...Pk),k为最终的路径点的个数;
步骤4.深度规划评价函数的建立
Lazy Theta星算法规划出二维路径点后,三维路径规划中路径点的X、Y轴坐标确定,问题转化为:路径点序列P(P1,P2,...Pk)的每个路径节点Pi、i<k所在X、Y方向上的坐标位置(Pi.x,Pi.y)确定,利用粒子群算法优化路径点Z轴方向上的坐标位置Pi.z,得到完整的三维路径;
根据三维路径规划寻找一条无碰长度最短路径的优化目标,设路径点序列P(P1,P2,...Pk)为路径上的节点集,其中k的值等于步骤3得到的路径节点数目,节点之间直线连接即成为一条无碰路径,评价函数f(P)设置为:
其中,ΔLi是第i段路径的直线欧式距离,Pi.x,Pi.y,Pi.z为第i个路径节点的X、Y、Z轴方向上的坐标,Pi-1.x,Pi-1.y,Pi-1.z为第i-1个路径节点的X、Y、Z轴方向上的坐标,其中i≤k,m1和m2分别是进入障碍物的节点数和中间节点数,M是路径进入障碍物的惩罚值、取值为起点S与终点D欧式距离的20%~50%;
在每个节点直线段中间均匀插入若干点,插入点的数量q根据规划难度而定,设置q=10,定位路径节点和路径节点间的点的X、Y坐标所在点的障碍物所在深度,若路径点的Z坐标位置在障碍物深度以下,视为路径点进入障碍物,在障碍物深度以上,则为没有进入障碍物;
步骤5.利用粒子群算法进行深度规划
步骤5.1.初始化粒子群算法的各类参数,包括粒子数目n、迭代次数N、粒子维数k、更新速度;
步骤5.2.随机产生一组初始解;
步骤5.3.根据步骤4确定的路径评价函数计算各个粒子的适应值,更新粒子的个体最优值和种群最优值;
若当前迭代的粒子个体适应度优于历史记录的个体最优值,则以当前粒子的适应度作为该粒子的个体最优值;若种群中出现某个体的最优值优于种群最优值,则以此个体的最优值作为种群最优值;
步骤5.4.计算当前迭代中粒子群的惯性权重和学习因子
(1)惯性权重ω的计算方法
惯性权重ω随迭代次数的增加设计成:
式中,ωmax为最大惯性权重,取值范围为0.75~1.05,取值为0.9,ωmin为最小惯性权重,取值范围为0.3~0.55,取值为0.4,l为当前迭代次数,N为最大迭代次数;
(2)学习因子的计算方法
两个学习因子中,c1越大粒子种群多样性越好;c2越大则粒子的收敛速度越快,在优化初期取较大的c1使得种群多样性丰富,在算法的后期提高c2加快算法收敛,根据这两个规则设计c1、c2随迭代次数变化的计算公式为:
其中,c1f,c1c,c2f,c2c为常数,c1f,c2c取值范围为0.3~0.7,c1c,c2f取值范围为2.2~2.8,取c1f=0.5,c1c=2.5,c2f=2.5,c2c=0.5,N为最大迭代次数;
步骤5.5.根据粒子更新公式更新粒子
设粒子群的种群粒子数目为n,k维空间中搜索最优解,k的值等于步骤3得到的路径节点数目,第j个粒子位置记为Pj=(Pj1,Pj2,...,Pjk),j=1,2,...,n,粒子速度记为Vj=(Vj1,Vj2,...,Vjk),j=1,2,...,n,第j个粒子至今搜索到的个体最优解记为Pjbest,群体至今搜索到的最优解记为Pgbest,第j个粒子的位置和速度根据以下公式进行更新:
V’j=ωVj+c1rand()(Pjbest-Pj)+c2rand()(Pgbest-Pj)
P’j=Pj+P’j
式中,Vj'是第j个粒子更新后的运动速度,Pj'是第j个粒子更新后的粒子位置、为粒子各路径点的深度信息,ω是惯性权重,c1,c2是学习因子,ω,c1,c2的值由步骤5.4求得,c1,c2调节的是粒子学习种群最优解和个体最优解的程度,rand()为在0到1之间取的随机数;
步骤5.6.重复步骤5.3~步骤5.5直到迭代次数达到N;
步骤5.7.输出路径节点深度坐标,粒子群算法路径规划结束,至此得到一条从起点S到终点D的最优三维路径;
步骤6.输出最优三维路径。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610887891.6A CN106444835B (zh) | 2016-10-11 | 2016-10-11 | 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610887891.6A CN106444835B (zh) | 2016-10-11 | 2016-10-11 | 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106444835A CN106444835A (zh) | 2017-02-22 |
CN106444835B true CN106444835B (zh) | 2019-11-26 |
Family
ID=58173405
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610887891.6A Active CN106444835B (zh) | 2016-10-11 | 2016-10-11 | 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106444835B (zh) |
Families Citing this family (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108388250B (zh) * | 2018-03-30 | 2021-03-05 | 哈尔滨工程大学 | 一种基于自适应布谷鸟搜索算法的水面无人艇路径规划方法 |
CN108664022B (zh) * | 2018-04-27 | 2023-09-05 | 湘潭大学 | 一种基于拓扑地图的机器人路径规划方法及系统 |
CN109190787B (zh) * | 2018-07-09 | 2022-02-18 | 广东工业大学 | 水下潜航器的双重粒子群多监测点访问路径规划方法 |
CN108899919A (zh) * | 2018-08-21 | 2018-11-27 | 江苏方天电力技术有限公司 | 一种基于Haar小波变换的有源配电网动态无功优化调度方法 |
CN109164815A (zh) * | 2018-09-06 | 2019-01-08 | 中国计量大学 | 一种基于改进蚁群算法的自主水下航行器路径规划方法 |
CN110501020B (zh) * | 2019-08-28 | 2022-12-02 | 湖南大学 | 一种多目标三维路径规划方法 |
CN110597280A (zh) * | 2019-09-04 | 2019-12-20 | 中国航空工业集团公司沈阳飞机设计研究所 | 一种飞机航路重规划方法 |
CN110702117B (zh) * | 2019-10-10 | 2021-09-14 | 浙江华睿科技股份有限公司 | 基于地图的路径规划方法、终端设备及计算机存储介质 |
CN111307158B (zh) * | 2020-03-19 | 2022-11-18 | 哈尔滨工程大学 | 一种auv三维航路规划方法 |
CN111385980B (zh) * | 2020-05-19 | 2022-06-14 | 桂林智慧产业园有限公司 | 一种基于粒子群的pcb板贴片方法 |
CN112363512B (zh) * | 2020-11-25 | 2023-08-01 | 中国船舶科学研究中心 | 一种基于艏向及航速的智能船避碰路径规划方法 |
CN112327931B (zh) * | 2020-12-01 | 2022-11-29 | 天津基点科技有限公司 | 一种基于sdf地图的无人机三维路径快速规划方法 |
CN112327885B (zh) * | 2020-12-01 | 2024-04-09 | 大连海事大学 | 一种无人船自适应全局-局部混合路径规划的方法 |
CN112731961B (zh) * | 2020-12-08 | 2024-06-25 | 深圳供电局有限公司 | 路径规划方法、装置、设备及存储介质 |
CN113074730B (zh) * | 2021-03-17 | 2023-05-05 | 华中科技大学 | 一种水下路径规划方法和系统 |
CN113110530B (zh) * | 2021-04-16 | 2023-11-21 | 大连海事大学 | 一种针对三维环境下的水下机器人路径规划方法 |
CN113847924B (zh) * | 2021-09-06 | 2024-07-16 | 西安交通大学 | 一种基于参数曲线优化的导航路径规划方法、系统 |
CN114152263B (zh) * | 2021-11-11 | 2024-04-16 | 上海应用技术大学 | 路径规划方法、系统、电子设备及存储介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2005164384A (ja) * | 2003-12-02 | 2005-06-23 | Nara Institute Of Science & Technology | 経路探索システム、サーバ、携帯端末、経路探索装置、経路探索プログラム |
CN102722177A (zh) * | 2012-06-27 | 2012-10-10 | 哈尔滨工程大学 | 具有pid反馈增益的自主水下航行器三维直线路径跟踪控制方法 |
CN102722749A (zh) * | 2012-06-01 | 2012-10-10 | 哈尔滨工程大学 | 一种基于粒子群算法的自适应三维空间路径规划方法 |
CN103557867A (zh) * | 2013-10-09 | 2014-02-05 | 哈尔滨工程大学 | 一种基于稀疏a*搜索的三维多uav协同航迹规划方法 |
CN103900573A (zh) * | 2014-03-27 | 2014-07-02 | 哈尔滨工程大学 | 一种基于s57标准电子海图的水下运载器多约束航路规划方法 |
CN103968841A (zh) * | 2014-06-03 | 2014-08-06 | 哈尔滨工程大学 | 一种基于改进萤火虫算法的auv三维航路规划方法 |
CN106595663A (zh) * | 2016-11-28 | 2017-04-26 | 四川航天系统工程研究所 | 结合搜索与优化的飞行器自主航迹规划方法 |
-
2016
- 2016-10-11 CN CN201610887891.6A patent/CN106444835B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2005164384A (ja) * | 2003-12-02 | 2005-06-23 | Nara Institute Of Science & Technology | 経路探索システム、サーバ、携帯端末、経路探索装置、経路探索プログラム |
CN102722749A (zh) * | 2012-06-01 | 2012-10-10 | 哈尔滨工程大学 | 一种基于粒子群算法的自适应三维空间路径规划方法 |
CN102722177A (zh) * | 2012-06-27 | 2012-10-10 | 哈尔滨工程大学 | 具有pid反馈增益的自主水下航行器三维直线路径跟踪控制方法 |
CN103557867A (zh) * | 2013-10-09 | 2014-02-05 | 哈尔滨工程大学 | 一种基于稀疏a*搜索的三维多uav协同航迹规划方法 |
CN103900573A (zh) * | 2014-03-27 | 2014-07-02 | 哈尔滨工程大学 | 一种基于s57标准电子海图的水下运载器多约束航路规划方法 |
CN103968841A (zh) * | 2014-06-03 | 2014-08-06 | 哈尔滨工程大学 | 一种基于改进萤火虫算法的auv三维航路规划方法 |
CN106595663A (zh) * | 2016-11-28 | 2017-04-26 | 四川航天系统工程研究所 | 结合搜索与优化的飞行器自主航迹规划方法 |
Non-Patent Citations (2)
Title |
---|
基于PSO和弹性绳算法的水下潜器三维空间路径规划方法研究;范志超;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑(月刊)》;20140415;第4-5、70-73页 * |
海洋环境下水下机器人快速路径规划研究;王磊;《中国优秀硕士学位论文全文数据库信息科技辑(月刊)》;20071015;第38-40页 * |
Also Published As
Publication number | Publication date |
---|---|
CN106444835A (zh) | 2017-02-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106444835B (zh) | 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 | |
CN106843235B (zh) | 一种面向无人自行车的人工势场路径规划法 | |
CN111338359B (zh) | 一种基于距离判断和角度偏转的移动机器人路径规划方法 | |
CN105717929B (zh) | 一种多分辨率障碍物环境下移动机器人混合路径规划方法 | |
CN109945873A (zh) | 一种用于室内移动机器人运动控制的混合路径规划方法 | |
CN106017472B (zh) | 全局路线规划方法、全局路线规划系统及无人机 | |
CN109798909A (zh) | 一种全局路径规划的方法 | |
CN111337930B (zh) | 一种auv目标搜索方法 | |
CN110108284A (zh) | 一种顾及复杂环境约束的无人机三维航迹快速规划方法 | |
CN107491070A (zh) | 一种移动机器人路径规划方法及装置 | |
CN106780524A (zh) | 一种三维点云道路边界自动提取方法 | |
CN106444740A (zh) | 基于mb‑rrt的无人机二维航迹规划方法 | |
CN112650237A (zh) | 基于聚类处理和人工势场的船舶路径规划方法和装置 | |
CN105844709A (zh) | 复杂河道地形流域洪水演进虚拟仿真的淹没线追踪方法 | |
CN108871364A (zh) | 一种基于结点算法的水下机器人路径规划方法 | |
CN107632616B (zh) | 一种基于三维空间曲线的无人机协同路径规划方法 | |
CN109900276A (zh) | 基于点线面障碍物模型构建的车站实时应急路径规划方法 | |
Li et al. | Approach for Optimizing 3D Highway Alignments Based on Two-stage Dynamic Programming. | |
CN106126816A (zh) | 重复建筑自动感知下的大规模als建筑点云建模方法 | |
CN111307158A (zh) | 一种auv三维航路规划方法 | |
CN110220510A (zh) | 一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法 | |
Teng et al. | A dynamic path planning method for terrain-aided navigation of autonomous underwater vehicles | |
CN115757604A (zh) | 一种基于夜光影像数据的gdp时空演变分析方法 | |
Li et al. | An overview on sensor map based localization for automated driving | |
CN109798899B (zh) | 一种面向海底未知地形搜索的树扩散启发式路径规划方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |