[发明专利]一种AUV松弛轨迹规划方法

权利要求书

1.一种AUV松弛轨迹规划方法，其特征在于，包括以下步骤：

步骤1：将全局规划航路按照设定路径长度划分为多个子航段；

步骤2：根据AUV当前位置以及障碍分布情况，确定本次局部航迹规划的起点、终点，以及航迹规划空间范围，具体为：

探测到障碍物后，以AUV当前位置作为局部规划起点，即子任务起点分别代表北向、东向和深度方向坐标，以未遭受碰撞威胁的最近准子目标点作为局部规划的终点，即子任务终点分别代表北向、东向和深度方向坐标；

根据子任务起点S_S、子任务终点T_S，以确定的北、东、深三个方向的正、负偏移距离，生成路径点搜索范围：

其中：Δn为北向负向偏移距离；为北向正向偏移距离；Δe为东向负向偏移距离；为东向正向偏移距离；Δd为深度负向偏移距离；为深度正向偏移距离；

步骤3：根据当前局部环境是否存在运动障碍物，确定本次优化过程是否对AUV速度指令进行优化，即确定执行路径规划或者轨迹规划，若仅有静态障碍则执行路径规划，航速指令与全局规划的航速指令保持一致；若存在运动障碍则执行轨迹规划，即在路径规划的基础上进行航速规划以进一步降低与运动障碍物碰撞的风险；

步骤4：利用自适应差分进化粒子群优化算法，完成局部航迹规划，得出中间路径点序列以及到达各个中间路径点的期望航速；

步骤5：根据AUV当前实际位置及规划结果，确定当前子目标点，进而利用空间几何原理解算AUV的期望航向、期望深度；

步骤6：输出航向指令、深度指令、航速指令，驱动AUV航行；

步骤7：判断AUV是否抵达当前全局路径规划终点，若是则转步骤8，若否则转步骤2，继续执行下一次优化过程；

步骤8：判断是否抵达全局规划最后一段航路终点，若否则转入下一全局规划航段，并转步骤2，若是则结束。

2.根据权利要求1所述的一种AUV松弛轨迹规划方法，其特征在于：步骤4所述利用自适应差分进化粒子群优化算法，完成局部航迹规划，得出中间路径点序列以及到达各个中间路径点的期望航速具体为：

步骤4.0：初始化，包括：

步骤4.0.1：设置算法参数；

步骤4.0.2：对粒子群进行初始化；

步骤4.0.3：对敏感粒子进行初始化；

步骤4.1：粒子适应度更新，包括：

步骤4.1.1：计算每个粒子的适应度；

步骤4.1.2：更新每个粒子的历史最优位置；

步骤4.1.3：更新全局最优位置；

步骤4.2：粒子状态更新，包括：

步骤4.2.1：更新每个粒子的速度；

步骤4.2.2：更新每个粒子的位置；

步骤4.3：敏感粒子适应度计算，包括：

步骤4.3.1：计算敏感粒子的适应度；

步骤4.3.2：计算敏感粒子适应度的变化量；

步骤4.4：执行差分进化操作，包括：

步骤4.4.1：根据敏感粒子适应度变化量确定粒子群的差分进化比率；

步骤4.4.2：完成变异、交叉和选择等差分进化操作；

步骤4.5：判定是否满足终止条件，若是则转步骤4.6，若否则转步骤4.1；

步骤4.6：把当前的全局最优位置作为优化结果输出，结束。