第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:
Ｚｓｔａｒｔ ＝ ｃｅｉｌ（ Ｓｈ － ＺＧｒｉｄｓｔａｒｔ ＺＧｒｉｄ ） × ＺＧｒｉｄ ＋ ＺＧｒｉｄｓｔａｒｔ （１３） 式中：ｃｅｉｌ 表示向正无穷方向取整。 ②主方向选取。 主方向的选取主要就是以纬度 方向为主方向还是以经度方向为主方向的问题。 选 择两个方向之中栅格变化数最多的方向作为四旋翼 无人机航迹规划主方向。 主方向选定后还要知道主 方向上从起始点到终止点坐标值变化趋势，即坐标 值是增加还是减少。 如果坐标值增加，那么当从当 前平面到下一平面上主方向上坐标值加 １ 个单位 值；否则坐标值减 １ 个单位值。 假设 Ａ 方向为主方 向，那么从 Πｉ 到 Πｉ＋１ 坐标变化为 Ａｉ＋１ ＝ Ａｉ ＋ ＡＧｒｉｄ ， Ａｓｔａｒｔ ≤ Ａｅｎｄ Ａｉ { － ＡＧｒｉｄ ， 其他 （１４） ③种群数选取。 ④迭代次数的选择。 ⑤航迹搜寻范围选取。 如果主方向是 Ａ ，非主 方向的那个纬度或经度方向是 Ｂ ，那么从平面 Πｉ 到平面 Πｉ＋１ ，对于 Ｂ 方向上以 Ｙｉ 为中心从 Ｙｉ － ｂｃｍａｘ 到 Ｙｉ ＋ ｂｃｍａｘ 范围内的点都是可选择作为 Ｙｉ＋１ 点。 同 样，对于 Ｚ 方向上来说以 Ｚｉ 为中心从 Ｚｉ －ｈｃｍａｘ 到 Ｚｉ ＋ ｈｃｍａｘ 范围内的点都是可选择作为 Ｚｉ＋１ 点。 ⑥信息素初始化。 在初始时刻，把三维栅格环 境地图中的所有栅格信息素值设置为固定值。 ２）航迹搜索 航迹搜索前首先需要确定航迹所选的所有的节 点都要在规划空间中，另外在航迹高度上也要有一 定的限制。 经过观察发现，从地面到距地面 ２ ｍ 范 围内障碍物主要是人和车，这些障碍物多是动态的， 四旋翼在这个高度范围内飞行容易危害行人和车的 正常行动。 在距地面 ２ ～ ５ ｍ 的范围内主要是较矮 的树木，在这个空间中进行飞行时也较容易碰到障 碍物，但总的来说还算安全。 本文所研究的航迹规 划主要是在高于地面 ２ ｍ 的范围内进行的。 如果无 人机起始点在地面就先让四旋翼飞行距地面一定高 度 Ｈｍｉｎ ， 飞行过程中也限制四旋翼飞行在这个高度 之上。 在航迹搜索过程中，假设 ＰｏｐＮｕｍ 只蚂蚁中的 第 ｋ 只蚂蚁已运行至平面 Πｉ 上的点 （Ｘｉ，Ｙｉ，Ｚｉ） 处， 搜索 在 平 面 Πｉ＋１ 上 以 （Ｘｉ＋１ ，Ｙｉ，Ｚｉ） 为 中 心 的 ｃｏｕｎｔ ＝（２ × ｂｃｍａｘ ＋ １） × （２ × ｈｃｍａｘ ＋ １） 个点。 将 ｃｏｕｎｔ 个栅格中所有的可行栅格，即没有障碍物的栅 格放入数组 Ａｌｌｏｗｅｄｉ 中。 如果数组 Ａｌｌｏｗｅｄｉ 为空， 那么将当前点 （Ｘｉ，Ｙｉ，Ｚｉ） 的 Ｚｉ 坐标值加 １，即当前 点坐标变为 （Ｘｉ，Ｙｉ，Ｚｉ ＋ １） ，重新搜索平面上的可 行栅 格， 直 到 数 组 Ａｌｌｏｗｅｄｉ 不 为 空。 从 数 组 Ａｌｌｏｗｅｄｉ 中按照轮盘赌法选出一个可行的栅格作为 平面 Πｉ＋１ 上的航迹节点。 下一步就是对平面 Πｉ 上 的节点进行局部信息素更新。 对上述内容重复进行，直到到达平面 Πｎ－１ ，平 面 Πｉ 上的节点 （Ｘｎ－１ ，Ｙｎ－１ ，Ｚｎ－１ ） 直接与平面 Πｎ 上 的节点 （Ｘｅｎｄ ，Ｙｅｎｄ ，Ｚｅｎｄ ） 即终止点相连，这样就构成 了一条完整的航迹。 按照适应度值函数计算每条航迹的适应度值， 比较找出最小适应度值，而最小适应度对应的航迹 即为当前最优航迹。 将上述过程迭代 Ｎ 次，这样就找到了迭代 Ｎ 次 的最优航迹。 ３）图形绘制 利用已经测试到的环境模型的经度、纬度和高 度数据绘制三维环境地图，然后在三维环境地图中 绘制最优航迹。 到此为止，本文就完成了基于蚁群 算法的航迹规划与仿真。 ２．３ 算法的改进 ２．３．１ 变主方向搜索策略 基本三维蚁群算法进行航迹规划时，将 Πｎ－１ 平 面上节点和 Πｎ 平面上的节点即终点直接相连容易 穿过障碍物。 因此首先需要判断两点间的连通性， 其具体过程如下。 假设在三维空间存在两点 （Ｘ１ ，Ｙ１ ，Ｚ１ ） 和 （Ｘ２ ， Ｙ２ ，Ｚ２ ） 。 两点间的空间直线方程为 Ｘ － Ｘ１ Ｘ２ － Ｘ１ ＝ Ｙ － Ｙ１ Ｙ２ － Ｙ１ ＝ Ｚ － Ｚ１ Ｚ２ － Ｚ１ （１５） 由式（１５）可知，若已知 Ｘ 的值，那么 Ｙ ＝ （Ｙ２ － Ｙ１ ） × （Ｘ － Ｘ１ ） ／ （Ｘ２ － Ｘ１ ） ＋ Ｙ１ Ｚ ＝ （Ｚ２ － Ｚ１ ） × （Ｘ － Ｘ１ ） ／ （Ｘ２ － Ｘ１ ） ＋ Ｚ１ { （１６） 同理，若已知 Ｙ 的值，那么 Ｘ ＝ （Ｘ２ － Ｘ１ ） × （Ｙ － Ｙ１ ） ／ （Ｙ２ － Ｙ１ ） ＋ Ｘ１ Ｚ ＝ （Ｚ２ － Ｚ１ ） × （Ｙ － Ｙ１ ） ／ （Ｙ２ － Ｙ１ ） ＋ Ｚ１ { （１７） 想要知道空间中两点是否连通，首先要判断空间 直线经过哪些栅格，然后判断这些栅格所在的经纬度 位置的障碍物高度和直线高度关系，如果障碍物高度 高于直线高度，那么这条直线经过障碍物，也就是说 空间中两点是不可连通的，否则就是可连通的。 对于空间两点，根据两点的 Ｘ 坐标和 Ｙ 坐标间 大小关系，可以分为以下 ６ 种关系： ａ） Ｘ１ ＝ Ｘ２ ，Ｙ１ 和 Ｙ２ 关系任意； ｂ） Ｙ１ ＝ Ｙ２ ，Ｘ１ 和 Ｘ２ 关系任意； ｃ） Ｘ１ ＞ Ｘ２ ，Ｙ１ ＞ Ｙ２ ； ｄ） Ｘ１ ＞ Ｘ２ ，Ｙ１ ＜ Ｙ２ ； ·２２０· 智 能 系 统 学 报 第 １１ 卷