本发明涉及路径规划方法
技术领域：
：，具体的说是一种考虑到均匀建模方案对复杂环境建模的不足，以非均匀栅格模型为基础对基本蚁群算法进行改进，能够得到更符合复杂环境中水下航行器需要的路径的基于非均匀建模的改进蚁群路径规划算法。
背景技术：
：：水下航行器路径规划是指在给定出发点和目标点之后，在水下环境中规划得到一条安全无碰撞且满足优化目标的路径。路径规划算法往往分成3步，第一步是环境建模，第二步是基于环境模型利用算法规划得到路径，第三步是路径优化。在以往研究中，环境建模往往采用均匀划分建模方案，例如栅格法、极坐标建模法等，这些建模方案在划分搜索空间过程中并没有考虑障碍物之间的分布信息，导致搜索算法基于这些建模方案得到的环境模型进行路径规划时存在不必要的计算量。当规划空间是简单环境时，基于这些方案的算法能求解得到解，但当规划空间为复杂环境时，基于均匀建模方案的算法效率低下甚至得不到解。由于水下航行器运行在水下空间，海流对auv的运行产生影响，因此对于水下航行器路径规划必须考虑海流的影响。流场信息可以通过远程观测获得，例如可以通过高频雷达(hfradar)测量和卫星观测得知，也可以通过系泊实地测量或数值预报模型获得。数值预报模型的一个例子是区域海洋模式系统(regionaloceanmodelsystem–roms)，它是一个开源的海洋模型程序，在整个海洋学和模拟社区得到广泛接受和支持。roms原本被用来研究美国西部沿海的海洋变化过程，分辨率从1公里到15公里不等。roms除了可以得到特定区域的流场之外，还可以利用解析方程模拟流场。bluelink是另外一个流场预测模型，该模型可以生成澳大利亚周边海岸和大陆架地区未来七天的高分辨率的流场预测。bluelink的使用主要针对大范围地区，例如数公里范围内的大型地区。因为粗略的流场地图对于数百米范围内的短程操作几乎没有用处，如果需要实际应用，需要插值来生成更高分辨率的海流地图。水下航行器工作在湍流、杂乱的海洋环境中，海流对水下航行器的运动产生影响，顺流可以减少水下航行器的能量消耗，逆流会增大水下航行器的能量消耗，在以往关于水下航行器路径规划的研究中很多文章并没有考虑海流的影响，现阶段电池技术还未取得突破性进展，水下航行器的路径能耗是一个值得考虑的因素。路径规划问题是一个典型的np(non-deterministicpolynomialproblems)问题。近些年，进化算法已经被广泛应用到解决np问题，并且进化算法已被证明是一种处理非确定性多项式时间难题的高效且有效的方法。蚁群算法是一种典型的进化算法，蚁群算法已经被广泛应用到路径规划中。在复杂环境中，如u型障碍物，环形障碍物等(如图7所示)，蚂蚁在搜索路径时容易陷入死锁状态。所谓的死锁状态是指：蚂蚁到达某栅格之后，下一步无可选择的栅格，该栅格的邻域栅格要么是位于已走过的路径点之中，要么是障碍物栅格。蚂蚁在搜索前期，由于搜索的盲目性，容易掉入死锁。如图8左图所示，蚂蚁在经过了栅格1、2、3、4到达了栅格5之后，下一步无可走栅格，陷入了死锁，其中灰色栅格为障碍物栅格。如图8右图所示，在非均匀栅格环境中，蚂蚁在经过栅格1、2到达栅格3之后，陷入死锁。在以往研究中针对蚂蚁死锁，提出了很多解决方案。温如春提出在环境建模时，将u型障碍物进行填充，这种做法以牺牲对环境表达的完整性来避免蚂蚁陷入死锁，对最优解的求解造成影响不值得借鉴。wang将陷入死锁的蚂蚁认定为“蚂蚁死亡”，并且停止对其走过的路径节点之间的信息素进行更新，这种做法减少了解的多样性，并不利于寻找最优解。屈鸿采取让陷入死锁的蚂蚁回退到上一个节点，对导致上一个栅格跟死锁栅格之间的信息素进行惩罚，并在上一个节点重新选择路径节点。这种做法减少了在上一个栅格中选择死锁栅格的概率，但是如果可选栅格只有1个时(如图8所示)，该做法并不能逃离死锁状态。技术实现要素：本发明针对现有技术中存在的缺点和不足，提出了一种能够得到更符合复杂环境中水下航行器需要的路径的基于非均匀建模的改进蚁群路径规划算法。本发明可以通过以下措施达到：一种基于非均匀建模的改进蚁群路径规划算法，其特征在于包括以下步骤：步骤一：利用四叉树来划分搜索空间，得到非均匀栅格模型，利用python实现用四叉树划分搜索空间算法，具体步骤为：步骤1-1：将整个空间作为一个四叉树对象存入堆中，步骤1-2：如果当前迭代次数(i)小于最大迭代次数(iterations)，则进行步骤1-3，否则转到步骤1-5，步骤1-3：将堆中的根元素弹出，步骤1-4：将根元素在x,y方向进行等分，得到4个子元素，将4个子元素存入堆中，步骤1-5：递归调用四叉树对象得到子节点的方法，步骤1-6：储存四叉树对象数据；步骤二：确定栅格的邻域栅格，在步骤一的四叉树建模的过程中，储存得到了四叉树对象二维坐标信息，分别是y轴上下限ymax、ymin，x轴上下限xmax、xmin，从栅格的上、下、左、右4个方向进行分析：栅格的上方可能存在的邻域栅格：邻域栅格的ymin等于栅格的ymax，然后满足以下条件之一：邻域栅格的xmin等于栅格的xmax或者是邻域栅格的xmax等于栅格的xmin，或者是邻域栅格的xmin小于栅格的xmin同时邻域栅格的xmax大于栅格的xmin，或者是是邻域栅格的xmin大于栅格的xmin同时邻域栅格的xmax小于栅格的xmin，栅格的下方可能存在的邻域栅格情况与之类似；栅格的左方可能存在的邻域栅格：邻域栅格的xmax等于栅格的xmin，然后满足以下条件之一：邻域栅格的ymin等于栅格的ymax或者是邻域栅格的ymax等于栅格的ymin，或者是邻域栅格的ymin小于栅格的ymin同时邻域栅格的ymax大于栅格的ymin，或者是是邻域栅格的ymin大于栅格的ymin同时邻域栅格的ymax小于栅格的ymin，栅格的上方可能存在的邻域栅格情况与之类似；步骤三：采用garau提出的粘性lambvortices模型来模拟流场，流场的速度用vc＝(u,v)来表示，其中：r＝(x,y)代表2d空间，r0＝(x0,y0)是涡旋的中心，τ和γ是参数控制旋涡的半径和强度；然后设计能够综合评价路径质量的目标函数，考虑到水下航行器运动在湍流、杂乱的海洋环境中，在规划路径时应该综合考虑该路径的能耗、路径的威胁度、转弯次数，公式如下：fp＝(μ1.ep-1+μ2.rp-1+μ3.wp-1)·cp(1-3)其中μ1、μ2、μ3、cp为常数，其中μ1、μ2、μ3相加和为1，分别表示各项的重要程度；ep表示该路径的能耗大小、rp表示该路径的危险度大小、wp表示该路径的转折次数；rp的计算如公式1-4所示，其大小为路径中各栅格节点中心坐标距离障碍物最近的距离之和的平均数的倒数；能耗公式是将规划得到的路径分割成m条，利用以下公式分别计算每一条的能耗，然后叠加起来得到最终的能耗消耗；其中确定vi为计算得到每一段路径能耗的关键步骤；其中wi为auv沿着某一小段路径前进所需克服海流阻力的能量消耗，ep为m段路径耗能叠加最终得到的路径耗能，ρ为跟所处海域海流有关的常数，c为跟水下航行器尺寸有关的常数，vi为auv相对于海流的速度，可以通过(公式1-8)跟(公式1-9)计算得知，其中vc为海流的速度，其速度和大小我们可以通过流场模拟公式得出，va为auv最终沿着xixi-1前进的速度，本发明设定va大小不变，方向为该段路径的前进方向，ea为该段路径的方向向量，通过(公式1-10)可以获得的大φa小，然后结合(公式1-8)、(公式1-9)可以确定vi大小，然后通过(公式1-5)、(公式1-6)得到wi，最终确定能耗ep；步骤四：改进蚁群全局路径规划算法，采用改进的蚁群算法进行路径规划，以非均匀栅格模型为基础，以目标函数为优化目标，对基本蚁群算法进行改进，具体包括以下内容：改进路径点选择策略，改进信息素更新方法，解决路径死锁，其中所述改进路径点选择策略包括：(1)采取合理的启发式距离，借鉴a*算法的启发式距离设计，定义启发式距离为当前栅格current中心坐标到待选择栅格n中心坐标之间的距离dcn与待选择栅格n中心坐标到目标点target之间的距离dnt之和，改进的启发式信息定义为：(2)在进行路径点选择时，考虑待选择栅格与障碍物之间距离，针对四叉树分割搜索空间得到非均匀栅格模型对基本蚁群算法进行改进，引入安全距离这一概念，并在路径点选择时考虑待选择栅格与障碍物之间的距离，设计了新的路径点选择策略，式中，dn表示栅格n与其最近障碍物之间的距离，计算公式(1-15)所示，d0表示所有栅格中距离障碍物最远的距离长度，ak(c)表示蚂蚁k在当前栅格c的邻域栅格中可以选择的栅格集合；如果待选择栅格n与其最近障碍物的距离dn小于安全距离dsafe，在进行路径点选择的时候考虑栅格与障碍物之间的距离，在路径点选择策略中加入dn/d0，在dn/d0中，dn越大，选择离障碍物远的栅格的几率也就越大，避免选择的栅格离障碍物太近，注意该项始终小于1；如果待选择栅格n与其最近障碍物的距离dn大于安全距离dsafe，进行路径点选择的时候不考虑当前栅格与障碍物之间的距离。本发明所述改进信息素更新方法采用“ant-cycle”的信息素更新方式，即在蚂蚁到达目标点之后更新经过路径上的局部信息素，当种群中的全部蚂蚁完成路径搜索之后，采用mmas的全局信息素更新策略，限制信息素大小。本发明所述解决路径死锁通过采用蚂蚁回退策略来解决路径死锁问题，具体步骤为：如果蚂蚁到达某栅格之后，经过判断得知该蚂蚁陷入死锁，周围无可走栅格，首先将当前栅格从已走过的路径节点(pathnode)中删除，并将该栅格加入到本次蚂蚁回退过程的禁忌表(tabootable)中，同时将蚂蚁回退到上一个栅格(previousnode)，如果不把该栅格加入到禁忌表中，根据路径点选择策略或者该栅格为上一个栅格的邻域栅格中唯一可选栅格，那么蚂蚁下一次仍然会选择该栅格作为下一个经过的栅格，那么依旧陷入死锁状态。重复以上过程，直到蚂蚁摆脱死锁状态，即蚂蚁的邻域栅格中除了已走过的路径节点(pathnode)、障碍物栅格、本次蚂蚁回退过程的“禁忌表”中的栅格以外，存在其他栅格可以访问，当蚂蚁结束“蚂蚁回退”过程，不再是死锁状态之后，禁忌表不再发挥作用，禁忌表的作用范围只为此次回退过程中，不影响蚂蚁跳出死锁之后，在进行路径点选择时，对于其中的栅格的选择；同时，采用“ant-cycle”的信息素更新方式，即等蚂蚁到达目标点之后再更新局部信息素，因此在蚂蚁回退过程中，信息素不更新，所以不会由于该过程导致信息素的变化。蚂蚁搜索得到最终路径中并不包括“陷入死锁”，然后“蚂蚁回退”离开死锁的过程中经过的栅格节点，通过对信息素的更新，可以避免下一次陷入死锁。本发明采用改进的蚁群算法规划得到路径节点，然后利用b样条拟合得到光滑路径，改进蚁群算法分2层循环，第一层循环是遍历蚂蚁种群中的蚂蚁，保证种群中的蚂蚁都到达目标点，在读取海洋环境模型之后，蚂蚁进行路径点选择，到达下一节点之后判断路径是否陷入死锁，如果陷入死锁就采用蚂蚁回退策略来摆脱死锁，如果没有陷入死锁则判断是否到达终点，如果没有到达终点，则重复以上过程；当蚂蚁到达终点之后，按照“ant-cycle”的信息素更新方式，更新走过路径上的局部信息素，此时判断蚂蚁个数是否大于等于蚂蚁种群个数，如果没有，则采用新的蚂蚁重复以上过程；如果大于，说明当前蚂蚁种群已经全部到达了终点，则进入下一层循环。本发明考虑到均匀建模方案对复杂环境建模的不足，提出利用四叉树建模得到非均匀栅格模型，提出了基于非均匀栅格模型的改进蚁群路径规划算法。以非均匀栅格模型为基础对基本蚁群算法进行改进，在路径点选择时考虑节点与障碍物之间的距离，在进行路径点选择时考虑待选择节点与障碍物之间的距离，采用“ant-cycle”的信息素更新方式，在全局信息素更新时限制信息素大小。仿真实验表明非均匀栅格模型相比较均匀栅格模型的优越性，改进蚁群算法相比较其他几种基本蚁群算法收敛速度更快，能够得到更符合复杂环境中水下航行器需要的路径。附图说明：附图1为本发明中四叉树划分搜索空间的示意图。附图2为本发明中四叉树划分搜索空间流程图。附图3为本发明中邻域栅格对比示意图。附图4是本发明中利用公式拟合的流场局部放大图。附图5是本发明中速度矢量合成示意图。附图6是本发明中路径点选择示意图。附图7是本发明中障碍物示意图。附图8是本发明中死锁示意图。附图9是本发明中改进蚁群算法流程图。附图10是本发明中改进蚁群算法在非均匀模型下规划的路径与均匀模型对应的路径对比图。附图11是本发明中acs算法在非均匀栅格模型下规划得到路径。附图12是本发明中mmas算法在非均匀栅格模型下规划得到路径。附图13是本发明中eac10算法在非均匀栅格模型下规划得到路径。附图14是本发明中eac30算法在非均匀栅格模型下规划得到路径。附图15是本发明中改进算法与其他蚁群算法适应度随进化代数变化图。附图16是本发明改进蚁群算法与a*在非均匀栅格模型下规划得到路径对比图。附图17是本发明中光滑前路径和光滑后路径对比。具体实施方式：下面结合附图和实施例对本发明作进一步的说明。在进行水下航行器路径规划之前，首先得对海洋环境建模，路径规划算法在环境模型的基础上进行路径规划。将海洋环境建模分成2部分，一部分是利用四叉树划分搜索空间得到非均匀栅格模型，以非均匀栅格模型表示2d水下空间。另一部分是对海洋环境中海流的模拟，即流场的模拟。本发明针对复杂环境采用均匀方案建模的不足，提出利用四叉树来划分搜索空间得到非均匀栅格模型，接下来介绍非均匀建模过程，包含3部分，分别是海图读取、四叉树划分搜索空间、节点连接关系确定。(1)海图读取通过python读取电子海图，利用python的image库以及imagedraw库操作读取进来的图片，将其转换成为图像栅格矩阵，矩阵中障碍物区域o(x,y)值为1，非障碍物区域o(x,y)值为0，将电子海图转换成栅格矩阵之后便于后续算法操作。(2)四叉树划分搜索空间利用四叉树来划分搜索空间，首先介绍四叉树的基本概念，然后介绍如何利用四叉树划分搜索空间。(a)四叉树基本概念四叉树是一种常见的数据结构，四叉树的种类很多，有点四叉树(pointquadtree)、线性四叉树、pr四叉树(pointregionquadtree)等。本发明采取线性四叉树对水下空间进行非均匀划分。图1为四叉树划分搜索空间的示意图。四叉树划分搜索空间的过程是指对空间进行2维的等分，得到2m×2n个基本元素。每个元素的长、宽不小于最小尺寸rmin(rmin的大小由人为设定)。每个元素属于自由区域、障碍区域、包含障碍的区域这3种的一种。如果该元素为包含障碍区域，则递归的将环境进行2维等分，分成4个大小相等的子区域，直到每个区域中包含的基本元素属于自由区域、障碍区域这2种中的一种。(b)四叉树划分搜索空间步骤利用python实现用四叉树划分搜索空间算法，算法中用到了优先级队列这个数据结构，python的headq模块提供了对堆、优先级队列操作的支持。图2为利用四叉树划分搜索空间得到非均匀栅格模型的流程图，该算法主要分成了以下几个步骤：步骤1：将整个空间作为一个四叉树对象存入堆中；步骤2：如果当前迭代次数(i)小于最大迭代次数(iterations)，则进行步骤3，否则转到步骤5；步骤3：将堆中的根元素弹出；步骤4：将根元素在x,y方向进行等分，得到4个子元素，将4个子元素存入堆中；步骤5：递归调用四叉树对象得到子节点的方法；步骤6：储存四叉树对象数据。(3)确定栅格的邻域栅格非均匀栅格模型中不包括栅格之间的连接关系，每一个栅格不清楚其周围是哪几个栅格与其相邻，将这些相邻栅格称为邻域栅格。本发明利用蚁群算法进行路径规划，蚂蚁在进行路径点选择时依赖这种连接关系。因此在路径规划之前，得先确定栅格的邻域栅格。下图为非均匀栅格模型的邻域栅格和均匀栅格模型的邻域栅格对比图，其中红色栅格表示需要确定邻域连接关系的栅格，灰色栅格为红色栅格的邻域栅格。从图3中可以看出，均匀栅格模型的邻域栅格个数固定、方向固定，为左、左上、左下、右、右上、右下6种情况。非均匀栅格模型的邻域栅格存在的情况更多、更复杂。在四叉树建模的过程中，我们储存得到了四叉树对象二维坐标信息，分别是y轴上下限ymax、ymin，x轴上下限xmax、xmin。我们从栅格的上、下、左、右4个方向进行分析。栅格的上方可能存在的邻域栅格：邻域栅格的ymin等于栅格的ymax，然后满足以下条件之一：邻域栅格的xmin等于栅格的xmax或者是邻域栅格的xmax等于栅格的xmin，或者是邻域栅格的xmin小于栅格的xmin同时邻域栅格的xmax大于栅格的xmin，或者是是邻域栅格的xmin大于栅格的xmin同时邻域栅格的xmax小于栅格的xmin。栅格的下方可能存在的邻域栅格情况与之类似。栅格的左方可能存在的邻域栅格：邻域栅格的xmax等于栅格的xmin，然后满足以下条件之一：邻域栅格的ymin等于栅格的ymax或者是邻域栅格的ymax等于栅格的ymin，或者是邻域栅格的ymin小于栅格的ymin同时邻域栅格的ymax大于栅格的ymin，或者是是邻域栅格的ymin大于栅格的ymin同时邻域栅格的ymax小于栅格的ymin。栅格的上方可能存在的邻域栅格情况与之类似。流场模拟：由于水下航行器运行在水下空间，海流对auv的运行产生影响，因此对于水下航行器路径规划必须考虑海流的影响。流场信息可以通过远程观测获得，例如可以通过高频雷达(hfradar)测量和卫星观测得知，也可以通过系泊实地测量或数值预报模型获得。数值预报模型的一个例子是区域海洋模式系统(regionaloceanmodelsystem–roms)，它是一个开源的海洋模型程序，在整个海洋学和模拟社区得到广泛接受和支持。roms原本被用来研究美国西部沿海的海洋变化过程，分辨率从1公里到15公里不等。roms除了可以得到特定区域的流场之外，还可以利用解析方程模拟流场。bluelink是另外一个流场预测模型，该模型可以生成澳大利亚周边海岸和大陆架地区未来七天的高分辨率的流场预测。bluelink的使用主要针对大范围地区，例如数公里范围内的大型地区。因为粗略的流场地图对于数百米范围内的短程操作几乎没有用处，如果需要实际应用，需要插值来生成更高分辨率的海流地图。本发明采用garau提出的粘性lambvortices模型来模拟流场，该模型被多篇论文中采用。流场的速度可以用vc＝(u,v)来表示，其中：r＝(x,y)代表2d空间，r0＝(x0,y0)是涡旋的中心，τ和γ是参数控制旋涡的半径和强度。在实际应用场景中，lambvortices的数量和具体位置、半径和强度可以通过安装在auv前面的水平adcp(h-adcp)测量出来。图4是利用上述公式拟合的流场局部放大图，该流场包含120个中心坐标随机，半径(γ)为2，强度(τ)为25的lambvortices。在实际任务情形中，通过海图读取得到环境大小，根据实际环境大小构建流场。本发明是在障碍物信息已知的情况下，在障碍物复杂分布情况下的全局路径规划，具体描述是：设计合理的算法规划，在水下空间规划得到一条从出发点到目标点、符合避障要求并且满足优化条件的路径。在得到非均匀栅格模型之后，利用改进蚁群算法搜索路径关节点，然后利用b样条拟合得到光滑路径。在进行路径规划之前，首先设计能够综合评价路径质量的目标函数，目的是规划得到光滑、低耗能、安全性高和可执行性高的优化路径。水下航行器工作在湍流、杂乱的海洋环境中，海流对水下航行器的运动产生影响，顺流可以减少水下航行器的能量消耗，逆流会增大水下航行器的能量消耗，在以往关于水下航行器路径规划的研究中很多文章并没有考虑海流的影响，本发明考虑海流对水下航行器前进的能耗影响，使规划出来的路径尽可能利用海流“顺流”效果来减少能量消耗。因为现阶段电池技术还未取得突破性进展，水下航行器的路径能耗是一个值得考虑的因素。一条有效的路径除了需要满足避碰条件以外，还必须是安全可执行的，这就要求规划得到路径需要与障碍物保持一定距离。一条好的路径同样也得考虑路径的拐弯次数，拐弯会增加航行器的机械损耗。适应度函数用于评价路径的好坏，考虑到水下航行器运动在湍流、杂乱的海洋环境中，在规划路径时应该综合考虑该路径的能耗、路径的威胁度、转弯次数。公式如下：fp＝(μ1.ep-1+μ2.rp-1+μ3.wp-1)·cp(1-3)其中μ1、μ2、μ3、cp为常数，其中μ1、μ2、μ3相加和为1，分别表示各项的重要程度。ep表示该路径的能耗大小、rp表示该路径的危险度大小、wp表示该路径的转折次数。rp的计算如(公式1-4)所示，其大小为路径中各栅格节点中心坐标距离障碍物最近的距离之和的平均数的倒数。能耗公式采取alvarez等提出的算法。具体算法是将规划得到的路径分割成m条，利用以下公式分别计算每一条的能耗，然后叠加起来得到最终的能耗消耗。其中确定vi为计算得到每一段路径能耗的关键步骤。其中wi为auv沿着某一小段路径前进所需克服海流阻力的能量消耗，ep为m段路径耗能叠加最终得到的路径耗能，ρ为跟所处海域海流有关的常数，c为跟水下航行器尺寸有关的常数。vi为auv相对于海流的速度，可以通过(公式1-8)跟(公式1-9)计算得知。其中vc为海流的速度，其速度和大小我们可以通过流场模拟公式得出。va为auv最终沿着xixi-1前进的速度，本发明设定va大小不变，方向为该段路径的前进方向。ea为该段路径的方向向量。通过(公式1-10)可以获得的大φa小，然后结合(公式1-8)、(公式1-9)可以确定vi大小，然后通过(公式1-5)、(公式1-6)得到wi，最终确定能耗ep。图5为速度矢量合成示意图。路径规划问题是一个典型的np(non-deterministicpolynomialproblems)问题。近些年，进化算法已经被广泛应用到解决np问题，并且进化算法已被证明是一种处理非确定性多项式时间难题的高效且有效的方法。蚁群算法是一种典型的进化算法，蚁群算法已经被广泛应用到路径规划中。在得到海洋环境模型之后，采用改进的蚁群算法进行路径规划。以非均匀栅格模型为基础，以目标函数为优化目标，对基本蚁群算法进行改进。acs算法(antcolonysystem蚁群系统算法)、mmas算法(maxminantsystem最大最小蚁群算法)、采用精英策略的蚁群算法的路径点选择策略公式如下所示：其中：式中，τcn(t)表示信息素大小。代表启发式信息，是栅格n中心坐标到目标点target的直线距离的倒数。α、β分别表示信息素和启发式信息的重要程度，其中α、β＜0，ak(c)代表当前栅格c可以被蚂蚁k访问的邻域栅格的集合。q是是[0,1]区间内产生的随机数，q0为人为设定的在[0,1]区间内的一个常数。以上公式表示，当q＜q0时，选取启发式因素和信息素因素乘积中最大的那个栅格；如果q＞q0时，则采用轮盘赌的形式选择邻域中的某个栅格。将蚁群算法应用到非均匀栅格模型，对基本蚁群算法中的路径点选择策略进行改进，具体改进措施有：(1)采取合理的启发式距离借鉴a*算法的启发式距离设计，定义启发式距离为当前栅格current中心坐标到待选择栅格n中心坐标之间的距离dcn与待选择栅格n中心坐标到目标点target之间的距离dnt之和。相比较于acs、mmas、采用精英策略的蚁群算法采用启发式距离等于待选择栅格n中心坐标到目标点target的欧式距离，由于四叉树分割搜索空间得到非均匀模型的特点，当前栅格与其邻域中可选择栅格之间的距离大小可能差异特别大。因此对于非均匀模型栅格模型，启发式距离中考虑当前栅格current中心坐标到待选择栅格n中心坐标之间的距离dcn是合理的，能够更好的引导蚂蚁向目标点前进。因此改进的启发式信息定义为：如图6显示所示，黄色星星表示当前栅格栅格current，其中心坐标为(30,30)，绿色圆形、蓝色正方形表示当前栅格邻域栅格中的2个待选择栅格，中心坐标分别为(35,45)和(60,60)，红色六芒星表示目标点target，其坐标为(35,80)。如果按照原来的启发式距离，蓝色正方形距离目标位置长度32.01，绿色圆形距离目标位置长度35，有更大的可能性选择蓝色正方形表示的栅格作为下一个经过的栅格。但是实际上，从黄色星星到蓝色正方形，然后再从蓝色正方形到目标位置的总长度(30+32.01＝62.01)远大于先从黄色星星到绿色圆形，然后从绿色圆形到目标位置的总长度(15.81+35＝50.81)，使用新的启发式距离公式可以更加合理的表述节点与目标位置的位置，更好的引导算法进行搜索。(2)考虑待选择栅格与障碍物之间距离基本蚁群算法的路径点选择策略中并没有考虑待选择栅格与障碍物之间的距离。如果在auv控制不稳的情况下，路径与障碍物距离越近，则有更大的可能性与障碍物发生碰撞。因此在进行路径点选择时，应该考虑选择栅格与障碍物之间距离。在开阔区域进行路径点选择时，此时考虑所选择的节点与障碍物之间的距离意义不大，因为此时路径足够“安全”；如果在障碍物拥挤区域、狭窄通道中选择路径节点，则应该让所选择的路径节点远离障碍物以获得安全路径。针对四叉树分割搜索空间得到非均匀栅格模型对基本蚁群算法进行改进，引入安全距离这一概念，并在路径点选择时考虑待选择栅格与障碍物之间的距离，设计了新的路径点选择策略。式中，dn表示栅格n与其最近障碍物之间的距离，计算公式(1-15)所示。d0表示所有栅格中距离障碍物最远的距离长度。ak(c)表示蚂蚁k在当前栅格c的邻域栅格中可以选择的栅格集合。如果待选择栅格n与其最近障碍物的距离dn小于安全距离dsafe，在进行路径点选择的时候考虑栅格与障碍物之间的距离，在路径点选择策略中加入dn/d0。在dn/d0中，dn越大，选择离障碍物远的栅格的几率也就越大，避免选择的栅格离障碍物太近，注意该项始终小于1；如果待选择栅格n与其最近障碍物的距离dn大于安全距离dsafe，进行路径点选择的时候不考虑当前栅格与障碍物之间的距离。主要思想是如果待选择栅格与障碍物距离较远，表明此时蚂蚁运动到开阔区域，由于四叉树划分得到的非均匀栅格环境的特点，开阔区域的栅格尺寸也比较大，此时主要利用改进的启发式距离引导蚂蚁快速穿过“开阔”区域/无障碍物区域；如果待选择栅格n与障碍物距离小于设定的dsafe，表明路径点此时离障碍物很近，需要考虑路径的安全性问题。在进行路径点选择时考虑待选择栅格与障碍物之间的距离，保证路径的安全性。本发明所述改进信息素更新方法如下：acs算法的信息素更新分为局部信息素更新和全局信息素更新。蚂蚁每行动一步，都对局部信息素进行更新。τt+1(c,n)＝(1-δ)·τt(c,n)+δ·τo(1-16)其中，τt(c,n)表示t时刻的栅格c与栅格n之间的信息素分布。τt+1(c,n)表示t+1时刻的栅格c与栅格n之间的信息素分布。δ表示局部信息素更新时的挥发系数，τo表示固定的信息素增量。mmas算法只进行全局信息素更新，当种群内所有蚂蚁完成路径搜索之后，更新最优蚂蚁走过的路径上的信息素。mmas算法将全局信息素更新后的信息素大小限制在区间[τmin,τmax]之中，但是acs算法的全局信息素更新并不限定更新后的信息素大小。其中：式中ρ为全局信息素更新时的挥发系数，0＜ρ＜1。pb为当代适应度值最大路径，lb为当代适应度值最大路径的长度，τmax、τmin分别表示信息素的最大、最小值。基于精英选择的蚁群算法是较早的一种对于蚁群算法性能的改进。其思想是在每次循环之后对所有已经发现的最优解给予额外的信息素量增强。其信息素更新方式是全局信息素更新，公式如下：τcn(t+1)＝(1-ρ)τcn(t)+δτ+δτcn*(1-20)其中ρ为信息素挥发系数，表示第k只蚂蚁在本次循环中留在栅格c与栅格n之间的信息素浓度，q是表示信息素浓度的常数，pk表示蚂蚁k在本次循环中走过的路径点集合，σ代表精英蚂蚁的个数，le表示蚂蚁种群在本次循环中最优解对应的路径长度，lk表示蚂蚁k在本次循环中所走路径长度，pe表示蚂蚁种群在本次循环中最优解走过的路节点集合。本发明借鉴基本蚁群算法与mmas算法中的信息素大小限制，采用“ant-cycle”的信息素更新方式，即在蚂蚁到达目标点之后更新经过路径上的局部信息素。当种群中的全部蚂蚁完成路径搜索之后，采用mmas的全局信息素更新策略，限制信息素大小。本发明中路径死锁的解决方案如下：在复杂环境中，如u型障碍物，环形障碍物等(如图7所示)，蚂蚁在搜索路径时容易陷入死锁状态。所谓的死锁状态是指：蚂蚁到达某栅格之后，下一步无可选择的栅格，该栅格的邻域栅格要么是位于已走过的路径点之中，要么是障碍物栅格。蚂蚁在搜索前期，由于搜索的盲目性，容易掉入死锁。如图8左图所示，蚂蚁在经过了栅格1、2、3、4到达了栅格5之后，下一步无可走栅格，陷入了死锁，其中灰色栅格为障碍物栅格。如图8右图所示，在非均匀栅格环境中，蚂蚁在经过栅格1、2到达栅格3之后，陷入死锁。在以往研究中针对蚂蚁死锁，提出了很多解决方案。温如春提出在环境建模时，将u型障碍物进行填充，这种做法以牺牲对环境表达的完整性来避免蚂蚁陷入死锁，对最优解的求解造成影响不值得借鉴。wang将陷入死锁的蚂蚁认定为“蚂蚁死亡”，并且停止对其走过的路径节点之间的信息素进行更新，这种做法减少了解的多样性，并不利于寻找最优解。屈鸿采取让陷入死锁的蚂蚁回退到上一个节点，对导致上一个栅格跟死锁栅格之间的信息素进行惩罚，并在上一个节点重新选择路径节点。这种做法减少了在上一个栅格中选择死锁栅格的概率，但是如果可选栅格只有1个时(如图8所示)，该做法并不能逃离死锁状态。本发明提出蚂蚁回退策略来解决路径死锁问题，具体步骤为：如果蚂蚁到达某栅格之后，经过判断得知该蚂蚁陷入死锁，周围无可走栅格，首先将当前栅格从已走过的路径节点(pathnode)中删除，并将该栅格加入到本次蚂蚁回退过程的禁忌表(tabootable)中，同时将蚂蚁回退到上一个栅格(previousnode)。如果不把该栅格加入到禁忌表中，根据路径点选择策略或者该栅格为上一个栅格的邻域栅格中唯一可选栅格，那么蚂蚁下一次仍然会选择该栅格作为下一个经过的栅格，那么依旧陷入死锁状态。重复以上过程，直到蚂蚁摆脱死锁状态，即蚂蚁的邻域栅格中除了已走过的路径节点(pathnode)、障碍物栅格、本次蚂蚁回退过程的“禁忌表”中的栅格以外，存在其他栅格可以访问。当蚂蚁结束“蚂蚁回退”过程，不再是死锁状态之后，禁忌表不再发挥作用，禁忌表的作用范围只为此次回退过程中，不影响蚂蚁跳出死锁之后，在进行路径点选择时，对于其中的栅格的选择。同时，采用“ant-cycle”的信息素更新方式，即等蚂蚁到达目标点之后再更新局部信息素，因此在蚂蚁回退过程中，信息素不更新，所以不会由于该过程导致信息素的变化。蚂蚁搜索得到最终路径中并不包括“陷入死锁”，然后“蚂蚁回退”离开死锁的过程中经过的栅格节点，通过对信息素的更新，可以避免下一次陷入死锁。采用改进的蚁群算法规划得到路径节点，然后利用b样条拟合得到光滑路径。改进蚁群算法分2层循环，第一层循环是遍历蚂蚁种群中的蚂蚁，保证种群中的蚂蚁都到达目标点。在读取海洋环境模型之后，蚂蚁进行路径点选择，到达下一节点之后判断路径是否陷入死锁。如果陷入死锁就采用蚂蚁回退策略来摆脱死锁。如果没有陷入死锁则判断是否到达终点。如果没有到达终点，则重复以上过程。当蚂蚁到达终点之后，按照“ant-cycle”的信息素更新方式，更新走过路径上的局部信息素。此时判断蚂蚁个数是否大于等于蚂蚁种群个数，如果没有，则采用新的蚂蚁重复以上过程；如果大于，说明当前蚂蚁种群已经全部到达了终点，则进入下一层循环。在进入下一个循环之前更新全局信息素，通过适应度函数得到各蚂蚁对应路径的数据，判断算法是否陷入局部收敛。局部收敛是指在算法到达设定的最大循环次数之前，如果在连续的nk次迭代内，算法最优解无明显提升，则认为蚂蚁陷入了局部最优。本发明采用提高算法接受随机解的概率来解决局部最优。(公式1-24)为新的路径点选择策略。式中q表示随机数，qr是一个正数。allowed(n)表示当前栅格c的邻域栅格ak(c)中可以选择的栅格。rand表示随机函数。上述公式的意思是如果q＜qr就从当前可以选择的栅格中随机选一个作为下一个经过的栅格节点。否则采取原先的路径点选择策略。通过这个方案，可以增加算法接受随机解的几率，增强算法的搜索能力和种群的多样性，增大跳出局部收敛的概率。如果陷入局部收敛，则采用新的路径点选择策略。然后判断当前迭代次数i是否大于设定好的迭代次数，如果没有则重复以上过程。如果大于，保存路径数据，算法运行结束。图9为改进蚁群算法流程图。实施例：在操作系统为windows10，cpu为intel酷睿i5-5250u，主频1.6ghz的电脑上，以matlabr2014b为平台验证算法。算法中的参数取值如下:τ＝15，γ＝2，α＝2，β＝1，q0＝0.6，δ＝0.2，ρ＝1，δτo＝0，σ＝5，q＝200，μ1＝0.3、μ2＝0.3、μ3＝0.4，cp＝100，qo＝100，τmax＝0.4，τmin＝0.0001，ρ＝1，c＝3，v＝5m/s，dsafe＝5km，rmin＝0.01km。蚂蚁种群包含30只蚂蚁，设置最大迭代次数为60次。(1)非均匀栅格模型与均匀栅格模型对比本发明采用四叉树建模得到非均匀栅格模型，为了验证非均匀栅格模型的有效性。将非均匀建模方法和均匀建模方法对相同的搜索空间(56.6km×52.7km的海域)建模，设置最小栅格尺度相同(最短长、宽>＝0.01km)，然后利用改进蚁群算法在两栅格模型中进行路径规划。表1-1为不同环境模型对比表，可以看出非均匀栅格模型的自由栅格数目、障碍栅格数目、总数目远远小于均匀栅格模型的自由栅格数目、障碍栅格数目、总数目，其中总数目约为0.70％，自由栅格数目约为0.50％，障碍物栅格数目约为1.26％，相差了2个数量级。从对比数据中可以看出非均匀栅格模型对环境空间进行有效压缩，相比较均匀栅格模型降低了存储空间。表1-1不同环境模型对比为了更全面的比较非均匀栅格模型与均匀栅格模型对于算法性能的影响，将出发点固定(17,510)，终点位置随机选取10个位置，将改进算法应用在非均匀栅格模型与均匀栅格模型，在相同的算法参数设置下运行10次，取最优解进行对比。表1-2为改进算法在非均匀模型和均匀模型下性能参数，其中fp为适应度、ep为路径耗能、rp为路径危险度、wp为路径转弯次数、tp为算法运行时间，表中数据按照ep从小到大排列。从表1-2中可以看出，改进算法在非均匀模型中规划得到的路径与改进算法在均匀模型下规划得到的路径相比各项均占优，而且随着搜索复杂度的提高，改进算法的优势更加明显。在第1个规划情形处，非均匀模型的规划时间为均匀模型的13.90％，而在第10个规划情形处，非均匀模型的规划时间为均匀模型的0.29％。这是因为在最小栅格尺度相同的情况下，均匀栅格模型搜索速度较慢，到达目标位置所需的路径节点数目较多，规划所需的时间更长；而非均匀模型由于在“开阔区域”搜索速度较快，到达目标位置所需的路径节点数目较少，规划所需的时间更短，由此可见非均匀栅格模型极大的提高了算法的搜索效率。随着规划区域地形复杂度的提升，非均匀模型的优势体现的更加明显，在第1个规划情形中，非均匀模型的能耗、路径危险度、转弯次数分别为均匀栅格模型的90.3％，89.9％，16.7％，在第10个规划情形中，非均匀模型的能耗、路径危险度、转弯次数分别为均匀栅格模型的64.9％，42.3％，0.22％，由此可见非均匀模型有效降低了路径能量消耗，提高了算法的安全性和可执行性。图10为非均匀模型与均匀模型对应的路径对比图。图中蓝色路径为改进蚁群算法在均匀栅格模型中规划得到的路径，红色路径为改进蚁群算法在非均匀栅格模型中规划得到的路径。从图中可以看出蓝色路径中在靠近出发点和目标点的范围中有一部分紧贴着障碍物，而红色路径与障碍物保持一定的距离。图中红色路径转折次数与蓝色路径相比少很多，可以得知如果水下航行器沿着红色路径前进肯定比沿着蓝色路径可执行性高。从图中可以看出蓝色路径存在波折，红色路径波折现象相比较蓝色路径则较少。表1-2改进算法在非均匀模型和均匀模型下性能参数对比改进蚁群算法与基本蚁群算法对比：为了更全面的评测改进蚁群算法性能，将改进蚁群算法与acs算法、mmas算法、精英蚂蚁个数为种群个数10％的“精英策略”蚁群算法(下文简称eac10)、精英蚂蚁个数为种群个数30％的“精英策略”蚁群算法(下文简称eac30)采取相同的算法参数设置，在出发点固定(17,510)，目标位置随机的10种情况下，在非均匀栅格模型环境中分别规划10次，取最优解进行对比。表1-3acs算与mmas算法在非均匀栅格模型规划数据表1-4eac10算法与eac30算法在非均匀栅格模型规划数据表1-3和表1-4分别为基本蚁群算法在非均匀栅格模型中规划得到的路径参数。将表1-2中改进的蚁群算法数据和表1-3和表1-4中几种基本蚁群算法数据进行对比，可以发改进蚁群算法在算法适应度、能耗、路径危险度、路径转弯次数、算法消耗时间上均占优。在第10个规划情形中，改进蚁群算法在算法耗时上为acs算法的36.6％，为mmas算法的76.4％，为eac10的68.9％，为eac30的67.2％，可以看出在改进的启发式距离引导下，改进算法能够进行更加有效的搜索，花费更少的时间，找到目标位置。在第10个规划情形中，改进算法的路径危险度为acs算法的51.4％，为mmas算法的48.6％，为eac10的48.2％，为eac30的49.3％，说明在路径点选择策略中考虑了待选择栅格距离障碍物距离之后，改进算法规划得到的路径相比较其他几种基本蚁群算法，在路径的安全性上得到极大的提升。在第1个规划情形中，改进算法的转弯次数与其他几种基本蚁群算法相同，但是在第10个情形中，改进蚁群算法的转弯次数为acs算法的28.9％，为mmas算法的20.4％，为eac10的20.7％，为eac30的“精英策略”蚁群算法的26.8％，可见随着搜索复杂度的提升，改进算法在路径可执行性优势更加明显。图11、图12、图13、图14分别为acs算法、mmas算法、eac10、eac30在序号10情况下规划得到的路径图，从图中可以看出这些算法规划得到的路径中很多路径节点离障碍物很近，并且算法转折次数较多。图15给出了这5种算法适应度随着迭代次数的变化情况。从图15中可以看出，基于非均匀栅格模型的改进蚁群算法收敛速度比acs算法和mmas算法快。eca10算法并没有收敛，适应度值发散，而eca30在迭代次数为21时已经到达最优，并保持稳定。这是由于采用“精英策略”的蚁群算法其算法是否收敛以及算法适应度值受到“精英蚂蚁”个数的影响，改参数的设置需要依靠人为经验或者进行多次尝试后才能确定。mmas算法在迭代次数为34时仍旧不稳定，存在小震荡。相比较其他几种基本蚁群算法，改进蚁群算法规划得到的路径适应度值更大，收敛速度更快并保持稳定(2)改进蚁群算法与a*算法对比将改进蚁群算法与传统启发式路径规划算法—a*算法在非均匀栅格模型中，规定相同的出发位置(17,510)和目标位置(495,65)独立规划多次，取平均值进行比较。从表1-5中可以看出a*算法的运行时间远远小于改进算法，这是因为a*运行1次就能得到解，蚁群算法是进化算法，通过迭代多次(具体多少次根据算法设置的最大迭代次数)找到最优解。改进蚁群算法规划得到路径的能耗略大于a*算法路径能耗，为a*算法路径能耗的104.4％。但是在路径适应度值、路径危险度和转弯次数的比较重，改进算法占优。改进算法规划得到路径的适应度值、危险度、转弯次数分别为a*算法规划得到路径的121.4％、50.7％、61.2％。在复杂的海洋环境中，相比较路径能耗和算法的规划时间，路径的安全性更重要，因此改进蚁群算法相比较a*算法更适用于工作环境是复杂、湍流的海洋环境中的水下航行器路径规划。如果任务要求尽可能快的得到规划路径或者得到低能耗的路径，那么应该选择a*算法。图16为2个算法的路径对比示意图。从图中可以看出，a*算法规划得到路径在某些位置紧挨着障碍物，这是因为a*算法并没有考虑选择栅格与障碍物之间的距离，而改进蚁群算法的路径节点则与障碍物保持一定距离。图17为改进蚁群算法得到的路径点连线图和利用b样条曲线拟合路径点得到的光滑路径对比。表1-5算法数据对比当前第1页12当前第1页12