第11卷第2期 智能系统学报 Vol.11 No.2 2016年4月 CAAI Transactions on Intelligent Systems Apr.2016 D0I:10.11992/is.201509009 基于蚁群算法的四旋翼航迹规划 莫宏伟，马靖雯 (哈尔滨工程大学自动化学院，黑龙江哈尔滨150001) 摘要：由于四旋翼无人机(UAV)自身的特点和其复杂的飞行环境，考虑到全球定位系统(GPS)定位的精度，在环 境模型方面，建立了一个基于高程图的三维环境模型，减小了碰到障碍物的概率。在规划算法方面，大部分现有的 路径规划算法只能规划二维平面路径，而一般的三维规划算法，大多数运算算法复杂，需要很大的存储空间，同时难 以进行全局路径规划。该蚁群算法具有分布式计算、群体智能等优势，在路径规划上有很大潜力。但在应用基本三 维蚁群算法进行航迹搜索时，两平面直接相连容易使航迹直接穿过障碍物，并且搜索出的航迹节点较多，适应度值 过大。针对这两个问题对算法做出了改进，提出了变主方向搜索策略和简化航迹策略。仿真实验证明改进后的蚁 群算法能够很好地避开障碍物，减小了路径长度，提高了搜索效率。 关键词：四旋翼无人机：航迹规划：三维环境模型：蚁群算法；变主方向搜索策略：简化航迹策略 中图分类号：TP18文献标志码：A文章编号：1673-4785(2016)02-0216-10 中文引用格式：莫宏伟，马靖雯.基于蚊群算法的四旋翼航迹规划[J].智能系统学报，2016,11(2)：216-225. 英文引用格式：MO Hongwei,MA Jingwen.Four-rotor route planning based on the ant colony algorithm[J】.CAAI transactions on intelligent systems,2016,11(2):216-225. Four-rotor route planning based on the ant colony algorithm MO Hongwei,MA Jingwen (College of Automation,Harbin Engineering University,Harbin 150001,China) Abstract:Given a four-rotor unmanned aerial vehicle's characteristics and complex flight environment,as well as the accuracy of the global positioning system in the environment model,the establishment of a 3D environment mod- el based on elevation maps has reduced the probability of encountering obstacles.In terms of planning algorithms, most of the existing path planning algorithms can only plan 2D paths.Numerous 3D planning algorithms have com- plex computations and require much storage space.A global path is also difficult to plan.The advantages of the ant colony algorithm include distributed computing and swarm intelligence.Moreover,this algorithm has great potential in path planning.However,when the fundamental ant colony algorithm is used in a 3D track search,the two direct- ly connected planes easily track straight through obstacles.The track then includes more nodes,and the fitness val- ue becomes too large.The algorithm was improved to address these issues by proposing the strategy of converting the main direction to search and the simplified track strategy.Ant simulation results showed that the improved algorithm could avoid obstacles,reduce path length,and improve search efficiency. Keywords:four-rotor unmanned aerial vehicle;route planning;three-dimensional environment model;ant colony algorithm;strategy of converting the main direction to search;track simplification strategy UAV三维航迹规划区域广，搜素空间巨大，所 收稿日期：2014-04-01. 以要想找到一条最优的飞行轨迹就必定需要占用较 通信作者：莫宏伟.E-mail:honwei2004@126.com 长的收敛时间，同时还需要较大的存储空间，这对大
第 １１ 卷第 ２ 期 智 能 系 统 学 报 Ｖｏｌ．１１ №．２ ２０１６ 年 ４ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ａｐｒ． ２０１６ ＤＯＩ：１０．１１９９２ ／ ｔｉｓ．２０１５０９００９ 基于蚁群算法的四旋翼航迹规划 莫宏伟，马靖雯 （哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 １５０００１） 摘 要：由于四旋翼无人机（ＵＡＶ）自身的特点和其复杂的飞行环境，考虑到全球定位系统（ＧＰＳ）定位的精度，在环 境模型方面，建立了一个基于高程图的三维环境模型，减小了碰到障碍物的概率。 在规划算法方面，大部分现有的 路径规划算法只能规划二维平面路径，而一般的三维规划算法，大多数运算算法复杂，需要很大的存储空间，同时难 以进行全局路径规划。 该蚁群算法具有分布式计算、群体智能等优势，在路径规划上有很大潜力。 但在应用基本三 维蚁群算法进行航迹搜索时，两平面直接相连容易使航迹直接穿过障碍物，并且搜索出的航迹节点较多，适应度值 过大。 针对这两个问题对算法做出了改进，提出了变主方向搜索策略和简化航迹策略。 仿真实验证明改进后的蚁 群算法能够很好地避开障碍物，减小了路径长度，提高了搜索效率。 关键词：四旋翼无人机；航迹规划；三维环境模型；蚁群算法；变主方向搜索策略；简化航迹策略 中图分类号： ＴＰ１８ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１６）０２⁃０２１６⁃１０ 中文引用格式：莫宏伟，马靖雯． 基于蚁群算法的四旋翼航迹规划［Ｊ］． 智能系统学报， ２０１６， １１（２）： ２１６⁃２２５． 英文引用格式：ＭＯ Ｈｏｎｇｗｅｉ， ＭＡ Ｊｉｎｇｗｅｎ． Ｆｏｕｒ⁃ｒｏｔｏｒ ｒｏｕｔｅ ｐｌａｎｎｉｎｇ ｂａｓｅｄ ｏｎ ｔｈｅ ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ［Ｊ］． ＣＡＡＩ ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ｉｎｔｅｌｌｉｇｅｎｔ ｓｙｓｔｅｍｓ， ２０１６， １１（２）： ２１６⁃２２５． Ｆｏｕｒ⁃ｒｏｔｏｒ ｒｏｕｔｅ ｐｌａｎｎｉｎｇ ｂａｓｅｄ ｏｎ ｔｈｅ ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ ＭＯ Ｈｏｎｇｗｅｉ， ＭＡ Ｊｉｎｇｗｅｎ （Ｃｏｌｌｅｇｅ ｏｆ Ａｕｔｏｍａｔｉｏｎ， Ｈａｒｂｉｎ Ｅｎｇｉｎｅｅｒｉｎｇ Ｕｎｉｖｅｒｓｉｔｙ， Ｈａｒｂｉｎ １５０００１， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｇｉｖｅｎ ａ ｆｏｕｒ⁃ｒｏｔｏｒ ｕｎｍａｎｎｅｄ ａｅｒｉａｌ ｖｅｈｉｃｌｅ’ｓ ｃｈａｒａｃｔｅｒｉｓｔｉｃｓ ａｎｄ ｃｏｍｐｌｅｘ ｆｌｉｇｈｔ ｅｎｖｉｒｏｎｍｅｎｔ， ａｓ ｗｅｌｌ ａｓ ｔｈｅ ａｃｃｕｒａｃｙ ｏｆ ｔｈｅ ｇｌｏｂａｌ ｐｏｓｉｔｉｏｎｉｎｇ ｓｙｓｔｅｍ ｉｎ ｔｈｅ ｅｎｖｉｒｏｎｍｅｎｔ ｍｏｄｅｌ， ｔｈｅ ｅｓｔａｂｌｉｓｈｍｅｎｔ ｏｆ ａ ３Ｄ ｅｎｖｉｒｏｎｍｅｎｔ ｍｏｄ⁃ ｅｌ ｂａｓｅｄ ｏｎ ｅｌｅｖａｔｉｏｎ ｍａｐｓ ｈａｓ ｒｅｄｕｃｅｄ ｔｈｅ ｐｒｏｂａｂｉｌｉｔｙ ｏｆ ｅｎｃｏｕｎｔｅｒｉｎｇ ｏｂｓｔａｃｌｅｓ． Ｉｎ ｔｅｒｍｓ ｏｆ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍｓ， ｍｏｓｔ ｏｆ ｔｈｅ ｅｘｉｓｔｉｎｇ ｐａｔｈ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍｓ ｃａｎ ｏｎｌｙ ｐｌａｎ ２Ｄ ｐａｔｈｓ． Ｎｕｍｅｒｏｕｓ ３Ｄ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍｓ ｈａｖｅ ｃｏｍ⁃ ｐｌｅｘ ｃｏｍｐｕｔａｔｉｏｎｓ ａｎｄ ｒｅｑｕｉｒｅ ｍｕｃｈ ｓｔｏｒａｇｅ ｓｐａｃｅ． Ａ ｇｌｏｂａｌ ｐａｔｈ ｉｓ ａｌｓｏ ｄｉｆｆｉｃｕｌｔ ｔｏ ｐｌａｎ． Ｔｈｅ ａｄｖａｎｔａｇｅｓ ｏｆ ｔｈｅ ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ ｉｎｃｌｕｄｅ ｄｉｓｔｒｉｂｕｔｅｄ ｃｏｍｐｕｔｉｎｇ ａｎｄ ｓｗａｒｍ ｉｎｔｅｌｌｉｇｅｎｃｅ． Ｍｏｒｅｏｖｅｒ， ｔｈｉｓ ａｌｇｏｒｉｔｈｍ ｈａｓ ｇｒｅａｔ ｐｏｔｅｎｔｉａｌ ｉｎ ｐａｔｈ ｐｌａｎｎｉｎｇ． Ｈｏｗｅｖｅｒ， ｗｈｅｎ ｔｈｅ ｆｕｎｄａｍｅｎｔａｌ ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ ｉｓ ｕｓｅｄ ｉｎ ａ ３Ｄ ｔｒａｃｋ ｓｅａｒｃｈ， ｔｈｅ ｔｗｏ ｄｉｒｅｃｔ⁃ ｌｙ ｃｏｎｎｅｃｔｅｄ ｐｌａｎｅｓ ｅａｓｉｌｙ ｔｒａｃｋ ｓｔｒａｉｇｈｔ ｔｈｒｏｕｇｈ ｏｂｓｔａｃｌｅｓ． Ｔｈｅ ｔｒａｃｋ ｔｈｅｎ ｉｎｃｌｕｄｅｓ ｍｏｒｅ ｎｏｄｅｓ， ａｎｄ ｔｈｅ ｆｉｔｎｅｓｓ ｖａｌ⁃ ｕｅ ｂｅｃｏｍｅｓ ｔｏｏ ｌａｒｇｅ． Ｔｈｅ ａｌｇｏｒｉｔｈｍ ｗａｓ ｉｍｐｒｏｖｅｄ ｔｏ ａｄｄｒｅｓｓ ｔｈｅｓｅ ｉｓｓｕｅｓ ｂｙ ｐｒｏｐｏｓｉｎｇ ｔｈｅ ｓｔｒａｔｅｇｙ ｏｆ ｃｏｎｖｅｒｔｉｎｇ ｔｈｅ ｍａｉｎ ｄｉｒｅｃｔｉｏｎ ｔｏ ｓｅａｒｃｈ ａｎｄ ｔｈｅ ｓｉｍｐｌｉｆｉｅｄ ｔｒａｃｋ ｓｔｒａｔｅｇｙ． Ａｎｔ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｓｈｏｗｅｄ ｔｈａｔ ｔｈｅ ｉｍｐｒｏｖｅｄ ａｌｇｏｒｉｔｈｍ ｃｏｕｌｄ ａｖｏｉｄ ｏｂｓｔａｃｌｅｓ， ｒｅｄｕｃｅ ｐａｔｈ ｌｅｎｇｔｈ， ａｎｄ ｉｍｐｒｏｖｅ ｓｅａｒｃｈ ｅｆｆｉｃｉｅｎｃｙ． Ｋｅｙｗｏｒｄｓ： ｆｏｕｒ⁃ｒｏｔｏｒ ｕｎｍａｎｎｅｄ ａｅｒｉａｌ ｖｅｈｉｃｌｅ； ｒｏｕｔｅ ｐｌａｎｎｉｎｇ； ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｅｎｖｉｒｏｎｍｅｎｔ ｍｏｄｅｌ； ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ； ｓｔｒａｔｅｇｙ ｏｆ ｃｏｎｖｅｒｔｉｎｇ ｔｈｅ ｍａｉｎ ｄｉｒｅｃｔｉｏｎ ｔｏ ｓｅａｒｃｈ； ｔｒａｃｋ ｓｉｍｐｌｉｆｉｃａｔｉｏｎ ｓｔｒａｔｅｇｙ 收稿日期：２０１４⁃０４⁃０１． 通信作者：莫宏伟．Ｅ⁃ｍａｉｌ：ｈｏｎｗｅｉ２００４＠ １２６．ｃｏｍ． ＵＡＶ 三维航迹规划区域广，搜素空间巨大，所 以要想找到一条最优的飞行轨迹就必定需要占用较 长的收敛时间，同时还需要较大的存储空间，这对大
第2期 莫宏伟，等：基于蚁群算法的四旋冀航迹规划 ·217. 部分无人机实际应用来说是不怎么实际的做法。近 单元格是否可通过，即单元格内障碍物占的面积如 些年来，国外一些专家对航路规划算法归结出了三 果大于一定比例就认为该单元格是不可通过，否则 类主要的方法：传统算法，如栅格法、Voronoi图法； 就认为是可以通过的。第二种方案比第一种方案找 智能优化算法，如遗传算法、蚁群算法：其他算法，如 到最优解的可能性高，但无人机碰到障碍物的可能 动态规划算法。传统算法对障碍物要求比较理想 性也更高，因此选择第一种方案的更多一些。 化，对实际地形规划出的结果影响很大。动态规划 在使用栅格图法进行路径搜索时需要考虑的一 算法，在局部路径上可以达到最优及适用于动态地 个重要问题就是分辨率的问题，即每一个单元格的 图，但不能确保全局路径上也可以实现。智能优化 长宽高值。分辨率过高时计算量过大，不符合实际 算法14，如遗传算法，是一种依据生物界的进化规 应用：而分辨率过低，规划出的最优解偏离理论最优 律演化而来的一种方法，它最重要的一个特点是不 解过大。 受函数求导的限制，在隐并行性方面和全局搜索方 10 面有很大优势，在稳定性方面也有显著提高，但算法 8 效率慢，不能适用于动态、真实的地图。 6 我国在航路规划算法方面也取得了不少的成 果，李翊)将动态标定遗传算法、自适应遗传算法 32 和大变异遗传算法结合在一起，提出了一种改进的 345678910 遗传算法，避免了陷入局部最优解与“早熟”；王玥 图1二维栅格图 等6)提出了基于降落伞型搜索域的变步长航迹点 Fig.1 Two-dimensional raster 搜索和带有威胁信息并归一化后的代价函数，改进 了A*算法，提高了搜索效率：韩超[提出了一种改 进粒子群的航迹规划方法，该方法将UAV的航路规 划问题通过目标转换，形成一个考虑威胁优先，路径 优化其次的单目标航路优化问题，并引入局部搜索 改进粒子群算法求解该问题的收敛性：文献[8]提 出了采用蚁群算法解决机器人三维路径规划的问 题，通过坐标变换对三维环境进行转换，环境中障碍 物抽象成圆形，然后将原点和终点的空间划分成三 图2三维栅格图 维立体网格，在原点和终点之间的每一个平面上选 Fig.2 Three-dimensional raster 定一个点作为航迹节点，从而找到有效的路径，但该 根据自由空间的维数，栅格图法可以分为二维 方法必须首先进行坐标变换，有误差：胡中华不 栅格图法和三维栅格图法。二维栅格图是一个平面 用进行坐标转换，采用在状态转移策略中引人导引 图形如图1，三维栅格图是一个三维模魔方形如图 因子和设定最大航迹节点数的方式来实行航迹规 2。二维栅格图中，设X轴最大值为X,最小值为 划，从而解决标准蚁群算法不易找到目标节点的问 Xn,X轴划分单位大小为Xca,Y轴最大值为 题。到目前为止，我国在航路规划技术方面的研究 Ys,最小值为Yin,Y轴划分单位大小为Yca,则 正朝着实用性、实时性、智能化的方向发展。 二维栅格图总的网格数目N为 本文研究四旋翼无人机航迹规划时对上述文献 (1) 各有借鉴，并将基本蚁群算法做了改进，减少了多余 YGrid 航迹节点，优化了航迹。 对于三维栅格图，设X轴最大值X,最小值 1 基于栅格图法的环境模型构建 Xn,X轴划分单位大小为Xcd,Y轴最大值Y, 最小值Ymin,Y轴划分单位大小为Ycd,Z轴最大 栅格图法[o1山，是把自由空间C划分成一系列 值Zmr,最小值Zn,Z轴划分单位大小为Zcd,则 规则的单元格，根据单元格中是否有障碍物和障碍 二维栅格图总的网格数目N为 物的覆盖面积来判断。机器人路径规划中栅格图法 N=XX x Yan-Yax2,2(2 得到广泛应用21] YGod 在对栅格单元划分时有两种方案：一种是只要 对比式(1)和(2)可以看出二维栅格图和三维栅格 单元格内有障碍物就判定该单元格是不能通过的： 图计算量的差别。 另一种方案是根据单元格内障碍物覆盖面积来判断 本文选用三维栅格图法来划分规划空间。在使用
部分无人机实际应用来说是不怎么实际的做法。 近 些年来，国外一些专家对航路规划算法归结出了三 类主要的方法：传统算法，如栅格法、Ｖｏｒｏｎｏｉ 图法； 智能优化算法，如遗传算法、蚁群算法；其他算法，如 动态规划算法。 传统算法对障碍物要求比较理想 化，对实际地形规划出的结果影响很大。 动态规划 算法，在局部路径上可以达到最优及适用于动态地 图，但不能确保全局路径上也可以实现。 智能优化 算法［１⁃４］ ，如遗传算法，是一种依据生物界的进化规 律演化而来的一种方法，它最重要的一个特点是不 受函数求导的限制，在隐并行性方面和全局搜索方 面有很大优势，在稳定性方面也有显著提高，但算法 效率慢，不能适用于动态、真实的地图。 我国在航路规划算法方面也取得了不少的成 果，李翊［５］将动态标定遗传算法、自适应遗传算法 和大变异遗传算法结合在一起，提出了一种改进的 遗传算法，避免了陷入局部最优解与“早熟”；王玥 等［６］提出了基于降落伞型搜索域的变步长航迹点 搜索和带有威胁信息并归一化后的代价函数，改进 了 Ａ∗算法，提高了搜索效率；韩超［７］提出了一种改 进粒子群的航迹规划方法，该方法将 ＵＡＶ 的航路规 划问题通过目标转换，形成一个考虑威胁优先，路径 优化其次的单目标航路优化问题，并引入局部搜索 改进粒子群算法求解该问题的收敛性；文献［８］ 提 出了采用蚁群算法解决机器人三维路径规划的问 题，通过坐标变换对三维环境进行转换，环境中障碍 物抽象成圆形，然后将原点和终点的空间划分成三 维立体网格，在原点和终点之间的每一个平面上选 定一个点作为航迹节点，从而找到有效的路径，但该 方法必须首先进行坐标变换，有误差；胡中华［９］ 不 用进行坐标转换，采用在状态转移策略中引入导引 因子和设定最大航迹节点数的方式来实行航迹规 划，从而解决标准蚁群算法不易找到目标节点的问 题。 到目前为止，我国在航路规划技术方面的研究 正朝着实用性、实时性、智能化的方向发展。 本文研究四旋翼无人机航迹规划时对上述文献 各有借鉴，并将基本蚁群算法做了改进，减少了多余 航迹节点，优化了航迹。 １ 基于栅格图法的环境模型构建 栅格图法［１０⁃１１］ ，是把自由空间 Ｃ 划分成一系列 规则的单元格，根据单元格中是否有障碍物和障碍 物的覆盖面积来判断。 机器人路径规划中栅格图法 得到广泛应用［１２⁃１３］ 。 在对栅格单元划分时有两种方案：一种是只要 单元格内有障碍物就判定该单元格是不能通过的； 另一种方案是根据单元格内障碍物覆盖面积来判断 单元格是否可通过，即单元格内障碍物占的面积如 果大于一定比例就认为该单元格是不可通过，否则 就认为是可以通过的。 第二种方案比第一种方案找 到最优解的可能性高，但无人机碰到障碍物的可能 性也更高，因此选择第一种方案的更多一些。 在使用栅格图法进行路径搜索时需要考虑的一 个重要问题就是分辨率的问题，即每一个单元格的 长宽高值。 分辨率过高时计算量过大，不符合实际 应用；而分辨率过低，规划出的最优解偏离理论最优 解过大。 图 １ 二维栅格图 Ｆｉｇ．１ Ｔｗｏ⁃ｄｉｍｅｎｓｉｏｎａｌ ｒａｓｔｅｒ 图 ２ 三维栅格图 Ｆｉｇ．２ Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｒａｓｔｅｒ 根据自由空间的维数，栅格图法可以分为二维 栅格图法和三维栅格图法。 二维栅格图是一个平面 图形如图 １，三维栅格图是一个三维模魔方形如图 ２。 二维栅格图中，设 Ｘ 轴最大值为 Ｘｍａｘ ，最小值为 Ｘｍｉｎ ， Ｘ 轴划分单位大小为 ＸＧｒｉｄ ， Ｙ 轴最大值为 Ｙｍａｘ， 最小值为 Ｙｍｉｎ ， Ｙ 轴划分单位大小为 ＹＧｒｉｄ ，则 二维栅格图总的网格数目 Ｎ 为 Ｎ ＝ Ｘｍａｘ － Ｘｍｉｎ ＸＧｒｉｄ × Ｙｍａｘ － Ｙｍｉｎ ＹＧｒｉｄ （１） 对于三维栅格图，设 Ｘ 轴最大值 Ｘｍａｘ ，最小值 Ｘｍｉｎ ， Ｘ 轴划分单位大小为 ＸＧｒｉｄ ， Ｙ 轴最大值 Ｙｍａｘ ， 最小值 Ｙｍｉｎ ， Ｙ 轴划分单位大小为 ＹＧｒｉｄ ， Ｚ 轴最大 值 Ｚｍａｘ ，最小值 Ｚｍｉｎ ， Ｚ 轴划分单位大小为 ＺＧｒｉｄ ，则 二维栅格图总的网格数目 Ｎ 为 Ｎ ＝ Ｘｍａｘ － Ｘｍｉｎ ＸＧｒｉｄ × Ｙｍａｘ － Ｙｍｉｎ ＹＧｒｉｄ × Ｚｍａｘ － Ｚｍｉｎ ＺＧｒｉｄ （２） 对比式（１）和（２）可以看出二维栅格图和三维栅格 图计算量的差别。 本文选用三维栅格图法来划分规划空间。 在使用 第 ２ 期 莫宏伟，等：基于蚁群算法的四旋翼航迹规划 ·２１７·
.218 智能系统学报 第11卷 三维栅格图作为环境模型时就必须考虑到三维栅格的 处。在原蚁群算法及改进算法的基础之上对其进行 分辨率问题，分辨率既不能太大也不能太小。 了变形，在变形时需要考虑到航迹规划的环境模型。 考虑三维栅格分辨率时，首先要考虑的问题就 在上文中，已经选定了以纬度值作为横坐标，以经度 是四旋翼无人机的体积。要保证四旋翼无人机在飞 值作为纵坐标，以高度值作为竖坐标的三维栅格图作 行过程中能够不碰到障碍物，栅格的长、宽、高要大 为环境模型。因为直接在三维栅格图中对所有的栅 于四旋翼的长、宽、高，另外还要考虑到误差。本文 格进行搜索找到最优路径需要的计算量和计算时间 使用的四旋翼无人机的旋翼轴长55cm,起落架高 很大，所以本文采用的方法并不是对所有的栅格进行 约25cm。另外一方面，设定三维栅格分辨率时还 搜索，而是对部分栅格进行搜索以找到最优航迹。 要考虑四旋翼的定位方式。GPS定位是目前最常用 的定位方式之一，在四旋翼无人机上安装一个GPS 40.50 导航模块就可以通过接收GS信号，进而判断四旋 40.45 翼无人机的当前位置，所以本文选择使用GP$定 40.40 位。GPS定位时，纬度上每变化0.001'约为 40.35 1.837m,经度上每变化0.001'约为1.281m,GPS定 位时会存在1~2m的误差。 40.30 综上考虑上述两个方面的因素，本文最终选定 40.25 以纬度值作为横坐标，单位长度为0.002'，约 46.7846.8246.8646.9046.9446.98 3.674m:经度值作为纵坐标，单位长度为0.003'，约 纬度') 3.843m:以地面障碍物的高度作为竖坐标，垂直于 图4投影图 海平面，竖坐标单位长度为1m。这样划分三维栅 Fig.4 Projection map 格单元，就可以兼顾四旋翼的尺寸，又考虑到了GPS 2.1 算法的数学模型 定位的误差，从而可以减小碰到障碍物的概率。现 在进行路径搜索时，已知起始点S所在的栅格 以学校部分地图为基础，绘制了基于三维栅格图的 的坐标值(Xaa,Yam,乙am),终止点所在栅格的坐 三维环境地图。地图所在的位置纬度起始点为45 标值(Xd,Ya,Zd),至于栅格单元的大小则是根 46.7839′，终止点为45°46.9839'；经度起始点为 据上文划分的大小，即Xca=0.003',Yca=0.002', 126°40.2172'，终止点为126°40.5172'：地面水平高 Zcd=1m。航迹规划时需要先选定X方向或y方 度为0。地图大小为(0.002'×100)×(0.003'×100)× 向作为四旋翼无人机运行的主方向。选择主方向的 (1m×40)。为方便MATLAB作图，纬度和经度上省 方法为：比较起始点和终止点横纵坐标的变化值大 略度部分上的值，只写分位，也就是说纬度方向上起 小，即比较(Xaa-Xna）/Xcaa和(Ym-Yd)/Yca 始点和终止点坐标分别为46.7839'和46.9839'，经 的大小，如果(Xt-Xnd)/Xca大于(Yaa- 度方向上起始点和终止点坐标分别为40.2172'和 Ymd)/Ycd,则选择X方向作为主方向；否则选择Y 40.5172'。绘制的三维环境的三维图和投影图分别 方向为主方向。 如图3、4所示。 选定主方向后沿主方向前进方向，例如已经选 定X方向为主方向，沿X轴方向从Xm到X划分 成n=Xm-Xd1+1个平面（如图5所示），编号 40 为Ⅱ，Ⅱ2，Ⅱ3，…，Ⅱ。，那么四旋翼无人机航迹就 40.25 40.30 分成(n-1)段。假设四旋翼无人机运行至第i个 40.35 40.40 40.45经度/0 平面Ⅱ上的一点(X,Y:,Z)处，那么下一个运行 0 40.50 46.96 46.9046.8446.7 的栅格就在Ⅱ1上，下一个栅格坐标选择的具体步 纬度) 骤为：X方向上直接以平面Ⅱ的横坐标作为下一 图3三维图 个节点的横坐标，即新的X坐标值为X+1:Y方向和 Fig.3 Three-dimensional map Z方向坐标值的选择不是直接选择的，而是在平面 Ⅱ，1选择没有障碍物的栅格放入数组Allowed:中； 2 三维蚁群算法 否则被舍弃。然后从中选择一个栅格点作为下一个 三维蚁群算法解决的问题主要是三维航迹规划 运行栅格。平面上任意一个栅格(X,Y,Z)作为下 问题，该模型和基本的蚁群算法模型有很多不同之 一个运行栅格的概率计算为
三维栅格图作为环境模型时就必须考虑到三维栅格的 分辨率问题，分辨率既不能太大也不能太小。 考虑三维栅格分辨率时，首先要考虑的问题就 是四旋翼无人机的体积。 要保证四旋翼无人机在飞 行过程中能够不碰到障碍物，栅格的长、宽、高要大 于四旋翼的长、宽、高，另外还要考虑到误差。 本文 使用的四旋翼无人机的旋翼轴长 ５５ ｃｍ，起落架高 约 ２５ ｃｍ。 另外一方面，设定三维栅格分辨率时还 要考虑四旋翼的定位方式。 ＧＰＳ 定位是目前最常用 的定位方式之一，在四旋翼无人机上安装一个 ＧＰＳ 导航模块就可以通过接收 ＧＰＳ 信号，进而判断四旋 翼无人机的当前位置，所以本文选择使用 ＧＰＳ 定 位。 ＧＰＳ 定 位 时， 纬 度 上 每 变 化 ０． ００１′ 约 为 １．８３７ ｍ，经度上每变化 ０．００１′约为 １．２８１ ｍ，ＧＰＳ 定 位时会存在 １～２ ｍ 的误差。 综上考虑上述两个方面的因素，本文最终选定 以纬 度 值 作 为 横 坐 标， 单 位 长 度 为 ０． ００２′， 约 ３．６７４ ｍ；经度值作为纵坐标，单位长度为 ０．００３′，约 ３．８４３ ｍ；以地面障碍物的高度作为竖坐标，垂直于 海平面，竖坐标单位长度为 １ ｍ。 这样划分三维栅 格单元，就可以兼顾四旋翼的尺寸，又考虑到了 ＧＰＳ 定位的误差，从而可以减小碰到障碍物的概率。 现 以学校部分地图为基础，绘制了基于三维栅格图的 三维环境地图。 地图所在的位置纬度起始点为 ４５° ４６．７８３９′，终止点为 ４５° ４６． ９８３ ９′； 经度起始点为 １２６°４０．２１７ ２′，终止点为 １２６°４０．５１７ ２′；地面水平高 度为 ０。 地图大小为（０．００２′×１００） ×（０．００３′×１００） × （１ｍ×４０）。 为方便 ＭＡＴＬＡＢ 作图，纬度和经度上省 略度部分上的值，只写分位，也就是说纬度方向上起 始点和终止点坐标分别为 ４６．７８３ ９′和４６．９８３ ９′，经 度方向上起始点和终止点坐标分别为４０．２１７２′和 ４０．５１７２′。 绘制的三维环境的三维图和投影图分别 如图 ３、４ 所示。 图 ３ 三维图 Ｆｉｇ．３ Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｍａｐ ２ 三维蚁群算法 三维蚁群算法解决的问题主要是三维航迹规划 问题，该模型和基本的蚁群算法模型有很多不同之 处。 在原蚁群算法及改进算法的基础之上对其进行 了变形，在变形时需要考虑到航迹规划的环境模型。 在上文中，已经选定了以纬度值作为横坐标，以经度 值作为纵坐标，以高度值作为竖坐标的三维栅格图作 为环境模型。 因为直接在三维栅格图中对所有的栅 格进行搜索找到最优路径需要的计算量和计算时间 很大，所以本文采用的方法并不是对所有的栅格进行 搜索，而是对部分栅格进行搜索以找到最优航迹。 图 ４ 投影图 Ｆｉｇ．４ Ｐｒｏｊｅｃｔｉｏｎ ｍａｐ ２．１ 算法的数学模型 在进行路径搜索时，已知起始点 Ｓ 所在的栅格 的坐标值 （Ｘｓｔａｒｔ，Ｙｓｔａｒｔ，Ｚｓｔａｒｔ） ，终止点所在栅格的坐 标值 （Ｘｅｎｄ ，Ｙｅｎｄ ，Ｚｅｎｄ ） ，至于栅格单元的大小则是根 据上文划分的大小，即 ＸＧｒｉｄ ＝ ０．００３′， ＹＧｒｉｄ ＝ ０．００２′， ＺＧｒｉｄ ＝ １ ｍ。 航迹规划时需要先选定 Ｘ 方向或 Ｙ 方 向作为四旋翼无人机运行的主方向。 选择主方向的 方法为：比较起始点和终止点横纵坐标的变化值大 小，即比较 （Ｘｓｔａｒｔ － Ｘｅｎｄ ） ／ ＸＧｒｉｄ 和 （Ｙｓｔａｒｔ － Ｙｅｎｄ ） ／ ＹＧｒｉｄ 的大 小， 如 果 （Ｘｓｔａｒｔ － Ｘｅｎｄ ） ／ ＸＧｒｉｄ 大 于 （Ｙｓｔａｒｔ － Ｙｅｎｄ ） ／ ＹＧｒｉｄ ，则选择 Ｘ 方向作为主方向；否则选择 Ｙ 方向为主方向。 选定主方向后沿主方向前进方向，例如已经选 定 Ｘ 方向为主方向，沿 Ｘ 轴方向从 Ｘｓｔａｒｔ 到 Ｘｅｎｄ 划分 成 ｎ ＝｜ Ｘｓｔａｒｔ － Ｘｅｎｄ ｜ ＋ １ 个平面（如图 ５ 所示），编号 为 Π１ ， Π２ ， Π３ ，…， Πｎ ，那么四旋翼无人机航迹就 分成 （ｎ － １） 段。 假设四旋翼无人机运行至第 ｉ 个 平面 Πｉ 上的一点 （Ｘｉ，Ｙｉ，Ｚｉ） 处，那么下一个运行 的栅格就在 Πｉ＋１ 上，下一个栅格坐标选择的具体步 骤为： Ｘ 方向上直接以平面 Πｉ＋１ 的横坐标作为下一 个节点的横坐标，即新的 Ｘ 坐标值为 Ｘｉ＋１ ； Ｙ 方向和 Ｚ 方向坐标值的选择不是直接选择的，而是在平面 Πｉ＋１ 选择没有障碍物的栅格放入数组 Ａｌｌｏｗｅｄｉ 中； 否则被舍弃。 然后从中选择一个栅格点作为下一个 运行栅格。 平面上任意一个栅格 （Ｘ，Ｙ，Ｚ） 作为下 一个运行栅格的概率计算为 ·２１８· 智 能 系 统 学 报 第 １１ 卷
第2期 莫宏伟，等：基于蚁群算法的四旋冀航迹规划 ·219- T(X,Y,Z)*H(X,Y,Z) 这些离散点的信息素值进行更新，所以对于每一个 ∑x(X,Y,Z)*H(X,Y,Z) 栅格来说都有一个信息素值，这个信息素值就代表 P(X,Y,Z)= (X,Y,Z)∈Allowed: 了该栅格对蚂蚁的吸引程度，每个栅格的信息素值 在蚂蚁经过之后都要进行更新。 0. (X,Y,Z)Allowed, 信息素的更新分为局部更新和全局更新两部 (3) 分。局部更新是指只要有蚂蚁经过某栅格，该栅格 式中：(X,Y,Z)是平面Ⅱ+1上坐标为(X,Y,Z)的 的信息素值就会被更新，更新后栅格的信息素值会 栅格的信息素值。H(X,Y,Z)是平面Ⅱ+1上坐标为 被减少，这样这个栅格在以后的搜索中被选中的概 (X,Y,Z)的栅格的启发函数，其计算公式为 率就被降低，而相应地增加其他未被搜索的栅格被 H(X,Y,Z)= 搜索的概率，这样就可以达到全局搜素的目的。局 D(X,Y,Z)1×S(X,Y,Z)2×Q(X,Y,Z)3 部搜索的信息素更新公式为 (4) Tx.2=(1-g)Tx,2 (8) 式中：D(X,Y,Z)是当前点和(X,Y,Z)的路径长 式中：5表示信息素衰减系数，Txy,z表示栅格(X, 度，这可以促使蚂蚁尽可能选则距离当前点最近的 Y,Z)的信息素值。 点，计算公式为 全局信息素更新是指蚂蚁完成一条航迹搜索 D(X,Y,Z)=√/(X:-X)2+(Y:-Y)2+(Z,-Z)2 时，计算该路径的适应度值，从现有的搜索到的路径 中选择出最短的航迹，更新适应度值最小的航迹所 (5) 经过的所有栅格的信息素值，信息素更新公式为 S(X,Y,Z)表示安全性因素，促使蚂蚁选择安全点。 (9) 当前栅格(X,Y,Z)和(X,Y,Z)是不可连通的， Tx.v.z =(1 -p)Tx.Y.z +pATx..z 或者栅格(X,Y,Z)内有障碍物，则称该栅格是不可 x..=min(length(m)) (10) 达的。S(X,Y,Z)的计算公式为 式中：length(m)表示蚂蚁m经过的路径长度；p表 s(x,y,z)=. (X,Y,Z)是可达点 (6) 示信息素挥发系数；K为系数。 0. 其他 2.2算法的主要流程 式中：Q(X,Y,Z)为栅格(X,Y,Z)到终止栅格 在充分考虑了评价函数、环境模型和三维蚁群 (Xa,Ymd,Za)的距离，Q(X,Y,Z)可以促使蚂蚁 算法模型这些内容之后，下一步需要考虑的就是航 选择离目标栅格更近的栅格，其计算公式为 迹规划实现的问题。本文中的程序实现主要分为参 Q(X,Y,Z)= 数初始化设置、航迹搜索和图形绘制3个主要部分， √/(X:-Xd)2+(Y:-Ya)2+(Z:-Zd) 其具体每一步如下所述。 (7) 1)参数初始化设置 式中：ω1、w2、0,是系数，其大小代表了上述各因素 在进行航迹规划实现时，首先要确定的就是各 的重要性程度，系数值越大代表该项越重要；否则说 项参数设置问题。这些参数设置包括起始点的确 明该项越不重要。 定、主方向的选取、种群数的确定、迭代次数的选择 42 航迹搜寻范围选取、信息素初始化设置。 i+1 ①起始点的确定。当把四旋翼无人机放置在规 划空间中的某一点时，放置四旋翼的位置不一定恰 好就是栅格图中栅格的位置，这时就需要把该四旋 翼无人机所在的栅格的栅格坐标作为航迹规划的起 始点。那么，三维栅格地图原点坐标为(Xc, Yd,Gridan）,四旋翼的放置位置(St,Sm,Sa) 主方向 和其所在栅格的栅格坐标位置(Xa,Yat,乙)的 图5主方向选取 关系为 Fig.5 Select the main direction =ceil 蚁群算法中蚂蚁是根据信息素浓度来进行搜索 Su-Xcridsant）xXca+X cndn 路径的，在每走完一段或全部路径时，就要对信息素 (11) 值进行更新，所以信息素初始值设定和更新对于蚁 群算法是否能够成功搜索具有重要影响。本文把信 息素值存储在三维空间一系列离散的点中，然后对 (12)
Ｐ（Ｘ，Ｙ，Ｚ） ＝ τ（Ｘ，Ｙ，Ｚ）∗Ｈ（Ｘ，Ｙ，Ｚ） ∑τ（Ｘ，Ｙ，Ｚ）∗Ｈ（Ｘ，Ｙ，Ｚ） ， （Ｘ，Ｙ，Ｚ） ∈ Ａｌｌｏｗｅｄｉ ０， （Ｘ，Ｙ，Ｚ） ∉ Ａｌｌｏｗｅｄｉ ì î í ï ï ï ï ïï （３） 式中： τ（Ｘ，Ｙ，Ｚ） 是平面 Πｉ＋１ 上坐标为 （Ｘ，Ｙ，Ｚ） 的 栅格的信息素值。 Ｈ（Ｘ，Ｙ，Ｚ） 是平面 Πｉ＋１ 上坐标为 （Ｘ，Ｙ，Ｚ） 的栅格的启发函数，其计算公式为 Ｈ（Ｘ，Ｙ，Ｚ） ＝ Ｄ （Ｘ，Ｙ，Ｚ） ω１ × Ｓ （Ｘ，Ｙ，Ｚ） ω２ × Ｑ （Ｘ，Ｙ，Ｚ） ω３ （４） 式中： Ｄ（Ｘ，Ｙ，Ｚ） 是当前点和 （Ｘ，Ｙ，Ｚ） 的路径长 度，这可以促使蚂蚁尽可能选则距离当前点最近的 点，计算公式为 Ｄ（Ｘ，Ｙ，Ｚ） ＝ （Ｘｉ － Ｘ） ２ ＋ （Ｙｉ － Ｙ） ２ ＋ （Ｚｉ － Ｚ） ２ （５） Ｓ（Ｘ，Ｙ，Ｚ） 表示安全性因素，促使蚂蚁选择安全点。 当前栅格 （Ｘｉ，Ｙｉ，Ｚｉ） 和 （Ｘ，Ｙ，Ｚ） 是不可连通的， 或者栅格 （Ｘ，Ｙ，Ｚ） 内有障碍物，则称该栅格是不可 达的。 Ｓ（Ｘ，Ｙ，Ｚ） 的计算公式为 Ｓ（Ｘ，Ｙ，Ｚ） ＝ １， （Ｘ，Ｙ，Ｚ） 是可达点 {０， 其他 （６） 式中： Ｑ（Ｘ，Ｙ，Ｚ） 为 栅 格 （Ｘ，Ｙ，Ｚ） 到 终 止 栅 格 （Ｘｅｎｄ ，Ｙｅｎｄ ，Ｚｅｎｄ ） 的距离， Ｑ（Ｘ，Ｙ，Ｚ） 可以促使蚂蚁 选择离目标栅格更近的栅格，其计算公式为 Ｑ（Ｘ，Ｙ，Ｚ） ＝ （Ｘｉ － Ｘｅｎｄ ） ２ ＋ （Ｙｉ － Ｙｅｎｄ ） ２ ＋ （Ｚｉ － Ｚｅｎｄ ） ２ （７） 式中： ω１ 、 ω２ 、 ω３ 是系数，其大小代表了上述各因素 的重要性程度，系数值越大代表该项越重要；否则说 明该项越不重要。 图 ５ 主方向选取 Ｆｉｇ．５ Ｓｅｌｅｃｔ ｔｈｅ ｍａｉｎ ｄｉｒｅｃｔｉｏｎ 蚁群算法中蚂蚁是根据信息素浓度来进行搜索 路径的，在每走完一段或全部路径时，就要对信息素 值进行更新，所以信息素初始值设定和更新对于蚁 群算法是否能够成功搜索具有重要影响。 本文把信 息素值存储在三维空间一系列离散的点中，然后对 这些离散点的信息素值进行更新，所以对于每一个 栅格来说都有一个信息素值，这个信息素值就代表 了该栅格对蚂蚁的吸引程度，每个栅格的信息素值 在蚂蚁经过之后都要进行更新。 信息素的更新分为局部更新和全局更新两部 分。 局部更新是指只要有蚂蚁经过某栅格，该栅格 的信息素值就会被更新，更新后栅格的信息素值会 被减少，这样这个栅格在以后的搜索中被选中的概 率就被降低，而相应地增加其他未被搜索的栅格被 搜索的概率，这样就可以达到全局搜素的目的。 局 部搜索的信息素更新公式为 τＸ，Ｙ，Ｚ ＝ （１ － ζ）τＸ，Ｙ，Ｚ （８） 式中： ζ 表示信息素衰减系数， τＸ，Ｙ，Ｚ 表示栅格 （Ｘ， Ｙ，Ｚ） 的信息素值。 全局信息素更新是指蚂蚁完成一条航迹搜索 时，计算该路径的适应度值，从现有的搜索到的路径 中选择出最短的航迹，更新适应度值最小的航迹所 经过的所有栅格的信息素值，信息素更新公式为 τＸ，Ｙ，Ｚ ＝ （１ － ρ）τＸ，Ｙ，Ｚ ＋ ρΔτＸ，Ｙ，Ｚ （９） ΔτＸ，Ｙ，Ｚ ＝ Ｋ ｍｉｎ（ｌｅｎｇｔｈ（ｍ）） （１０） 式中： ｌｅｎｇｔｈ（ｍ） 表示蚂蚁 ｍ 经过的路径长度； ρ 表 示信息素挥发系数； Ｋ 为系数。 ２．２ 算法的主要流程 在充分考虑了评价函数、环境模型和三维蚁群 算法模型这些内容之后，下一步需要考虑的就是航 迹规划实现的问题。 本文中的程序实现主要分为参 数初始化设置、航迹搜索和图形绘制 ３ 个主要部分， 其具体每一步如下所述。 １）参数初始化设置 在进行航迹规划实现时，首先要确定的就是各 项参数设置问题。 这些参数设置包括起始点的确 定、主方向的选取、种群数的确定、迭代次数的选择、 航迹搜寻范围选取、信息素初始化设置。 ①起始点的确定。 当把四旋翼无人机放置在规 划空间中的某一点时，放置四旋翼的位置不一定恰 好就是栅格图中栅格的位置，这时就需要把该四旋 翼无人机所在的栅格的栅格坐标作为航迹规划的起 始点。 那么，三维栅格地图原点坐标为 （ＸＧｒｉｄｓｔａｒｔ， ＹＧｒｉｄｓｔａｒｔ，ＺＧｒｉｄｓｔａｒｔ） ，四旋翼的放置位置 （Ｓｌａｔ，Ｓｌｏｎ ，Ｓｈ ） 和其所在栅格的栅格坐标位置 （Ｘｓｔａｒｔ，Ｙｓｔａｒｔ，Ｚｓｔａｒｔ） 的 关系为 Ｘｓｔａｒｔ ＝ ｃｅｉｌ（ Ｓｌａｔ － ＸＧｒｉｄｓｔａｒｔ ＸＧｒｉｄ ） × ＸＧｒｉｄ ＋ ＸＧｒｉｄｓｔａｒｔ （１１） Ｙｓｔａｒｔ ＝ ｃｅｉｌ（ Ｓｌｏｎ － ＹＧｒｉｄｓｔａｒｔ ＹＧｒｉｄ ） × ＹＧｒｉｄ ＋ ＹＧｒｉｄｓｔａｒｔ （１２） 第 ２ 期 莫宏伟，等：基于蚁群算法的四旋翼航迹规划 ·２１９·
·220· 智能系统学报 第11卷 S.Zomsbum x ZoZo(13) Allowed,中按照轮盘赌法选出一个可行的栅格作为 Z.an ceil( 平面Ⅱ，1上的航迹节点。下一步就是对平面Ⅱ：上 式中：ceil表示向正无穷方向取整。 的节点进行局部信息素更新。 ②主方向选取。主方向的选取主要就是以纬度 对上述内容重复进行，直到到达平面Ⅱ。-1，平 方向为主方向还是以经度方向为主方向的问题。选 面Ⅱ上的节点(X-1,Y。-1,乙.-)直接与平面Ⅱ，上 择两个方向之中栅格变化数最多的方向作为四旋翼 的节点(Xd,Yd,Z)即终止点相连，这样就构成 无人机航迹规划主方向。主方向选定后还要知道主 了一条完整的航迹。 方向上从起始点到终止点坐标值变化趋势，即坐标 按照适应度值函数计算每条航迹的适应度值， 值是增加还是减少。如果坐标值增加，那么当从当 比较找出最小适应度值，而最小适应度对应的航迹 前平面到下一平面上主方向上坐标值加1个单位 即为当前最优航迹。 值：否则坐标值减1个单位值。假设A方向为主方 将上述过程迭代V次，这样就找到了迭代N次 向，那么从Ⅱ到Ⅱ+坐标变化为 的最优航迹。 (A:AGnd, Aat≤Aend 3)图形绘制 AA:-Acmd (14) 其他 利用已经测试到的环境模型的经度、纬度和高 ③种群数选取。 度数据绘制三维环境地图，然后在三维环境地图中 ④迭代次数的选择。 绘制最优航迹。到此为止，本文就完成了基于蚁群 ⑤航迹搜寻范围选取。如果主方向是A,非主 算法的航迹规划与仿真。 方向的那个纬度或经度方向是B,那么从平面Ⅱ 2.3算法的改进 到平面Ⅱ+1，对于B方向上以Y为中心从y:-bc 2.3.1变主方向搜索策略 到Y:+bcmm范围内的点都是可选择作为Y:+1点。同 基本三维蚁群算法进行航迹规划时，将Ⅱ。-1平 样，对于Z方向上来说以Z,为中心从Z,-hcs到 面上节点和Ⅱ，平面上的节点即终点直接相连容易 穿过障碍物。因此首先需要判断两点间的连通性， Z.+hcm范围内的点都是可选择作为Z1点。 ⑥信息素初始化。在初始时刻，把三维栅格环 其具体过程如下。 境地图中的所有栅格信息素值设置为固定值。 假设在三维空间存在两点(X,Y,Z)和(X2, 2)航迹搜索 Y2,Z2)。两点间的空间直线方程为 航迹搜索前首先需要确定航迹所选的所有的节 X-X Y-Y Z-Z (15) 点都要在规划空间中，另外在航迹高度上也要有一 X2-X1 Y2-Y Z2-Z 定的限制。经过观察发现，从地面到距地面2m范 由式(15)可知，若已知X的值，那么 围内障碍物主要是人和车，这些障碍物多是动态的， (Y=(Y2-Y)×(X-X)/(X2-X)+Y 四旋翼在这个高度范围内飞行容易危害行人和车的 Z=(Z2-Z)×(X-X)/(X2-X)+Z 正常行动。在距地面2~5m的范围内主要是较矮 (16) 的树木，在这个空间中进行飞行时也较容易碰到障 同理，若已知Y的值，那么 碍物，但总的来说还算安全。本文所研究的航迹规 X=(X2-X)×(Y-Y)/(Y2-Y)+X: 划主要是在高于地面2m的范围内进行的。如果无 Z=(Z2-Z1)×(Y-Y)/(Y2-Y)+Z 人机起始点在地面就先让四旋翼飞行距地面一定高 (17) 度H,飞行过程中也限制四旋翼飞行在这个高度 想要知道空间中两点是否连通，首先要判断空间 之上。 直线经过哪些栅格，然后判断这些栅格所在的经纬度 在航迹搜索过程中，假设PopNum只蚂蚁中的 位置的障碍物高度和直线高度关系，如果障碍物高度 第k只蚂蚁已运行至平面Ⅱ上的点(X:,Y,Z:)处， 高于直线高度，那么这条直线经过障碍物，也就是说 搜索在平面Ⅱ1上以(X,Y,乙)为中心的 空间中两点是不可连通的，否则就是可连通的。 count=(2×bCmas+1)×(2×hcmx+1)个点。将 对于空间两点，根据两点的X坐标和Y坐标间 count个栅格中所有的可行栅格，即没有障碍物的栅 大小关系，可以分为以下6种关系： 格放入数组Allowed中。如果数组Allowed:为空， a)X1=X2,Y,和Y2关系任意； 那么将当前点(X,Y:,Z)的Z:坐标值加1，即当前 b)Y1=Y2,X,和X2关系任意； 点坐标变为(X:,Y:,Z:+1),重新搜索平面上的可 c)X1>X2,Y,>Y2: 行栅格，直到数组Allowed:不为空。从数组 d)X1>X2,Y,<Y2:
Ｚｓｔａｒｔ ＝ ｃｅｉｌ（ Ｓｈ － ＺＧｒｉｄｓｔａｒｔ ＺＧｒｉｄ ） × ＺＧｒｉｄ ＋ ＺＧｒｉｄｓｔａｒｔ （１３） 式中：ｃｅｉｌ 表示向正无穷方向取整。 ②主方向选取。 主方向的选取主要就是以纬度 方向为主方向还是以经度方向为主方向的问题。 选 择两个方向之中栅格变化数最多的方向作为四旋翼 无人机航迹规划主方向。 主方向选定后还要知道主 方向上从起始点到终止点坐标值变化趋势，即坐标 值是增加还是减少。 如果坐标值增加，那么当从当 前平面到下一平面上主方向上坐标值加 １ 个单位 值；否则坐标值减 １ 个单位值。 假设 Ａ 方向为主方 向，那么从 Πｉ 到 Πｉ＋１ 坐标变化为 Ａｉ＋１ ＝ Ａｉ ＋ ＡＧｒｉｄ ， Ａｓｔａｒｔ ≤ Ａｅｎｄ Ａｉ { － ＡＧｒｉｄ ， 其他 （１４） ③种群数选取。 ④迭代次数的选择。 ⑤航迹搜寻范围选取。 如果主方向是 Ａ ，非主 方向的那个纬度或经度方向是 Ｂ ，那么从平面 Πｉ 到平面 Πｉ＋１ ，对于 Ｂ 方向上以 Ｙｉ 为中心从 Ｙｉ － ｂｃｍａｘ 到 Ｙｉ ＋ ｂｃｍａｘ 范围内的点都是可选择作为 Ｙｉ＋１ 点。 同 样，对于 Ｚ 方向上来说以 Ｚｉ 为中心从 Ｚｉ －ｈｃｍａｘ 到 Ｚｉ ＋ ｈｃｍａｘ 范围内的点都是可选择作为 Ｚｉ＋１ 点。 ⑥信息素初始化。 在初始时刻，把三维栅格环 境地图中的所有栅格信息素值设置为固定值。 ２）航迹搜索 航迹搜索前首先需要确定航迹所选的所有的节 点都要在规划空间中，另外在航迹高度上也要有一 定的限制。 经过观察发现，从地面到距地面 ２ ｍ 范 围内障碍物主要是人和车，这些障碍物多是动态的， 四旋翼在这个高度范围内飞行容易危害行人和车的 正常行动。 在距地面 ２ ～ ５ ｍ 的范围内主要是较矮 的树木，在这个空间中进行飞行时也较容易碰到障 碍物，但总的来说还算安全。 本文所研究的航迹规 划主要是在高于地面 ２ ｍ 的范围内进行的。 如果无 人机起始点在地面就先让四旋翼飞行距地面一定高 度 Ｈｍｉｎ ， 飞行过程中也限制四旋翼飞行在这个高度 之上。 在航迹搜索过程中，假设 ＰｏｐＮｕｍ 只蚂蚁中的 第 ｋ 只蚂蚁已运行至平面 Πｉ 上的点 （Ｘｉ，Ｙｉ，Ｚｉ） 处， 搜索 在 平 面 Πｉ＋１ 上 以 （Ｘｉ＋１ ，Ｙｉ，Ｚｉ） 为 中 心 的 ｃｏｕｎｔ ＝（２ × ｂｃｍａｘ ＋ １） × （２ × ｈｃｍａｘ ＋ １） 个点。 将 ｃｏｕｎｔ 个栅格中所有的可行栅格，即没有障碍物的栅 格放入数组 Ａｌｌｏｗｅｄｉ 中。 如果数组 Ａｌｌｏｗｅｄｉ 为空， 那么将当前点 （Ｘｉ，Ｙｉ，Ｚｉ） 的 Ｚｉ 坐标值加 １，即当前 点坐标变为 （Ｘｉ，Ｙｉ，Ｚｉ ＋ １） ，重新搜索平面上的可 行栅 格， 直 到 数 组 Ａｌｌｏｗｅｄｉ 不 为 空。 从 数 组 Ａｌｌｏｗｅｄｉ 中按照轮盘赌法选出一个可行的栅格作为 平面 Πｉ＋１ 上的航迹节点。 下一步就是对平面 Πｉ 上 的节点进行局部信息素更新。 对上述内容重复进行，直到到达平面 Πｎ－１ ，平 面 Πｉ 上的节点 （Ｘｎ－１ ，Ｙｎ－１ ，Ｚｎ－１ ） 直接与平面 Πｎ 上 的节点 （Ｘｅｎｄ ，Ｙｅｎｄ ，Ｚｅｎｄ ） 即终止点相连，这样就构成 了一条完整的航迹。 按照适应度值函数计算每条航迹的适应度值， 比较找出最小适应度值，而最小适应度对应的航迹 即为当前最优航迹。 将上述过程迭代 Ｎ 次，这样就找到了迭代 Ｎ 次 的最优航迹。 ３）图形绘制 利用已经测试到的环境模型的经度、纬度和高 度数据绘制三维环境地图，然后在三维环境地图中 绘制最优航迹。 到此为止，本文就完成了基于蚁群 算法的航迹规划与仿真。 ２．３ 算法的改进 ２．３．１ 变主方向搜索策略 基本三维蚁群算法进行航迹规划时，将 Πｎ－１ 平 面上节点和 Πｎ 平面上的节点即终点直接相连容易 穿过障碍物。 因此首先需要判断两点间的连通性， 其具体过程如下。 假设在三维空间存在两点 （Ｘ１ ，Ｙ１ ，Ｚ１ ） 和 （Ｘ２ ， Ｙ２ ，Ｚ２ ） 。 两点间的空间直线方程为 Ｘ － Ｘ１ Ｘ２ － Ｘ１ ＝ Ｙ － Ｙ１ Ｙ２ － Ｙ１ ＝ Ｚ － Ｚ１ Ｚ２ － Ｚ１ （１５） 由式（１５）可知，若已知 Ｘ 的值，那么 Ｙ ＝ （Ｙ２ － Ｙ１ ） × （Ｘ － Ｘ１ ） ／ （Ｘ２ － Ｘ１ ） ＋ Ｙ１ Ｚ ＝ （Ｚ２ － Ｚ１ ） × （Ｘ － Ｘ１ ） ／ （Ｘ２ － Ｘ１ ） ＋ Ｚ１ { （１６） 同理，若已知 Ｙ 的值，那么 Ｘ ＝ （Ｘ２ － Ｘ１ ） × （Ｙ － Ｙ１ ） ／ （Ｙ２ － Ｙ１ ） ＋ Ｘ１ Ｚ ＝ （Ｚ２ － Ｚ１ ） × （Ｙ － Ｙ１ ） ／ （Ｙ２ － Ｙ１ ） ＋ Ｚ１ { （１７） 想要知道空间中两点是否连通，首先要判断空间 直线经过哪些栅格，然后判断这些栅格所在的经纬度 位置的障碍物高度和直线高度关系，如果障碍物高度 高于直线高度，那么这条直线经过障碍物，也就是说 空间中两点是不可连通的，否则就是可连通的。 对于空间两点，根据两点的 Ｘ 坐标和 Ｙ 坐标间 大小关系，可以分为以下 ６ 种关系： ａ） Ｘ１ ＝ Ｘ２ ，Ｙ１ 和 Ｙ２ 关系任意； ｂ） Ｙ１ ＝ Ｙ２ ，Ｘ１ 和 Ｘ２ 关系任意； ｃ） Ｘ１ ＞ Ｘ２ ，Ｙ１ ＞ Ｙ２ ； ｄ） Ｘ１ ＞ Ｘ２ ，Ｙ１ ＜ Ｙ２ ； ·２２０· 智 能 系 统 学 报 第 １１ 卷
第2期 莫宏伟，等：基于蚁群算法的四旋冀航迹规划 .221· e)X,Y2; 9 f)XY2 都是直线经过的栅格。所以，当需要判断三维直线 Fig.10 X X2,Y1>Y2 航迹节点过多意味着四旋翼无人机在飞行的过 Fig.8 X>X2,Y Y2 程中需要转折次数过多，采用一定的简化策略来减 少航迹节点既能减小适应度值还能减小航迹主要思 路是：假设变主方向搜素策略所搜索出的一条可行 航迹为route=(r1,2,…,In)。首先把节点r1放入 新航迹数组newroute中，判断1与rn的连通性，如 果是连通的，把r1放入数组newroute中；否则判断 ■)】 1与r。-1的连通性，直到找到一个点r与r,是连通 2345678910 的。对进行相同的操作，直到终止点。 图9X1>X2,YX2,Y Y2 3三维粒子群算法 3.1粒子群算法的主要流程 粒子群算法进行航迹搜索时分以下几个步骤：
ｅ） Ｘ１ ＜ Ｘ２ ，Ｙ１ ＞ Ｙ２ ； ｆ） Ｘ１ ＜ Ｘ２ ，Ｙ１ ＜ Ｙ２ 。 图 ６～１１ 绘制了三维空间两点间直线关系在二 维平面上的投影的 ６ 种情况。 分析这 ６ 种情况可以 发现，当直线与栅格的横向直线相交时，这个交点的 上下两个栅格都是直线经过的栅格；当直线与栅格 的纵向直线相交时，那么这个交点的左右两个栅格 都是直线经过的栅格。 所以，当需要判断三维直线 投影经过哪些栅格时，只要找出直线与栅格的横纵 坐标的交点就可以。 图 ６ Ｘ１ ＝ Ｘ２ ，Ｙ１ 和 Ｙ２ 关系任意 Ｆｉｇ．６ Ｘ１ ＝ Ｘ２ ，Ｙ１ ａｎｄ Ｙ２ ａｒｂｉｔｒａｒｙ ｒｅｌａｔｉｏｎｓｈｉｐ 图 ７ Ｙ１ ＝ Ｙ２ ，Ｘ１ 和 Ｘ２ 关系任意 Ｆｉｇ．７ Ｙ１ ＝ Ｙ２ ，Ｘ１ ａｎｄ Ｘ２ ａｒｂｉｔｒａｒｙ ｒｅｌａｔｉｏｎｓｈｉｐ 图 ８ Ｘ１ ＞ Ｘ２ ，Ｙ１ ＞ Ｙ２ Ｆｉｇ．８ Ｘ１ ＞ Ｘ２ ，Ｙ１ ＞ Ｙ２ 图 ９ Ｘ１ ＞ Ｘ２ ，Ｙ１ ＜ Ｙ２ Ｆｉｇ．９ Ｘ１ ＞ Ｘ２ ，Ｙ１ ＜ Ｙ２ 图 １０ Ｘ１ ＜ Ｘ２ ，Ｙ１ ＞ Ｙ２ Ｆｉｇ．１０ Ｘ１ ＜ Ｘ２ ，Ｙ１ ＞ Ｙ２ 图 １１ Ｘ１ ＜ Ｘ２ ，Ｙ１ ＜ Ｙ２ Ｆｉｇ．１１ Ｘ１ ＜ Ｘ２ ，Ｙ１ ＜ Ｙ２ 变主方向搜索策略的基本思想是：如果 Πｎ－１ 平 面上的节点和 Πｎ 平面上终点是不连通的，那么就 以 Πｎ－１ 平面上的节点 （Ｘｎ－１ ，Ｙｎ－１ ，Ｚｎ－１ ） 为起始点， 以 Πｎ 平面上终点 （Ｘｅｎｄ ，Ｙｅｎｄ ，Ｚｅｎｄ ） 为终止点，再次 搜索航迹。 记第一次搜索的主方向上平面划分为 Π１，１ ， Π１，２ ，…， Π１，ｎ１ ，搜索节点依次为 （Ｘ１，１ ，Ｙ１，１ ， Ｚ１，１ ） ， （Ｘ１，２ ，Ｙ１，２ ，Ｚ１，２ ） ， …， （Ｘ１，ｎ１－１ ，Ｙ１，ｎ１－１ ， Ｚ１，ｎ１－１ ） ；同理，第 ｍ 次搜索的主方向上平面划分为 Πｍ，１ ， Πｍ，２ ， …， Πｍ，ｎｍ ， 搜索节点依次为 （Ｘｍ，１ ， Ｙｍ，１ ，Ｚｍ，１ ） ， （Ｘｍ，２ ，Ｙｍ，２ ，Ｚｍ，２ ） ， …， （Ｘｍ，ｎ１－１ ， Ｙｍ，ｎ１－１ ，Ｚｍ，ｎ１－１ ） 。 最 终 的 三 维 航 迹 为 （Ｘ１，１ ，Ｙ１，１ ， Ｚ１，１ ） ， …， （Ｘ１，ｎ１－１ ，Ｙ１，ｎ１－１ ，Ｚ１，ｎ１－１ ） ， （Ｘ２，２ ，Ｙ２，２ ， Ｚ２，２ ） ，…， （Ｘ２，ｎ１－１ ，Ｙ２，ｎ１－１ ，Ｚ２，ｎ１－１ ） ，…， （Ｘｍ，２ ，Ｙｍ，２ ， Ｚｍ，２ ） ， …， （Ｘｍ，ｎｍ－１ ，Ｙｍ，ｎｍ－１ ，Ｚｍ，ｎｍ－１ ） ， （Ｘｅｎｄ ，Ｙｅｎｄ ， Ｚｅｎｄ ） 。 ２．３．２ 简化航迹策略 航迹节点过多意味着四旋翼无人机在飞行的过 程中需要转折次数过多，采用一定的简化策略来减 少航迹节点既能减小适应度值还能减小航迹主要思 路是：假设变主方向搜素策略所搜索出的一条可行 航迹为 ｒｏｕｔｅ ＝ （ｒ１ ，ｒ２ ，…，ｒｎ ） 。 首先把节点 ｒ１ 放入 新航迹数组 ｎｅｗｒｏｕｔｅ 中，判断 ｒ１ 与 ｒｎ 的连通性，如 果是连通的，把 ｒ１ 放入数组 ｎｅｗｒｏｕｔｅ 中；否则判断 ｒ１ 与 ｒｎ －１ 的连通性，直到找到一个点 ｒｉ 与 ｒ１ 是连通 的。 对 ｒｉ 进行相同的操作，直到终止点。 ３ 三维粒子群算法 ３．１ 粒子群算法的主要流程 粒子群算法进行航迹搜索时分以下几个步骤： 第 ２ 期 莫宏伟，等：基于蚁群算法的四旋翼航迹规划 ·２２１·
.222· 智能系统学报 第11卷 1)确定起始点和终止点，对粒子群种群数、迭 止点为126°40.2472'：地面水平高度为0.25m。即 代次数等参数进行设定： (46.9639,40.5022,0.25)为起点，(46.8039， 2)搜索num条可行航路，根据适应度函数计算 40.2472,0.25)为终点。图12、13分别是用基本三 个航路的适应度值，选取适应度值最小的航路R, 维蚁群算法搜索到的三维航迹图形和三维航迹在二 根据主方向选取的不同将航路分成d段： 维平面上的投影。图14、15分别是采用了变主方向 3)根据主方向的不同对速度进行分段初始化， 策略改进后的算法搜索得到的三维航迹图形和三维 由初始速度和航路R找出PopNum条航路作为各个 航迹在二维平面上的投影。 粒子的初始航路： 4)根据粒子群初始航路计算各粒子的局部最 优航路，再比较粒子群的局部最优航路找出适应度 值最小的航路作为全局最优航路： 0, 30 140.25 40.30 5)迭代开始，迭代次数k=0: 恒2 40.35 0 40.40 6)粒子群i=0: 0 40.45经度/) 40.50 7)根据迭代次数计算每一代ω的值，根据当前 46.9646.9046.8446.78 全局最优航路和局部最优航路对粒子群速度进行分 纬度/ 段更新，更新完速度之后再对粒子群中各粒子的位 图12蚊群算法搜索到的三维航迹图形 置进行更新： Fig.12 Three-dimensional graphics searched by ant 8)计算粒子i的新的航路的适应度值与粒子的 colony algorithm 局部最优值进行比较，如果当前航路的适应度值小 40.50 于粒子的最优航路的适应度值，以当前航路替换 40.45 最优航路成为粒子i的新的最优航路： 9)如果i<PopNum,i值加1，重复7)，否则执 行10)： 40.35 10)更新完所有粒子的位置后比较全局最优航 40.30 路适应度值和粒子群当前局部最优航路适应度值， 40.25 如果有某粒子的局部最优航路的适应度值小于全局 46.7846.8246.8646.9046.9446.98 最优航路的适应度值就用该粒子的航路代替全局最 纬度) 优航路成为新的全局最优航路； 图13三维航迹在二维平面上的投影 11)如果k<N,k值加1，重复6)，否则转12)： Fig.13 Three-dimensional trajectory projected on a 12)绘制三维地图和三维航迹。 two-dimensional plane 粒子群算法被用来解决四旋翼无人机航迹规划 从图12、13中可以看出，基本蚁群算法搜索到 问题，会存在适应度值过大的问题。即使增加种群 的航迹出现了穿过障碍物的现象，这显然是不允许 数量和迭代次数也不能很好解决这个问题，文中提 的。改进后的航迹节点数为173个，航迹长度为 出简化航迹的策略来减少航迹节点，降低适应度值。 808.0402m,如果四旋翼以1~2m/s的速度飞行时， 简化策略的具体实行步骤与前文中的蚁群算法 可以飞行6.7336~13.4673'。从图14、15可以看 航迹简化策略相似，也是先判断终点和当前点的连 出，改进后的算法满足航迹规划的需要，且能够有效 通性，如果是可连通的则简化结束，否则判断和倒数 避免航迹穿过障碍物的现象出现。 第二个节点的连通性，如此反复，直到简化完成。在 粒子群算法中，每次计算适应度值时不是直接计算， 40 越 40.50 而是先将航迹简化，比较各航迹简化后的适应度值， 201 40.45 40.40 最后保留适应度值最小的航路，该最小航路的简化 0 40.35 40.30经度/) 航路就是最终的航迹，直接在三维环境中绘制即可。 ￥46.8 46.8546.9046.96 40.25 纬度/0 4 仿真与分析 图14加入变主方向策略后的三维航迹图 4.1三维蚊群算法仿真分析 Fig.14 Three-dimensional graphics including 实验时，设定纬度起始点为45°46.9639'，终止 the strategy of converting the main di- rection 点为4546.8039'：经度起始点为126°40.5022'，终
１）确定起始点和终止点，对粒子群种群数、迭 代次数等参数进行设定； ２）搜索 ｎｕｍ 条可行航路，根据适应度函数计算 个航路的适应度值，选取适应度值最小的航路 Ｒ ， 根据主方向选取的不同将航路分成 ｄ 段； ３）根据主方向的不同对速度进行分段初始化， 由初始速度和航路 Ｒ 找出 ＰｏｐＮｕｍ 条航路作为各个 粒子的初始航路； ４）根据粒子群初始航路计算各粒子的局部最 优航路，再比较粒子群的局部最优航路找出适应度 值最小的航路作为全局最优航路； ５）迭代开始，迭代次数 ｋ ＝ ０； ６）粒子群 ｉ ＝ ０； ７）根据迭代次数计算每一代 ω 的值，根据当前 全局最优航路和局部最优航路对粒子群速度进行分 段更新，更新完速度之后再对粒子群中各粒子的位 置进行更新； ８）计算粒子 ｉ 的新的航路的适应度值与粒子的 局部最优值进行比较，如果当前航路的适应度值小 于粒子 ｉ 的最优航路的适应度值，以当前航路替换 最优航路成为粒子 ｉ 的新的最优航路； ９）如果 ｉ ＜ ＰｏｐＮｕｍ ， ｉ 值加 １，重复 ７），否则执 行 １０）； １０）更新完所有粒子的位置后比较全局最优航 路适应度值和粒子群当前局部最优航路适应度值， 如果有某粒子的局部最优航路的适应度值小于全局 最优航路的适应度值就用该粒子的航路代替全局最 优航路成为新的全局最优航路； １１）如果 ｋ ＜ Ｎ ， ｋ 值加 １，重复 ６），否则转 １２）； １２）绘制三维地图和三维航迹。 粒子群算法被用来解决四旋翼无人机航迹规划 问题，会存在适应度值过大的问题。 即使增加种群 数量和迭代次数也不能很好解决这个问题，文中提 出简化航迹的策略来减少航迹节点，降低适应度值。 简化策略的具体实行步骤与前文中的蚁群算法 航迹简化策略相似，也是先判断终点和当前点的连 通性，如果是可连通的则简化结束，否则判断和倒数 第二个节点的连通性，如此反复，直到简化完成。 在 粒子群算法中，每次计算适应度值时不是直接计算， 而是先将航迹简化，比较各航迹简化后的适应度值， 最后保留适应度值最小的航路，该最小航路的简化 航路就是最终的航迹，直接在三维环境中绘制即可。 ４ 仿真与分析 ４．１ 三维蚁群算法仿真分析 实验时，设定纬度起始点为 ４５°４６．９６３ ９′，终止 点为 ４５°４６．８０３ ９′；经度起始点为 １２６°４０．５０２ ２′，终 止点为 １２６°４０．２４７ ２′；地面水平高度为０．２５ ｍ。 即 （４６． ９６３ ９， ４０． ５０２ ２， ０． ２５ ） 为 起 点， （ ４６． ８０３ ９， ４０．２４７ ２，０．２５）为终点。 图 １２、１３ 分别是用基本三 维蚁群算法搜索到的三维航迹图形和三维航迹在二 维平面上的投影。 图 １４、１５ 分别是采用了变主方向 策略改进后的算法搜索得到的三维航迹图形和三维 航迹在二维平面上的投影。 图 １２ 蚁群算法搜索到的三维航迹图形 Ｆｉｇ．１２ Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｇｒａｐｈｉｃｓ ｓｅａｒｃｈｅｄ ｂｙ ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ 图 １３ 三维航迹在二维平面上的投影 Ｆｉｇ．１３ Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｔｒａｊｅｃｔｏｒｙ ｐｒｏｊｅｃｔｅｄ ｏｎ ａ ｔｗｏ⁃ｄｉｍｅｎｓｉｏｎａｌ ｐｌａｎｅ 从图 １２、１３ 中可以看出，基本蚁群算法搜索到 的航迹出现了穿过障碍物的现象，这显然是不允许 的。 改进后的航迹节点数为 １７３ 个，航迹长度为 ８０８．０４０ ２ ｍ，如果四旋翼以 １～２ ｍ ／ ｓ 的速度飞行时， 可以飞行 ６．７３３ ６ ～ １３．４６７ ３′。 从图 １４、１５ 可以看 出，改进后的算法满足航迹规划的需要，且能够有效 避免航迹穿过障碍物的现象出现。 图 １４ 加入变主方向策略后的三维航迹图 Ｆｉｇ． １４ Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｇｒａｐｈｉｃｓ ｉｎｃｌｕｄｉｎｇ ｔｈｅ ｓｔｒａｔｅｇｙ ｏｆ ｃｏｎｖｅｒｔｉｎｇ ｔｈｅ ｍａｉｎ ｄｉ⁃ ｒｅｃｔｉｏｎ ·２２２· 智 能 系 统 学 报 第 １１ 卷
第2期 莫宏伟，等：基于蚁群算法的四旋翼航迹规划 ·223. 40.50 起40 40.45 20 0 40.40 46.8 50 46.85 40.35 纬度6946.950碧移场 46.90 经度/) 40.30 图18使用简化策略的三维航迹图 40.25 Fig.18 Three-dimensional graphics with simplifica- 46.7846.8246.8646.9046.9446.98 纬度') tion strategy 图15加入变主方向策略的航迹二维投影图 40.50 Fig.15 The two-dimensional projection of track in- 40.45 cluding the strategy of converting the main direction 40.40 40.35 选定起点坐标为(46.9639,40.5022,0.25)，终点 坐标为(46.8039,40.2472,0.25)，种群数PopNum= 40.30 25,迭代次数V=50,其他参数都相等的情况下，对 40.25 简化策略使用前和使用后进行对比。 46.7846.82 46.8646.9046.9446.98 图16、17分别使用简化策略前的三维航迹及三 纬度) 维航迹在二维平面上的投影，航迹经过173个航迹节 图19使用简化策略的航迹二维投影图 点，航迹长度为812.0706m。图18、19是使用简化策 Fig.19 The two-dimensional projection of track with 略后的三维航迹及三维航迹在二维平面上的投影，航 simplification strategy 迹中经过5个航迹节点，航迹节点长度为 为了进一步评价简化策略实施对航迹搜索结果 522.7658m。可以看出，简化后的航迹比简化前的航 的影响，图20、21分别是简化策略实施前后适应度 迹，在有效避开障碍物的同时极好地改善了三维航迹 值统计图和搜索时间统计图。 的形状，航迹长度大大缩短，更符合实际的需要。 ×10 22 20 1 40 延20 14 0.50 40.4 ÷46.80 40.40 10t 6.8 40.3 46.9 40.30 纬度/( 46.95 40.25 经度 86 2 图16未使用简化策略的三维航迹图 种群数量 Fig.16 Three-dimensional graphics without simplifi- 图20简化前后适应度值统计 cation strategy Fig.20 Statistics of fitness value with and without 40.50 simplification 40.45 26.5 40.40 26.0 40.35 目25 40.30 25.0 40.25 24.5 46.7846.8246.8646.9046.9446.98 纬度/) 种群数量 图17未使用简化策略的航迹二维投影图 图21简化前后搜索时间统计 Fig.17 The two-dimensional projection of track with- Fig.21 Statistics of searching time with and without out simplification strategy simplification
图 １５ 加入变主方向策略的航迹二维投影图 Ｆｉｇ．１５ Ｔｈｅ ｔｗｏ⁃ｄｉｍｅｎｓｉｏｎａｌ ｐｒｏｊｅｃｔｉｏｎ ｏｆ ｔｒａｃｋ ｉｎ⁃ ｃｌｕｄｉｎｇ ｔｈｅ ｓｔｒａｔｅｇｙ ｏｆ ｃｏｎｖｅｒｔｉｎｇ ｔｈｅ ｍａｉｎ ｄｉｒｅｃｔｉｏｎ 选定起点坐标为（４６．９６３９，４０．５０２２，０．２５），终点 坐标为（４６．８０３９，４０．２４７２，０．２５），种群数 ＰｏｐＮｕｍ ＝ ２５，迭代次数 Ｎ ＝ ５０，其他参数都相等的情况下，对 简化策略使用前和使用后进行对比。 图 １６、１７ 分别使用简化策略前的三维航迹及三 维航迹在二维平面上的投影，航迹经过 １７３ 个航迹节 点，航迹长度为 ８１２．０７０ ６ ｍ。 图 １８、１９ 是使用简化策 略后的三维航迹及三维航迹在二维平面上的投影，航 迹中 经 过 ５ 个 航 迹 节 点， 航 迹 节 点 长 度 为 ５２２．７６５ ８ ｍ。 可以看出，简化后的航迹比简化前的航 迹，在有效避开障碍物的同时极好地改善了三维航迹 的形状，航迹长度大大缩短，更符合实际的需要。 图 １６ 未使用简化策略的三维航迹图 Ｆｉｇ．１６ Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｇｒａｐｈｉｃｓ ｗｉｔｈｏｕｔ ｓｉｍｐｌｉｆｉ⁃ ｃａｔｉｏｎ ｓｔｒａｔｅｇｙ 图 １７ 未使用简化策略的航迹二维投影图 Ｆｉｇ．１７ Ｔｈｅ ｔｗｏ⁃ｄｉｍｅｎｓｉｏｎａｌ ｐｒｏｊｅｃｔｉｏｎ ｏｆ ｔｒａｃｋ ｗｉｔｈ⁃ ｏｕｔ ｓｉｍｐｌｉｆｉｃａｔｉｏｎ ｓｔｒａｔｅｇｙ 图 １８ 使用简化策略的三维航迹图 Ｆｉｇ．１８ Ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｇｒａｐｈｉｃｓ ｗｉｔｈ ｓｉｍｐｌｉｆｉｃａ⁃ ｔｉｏｎ ｓｔｒａｔｅｇｙ 图 １９ 使用简化策略的航迹二维投影图 Ｆｉｇ．１９ Ｔｈｅ ｔｗｏ⁃ｄｉｍｅｎｓｉｏｎａｌ ｐｒｏｊｅｃｔｉｏｎ ｏｆ ｔｒａｃｋ ｗｉｔｈ ｓｉｍｐｌｉｆｉｃａｔｉｏｎ ｓｔｒａｔｅｇｙ 为了进一步评价简化策略实施对航迹搜索结果 的影响，图 ２０、２１ 分别是简化策略实施前后适应度 值统计图和搜索时间统计图。 图 ２０ 简化前后适应度值统计 Ｆｉｇ．２０ Ｓｔａｔｉｓｔｉｃｓ ｏｆ ｆｉｔｎｅｓｓ ｖａｌｕｅ ｗｉｔｈ ａｎｄ ｗｉｔｈｏｕｔ ｓｉｍｐｌｉｆｉｃａｔｉｏｎ 图 ２１ 简化前后搜索时间统计 Ｆｉｇ．２１ Ｓｔａｔｉｓｔｉｃｓ ｏｆ ｓｅａｒｃｈｉｎｇ ｔｉｍｅ ｗｉｔｈ ａｎｄ ｗｉｔｈｏｕｔ ｓｉｍｐｌｉｆｉｃａｔｉｏｎ 第 ２ 期 莫宏伟，等：基于蚁群算法的四旋翼航迹规划 ·２２３·
.224 智能系统学报 第11卷 从图中可以看出，简化策略实施前后花费差不 样本的均值描述了样本的平均情况，表示为 多的时间，与简化策略实施前相比，简化策略实施后 5-/m (19) 适应度值大大减小，从而进一步证明了简化策略实 施的有效性。 样本标准差反映了样本的离散程度，表示为 4.2三维蚊群算法与三维粒子群算法对比 基于粒子群算法的航迹规划所使用的评价函数 (S.-S.)2 i=1 和环境模型与蚁群算法中所使用的评价函数和环境 (20) m 模型相同。但用传统粒子群算法实现航迹规划，也 统计蚁群算法和粒子群算法航迹规划适应度值 存在航迹节点过多的问题，所以同样采用了上文提 如表1所示。从表中可以看出，蚁群算法搜索出的 出的简化策略。将加入简化策略后的粒子群算法与 航迹的适应度值更为稳定一些。 改进后的蚁群算法相比较。 表1航迹规划适应度值统计 设定蚁群算法和粒子群算法主要参数为： Table 1 Statistics of fitness value of route planning 蚁群算法：种群数取值25，高度系数取值1，信 算法 最小值最大值均值 中值 标准差 息素挥发系数取0.5：粒子群算法：初始时选定可行 ACO 225.958238.094232224232.6582.665 航迹条数取值50，种群数量取值20，高度系数取值 PS0218.996284.540248.474239.35122.283 1,学习因子取2。 误差率描述了样本平均值与理论值的偏差程 起点坐标均为(46.9349,40.4407,0.25)，终点坐 度，反映了算法对问题的优化程度，误差率越小则表 标均为(46.8809,40.3387,0.25)。运行20次。得到蚁 示算法的优化性能越好，也就是找到的最优值越接 群算法和粒子群算法的最优航迹如图22、23。 近理论最优值。误差率计算公式为 40 Saan-S 20 e= ×100% (21) 关0 40.50 1049 46.80 40.40 式中：S·是理论最优值，在本文中就是指理论最优 46.8 40 46.90 4030经度/(') 航迹。 纬度 46.9540.25 本文的航迹规划过程中，环境相同时，以粒子群 图22蚊群算法搜索到的最优三维航迹图形 算法和蚁群算法规划处的航迹适应度值最小值作为 Fig.22 The best three-dimensional graphic searched 理论最优值，以每种算法各自的平均值来计算两种 by Ant colony algorithm 算法各自的误差率。两种算法的误差率如表2。 表2航迹规划误差率统计 20 Table 2 Statistics of error rate of route planning 46.80 算法 理论最优值 均值 误差率/% 46.85 纬度/46.，90 0250.30050 ACO 218.966 232.223 6.055 46.95 经度) PSO 218.966 248.474 13.476 图23粒子群算法搜索到的最优三维航迹图形 航迹规划搜索时间是指从算法开始到找到最优 Fig.23 The best three-dimensional graphic searched 航迹经历的时间。一般来说，规划时间越少算法优 by Particle swarm optimization 化性能越好。表3是蚁群算法和粒子群算法分别进 为了评价一个算法的优劣，需要用一些指标来 行航迹搜索时的搜索时间，从表中可以看出，粒子群 评价，常用的算法指标包括：时间复杂度、空间复杂 算法进行航迹搜索时的搜索时间比蚁群算法的小得 度、正确性、可读性、健壮性。 多，同时，蚁群算法的规划时间更稳定一些，也间接 结合本文所研究课题内容，重点引人了稳定 说明蚁群算法搜索稳定性更高一些。 性[4】、误差率、航迹规划搜索时间这3个指标5]进 表3航迹规划时间统计 行对比。 Table 3 Statistics of time of route planning 航迹规划时已经有S,S2,…,Sm共m个样本 算法 最小值最大值均值 中值标淮差 个体的样本，样本最大值S,最小值Sm,那么样 ACO 20.091 20.831 20.457 20.440 0.141 本绝对误差可以表示为 PSO 0.026 0.578 0.228 0.171 0.162 Sae =(Smax -Smin)/2 (18) 从标准差、误差率、规划时间3个方面对两种算
从图中可以看出，简化策略实施前后花费差不 多的时间，与简化策略实施前相比，简化策略实施后 适应度值大大减小，从而进一步证明了简化策略实 施的有效性。 ４．２ 三维蚁群算法与三维粒子群算法对比 基于粒子群算法的航迹规划所使用的评价函数 和环境模型与蚁群算法中所使用的评价函数和环境 模型相同。 但用传统粒子群算法实现航迹规划，也 存在航迹节点过多的问题，所以同样采用了上文提 出的简化策略。 将加入简化策略后的粒子群算法与 改进后的蚁群算法相比较。 设定蚁群算法和粒子群算法主要参数为： 蚁群算法：种群数取值 ２５，高度系数取值 １，信 息素挥发系数取 ０．５；粒子群算法：初始时选定可行 航迹条数取值 ５０，种群数量取值 ２０，高度系数取值 １，学习因子取 ２。 起点坐标均为（４６．９３４９，４０．４４０７，０．２５），终点坐 标均为（４６．８８０９，４０．３３８７，０．２５）。 运行 ２０ 次。 得到蚁 群算法和粒子群算法的最优航迹如图 ２２、２３。 图 ２２ 蚁群算法搜索到的最优三维航迹图形 Ｆｉｇ．２２ Ｔｈｅ ｂｅｓｔ ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｇｒａｐｈｉｃ ｓｅａｒｃｈｅｄ ｂｙ Ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ 图 ２３ 粒子群算法搜索到的最优三维航迹图形 Ｆｉｇ．２３ Ｔｈｅ ｂｅｓｔ ｔｈｒｅｅ⁃ｄｉｍｅｎｓｉｏｎａｌ ｇｒａｐｈｉｃ ｓｅａｒｃｈｅｄ ｂｙ Ｐａｒｔｉｃｌｅ ｓｗａｒｍ ｏｐｔｉｍｉｚａｔｉｏｎ 为了评价一个算法的优劣，需要用一些指标来 评价，常用的算法指标包括：时间复杂度、空间复杂 度、正确性、可读性、健壮性。 结合本文所研究课题内容，重点引入了稳定 性［ １４ ］ 、误差率、航迹规划搜索时间这 ３ 个指标［１５］进 行对比。 航迹规划时已经有 Ｓ１ ， Ｓ２ ，…， Ｓｍ 共 ｍ 个样本 个体的样本，样本最大值 Ｓｍａｘ ，最小值 Ｓｍｉｎ ，那么样 本绝对误差可以表示为 Ｓａｅ ＝ （Ｓｍａｘ － Ｓｍｉｎ ） ／ ２ （１８） 样本的均值描述了样本的平均情况，表示为 Ｓ ＝ ∑ ｍ ｉ ＝ １ Ｓｉ ／ ｍ （１９） 样本标准差反映了样本的离散程度，表示为 σ ＝ ∑ ｍ ｉ ＝ １ （Ｓｉ － Ｓｍｅａｎ ） ２ ｍ （２０） 统计蚁群算法和粒子群算法航迹规划适应度值 如表 １ 所示。 从表中可以看出，蚁群算法搜索出的 航迹的适应度值更为稳定一些。 表 １ 航迹规划适应度值统计 Ｔａｂｌｅ １ Ｓｔａｔｉｓｔｉｃｓ ｏｆ ｆｉｔｎｅｓｓ ｖａｌｕｅ ｏｆ ｒｏｕｔｅ ｐｌａｎｎｉｎｇ 算法 最小值 最大值 均值 中值 标准差 ＡＣＯ ２２５．９５８ ２３８．０９４ ２３２．２２４ ２３２．６５８ ２．６６５ ＰＳＯ ２１８．９９６ ２８４．５４０ ２４８．４７４ ２３９．３５１ ２２．２８３ 误差率描述了样本平均值与理论值的偏差程 度，反映了算法对问题的优化程度，误差率越小则表 示算法的优化性能越好，也就是找到的最优值越接 近理论最优值。 误差率计算公式为 ｅ ＝ Ｓｍｅａｎ － Ｓ ∗ Ｓ ∗ × １００％ （２１） 式中： Ｓ ∗ 是理论最优值，在本文中就是指理论最优 航迹。 本文的航迹规划过程中，环境相同时，以粒子群 算法和蚁群算法规划处的航迹适应度值最小值作为 理论最优值，以每种算法各自的平均值来计算两种 算法各自的误差率。 两种算法的误差率如表 ２。 表 ２ 航迹规划误差率统计 Ｔａｂｌｅ ２ Ｓｔａｔｉｓｔｉｃｓ ｏｆ ｅｒｒｏｒ ｒａｔｅ ｏｆ ｒｏｕｔｅ ｐｌａｎｎｉｎｇ 算法 理论最优值 均值 误差率／ ％ ＡＣＯ ２１８．９６６ ２３２．２２３ ６．０５５ ＰＳＯ ２１８．９６６ ２４８．４７４ １３．４７６ 航迹规划搜索时间是指从算法开始到找到最优 航迹经历的时间。 一般来说，规划时间越少算法优 化性能越好。 表 ３ 是蚁群算法和粒子群算法分别进 行航迹搜索时的搜索时间，从表中可以看出，粒子群 算法进行航迹搜索时的搜索时间比蚁群算法的小得 多，同时，蚁群算法的规划时间更稳定一些，也间接 说明蚁群算法搜索稳定性更高一些。 表 ３ 航迹规划时间统计 Ｔａｂｌｅ ３ Ｓｔａｔｉｓｔｉｃｓ ｏｆ ｔｉｍｅ ｏｆ ｒｏｕｔｅ ｐｌａｎｎｉｎｇ 算法 最小值 最大值 均值 中值 标准差 ＡＣＯ ＰＳＯ ２０．０９１ ０．０２６ ２０．８３１ ０．５７８ ２０．４５７ ０．２２８ ２０．４４０ ０．１７１ ０．１４１ ０．１６２ 从标准差、误差率、规划时间 ３ 个方面对两种算 ·２２４· 智 能 系 统 学 报 第 １１ 卷
第2期 莫宏伟，等：基于蚁群算法的四旋冀航迹规划 .225. 法进行对比和评估，从对比和评估的结果看来，蚁群 HAN Chao,WANG Ying.Path planning of UAV based on 算法稳定性更高一些，而粒子群算法航迹规划时间 an improved PSO algorithmJ.Ship electronic engineer- 更少一些。 ing,2014,34(4):49-53. [8]胡小兵，黄席樾.基于蚁群算法的三维空间机器人的路 5结束语 径规划[J刀.重庆大学学报，2004,27(8)：132-135. HU Xiaobing,HUANG Xiyue.Path planning in 3-D space 本文详细介绍了四旋翼无人机航迹规划的三维 for robot based on ant colony algorithm[].Journal of 环境模型构建并结合蚁群算法在航迹搜索中出现的 Chongqing university:natural science edition,2004,27 问题对算法提出了改进，仿真实验证实改进后的算 (8):132-135. 法减小了适应度值，使航迹得到了优化。但是根据 [9]胡中华.基于智能优化算法的无人机航迹规划若干关键 搜索时间的统计图可以得出搜索时间较改进前长。 技术研究[D].南京：南京航空航天大学，2011：46-52. 本文还与同样改进策略的粒子群算法作比较，得出 [10]KHATIB O.Real-time obstacle avoidance for manipulators 蚁群算法稳定性更高，但是规划时间有点长。因此， and mobile robots[J].International Journal of Robotics 在后续的工作研究中要结合实际飞行测试实验并将 Research,1986,5(1):90-99. 蚁群算法与更多智能算法做横向对比，进而优化参 [11]BAIRD C,ABRARNSON M.A comparison of several dig- ital map-aided navigation technique C//The Proceedings 数以便提高算法的规划时间。 of IEEE PLANS.Halifax,Nova Scotia,Canada,1984: 参考文献： 286-293. [12]任世军，洪炳熔，黄德海.一种基于橱格扩展的机器人 [1]METEA M,TSAI J.Route planning for intelligent autono- 路径规划方法[J].哈尔滨工业大学学报，2001,33 mous land vehicles using hierarchical terrain representation (1):68-72. [C]//Proceedings of IEEE International Conference on Ro- REN Shijun,HONG Bingrong,HUANG Dehai.A robot botics and Automation.Raleigh,NC,USA,2010:1947- path planning algorithm based on grid expansion[J].Jour- 1952. nal of Harbin institute of technology,2001,33(1):68- [2]谢奉军，张丹平，黄蕾，等.旋翼无人机专利申请现状与 72. 技术发展趋势分析[C]/第六届中国航空学会青年科技 [13]吕太之，赵春霞.基于改进概率栅格分解的路径规划算 论坛文集.沈阳，2014 法[J].计算机工程，2007,33(21)：160-162,165. [3]LIU Hongyun,JIANG Xiao,JU Hehua.Multi-goal path LV Taizhi,ZHAO Chunxia.Path planning based on im- planning algorithm for mobile robots in grid space[C]// proved probabilistic cell decomposition[J].Computer engi- Proceedings of the 25th Chinese Control and Decision Con- neering,2007,33(21):160-162,165. ference.Guiyang,2013:2872-2876. [14]王凌.智能优化算法及其应用[M].北京：清华大学出 [4]GALCERAN E,CARRERAS M.A survey on coverage path 版社，2001. planning for robotics[J].Robotics and Autonomous Sys- [15]WEISS M A.数据结构与算法分析[M].冯舜玺，译.2 tems,2013,61(12):1258-1276. 版.北京：机械工业出版社，2004. [5]李翊，王朕，姜鹏，等.四旋翼无人飞行器的航迹规划 作者简介： [J].舰船电子工程，2014,34(12)：58-61. 莫宏伟，男，1973年生，教授，主持 LI Yi,WANG Zhen,JIANG Peng,et al.Path planning of 完成国家自然科学基金等国家、省部级 quad-rotor[J].Ship electronic engineering,2013,61(12): 及横向课题16项，获得省科技进步奖 1258-1276 两项，主要研究方向为自然计算理论与 [6]王玥，张志强，曹晓文.A*改进算法及其在航迹规划中 应用，机器人，机器学习与数据挖掘，发 的应用[J].信息系统工程，2014(1)：89-91. 表论文60余篇，其中被SCI检索11篇， [7]韩超，王赢.一种基于改进PS0的无人机航路规划方法 被EI检索40余篇。 [J].舰船电子工程，2014,34(4)：49-53
法进行对比和评估，从对比和评估的结果看来，蚁群 算法稳定性更高一些，而粒子群算法航迹规划时间 更少一些。 ５ 结束语 本文详细介绍了四旋翼无人机航迹规划的三维 环境模型构建并结合蚁群算法在航迹搜索中出现的 问题对算法提出了改进，仿真实验证实改进后的算 法减小了适应度值，使航迹得到了优化。 但是根据 搜索时间的统计图可以得出搜索时间较改进前长。 本文还与同样改进策略的粒子群算法作比较，得出 蚁群算法稳定性更高，但是规划时间有点长。 因此， 在后续的工作研究中要结合实际飞行测试实验并将 蚁群算法与更多智能算法做横向对比，进而优化参 数以便提高算法的规划时间。 参考文献： ［１］ ＭＥＴＥＡ Ｍ， ＴＳＡＩ Ｊ． Ｒｏｕｔｅ ｐｌａｎｎｉｎｇ ｆｏｒ ｉｎｔｅｌｌｉｇｅｎｔ ａｕｔｏｎｏ⁃ ｍｏｕｓ ｌａｎｄ ｖｅｈｉｃｌｅｓ ｕｓｉｎｇ ｈｉｅｒａｒｃｈｉｃａｌ ｔｅｒｒａｉｎ ｒｅｐｒｅｓｅｎｔａｔｉｏｎ ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏ⁃ ｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｒａｌｅｉｇｈ， ＮＣ， ＵＳＡ， ２０１０： １９４７⁃ １９５２． ［２］谢奉军， 张丹平， 黄蕾，等． 旋翼无人机专利申请现状与 技术发展趋势分析［Ｃ］ ／ ／ 第六届中国航空学会青年科技 论坛文集． 沈阳， ２０１４． ［３］ ＬＩＵ Ｈｏｎｇｙｕｎ， ＪＩＡＮＧ Ｘｉａｏ， ＪＵ Ｈｅｈｕａ． Ｍｕｌｔｉ⁃ｇｏａｌ ｐａｔｈ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ｉｎ ｇｒｉｄ ｓｐａｃｅ ［ Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ２５ｔｈ Ｃｈｉｎｅｓｅ Ｃｏｎｔｒｏｌ ａｎｄ Ｄｅｃｉｓｉｏｎ Ｃｏｎ⁃ ｆｅｒｅｎｃｅ． Ｇｕｉｙａｎｇ， ２０１３： ２８７２⁃２８７６． ［４］ＧＡＬＣＥＲＡＮ Ｅ， ＣＡＲＲＥＲＡＳ Ｍ． Ａ ｓｕｒｖｅｙ ｏｎ ｃｏｖｅｒａｇｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ｆｏｒ ｒｏｂｏｔｉｃｓ ［ Ｊ］． Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｎｏｍｏｕｓ Ｓｙｓ⁃ ｔｅｍｓ， ２０１３， ６１（１２）： １２５８⁃１２７６． ［５］李翊， 王朕， 姜鹏， 等． 四旋翼无人飞行器的航迹规划 ［Ｊ］． 舰船电子工程， ２０１４， ３４（１２）： ５８⁃６１． ＬＩ Ｙｉ， ＷＡＮＧ Ｚｈｅｎ， ＪＩＡＮＧ Ｐｅｎｇ， ｅｔ ａｌ． Ｐａｔｈ ｐｌａｎｎｉｎｇ ｏｆ ｑｕａｄ⁃ｒｏｔｏｒ［Ｊ］． Ｓｈｉｐ ｅｌｅｃｔｒｏｎｉｃ ｅｎｇｉｎｅｅｒｉｎｇ， ２０１３， ６１（１２）： １２５８⁃１２７６． ［６］王玥， 张志强， 曹晓文． Ａ∗改进算法及其在航迹规划中 的应用［Ｊ］． 信息系统工程， ２０１４ （１）： ８９⁃９１． ［７］韩超， 王赢． 一种基于改进 ＰＳＯ 的无人机航路规划方法 ［Ｊ］． 舰船电子工程， ２０１４， ３４（４）： ４９⁃５３． ＨＡＮ Ｃｈａｏ， ＷＡＮＧ Ｙｉｎｇ． Ｐａｔｈ ｐｌａｎｎｉｎｇ ｏｆ ＵＡＶ ｂａｓｅｄ ｏｎ ａｎ ｉｍｐｒｏｖｅｄ ＰＳＯ ａｌｇｏｒｉｔｈｍ ［ Ｊ］． Ｓｈｉｐ ｅｌｅｃｔｒｏｎｉｃ ｅｎｇｉｎｅｅｒ⁃ ｉｎｇ， ２０１４， ３４（４）： ４９⁃５３． ［８］胡小兵， 黄席樾． 基于蚁群算法的三维空间机器人的路 径规划［Ｊ］． 重庆大学学报， ２００４， ２７（８）： １３２⁃１３５． ＨＵ Ｘｉａｏｂｉｎｇ， ＨＵＡＮＧ Ｘｉｙｕｅ． Ｐａｔｈ ｐｌａｎｎｉｎｇ ｉｎ ３⁃Ｄ ｓｐａｃｅ ｆｏｒ ｒｏｂｏｔ ｂａｓｅｄ ｏｎ ａｎｔ ｃｏｌｏｎｙ ａｌｇｏｒｉｔｈｍ ［ Ｊ ］． Ｊｏｕｒｎａｌ ｏｆ Ｃｈｏｎｇｑｉｎｇ ｕｎｉｖｅｒｓｉｔｙ： ｎａｔｕｒａｌ ｓｃｉｅｎｃｅ ｅｄｉｔｉｏｎ， ２００４， ２７ （８）： １３２⁃１３５． ［９］胡中华． 基于智能优化算法的无人机航迹规划若干关键 技术研究［Ｄ］． 南京： 南京航空航天大学， ２０１１： ４６⁃５２． ［１０］ＫＨＡＴＩＢ Ｏ． Ｒｅａｌ⁃ｔｉｍｅ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｆｏｒ ｍａｎｉｐｕｌａｔｏｒｓ ａｎｄ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ［ Ｊ］． Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ， １９８６， ５（１）： ９０⁃９９． ［１１］ＢＡＩＲＤ Ｃ， ＡＢＲＡＲＮＳＯＮ Ｍ． Ａ ｃｏｍｐａｒｉｓｏｎ ｏｆ ｓｅｖｅｒａｌ ｄｉｇ⁃ ｉｔａｌ ｍａｐ⁃ａｉｄｅｄ ｎａｖｉｇａｔｉｏｎ ｔｅｃｈｎｉｑｕｅ［Ｃ］ ／ ／ Ｔｈｅ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ＩＥＥＥ ＰＬＡＮＳ． Ｈａｌｉｆａｘ， Ｎｏｖａ Ｓｃｏｔｉａ， Ｃａｎａｄａ， １９８４： ２８６⁃２９３． ［１２］任世军， 洪炳熔， 黄德海． 一种基于栅格扩展的机器人 路径规划方法［ Ｊ］． 哈尔滨工业大学学报， ２００１， ３３ （１）： ６８⁃７２． ＲＥＮ Ｓｈｉｊｕｎ， ＨＯＮＧ Ｂｉｎｇｒｏｎｇ， ＨＵＡＮＧ Ｄｅｈａｉ． Ａ ｒｏｂｏｔ ｐａｔｈ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｇｒｉｄ ｅｘｐａｎｓｉｏｎ［Ｊ］． Ｊｏｕｒ⁃ ｎａｌ ｏｆ Ｈａｒｂｉｎ ｉｎｓｔｉｔｕｔｅ ｏｆ ｔｅｃｈｎｏｌｏｇｙ， ２００１， ３３（ １）： ６８⁃ ７２． ［１３］吕太之， 赵春霞． 基于改进概率栅格分解的路径规划算 法［Ｊ］． 计算机工程， ２００７， ３３（２１）： １６０⁃１６２， １６５． ＬＶ Ｔａｉｚｈｉ， ＺＨＡＯ Ｃｈｕｎｘｉａ． Ｐａｔｈ ｐｌａｎｎｉｎｇ ｂａｓｅｄ ｏｎ ｉｍ⁃ ｐｒｏｖｅｄ ｐｒｏｂａｂｉｌｉｓｔｉｃ ｃｅｌｌ ｄｅｃｏｍｐｏｓｉｔｉｏｎ［Ｊ］． Ｃｏｍｐｕｔｅｒ ｅｎｇｉ⁃ ｎｅｅｒｉｎｇ， ２００７， ３３（２１）： １６０⁃１６２， １６５． ［１４］王凌． 智能优化算法及其应用［Ｍ］． 北京： 清华大学出 版社， ２００１． ［１５］ＷＥＩＳＳ Ｍ Ａ． 数据结构与算法分析［Ｍ］． 冯舜玺， 译． ２ 版． 北京： 机械工业出版社， ２００４． 作者简介： 莫宏伟，男，１９７３ 年生，教授，主持 完成国家自然科学基金等国家、省部级 及横向课题 １６ 项，获得省科技进步奖 两项，主要研究方向为自然计算理论与 应用，机器人，机器学习与数据挖掘，发 表论文 ６０ 余篇，其中被 ＳＣＩ 检索 １１ 篇， 被 ＥＩ 检索 ４０ 余篇。 第 ２ 期 莫宏伟，等：基于蚁群算法的四旋翼航迹规划 ·２２５·
