CN111457927B - 动态障碍物下的无人巡航船多目标路径规划方法 - Google Patents

Abstract

本发明公开了一种动态障碍物下的无人巡航船多目标路径规划方法，涉及水质采样和路径规划技术领域。首先，无人机对湖面环境采集图像，进行栅格分割，在栅格地图设置起始点和若干采样点；采用改进的灰狼优化算法对若干采样点进行顺序优化，并将最优顺序的各采样点一一标记到地图上；利用D*Lite算法计算栅格地图中标记的每两个采样点之间的最优栅格路径，得到一条从起始点到最终采样点之间的最优路径；最后，自主巡航船沿最优路径完成巡航。本发明对灰狼优化算法中的收敛因子进行改进，平衡了灰狼优化算法的全局搜索和局部搜索能力，提高了灰狼优化算法的收敛速度和稳定性，可以实现在动态未知环境情况下多个目标点的路径规划。

Description

动态障碍物下的无人巡航船多目标路径规划方法

技术领域

本发明属于湖泊流域水质采样和路径规划技术领域，具体是一种动态障碍物下的无人巡航船多目标路径规划方法。

背景技术

如今，水环境保护问题越来越受到国家的重视。随着科技的发展，在水质采样领域，智能水质采样巡航船逐渐替代人工采集，应用越来越广泛。

但是，智能水质采样巡航船的路径规划问题一直以来都是国内外学者所关注的重点以及难点，其路径的选择是否合理直接影响着巡航船的运行效率。一种适合当前环境、能快速规划出采样最优路径的方法能显著地提高采样效率同时降低采样成本。

当前已有很多传统方法和智能方法被用来解决巡航船的多目标点路径规划问题，如：遗传算法和蚁群算法等。

灰狼优化算法是2014年提出的一种群体智能优化算法，可以实现对多个目标点的顺序优化。但是，该算法无法解决动态未知环境的路径规划问题，并且该算法的收敛性较缓慢。

D*Lite算法是一种能够解决动态未知环境的路径规划问题的算法，它采用的是反向搜索方式。但是，该算法只能够解决单目标点的问题，不能够仅用该算法去遍历多个目标点。

发明内容

本发明针对自主巡航船在动态环境中采集多个点的需求问题，提出了一种动态障碍物下的无人巡航船多目标路径规划方法，利用D*Lite算法结合改进后的灰狼优化算法，充分利用两种算法的优势，缩短了搜索路径所消耗的时间，减少了设备成本，并有效解决了动态环境下多个目标点的路径规划问题。

所述的动态障碍物下的无人巡航船多目标路径规划方法，具体步骤如下：

步骤一：无人机对湖面的环境采集图像，采用栅格法进行分割，得到栅格地图并设置起始点和d个采样点；

具体为：首先，将湖面图像分割成大小相等的栅格，组成一个栅格地图；并将环境中的障碍物设置为黑色栅格，将不规则障碍物进行填充处理，将可行区域设置为白色栅格，在栅格地图中运用二维坐标设置一个起始点和d个采样点。

d为正整数。

步骤二：采用改进的灰狼优化算法对d个采样点进行顺序优化，得到最优顺序并保存。

所述采用改进的灰狼优化算法得到最优顺序的步骤如下：

步骤201：初始化灰狼优化算法的参数。

参数包括：灰狼种群数量n，最大迭代次数t，将采样点个数定义为搜索空间的维数d，灰狼种群的初始位置，以及每只灰狼各自一条随机路径等；

步骤202：每只灰狼在d个采样点的空间中搜索猎物，构建搜索空间与灰狼位置关系的空间域矩阵P；

首先，定义第i只灰狼的位置X_i为一组互不相同的正整数序列，公式如下：

则

代表灰狼种群中的第i只灰狼在第d个采样点上的值；

然后，n只灰狼的位置组成空间域矩阵P，公式如下：

式中，

表示第i只灰狼在第m个采样点的位置，空间域矩阵P中每一行代表一只狼的路径。

步骤203：构建距离矩阵W来表示每个采样点与其它采样点之间的距离；

公式如下：

s_(n,m)为第n个采样点与第m个采样点之间的距离。

步骤204：利用空间域矩阵P和距离矩阵W构建适应度函数f；

表达式为：

P_i是指空间域矩阵P中的第i行只狼的路径；

是指第i行只狼的路径中，对每两个采样点之间的距离相加求和。

步骤205：运用灰狼优化算法求解适应度函数，得到最优的遍历顺序；

过程如下：

首先是搜寻过程：

在t代搜索过程中，灰狼的位置为X(t)，猎物的位置为X_P(t)，灰狼与猎物的距离D表示为：

t表示当前迭代次数，r₂为从0到1的随机数；C是一个随r₂变化的系数。

然后是包围过程：

在灰狼群体包围猎物的过程中，构造灰狼与猎物的关系模型，公式如下：

式中，A·D为包围步长，a为改进后的收敛因子，r₁为从0到1的随机数，t_max表示最大迭代次数，μ₁和μ₂为常数，λ₁和λ₂为调节系数，B为非线性的连续函数Beta函数。

紧接着，位置更新过程：

根据灰狼与猎物的关系模型，分别得到α，β和δ这三类狼的最终位置X₁，X₂和X₃，在α，β和δ的指引下，整个狼群逐步向猎物靠近，通过α，β和δ狼的位置更新，不断确定目标的位置，从而实现灰狼对猎物最终的攻击位置。

式中，X_α，X_β，X_δ表示α，β和δ这三类狼的更新位置，A₁，A₂，A₃表示三个随机数，X则表示灰狼对猎物最终的攻击位置。A₁·D_α，A₂·D_β，A₃·D_δ表示α，β和δ这三类狼的包围步长。

最后判断是否达到最大迭代次数，如果是，则输出迭代过程中最优顺序；否则，依次保存适应度函数最小时得到的三个最优解，再次对α，β和δ这三类狼的位置进行更新。

步骤三：按照灰狼算法得到的最优顺序，将各采样点一一标记到栅格地图上；

步骤四：利用D*Lite算法计算栅格地图中标记的每两个采样点之间的最优栅格路径，得到一条从起始点到最终采样点之间的最优路径；

步骤五：自主巡航船从栅格地图对应的起始点开始巡航，沿着最优栅格路径依次经过各采样点，最终返回起始点完成巡航。

本发明的优点在于：

(1)一种动态障碍物下的无人巡航船多目标路径规划方法，对湖泊环境转变成二维坐标栅格图，简化了地图，缩短了智能水质采样巡航船的路径规划时间。

(2)一种动态障碍物下的无人巡航船多目标路径规划方法，基于改进的灰狼优化算法，解决了多个目标点的顺序优化问题，更适用于实际问题操作，并且收敛因子的改进增强了算法的收敛能力，直接提高了巡航船水质采样的效率。

(3)一种动态障碍物下的无人巡航船多目标路径规划方法，采用了D*Lite算法，满足巡航船在动态未知环境中的路径规划需求，并与改进灰狼优化算法的结合，有效解决了多个目标点的路径规划问题。

附图说明

图1为本发明中一种动态障碍物下的无人巡航船多目标路径规划方法的流程图；

图2为本发明中改进的的灰狼优化算法流程图；

图3为本发明中D*Lite算法路径规划流程图。

具体实施方式

下面结合附图对本发明进行详细说明。

本发明提出一种动态障碍物下的无人巡航船多目标路径规划方法，采用D*Lite算法和改进的灰狼优化算法，为水质采样巡航船采集多个目标点规划最优路径，如图1所示，具体包括如下步骤：

步骤一：无人机对湖面的环境采集图像，采用栅格法进行分割，得到栅格地图并设置起始点和d个采样点；

具体为：首先，将湖面图像分割成大小相等的栅格，组成一个栅格地图；并将环境中的障碍物设置为黑色栅格，将不规则障碍物进行填充处理，将可行区域设置为白色栅格，在栅格地图中运用二维坐标选取和设置一个起始点和d个采样点。

d为正整数。

步骤二：采用改进的灰狼优化算法对d个采样点进行顺序优化，得到起始点经过多个采样点并且返回到起始点的最优顺序并保存。

如图2所示，所述采用改进的灰狼优化算法得到最优顺序的步骤如下：

步骤201：初始化灰狼优化算法的参数。

参数包括：灰狼种群数量n为15，最大迭代次数t为500，将采样点个数定义为搜索空间的维数d选择10，灰狼种群的初始位置，以及每只灰狼各自一条随机路径等；

步骤202：每只灰狼在d个采样点的空间中搜索猎物，构建搜索空间与灰狼位置关系的空间域矩阵P；

首先，定义第i只灰狼的位置X_i为一组互不相同的正整数序列，公式如下：

则

代表灰狼种群中的第i只灰狼在第d个采样点上的值；

然后，n只灰狼在d维空间中搜寻猎物，位置组成空间域实体矩阵P，公式如下：

式中，

表示第i只灰狼在第m个采样点的位置，空间域矩阵P中首行表示第1只灰狼在搜索空间中的位置序列，矩阵尾列表示在d维搜索空间中的n只灰狼，每一行代表一只狼的路径。

步骤203：构建距离矩阵W来表示每个采样点与其它采样点之间的距离；

公式如下：

s_(n,m)为第n个采样点与第m个采样点之间的距离。

步骤204：利用空间域矩阵P和距离矩阵W构建适应度函数f；求解最短路径问题的本质是求出一个最优的灰狼种群位置，使得适应度函数最小，

表达式为：

P_i是指空间域矩阵P中的第i行只狼的路径；

是指第i行只狼的路径中，对每两个采样点之间的距离相加求和。比如在灰狼位置P_i＝(2,3,1,5,4)的顺序下，在距离矩阵分别对应着s_(2,3)、s_(3,1)、s_(1,5)、s_(5,4)、s_(4,2)，对应的适应度函数f＝sum(s_(2,3),s_(3,1),s_(1,5),s_(5,4),s_(4,2))。

步骤205：运用灰狼优化算法求解适应度函数，得到最优的遍历顺序；

过程如下：

首先是搜寻过程：

灰狼群体在搜寻猎物时的一个重要标准就是与猎物间的距离，在t代搜索过程中，灰狼的位置为X(t)，猎物的位置为X_P(t)，灰狼与猎物的距离D表示为：

t表示当前迭代次数，r₂为从0到1的随机数；C是一个随r₂变化的系数。

然后是包围过程：

在灰狼群体包围猎物的过程中，构造灰狼与猎物的关系模型，公式如下：

式中，A·D为包围步长，a为改进后的收敛因子，r₁为从0到1的随机数，t_max表示最大迭代次数，μ₁为常数0.01，μ₂为常数0.1，λ₁为调节系数1，λ₂为调节系数0.1，B为非线性的连续函数Beta函数。

改进后的收敛因子a的曲线趋势较好，收敛因子a会随着迭代次数的增加而非线性减少，在刚开始迭代时，收敛因子a会非线性的迅速下降，提高了灰狼优化算法前期的全局搜索能力，而随着迭代次数的增加，收敛因子a的下降曲线近似于线性，保证了灰狼优化算法的后期的局部搜索能力，在整体上提高了灰狼优化算法的收敛能力和稳定性。

紧接着，位置更新过程：

根据灰狼与猎物的关系模型，分别得到α，β和δ这三类狼的最终位置X₁，X₂和X₃，在α，β和δ的指引下，整个狼群逐步向猎物靠近，通过α，β和δ狼的位置更新，不断确定目标的位置，从而实现灰狼对猎物最终的攻击位置。

式中，X_α，X_β，X_δ表示α，β和δ这三类狼的更新位置，A₁，A₂，A₃表示三个随机数，X则表示灰狼对猎物最终的攻击位置。A₁·D_α，A₂·D_β，A₃·D_δ表示α，β和δ这三类狼的包围步长。

因为灰狼的位置是用采样点的顺序表示的，故为整数，但在灰狼迭代计算的过程中必然会出现小数，会出现无法继续迭代的情况。因此，在每一次迭代之后，需要将灰狼的位置进行近似处理，公式如下：

[C,I]＝sort(P,2)

其中I是一个大小等于size(P)的数组，为返回的排序后元素在原数组中的行位置或列位置。

最后判断是否达到最大迭代次数1000的上限，如果是，则输出迭代过程中最优顺序和迭代过程中的最优适应度函数f_min；否则，依次保存适应度函数最小时得到的三个最优解，再次对α，β和δ这三类狼的位置进行更新。

步骤三：按照灰狼算法得到的最优顺序，将各采样点一一标记到栅格地图上；

步骤四：利用D*Lite算法计算栅格地图中标记的每两个采样点之间的最优栅格路径，得到一条从起始点到最终采样点之间的最优路径；

如图3所示，具体过程为：

步骤401：初始化各栅格点的路径代价估计值rhs(s)值、g(s)值和k_m值。

根据获取的湖面二维坐标图，以及由灰狼优化算法计算出的最优顺序，设定D*Lite算法的相关参数，其中，每个栅格点分别作为一个节点；S为地形图中的路径节点的集合，s属于集合S。

步骤402：从栅格地图的当前节点s_goal开始，自当前路径点向周围8个方向邻格扩展，通过比较各扩展相邻节点的估值函数k(s)，选取k(s)值最小点为下一个扩展节点。

当前节点s_goal初始值为第一个采样点；由于水面里大中型生物的移动，水面高度的变化导致岩石的出现，在地图中设置动态障碍物，设水面出现障碍物的概率和障碍物消失的概率都为3％，即每个空白栅格变成障碍物栅格和障碍物栅格变为空白栅格的概率为3％。在探测的过程中，巡航船采用激光探测仪，探测距离为5格*5格。

其中，估值函数k(s)包括两个值：k₁(s)和k₂(s)，优先比较k₁(s)大小，当k₁(s)值相等时，比较k₂(s)大小，公式如下：

k₁(s)＝min(g(s),rhs(s))+h(s_start,S)+k_m

k₂(s)＝min(g(s),rhs(s))

式中，h(s_start,S)为启发函数，表示起始点到当前节点的路径代价；k_m为环境变化时更新的第m次变量；

步骤403：将下一个扩展节点作为当前节点，继续扩展相邻节点，直至达到节点s_start，得到一条从节点s_goal到节点s_start之间的路径；

节点s_start初始值为栅格地图的起始点；

步骤404：针对当前节点s_start，计算该节点所有后继节点的rhs(s)值，选取rhs(s)最小的节点作为新起始点s'_start，巡航船移动到该点s'_start。

步骤405：扫描地图，更新节点信息，判断周围环境是否发生变化，如果没有发生变化，进入步骤406；否则，进入步骤407；

步骤406、将巡航船移动到的新节点s'_start作为当前节点，再次返回步骤402，得到从节点s_goal到该节点之间的路径，并继续移动巡航船的位置，直至巡航船到达节点s_goal，进入步骤408；

步骤407、更新估值函数k(s)中的变量k_m值和节点s'_start，返回步骤402；

表达式如下：

k_m＝k_m-1+h(s_last,s'_start)

s_last＝s'_start

式中，k_m-1为环境变化时更新的第m-1次变量，其中k₀＝0；s_last表示巡航船前一个位置节点，h(s_last,s'_start)表示巡航船前一个位置点到当前节点s'_start的路径代价。

如果航船移动过程中，周围环境没有发生变化，则航船移动的路径与从节点s_goal到节点s_start之间扩展的路径吻合；

当周围环境发生变化后，估值函数的值会发生改变，各节点的扩展节点也会发生改变，从而节点s_goal到节点s_start之间扩展的路径也会发生改变，巡航船的采样路径也会动态变化。

步骤408：判断节点s_goal是否为最终采样点，如果是，终止算法，得到一条遍历多个采样点的最优路径。否则，将该节点s_goal设置为新起始点s'_start，将下一次采样点设置为目标点s_goal，返回步骤402；

步骤五：自主巡航船从栅格地图对应的起始点开始巡航，沿着最优栅格路径依次遍历灰狼优化算法得到的各采样点，最终返回起始点完成巡航。

Claims (1)

1.动态障碍物下的无人巡航船多目标路径规划方法，其特征在于，具体步骤如下：

步骤一：无人机对湖面的环境采集图像，采用栅格法进行分割，得到栅格地图并设置起始点和d个采样点；

具体为：首先，将湖面图像分割成大小相等的栅格，组成一个栅格地图；并将环境中的障碍物设置为黑色栅格，将不规则障碍物进行填充处理，将可行区域设置为白色栅格，在栅格地图中运用二维坐标设置一个起始点和d个采样点；

d为正整数；

步骤二：采用改进的灰狼优化算法对d个采样点进行顺序优化，得到最优顺序并保存；

所述采用改进的灰狼优化算法得到最优顺序的步骤如下：

步骤201：初始化灰狼优化算法的参数；

所述的参数包括：

灰狼种群数量n，最大迭代次数t，将采样点个数定义为搜索空间的维数d，灰狼种群的初始位置，以及每只灰狼各自一条随机路径；

步骤202：每只灰狼在d个采样点的空间中搜索猎物，构建搜索空间与灰狼位置关系的空间域矩阵P；

首先，定义第i只灰狼的位置X_i为一组互不相同的正整数序列，公式如下：

则

代表灰狼种群中的第i只灰狼在第d个采样点上的值；

然后，n只灰狼的位置组成空间域矩阵P，公式如下：

式中，

表示第i只灰狼在第m个采样点的位置，空间域矩阵P中每一行代表一只狼的路径；

步骤203：构建距离矩阵W来表示每个采样点与其它采样点之间的距离；

公式如下：

s_(n,m)为第n个采样点与第m个采样点之间的距离；

步骤204：利用空间域矩阵P和距离矩阵W构建适应度函数f；

表达式为：

P_i是指空间域矩阵P中的第i行只狼的路径；∑s_i(n,m)是指第i行只狼的路径中，对每两个采样点之间的距离相加求和；

步骤205：运用灰狼优化算法求解适应度函数，得到最优的遍历顺序；

具体过程如下：

首先是搜寻过程：

在t代搜索过程中，灰狼的位置为X(t)，猎物的位置为X_P(t)，灰狼与猎物的距离D表示为：

t表示当前迭代次数，r₂为从0到1的随机数；C是一个随r₂变化的系数；

然后是包围过程：

在灰狼群体包围猎物的过程中，构造灰狼与猎物的关系模型，公式如下：

式中，A·D为包围步长，a为改进后的收敛因子，r₁为从0到1的随机数，t_max表示最大迭代次数，μ₁和μ₂为常数，λ₁和λ₂为调节系数，B为非线性的连续函数Beta函数；

改进后的收敛因子a的曲线会随着迭代次数的增加而非线性减少，在刚开始迭代时，收敛因子a会非线性的迅速下降，而随着迭代次数的增加，收敛因子a的下降曲线近似于线性；

紧接着，位置更新过程：

根据灰狼与猎物的关系模型，分别得到α，β和δ这三类狼的最终位置X₁，X₂和X₃，在α，β和δ的指引下，整个狼群逐步向猎物靠近，通过α，β和δ狼的位置更新，不断确定目标的位置，从而实现灰狼对猎物最终的攻击位置；

式中，X_α，X_β，X_δ表示α，β和δ这三类狼的更新位置，A₁，A₂，A₃表示三个随机数，X则表示灰狼对猎物最终的攻击位置；A₁·D_α，A₂·D_β，A₃·D_δ表示α，β和δ这三类狼的包围步长；

最后判断是否达到最大迭代次数，如果是，则输出迭代过程中最优顺序；否则，依次保存适应度函数最小时得到的三个最优解，再次对α，β和δ这三类狼的位置进行更新；

步骤三：按照灰狼算法得到的最优顺序，将各采样点一一标记到栅格地图上；

步骤四：利用D*Lite算法计算栅格地图中标记的每两个采样点之间的最优栅格路径，得到一条从起始点到最终采样点之间的最优路径；

具体过程为：

步骤401：初始化各栅格点的路径代价估计值rhs(s)值、g(s)值和k_m值；

根据获取的湖面二维坐标图，以及由灰狼优化算法计算出的最优顺序，设定D*Lite算法的相关参数，其中，每个栅格点分别作为一个节点；S为地形图中的路径节点的集合，s属于集合S；

步骤402：从栅格地图的当前节点s_goal开始，自当前路径点向周围8个方向邻格扩展，通过比较各扩展相邻节点的估值函数k(s)，选取k(s)值最小点为下一个扩展节点；

当前节点s_goal初始值为第一个采样点；由于水面里大中型生物的移动，水面高度的变化导致岩石的出现，在地图中设置动态障碍物，设水面出现障碍物的概率和障碍物消失的概率都为3％，即每个空白栅格变成障碍物栅格和障碍物栅格变为空白栅格的概率为3％；在探测的过程中，巡航船采用激光探测仪，探测距离为5格*5格；

其中，估值函数k(s)包括两个值：k₁(s)和k₂(s)，优先比较k₁(s)大小，当k₁(s)值相等时，比较k₂(s)大小，公式如下：

k₁(s)＝min(g(s),rhs(s))+h(s_start,S)+k_m

k₂(s)＝min(g(s),rhs(s))

式中，h(s_start,S)为启发函数，表示起始点到当前节点的路径代价；k_m为环境变化时更新的第m次变量；

步骤403：将下一个扩展节点作为当前节点，继续扩展相邻节点，直至达到节点s_start，得到一条从节点s_goal到节点s_start之间的路径；

节点s_start初始值为栅格地图的起始点；

步骤404：针对当前节点s_start，计算该节点所有后继节点的rhs(s)值，选取rhs(s)最小的节点作为新起始点s'_start，巡航船移动到该点s'_start；

步骤405：扫描地图，更新节点信息，判断周围环境是否发生变化，如果没有发生变化，进入步骤406；否则，进入步骤407；

步骤406、将巡航船移动到的新节点s'_start作为当前节点，再次返回步骤402，得到从节点s_goal到该节点之间的路径，并继续移动巡航船的位置，直至巡航船到达节点s_goal，进入步骤408；

步骤407、更新估值函数k(s)中的变量k_m值和节点s'_start，返回步骤402；

表达式如下：

k_m＝k_m-1+h(s_last,s'_start)

s_last＝s'_start

式中，k_m-1为环境变化时更新的第m-1次变量，其中k₀＝0；s_last表示巡航船前一个位置节点，h(s_last,s'_start)表示巡航船前一个位置点到当前节点s'_start的路径代价；

如果航船移动过程中，周围环境没有发生变化，则航船移动的路径与从节点s_goal到节点s_start之间扩展的路径吻合；

当周围环境发生变化后，估值函数的值会发生改变，各节点的扩展节点也会发生改变，从而节点s_goal到节点s_start之间扩展的路径也会发生改变，巡航船的采样路径也会动态变化；

步骤408：判断节点s_goal是否为最终采样点，如果是，终止算法，得到一条遍历多个采样点的最优路径；否则，将该节点s_goal设置为新起始点s'_start，将下一次采样点设置为目标点s_goal，返回步骤402；

步骤五：自主巡航船从栅格地图对应的起始点开始巡航，沿着最优栅格路径依次经过各采样点，最终返回起始点完成巡航。

Images