[发明专利]一种基于改进QPSO算法的节能型水下三维路径规划方法

权利要求书

1.一种基于改进QPSO算法的节能型水下三维路径规划方法，其特征在于，包括以下步骤：

步骤一：建立海洋环境模型建立海洋环境模型，所述海洋环境模型包含三维地形环境信息模型，此外采用兰姆奥森涡建立复杂洋流场景，同时自定义内波区、三维障碍物，并添加障碍物约束，从而得到更完善的海洋环境模型；

步骤二：基于改进QPSO算法的全局路径规划

首先基于步骤一得到的海洋环境模型建立环境信息库，其次输入起始点与目标点，进行粒子的初始化，将初始化的粒子通过适应度函数计算适应度值并建立个体最优Pbest与全局最优Gbest，随后采用改进QPSO算法进行迭代循环，更新路径节点，循环过程中路径节点的生成采用B样条插值的方法进行平滑度处理，对新的路径计算适应度函数值，根据对比之前适应度值判断是否更新个体最优与全局最优，迭代完成后得到能耗最低的平滑路径，完成全局路径规划；

步骤三：基于步骤二得到的全局路径规划，通过搭载的传感器实时检测海洋环境信息，如果运行路径上存在新的障碍物，则判断是否需要更新路径，对于动态障碍物进行停止或减速实现避障，对于静态障碍物则在当前点重规划路径；

步骤(3.1)，AUV以步骤二全局路径规划生成的路径信息进行巡航；

步骤(3.2)，AUV巡航过程中通过搭载的传感器进行环境实时监测，传感器用于监测运行路径上实时环境中未知障碍物信息，包括未知静态障碍物和未知动态障碍物；

步骤(3.3)，利用传感器所得到的信息判断环境未知障碍物类型，若为未知静态障碍物，则在当前点与目标点基于改进QPSO算法进行路径重新规划，路径规划过程中的初始化信息按照当前节点与目标节点之间的路径信息作为初始化数据；若为未知动态障碍物，则通过停止或减速避让的形式等待动态障碍物通过，之后按全局路径继续跟踪巡航；

步骤(3.4)，判断是否到达目标点，到达则结束进程；反之，返回步骤(3.2)。

2.根据权利要求1所述的一种基于改进QPSO算法的节能型水下三维路径规划方法，其特征在于，所述步骤一具体步骤如下：

步骤(1.1)，随机构建n*n个数据集点，将其作为三维地形图高度属性，对应(x，y)的点低于属性值为障碍物，高于此值为安全区，利用栅格地图法构建三维地形图；

步骤(1.2)，利用兰姆奥森涡流函数模拟洋流，同样利用栅格地图法进行数据处理，栅格内洋流数据取当前栅格内所包含洋流数据的平均值；

步骤(1.3)，在已知地形图上定义某深度为高、低密度海流分界线，该节面周围(-h₁，+h₁)范围内定义为内波核心区，(h₁，h₂)和(-h₁，-h₂)为内波区域，剩余区域为海流区；

步骤(1.4)，在已知地形图上随机生成新的三维障碍物，并对障碍物进行膨胀化处理，其值为d_min；其中，AUV在进行路径规划过程中，和障碍物满足下列约束：d≥d_min。