CN115202356A - 一种三维水下欠驱动auv回收路径规划方法 - Google Patents

Abstract

本发明提供本发明公开了一种三维水下欠驱动AUV回收路径规划方法及装置，方法包括：基于小山丘的位置构建水下三维环境模型；基于AUV在回收过程中起点位置、起点姿态、终点位置及终点姿态设置中间候选路径点；构建路径规划约束条件，所述路径规划约束条件包括硬约束条件和软约束条件；基于所述硬约束条件由候选路径中筛选出可行路径；通过改进的遗传算法对可行路径进行迭代，当到达指定迭代次数后，终止算法得到最优的全局路径，所述改进的遗传算法中对种群的交叉系数以及变异系数进行自适应的处理。本发明利用优化的自适应参数以及考虑多种约束条件对传统遗传方法中过早出现的局部最优问题进行全局寻优，从而提高路径规划的效率。

Description

一种三维水下欠驱动AUV回收路径规划方法

技术领域

本发明涉及欠驱动机器人控制技术领域，具体而言，尤其涉及一种三维水下欠驱动AUV回收路径规划方法。

背景技术

海洋是人类巨大的共同资源宝库，蕴含着丰富的矿产资源、海洋生物资源和可再生能源。然而，恶劣的海洋环境给人类对海洋的研究、开发和利用造成了极大的困难，受困于缺少有效的工具，人们对海洋的开发利用是很不充分且极为缓慢的。为了最大限度的减少人类暴露于危险的海洋环境，人们利用自主水下机器人(Autonomous UnderwaterVehicle，AUV)来协助水下的勘探和资源的开发。AUV执行任务时需要自带能源，受体积和质量的限制，所携带的能源十分有限，特别是潜艇搭载的AUV，需要在水下回收对接以进行能量补充与工作数据交换。因此，水下对接回收技术成为目前AUV研究的一个重要方向。

常用的AUV路径规划方法根据环境中障碍物的分类，可将路径规划分为全局路径和局部路径两种规划。其中，全局路径规划主要针对环境中静止、信息已知的障碍物对象。为获得最优的全局路径，通常会根据环境模型构建一些目标函数，并将全局路径规划问题转化为多目标函数优化问题。当前在路径规划领域比较有代表性的目标优化算法一般可以分为传统算法以及智能规划算法两类。传统算法包括A*算法、D*算法、单元分解法、人工势场法等，传统方法能够保证在可行解存在的情况下找到最优和次优解，但是需要精确的环境数学模型和较大的计算量，搜索效率低，得到的优化效果也不太理想。智能规划算法在处理复杂问题上具有传统算法无法比拟的优越性，智能算法包括遗传算法、粒子群算法、蚁群算法等。其中遗传算法(Genetic Algorithm，GA)具有强大的全局搜索能力，特别是在复杂优化问题中的搜索能力，这使得GA广泛应用于路径规划、任务分配等问题研究中。但是GA也存在这收敛速度慢、易陷入局部最优解等问题。

发明内容

本发明提供一种三维水下欠驱动AUV回收路径规划方法及装置，利用优化的自适应参数以及考虑多种约束条件对传统遗传方法中过早出现的局部最优问题进行全局寻优，从而提高路径规划的效率。

本发明采用的技术手段如下：

本发明公开了一种三维水下欠驱动AUV回收路径规划方法，包括以下步骤：

S1、获取水下环境中小山丘的位置，并基于小山丘的位置构建水下三维环境模型，并确定AUV在回收过程中起点位置、起点姿态、终点位置及终点姿态；

S2、基于所述AUV在回收过程中起点位置、起点姿态、终点位置及终点姿态设置中间路径点；

S3、构建路径规划约束条件，所述路径规划约束条件包括硬约束条件和软约束条件，所述硬约束条件包括欠驱动机器人自身的回转半径约束条件、潜浮角约束条件以及对于静态障碍物的避碰约束条件，所述软约束条件包括路径长度约束条件以及能耗约束条件；

S4、在改进的遗传算法中依据软硬约束条件设计相应适应度函数，通过迭代利用硬约束条件设计的适应度函数进行可行路径搜索，利用软约束条件设计的适应度函数在可行路径中寻优，当到达指定迭代次数后，终止算法得到利用三次B样条平滑后的最优全局路径，所述改进的遗传算法中对种群的交叉系数以及变异系数进行自适应的处理。

较现有技术相比，本发明具有以下优点：

本发明提供的一种针对三维环境下的欠驱动水下机器人回收路径规划方法，改进的遗传算法继承了传统遗传算法强大的全局搜索能力的优点，从规划路径的长短以及能耗方面来看，改进的遗传算法规划在这两方面均由于传统的遗传算法，并且经过多次实验的比较，改进的遗传算法在求解的稳定性上更优于传统的遗传算法，从路径的平滑度来看，本发明采用了三次B样条曲线来构成回收路径，在保证平滑度的情况下，由于三次B样条曲线具有位置连续、一阶连续、二阶微分连续的特性，这就使得构成的路径曲率连续，符合欠驱动AUV的运动要求，从而使得规划出的路径能够实际的跟踪。综合多组仿真数据，改进的遗传算法在保证了路径的长短及能耗的同时，也保证了回收路径的平滑度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的环境模型示意图。

图2为本发明中应用的三次B样条示意图。

图3为本发明的中间路径点选择示意图。

图4为本发明的算法流程图。

图5为测试环境以及各算法规划路径对比。其中(a)为测试环境测例，(b)传统遗传算法规划结果，(c)为启发式自适应遗传算法规划结果，(d)为本文改进遗传算法规划结果。

图6为测试环境以及设置不同权重规划出的路径对比。其中(a)为测试环境测例，(b)kL＝1，kE＝0，(c)kL＝0.5，kE＝0.5，(d)kL＝0，kE＝1。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

本发明公开了一种三维水下欠驱动AUV回收路径规划方法，包括以下步骤：

S1、获取水下环境中小山丘的位置，并基于小山丘的位置构建水下三维环境模型，并确定AUV在回收过程中起点位置、起点姿态、终点位置及终点姿态。

环境建模是全局路径规划的前提，由于AUV对接回收时声学定位的范围为0-3000m，属于较小的任务环境，而电子海图的分辨率难以呈现这种环境，所以本实验通过下面公式来模拟海底地形，将海底地形当做环境中主要的静态障碍物。所以S1中的海底环境的仿真利用下方表达式来模拟：

式(1)中，X，Y和Z代表三维空间域，x₀表示海床中小山丘中心处的横坐标，y₀表示海床中小山丘中心处的纵坐标。σ，a和b表示海床中小山丘的宽度控制系数，σ越大，则海床中小山丘的宽度越大，σ越小，则海床中小山丘的宽度越小，a越大，则海床中小山丘沿y轴宽度越小，a越小，则海床中小山丘沿y轴宽度越大，b越大，则海床中小山丘沿x轴宽度越小，b越大，则海床中小山丘沿x轴宽度越小。h表示海床中小山丘的高度。n表示海床中小山丘的个数。

S2、基于所述AUV在回收过程中起点位置、起点姿态、终点位置及终点姿态设置中间路径点。

具体地，本申请中最优路径曲线是由两端的确定路径点以及算法获取的中间路径点利用三次B样条曲线构成的。而本申请算法所需要做的是从中间候选路径点中，选取最优的中间路径点构成回收路径。正如图3所示，P₄和Pn_-4之间为中间候选路径点区域，其中每条垂直于P₄Pn_-4线段的虚线上都有多个候选路径点，每条虚线上只会选取一个点作为形成最优路径曲线的中间路径点，而中间路径点就是通过算法获取的。中间路径点的个数是由实验得来的，本实施例通过测试优选为七个中间路径点。

由于本申请的全局路径规划是应用在欠驱动AUV的回收对接场景中的，相较于其他场景的全局路径规划将水下机器人作为一个二维环境下的质点考虑，仅仅考虑水下机器人的位置，本申请在三维环境中不仅需要考虑AUV的位置，还要考虑它的姿态。首先对欠驱动AUV的回收过程简单介绍，当欠驱动AUV完成自身的任务时需要进行回收操作时，欠驱动AUV会向回收平台发送需要回收的指令以及当前的位置信息。当回收平台接收到欠驱动AUV发送的请求后，会将回收平台的位置信息发送给欠驱动AUV。起点位置和姿态需要符合欠驱动水下机器人进行回收对接任务时的位置信息，而对于终点的位置及姿态并不是要符合回收平台的位置信息，因为回收对接情况下的全局路径规划的终点一般是距离回收平台一定距离并且姿态方向指向回收平台中心处，这段距离是为了后续欠驱动AUV的光学引导入坞设计的。

针对欠驱动AUV回收过程的特点可设置全局路径的起始端路径点以及终点端路径点，首先是考虑路径起始端的路径点位置要与回收时的欠驱动AUV位置相同，并且路径切线方向要与欠驱动AUV的速度方向一致，从而使得路径更加符合实际需求，起始端的路径点表达式为：

上式中，P_s为起始端路径点位置，即欠驱动AUV进行回收时的起始位置，

表示为路径曲线在起始端的一阶导数，

为欠驱动AUV进行回收时的速度向量，k₁为正实数。

针对终点端的路径点，需要将其设置在坞站平台的中心线上则终点端的路径点表达式为：

其中，P_E表示为全局规划路径的终点，

表示为路径曲线在终点端的一阶导数，

表示从P_E指向坞站中心的单位向量，k₂表示为正实数

S3、构建路径规划约束条件，所述路径规划约束条件包括硬约束条件和软约束条件，所述硬约束条件包括欠驱动机器人自身的回转半径约束条件、潜浮角约束条件以及对于静态障碍物的避碰约束条件，所述软约束条件包括路径长度约束条件以及能耗约束条件。

欠驱动水下机器人在进行回收对接的全局路径规划中需要考虑多种约束限制，例如欠驱动水下机器人自身的机动性约束、路径最短、能耗最优以及安全避障等约束。将这些约束条件加入到算法中去需要先将其转化为数学语言，建立适应度函数将全局路径规划问题转化为求极值的问题，经过算法迭代寻优，获取最优的返航路径。

本文针对全局路径规划中考虑到的安全约束、路径长度约束、能耗约束以及欠驱动机器人机动性约束所建立的数学模型如下：

(1)静态障碍物的避碰约束条件及安全约束

在全局路径规划中，安全约束主要考虑的是环境中静态障碍物对欠驱动水下机器人的威胁，规划出的全局路径应该不与障碍物有接触。在完成任务进行回收路径规划时，为了避免欠驱动水下机器人与作为静态障碍物的海底障碍物发生碰撞，欠驱动水下机器人的航行高度应该始终高于海底障碍物的高度，故将有关地形的安全约束条件建模为：

Z₁(x_i,y_i)>Z(x_i,y_i),i＝1,2,…,n (4)

上式中，Z₁(x_i,y_i)为全局规划路径曲线在坐标(x_i,y_i)处的高度值，Z(x_i,y_i)为海底静态障碍物在坐标(x_i,y_i)处的高度值。

(2)路径长度约束

全局路径规划中的航程代价主要涉及到欠驱动水下机器人从起点到终点的航行路径，假设总路径由n个路径点组成，则路径长度的总代价可以表示为：

上式中，(x_i,y_i,z_i)表示全局路径点的三维坐标，对于一般的路径规划问题通常选取路径长度最短作为路径长度约束。

(3)能耗约束

对于无海流环境下的路径规划，欠驱动水下机器人的能量消耗仅与路径长度有关，距离最短的路径即是能量消耗最低的路径。但是欠驱动水下机器人的实际工作环境是有海流存在的，而欠驱动水下机器人在水下航行时受海流的影响要远比受风浪的影响大，欠驱动水下机器人的位置与姿态在受到海流干扰时会发生变化，从而也会影响到欠驱动水下机器人的能量消耗，因此在对欠驱动水下机器人的航行进行全局路径规划时，海流的影响不可忽略。假设全局规划路径由路径点l_i(i＝1,2,…,n)组成，则欠驱动水下机器人沿路径曲线航行的能量消耗就是各个子路径

能量消耗之和：

上式中，e_i表示每段路径

的能量消耗，其中路径段的个数比路径点个数少一个，故i从1取到n-1，P为欠驱动水下机器人的推进功率，它与AUV的推进速度

的三次方成正比。k为阻力系数，由具体AUV的结构决定。

表示欠驱动水下机器人在固定坐标系下的速度，该速度与推进速度和海流速度

存在矢量关系如下：

对于全局路径规划问题也可选取能耗最少作为能耗约束进行路径的寻优。

(4)欠驱动机器人自身的回转半径约束条件以及潜浮角约束条件即欠驱动机器人自身的回转半径约束条件、潜浮角约束条件

由于本文考虑的水下机器人的类型为欠驱动类型，所以需要考虑欠驱动水下机器人的机动性约束条件，例如欠驱动水下机器人在水平面的回转能力以及在垂直面的潜浮能力，水平面的回转能力可以通过回转半径R_s来表示，垂直面的潜浮能力可以通过潜浮角θ来表示，欠驱动水下机器人自身存在最小回转半径R_min以及最大潜浮角θ_max，这两个机动约束条件最终可转化为对全局规划曲线的约束，例如最小回转半径可转换为对曲线曲率的约束要求，最大潜伏角可转化为曲线在坐标系E-ξηζ下三个方向上的梯度约束。

故对于全局规划得到的三次B样条路径曲线需要满足以下的几何性质：

上式中，

表示三次B样条曲线在E-ξη面投影的曲线曲率，

和

分别表示三次B样条曲线在坐标系下三个坐标轴方向的梯度。

本发明针对欠驱动水下机器人的路径规划，将约束条件划分为两类，一类为硬约束条件，表示所规划的全局路径必须满足这些约束条件，否则将规划路径判定为不可行路径，其中将安全约束以及欠驱动水下机器人的机动性约束划分为硬约束条件，一类为软约束条件，表示所规划的全局路径尽可能的满足，即在满足硬约束条件的可行路径中寻优，其中将路径约束以及能耗约束划分为软约束条件，尽可能使得规划出的全局路径满足路径最短以及能耗最少的约束。

针对软约束条件中的路径长度约束以及能耗约束，主要考虑的是进行回收对接任务时的欠驱动AUV的状态需求，如果欠驱动AUV进行回收对接任务时能量较少，并且对于回收路径时间要求不高，则可以优先考虑能耗，利用算法获取能耗最优的路径。如果欠驱动AUV进行回收对接任务时能量充足，当时要求尽快进行回收时，则可以优先考虑路径长度，利用算法获取路径长度最短的路径。一般情况下可能会既考虑能耗也考虑，此时算法得出的路径会尽可能在路径短的情况下能耗小，虽然此时的路径长度可能长于仅考虑路径长度时的路径，能耗也大于仅考虑能耗时的路径。对于路径长度和能耗的取舍，本发明根据两者前面的系数设置来控制。

S4、在改进的遗传算法中依据软硬约束条件设计相应适应度函数，通过迭代利用硬约束条件设计的适应度函数进行可行路径搜索，利用软约束条件设计的适应度函数在可行路径中寻优，当到达指定迭代次数后，终止算法得到利用三次B样条平滑后的最优全局路径，所述改进的遗传算法中对种群的交叉系数以及变异系数进行自适应的处理。

考虑到规划路径的平滑性，本发明采用三次B样条曲线来平滑路径，以此来保证规划出的路径是实际可跟踪的，三次B样条曲线的表达式为：

上式中，P_j(t)为整个路径曲线的分段表示形式，表示第j段路径曲线，全局路径由M+1段三次B样条曲线组合而成。每段三次B样条曲线由四个路径点确定。t为自变量系数，其中P_j(t)中t为0时表示第j段路径曲线的起点，P_j(t)中t为1时表示第j段路径曲线的终点。本实施例中通过实验调整，路径曲线由13段三次B样条曲线构成较为合适，即优选M＝12。

依据水下环境模型以及三次B样条曲线的性质，可以利用在起始点P_s处设置三重路径点来使得起始点P_s在三次B样条曲线上，因此起始端三重路径点可设置为：

P₁＝P₂＝P₃＝P_s (11)

根据欠驱动水下机器人在起始端的速度方向可设置路径点P₄，目的是使得起始端的路径切线方向与欠驱动水下机器人的速度方向保持一致，并且假设欠驱动水下机器人的速度方向近似等于艏向，则P₄可表示为：

P₄＝(ξ_s+l₁cosθ_scosψ_s,η_s+l₁cosθ_ssinψ_s,ζ_s+l₁sinθ_s) (12)

上式中，θ_s和ψ_s分别表示欠驱动水下机器人在回收起点处的纵倾角和偏航角，l₁为正实数。

对于终点端也可通过设置三重路径点来使得终点P_E在三次B样条曲线上，同样终点端的三重路径点可设置为：

P_n＝P_n-1＝P_n-2＝P_E (13)

同样为了使得欠驱动水下机器人在路径末段可以获取到坞站平台上的光源信息，可设置两个路径点构成的线段在坞站平台的中心线上，则这两个路径点可表示为：修改

P_n-3＝(ξ_E-l₂cosψ_e,η_E-l₂sinψ_e,ζ_E) (14)

P_n-4＝(ξ_E-l₃cosψ_e,η_E-l₃sinψ_e,ζ_E) (15)

上式中，ψ_e表示欠驱动AUV在终点处的偏航角，l₂和l₃为正实数，并且有l₃>l₂。

确定起始端和终止端的路径点后，利用栅格法来设置中间的路径点，由于栅格地图在二维情况下可以获得较好的规划结果，直接应用到三维环境中则会使得算法效率大大减低，这是由于三维环境的栅格数相较二维环境的栅格数大量增加，极大增加了算法计算量。考虑到现实航行中的欠驱动水下机器人不可能频繁的进行上浮或者下潜操作，在绝大部分时间上都会保持恒定的舵角进行航行，所以可以将三维环境进行简单的高度降维。具体来说，本申请中的高度降维就是将中间路径点设计到包含起始端路径点P₄和终点端路径点P_n-4的平面中，并不一定是投影到水平面的，因为P₄和P_n-4高度不同。作过这两个路径点并且平行于坐标系的E-ζ轴的平面α，再作垂直于α平面且包含路径点P₄和路径点P_n-4的平面β，在平面β中依据路径点P₄和路径点P_n-4为界限设置非均匀栅格作为中间搜索路径点。

如图3所示，首先通过起始端路径点P₄和终点端路径点P_n-4，作过这两个路径点并且平行于坐标系的E-ζ轴的平面α，再作垂直于α平面且包含路径点P₄和路径点P_n-4的平面β，在平面β中依据路径点P₄和路径点P_n-4为界限设置非均匀栅格作为中间搜索路径点，自此两端的确定路径点以及中间搜索路径点均已设置完毕。

本发明对于软硬约束条件设计不同的启发式函数，也称为适应度函数。通过适应度函数可以计算出种群中个体的适应度值，依据适应度值得高低进行后续的交叉、变异等操作。

种群进行初始化操作之后，遗传算法需要进行适应度函数的设计，以此来评估种群中个体的性能好坏，即判断所生成路径的优劣情况。考虑到本文将约束条件划分为了硬约束条件以及软约束条件，则需依据不同的约束条件设计不同的适应度函数到改进的遗传算法中去，则依据硬约束条件设计的适应度函数为：

上式中，n表示算法所得全局路径规划的路径点总数，由于采用的三次B样条曲线进行路径优化，则全局路径是由n-3段三次B曲线连接而成，其中w_i表示每段曲线的权重系数，b_i表示每段曲线是否满足硬约束条件，表达式如下：

依据硬约束条件设计的适应度函数可以选取种群中可行的全局路径，如果想要获取路径最短或者是能耗最优的路径，则需要依据软约束的条件进行继续的寻优，则需要针对软约束条件设计适应度函数：

上式中，k_E和k_L分别表示路径长度L和能耗E的权重系数，L_min和E_min分别表示各自理想的最短路径以及最少能耗。

对于传统遗传算法中的选择策略，为了避免算法出现早熟现象，同时保证更好的个体可以直接进入下一代种群，本发明设计了一种基于软硬适应度函数的混合选择策略，该策略是由两种选择方法组合而成的。第一种方式是结合软硬适应度函数的精英选择策略，传统的精英选择策略是将一小部分最优的候选解原封不动的复制到下一代中，而本文设计的结合软硬适应度函数的精英选择策略，当该轮种群中的个体的最高适应度值未达到最大，即最大的适应度值为式(16)中b_i全为1时，则将适应度高的一小部分个体直接保留到下一代中，可提高算法收敛效果，当该轮种群中的部分个体已达到最大的适应度值时，则把这些个体以软约束适应度函数排序，将适应度值f₁达到最大且f₂值小的部分个体直接保留到下一代中，以此即保留了当前最优的个体，又可通过后续的交叉操作优化种群。第二种方式是轮盘赌选择策略，个体的入选概率与其适应度值成正比。

现有遗传算法中合理选择交叉概率P_c和变异概率P_m是影响算法收敛性及求解最优路径质量的关键因素。其中P_c和P_m值过小，计算的迭代过程难以产生新个体，导致搜索过程缓慢，但是P_c和P_m值过大不利于保留优良的个体，导致搜索缺乏导向性，破坏遗传模式。由于P_c和P_m对算法性能和收敛性影响巨大，并且在迭代初期期望算法能够拥有较强的搜索能力，尽可能保证种群个体多样性，而到了迭代末期期望算法保证优良个体得到稳定保留，算法运行稳定且不易陷入局部最优，因此引入了自适应交叉概率P_c1和自适应变异概率P_m1来替换P_c和P_m。

具体来说，优化过程主要包括：

1、设计初始化种群策略

传统遗传算法的初始化种群策略是在搜索区域随机选取路径点构成一个个种群个体，实现种群初始化，这种方法虽然容易实现，但是生成的种群个体中存在较多的不可行路径，这会影响遗传算法的收敛速度。为了在种群初始化时可以获取较多的可行路径的个体，本发明充分考虑硬约束条件，设计了一种把随机法和基于硬约束的启发式方法结合的策略。该策略的具体过程如下：

a.首先确定种群个体数量M；

b.根据两端确定的路径点P₂、P₃和P₄，在P₅的搜索路径点中选取构成三次B样条曲线满足硬约束条件的路径点区间，同样的根据确定路径点P_n-2、P_n-3和P_n-4，在P_n-5的搜索路径点中选取构成三次B样条曲线满足硬约束条件的路径点区间，在这两个可行路径点区间中进行随机选取；

c.对于P₅到P_n-5之间的路径点则依据原本的随机法，在搜索路径点中进行随机的生成。

2、设计选择策略

由于依据不同的约束条件设计了两种不同的适应度函数，所以利用两种不同的约束条件设计混合的选择策略，该策略包含双适应度函数的精英策略和轮盘赌策略。选择策略的目的是为了种群向更优个体进化，由此会更大概率选择出种群中的优质个体进行交叉处理，通过选择策略使得种群优质个体增多，劣质个体进行剔除。本发明的双适应度函数的精英策略是当种群中没有个体到达f₁的最大值，即依据硬约束条件设计的适应度函数式16中的b_i全为1时，则将种群依据f₁的适应度值进行从大到小排序，把适应度值高的一小部分个体直接保留到下一代中，这样可以提高可行路径的生成速度，当种群中的部分个体达到了f₁的最大值时，则将达到f₁的个体按照f₂的适应度值进行从小到大排序，将f₂适应度值小的一小部分个体直接保留到下一代中，这样会保留当代种群的部分优质个体，可通过后续的交叉操作优化种群。基于双适应度函数的精英策略会选择部分个体进入下一代中，而余下的个体则由轮盘赌策略进行选择，轮盘赌策略主要是依据硬约束设计的适应度函数f₁进行选择的，这是由于最优路径的获取是要基于可行路径的生成，种群中个体的选择概率是依据个体f₁的适应度函数值决定的，表达式如下：

上式中，P(i)表示种群中个体i的选择概率，f_1,i表示个体i的适应度值，m表示种群的个体总数，若从种群中选取M个个体则就是需要旋转M次轮盘。

3、设计自适应交叉系数及变异系数

交叉系数P_c用于表示在种群个体进行交叉操作时两个父代个体会进行交换的概率，进行交叉操作后会得到两个子代个体，两个子代个体的适应度值可能会优于父代个体，从而实现迭代寻优的目的。变异系数P_m用于表示在种群中单一个体是否会在某一路径点处进行路径点位置变换的概率，变异后的个体也可能会获得更高的适应度值。由此可以得知在遗传算法中选取适当的交叉系数P_c以及变异系数P_m是影响算法收敛性以及能否寻找到最优路径的关键因素。在传统的遗传算法中一般会将交叉系数P_c以及变异系数P_m设置为定值，即这两个系数不会依据算法的迭代次数进行变化。但是考虑到对最优路径搜素的需求，在算法迭代的初期希望能够获取更多新的个体，对于路径的搜索区域有较强的搜索能力，而到了算法迭代的末期则希望优质的个体可以尽可能的保留，对于路径的搜索区间可能缩小到优质个体们所在的区间，并且保证算法可以稳定运行。因此在此引入了自适应交叉系数P_c1和自适应变异系数P_m1，它们的表达式如下：

上式中，g表示算法的迭代次数，g_max表示算法的最大迭代次数，从上式可得出随着算法的迭代次数越大，自适应交叉系数P_c1以及自适应变异系数P_m1由g控制的部分会减小，但是系数不应该仅仅取决于迭代次数还应该考虑种群中个体的适应度值与总体适应度之间的关系，即f_max表示本次迭代中种群最优个体的适应度值，f_c1表示本次迭代中进行交叉操作的两个个体中适应度值低的那个，f_avg表示本次迭代中种群的平均适应度值，f_m1表示本次迭代中进行变异操作个体的适应度值。从上式可得出当个体的适应度值大于种群平均适应度值时，个体的适应度值越高，则由适应度值控制的部分则会数值减小，即对优质个体有保护作用，当个体适应度值小时，由适应度值控制的部分则会数值增加，促进个体进行交叉及变异操作产生新个体。k₁、k₂、k₃和k₄均为常数，并且k₁大于k₃，k₂大于k₄。

由于期望交叉概率和变异概率在算法迭代初期和末期不同，所以自适应的交叉和变异概率随迭代次数的增大而减小，这样可实现初期种群个体的多样性，末期种群个体稳定，而对于适应度值高的个体应该加以保护，所以当该个体的适应度值与该代最优个体的适应度值越接近时则交叉概率和变异概率应该有所减小。随后经过种群的不断迭代，到达迭代结束时可得到最优的全局回收路径。整体的算法流程图如图4所示。

下面通过一个具体的应用实例，对本发明的方案和效果做进一步说明。

图5(a)为仿真的测试环境，其中测试环境的大小为600m×600m×50m，定义AUV的起点位置为(300,400,35)，AUV的初始姿态为(0,0,0)其中姿态依次表示横倾角、纵倾角以及艏向角，定义AUV的终点位置为(0，0，5)，AUV的最终姿态为

其中图(b)为传统遗传算法所规划出来的路径，图5(c)为启发式自适应遗传算法规划出的路径，图5(d)为本文改进遗传算法所规划出来的路径，从这三种算法的规划结果可以看出，本文所改进的遗传算法规划的路径最优。

对于该测试环境，在每个算法上运行了20次，并统计了20次实验算法的平均路径长度，最长路径以及最短路径。由下表可知，本文改进的遗传算法相较于传统遗传算法和启发式自适应遗传算法在生成路径的长度上，这20次实验中不论是平均路径长度、最长路径还是最短路径均优于其他两种算法。由于传统的遗传算法交叉变异系数固定在种***叉过程中，可能会破坏最优个体，所以实验结果中最小路径和最大路径相差巨大，表明算法搜索能力不够稳定。启发式自适应遗传算法，针对遗传法的交叉系数和变异系数进行自适应设计，针对较优个体进行保护，所以相较于传统遗传算法最长路径长度和最短路径长度之差有所减小，平均路径长度也有所减少，算法稳定性有所提升。本文的改进遗传算法，经过对算法交叉系数和变异系数的进一步改进搜索到的最长路径长度和最短路径长度相近，并且平均路径长度更短，并且基本等于搜索到的最短路径长度，表明该算法搜索的稳定性较强。

表1各算法生成路径长度对比

第二种情况是在模拟的有海流无障碍物情况下规划从起点到终点的路径，在这种情况下，假定AUV在整个运行过程中的速度稳定不变，此时AUV的能量消耗不仅受到路径长度的影响，还受到海流的影响，即顺着海流时可减少能量消耗、逆着海流时会增加能量消耗，因此对于软约束中的权重系数，可通过设置权重来满足任务的需求，即如果对回收的路径长路要求尽可能短时，则尽可能大的设置k_L的值，如果对于回收时能量消耗尽可能少时，则可可能大的设置k_E的值。其中图6(a)为仿真的测试环境，其中测试环境的大小为600m×600m的二维环境，以x＝200为分界线，x<200处的海流方向为3π/4，x>200处的海流方向为3π/4，假设海流大小为0.5m/s，定义AUV的起点位置为(400,100,0)，AUV的初始姿态为(0,0,-π)，定义AUV的终点位置为(0,100,0)，AUV的最终姿态为(0,0,-π)。其中图6(b)、图6(c)和图6(d)分别为设置不同权重值本文改进遗传算法所规划出来的路径，表2为设置不同权重分别进行20次实验的平均路径长度和能耗结果，从规划结果可以看出，本文所改进的遗传算法规划的路径可根据任务设置的权重来获取对应的全局最优路径。

表2不同权重的路径长度和能耗对比

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种三维水下欠驱动AUV回收路径规划方法，其特征在于，包括以下步骤：

S1、获取水下环境中小山丘的位置，并基于小山丘的位置构建水下三维环境模型，并确定AUV在回收过程中起点位置、起点姿态、终点位置及终点姿态；

S2、基于所述AUV在回收过程中起点位置、起点姿态、终点位置及终点姿态设置中间路径点；

S3、构建路径规划约束条件，所述路径规划约束条件包括硬约束条件和软约束条件，所述硬约束条件包括欠驱动机器人自身的回转半径约束条件、潜浮角约束条件以及对于静态障碍物的避碰约束条件，所述软约束条件包括路径长度约束条件以及能耗约束条件；

S4、在改进的遗传算法中依据软硬约束条件设计相应适应度函数，通过迭代利用硬约束条件设计的适应度函数进行可行路径搜索，利用软约束条件设计的适应度函数在可行路径中寻优，当到达指定迭代次数后，终止算法得到利用三次B样条平滑后的最优全局路径，所述改进的遗传算法中对种群的交叉系数以及变异系数进行自适应的处理。

2.根据权利要求1所述的一种三维水下欠驱动AUV回收路径规划方法，其特征在于，S1中所述水下三维环境模型为：

其中，X，Y和Z代表三维空间域，x₀表示海床中小山丘中心处的横坐标，y₀表示海床中小山丘中心处的纵坐标，σ，a和b表示海床中小山丘的宽度控制系数，σ越大，则海床中小山丘的宽度越大，σ越小，则海床中小山丘的宽度越小，a越大，则海床中小山丘沿y轴宽度越小，a越小，则海床中小山丘沿y轴宽度越大，b越大，则海床中小山丘沿x轴宽度越小，b越大，则海床中小山丘沿x轴宽度越小，h表示海床中小山丘的高度，n表示海床中小山丘的个数。

3.根据权利要求1所述的一种三维水下欠驱动AUV回收路径规划方法，其特征在于，S2中路径的起始端路径点表达式如下：

其中，P_s为起始端路径点位置，

表示为路径曲线在起始端的一阶导数，

为欠驱动AUV进行回收时的速度向量，k₁为正实数；

终点位置表达式如下：

其中，P_E表示为全局规划路径的终点，该终点在回收坞站平台的中心线上且姿态指向坞站平台中心处，，

表示为路径曲线在终点端的一阶导数，

表示从P_E指向坞站中心的单位向量，k₂表示为正实数。

4.根据权利要求1所述的一种三维水下欠驱动AUV回收路径规划方法，其特征在于，S4中三次B样条曲线的表达式如下：

其中，P_j(t)为整个路径曲线的分段表示形式，表示的是第j段曲线，每段B样条曲线由4个路径点构成，t为自变量系数，P_j(t)中t＝0表示第j段曲线起点，P_j(t)中t＝1表示第j段曲线终点，N_i(t)为三次B样条的基函数，整体曲线由M+1段三次B样条曲线构成。

5.根据权利要求1所述的一种三维水下欠驱动AUV回收路径规划方法，其特征在于，S3中所述硬约束条件根据以下表达式获取：

Z(x_i,y_i)>Z₁(x_i,y_i),i＝1,2,…,n

其中，Z(x_i,y_i)为路径曲线在(x_i,y_i)处的高度值，Z₁(x_i,y_i)为海床模型在(x_i,y_i)处的高度值；

其中，

表示曲线在水平面投影的曲率，

和

表示曲线在三个方向上的梯度，R_min为最小回转半径，θ_max为最大潜浮角。

6.根据权利要求5所述的一种三维水下欠驱动AUV回收路径规划方法，其特征在于，S3中所述软约束条件根据以下表达式获取：

其中，(x_i,y_i,z_i)、(x_i+1,y_i+1,z_i+1)分别表示第i个和第i+1个路径点的三维状态空间坐标；

其中，e_i为每段路径

的能量消耗，P为AUV的推进功率，k为阻力系数，

表示AUV在固定坐标系下的速度。

7.根据权利要求6所述的一种三维水下欠驱动AUV回收路径规划方法，其特征在于，S4还包括：

对硬约束条件设置适应度函数的表达式如下：

其中，w_i为调整权重，b_i表示当该段曲线n个点均满足硬约束条件时取1，否则取0；

对软约束条件设置适应度函数的表达式如下：

f₂＝k_L·L/L_min+k_E·E/E_min

上式中k_L，k_E分别为路径长度L和能耗E的权重系数，L_min、E_min分别表示理想最短路径以及能耗。

8.根据权利要求1所述的一种三维水下欠驱动AUV回收路径规划方法，其特征在于，S4中针对遗传算法中交叉系数以及变异系数进行自适应处理，包括根据以下表达式计算自适应交叉概率P_c1和自适应变异概率P_m1：

其中f′₁为进行交叉操作的两个个体中适应度值小的个体，f′₂为变异操作的个体的适应度值，g为迭代次数，g_max为最大迭代次数，f_avg为本次迭代种群的平均适应度值，f_max为本次迭代种群中最大的适应度值，k₁、k₂、k₃和k₄均为预设常数。

Images