CN105607646B

CN105607646B - 一种障碍环境下有必经点的uuv航路规划方法

Info

Publication number: CN105607646B
Application number: CN201610082314.XA
Authority: CN
Inventors: 王宏健; 张雪莲; 李庆; 陈涛; 严浙平
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2016-02-05
Filing date: 2016-02-05
Publication date: 2018-06-26
Anticipated expiration: 2036-02-05
Also published as: CN105607646A

Abstract

一种障碍环境下有必经点的UUV航路规划方法，属于UUV控制技术领域。为了使UUV从布放点出发遍历完必经点后回到回收点，且路径长度最短。计算在障碍环境下UUV布放点和回收点以及所有必经点的两两间航路估计距离；以布放点为起点、回收点为终点，使用TSP算法规划出UUV所有必经点的遍历顺序，使UUV从布放点出发、遍历完所有必经点后回到回收点的航路估计距离总和最短；经优化得出从布放点出发经过所有必经点最终到达回收点且能避开所有障碍的航路。仅使用几何判断估算两两必经点间的路径代价，然后使用蚁群算法基于估算代价规划出一个TSP方案，即必经点遍历顺序，最后再使用可变子目标点的蚁群算法规划出最终能避开障碍物的航路。

Description

一种障碍环境下有必经点的UUV航路规划方法

技术领域

本发明涉及一种UUV航路规划方法，属于UUV控制技术领域。

背景技术

目前关于路径规划的文献非常多，大多数的规划目标都是找到一条从布放点到回收点能够避开障碍物的最短避障路径。而实际应用中，UUV(无人水下航行器)的使命可能是遍历完若干个必经点完成相应的任务后才能回到回收点，这似乎是一个TSP问题。但是在障碍环境中TSP问题与传统TSP问题是有很大差异的，因为随机分布的障碍物会导致两点间的路径代价不确定，这就需要增加附加的处理算法。目前，在障碍环境下考虑必经点的文献还不多，具有代表性如文献[1]，首先将使用传统的TSP算法规划出一条遍历所有必经点的回路，然后向与障碍物相交的路径段中插入障碍物的顶点，使其能沿障碍物边缘绕行，以此避过障碍。文献[2]采用的策略是先用Fast Marching法规划出两两必经点间的最短距离，即是确定了任意两必经点间的路径代价，然后使用遗传算法完成一个传统的TSP规划就得到了最优路径。文献[3]首先判断两必经点是否与障碍相交，并将其相交的特性设置到一个代价矩阵中，随后在使用遗传算法规划时，限制具有该特性的两点相邻，以此解决与障碍相交的问题。

现有技术的缺陷：由于障碍物的分布，使两必经点间的路径代价不确定，使用传统的 TSP规划再处理相交线段的方法并不能保证路径的最优性，如图1所示，按照原始TSP的遍历顺序，加入障碍物后得到的路径就不合理了。文献[2]提出的方案是可以得到最优解的，但由于其需要大量规划，实时性很差。文献[3]提出的方法没有仿真验证，算法优劣难以定论。

[1]Murat Cakir.2D Path Planning of UAVs with Genetic Algorithm in aConstrained Environment

[2]于晖,王永骥.基于Fast Marching方法的多目标点路径规划的研究[J].计算技术与自动化.2015,34(3).P11-15

[3]陈晶.基于遗传算法的避障TSP问题算法设计.研究与开发.2006,第229期:P24-26

发明内容

本发明的目的是提供一种障碍环境下有必经点的UUV航路规划方法，以使UUV从布放点出发遍历完必经点后回到回收点，且路径长度最短。

本发明为解决上述技术问题采取的技术方案是：

一种障碍环境下有必经点的UUV航路规划方法，UUV所处障碍环境采用几何环境模型建模，环境中的障碍是用点和线组成的封闭几何图形来描述的；

所述UUV航路规划方法的实现过程为：

步骤一、计算在障碍环境下UUV布放点和回收点以及所有必经点的两两间航路估计距离：

步骤二、以布放点为起点、回收点为终点，使用TSP(传统TSP)算法规划出UUV 所有必经点的遍历顺序，使UUV从布放点出发、遍历完所有必经点后回到回收点的航路估计距离总和最短；

步骤三、利用可变子目标点蚁群算法，经优化得出一条从布放点出发经过所有必经点最终到达回收点且能避开所有障碍的航路。

两两间航路估计距离的求取过程为：

设A、B为布放点、回收点和所有必经点中的任意两点，若直线段AB不与障碍相交时， A、B的航路估计距离即为直线段AB的长度；

若直线段AB至少与一个障碍相交时，分别求得直线段AB两侧障碍顶点到该直线段AB距离最远的两个障碍顶点，从两个障碍顶点中选取与直线段AB距离较近的障碍顶点作为估算顶点，然后再求估算顶点分别与A、B两点的直线距离之和，即获得A、B的航路估计距离。

在步骤三中，利用可变子目标点蚁群算法，经优化得出一条从布放点出发经过所有必经点最终到达回收点且能避开所有障碍的航路，具体过程为：UUV从布放点出发、按照步骤二中得到的所有必经点的遍历顺序，使各个必经点依次作为蚁群算法中的目标点进行避障规划，最后再将回收点作为蚁群算法中的最后一个目标点，使得蚁群算法中的目标点是变化的。

本发明的有益效果是：

本发明使UUV从布放点出发遍历完必经点后回到回收点(有别于传统TSP问题中回到布放点)，且路径长度最短。本发明致力于找到最优解，同时解决了文献[2]中存在的耗时长问题。本发明的关键点为：1、障碍环境下两必经点间的路径代价估算方法；2、可变子目标点蚁群算法。

本发明与文献[2]相比，仅使用几何判断估算两两必经点间的路径代价，然后使用蚁群算法基于估算代价规划出一个TSP方案，即必经点遍历顺序，最后再使用可变子目标点的蚁群算法规划出最终能避开障碍物的航路。

附图说明

图1是传统TSP规划后再处理相交线段图，图中：(a)为无障碍环境下直接使用必经点间的直线距离作为路径代价后，使用TSP规划出UUV必经点遍历顺序的结果图；

附图标记：1表示布放点，2表示回收点，必1、必2、必3、必4分别表示必经点1、必经点2、必经点3、必经点4。

图1(b)为按图1(a)所示的必经点遍历顺序使用避障方法在有障碍环境下得到的UUV 航路结果图；

图2是本发明路径代价(路径距离)估算方法原理示意图；图中：(a)为单个障碍的情况，(b)为有多个障碍的情况下。

图3是本发明方法的仿真验证效果图；

图4是利用本发明方法在较复杂环境下的规划仿真图，图中：(a)为显示效果图，(b) 为体现出算法耗时的仿真结果截图。

具体实施方式

具体实施方式一：如图1～2所示，本实施方式所述的一种障碍环境下有必经点的UUV 航路规划方法，具体过程为：

UUV所处障碍环境采用几何环境模型建模，环境中的障碍是用点和线组成的封闭几何图形来描述的；

所述UUV航路规划方法的实现过程为：

两两间航路估计距离的求取过程为：

实施例：

针对上述具体实施方式所述的障碍环境下有必经点的UUV航路规划方法，给出如下实施例：

一、两两必经点间路径代价的估算方法

如图2所示。假设要估计A、B两必经点间的路径代价，线段AB与障碍物C相交，因此A、B之间相互转移的路径代价不能直接使用直线距离。本文取绕行所需的最短路径作为A、B间的转移代价，计算原理和步骤如下(以图2(a)为例)：

1根据必经点A、B的坐标，求得AB直线方程y-Mx-N＝0，其中于是形成可以区分某点在直线上侧或下侧的区域判别函数 F(x,y)＝y-Mx-N

2将与直线相交的障碍区域C的各顶点坐标带入方程F(x,y)中，可以得到在直线AB上侧的有a点，在下侧的有b、c、d点。

3取每一侧障碍区域顶点到点A、B直线距离之和的最大值作为从该侧绕行的路径长度。如直线AB的上侧只有a点，那么从上侧绕行的路径长度为|Aa|+|aB|；直线AB的下侧有b、c、 d三点，其中|Ac|+|cB|大于Ab|+|bB|或Ad|+|dB|,因此从下侧绕行的路径长度为 |Ac|+|cB|。值得注意的是，基于估算代价的TSP规划的目的是取得一个合理的必经点遍历顺序，所计算的绕行路径长度只是为了体现各种绕行方案之间的差异，为TSP算法提供状态转移代价参考，并不需要准确计算TSP路径长度，因此计算所得的绕行路径(如 |Aa|+|aB|、|Ac|+|cB|)不一定等于真实路径。

4比较从上侧绕行的路径|Aa|+|aB|和从下侧绕行的路径|Ac|+|cB|，取其中的较小值作为从A、B之间的转移距离。A、B之间的转移距离为|Aa|+|aB|。

当与线段AB相交的障碍为多个时(图2(b))依然可以采取上述方法计算绕行距离，计算时将所有相交障碍的所有顶点划分到两侧，而后计算每侧的绕行路径长度，最后比较两侧路径，取较小者为A、B间的转移距离。图2(b)中，假设上侧绕行路径长度经计算为|Af|+|fB|,下侧绕行路径长度经计算为|Ac|+|cB|,比较可得A、B间的路径代价为 |Af|+|fB|。

二、可变子目标点蚁群算法

根据如前所述的估算方法，使用传统的TSP规划后，就可以得到一个合理的必经点遍历顺序方案，按此遍历顺序，使用即将描述的可变子目标点蚁群算法即可得到避开障碍的路径。与传统的蚁群算法在规划过程中有固定的目标点不同，本发明提出的算法其目标点在规划过程中会按照遍历顺序依次变化，每只蚂蚁得到的路径都是从布放点出发遍历过所有的必经点后回到回收点的完整路径。虽然如文献[2]中依次规划出必经点间的子路径，然后拼接，能得到很好的效果，但因其破坏了路径的完整性使某些指标无法直接评估。例如，如果环境中存在一些危险区，UUV在保证安全概率的前提下可以选择通过某些危险区，但使用分段规划拼接的方法就很难控制各段对危险区的处理，从而难以使整体的拼接结果最优。而本文提出的算法则可很容易地将这类整体性能要求纳入规划指标中，并使其在循环迭代中得到优化。本发明中可变子目标点蚁群算法在可视图基础上进行规划，具体步骤如下：

1)传入布放点、回收点，按遍历顺序存储必经点的数组list[]；设置蚁群种群数量m_AntNum、最大迭代次数Max_generation、历史最优保留次数Max_histBest、信息素挥发系数vol、信息素上限up_limit信息素下限low_limit。

2)初始化信息素矩阵、历史最优蚂蚁HistoryBestAnt，历史最优蚂蚁已保存次数history_best_hold＝0、已循环代数m_generation＝0。

3)设置k＝0,转到4)。

4)如果k>m_AntNum，转到8)；否则获取必经点链表的长度listnum，设置t＝0,转到5)。

5)如果t<listnum，设置子目标点subAimPoint为必经点list[t]转到6)；如果t＝listnum设置子目标点subAimPoint为回收点。

6)设此时蚂蚁所在点为g_i，若g_i为subAimPoint，且subAimPoint为必达区，蚂蚁已找到该子目标，t＝t+1，转到5)；若g_i为subAimPoint，且subAimPoint为回收点，蚂蚁完成整个路径搜索，k＝k+1，转到4)；否则转到7)。

7)若g_i对应的可视点集合allow_i为空，蚂蚁死亡，k＝k+1，转到4)；若g_i对应的可视点集合allow_i不为空，利用轮盘赌选出转移点g_j∈allow_i，并从g_j的可视点集合allow_j中删除点g_i(防止蚂蚁向回搜索)，蚂蚁转移到点g_j，转到6)。

8)m_generation＝m_generation+1；若本次迭代中，蚂蚁走过的路径比历史最优蚂蚁 HistoryBestAnt更优，更新HistoryBestAnt，history_best_hold＝0；否则 history_best_hold＝history_best_hold+1；

9)如果m_generation>Max_generation或history_best_hold>Max_histBest，算法结束；否则更新信息素，转到3)

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

对本发明的效果的验证：

图3是利用本发明中提出的算法对图1中的情景进行的规划，可以看到此时的遍历顺序与传统TSP算法给出的遍历顺序是不同的，但显然图3所示的规划结果是合理且优化的。图4 中设置了一个与文献[2]类似的仿真环境，可以看到规划效果是优化的，且算法耗时仅用了 1156(ms)，而文献[2]中的算法消耗了8.857s。

Claims

1.一种障碍环境下有必经点的UUV航路规划方法，UUV所处障碍环境采用几何环境模型建模，环境中的障碍是用点和线组成的封闭几何图形来描述的；

其特征在于：所述UUV航路规划方法的实现过程为：

步骤一、计算在障碍环境下UUV布放点和回收点以及所有必经点的两两间航路估计距离，其具体过程为：

设A、B为布放点、回收点和所有必经点中的任意两点，若直线段AB不与障碍相交时，A、B的航路估计距离即为直线段AB的长度；

若直线段AB至少与一个障碍相交时，

分别求得直线段AB两侧障碍顶点到该直线段AB距离最远的两个障碍顶点，从两个障碍顶点中选取与直线段AB距离较近的障碍顶点作为估算顶点，然后再求估算顶点分别与A、B两点的直线距离之和，即获得A、B的航路估计距离；

步骤二、以布放点为起点、回收点为终点，使用TSP算法规划出UUV所有必经点的遍历顺序，使UUV从布放点出发、遍历完所有必经点后回到回收点的航路估计距离总和最短；

步骤三、利用可变子目标点蚁群算法，经优化得出一条从布放点出发经过所有必经点最终到达回收点且能避开所有障碍的航路，具体过程为：UUV从布放点出发、按照步骤二中得到的所有必经点的遍历顺序，使各个必经点依次作为蚁群算法中的目标点进行避障规划，最后再将回收点作为蚁群算法中的最后一个目标点，使得蚁群算法中的目标点是变化的。