基于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星算法搜索得到的路径有两个明显的缺点，一是路径长度比实际最短路径长，二是存在许多折角需要进行后期加工平滑。Lazy Theta星算法是解决A星算法不足的一种变体。Lazy Theta星算法不仅路径不受地图网格边缘限制，而且路径节点少，路径长度短，但是在三维问题求解上适应性不高，高维大范围问题的求解计算复杂度仍很大。

三维环境相比于二维环境无论在地形障碍物建模或是路径搜索上都更为复杂。目前备受关注的三维路径规划的算法是启发式智能优化算法，如粒子群算法、蚁群算法、萤火虫算法等。例如申请号为201210178003.5的专利文件中，公开了一种基于粒子群算法的自适应三维空间路径规划方法；申请号为201310744400.9的专利文件中，公开了一种基于布谷鸟搜索算法的水下潜器三维路径规划方法；申请号为201310235123.9的专利文件中，公开了一种基于高程图和蚁群寻食的三维路径规划方法。这些方法都在三维路径规划问题的求解上有较强的智能性和适应性，然而在大范围路径搜索上依然存在计算复杂度大、盲目性大等问题。

发明内容

本发明的目的在于提供一种能降低计算复杂度，提高三维路径规划的快速性和可靠性的基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法。

本发明的目的是这样实现的：

步骤1.以起点S和终点D为基准建立航行空间，以水下潜器的最小安全潜行深度z_safemin形成的水平面z为基准，建立Lazy>safemin；

步骤2.建立Lazy Theta星算法代价函数；

步骤3.以z＝z_safemin水平面为Lazy>s,y_s)和路径终点D的X、Y轴坐标(x_d,y_d)为二维路径的起点和终点，在水平面内进行二维路径规划；

步骤4.根据三维路径规划寻找一条无碰长度最短路径的优化目标，设计深度规划评价函数；

步骤5.利用粒子群算法进行深度规划；

步骤6.输出最优三维路径。

本发明还可以包括：

1、步骤3具体包括：栅格地图中的网格点总数为n，具体步骤如下，

步骤3.1.定义OPEN和CLOSED两个表；

步骤3.2.初始化OPEN表和CLOSED表；

步骤3.3.判断OPEN表是否为空，为空则搜索路径失败，若不为空，执行步骤3.4；

步骤3.4.把OPEN表中表头网格点P_i移入CLOSED表中，i<n；

步骤3.5.判断节点P_i是否是终点D，若是，则路径搜索成功，转至步骤3.9，若不是，执行步骤3.6；

步骤3.6.对节点P_i和其父节点进行障碍物检测，若P_i和连线之间没有障碍物，保留为P_i的父节点，否则，更新节点P_i的适应值和父节点；

步骤3.7.扩展P_i的东、西、南、北、东南、东北、西南、西北八个方向地理位置相邻的子节点P_j，j＜n，j≠i；

步骤3.8.对OPEN表中各个网格点按照适应值的大小进行升序排序，返回步骤3.3；

步骤3.9.算法终止，输出包含路径起点和终点的二维路径序列P(P₁,P₂,...P_k)，k为最终的路径点的个数。

2、所述扩展P_i的东、西、南、北、东南、东北、西南、西北八个方向地理位置相邻的子节点P_j，具体包括以下几种情况：

步骤3.7.1.如果P_j不在OPEN表或者CLOSED表中，将其存放在OPEN表中，设P_i的父节点为设置P_j父节点为

步骤3.7.2.如果P_j在OPEN表中，重新计算在OPEN表中的适应值F(P_j)，若新适应值小于原有适应值，更新节点P_j的适应值和父节点，若新适应值大于原有适应值，保持原有适应值和父节点的设置；

步骤3.7.3.如果P_j已经存在于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，以坐标为(x_s,y_s,z_s)的起点S和坐标为(x_d,y_d,z_d)的终点D建立航行空间。

步骤1.2.建立Lazy Theta星算法二维地图

在步骤1.1建立的航行空间内，以水下潜器潜行的最小安全潜行深度z_safemin为标准作水平面，形成z＝z_safemin的水平面。以z＝z_safemin水平面上的障碍物为基准，建立障碍物栅格地图。沿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的父节点P_parent走过的实际路径长度加上父节点P_parent到节点P的欧式距离H(P)是路径的启发值，是节点P到终点D的欧式距离。二维欧式距离计算公式如下：

其中，P.x，P.y为节点P的X、Y轴坐标，P_parent.x，P_parent.y为节点P_parent的X、Y轴坐标，D.x，D.y为终点D的X、Y轴坐标。

步骤3.利用Lazy Theta星算法在水平面内进行二维路径规划

以z＝z_safemin水平面为Lazy>s,y_s)和路径终点D的X、Y轴坐标(x_d,y_d)为二维路径的起点和终点，进行路径规划。设栅格地图中一共有n个网格点，Lazy>

步骤3.1.定义OPEN和CLOSED两个表，OPEN表用于存放未检测过的网格点，CLOSED表存放已检测过的网格点。

步骤3.2.将起始点S放入OPEN表中，初始化起始点S的父节点为其自身，适应值F(S)＝0。CLOSED表初始化为空，第i个节点定义为P_i，其中i为网格点的索引(i<n)。

步骤3.3.判断OPEN表是否为空，为空则搜索路径失败，若不为空，转至步骤3.4。

步骤3.4.把OPEN表中表头网格点P_i移入CLOSED表中。

步骤3.5.判断节点P_i是否是目标终点D，若是，则路径搜索成功，转至步骤3.9，若不是，转至步骤3.6。

步骤3.6.对节点P_i和其父节点进行障碍物检测，等距离取节点P_i和节点连线上的q个点，定位这q个点坐标是否有在障碍内的点，q的值可根据栅格地图分辨率大小而定，在本发明中，取q＝10。若P_i和连线之间没有障碍物，保留为P_i的父节点，若P_i和连线之间存在障碍物，计算起始点S到节点P_i已经扩展过的所有邻居节点P_c(此节点在CLOSED表中)的路径长度G(P_c)，并计算节点P_i到节点P_c的直线距离选取邻居节点中使得值最小的节点P_c'作为节点P_i的父节点，更新节点P_i的适应值和父节点。

步骤3.7.扩展P_i的八个方向(东，西，南，北，东南，东北，西南，西北)地理位置相邻的子节点P_j(j＜n，j≠i)，考虑以下几种情况：

(1)如果P_j不在OPEN表或者CLOSED表中，将其存放在OPEN表中，设P_i的父节点为直接设置P_j父节点为计算节点P_j的适应值F(P_j)＝G(P_j)+H(P_j)，G(P_j)是自起始节点S到节点P_j所走过的实际路径长度，其值等于起始节点S到节点P_j的父节点P_iparent走过的实际路径长度加上父节点P_iparent到节点P_j的欧式距离H(P_j)是路径的启发值，是节点P_j到终点D的欧式距离。

(2)如果P_j在OPEN表中，重新计算在OPEN表中的适应值F(P_j)。若新适应值小于原有适应值，更新节点P_j的适应值和父节点，若新适应值大于原有适应值，保持原有适应值和父节点的设置。

(3)如果P_j已经存在于CLODED表中或者是障碍物点，忽略此节点，返回步骤3.7。

步骤3.8.对OPEN表中各个网格点按照适应值的大小进行升序排序，返回3.3步骤。

步骤3.9.算法终止，输出包含路径起始点和终点二维路径点序列P(P₁,P₂,...P_k)，k为最终的路径点的个数。

步骤4.深度规划评价函数的建立

Lazy Theta星算法规划出二维路径点后，三维路径规划中路径点的X、Y轴坐标确定。问题转化为:路径P(P₁,P₂,...P_k)的每个路径点P_i(i＜k)所在X、Y方向上的坐标位置(P_i.x,P_i.y)确定(在步骤3得到)，利用粒子群算法(PSO)优化路径点Z轴方向上的坐标位置P_i.z，得到完整的三维路径，k为二维路径点的个数。

粒子群中每个粒子代表一条路径，粒子的维数即路径的节点数。粒子各维的值是路径节点的深度，因此只要以航程最短为目标，搜索合适的节点深度组合就可以达到三维规划的目的。根据三维路径规划寻找一条无碰长度最短路径的优化目标，设路径P(P₁,P₂,...P_k)为路径上的节点集，其中k为粒子维数，其值等于步骤3得到的路径节点数目，节点之间直线连接即成为一条无碰路径，评价函数f(P)设置为：

其中，ΔL_i是第i段路径的直线欧式距离。P_i.x,P_i.y,P_i.z为第i个路径节点的X、Y、Z轴方向上的坐标，P_i-1.x,P_i-1.y,P_i-1.z为第i-1个路径节点的X、Y、Z轴方向上的坐标，其中i≤k。m₁和m₂分别是进入障碍物的节点数和中间节点数，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)学习因子的计算方法

两个学习因子中，c₁越大粒子种群多样性越好。c₂越大则粒子的收敛速度越快。为了使算法不要过早收敛，提高种群的多样性，在优化初期取较大的c₁使得种群多样性丰富，在算法的后期提高c₂加快算法收敛。根据这两个规则设计c₁、c₂随迭代次数变化的计算公式为:

其中，c_1f,c_1c,c_2f,c_2c为常数，c_1f,c_2c取值范围为0.3～0.7，c_1c,c_2f取值范围为2.2～2.8，本发明取c_1f＝0.5，c_1c＝2.5，c_2f＝2.5，c_2c＝0.5，i为当前迭代次数，N为最大迭代次数。

步骤5.5.根据粒子更新公式更新粒子

设粒子群的种群粒子数目为n，k维空间中搜索最优解，k的值等于步骤3得到的路径节点数目。第j个粒子位置记为P_j＝(P_j1,P_j2,...,P_jk),j＝1,2,...,n，粒子速度记为V_j＝(V_j1,V_j2,...,V_jk),(j＝1,2,...,n)。第j个粒子至今搜索到的个体最优解记为P_jbest，群体至今搜索到的最优解记为P_gbest。第j个粒子的位置和速度根据以下公式进行更新：

V′_j＝ωV_j+c₁rand()(P_jbest-P_j)+c₂rand()(P_gbest-P_j)

P′_j＝P_j+P′_j

式中，V_j'是第j个粒子更新后的运动速度，P_j'是第j个粒子更新后的粒子位置，在本发明中为粒子各路径点的深度信息。ω是惯性权重系数，c₁,c₂是学习因子，ω，c₁,c₂的值由步骤5.4求得。c₁,c₂调节的是粒子学习种群最优解和个体最优解的程度。rand()为在0到1之间取的随机数。

步骤5.6.重复步骤5.3～步骤5.5直到迭代次数达到N。

步骤5.7.输出路径节点深度坐标，粒子群算法路径规划结束，至此得到一条从起点S到终点D的最优三维路径。

步骤6.输出最优三维路径。