[发明专利]基于非均匀建模的改进蚁群路径规划算法

摘要

本发明涉及路径规划方法技术领域，具体的说是一种基于非均匀建模的改进蚁群路径规划算法，本发明考虑到均匀建模方案对复杂环境建模的不足，提出利用四叉树建模得到非均匀栅格模型，提出了基于非均匀栅格模型的改进蚁群路径规划算法，以非均匀栅格模型为基础对基本蚁群算法进行改进，在路径点选择时考虑节点与障碍物之间的距离，在进行路径点选择时考虑待选择节点与障碍物之间的距离，采用“ant‑cycle”的信息素更新方式，在全局信息素更新时限制信息素大小，仿真实验表明非均匀栅格模型相比较均匀栅格模型的优越性，改进蚁群算法相比较其他几种基本蚁群算法收敛速度更快，能够得到更符合复杂环境中水下航行器需要的路径。

主权项

1.一种基于非均匀建模的改进蚁群路径规划算法，其特征在于包括以下步骤：步骤一：利用四叉树来划分搜索空间，得到非均匀栅格模型，利用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提出的粘性Lamb vortices模型来模拟流场，流场的速度用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)可以确定v_i大小，然后通过(公式1‑5)、(公式1‑6)得到w_i，最终确定能耗E_p；步骤四：改进蚁群全局路径规划算法，采用改进的蚁群算法进行路径规划，以非均匀栅格模型为基础，以目标函数为优化目标，对基本蚁群算法进行改进，具体包括以下内容：改进路径点选择策略，改进信息素更新方法，解决路径死锁，其中所述改进路径点选择策略包括：(1)采取合理的启发式距离，借鉴A*算法的启发式距离设计，定义启发式距离为当前栅格current中心坐标到待选择栅格n中心坐标之间的距离d_cn与待选择栅格n中心坐标到目标点target之间的距离d_nt之和，改进的启发式信息定义为：(2)在进行路径点选择时，考虑待选择栅格与障碍物之间距离，针对四叉树分割搜索空间得到非均匀栅格模型对基本蚁群算法进行改进，引入安全距离这一概念，并在路径点选择时考虑待选择栅格与障碍物之间的距离，设计了新的路径点选择策略，式中，Dn表示栅格n与其最近障碍物之间的距离，计算公式(1‑15)所示，D0表示所有栅格中距离障碍物最远的距离长度，Ak(c)表示蚂蚁k在当前栅格c的邻域栅格中可以选择的栅格集合；如果待选择栅格n与其最近障碍物的距离Dn小于安全距离Dsafe，在进行路径点选择的时候考虑栅格与障碍物之间的距离，在路径点选择策略中加入Dn/D0，在Dn/D0中，Dn越大，选择离障碍物远的栅格的几率也就越大，避免选择的栅格离障碍物太近，注意该项始终小于1；如果待选择栅格n与其最近障碍物的距离Dn大于安全距离Dsafe，进行路径点选择的时候不考虑当前栅格与障碍物之间的距离。