CN102722749A - 一种基于粒子群算法的自适应三维空间路径规划方法 - Google Patents

Abstract

本发明提出一种基于粒子群算法的自适应三维空间路径规划方法，针对海底地形高程图。首先初始化粒子的空间位置和位移，在初始化空间位置时进行了维度重构，初始化第一代粒子所经过的最佳位置及群体当代所发现的最佳位置，然后更新粒子的下一代位移以及空间位置，在更新中引入吸引算子和排除算子，通过计算粒子的适应度，更新粒子下一代所经过的最佳位置以及种群所发现的最佳位置，反复更新粒子的位移以及空间位置，直到完成所要求的迭代次数。本发明方法对寻路环境没有特殊要求，在路径规划过程中的收敛速度、收敛精度及自适应性都得到了提高，并使粒子节点在空间中自由移动成为可能，增大了寻路的成功率，减少了路径规划的计算量。

Description

一种基于粒子群算法的自适应三维空间路径规划方法

技术领域

本发明属于计算智能技术领域，涉及一种通过模拟群智能寻优方式设计的三维空间路径规划方法。

背景技术

随着海空事业的发展，三维路径规划越来越受到人们的重视，其在资源与时间的有效利用上起着至关重要的作用。但已有的路径规划方法多是针对二维空间提出的，现有的三维空间路径规划方法多是二维方法向三维方法的推广，由于三维空间的复杂度提高，在由二维向三维推广过程中必会带来诸多问题，如现已采用的针对三维空间路径规划方法人工势场法、A^*搜索法、基于案例的推理法和遗传算法等，其中势场法不可避免的会陷于局部最小，而且当采用复杂的优化准则时，势场法不能直接的加以推广；A^*搜索法能够用于高维问题，但随着维数的增加，A^*搜索法的时空要求将很难得到满足；基于案例的推理法根据局部的障碍物调整路径，有时不能获得全局最优的路径；遗传算法采用随机的方式产生初始路径，利用遗传算子操作不断的对路径进行改进，当环境条件比较简单时，遗传算法可以完成规划，当环境复杂时，遗传算法将很难找到一条满足约束条件的可行路径。出现上述问题是由于算法自身构造带来的，要想很好的解决，就需要有新理论新方法的提出。

发明内容

本发明在基本粒子群算法的基础上，结合三维空间路径规划的具体情况，提出了一种基于粒子群算法的自适应三维空间路径规划方法，克服了现有三维空间路径规划方法因维度的提高，使得运算量急剧增大，而很难或完全不能完成路径规划任务的问题，以及寻得最优路径不平滑的问题。

本发明提出一种基于粒子群算法的自适应三维空间路径规划方法，环境数据为海底地形高程图，具体包括以下几个步骤：

步骤一：初始化参数：

首先，在海底地形高程图的范围内随机初始化粒子

其中

为组成粒子的节点(路径是由从起点到终点的折线段组成，线段的端点即组成粒子的节点)；i＝1,2,..,n，表示第i个粒子，n≥1为种群数量；j＝1,2,..,m，表示某粒子的第j个节点，m≥3为节点个数；令

其中S,D分别为寻路的起始点和目标点；初始的种群代数k＝1；然后对各粒子的节点的次序，按照重构指标进行重构，第i个粒子的第j个节点

的重构指标λ_j为：

${\mathrm{\λ}}_j=\sqrt{\left|\right|\stackrel{\→}{{\mathrm{SA}}_{i,j}^1}\left|\right|}-\sqrt{\left|\right|\stackrel{\→}{{\mathrm{DA}}_{i,j}^1}\left|\right|}$

其中j＝1,2,...,m，在中λ_j值越大的节点j值越大，反之则越小；初始化位移

i＝1,2,...,n，j＝1,2,...,m；最大移动步长和最小步长分别为step_max＞0，0≤step_min≤step_max；惯性权重的最大最小值分别为0＜ω_max≤1，0≤ω_min≤ω_max；自身学习因子的最大值和最小值分别为

全局学习因子的最大值和最小值分别为

最大迭代次数为k_max；

步骤二：首先令

（i＝1,2,...,n，j＝1,2,...,m），记录第k＝1代时，第i个粒子所经过的最佳位置。

然后确定各粒子的适应度，

的适应度为：

$F\left(X_i^1\right)=\underset{j=1}{\overset{m-1}{\mathrm{\Σ}}}\left|\right|\stackrel{\→}{A_{i,j}^1{A}_{i,j+1}^1}\left|\right|+{\mathrm{Md}}_i^1$

其中

为粒子X_i所表示的路径的长度；

为惩罚因子，其中M＞0为常量；

为粒子的节点在障碍物内个数。

最后，取适应度值最小的粒子，设为X_i′，令

记录第k＝1代时，种群所发现的最佳位置。

步骤三：确定粒子各节点

（i＝1,2,...,n，j＝2,3,...,m-1）的位移包括两种方法：

（1）吸引算子与粒子群算法采用紧耦合策略，则位移

为：

$\stackrel{\→}{{\mathrm{\υ}}_{i,j}^{\′k+1}}=\mathrm{\ω}\·\stackrel{\→}{{\mathrm{\υ}}_{i,j}^k}+c_1{r}_1^k\·\stackrel{\→}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\·\stackrel{\→}{A_{i,j}^k{P}_j^g}+\stackrel{\→}{s_{i,j}^{k+1}}+\stackrel{\→}{e_{i,j}^{k+1}}$

（2）吸引算子与粒子群算法采用松耦合策略，则位移

为：

$\left\{\begin{array}{c}\stackrel{\→}{{\mathrm{\υ}0}_{i,j}^{k+1}}=\mathrm{\ω}\·\stackrel{\→}{{\mathrm{\υ}0}_{i,j}^k}+c_1{r}_1^k\·\stackrel{\→}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\·\stackrel{\→}{A_{i,j}^k{P}_j^g}\\ \stackrel{\→}{{\mathrm{\υ}}_{i,j}^{\′k+1}}=\stackrel{\→}{{\mathrm{\υ}0}_{i,j}^{k+1}}+\stackrel{\→}{s_{i,j}^{k+1}}\end{array}\right.$

（1）和（2）中，惯性权重

惯性权重单调控制量u_ω≥0；自身学习因子 $c_1=c_1^{\max }-{\left(\frac{k}{k_{\max }}\right)}^{u_c}\·(c_1^{\max }-c_1^{\min }),$ 全局学习因子 $c_2=c_2^{\min }+{\left(\frac{k}{k_{\max }}\right)}^{u_c}\·(c_2^{\max }-c_2^{\min }),$ 学习因子单调控制量u_c＞0；为[0,1]区间的随机数；吸引算子

定义为：

其中

为阻力系数，

为排除算子；（2）中的

表示原粒子群粒子位移方法；

排除算子

的确定过程为：首先确定节点

在xoy面内的映射所在的三角网格△A′B′C′，该网格△A′B′C′所对应的障碍物表面三点为A,B,C（沿z轴负方向观察A,B,C为逆时针排列），则网格△A′B′C′所对应的障碍物表面的法线方向表示为：

$\stackrel{\→}{n}=\stackrel{\→}{\mathrm{AB}}\×\stackrel{\→}{\mathrm{AC}}$

易知

的方向指向障碍物外，于是排除算子

定义如下

$\stackrel{\→}{e_{i,j}^{k+1}}=\left\{\begin{array}{cc}\frac{\stackrel{\→}{A_{i,j}^{k}A}\·\stackrel{\→}{n}}{{\left|\right|\stackrel{\→}{n}\left|\right|}^2}\·\stackrel{\→}{n}& (\stackrel{\→}{A_{i,j}^{k}A}\·\stackrel{\→}{n}>0)\\ 0& (\stackrel{\→}{A_{i,j}^{k}A}\·\stackrel{\→}{n}\≤0)\end{array}\right.$

当

时点

在障碍物内，当

时点

在障碍物外。在计算适应度函数时，节点是否在障碍物内的判断，也是通过此方法实现的。

步骤四：确定当代最大位移模量step_k为： ${\mathrm{step}}_k={\mathrm{step}}_{\max }-{\left(\frac{k}{k_{\max }}\right)}^{u_s}\·({\mathrm{step}}_{\max }-{\mathrm{step}}_{\min }),$ 最大位移模量单调控制量u_s＞0，判断是否大于step_k，若是则令

否则令

确定节点

（i＝1,2,...,n，j＝2,3,...,m-1）下一代的位置

$A_{i,j}^{k+1}=A_{i,j}^k+\stackrel{\→}{{\mathrm{\υ}}_{i,j}^{k+1}}$

于是第k+1代粒子

（i＝1,2,...，n，j＝1,2,...,m）。

步骤五：确定第k+1代粒子

（i＝1,2，...,n）的适应度值，适应度

为： $F\left(X_i^{k+1}\right)=\underset{j=1}{\overset{m-1}{\mathrm{\&Sigma;}}}\left|\right|\stackrel{\&RightArrow;}{A_{i,j}^{k+1}{A}_{i,j+1}^{k+1}}\left|\right|+{\mathrm{Md}}_i^{k+1},$ 若 $F\left(X_i^{k+1}\right)<F\left(P_i^b\right),$ 则令 $P_i^b=X_i^{k+1};$

步骤六：设

为

中适应度值最小的位置，若

则令

步骤七：更新当前迭代次数k＝k+1，并判断当前迭代次数k是否大于最大迭代次数k_max，若是则结束本方法，否则转向步骤三执行。

本发明的优点和积极效果在于：

1、本发明提出一种基于粒子群算法的三维空间路径规划方法，令粒子节点自由的在空间中运动，相对于栅格化方法，提高了路径规划方法的自适应性。

2、本发明的三维空间路径规划方法，在粒子群算法的速度位移公式中，紧耦合的引入了吸引算子，吸引算子在路径规划中起到了路径的成型、平滑和趋向最优解的启发作用，由于吸引算子的引入使得路径规划方法的收敛速度、收敛精度、以及自适应性都得到了提高，并使粒子节点在空间中自由移动成为可能。

3、本发明的三维空间路径规划方法，对寻路环境没有特殊要求，空间建模简单。

4、本发明的三维空间路径规划方法，在粒子群算法的基础上引入排除算子，增大了寻路的成功率，增强了粒子群算法的全局寻优能力。

5、本发明的三维空间路径规划方法，在种群初始化时，引入了维度重构指标，对粒子维度进行重构，大大减少了路径规划的计算量，使粒子的初始化更加合理。

6、本发明的三维空间路径规划方法，经过实验证明收敛速度快，全局寻优能力强。

附图说明

图1是本发明设计的吸引算子的计算示意图；

图2是本发明提出的三维空间路径规划方法的步骤流程图；

图3是加入本发明所设计的吸引算子时的俯视效果图；

图4是加入本发明所设计的吸引算子时的侧视效果图；

图5是不加入本发明设计的吸引算子时的俯视效果图；

图6是不加入本发明设计的吸引算子时的侧视效果图；

图7是应用本发明的三维空间路径规划方法得到的最优位置收敛曲线示意图。

具体实施方式

下面将结合附图和实施例对本发明的技术方案进行详细说明。

首先，下面将说明本发明方法所用到的主要技术。

粒子编码方式：路径是由从起点到终点的折线段组成，称线段的端点为节点，节点组成的集合即构成粒子群算法中所述的粒子，于是粒子的编码方式可表示为：

$X_i^k=\left\{A_{i,j}^k\right\}$

其中，

表示第k次迭代的粒子X_i，i＝1,2,...,n，表示第i个粒子，n为粒子个数，即群体大小；j＝1,2,...,m，j表示粒子第j个端点，m为组成各粒子节点的个数，即粒子的维度（由于节点的维度是3维的，所以实际上粒子的维度为3×m维的）；k＝1,2,...,k_max为迭代次数，k_max为最大迭代次数，

表示节点的空间坐标，

表示路径的起点和终点，它们的坐标值是固定的。

适应度函数：本发明的三维空间路径规划方法的目的为寻找一条不与障碍物相交的最短的路径，所以适应度函数规定如下：

$F\left(X_i^k\right)=\underset{j=1}{\overset{m-1}{\mathrm{\&Sigma;}}}\left|\right|\stackrel{\&RightArrow;}{A_{i,j}^k{A}_{i,j+1}^k}\left|\right|+{\mathrm{Md}}_i---\left(1\right)$

其中，

为粒子所表示的路径的长度，长度越小则适应度

值越小；Md_i为惩罚因子，其中M＞0为常量，其值越大则粒子穿越障碍物的能力越小，越容易陷入局部最优解，但避障能力越强，其值越小则粒子穿越能力越大，越容易跳出局部最优解，但避障能力越弱；d_i为粒子

的节点在障碍物内个数，进入障碍物的节点越少，惩罚因子越小，适应度函数

值越小，这说明长度越短、进入障碍物的节点个数越少的粒子，适应度函数值

值越小，即适应度值越小路径位置越佳。

基本粒子群算法：基本粒子群位置更新公式

$\left\{\begin{array}{c}{V}_i^{k+1}=\mathrm{\&omega;}\&CenterDot;V_i^k+c_1{r}_1^k\&CenterDot;(P_i^b-X_i^k)+c_2{r}_2^k\&CenterDot;(P^g-X_i^k)\\ X_i^{k+1}=X_i^k+V_i^{k+1}\end{array}\right.---\left(2\right)$

将（2）式以节点形式表示有;

$\left\{\begin{array}{c}\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}=\mathrm{\&omega;}\&CenterDot;\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^k}+c_1{r}_1^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_j^g}\\ A_{i,.j}^{k+1}=A_{i,j}^k+\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}\end{array}\right.---\left(3\right)$

其中，k为迭代次数，上角标k表示第k次迭代，上角标k+1表示第k+1次迭代；

表示第k次迭代的粒子X_i的位移，

为第k次迭代的粒子X_i所经过的最佳位置；

为群体当前所发现的最佳位置；ω为非负惯性权重，c₁,c₂分别为自身学习因子和全局学习因子，

为[0,1]区间的随机数。

吸引算子：吸引算子令粒子相邻节点之间总是相互吸引的，如图1所示，第k+1次迭代过程中粒子X_i的吸引算子定义为：

其中，

为阻力系数；

根据Banach压缩映像原理证明，若仅有吸引算子作用，即第k+1次迭代粒子X_i的位置更新为:则

时吸引算子是收敛的，并且总是收敛到起点到终点的连线上。图1中的B_i表示节点

在阻力系数

时，将会移动到的位置。本发明提出的吸引算子是一种启发式算子，由于两点之间直线最短，越是接近该线段，则路径越短，该吸引算子启发粒子向起点到终点的线段移动，可大大提高整个粒子群算法的收敛速度和收敛效果。

下面分析吸引算子的作用：

路径的节点数是本发明方法精度的关键，节点数越大，所得路径的精度越高，同时节点数决定着粒子的维度，由粒子编码方式可知，当节点数为m时，粒子的维度为3×m维，即粒子的自由度为3×m，粒子的节点越多粒子的自由度越大，大自由度使得粒子群算法很难发挥其全局寻优能力，甚至单纯的粒子群算法几乎不能规划出一条合理的路径。针对粒子维度过大这一问题，已有方法一般采用栅格法来降低维度，即当节点数为m时，将空间（三维规划）或平面（二维规划）用m个平行的面或线分割开，令粒子各节点仅在对应的面或线上运动，这样粒子的维度便可以减少m，此法大大减少了粒子的维度，但由于栅格的位置需要人为设定，需要操作者首先得对地型十分的了解，才可以设定栅格的位置，减弱了路径规划方法的自适应性，并且对于三维路径规划来说，在减少m维后，仍有2×m维的自由度，这样的自由度仍然是不小的，路径的确定依然很困难。

通过观察发现，路径各节点之间不是孤立的，它们之间是存在联系的，如相邻节点总是要相互靠近的，节点总是由起点到终点逐一排列的，称上述过程为路径的成型。粒子群算法在路径寻优的过程中，不但需要寻找路径的最优值，还承担着路径成型的工作，这大大加重了粒子群算法的负担，使其全局寻优能力不能得到很好的发挥，甚至丧失了全局寻优能力。

本发明方法引入吸引算子来帮助完成路径的成型工作，这样粒子群算法便可以更加专注于全局寻优，并且各相邻节点之间通过吸引算子建立起一定的联系，使得粒子各节点构成了一个整体，间接降低了粒子的维度。实验表明吸引算子的引入不但提高了路径的平滑度，加快了粒子群的收敛速度，而且粒子各节点构成一个整体参与寻路，间接降低了粒子的维度，使粒子群算法的全局寻优能力得到充分的发挥。

吸引算子加入方式：吸引算子与粒子群算法的结合可分为两种策略，它们分别是紧耦合策略和松耦合策略，紧耦合策略是指吸引算子作用于惯性位移项，松耦合策略是指吸引算子不作用于惯性位移项，此两种策略又可衍生出不同的方法，两种策略各举一例加以说明。

紧耦合策略的一种结合方法：

$\left\{\begin{array}{c}\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}=\mathrm{\&omega;}\&CenterDot;\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^k}+c_1{r}_1^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_j^g}+\stackrel{\&RightArrow;}{s_{i,j}^{k+1}}\\ A_{i,.j}^{k+1}=A_{i,j}^k+\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}\end{array}\right.---\left(5\right)$

式(5)是将吸引算子加入到式(3)中所得，其中0≤ω≤1；

松耦合策略的一种结合方法，将吸引算子加入到式(3)得到：

$\left\{\begin{array}{c}\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}0}_{i,j}^{k+1}}=\mathrm{\&omega;}\&CenterDot;\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}0}_{i,j}^k}+c_1{r}_1^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_j^g}\\ \stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}=\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}0}_{i,j}^{k+1}}+\stackrel{\&RightArrow;}{s_{i,j}^{k+1}}\\ A_{i,j}^{k+1}=A_{i,.j}^k+\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}\end{array}\right.---\left(6\right)$

其中，

表示原粒子群粒子位移方法。本发明实施例中采用式（5）所示的紧耦合策略来进行步骤说明和仿真实验。

粒子初始化：传统粒子群算法中，粒子随机分布在一定的空间范围内，在空间路径规划中，组成粒子的节点，存储时有先后顺序，如粒子

其中节点

排在第i个粒子的第j位上，此顺序的先后最终会体现为路径各节点的排列顺序，观察到距离起点越近的节点的j值应越小，反之则越大，距离终点越近的节点的j值应越大，反之则越小，随机初始化后的粒子各节点若也服从此规律，便可省去很多没有必要的运算，称上述初始化过程为维度重构。首先在一定的空间内随机初始化粒子各节点的空间位置，然后按照如下定义的指标对粒子进行维度重构，以粒子的第j个节点

为例，该节点的重构指标λ_j定义如下：

${\mathrm{\&lambda;}}_j=\sqrt{\left|\right|\stackrel{\&RightArrow;}{{\mathrm{SA}}_{i,j}^1}\left|\right|}-\sqrt{\left|\right|\stackrel{\&RightArrow;}{{\mathrm{DA}}_{i,j}^1}\left|\right|}---\left(7\right)$

其中S,D分别为路径的起点和终点，在

中λ_j值越大的节点j值越大，反之则越小。

节点位移大小控制：考虑到粒子群算法的全局寻优作用，同时为确保粒子群算法的收敛性，以及路径的平滑性，希望粒子各节点的位移大小在迭代过程中由大到小变化，以第k次迭代为例，节点在第k次迭代的最大位移step_k为：

${\mathrm{step}}_k={\mathrm{step}}_{\max }-{\left(\frac{k}{k_{\max }}\right)}^{u_s}\&CenterDot;({\mathrm{step}}_{\max }-{\mathrm{step}}_{\min })---\left(8\right)$

其中step_max＞0，0≤step_min≤step_max为位移大小的上界和下界，上界的取值范围为step_max≤L，其中L为寻路环境最大直径大小，由于节点位移大小的最大为L，若节点位移大小超过L，节点必定会移动到寻路环境外，所以令step_max≤L；k_max≥2为最大迭代次数，由于k＝1时，粒子是随机初始化的，当k＝2时，粒子群算法才进行了一次寻优运算，所以令k_max≥2；参数u_s用于控制step_k的递减速度；在第k次迭代中，若某节点位移的模大于step_k则令其位移大小为step_k，方向不变，否则位移大小方向均不变。初始寻路时较大的位移保证了收敛的速度，以及全局寻优的功能，在寻路的后期较小的位移保证了粒子群算法的收敛性，以及路径的平滑性和准确性。

排除算子：由于粒子群算法并不具有主动避障能力，其避障功能依赖于惩罚因子，这种避障方法具有一定的随机性，使得粒子的避障非常的盲目和被动，这种盲目性使得大量的迭代运算变的没有意义，甚至当种群中没有可行路径时（所有粒子均穿过障碍物），避障的盲目性会使得粒子群算法无法跳出非可行解，最终导致寻路失败。为此引入排除算子，曾强粒子群算法的主动避障能力。

粒子某节点在进入障碍物时，希望有移向障碍物外运动的趋势，但又不希望排除运动在这里起到绝对的作用，于是有如下速度位置更新公式：

$\left\{\begin{array}{c}\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}=\mathrm{\&omega;}\&CenterDot;\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^k}+c_1{r}_1^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_j^g}+\stackrel{\&RightArrow;}{s_{i,j}^{k+1}}+\stackrel{\&RightArrow;}{e_{i,j}^{k+1}}\\ A_{i,j}^{k+1}=A_{i,j}^k+\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}\end{array}\right.---\left(9\right)$

其中，是排除算子，排除算子的定义可以有多种形式，不同的空间环境排除算子不尽相同，排除算子定义的好坏直接影响到粒子群算法的寻优质量。本发明方法中定义排除算子以一定的大小沿障碍物表面的法线方向运动，由于本发明方法所应用的地形是均匀网格三维高程图，所以为运算方便，计算节点在xoy面的映射所在的三角网格对应点表面的法线，以该法线方向为

的方向，设点

在xoy面内的映射所在三角网格为A′B′C′，该网格所对应的障碍物表面三点为A,B,C（沿z轴负方向观察A,B,C为逆时针排列），则该表面的法线方向

可表示为：

$\stackrel{\&RightArrow;}{n}=\stackrel{\&RightArrow;}{\mathrm{AB}}\&times;\stackrel{\&RightArrow;}{\mathrm{AC}}---\left(10\right)$

由于环境为高程图，易知

的方向指向障碍物外，于是排除算子定义如下：

$\stackrel{\&RightArrow;}{e_{i,j}^{k+1}}=\left\{\begin{array}{cc}\frac{\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}}{{\left|\right|\stackrel{\&RightArrow;}{n}\left|\right|}^2}\&CenterDot;\stackrel{\&RightArrow;}{n}& (\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}>0)\\ 0& (\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}\&le;0)\end{array}\right.---\left(11\right)$

当

时点

在障碍物内，当时点在障碍物外。在计算适应度函数时，节点是否在障碍物内的判断，也是通过此方法实现的。

本发明提出一种基于粒子群算法的自适应三维空间路径规划方法，如图2所示，具体包括以下几个步骤：

步骤一：初始化参数：

首先，在所应用的海底地形高程图的范围内随机初始化粒子

其中

为组成粒子的节点，i＝1,2,...,n，表示第i个粒子，n≥1为种群数量；j＝1,2,...,m，表示某粒子的第j个节点，m≥3为节点个数；令

其中S,D分别为寻路的起始点和目标点；初始种群迭代次数k＝1。

然后，对各粒子的节点的次序，按照重构指标进行重构，根据（7）式得重构指标λ_j为：

${\mathrm{\&lambda;}}_j=\sqrt{\left|\right|\stackrel{\&RightArrow;}{{\mathrm{SA}}_{i,j}^1}\left|\right|}-\sqrt{\left|\right|\stackrel{\&RightArrow;}{{\mathrm{DA}}_{i,j}^1}\left|\right|}---\left(12\right)$

在粒子

中λ_j值越大的节点j值越大，反之则越小；初始化位移

i＝1,2,...,n，j＝1,2,...,m；最大移动步长和最小步长分别为step_max＞0，0≤step_min≤step_max；惯性权重的最大最小值分别为0＜ω_max≤1，0≤ω_min≤ω_max；自身学习因子的最大最小值分别为

全局学习权重的最大最小值分别为

设置最大迭代次数为k_max；

步骤二：令

（i＝1,2,...,n，j＝1,2,...,m），记录第k＝1代时，粒子X_i所经过的最佳位置。根据（1）式计算适应度

为：

$F\left(X_i^1\right)=\underset{j=1}{\overset{m-1}{\mathrm{\&Sigma;}}}\left|\right|\stackrel{\&RightArrow;}{A_{i,j}^1{A}_{i,j+1}^1}\left|\right|+{\mathrm{Md}}_i^1---\left(13\right)$

其中，

为粒子X_i所表示的路径的长度；

为惩罚因子，其中M＞0为常量；

为粒子

的节点在障碍物内个数。然后取适应度值最小的粒子，设为X_i′，令

记录第k＝1代时，种群所发现的最佳位置；

步骤三：根据（9）式，计算粒子各节点

（i＝1,2,...，n，j＝2,3,...,m-1）的位移

为：

$\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{\&prime;k+1}}=\mathrm{\&omega;}\&CenterDot;\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^k}+c_1{r}_1^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_j^g}+\stackrel{\&RightArrow;}{s_{i,j}^{k+1}}+\stackrel{\&RightArrow;}{e_{i,j}^{k+1}}---\left(14\right)$

其中 $\mathrm{\&omega;}={\mathrm{\&omega;}}_{\max }-{\left(\frac{k}{k_{\max }}\right)}^u\&CenterDot;({\mathrm{\&omega;}}_{\max }-{\mathrm{\&omega;}}_{\min }),$ 为惯性权重； $c_1=c_1^{\max }-{\left(\frac{k}{k_{\max }}\right)}^u\&CenterDot;(c_1^{\max }-c_1^{\min })$ 为自身学习因子；

为全局学习因子；式中u≥0为单调控制量；为[0,1]区间的随机数；

为吸引算子，根据（4）式，吸引算子

为：

其中为阻力系数，令

为排除算子，设点

在xoy面内的映射所在三角网格为△A′B′C′，该网格所对应的障碍物表面三点为A,B,C（沿z轴负方向观察A,B,C为逆时针排列），则根据（10）式该表面的法线

为：

$\stackrel{\&RightArrow;}{n}=\stackrel{\&RightArrow;}{\mathrm{AB}}\&times;\stackrel{\&RightArrow;}{\mathrm{AC}}---\left(16\right)$

易知

的方向指向障碍物外，根据（11）式，计算排除算子为：

$\stackrel{\&RightArrow;}{e_{i,j}^{k+1}}=\left\{\begin{array}{cc}\frac{\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}}{{\left|\right|\stackrel{\&RightArrow;}{n}\left|\right|}^2}\&CenterDot;\stackrel{\&RightArrow;}{n}& (\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}>0)\\ 0& (\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}\&le;0)\end{array}\right.---\left(17\right)$

当

时点

在障碍物内，当

时点在障碍物外。

步骤四：根据（8）式，确定第k代的最大位移模量step_k：

${\mathrm{step}}_k={\mathrm{step}}_{\max }-{\left(\frac{k}{k_{\max }}\right)}^u\&CenterDot;({\mathrm{step}}_{\max }-{\mathrm{step}}_{\min })---\left(18\right)$

判断

是否大于step_k，若是则令

否则令根据（9）式，计算节点

（i＝1,2,...,n，j＝2,3,...,m-1）下一代的位置

为：

$A_{i,j}^{k+1}=A_{i,j}^k+\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}---\left(19\right)$

于是

（i＝1,2,...，n，j＝1,2，...,m）；

步骤五：根据（1）式确定第k+1代各粒子

（i＝1,2，...,n）的适应度值，适应度函数

（i＝1,2,…,n）为：

$F\left(X_i^{k+1}\right)=\underset{j=1}{\overset{m-1}{\mathrm{\&Sigma;}}}\left|\right|\stackrel{\&RightArrow;}{A_{i,j}^{k+1}{A}_{i,j+1}^{k+1}}\left|\right|+{\mathrm{Md}}_i^{k+1}---\left(20\right)$

并更新各粒子的到第k+1代为止所经过的最佳位置，若

则令

步骤六：更新种群第k+1代所发现的最佳位置。设

为

中适应度值最小的位置，若 $F\left(P_{i^{\&prime;}}^b\right)<F\left(P^g\right),$ 则令 $P^g=P_{i^{\&prime;}}^b.$

步骤七：更新当前迭代次数k＝k+1，并判断当前迭代次数k是否大于最大迭代次数k_max，若是则结束本方法，否则转向步骤三执行。最终，所得到的集合

记载了各粒子的所要经过的最优为止，P^g记载了种群发现的最佳位置。

实施例：

起点坐标S＝(111250 15700-600)；终点坐标D＝(112600 17210-600)m；种群数n＝10；粒子节点数m＝42；最大迭代次数k_max＝300；位移的上界和下界分别为

其中L＝2272为地图的长宽较大值；惯性权重根据公式确定，其中ω_max＝0.9，ω_min＝0.4，k为迭代次数；学习因子c₁根据公式 $c_1=c_1^{\max }-{\left(\frac{k}{k_{\max }}\right)}^{u_c}\&CenterDot;(c_1^{\max }-c_1^{\min })$ 确定，其中 $c_1^{\max }=0.8,$ $c_1^{\min }=0.1;$ 学习因子c₂根据公式 $c_2=c_2^{\min }+{\left(\frac{k}{k_{\max }}\right)}^{u_c}\&CenterDot;(c_2^{\max }-c_2^{\min })$ 确定，其中 $c_2^{\max }=0.8,$ $c_2^{\min }=0.1;$ 上述等式中单调控制量u_ω＝u_c＝1；为阻力系数

惩罚因子中的

其中L′=2026m，为起点S到终点D的直线距离。

分别观察在有排除算子作用的情况下，加入吸引算子和不加吸引算子的收敛效果。图3图4分别为加入吸引算子的俯视图和侧视图，可以看到路径平滑度和准确度都非常理想；图5图6分别为不加吸引算子的俯视图和侧视图，可以看到由于各节点之间没有一定的关联，导致粒子各节点在复杂的空间环境中几乎不能规划出一条路径。可见收缩算子在规划中起到了路径的成型，平滑，和趋向最优解的启发的作用。

图7为加入吸引算子的种群最优解随迭代次数的长度变化曲线，在迭代到第50次左右时，曲线有一个跳跃，而后又回到原曲线上，这个跳跃是由于种群陷入局部最优解而引起的，而后又回到原曲线上，证明粒子群跳出了局部最优解，找到了全局最优解，说明粒子群算法起到了全局寻优能力。

在有吸引算子作用时，分别观察加入排除算子和不加排除算子对粒子群算法的影响，在相同的条件下，分别进行20次计算，观察得到全局最优解的次数，得到局部最优解的次数和寻路失败（即路径穿过障碍物）的次数。实验所得结果如表1所示。

表1排除算子对寻优效果的影响

	总次数	全局最优解	局部最优解	寻路失败
					加排除	20	16	1	3
不加排除	20	6	7	7

可见在20次实验中加入排除算子得到全局最优解的次数为16，成功率达80%，而不加排除算子时，得到全局最优解的次数仅为6次，成功率为30%，这说明排除算子不但没有对粒子群的全局寻优能力造成干扰，反而起到了促进的作用，这是由于加入排除算子后，增大了粒子在寻优时避开障碍物的几率，种群可以在更多的可行解（路径不穿过障碍物）之间寻找最优解，因此对算法的全寻优能力是有促进作用的。

加排除算子与不加排除算子陷入局部最优解的几率分别为5%和35%，可见不加排除算子时陷入局部最优解的几率较大，这是由于不加排除算子时，粒子多数情况小总是穿过障碍物的，一旦有一个较优越可行解出现，种群很容易便会陷入局部最优解而无法跳出，这就导致陷入局部最优解的几率要较大。

加排除算子与不加排除算子寻路失败的几率分别为15%和35%，可见不加排除算子时算法的寻路失败率同样较大，这是由于原粒子算法的避障能力完全依赖于惩罚因子，此法的避障能力较弱，是一种概率式避障，具有随机性，所以很容易导致避障失败，这便是寻路的失败率较高的原因。

本发明方法中令粒子节点自由地在空间中运动，相对于栅格化方法，提高了路径规划的自适应性。在粒子群算法的速度位移公式中，紧耦合的引入了吸引算子和排除算子，其中应用吸引算子起到了路径的成型，平滑，和趋向最优解的启发作用，正是由于此吸引算子的引入才使得粒子节点在空间中自由运动的情况下，仍能完成规划任务，另外排除算子的引入增大了寻路的成功率，增强了粒子群算法的全局寻优能力。在种群初始化时，引入了维度重构指标，对粒子维度进行重构，大大减少了粒子群算法的计算量，使粒子的初始化更加合理。实验表明本发明方法的收敛速度快，全局寻优能力强，能够得到比较优化的三维空间路径，适用于三维空间的路径规划。

Claims (2)

1.一种基于粒子群算法的自适应三维空间路径规划方法，环境数据为海底地形高程图，其特征在于，该方法包括以下步骤：

步骤1，初始化参数，过程为：

首先，在海底地形高程图的范围内随机初始化粒子初始种群迭代次数k＝1，为组成粒子的节点；i＝1,2,...,n，表示第i个粒子，n≥1为种群数量；j＝1,2,...,m，表示某粒子的第j个节点，m≥3为节点个数；令

其中S,D分别为寻路的起始点和目标点；

然后，对各粒子的节点的次序，按照重构指标进行重构，第i个粒子

的第j个节点

的重构指标λ_j为：

初始化第i个粒子

的第j个节点位移

i＝1,2,...,n，j＝1,2，...,m；设置最大移动步长step_max和最小步长step_min，0≤step_min≤step_max；设置惯性权重的最大值ω_max和最小值ω_min，0＜ω_max≤1，0≤ω_min≤ω_max；设置自身学习因子的最大和最小值分别为

设置全局学习因子的最大值和最小值分别为

设置最大种群迭代次数为k_max；

步骤2，记录第k＝1代时，第i个粒子

所经过的最佳位置

然后确定各粒子的适应度值，

的适应度

为：

$F\left(X_i^1\right)=\underset{j=1}{\overset{m-1}{\mathrm{\&Sigma;}}}\left|\right|\stackrel{\&RightArrow;}{A_{i,j}^1{A}_{i,j+1}^1}\left|\right|+{\mathrm{Md}}_i^1$

其中，

为粒子

所表示的路径的长度；

为惩罚因子，常量M＞0，

为粒子

的节点在障碍物内的个数；

最后，取集合

中适应度值最小的粒子，设为X_i′，令群体当前所发现的最佳位置 $P^g=\left\{P_j^g\right\}=X_{i^{\&prime;}};$

步骤3，确定粒子各节点

（i＝1,2,...,n，j＝2,3,...,m-1）的位移

包括两种方法：

（1）吸引算子与粒子群算法采用紧耦合策略，则位移

为：

$\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{\&prime;k+1}}=\mathrm{\&omega;}\&CenterDot;\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^k}+c_1{r}_1^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_j^g}+\stackrel{\&RightArrow;}{s_{i,j}^{k+1}}+\stackrel{\&RightArrow;}{e_{i,j}^{k+1}}$

（2）吸引算子与粒子群算法采用松耦合策略，则位移

为：

$\left\{\begin{array}{c}\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}0}_{i,j}^{k+1}}=\mathrm{\&omega;}\&CenterDot;\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}0}_{i,j}^k}+c_1{r}_1^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_{i,j}^b}+c_2{r}_2^k\&CenterDot;\stackrel{\&RightArrow;}{A_{i,j}^k{P}_j^g}\\ \stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{\&prime;k+1}}=\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}0}_{i,j}^{k+1}}+\stackrel{\&RightArrow;}{s_{i,j}^{k+1}}\end{array}\right.$

（1）和（2）中，惯性权重惯性权重单调控制量u_ω≥0；自身学习因子 $c_1=c_1^{\max }-{\left(\frac{k}{k_{\max }}\right)}^{u_c}\&CenterDot;(c_1^{\max }-c_1^{\min }),$ 全局学习因子 $c_2=c_2^{\min }+{\left(\frac{k}{k_{\max }}\right)}^{u_c}\&CenterDot;(c_2^{\max }-c_2^{\min }),$ 学习因子单调控制量u_c＞0；

为[0,1]区间的随机数；吸引算子

阻力系数

为排除算子；（2）中的

表示原粒子群粒子位移方法；

步骤4：确定当代最大位移模量step_k：

${\mathrm{step}}_k={\mathrm{step}}_{\max }-{\left(\frac{k}{k_{\max }}\right)}^{u_s}\&CenterDot;({\mathrm{step}}_{\max }-{\mathrm{step}}_{\min })$

最大位移模量单调控制量u_s＞0；判断步骤3得到的是否大于step_k，若是，则令 $\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}=\frac{\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{\&prime;k+1}}}{\left|\right|\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{\&prime;k+1}}\left|\right|}\&CenterDot;{\mathrm{step}}_k,$ 否则令 $\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{k+1}}=\stackrel{\&RightArrow;}{{\mathrm{\&upsi;}}_{i,j}^{\&prime;k+1}};$

确定节点

（i＝1,2,...,n，j＝2,3,...,m-1）下一代的位置

于是第k+1代粒子

（i＝1,2,...,n，j＝1,2,...,m）；

步骤5：确定第k+1代粒子（i＝1,2,...,n）的适应度值，适应度为： $F\left(X_i^{k+1}\right)=\underset{j=1}{\overset{m-1}{\mathrm{\&Sigma;}}}\left|\right|\stackrel{\&RightArrow;}{A_{i,j}^{k+1}{A}_{i,j+1}^{k+1}}\left|\right|+{\mathrm{Md}}_i^{k+1},$ 若 $F\left(X_i^{k+1}\right)<F\left(P_i^b\right),$ 则令 $P_i^b=X_i^{k+1};$

步骤6：设

为

中适应度值最小的位置，若

则令

步骤7：更新当前迭代次数k＝k+1，并判断当前迭代次数k是否大于最大迭代次数k_max，若是则结束本方法，否则转步骤3执行。

2.根据权利要求1所述的一种基于粒子群算法的自适应三维空间路径规划方法，其特征在于，步骤3中所述的排除算子

的确定过程为：

首先，确定节点

在xoy面内的映射所在的三角网格△A′B′C′，该网格△A′B′C′所对应的障碍物表面三点为A,B,C，沿z轴负方向观察A,B,C为逆时针排列，则网格△A′B′C′所对应的障碍物表面的法线方向

为：

$\stackrel{\&RightArrow;}{n}=\stackrel{\&RightArrow;}{\mathrm{AB}}\&times;\stackrel{\&RightArrow;}{\mathrm{AC}}$

的方向指向障碍物外；然后，得到排除算子

为：

$\stackrel{\&RightArrow;}{e_{i,j}^{k+1}}=\left\{\begin{array}{cc}\frac{\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}}{{\left|\right|\stackrel{\&RightArrow;}{n}\left|\right|}^2}\&CenterDot;\stackrel{\&RightArrow;}{n}& (\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}>0)\\ 0& (\stackrel{\&RightArrow;}{A_{i,j}^{k}A}\&CenterDot;\stackrel{\&RightArrow;}{n}\&le;0)\end{array}\right.$

当

时，节点

在障碍物内，当

时，节点

在障碍物外。

Images