基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法技术

本发明专利技术提供的是一种基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法。步骤1.航行空间的建模；步骤2.建立Lazy Theta星算法代价函数；步骤3.以z＝zsafemin水平面为Lazy Theta星算法二维路径规划平面，以路径起点S的X、Y轴坐标(xs,ys)和路径终点D的X、Y轴坐标(xd,yd)为二维路径的起点和终点，在水平面内进行二维路径规划；步骤4.根据三维路径规划寻找一条无碰长度最短路径的优化目标，设计深度规划评价函数；步骤5.利用粒子群算法进行深度规划；步骤6.输出最优三维路径。本发明专利技术通过对三维问题的简化，结合两种不同算法的优点，降低算法的计算复杂度，提高三维路径规划的快速性和可靠性。

【技术实现步骤摘要】

本专利技术涉及的是一种水下潜器路径规划方法。具体地说是一种水下潜器的三维路径规划方法。
技术介绍
三维航路规划是水下潜器等具有空间移动能力的智能机器执行水下任务的关键技术之一，越来越受到人们的重视。现有的三维路径规划算法大部分由二维路径规划算法发展而来，在二维路径规划中，A星算法应用广泛。例如申请号为201110172301.9的专利文件中，提出了一种游戏路径搜索的简化方法；申请号为201410531309.3的专利文件中，他提出了一种基于二叉堆节点排序的A星寻路方法系统；申请号为201410010003.3的专利文件中，提出了一种基于自适应A星算法的飞行器最优路径确定方法。上述三个技术方案中利用A星算法，能够快速搜索得到二维空间中一条无碰最短路径。然而A星算法搜索得到的路径有两个明显的缺点，一是路径长度比实际最短路径长，二是存在许多折角需要进行后期加工平滑。LazyTheta星算法是解决A星算法不足的一种变体。LazyTheta星算法不仅路径不受地图网格边缘限制，而且路径节点少，路径长度短，但是在三维问题求解上适应性不高，高维大范围问题的求解计算复杂度仍很大。三维环境相比于二维环境无论在地形障碍物建模或是路径搜索上都更为复杂。目前备受关注的三维路径规划的算法是启发式智能优化算法，如粒子群算法、蚁群算法、萤火虫算法等。例如申请号为201210178003.5的专利文件中，公开了一种基于粒子群算法的自适应三维空间路径规划方法；申请号为201310744400.9的专利文件中，公开了一种基于布谷鸟搜索算法的水下潜器三维路径规划方法；申请号为201310235123.9的专利文件中，公开了一种基于高程图和蚁群寻食的三维路径规划方法。这些方法都在三维路径规划问题的求解上有较强的智能性和适应性，然而在大范围路径搜索上依然存在计算复杂度大、盲目性大等问题。
技术实现思路
本专利技术的目的在于提供一种能降低计算复杂度，提高三维路径规划的快速性和可靠性的基于LazyTheta星和粒子群混合算法的水下潜器三维路径规划方法。本专利技术的目的是这样实现的：步骤1.以起点S和终点D为基准建立航行空间，以水下潜器的最小安全潜行深度zsafemin形成的水平面z为基准，建立LazyTheta星算法二维障碍物栅格地图，z＝zsafemin；步骤2.建立LazyTheta星算法代价函数；步骤3.以z＝zsafemin水平面为LazyTheta星算法二维路径规划平面，以路径起点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.输出最优路径节点深度坐标，粒子群算法路径规划结束，得到一条从起点到终点的最优三维路径。本专利技术充分利用LazyTheta星算法和粒子群算法的优点，提出了一种基于LazyTheta星和粒子群混合算法的水下潜器三维路径规划方法。本专利技术克服了传统路径规划算法在三维路径规划问题的求解上适应性不强的缺陷。利用降维的思想，将三维问题转化为一个二维平面路径规划和一个深度规划，二维路径规划问题利用LazyTheta星算法进行求解，得到具有二维信息的路径节点。在得到的二维路径节点的基础上，利用粒子群算法对二维路径点进行深度规划，得到路径点的深度信息。通过对三维问题的简化，结合两种不同算法的优点，降低算法的计算复杂度，提高三维路径规划的快速性和可靠性。附图说明图1：本专利技术中基于LazyTheta星和粒子群混合算法的三维路径规划的流程图；图2：本专利技术中基于LazyTheta星算法的二维路径规划流程图；图3：本专利技术中基于粒子群算法的深度规划流程图。具体实施方式本专利技术先用LazyTheta星进行二维路径规划，再利用粒子群算法对路径在深度方向上进行深度规划，得到水下潜器的三维路径，具体包括以下几个步骤：步骤1.航行空间的建模步骤1.1.航行空间的建立在水下潜器三维路径规划范围内建立全局坐标系Oxyz，以坐标为(xs,ys,zs)的起点S和坐标为(xd,yd,zd)的终点D建立航行空间。步骤1.2.建立LazyTheta星算法二维地图在步骤1.1建立的航行空间内，以水下潜器潜行的最小安全潜行深度zsafemin为标准作水平面，形成z＝zsafemin的水平面。以z＝zsafemin水平面上的障碍物为基准，建立障碍物栅格地图。沿X、Y方向，即沿着经度、纬度方向分别以10′为单位划分，形成10′×10′的栅格区域。检测每个网格的中心点的X、Y坐标位置是否处于障碍物内，处于障碍物内的网格值置为1，处于障碍物外的网格值置为0。遍历所有的网格点，得到具有障碍物信息的0-1栅格地图。步骤2.LazyTheta星算法代价函数建立为了达到LazyTheta星算法搜索一条无碰的最短二维路径的目的，设计代价函数F(P)为：其中，F(P)是适应值函数，G(P)是自起始节点S到节点P所走过的实际路径长度，其值等于起始节点S到节点P本文档来自技高网...

【技术保护点】
一种基于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.一种基于LazyTheta星和粒子群混合算法的水下潜器三维路径规划方法，其特征是包括如下步骤：步骤1.以起点S和终点D为基准建立航行空间，以水下潜器的最小安全潜行深度zsafemin形成的水平面z为基准，建立LazyTheta星算法二维障碍物栅格地图，z＝zsafemin；步骤2.建立LazyTheta星算法代价函数；步骤3.以z＝zsafemin水平面为LazyTheta星算法二维路径规划平面，以路径起点S的X、Y轴坐标(xs,ys)和路径终点D的X、Y轴坐标(xd,yd)为二维路径的起点和终点，在水平面内进行二维路径规划；步骤4.根据三维路径规划寻找一条无碰长度最短路径的优化目标，设计深度规划评价函数；步骤5.利用粒子群算法进行深度规划；步骤6.输出最优三维路径。2.根据权利要求1所述的基于LazyTheta星和粒子群混合算法的水下潜器三维路径规划方法，其特征是步骤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和其父节点Piparent进行障碍物检测，若Pi和Piparent连线之间没有障碍物，保留Piparent为Pi的父节点，否则，更新节点Pi的适应值和父节点；步骤3.7.扩展Pi的东、西、南、北、东南、东北、西南、西北八个方向地理位...

【专利技术属性】
技术研发人员：刘厂，雷宇宁，赵玉新，高峰，金娜，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23