[发明专利]基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法

摘要

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

主权项

1.一种基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法，其特征是包括如下步骤：/n步骤1.航行空间的建模/n步骤1.1.航行空间的建立/n在水下潜器三维路径规划范围内建立全局坐标系Oxyz，以坐标为(x_s,y_s,z_s)的起点S和坐标为(x_d,y_d,z_d)的终点D建立航行空间；/n步骤1.2.建立Lazy Theta星算法二维地图/n在步骤1.1建立的航行空间内，以水下潜器潜行的最小安全潜行深度z_safemin为标准作水平面，形成z＝z_safemin的水平面；以z＝z_safemin水平面上的障碍物为基准，建立障碍物栅格地图，沿X、Y方向，即沿着经度、纬度方向分别以10′为单位划分，形成10′×10′的栅格区域，检测每个网格的中心点的X、Y坐标位置是否处于障碍物内，处于障碍物内的网格值置为1，处于障碍物外的网格值置为0，遍历所有的网格点，得到具有障碍物信息的0-1栅格地图；/n步骤2.Lazy Theta星算法代价函数建立/n设计代价函数F(P)为：/n /n其中，F(P)是适应值函数，G(P)是自起始节点S到节点P所走过的实际路径长度、其值等于起始节点S到节点P的父节点P_parent走过的实际路径长度加上父节点P_parent到节点P的欧式距离H(P)是路径的启发值，是节点P到终点D的欧式距离，二维欧式距离计算公式如下：/n /n其中，P.x，P.y为节点P的X、Y轴坐标，P_parent.x，P_parent.y为节点P_parent的X、Y轴坐标，D.x，D.y为终点D的X、Y轴坐标；/n步骤3.利用Lazy Theta星算法在水平面内进行二维路径规划/n以z＝z_safemin水平面为Lazy Theta星算法二维路径规划平面，以路径起点S的X、Y轴坐标(x_s,y_s)和路径终点D的X、Y轴坐标(x_d,y_d)为二维路径的起点和终点，进行路径规划，设栅格地图中一共有n个网格点，具体步骤如下：/n步骤3.1.定义OPEN和CLOSED两个表，OPEN表用于存放未检测过的网格点，CLOSED表存放已检测过的网格点；/n步骤3.2.将起始点S放入OPEN表中，初始化起始点S的父节点为其自身，适应值F(S)＝0，CLOSED表初始化为空，第i个节点定义为节点P_i，其中i为节点点的索引，i<n；/n步骤3.3.判断OPEN表是否为空，为空则搜索路径失败，若不为空，转至步骤3.4；/n步骤3.4.把OPEN表中表头节点P_i移入CLOSED表中；/n步骤3.5.判断节点P_i是否是目标终点D，若是，则路径搜索成功，转至步骤3.9，若不是，转至步骤3.6；/n步骤3.6.对节点P_i和其父节点P_iparent进行障碍物检测，等距离取节点P_i和其父节点P_iparent连线上的q个点，定位这q个点坐标是否有在障碍内的点，q的值根据栅格地图分辨率大小而定，取q＝10，若节点P_i和其父节点P_iparent连线之间没有障碍物，保留P_iparent为P_i的父节点，若节点P_i和其父节点P_iparent连线之间存在障碍物，计算起始点S到节点P_i已经扩展过的所有邻居节点P_c的路径长度G(P_c)，并计算节点P_i到节点P_c的直线距离选取邻居节点中使得值最小的节点P_c'作为节点P_i的父节点，更新节点P_i的适应值和父节点；/n步骤3.7.扩展节点P_i的东、西、南、北、东南、东北、西南、西北八个方向地理位置相邻的子节点P_j，j＜n，j≠i，考虑以下几种情况：/n(1)如果节点P_j不在OPEN表或者CLOSED表中，将其存放在OPEN表中，设节点P_i的父节点为P_iparent，直接设置节点P_j父节点为P_iparent，计算节点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的欧式距离；/n(2)如果节点P_j在OPEN表中，重新计算在OPEN表中的适应值F(P_j)，若新适应值小于原有适应值，更新节点P_j的适应值和父节点，若新适应值大于原有适应值，保持原有适应值和父节点的设置；/n(3)如果节点P_j已经存在于CLODED表中或者是障碍物点，忽略此节点，返回步骤3.7；/n步骤3.8.对OPEN表中各个网格点按照适应值的大小进行升序排序，返回3.3步骤；/n步骤3.9.算法终止，输出包含路径起始点和终点二维路径点序列P(P₁,P₂,...P_k)，k为最终的路径点的个数；/n步骤4.深度规划评价函数的建立/nLazy Theta星算法规划出二维路径点后，三维路径规划中路径点的X、Y轴坐标确定，问题转化为:路径点序列P(P₁,P₂,...P_k)的每个路径节点P_i、i<k所在X、Y方向上的坐标位置(P_i.x,P_i.y)确定，利用粒子群算法优化路径点Z轴方向上的坐标位置P_i.z，得到完整的三维路径；/n根据三维路径规划寻找一条无碰长度最短路径的优化目标，设路径点序列P(P₁,P₂,...P_k)为路径上的节点集，其中k的值等于步骤3得到的路径节点数目，节点之间直线连接即成为一条无碰路径，评价函数f(P)设置为：/n /n /n其中，Δ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％；/n在每个节点直线段中间均匀插入若干点，插入点的数量q根据规划难度而定，设置q＝10，定位路径节点和路径节点间的点的X、Y坐标所在点的障碍物所在深度，若路径点的Z坐标位置在障碍物深度以下，视为路径点进入障碍物，在障碍物深度以上，则为没有进入障碍物；/n步骤5.利用粒子群算法进行深度规划/n步骤5.1.初始化粒子群算法的各类参数，包括粒子数目n、迭代次数N、粒子维数k、更新速度；/n步骤5.2.随机产生一组初始解；/n步骤5.3.根据步骤4确定的路径评价函数计算各个粒子的适应值，更新粒子的个体最优值和种群最优值；/n若当前迭代的粒子个体适应度优于历史记录的个体最优值，则以当前粒子的适应度作为该粒子的个体最优值；若种群中出现某个体的最优值优于种群最优值，则以此个体的最优值作为种群最优值；/n步骤5.4.计算当前迭代中粒子群的惯性权重和学习因子/n(1)惯性权重ω的计算方法/n惯性权重ω随迭代次数的增加设计成：/n /n式中，ω_max为最大惯性权重，取值范围为0.75～1.05，取值为0.9，ω_min为最小惯性权重，取值范围为0.3～0.55，取值为0.4，l为当前迭代次数，N为最大迭代次数；/n(2)学习因子的计算方法/n两个学习因子中，c₁越大粒子种群多样性越好；c₂越大则粒子的收敛速度越快，在优化初期取较大的c₁使得种群多样性丰富，在算法的后期提高c₂加快算法收敛，根据这两个规则设计c₁、c₂随迭代次数变化的计算公式为:/n /n /n其中，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，N为最大迭代次数；/n步骤5.5.根据粒子更新公式更新粒子/n设粒子群的种群粒子数目为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个粒子的位置和速度根据以下公式进行更新：/nV’_j＝ωV_j+c₁rand()(P_jbest-P_j)+c₂rand()(P_gbest-P_j)/nP’_j＝P_j+P’_j/n式中，V_j'是第j个粒子更新后的运动速度，P_j'是第j个粒子更新后的粒子位置、为粒子各路径点的深度信息，ω是惯性权重，c₁,c₂是学习因子，ω，c₁,c₂的值由步骤5.4求得，c₁,c₂调节的是粒子学习种群最优解和个体最优解的程度，rand()为在0到1之间取的随机数；/n步骤5.6.重复步骤5.3～步骤5.5直到迭代次数达到N；/n步骤5.7.输出路径节点深度坐标，粒子群算法路径规划结束，至此得到一条从起点S到终点D的最优三维路径；/n步骤6.输出最优三维路径。/n