CN113985899B - 基于区间多目标优化的水下机器人全局路径规划方法 - Google Patents

Abstract

本发明公开了基于区间多目标优化的水下机器人全局路径规划方法，包括以下步骤：根据海流数据建立规划环境模型；编码粒子，初始化各路径的B样条控制点；将各个路径控制点的坐标代入到B样条曲线的计算公式中计算路径曲线的各个路径点坐标；计算各路径航行时间区间和危险度区间；利用区间可能度模型对候选路径进行占优排序并存入外部储备集，利用区间拥挤距离公式按外部储备集中各个路径的拥挤距离排序；对不可行路径进行的变异操作；根据粒子位置更新公式更新各条路径的位置；判断是否达到迭代次数，不满足则返回步骤(3)，满足则输出最优路径集。本发明解决目前水下机器人路径规划方法只考虑单一的规划目标，路径鲁棒性较差的问题。

Description

基于区间多目标优化的水下机器人全局路径规划方法

技术领域

本发明涉及自动控制领域，尤其涉及基于区间多目标优化的水下机器人全局路径规划方法。

背景技术

在水下机器人工作的环境中，海流和障碍对其航行有很大影响。如何最大化的利用海水流动的能量并避开障碍物，使水下机器人快速且安全地航行到目标区域，已成为近年来水下机器人路径规划的主要研究内容之一。目前的规划方法主要集中在静态的全局路径规划上，没有考虑到实际环境中存在的许多不确定约束条件。由于海水流动的不确定性和测量方法的不精确，使得测量得到的海流数据与真实的海流数据存在一些差异；同时，有些危险源位置不能精确掌握，也会影响水下机器人航行。因此对前述差异的处理非常重要，且具有实际意义。

海流对路径规划的影响主要是海流的流动会使水下机器人的航行速度和航向变化，因此规划路径时应尽量选择顺着海流航行，避开障碍及漩涡等，以节约水下机器人的航行时间及能量消耗；现有的路径搜索方法一般是根据先前测量得到的海洋数据，利用图搜索算法或进化算法等搜索由起点至终点的航行时间最小或能量消耗最低的路径，因为未考虑海流不确定性和危险源的不确定性，得到的路径鲁棒性较差，有时会出现规划的路径不能抵达终点的情况。现有的基于区间优化的水下机器人时间最优路径规划方法，是利用区间序关系的方法，将不确定优化问题转化为确定性优化问题，然后利用传统的单目标优化算法求解，但在转化过程中，不可避免地会损失一些有价值的信息，并且上述方法求解的都只考虑了单一的规划目标，并不全面。

CN110244759A一种基于区间优化的水下机器人时间最优路径规划方法，基于海流预测系统给出的预测值，建立不确定海流模型，利用蚁群系统算法，搜索由起点至终点候选路径，应用矢量分析法计算候选路径在区间海流环境下航行时间最大值和最小值，再以区间半径和区间均值计算该候选路径评价函数值，最后将评价函数值返回到蚁群系统算法，根据寻优机制搜索最优路径。虽然考虑了海流不确定性，但是未考虑危险源的不确定性，得到的路径鲁棒性较差，有时会出现规划的路径不能抵达终点的情况。

发明内容

发明目的：为了解决目前水下机器人路径规划方法只考虑单一的规划目标，路径鲁棒性较差，规划的路径不能抵达终点的问题，本发明提供基于区间多目标优化的水下机器人全局路径规划方法，利用区间的占优排序和拥挤距离结合多目标量子粒子群算法解决不确定海流环境下的多目标路径规划问题。

技术方案：基于区间多目标优化的水下机器人全局路径规划方法，包括以下步骤：

(1)确定水下机器人路径规划的区域，根据海流数据建立规划环境模型，设定海流的不确定度以及不确定危险源的位置和不确定度；

(2)编码粒子，初始化各路径的B样条控制点，将路径编码为准均匀三次B样条曲线的控制点序列B(b₁，b₂，...，b_n)，其中前三个点为起点，最后三个点为终点，除起点和终点外，其他点随机初始化一个坐标b_i＝(x_i，y_i，z_i)；

(3)根据B样条控制点计算实际航行路径，将各个路径控制点的坐标代入到B样条曲线的计算公式中计算路径曲线的各个路径点坐标；

(4)计算各路径航行时间区间和危险度区间；

(5)利用区间可能度模型对候选路径进行占优排序并存入外部储备集，利用区间拥挤距离公式按外部储备集中各个路径的拥挤距离排序；

(6)在占优等级最高的个体中随机选择一个作为群体最优位置，基于区间占优关系选择个体历史最优位置；

(7)对不可行路径进行的变异操作；

(8)根据动态权重的量子粒子群算法的粒子位置更新公式更新各条路径的位置；

(9)基于区间占优关系和区间拥挤度对各路径进行优劣排序，判断是否达到迭代次数，不满足则返回步骤(3)，满足则输出最优路径集。

所述步骤(1)设定海流的不确定度以及不确定危险源的位置和不确定度为：

设海流的方向角为c_d，海流大小为c_v不确定海流的方向和大小不确定水平分别为θ和φ，通过统计历史数据的误差进行取值，海流方向和大小的区间数分别为[c_d-θ，c_d+θ]和[c_v-φ，c_v+φ]；

设危险源的不确定范围为半径为r的球体Ω内，设球心为O，路径与危险源距离最近的点p到危险源实际位置的可能最短距离为：

可能最长距离为：

d_max＝d(p，O)+r

可得路径点与危险源的可能距离为区间数[d_min，d_max]。

所述步骤(3)将各个路径控制点的坐标代入到B样条曲线的计算公式中计算路径曲线的各个路径点坐标的过程为：

其中B₃(t)是3次B样条曲线的基函数，X、Y、Z为计算得到的路径航行点。

所述步骤(4)为计算由步骤(3)得到的路径在不确定海流和不确定危险源下的响应区间，即航行时间的最大值最小值和危险度的最大值最小值，

水下机器人在海流条件下所消耗的时间为在各条子路径上消耗时间之和:

其中是水下机器人相对于海底的速度，是通过推进速度和海流矢量叠加得到的，海流的大小和方向是一个区间量，计算得到的航行时间为区间/>

路径的危险度与路径和危险源的距离成反比，危险源可能的位置范围为区间d＝[d_min,d_max]，定义一个安全距离l_max和一个危险距离l_min，根据下式计算路径相对第i个危险源的危险度：

计算得到的路径危险度为区间数

所述步骤(5)将基于区间可能度的占优关系定义为：

设候选路径X₁和X₂在区间海流作用下的航行时间区间分别为T₁ ^I和区间可能度模型表述如下：

其中，T₁ ^w和分别为两个区间的区间宽度，/>为区间/>优于T₁ ^I的可能性，设候选路径X₁和X₂的危险度区间分别为/>和/>如果同时有/>和则路径X₁占优X₂；

基于区间的拥挤距离计算方法为：设路径X₁和X₂的适应度区间分别为T₁ ^I、和它们交区间的宽度为/>和/>那么两路径的重叠度可以表示为：

记区间的中点为m(T^I)和m(H^I)，路径X₁的目标函数超体的体积为两目标区间宽度的乘积

路径X₁和X₂的拥挤距离与两区间中点之间的距离成正比，与两路径适应度超体的重叠度大小和区间自身的大小成反比，则拥挤距离可表示为：

设由上式得到的与路径X₁距离最近的两条在占有排序中与X₁具有相同序值的个体分别为X₂和X₃，则个体X₁在相同序值的群体中的拥挤度表示为：

拥挤距离越大，个体有越好的分布性。

所述步骤(7)中变异操作为随机单点变异，即随机生成一个路径曲线控制点个数n以内的随机数k，选择B样条曲线控制点序列B(b₁,b₂，...,b_n)中第k个控制点b_k进行变异操作，其中被选中控制点b_k＝(x_k,y_k,z_k)，u为每次变异的范围，x_max、y_max、z_max为规划区域的边界范围。

所述步骤(8)中量子粒子群算法的粒子位置更新公式为：

其中i(i＝1，2，...，M)表示第i个粒子，M为群体规模，j(j＝1，2，...，N)表示粒子的第j维，N为搜索空间维数，u_i，j(t)和均为[0，1]区间上均匀分布的随机数，p_i，j(t)表示迭代代数为t时吸引子位置，y_i(t)为个体历史最优位置，/>为群体最优位置，C(t)为所有粒子历史最好位置的平均，扩张收缩因子α决定了粒子的收敛性。

所述扩张收缩因子α按余弦衰减方式衰减，先缓慢衰减广泛搜索，再快速衰减进行局部优化，

其中iter为总迭代次数，t为当前迭代次数，α从1.7随迭代次数先慢后快地减小到0.5。

有益效果：与现有技术相比，本发明具有如下显著优点：

不同于现有技术只考虑了单一的规划目标，本发明同时考虑不确定海流和不确定危险源的情况，得到的路径鲁棒性好；利用基于区间可能度模型的占优排序和基于区间的拥挤距离计算方法，结合量子粒子群算法寻找最优路径，一定程度上解决了不确定环境下的多目标路径规划问题；本方法可以在不确定环境中规划出多条分布性好的可行路径，极大减少规划的路径不能抵达终点的情况。

附图说明

图1为基于区间多目标优化的水下机器人全局路径规划方法的流程图；

图2为样条曲线构造的路径示意图；

图3为海流不确定区间示意图；

图4为路径与不确定危险源示意图；

图5为路径规划海洋环境图；

图6为路径规划结果图。

具体实施方式

下面结合附图对本发明的技术方案作进一步说明。

基于区间多目标优化的水下机器人全局路径规划方法，如图1所示，包括以下步骤：

(1)确定水下机器人路径规划的区域，根据海流数据建立规划环境模型，设定海流的不确定度以及不确定危险源的位置和不确定度，获取海洋环境和海流数据，建立如图5所示的海洋环境模型并初始化模型参数，模型参数包括航行范围、水下机器人航行速度、不确定危险源的不确定范围、不确定海流的范围、路径粒子的数目、迭代次数、扩张收缩因子α和外部储备集元素数目S；

(2)编码粒子，初始化各路径的B样条控制点，将粒子编码为B样条曲线控制点坐标组成的序列B(b₁,b₂,...,b_n)，其中每个控制点b_i＝(x_i,y_i,z_i)，设定每个控制点的搜索范围，在搜索范围内随机初始化每条路径；

(3)根据B样条曲线生成规则：

代入控制点坐标，B₃(t)为3次B样条基函数由控制点生成具体的路径效果如图2所示；

(4)计算候选路径在不确定海流和不确定危险源下的响应区间，即航行时间的最大值最小值和危险度的最大值最小值；

航行时间计算：水下机器人在海流条件下所消耗的时间为在各条子路径上消耗时间之和:

其中是水下机器人相对于海底的速度，是通过推进速度和海流矢量叠加得到的。由于海流的大小和方向是一个区间量，不确定海流如图3所示，计算得到的航行时间也为区间/>

危险度计算：不确定危险源与路径的关系如图4所示，设危险源的不确定范围为半径为r的球体Ω内，设球心为O,路径与危险源距离最近的点p到危险源实际位置的可能最短距离为：

可能最长距离为：d_max＝d(p,O)+r

可得路径点与危险源的可能距离为区间数[d_min,d_max]；

路径的危险度与路径和危险源的距离成反比，由于危险源可能的位置范围为区间，定义一个安全距离l_max和一个危险距离l_min，根据下式计算路径相对第i个危险源的危险度：

计算得到的路径危险度为区间数

(5)利用区间可能度模型对候选路径进行占优排序并存入外部储备集，利用区间拥挤距离公式按外部储备集中各个路径的拥挤距离排序；

基于区间可能度的占优关系定义为：设候选路径X₁和X₂在区间海流作用下的航行时间区间分别为T₁ ^I和区间可能度模型表述如下：

其中，T₁ ^w和分别为两个区间的区间宽度，/>为区间/>优于T₁ ^I的可能性。候选路径X₁和X₂的危险度区间分别为/>和/>如果同时有/>和则路径X₁占优X₂；

基于区间的拥挤距离计算方法为：设路径X₁和X₂的适应度区间分别为T₁ ^I、和它们交区间的宽度为/>和/>那么两路径的重叠度可以表示为:

记区间的中点为m(T^I)和m(H^I),路径X₁的目标函数超体的体积为两目标区间宽度的乘积：

路径X₁和X₂的拥挤距离与两区间中点之间的距离成正比，与两路径适应度超体的重叠度大小和区间自身的大小成反比，则拥挤距离可表示为：

设由上式得到的与路径X₁距离最近的两条在占有排序中与X₁具有相同序值的个体分别为X₂和X₃，则个体X₁在相同序值的群体中的拥挤度表示为：

(6)在占优等级最高的个体中随机选择一个作为群体最优位置，基于区间占优关系选择个体历史最优位置；

(7)对不可行路径进行的变异操作为随机单点变异，即随机生成一个路径曲线控制点个数n以内的随机数k，选择B样条曲线控制点序列B(b₁,b₂,...,b_n)中第k个控制点b_k进行变异操作，

其中被选中控制点b_k＝(x_k,y_k,z_k)，u为变异的范围，x_max、y_max、z_max为规划区域的边界范围；

(8)根据动态权重的量子粒子群算法的粒子位置更新公式更新各条路径的位置，量子粒子群的粒子位置更新方式如下：

其中i(i＝1,2,...,M)表示第i个粒子，M为群体规模；j(j＝1,2,...,N)表示粒子的第j维，N为搜索空间维数；u_i,j(t)和均为[0,1]区间上均匀分布的随机数，p_i,j(t)表示迭代代数为t时吸引子位置；y_i(t)为个体历史最优位置，/>为群体最优位置，C(t)为所有粒子历史最好位置的平均；扩张收缩因子α决定了粒子的收敛性，α按余弦衰减方式衰减，先缓慢衰减广泛搜索，再快速衰减进行局部优化，

其中iter为总迭代次数，t为当前迭代次数，α从1.7随迭代次数先慢后快地减小到0.5。

(9)基于区间占优关系和区间拥挤度对各路径进行优劣排序，判断是否达到迭代次数，不满足则返回步骤(3)，满足则输出如图6所示的最优路径集。

根据图6迭代500次的规划结果可以看出，算法可以成功地在不确定环境中规划出多条可行路径。

Claims (8)

1.基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，包括以下步骤：

(1)确定水下机器人路径规划的区域，根据海流数据建立规划环境模型，设定海流的不确定度以及不确定危险源的位置和不确定度；

(2)编码粒子，初始化各路径的B样条控制点，将路径编码为准均匀三次B样条曲线的控制点序列B(b₁，b₂，...，b_n)，其中前三个点为起点，最后三个点为终点，除起点和终点外，其他点随机初始化一个坐标b_i＝(x_i，y_i，z_i)；

(3)根据B样条控制点计算实际航行路径，将各个路径控制点的坐标代入到B样条曲线的计算公式中计算路径曲线的各个路径点坐标；

(4)计算各路径航行时间区间和危险度区间；

(5)利用区间可能度模型对候选路径进行占优排序并存入外部储备集，利用区间拥挤距离公式按外部储备集中各个路径的拥挤距离排序；

(6)在占优等级最高的个体中随机选择一个作为群体最优位置，基于区间占优关系选择个体历史最优位置；

(7)对不可行路径进行的变异操作；

(8)根据动态权重的量子粒子群算法的粒子位置更新公式更新各条路径的位置；

(9)基于区间占优关系和区间拥挤度对各路径进行优劣排序，判断是否达到迭代次数，不满足则返回步骤(3)，满足则输出最优路径集。

2.根据权利要求1所述的基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，所述步骤(1)设定海流的不确定度以及不确定危险源的位置和不确定度为：

设海流的方向角为c_d,海流大小为c_v不确定海流的方向和大小不确定水平分别为θ和φ，通过统计历史数据的误差进行取值，海流方向和大小的区间数分别为[c_d-θ,c_d+θ]和[c_v-φ,c_v+φ]；

设危险源的不确定范围为半径为r的球体Ω内，设球心为O,路径与危险源距离最近的点p到危险源实际位置的可能最短距离为：

可能最长距离为：

d_max＝d(p,O)+r

可得路径点与危险源的可能距离为区间数[d_min,d_max]。

3.根据权利要求1所述的基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，所述步骤(3)将各个路径控制点的坐标代入到B样条曲线的计算公式中计算路径曲线的各个路径点坐标的过程为：

其中B₃(t)是3次B样条曲线的基函数，X、Y、Z为计算得到的路径航行点。

4.根据权利要求1所述的基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，所述步骤(4)为计算由步骤(3)得到的路径在不确定海流和不确定危险源下的响应区间，即航行时间的最大值最小值和危险度的最大值最小值，

水下机器人在海流条件下所消耗的时间为在各条子路径上消耗时间之和:

其中是水下机器人相对于海底的速度，是通过推进速度和海流矢量叠加得到的，海流的大小和方向是一个区间量，计算得到的航行时间为区间/>

路径的危险度与路径和危险源的距离成反比，危险源可能的位置范围为区间d＝[d_min,d_max]，定义一个安全距离l_max和一个危险距离l_min，根据下式计算路径相对第i个危险源的危险度：

计算得到的路径危险度为区间数

5.根据权利要求1所述的基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，所述步骤(5)将基于区间可能度的占优关系定义为：

设候选路径X₁和X₂在区间海流作用下的航行时间区间分别为T₁ ^I和区间可能度模型表述如下：

其中，T₁ ^w和分别为两个区间的区间宽度，/>为区间/>优于T₁ ^I的可能性，

设候选路径X₁和X₂的危险度区间分别为和/>如果同时有/>和/>则路径X₁占优X₂；

基于区间的拥挤距离计算方法为：设路径X₁和X₂的适应度区间分别为T₁ ^I、和它们交区间的宽度为/>和/>那么两路径的重叠度可以表示为：

记区间的中点为m(T^I)和m(H^I),路径X₁的目标函数超体的体积为两目标区间宽度的乘积

路径X₁和X₂的拥挤距离与两区间中点之间的距离成正比，与两路径适应度超体的重叠度大小和区间自身的大小成反比，则拥挤距离可表示为：

设由上式得到的与路径X₁距离最近的两条在占有排序中与X₁具有相同序值的个体分别为X₂和X₃，则个体X₁在相同序值的群体中的拥挤度表示为：

拥挤距离越大，个体有越好的分布性。

6.根据权利要求1所述的基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，所述步骤(7)中变异操作为随机单点变异，即随机生成一个路径曲线控制点个数n以内的随机数k，选择B样条曲线控制点序列B(b₁,b₂,...,b_n)中第k个控制点b_k进行变异操作，其中被选中控制点b_k＝(x_k,y_k,z_k)，u为每次变异的范围，x_max、y_max、z_max为规划区域的边界范围。

7.根据权利要求1所述的基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，所述步骤(8)中量子粒子群算法的粒子位置更新公式为：

其中i(i＝1,2,...,M)表示第i个粒子，M为群体规模，j(j＝1,2,...,N)表示粒子的第j维，N为搜索空间维数，u_i,j(t)和均为[0,1]区间上均匀分布的随机数，p_i,j(t)表示迭代代数为t时吸引子位置，y_i(t)为个体历史最优位置，/>为群体最优位置，C(t)为所有粒子历史最好位置的平均，扩张收缩因子α决定了粒子的收敛性。

8.根据权利要求7所述的基于区间多目标优化的水下机器人全局路径规划方法，其特征在于，所述扩张收缩因子α按余弦衰减方式衰减，先缓慢衰减广泛搜索，再快速衰减进行局部优化，

其中iter为总迭代次数，t为当前迭代次数，α从1.7随迭代次数先慢后快地减小到0.5。