基于种群超启发式算法的洋流环境下AUV路径规划方法与流程

本发明属于水下机器人领域，涉及基于种群超启发式算法的洋流环境下auv路径规划方法。

背景技术：

auv，autonomousunderwatervehicle，又叫自主水下机器人，具有活动范围大、机动性好、安全、智能化等优点，是完成各种水下任务的重要工具。传统的移动机器人路径规划方法主要是借助将环境表示为离散化的栅格地图，采用d和a*等搜索算法搜索从起点到终点的绝对最短路径。这种基于搜索的路径规划方法存在两个问题，一方面当地图规模增大时，尤其是扩展到三维空间时，搜索算法效率会急剧下降；另一方面该方法得到的是一种离散的、不光滑的路径，机器人难以跟踪，尤其是欠驱动型的机器人。

由于鱼雷型auv仅依靠尾部的螺旋桨产生动力，是一个典型的欠驱动系统，现有的移动机器人得到离散路径的方式难以应用在欠驱动系统的鱼雷型auv上。所以auv路径规划方法对规划出的路径的光滑性提出了更高的要求，路径不仅需要光滑，而且不能出现超过其最大转弯半径的情况。

另外，auv在水下的运动受洋流信息的影响很大，当auv运动方向和洋流流向相差较大时，不仅会对其运动精度产生较大的影响，而且auv为了保持原有航线还会耗费额外的能源。因此，auv的路径规划方法规划出的路径不仅对光滑性有要求，还要尽量顺着洋流运动的方向，使得auv具有更高的运动精度和更加节省能源的能力。

用b样条曲线表示auv运动路径，将其控制点位置作为参数，把路径规划问题建模为一个多约束条件下的非线性优化问题，可以使得最终路径满足多种约束。基于种群的元启发式算法对求解非线性优化问题上有着很大的优势，如萤火虫算法、粒子群算法和鱼群算法等。近年来出现了很多类似的算法，有学者认为这些算法缺乏创新性，不过是将类似的思想换了一种模拟场景而已。因此，产生了一种试图综合种群元启发式算法的种群超启发式算法，通过管理种群操作设计一种超启发式算法框架，停止了创新性较低的模拟场景替换算法。

申请号为“2018105195080”的专利公开了一种考虑海洋环流影响的uuv基于能量消耗优化的远程航海路径规划方法。该方法首先确定uuv实际的对地航行速度以及实时更新的海流信息；初始化优化算法所需的各类参数；通过空间分解建模随机产生路径控制节点，使用b-spline拟合生成初始路径；之后进入qpso算法迭代过程，计算当前路径对应粒子平均最佳位置，计算当前路径对应uuv的能量消耗；最后通过相应的优化过程确定最终的最优解，输出最终的路径控制节点，根据最终的控制节点拟合生成能耗最优的路径轨迹。该方法以b样条曲线表示路径，并采用qpso算法优化选取样条曲线控制点位置，但是其优化函数考虑uuv动力学模型以实际能耗为代价，计算复杂且扩展到其他类型uuv较为困难。

申请号为“2014107663462”的专利公开了一种基于动态规划的水下航行器路径规划方法。该发明在洋流的未来状况已知的情况下，先将所要规划的区域栅格化然后利用速度合成模型来计算搜索点之间所耗费的时间值，并以时间值作为权重，应用动态规划算法在二维平面内规划出一条可达最优路径。该方法在栅格化后的地图上搜索满足洋流信息的路径，所得路径虽然满足洋流约束，但是并不能满足光滑性约束。

因此，针对上述问题，本发明提出了一种基于种群超启发式算法的洋流环境下auv路径规划方法。

技术实现要素：

本发明的目的在于：提供了基于种群超启发式算法的洋流环境下auv路径规划方法，解决了现有auv路径规划方法规划的路径不光滑、难以跟踪和跟踪耗能高的问题。

本发明采用的技术方案如下：

基于种群超启发式算法的洋流环境下auv路径规划方法，包括以下步骤：

步骤1：初始化种群，再根据综合了时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的代价函数得到整个种群中所有个体的初始综合代价值；

步骤2：设置种群基本操作集合和对应的操作选择概率向量；选择种群中的一个个体，再选择操作选择概率向量中最大概率对应的种群基本操作，对该个体使用该种群基本操作，并更新其综合代价值；重复上述流程直至遍历整个种群，至此完成一次迭代；然后，调整操作选择概率向量，进行下一次迭代，直到迭代次数达到设定值；

步骤3：迭代完成后，选择代价值最小的个体，即最优个体，结合b样条曲线的起点、终点和固定控制点构成一组控制点，再由这组控制点生成最优路径。

进一步地，所述步骤1的具体步骤为：

步骤1.1：设定auv路径的起点和终点，连接起点和终点，并在该连线上生成m个等分点，在每个等分点上根据起点和终点的连线作垂线，得到m条垂线，每条垂线上生成imax个随机点，从而得到imax组随机点，第i组随机点表示为rpm,i(rp1,i,rp2,i,…rpk,i,…rpm,i，rpk,i∈r²)，i∈(1,2,…,imax)；

步骤1.2：从起点出发，沿auv的起始方向短距离增加一个固定控制点p1，从终点出发，沿auv的终止方向的反方向以短距离增加一个固定控制点p2，用于控制曲线路径的起始方向和终止方向；

步骤1.3：根据步骤1.1和步骤1.2，以起点、终点、一组随机点rpm,i(rp1,i,rp2,i,…rpk,i,…rpm,i，rpk,i∈r²)、固定控制点p1和固定控制点p2作为一条b样条曲线的控制点，则控制点的数量为n，n＝m+4，控制点的位置为再结合曲线阶次k，得到离散点组成的imax条b样条曲线，曲线方程为：

其中，bi,k(t)是k次b样条曲线的基函数；

步骤1.4：由imax条b样条曲线得到imax条auv路径，路径表示为：

其中，auv的朝向ψ为：

步骤1.5：将imax组随机点初始化得到种群，一组随机点rpm,i(rp1,i,rp2,i,…rpk,i,…rpm,i，rpk,i∈r²)表示一个个体，即一个解xi，其中，个体的维度为m，个体的数量为imax，种群p为：

即，

步骤1.6：根据综合了时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的代价函数计算每个个体的初始代价值，所述代价函数结合了时间和罚函数，表达式为：

其中，tcost是时间代价，通过路径距离除以平均速度得到，

ρcost是曲率代价，通过累加每个离散点处的近似曲率得到，

mcost是地图代价，通过累加每个离散点在地图上的值(障碍物为1，可通过区域为0)得到，

ocost是洋流信息约束，通过累加每个离散点路径方向和水流方向的夹角及水流向量模的乘积得到，h是b样条曲线上离散点的数量；

步骤1.7：将四项初始代价值分别进行归一化处理，由于不同的代价值具有不同的量纲，通过最大代价值和最小代价值来得到归一化的代价值

步骤1.8：将归一化后的四项代价值通过不同的权重加权求和，可以得到整个种群p中所有个体的初始综合代价值：

其中，ω1，ω2，ω3，ω4是不同代价值的权重。

进一步地，所述步骤2的具体步骤为：

步骤2.1：设置种群基本操作集合和对应的操作选择概率向量，所述种群基本操作用o<·>表示，对输入的解xt，在一次迭代之后输出一个新解x^t+1，表达式为o<x^t>＝x^t+1，所述种群基本操作为随机移动、随机长距离跳动、跟随代价值小于自身的个体、跟随代价值最小的个体、跟随历史最佳个体或突变，所述种群基本操作集合将上述种群基本操作合并表示为o-{o1,o2,…,ok}，其中每个种群基本操作对应一个操作选择概率向量，所述操作选择概率向量为p-{p1,p2,...,pk}；

步骤2.2：选择步骤1.5种群p中的一个个体，再选择操作选择概率向量中最大概率对应的种群基本操作oi；

步骤2.3：对步骤2.2选择的个体使用种群基本操作oi，并根据步骤1.6至1.8，更新当前个体的综合代价值；

步骤2.4：重复步骤2.2和2.3，直至遍历整个种群，对每个个体均使用一次种群基本操作oi之后，至此完成一次迭代；

步骤2.5：评估步骤2.4的迭代的运行情况，并调整操作选择概率向量后，返回步骤2.2进行下一次迭代，直到迭代次数达到设定值。

更进一步地，所述步骤2.1的种群基本操作中的：

所述随机移动是指移动解xi到其邻域，表达式为：xi:＝xi+λmin·rand·u，其中，λmin是集中化搜索的步长，是服从均匀分布的随机数，u是随机单位方向向量；

所述随机长距离跳动是指移动解xi到较远的位置，表达式为：xi:＝xi+λmax·u，其中，λmax是多样化搜索的步长，u是随机单位方向的向量；

所述跟随代价值小于自身的个体是指将解xi向所有代价值小于xi的解移动，表达式为：xi:＝xi+λ·rand·(xj-xi)，对于所有满足f(xj)＜f(xi)的解xj，解xi都要向其移动；

所述跟随代价值最小的个体是指将解xi向代价值最小的，也就是最优的解移动，表达式为：xi:＝xi+λ·rand·(xb-xi)，其中，xb是种群中的最优个体表示的解；

所述跟随历史最佳个体是指将解xi向其多次进化后得到的多个代价值中最小的个体，也就是历史最佳个体的位置移动，表达式为：其中，表示解xi的历史最佳位置；

所述突变是指将解xi突变得到一个新解xi′。

更进一步地，所述步骤2.5中评估步骤2.4的迭代的运行情况是指要保证集中化指数与多样化指数达到平衡，所述集中化指数通过迭代前后最佳代价值来评估，即根据和的大小关系来确定；所述多样化指数通过所有解和中心解的距离之和来评估，公式为：其中，中心解即根据d^t-1和d^t的大小关系来确定。

综上所述，由于采用了上述技术方案，本发明的有益效果是：

1.基于种群超启发式算法的洋流环境下auv路径规划方法，采用b样条曲线表示auv路径，用种群超启发式算法优化选取b样条曲线的控制点位置，进而得到满足时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的最优路径，得到的路径是连续的、光滑的且满足洋流信息约束，降低了auv的跟踪难度和跟踪耗能。

2.本发明采用的b样条曲线，使用数量较少的控制点便可以得到复杂且光滑的路径，且只通过路径和洋流信息的几何关系定义洋流信息约束，减少了方法的复杂程度。

3.本发明将路径规划问题用一个代价函数建模成一个非线性的优化问题，用种群超启发式算法进行求解，进而得到b样条曲线控制点，种群超启发式算法综合了各种种群元启发式算法的优点，在求解非线性优化问题时有较大优势，计算简单且收敛性更好。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图，其中：

图1是基于种群超启发式算法的洋流环境下auv路径规划方法的流程图；

图2是本发明的步骤1.2中固定控制点的示意图；

图3是本发明在仅包含洋流信息环境中的路径规划结果示意图；

图4是本发明在包含障碍物和洋流信息环境中的路径规划结果示意图；

图5是本发明运行200次的时间和代价稳定性分析图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明，即所描述的实施例只是本发明的一部分实施例，而不是全部的实施例。通常在此处描述和附图中示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

基于种群超启发式算法的洋流环境下auv路径规划方法，解决了现有auv路径规划方法规划的路径不光滑、难以跟踪和跟踪耗能高的问题。

基于种群超启发式算法的洋流环境下auv路径规划方法，包括以下步骤：

步骤1：初始化种群，再根据综合了时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的代价函数得到整个种群中所有个体的初始综合代价值；

步骤2：设置种群基本操作集合和对应的操作选择概率向量；选择种群中的一个个体，再选择操作选择概率向量中最大概率对应的种群基本操作，对该个体使用该种群基本操作，并更新其综合代价值；重复上述流程直至遍历整个种群，至此完成一次迭代；然后，调整操作选择概率向量，进行下一次迭代，直到迭代次数达到设定值；

步骤3：迭代完成后，选择代价值最小的个体，即最优个体，结合b样条曲线的起点、终点和固定控制点构成一组控制点，再由这组控制点生成最优路径。

本发明采用b样条曲线表示auv路径，使用数量较少的控制点便可以得到复杂且光滑的路径；将路径规划问题用一个代价函数建模成一个非线性的优化问题，用种群超启发式算法优化选取b样条曲线的控制点位置，种群超启发式算法综合了各种种群元启发式算法的优点，在求解非线性优化问题时有较大优势，计算简单且收敛性更好；进而得到满足时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的最优路径，只通过路径和洋流信息的几何关系定义洋流信息约束，减少了方法的复杂程度；最后得到的路径是连续的、光滑的且满足洋流信息约束，降低了机器人的跟踪难度和跟踪路径耗能。

下面结合实施例对本发明的特征和性能作进一步的详细描述。

实施例一

本发明的较佳实施例，提供了基于种群超启发式算法的洋流环境下auv路径规划方法，如图1所示，包括以下步骤：

步骤1：初始化种群，再根据综合了时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的代价函数得到整个种群中所有个体的初始综合代价值；

步骤1.1：设定auv路径的起点和终点，连接起点和终点，并在该连线上生成m个等分点，在每个等分点上根据起点和终点的连线作垂线，得到m条垂线，每条垂线上生成imax个随机点，从而得到imax组随机点，第i组随机点表示为rpm,i(rp1,i,rp2,i,…rpk,i,…rpm,i，rpk,i∈r²)，i∈(1,2,…,imax)；

步骤1.2：从起点出发，沿auv的起始方向短距离增加一个固定控制点p1，从终点出发，沿auv的终止方向的反方向以短距离增加一个固定控制点p2，用于控制曲线路径的起始方向和终止方向；

步骤1.3：根据步骤1.1和步骤1.2，以起点、终点、一组随机点rpm,i(rp1,i,rp2,i,…rpk,i,…rpm,i，rpk,i∈r²)、固定控制点p1和固定控制点p2作为一条b样条曲线的控制点，则控制点的数量为n，n＝m+4，控制点的位置为再结合曲线阶次k，得到离散点组成的imax条b样条曲线，曲线方程为：

其中，bi,k(t)是k次b样条曲线的基函数；

步骤1.4：由imax条b样条曲线得到imax条auv路径，路径表示为：

其中，auv的朝向ψ为

步骤1.5：将imax组随机点初始化得到种群，一组随机点rpm,i(rp1,i,rp2,i,…rpk,i,…rpm,i，rpk,i∈r²)表示一个个体，即一个解xi，其中，个体的维度为m，个体的数量为imax，种群p为：

即，

步骤1.6：根据综合了时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的代价函数计算每个个体的初始代价值，所述代价函数结合了时间和罚函数，表达式为：

其中，tcost是时间代价，通过路径距离除以平均速度得到，

ρcost是曲率代价，通过累加每个离散点处的近似曲率得到，

mcost是地图代价，通过累加每个离散点在地图上的值(障碍物为1，可通过区域为0)得到，

ocost是洋流信息约束，通过累加每个离散点路径方向和水流方向的夹角及水流向量模的乘积得到，h是b样条曲线上离散点的数量；

步骤1.7：将四项初始代价值分别进行归一化处理，由于不同的代价值具有不同的量纲，通过最大代价值和最小代价值来得到归一化的代价值

步骤1.8：将归一化后的四项代价值通过不同的权重加权求和，可以得到整个种群p中所有个体的初始综合代价值：

其中，ω1，ω2，ω3，ω4是不同代价值的权重，auv路径综合代价值越小，auv路径越优，将auv路径规划问题建模为一个非线性的代价函数的最小化问题；

步骤2：设置种群基本操作集合和对应的操作选择概率向量；选择种群中的一个个体，再选择操作选择概率向量中最大概率对应的种群基本操作，对该个体使用该种群基本操作，并更新其综合代价值；重复上述流程直至遍历整个种群，至此完成一次迭代；然后，调整操作选择概率向量，进行下一次迭代，直到迭代次数达到设定值；

步骤2.1：设置种群基本操作集合和对应的操作选择概率向量，所述种群基本操作用o<·>表示，对输入的解x^t，在一次迭代之后输出一个新解x^t+1，表达式为o<x^t>＝x^t+1，所述种群基本操作为随机移动、随机长距离跳动、跟随代价值小于自身的个体、跟随代价值最小的个体、跟随历史最佳个体或突变，所述种群基本操作集合将上述种群基本操作合并表示为o-{o1,o2,…,ok}，其中每个种群基本操作对应一个操作选择概率向量，所述操作选择概率向量为p-{p1,p2,...,pk}，其中：

所述随机移动是指移动解xi到其邻域，表达式为：xi:＝xi+λmin·rand·u，其中，λmin是集中化搜索的步长，是服从均匀分布的随机数，u是随机单位方向向量；

所述随机长距离跳动是指移动解xi到较远的位置，表达式为：xi:＝xi+λmax·u，其中，λmax是多样化搜索的步长，u是随机单位方向的向量；

所述跟随代价值小于自身的个体是指将解xi向所有代价值小于xi的解移动，表达式为：xi:＝xi+λ·rand·(xj-xi)，对于所有满足f(xj)＜f(xi)的解xj，解xi都要向其移动；

所述跟随代价值最小的个体是指将解xi向代价值最小的，也就是最优的解移动，表达式为：xi:＝xi+λ·rand·(xb-xi)，其中，xb是种群中的最优个体表示的解；

所述跟随历史最佳个体是指将解xi向其多次进化后得到的多个代价值中最小的个体，也就是历史最佳个体的位置移动，表达式为：其中，表示解xi的历史最佳位置；

所述突变是指将解xi突变得到一个新解xi′；

步骤2.2：选择步骤1.5种群p中的一个个体，再选择操作选择概率向量中最大概率对应的种群基本操作oi；

步骤2.3：对步骤2.2选择的个体使用种群基本操作oi，并根据步骤1.6至1.8，更新当前个体的综合代价值；

步骤2.4：重复步骤2.2和2.3，直至遍历整个种群，对每个个体均使用一次种群基本操作oi之后，至此完成一次迭代；

步骤2.5：评估步骤2.4的迭代的运行情况，并调整操作选择概率向量后，返回步骤2.2进行下一次迭代，直到迭代次数达到设定值，所述评估步骤2.4的迭代的运行情况是指要保证集中化指数与多样化指数达到平衡；所述集中化指数通过迭代前后最佳代价值来评估，即根据和的大小关系来确定；所述多样化指数通过所有解和中心解的距离之和来评估，公式为：其中，中心解即根据d^t-1和d^t的大小关系来确定；所述集中化指数与多样化指数的递增关系如表1所示，

表1

根据上表可知，在评估迭代运行情况时，若则说明在这一次迭代中集中化指数较高，若同时d^t-1＞d^t，则说明在这一次迭代中还存在多样化指数较小，因此，需要减少集中化指数，增加多样化指数，对应地调整选择概率后，再返回步骤2.2继续进行迭代；其他情况对应表1进行调整选择概率后，返回步骤2.2继续进行迭代。

步骤3：迭代完成后，选择代价值最小的个体，即最优个体，结合b样条曲线的起点、终点和固定控制点构成一组控制点，再由这组控制点生成最优路径。

将本发明应用于仅包含洋流信息环境中，规划出的路径如图3所示；将本发明应用于包含障碍物和洋流信息环境中，规划出的路径如图4所示；可以看出，得到的路径是连续的、光滑的，可以降低auv的跟踪难度。

在实验过程中，记录本发明运行200次的时间稳定性和代价稳定性，做出如图5所示的分析图，可见运行时间集中在0.6s到0.64s之间，在最大值和最小值范围之上出现了两个异常值，但是都在0.66s以内；最终代价值集中在610到635之间，在最大值635之上出现了三个异常值，但都在655以内，由此可知，本发明的稳定性较好，可以运用于实际auv的路径规划过程中。

综上所述，本发明采用b样条曲线表示auv路径，使用数量较少的控制点便可以得到复杂且光滑的路径；将路径规划问题用一个代价函数建模成一个非线性的优化问题，用种群超启发式算法优化选取b样条曲线的控制点位置，种群超启发式算法综合了各种种群元启发式算法的优点，在求解非线性优化问题时有较大优势，计算简单且收敛性更好；进而得到满足时间效率、环境地图、auv本体运动曲率半径和洋流信息约束的最优路径，只通过路径和洋流信息的几何关系定义洋流信息约束，减少了方法的复杂程度；最后得到的路径是连续的、光滑的且满足洋流信息约束，降低了机器人的跟踪难度和跟踪路径耗能。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明的保护范围，任何熟悉本领域的技术人员在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。