一种基于改进蚁群算法的自主水下航行器路径规划方法与流程

本发明涉及自主水下航行器智能控制技术领域，尤其涉及一种基于改进蚂蚁算法的自主水下航行器路径规划方法。

背景技术：

自主水下航行器作为一种高新技术手段在海洋开发和利用领域发挥着极其重要的作用。它可以代替人类完成复杂海洋环境的探测，减少了普通载人检测船对系统的投入，提高了安全性能，提高投资的经济效益，实现智能化、自动化、低成本海洋环境探测、资源调查和开发。因而，无论在民用还是军事方面都有广泛的应用，产业化前景非常广阔。

随着水下机器人在民用海洋开发和军事领域的应用越来越广泛，人们对自主水下航行器的智能化程度要求也在提高，期望自主水下航行器在运动中能够感知周围环境，并且根据周围环境状况适时调整自身运动状态，能够规划出避碰的运动路径，这即为路径规划问题。路径规划的基本任务是解决自主水下航行器如何有效地利用环境信息，按照一定的评价标准，在具有障碍物的水下环境中能够快速响应变化的环境，并使用所选择的最小能量或时间成本，寻找一条安全将自主水下航行器从给定的起始点到目标点的最优或次优运动路径。

自主水下航行器不能通过gps进行定位，并且ins的误差累积会将导航精度降低到不可接受的程度。因此路径规划的重要性就显现出来了，是自主水下航行器的关键技术之一，在一定程度上它标志着水下机器人智能水平的高低，是自主水下航行器实现自主航行的重要环节。其任务主要是从起始点到目标位置点选择一条最短无碰撞的路径，目前路径规划方法有很多种，如人工势场法，图搜索法等传统算法，以及如蚁群算法、神将网络和遗传算法等的智能控制理论算法。蚁群算法有着较强的优化搜索性能，因为它结合了正反馈机制、分布式计算和贪婪式搜索的功能，同时结合性能好。但蚁群算法也同样存在着自身不足的地方，如耗时长和容易陷入局部最优解(即停滞现象)等缺点。

技术实现要素：

发明目的：针对传统蚁群算法的不足，本发明提出一种改进的蚁群算法的路径规划方法，来最大可能的获得路径总长度、平滑性和安全性三个性能指标同时优化的路径规划结果。提高自主水下航行器实际应用中的作业的效率和准确性。

技术方案：一种基于改进蚁群算法的自主水下航行器路径规划方法，包括以下步骤：

(1)构建环境模型，初始化各参数，输入基础数据，禁忌表tabuk初始化，设置允许列表allowedk，确定起点和目标点在环境模型中的位置；

(2)对于任意一只蚂蚁k以当前节点i为中心，按照转移规则选择走到下一个节点j；

(3)按局部信息素更新规则式τijk＝(1-μ)τijk+μ·τ0，更新信息素；

(4)判断所有蚂蚁是否完成一次路径构建，否则，转到“步骤(2)”；

(5)按公式τijk＝(1-ρ)τijk+ρδτijk进行全局信息素更新，判断算法是否满足停止条件，若满足则输出最优结果，否则，转到“步骤(2)”。

进一步地，步骤(1)中所述的构建环境模型是自主水下航行器工作的三维海域环境模型，跨度为21km×21km，取该区域内最深点的高度为0，其他点高度根据和最深点的高度差依次取得。根据三维空间抽象建模内容，把规划空间抽象为21km×21km×21km的规划空间，其中，x轴、y轴方向每个节点的间距为1km，z轴方向每个节点的间距为200m。

进一步地，步骤(2)中采用改进的转移规则，使其具有一定的倾向性，同时添加深度信息作为新的启发函数hj(t)，启发因子为λ，使得深度上的选择上也有方向性，一定程度上弥补了深度信息缺失的问题。

进一步地，步骤(3)中的局部信息素更新是指当蚂蚁经过该点时，该点的信息素就减少，其目的是增加蚂蚁搜索未经过点的概率。三维空间路径规划问题的环境模型构造图中的节点较多，考虑算法的空间复杂度，将信息素的储存落在环境模型的离散点上来降低信息存储空间。

进一步地，重复步骤(2)、(3)直至m只蚂蚁都到达重点end，完成一次路径构建。

进一步地，步骤(5)中的全局信息素更新规则使信息素增量δτijk也受到挥发系数ρ的影响。在ρ的作用下，路径上的信息素增量在一定合理范围内，使算法不至于收敛过快，增加算法搜索的随机性，使蚂蚁能够在全局范围内找到最优的路径。

本发明的有益效果是：在蚁群算法应用于自主水下航行器三维路径规划过程中主要做了如下改进，把信息素释放在节点上而不是在节点边上，节省存储空间，降低运算量；算法的启发式函数设计添加新型启发因子，将深度信息作为新的启发函数，提高了算法的收敛性；信息素的更新策略采用全局信息素和局部信息素相结合的更新方式，能较好的克服基本蚁群算法计算时间长、易出现停滞等缺陷。

附图说明

图1是本发明实例中路径规划方法的流程图；

图2是本发明实例中海底环境的三维地图模型；

图3是本发明实例中经典蚁群算法的自主水下航行器路径规划的最优解示例图

图4是本发明实例中经典蚁群算法的自主水下航行器路径规划的最佳个体适应度变化趋势图

图5是本发明实例中改进后蚁群算法的自主水下航行器路径规划的最优解示例图

图6是本发明实例中改进后蚁群算法的自主水下航行器路径规划的最佳个体适应度变化趋势图

具体实施方式

为使本发明实施的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行更加详细的描述。

具体而言，如图1所述，基于改进蚁群算法的自主水下航行器路径规划方法的步骤具体包括：

步骤1，构建环境模型，初始化各参数，输入基础数据，禁忌表tabuk初始化，设置允许列表allowedk，确定起点和目标点在环境模型中的位置。

步骤2，对于任意一只蚂蚁k以当前节点i为中心，按照转移规则选择走到下一个节点j。

步骤3，按局部信息素更新规则式τijk＝(1-μ)τijk+μ·τ0，更新信息素。

步骤4，判断所有蚂蚁是否完成一次路径构建，否则，转到“步骤2”。

步骤5，按公式τijk＝(1-ρ)τijk+ρδτijk进行全局信息素更新，判断算法是否满足停止条件，若满足则输出最优结果，否则，转到“步骤2”。

具体而言，在步骤1中，对跨度为21km×21km的海域地形在matlab上用栅格法进行三维空间抽象建模具体步骤包括；

首先在三维地图中选取一个顶点作为三维空间的坐标原点a，在a点建立三维坐标系，x轴沿经度增加方向，y轴沿纬度增加方向，z轴垂直于海平面；

沿x轴、y轴和z轴各方向取三维地图的最大长度构成一个包含三维地图的立方体区域；

采用等分空间的方法从三维空间中抽取出三维路径规划所需的网格点，将整个规划空间离散为一个三维点集合。

在所选取的海域内取最深点的高度为0，其他点高度根据和最深点的高度差依次取得。得到一个可建模地图信息矩阵。

参见图2，展示了在matlab上用栅格法进行环境建模的结果。

具体而言，初始化各参数，手动给这些参数一个常数值，如：蚂蚁数量m＝20，算法迭代次数nc＝100，表示路径上留存的信息素在蚂蚁选择路径对其反馈作用强度的参数α＝1，表示搜索的路径长度对蚂蚁选择路径的影响程度的参数β＝1，信息素挥发系数ρ＝0.2，使蚁群初始化在起始点处。

步骤2，每只蚂蚁从起始点开始遍历搜索，任意一只蚂蚁k以当前节点i为中心，按照转移规则选择走到下一个节点j，其转移概率为：

其中，allowedk＝{0,1…,n-1}-tabuk表示蚂蚁k下一步允许选择的城市。tabuk(k＝1,2…,m)用以记录蚂蚁k当前走过的城市，集合tabuk随着进化过程做动态调整，τj为点j上存储的信息素量；ηj能见度启发函数，其对保证蚁群算法在合理的时间内搜索到全局最优解非常重要，其定义为：

作为改进的启发函数ηj(t)，由待选节点与目标节点的距离决定，同样函数定义为距离的倒数。

其中新引进的深度启发函数hj(t)，启发因子为λ，考虑到三维环境中，自主水下航行器并不总是绕开障碍物，可以通过跨越障碍物来躲避障碍，将待选节点与目标点的深度差作为影响启发函数的因子，我们将距目标点深度越近的节点作为优先选择，使得深度的选择上也有方向性。由于深度差可能出现为0的情况，所以添加深度强度信息，记为w，取w＝1。由此，假设蚂蚁处于第i个栅格，待选栅格高度标记为hj，目标栅格高度记为hend，则深度启发函数定义为：

启发因子λ越大说明深度对于路径选择影响越大，栅格之间的距离是确定的，所以蚂蚁在局部区域进行选择时会优先选择深度差小的栅格，有利于提高算法的收敛速度，增加搜索到最佳路径的概率。

为了增加蚂蚁选择的随机性，减小搜索陷入局部最优点的可能性，在能见度启发函数和深度启发函数共同作用得到转移概率后，根据轮盘赌的方法，选择下一个移动栅格。

步骤3，蚂蚁在选定一个路径点后，都将立刻调用局部信息素更新规则τijk＝(1-μ)τijk+μ·τ0来更新路径点上的信息素，其中μ为0，1之间的一参数，选取μ＝0.2，τ0为各可行点信息素的初值。

步骤4，重复步骤2、3直至m只蚂蚁都到达重点end，完成一次路径构建。

步骤5，当蚂蚁完成一条路径的搜索时，以该路径的长度作为评价值，从路径集合中选择出最短路径，增加最短路径各点的信息素值。全局信息素更新规则为：

τijk＝(1-ρ)τijk+ρδτijk(6)

其中，lk为蚂蚁完成一次路径搜索后走过的总长度，k为信息素强度，改进后的信息素更新规则使信息素增量δτijk也受到挥发系数ρ的影响。根据更新规则可知，只有那些属于全局最优路径的弧段上的信息素才得到增强。在ρ的作用下，路径上的信息素增量在一定合理范围内，使蚂蚁的搜索行为集中到最优路径附件，从而提高了算法搜索的效率。

判断是否达到最大迭代次数nc>ncmax，若没有则转至步骤2，否则，终止迭代，输出最优解，即为起点到终点的最优路径路线图。

图3是本发明实例中经典蚁群算法的自主水下航行器路径规划的最优解示例图，图5是本发明实例中改进后蚁群算法的自主水下航行器路径规划的最优解示例图。结合这两个最优路线图可以看出，图5相对图2最优路径明显缩短的了一定的距离，同时转弯次数也明显减少，路径的平滑性得到了优化，提高了自主水下航行器的作业效率，弥补传统蚁群算法存在的不足。

图4是本发明实例中经典蚁群算法的自主水下航行器路径规划的最佳个体适应度变化趋势图，图6是本发明实例中改进后蚁群算法的自主水下航行器路径规划的最佳个体适应度变化趋势图，通过对比不难发现，改进后的蚁群算法只要迭代到5次左右就能收敛到最优解了，而且梯度平稳且迅速趋于稳定，表明改进后的蚁群算法可以少走很多弯路，提高了算法的收敛性。