无人机自主飞行航迹规划问题

目 录

1. 问题的重述------------------------------------------------------------------------------------2

2. 问题的分析------------------------------------------------------------------------------------2

3. 模型假设----------------------------------------------------------------------------------------3

4. 符号说明----------------------------------------------------------------------------------------3

5. 模型的建立-------------------------------------------------------------------------------------3

5.1问题一模型的分析、建立与求解-----------------------------------------------------3

5.2问题二模型的分析、建立与求解-----------------------------------------------------6

6. 模型的可行性分析与仿真-------------------------------------------------------------------9

6.1模型的可行性分析-----------------------------------------------------------------------9

6.2模型的仿真-------------------------------------------------------------------------------10

7. 模型的评价、改进及推广-------------------------------------------------------------------12

8. 参考文献----------------------------------------------------------------------------------------14

9. 附录----------------------------------------------------------------------------------------------15

一、问题的重述

无人机的发展至今已有70多年的历史，其军事应用主要是执行各种侦察任务。随着无人机平台技术和机载遥感技术的不断发展，它的军事应用范围已经得到大大的扩展，并且这种扩展还将持续下去，如通信中继、军事测绘、电子对抗、信息攻击等。特别是精确制导武器技术的发展，又使它成为搭载这种武器的理想平台。

众所周知，“自主飞行”的能力是无人驾驶飞机所必须具有的。如果要实现无人驾驶飞机的自主飞行,那么就要求无人驾驶飞机具有相当程度的飞行航迹规划能力。无人机的航迹规划是指其为了圆满完成任务而作的计划。它往往指单机在初始位置、终止位置和一些目标任务结点确定之后的航迹规划问题,其基本功能是根据无人机的性能和飞经的地理环境、威胁环境等因素,对已知的目标规划提出满足要求的航迹,以便在实际飞行时可以根据需要进行实时的局部修改。

现在要讨论如下的情况：

假定无人机的活动范围为20km×20km的区域，无人机起点的平面坐标为[1,2](单位：km), 攻击目标的平面坐标为[19,18](单位：km)，同时不考虑无人机起飞和降落时的限制。数字地图和敌方威胁情况(主要考虑雷达威胁)可以从附件中查得。数字地图可以做适当的简化，比如可以把地形近似分为三种：高地，低地以及过渡地带。

具体问题如下：

问题1：忽略地形和无人机操作性能等因素的影响，综合考虑敌方威胁情况、无人机航程等因素，基于二维平面建立单机单目标的航迹规划模型。

问题2：把模型扩展到三维空间，并同时考虑无人机的操作性能（主要考虑拐弯）和地形因素。

问题3：试讨论和分析上述模型的可行性，并做仿真分析。

二、问题的分析

对于问题一，经过分析后我们认为平面是一个连续的集合，为了便于研究，我们将无人机能够活动的平面划分成有限个正方形的网格，这样就可以把无限的、连续的研究对象转化为有限的、离散的，便于计算和研究。另外，这样划分也可以保证计算结果的精度。

另外，要考虑敌方的威胁（这里主要指雷达威胁），那么就要将雷达的“威胁程度”进行量化。在进行了量化之后，就可以考虑构建威胁模型。在上述准备工作完成之后，就要根据量化的数据进行最优航迹的求解。因为我们在本问中所建立的模型求解的是最优航迹，所以可以使用Dijkstra算法进行求解。

问题二要求把模型扩展到三维空间，并同时考虑无人机的操作性能（主要考虑拐弯）和地形因素。经过分析我们认为，问题二是在问题一的基础上,把问题拓展到三维空间里,综合考虑雷达威胁因素、地形因素和飞机本身的因素,建立一个可以确定飞机最优航迹的综合模型。因此，无人机的航迹规划问题可转化为一个带约束的优化问题。如果对规划空间进行三维网格划分，可得到若干节点,从而构成一个网格图，则优化问题的搜索空间就转化为一个离散的空间节点集,而问题的求解也可简单归结为一个求解网络图最短路径的组合优化问题,使得无人机在沿着这些节点所形成的路径上飞行时具有最小代价。对此我们采用一种基于改进蚁群算法的无人机三维航迹规划方法, 将最短路径的信息反馈到系统中作为搜索的指导信号,并改进节点选择方法,以提高应用蚁群算法搜索无人机三维航路的效率，以保证在敌方防御区域内以最小的被发现概率以及可接受的航程到达目标。

对于第三问，我们可以在对相关参数进行适当赋值后，在MATLAB中进行仿真模拟。

三、模型假设

（1）假设附件中所提供雷达威胁的坐标方位表和数字地图真实有效，并在短期内不会改变。

（2）假设无人机的活动范围为题目中所述的20km×20km的区域。

（3）假设所有雷达全天24小时都开机。

（4）假设每个雷达的作用方式完全一致，且无人机具有相同的雷达反射截面。

（5）假设每个雷达之间不存在信息交流，即当一个雷达发现目标时，不会通知其他雷达。

（6）假设无人机在执行任务的过程中不会出现故障。

（7）不考虑地形的变化对气流造成的影响。

四、符号说明

：雷达对无人机的杀伤概率

：突防高度下绝对杀伤区半径

:突防高度下非绝对杀伤区半径

：雷达对无人机的威胁代价

：无人机飞行时的的燃油代价

：无人机飞行时的的高度代价

文中出现的其它符号在用到时另行说明。

五、模型的建立

5.1问题一模型的分析、建立与求解

5.1.1问题一模型的分析

首先，针对本问中的模型，我们做出如下假设：

（1）忽略地形和无人机操作性能等因素的影响，而且认为无人机可以任意角度转弯。

（2）不考虑气候的变化对飞行造成的影响。

（3）飞行所消耗的燃油量和飞行距离成正比。

（4）不考虑无人机起飞和降落时的限制。

问题一要求我们在忽略地形和无人机操作性能等因素影响的条件下，综合考虑敌方威胁，无人机航程等，基于二维平面建立单机单目标的航迹规划模型。考虑到平面是一个连续的集合，为了便于计算，我们将无人机能够活动的平面划分成有限个正方形的网格，这样就可以较好地把无限的、连续的研究对象转化为有限的、离散的，便于计算和研究。另外，这样划分也可以保证计算结果的精度。

经过分析我们认为，要考虑敌方的威胁（这里主要指雷达威胁），那么首先就要将雷达的“威胁程度”进行量化。在进行了量化之后，就可以考虑构建威胁模型。在上述准备工作完成之后，就要根据量化的数据进行最优航迹的求解。因为我们在本问中所建立的模型求解的是最优航迹，所以可以使用Dijkstra算法进行求解。

5.1.2问题一模型的建立

无人机二维航行的代价应包含其所受的威胁代价和燃油代价。我们假定每个雷达的作用方式完全一致，无人机具有相同的雷达反射截面，因此无人机反射雷达回波的强度就与其到雷达的距离的四次方成反比(1/d4)，通常认为,在地对空威胁作用范围内,飞机离其越近,所受到的威胁就越大。以圆盘的方式建模,内侧称为绝对杀伤区,外侧称为非绝对杀伤区。杀伤概率定义为

           （1-1）

其中p(m)表示无人机处于点m时的被击毁概率，

表征突防高度下绝对杀伤区半径,依据情报给定;

表征突防高度下非绝对杀伤区半径,可作为威胁范围估计补偿加入;r表征当前位置到威胁点距离值。对于威胁重叠部分,不同的威胁体对于无人机的杀伤概率计为

, i=1,2,3,4,…,p(m)综合评价杀伤概率则由以下公式求取

威胁模型示意图如下：

为了便于研究，我们将地形图划分为若干等大的方块，我们在求最优航线时，飞机沿着方块的边线和对角线飞行，可以到达与之相邻的任何一个点。那么无人机沿每条边或对角线飞行的雷达威胁代价是飞过该边的积分：         

                  （1-3）

其中，r(t)表示无人机到雷达的距离，p(m)表示雷达的毁伤概率。为了简化计算，我们将每条边均匀地分为6等分，取其中的三个点来代替整条边的代价，这三个点分别是

，

，

，Li是第i边的长度，这样第i条边的雷达威胁代价为：

       （1-4）

其中N为威胁雷达的个数。

是第i条边上的1/2处到第j个雷达的距离。

另一方面，在假定无人机以恒定的速度在同一高度水平飞行的情况下，无人机的燃料消耗和航迹的几何长度成正比，比例常数为c,可表示为

                        （1-5）

综合考虑上面两方面的代价，无人机第i条边的总代价为：

                （1-6）

其中，k为安全性能与燃油性能的系数，可根据任务需求调整k 的大小，k 越接近1 表示越重视燃油消耗情况，越接近0 表示越重视危险性代价。

                   （1-7）

其中M为边的条数。

5.1.3问题一模型的求解

我们采用Dijkstra算法对上述模型进行求解。

Dijkstra算法是典型的单源最短路径算法，用于计算一个节点到其他所有节点的最短路径。基本思路是从出发点VS出发，逐步地向外寻找最短航迹。执行过程中，给每个顶点予以标号，它或者表示从Vs到该点的最短航迹的权，称为P标号（或永久性标号，即某点Vj一旦得到P标号，其标号值在整个计算过程中不改变）；或者表示从Vs到该点的最短航迹的权的上界，称为T标号（也称临时性标号或试探性标号，即某点Vj得到T标号，其标号值是可以改变的）。方法的每一步是修改T标号，并且把某一个具有T标号的点改为具有P标号的点，最多经过n-1步（n为顶点数）就可以求出从Vs到各点的最短航迹。算法流程图如下：

其具体步骤如下：

设

为一个非负权网络，

，开始给起始点

，标上P标号，

（即

），其余各点标上T标号，

。 S为已得P标号的的集合，即

，

。

第一步：设

是刚刚取得P标号的点，考虑所有与

相邻的点

（指以

为始点的弧的中点

），即

，且

的标号为T标号（具有P标号的

不考虑），则修改

的T标号，使得：

，对于所有与

不相邻的点

；

。

第二步：若D中没有T标号的点，则算法停止，即已求出始点到达各点的最短距离。否则：

。若有多个

最小，则任取一个，然后把点

的T标号修改为P标号，转入第一步。本算法的MATLAB程序见附录1。

5.2问题二模型的分析、建立与求解

5.2.1问题二模型的分析

首先，针对本问中的模型，我们做出如下假设：

（1）假设飞机拐弯的最大角度为90°。

（2）飞机的燃料能够使得飞机到达目的地。

（3）雷达对飞机的探测范围有限。

（4）假设无人机能够飞过各种地形。

（5）无人机的航迹规划系统要求得到的航迹能够有效避开敌方雷达的探测和敌方威胁的攻击。

经过分析后我们认为，根据无人机三维航迹规划的具体特征,采用蚁群算法对无人机进行三维航迹规划，可以得到较好的结果。但是注意到在传统的蚁群算法中，在对节点的选择上存在一些尚不完善的因素。因此，综上所述我们建模的整体思想是将最短航迹的信息反馈到蚁群系统中,并且对节点的选择方法进行改进,以便在较短的时间内规划出无人机的三维航迹。

5.2.2问题二模型的建立

影响无人机航迹规划的因素众多，因此无人机的航迹规划问题就转化为一个带约束的优化问题。

我们对规划空间进行三维网格划分，可以得到若干节点,从而构成一个网格图。那么该优化问题的搜索空间就转化为一个离散的空间节点集,因此问题的求解也就归结为“求解网络图最短路径”的组合优化问题，使得无人机在沿着由这些节点所形成的路径上飞行时具有最小代价。

由于要同时考虑无人机的操作性能（主要考虑拐弯）和地形因素，我们需要在模型一建立的燃油代价和雷达威胁代价的基础上引入高度代价和无人机操作性能两个限制因素，这本模型中，燃油代价主要由飞行距离和飞行速度决定的，如果无人机在飞行中保持速度恒定,则燃油代价

与航路的路程L成正比，表示为：

                        （2-1）

高度代价可以表示为：

                          （2-2）

其中

为比例系数。为了降低模型的复杂度，我们对雷达威胁代价的表达式做适当的简化，不妨认为雷达威胁代价的大小与无人机与雷达的距离的四次方成反比，于是雷达威胁代价可以表示为：

                         （2-3）

其中

为比例系数，N为威胁点的个数。

基于以上指标，我们对各个威胁富于一定的权重后，可以建立如下最优模型，其中

为雷达威胁代价的权重，

为燃油代价的权重，其值可根据不同的任务要求进行调整，于是：

             （2-4）

无人机在飞行时受到许多约束条件的限制,主要有如下约束条件：

1. 最小步长

   

   :它限制了无人机在改变姿态时必须直飞的距离,对每一点必须满足下式

                         （2-5）

1. 最大飞行高度

   

   和最小飞行高度

   

   :飞得较高容易被雷达发现,但又要避开山峰等地理威胁。因此,每一点(x,y,z)必须满足下式

                       （2-6）

（3）最远航程Lmax:需要在无人机油耗完之前安全返回,限定了L的最大取值。因此,性能指标中L必须满足下式

                          （2-7）

1. 最大偏航角

   

   和最大爬升角

   

   :它限制了无人机在水平方向和垂直方向偏转的最大角度。设无人机沿着x坐标轴的正方向飞行,当前所在节点的坐标为(x1,y1,z1),下一节点的坐标为(x2,y2,z2),则两个节点的坐标必须满足下式

                 （2-8）

因此,整个航路规划的模型就可以表示为在满足式(2-5)

式(2-8)的条件下,求使式(2-4)最小的一系列节点的集合。

5.2.3问题二模型的求解

对于本模型，我们采用蚁群算法来求解模型。设M是蚁群中蚂蚁的数量,起点为点(1,2,0.3),以目标点与起点的连线为对角线建立一个立方体,此时保证满足式(2-7),否则此次任务不可执行。然后根据地图信息和最高飞行高度将范围适当地进行扩展以满足需要。设X轴方向的长度为Length,Y轴方向的长度为Width,Z轴方向的最高飞行高度为Hight,然后在X、Y和Z轴上分别以dx、dy和dz为等分间隔, 将整个空间划分成 ：

 (Length/dx)×(Width/dy)×(Hight/dz)

个相等的小立方块,各立方块的顶点就是搜索的节点。以

表示任意节点a和其相邻节点b之间的直线距离,以

(1≤i≤Length/dx,1≤j≤Width/dy,1≤k≤Hight/dz)

表示各节点上的信息浓度。蚂蚁

在运动过程中,根据各节点上的信息素的浓度及与相邻节点间的距离决定转移方向, 以

表示在t时刻蚂蚁l从可行节点a转移到b的概率,其计算公式为

                     (2-9)

在上式中，

为节点b上的信息素浓度,反映了蚁群在路径选择时的先验经验,其值越大则表示前面的蚂蚁经过此点的次数越多,于是该节点被选择中的概率越大;

为节点a和节点b之间的能见度，计算式为

                                （2-9）

α和β为信息素浓度

与能见度

的相对重要性的权值;

为节点a的所有相邻节点的集合。

在刚开始第一只蚂蚁位于原点,在地图上满足式(2-5)

式(2-8)的相邻节点中,根据它们的初始化的信息素的值和能见度,按照式(2-9)选择下一节点,然后再以被选中的节点为出发点再按照同样的规则选择下一节点,直到到达终点。当第一只到达终点后,按照式(2-4)来计算这条路径的性能指标,然后再按照下面介绍的调整规则对各节点的信息素浓度值进行调整,同时为了防止陷入局部最优,模拟自然界的真实情况,假设留在各条路径上的信息素随时间挥发,用参数ρ(0<ρ<1)表示信息素的挥发程度,经过m时刻,蚂蚁到达目标点,各路径上的信息素的浓度可根据如下公式进行调整

             （2-10）

式中，

，表示所有蚂蚁在本次循环中留在节点b上的信息素的浓度之和，其中

式中，

为本次循环的性能指标值,为式(2-4)的形式，

、

为性能指标对于信息素的更新的比例系数。

当地图上各节点的信息素调整好后,第二只蚂蚁再出发,按照同样的规则进行路径搜索直到所有的蚂蚁到达终点。当地图上各节点的信息素调整好后,第二只蚂蚁再出发,按照同样的规则进行路径搜索直到所有的蚂蚁到达终点。

基于以上的描述，采用蚁群算法对无人机在三维空间进行航路规划的步骤如下:

（1）初始化信息素浓度矩阵,将危险区域和受地形限制区域中的节点置0,以示惩罚,而其他节点置1。

（2）将M只蚂蚁置于出发点。

（3）每只蚂蚁根据转换规则式(2-9)在满足式(2-5)

式(11)的相邻节点中寻找下一节点,并保存起来,直到到达终点。

蚁群算法的MATLAB代码参见附录二。

六、模型的可行性分析与仿真

6.1模型的可行性分析

无人机的发展至今已有70多年的历史，其军事应用主要是执行各种侦察任务。众所周知自主飞行的能力是无人驾驶飞机所必须具有的。如果要实现无人驾驶飞机的自主飞行,则要求具有相当程度的飞行航迹规划能力。无人机航迹规划技术还存在以下问题:

首先，无人机缺乏相应的有人决策，难以实现机动灵活的驾驶和控制。

其次，无人机的控制由机载设备实现，任务管理在地面控制站上进行，这种控制和管理的分离，存在着由于数据链路处理和传输导致的命令滞后和延迟，所以无法实时规划航迹。

再次，无人机面临的战场环境异常复杂辽阔，规划约束条件众多而且模糊性大，各因素之间存在模糊性及其自身独特的控制和任务方式，使得航迹规划方法的研究更加复杂。

本文第一问在二维的基础上，将雷达威胁和燃油威胁的两个指标进行了量化处理，在量化的过程中，为了降低问题的复杂度，我们适当忽略了一些次要因素。当然，这些因素在实际战场上不能忽略的，比如雷达的功率等。之后我们将二维地形图划分为若干方块，通过Dijkstra算法来搜寻最优路径，进而确定最优航迹。由于该算法的精确程度与划分块的多少有关，因此在实际战场上可以根据战势的变化和复杂度来确定划分块的多少。从这个角度来看，我们的模型在一定程度上是可行的。

本文第二问的模型建立在第一问的基础上，在充分考虑了战场地势和无人机操作性能的基础上进行三维建模，使得模型与实际战场的情况进一步相吻合。然后我们采用了蚁群算法来求解最优航迹。由于算法的复杂度比较高，因此我们通过MATLAB编程来实现最优航迹的求解。此模型更好地模拟了实际战场的情况，因此该模型具有一定的实用参考价值。

6.2模型的仿真

6.2.1、模型一的仿真分析

首先我们将题目中所给的数字地图转化成了用等高线表示的地形图，并将威胁点、出发点和目标点在图上进行了标注，如下图所示：

我们将地图划分成了20*20的方格后采用Dijkstra算法进行了模拟，其中非绝对杀伤区半径

取2.5KM，绝对杀伤区

取2.0KM,燃油代价比例常数C取1，权重系数k取0.3，得到了如下路径图（MATLAB源代码见附录一）    

图中以威胁点为圆心的圆为雷达的威胁区域，红色圆表示绝对杀伤区，蓝色圆表示非绝对杀伤区，黑色粗线为最优航迹之一。由于忽略了高度影响，且划分的方格较少，所以可能出现多条最优航迹的情况。

6.2.2模型二的仿真分析

对于问题二，我们首先用MATLAB将数字地图转化成了等高线地形图，图中圆的意义如前文所述。

然后我们用MATLAB对航线做了仿真。参数值如下：

仿真结果如下图所示（其中黑色粗线代表最优航迹）：

七、模型的评价、改进及推广

Ⅰ.模型的评价

在本题的求解过程中，我们将图论、Dijkstra算法、蚁群算法的相关理论与本题有机地结合起来，系统地解决了基于二维平面建立单机单目标的航迹规划模型，并把模型扩展到三维空间，并同时考虑到无人机的操作性能（主要考虑拐弯）和地形因素。除此之外，我们不但解决了单机在初始位置、终止位置和一些目标任务结点确定之后的航迹规划问题,并且满足了其根据无人机的性能和飞经的地理环境、威胁环境等因素,对已知的目标规划提出满足要求的航迹,以便在实际飞行时可以根据需要进行实时局部修改的需要。

本文模型的优点：

（一）模型一的优越性：

1)基于Dijkstra算法航迹生成算法最大的优点就是利用Dijkstra算法把复杂的空间区域内航迹搜索问题转化成计算机易于实现的路径搜索问题。

2) Dijkstra 算法的时间复杂度为O(n^2)，体现出算法的快速性，这是算法好坏的一个重要品质。

3)根据Dijkstra 算法规划出来的航迹是最优航迹，又体现了算法的另一个重要品质，规划出航迹的最优性。

所以，对于威胁分布已知的环境下的单机无人机航迹规划，基于Dijkstra 算法航迹规划算法是一个性能较好，可行性较高的航迹规划算法。

（二）模型二的优越性：

新发展起来的蚁群算法,由于具有正反馈寻优等优点,在各领域已有很多应用。本文根据无人机三维航路规划的具体特征,采用蚁群算法进行三维航路规划。具体实现是将最短路径的信息反馈到系统中作为搜索的指导信号,并改进了节点选择方法,加快了搜索的效率,也容易找到最优解。仿真结果表明,在考虑无人机的各种性能约束的条件下,用改进后的蚁群算法规划出来的航路有效地避开了威胁。

Ⅱ.对于模型二的改进

在蚁群算法中，当问题规模比较大时,由于信息素挥发因子ρ的存在,使那些从没有被搜索到的节点上的信息素会减少到接近于0,降低了算法的全局搜索能力,而且ρ过小时,以前搜索过的解被选择的可能性过大,也会影响算法的全局搜索能力。故需要限定ρ的范围,制定如下规则

考虑到无人机三维航路规划的特殊性,即偏航角和爬升角是对称的,也就是说,下一节点的Y值和Z值相对于当前节点来说可变大也可变小,最特殊的是保持不变。而在一般寻优问题中不会出现这种问题,故常选择能见度为ηab=1/dab,dab表示节点a和节点b之间的直线距离。这样,Y值和Z值不变的节点能见度显然是最大的,也就是说,它会优先选择保持Y值和Z值不变的节点作为下一节点,这样就陷入了局部最优。

另外,由于无人机飞行空间可能很大,因此三维航路规划是一个维数极大的问题,当蚂蚁处于某X坐标轴上的一点,它需要在一定的范围内选择一个节点作为下一节点,而下一节点又以同样的方式继续选择。如果每一个节点都按给出概率公式来选择,为了搜索到较好的解,不仅需要非常多的蚂蚁数,而且需要循环很多次,这样将使搜索时间更长。鉴于上述两个问题,做如下改进：

(1)对能见度的选择的改进。受神经网络训练中有监督学习方式的启发,将理想输出引入反馈,从而加快神经网络的学习速度,并且能使输出较好地接近于理想输出。直接连接出发点和目标点,显然这条线段是最短的航路,虽然这不一定是可行的航路,但可以将此信息反馈到系统中作为搜索的指导信号,加快了搜索的效率,也容易找到最优解,所以选择ηab=1/d‘ab,d’ab为节点b到节点a的距离和节点b到此线段距离的加权和。

(2)对节点选择方法的改进。将选择概率的值与一个0与1之间的随机数相乘作为新的选择概率,然后以最大的选择概率点作为下一节点。这样,在总的选择概率上来说保持选择概率与原选择概率式一致,又大大减小了搜索的时间,在改进(1)的基础上能够很快地找到优化路径。

八、参考文献

[1]冯琦,周德云,飞行器三维航迹规划算法[J],弹箭与制导学报, 24(4):85-87,2004.

[2]段海滨,王道波,于秀芬,基于云模型的小生境MAX-MIN相遇蚁群算法[J],吉林大学学报:工学版, 36(5):803-808, 2006.

[3]叶文,范洪达,基于改进蚁群算法的飞机低空突防航迹规划[J],飞行力学,22(3):36-38, 2004.

[4]白俊强,柳长安,基于蚁群算法的无人机航迹规划[J], 23(2):35-38, 飞行力学，2005.

[5]陈　谋,肖　健,姜长生，基于改进蚁群算法的无人机三维航迹规划[J], 吉林大学学报:工学版,38（4）：992-995，2008.

九、附录

1.Dijkstra算法计算最短路径的MATLAB源代码：

function [d,DD]=dijkstra(D,s)

%Dijkstra最短路算法Matlab程序用于求从起始点s到其它各点的最短路

%D为赋权邻接矩阵

%d为s到其它各点最短路径的长度

%DD记载了最短路径生成树

[m,n]=size(D);

d=inf.*ones(1,m);

d(1,s)=0;

dd=zeros(1,m);

dd(1,s)=1;

y=s;

DD=zeros(m,m);

DD(y,y)=1;

counter=1;

while length(find(dd==1))<m

     for i=1:m

         if dd(i)==0

             d(i)=min(d(i),d(y)+D(y,i));

         end

     end

     ddd=inf;

     for i=1:m

         if dd(i)==0&&d(i)<ddd

             ddd=d(i);

         end

     end

     yy=find(d==ddd);

     counter=counter+1;

     DD(y,yy(1,1))=counter;

     DD(yy(1,1),y)=counter;

     y=yy(1,1);

     dd(1,y)=1;

end

2. 蚁群算法计算三维最优路径的MATLAB代码：

function [ROUTES,PL,Tau]=ACASP(G,Tau,K,M,S,E,Alpha,Beta,Rho,Q)

% 蚁群算法动态寻路算法

% 输入参数列表

% G 地形图为0，1矩阵，如果为1表示障碍物

% Tau 初始信息素矩阵（认为前面的觅食活动中有残留的信息素） 

% K 迭代次数（指蚂蚁出动多少波） 

% M 蚂蚁个数（每一波蚂蚁有多少个） 

% S 起始点（最短路径的起始点）

% E 终止点（最短路径的目的点）

% Alpha 表征信息素重要程度的参数

% Beta 表征启发式因子重要程度的参数

% Rho 信息素蒸发系数

% Q 信息素增加强度系数

% 输出参数列表

% ROUTES 每一代的每一只蚂蚁的爬行路线 

% PL 每一代的每一只蚂蚁的爬行路线长度

% Tau 输出动态修正过的信息素

%%变量初始化%load

D=G2D(G);

N=size(D,1);     %N表示问题的规模（象素个数） 

MM=size(G,1);

a=1;        %小方格象素的边长

Ex=a*(mod(E,MM)-0.5);     %终止点横坐标

if Ex==-0.5

Ex=MM-0.5;

end

Ey=a*(MM+0.5-ceil(E/MM));        %终止点纵坐标 

Eta=zeros(1,N);         %启发式信息，取为至目标点的直线距离的倒数 

%下面构造启发式信息矩阵 

for i=1:N

if ix==-0.5

ix=MM-0.5;

end

iy=a*(MM+0.5-ceil(i/MM));

if i~=E

Eta(1,i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;

else

Eta(1,i)=100;

end

end

ROUTES=cell(K,M);      %用细胞结构存储每一代的每一只蚂蚁的爬行路线

PL=zeros(K,M)      ;%用矩阵存储每一代的每一只蚂蚁的爬行路线长度

%%、启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁 

for k=1:K

disp(k);

for m=1:M

%% 第一步：状态初始化

W=S;         %当前节点初始化为起始点

Path=S;        %爬行路线初始化

PLkm=0;       %爬行路线长度初始化

TABUkm=ones(1,N);       %禁忌表初始化

TABUkm(S)=0;       %已经在初始点了，因此要排除 

DD=D;        %邻接矩阵初始化

%% 第二步：下一步可以前往的节点 

DW=DD(W,:);

DW1=find(DW

for j=1:length(DW1)

if TABUkm(DW1(j))==0

DW(j)=inf;

end

end

LJD=find(DW

Len_LJD=length(LJD);       %可选节点的个数

%% 觅食停止条件：蚂蚁未遇到食物或者陷入死胡同

while W~=E&&Len_LJD>=1

%% 第三步：转轮赌法选择下一步怎么走

PP=zeros(1,Len_LJD);

for i=1:Len_LJD

PP(i)=(Tau(W,LJD(i))^Alpha)*(Eta(LJD(i))^Beta);

end

PP=PP/(sum(PP));       %建立概率分布

P***=***sum(PP);

Select=find(P***>=rand);

%% 第四步：状态更新和记录

Path=[Path,to_visit];       %路径增加

PLkm=PLkm+DD(W,to_visit);       %路径长度增加

W=to_visit;       %蚂蚁移到下一个节点

for kk=1:N

if TABUkm(kk)==0

DD(W,kk)=inf;

DD(kk,W)=inf;

end

end

TABUkm(W)=0;       %已访问过的节点从禁忌表中删除

for j=1:length(DW1)

if TABUkm(DW1(j))==0

DW(j)=inf;

end

end

LJD=find(DW

Len_LJD=length(LJD)       ;%可选节点的个数

end

%% 第五步：记下每一代每一只蚂蚁的觅食路线和路线长度

ROUTES{k,m}=Path;

if Path(end)==E

PL(k,m)=PLkm;

else

PL(k,m)=inf;

end

end

%% 第六步：更新信息素

Delta_Tau=zeros(N,N);       %更新量初始化

for m=1:M

if PL(k,m) ROUT=ROUTES{k,m};

TS=length(ROUT)-1;       %跳数

PL_km=PL(k,m);

for s=1:TS

x=ROUT(s);

Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;

end

end

end

Tau=(1-Rho).*Tau+Delta_Tau;       %信息素挥发一部分，新增加一部分

end

%%绘图

plotif=1;%是否绘图的控制参数

if plotif==1

%绘收敛曲线

meanPL=zeros(1,K);

minPL=zeros(1,K);

for i=1:K

PLK=PL(i,:);

Nonzero=find(PLK

PLKPLK=PLK(Nonzero);

meanPL(i)=mean(PLKPLK);

minPL(i)=min(PLKPLK);

end

figure(1)

plot(minPL);

hold on

plot(meanPL);

grid on

title('收敛曲线（平均路径长度和最小路径长度）');

xlabel('迭代次数');

ylabel('路径长度');

%绘爬行图

figure(2)

axis([0,MM,0,MM])

for i=1:MM

for j=1:MM

if G(i,j)==1

x1=j-1;y1=MM-i;

x2=j;y2=MM-i;

x4=j-1;y4=MM-i+1;

fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);

hold on

else

x1=j-1;y1=MM-i;

x2=j;y2=MM-i;

x3=j;y3=MM-i+1;

x4=j-1;y4=MM-i+1;

fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);

hold on

end

end

end

hold on

LENROUT=length(ROUT);

Rx=ROUT;

Ry=ROUT;

for ii=1:LENROUT

Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);

if Rx(ii)==-0.5

Rx(ii)=MM-0.5;

end

Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));

end

plot(Rx,Ry)

end

plotif2=1;       %绘各代蚂蚁爬行图

if plotif2==1

figure(3)

axis([0,MM,0,MM])

for i=1:MM

for j=1:MM

if G(i,j)==1

x1=j-1;y1=MM-i;

x2=j;y2=MM-i;

x4=j-1;y4=MM-i+1;

fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);

hold on

else

x1=j-1;y1=MM-i;

x2=j;y2=MM-i;

x3=j;y3=MM-i+1;

x4=j-1;y4=MM-i+1;

fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);

hold on

end

end

end

for k=1:K

PLK=PL(k,:);

minPLK=min(PLK);

pos=find(PLK==minPLK);

m=pos(1);

ROUT=ROUTES{k,m};

LENROUT=length(ROUT);

Rx=ROUT;

Ry=ROUT;

for ii=1:LENROUT

Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);

if Rx(ii)==-0.5

Rx(ii)=MM-0.5;

end

Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));

end

plot(Rx,Ry)

hold on

end

end