第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)Ｐ（Ｘ，Ｙ，Ｚ） ＝ τ（Ｘ，Ｙ，Ｚ）∗Ｈ（Ｘ，Ｙ，Ｚ） ∑τ（Ｘ，Ｙ，Ｚ）∗Ｈ（Ｘ，Ｙ，Ｚ） ， （Ｘ，Ｙ，Ｚ） ∈ Ａｌｌｏｗｅｄｉ ０， （Ｘ，Ｙ，Ｚ） ∉ Ａｌｌｏｗｅｄｉ ì î í ï ï ï ï ïï （３） 式中： τ（Ｘ，Ｙ，Ｚ） 是平面 Πｉ＋１ 上坐标为 （Ｘ，Ｙ，Ｚ） 的 栅格的信息素值。 Ｈ（Ｘ，Ｙ，Ｚ） 是平面 Πｉ＋１ 上坐标为 （Ｘ，Ｙ，Ｚ） 的栅格的启发函数，其计算公式为 Ｈ（Ｘ，Ｙ，Ｚ） ＝ Ｄ （Ｘ，Ｙ，Ｚ） ω１ × Ｓ （Ｘ，Ｙ，Ｚ） ω２ × Ｑ （Ｘ，Ｙ，Ｚ） ω３ （４） 式中： Ｄ（Ｘ，Ｙ，Ｚ） 是当前点和 （Ｘ，Ｙ，Ｚ） 的路径长 度，这可以促使蚂蚁尽可能选则距离当前点最近的 点，计算公式为 Ｄ（Ｘ，Ｙ，Ｚ） ＝ （Ｘｉ － Ｘ） ２ ＋ （Ｙｉ － Ｙ） ２ ＋ （Ｚｉ － Ｚ） ２ （５） Ｓ（Ｘ，Ｙ，Ｚ） 表示安全性因素，促使蚂蚁选择安全点。 当前栅格 （Ｘｉ，Ｙｉ，Ｚｉ） 和 （Ｘ，Ｙ，Ｚ） 是不可连通的， 或者栅格 （Ｘ，Ｙ，Ｚ） 内有障碍物，则称该栅格是不可 达的。 Ｓ（Ｘ，Ｙ，Ｚ） 的计算公式为 Ｓ（Ｘ，Ｙ，Ｚ） ＝ １， （Ｘ，Ｙ，Ｚ） 是可达点 {０， 其他 （６） 式中： Ｑ（Ｘ，Ｙ，Ｚ） 为 栅 格 （Ｘ，Ｙ，Ｚ） 到 终 止 栅 格 （Ｘｅｎｄ ，Ｙｅｎｄ ，Ｚｅｎｄ ） 的距离， Ｑ（Ｘ，Ｙ，Ｚ） 可以促使蚂蚁 选择离目标栅格更近的栅格，其计算公式为 Ｑ（Ｘ，Ｙ，Ｚ） ＝ （Ｘｉ － Ｘｅｎｄ ） ２ ＋ （Ｙｉ － Ｙｅｎｄ ） ２ ＋ （Ｚｉ － Ｚｅｎｄ ） ２ （７） 式中： ω１ 、 ω２ 、 ω３ 是系数，其大小代表了上述各因素 的重要性程度，系数值越大代表该项越重要；否则说 明该项越不重要。 图 ５ 主方向选取 Ｆｉｇ．５ Ｓｅｌｅｃｔ ｔｈｅ ｍａｉｎ ｄｉｒｅｃｔｉｏｎ 蚁群算法中蚂蚁是根据信息素浓度来进行搜索 路径的，在每走完一段或全部路径时，就要对信息素 值进行更新，所以信息素初始值设定和更新对于蚁 群算法是否能够成功搜索具有重要影响。 本文把信 息素值存储在三维空间一系列离散的点中，然后对 这些离散点的信息素值进行更新，所以对于每一个 栅格来说都有一个信息素值，这个信息素值就代表 了该栅格对蚂蚁的吸引程度，每个栅格的信息素值 在蚂蚁经过之后都要进行更新。 信息素的更新分为局部更新和全局更新两部 分。 局部更新是指只要有蚂蚁经过某栅格，该栅格 的信息素值就会被更新，更新后栅格的信息素值会 被减少，这样这个栅格在以后的搜索中被选中的概 率就被降低，而相应地增加其他未被搜索的栅格被 搜索的概率，这样就可以达到全局搜素的目的。 局 部搜索的信息素更新公式为 τＸ，Ｙ，Ｚ ＝ （１ － ζ）τＸ，Ｙ，Ｚ （８） 式中： ζ 表示信息素衰减系数， τＸ，Ｙ，Ｚ 表示栅格 （Ｘ， Ｙ，Ｚ） 的信息素值。 全局信息素更新是指蚂蚁完成一条航迹搜索 时，计算该路径的适应度值，从现有的搜索到的路径 中选择出最短的航迹，更新适应度值最小的航迹所 经过的所有栅格的信息素值，信息素更新公式为 τＸ，Ｙ，Ｚ ＝ （１ － ρ）τＸ，Ｙ，Ｚ ＋ ρΔτＸ，Ｙ，Ｚ （９） ΔτＸ，Ｙ，Ｚ ＝ Ｋ ｍｉｎ（ｌｅｎｇｔｈ（ｍ）） （１０） 式中： ｌｅｎｇｔｈ（ｍ） 表示蚂蚁 ｍ 经过的路径长度； ρ 表 示信息素挥发系数； Ｋ 为系数。 ２．２ 算法的主要流程 在充分考虑了评价函数、环境模型和三维蚁群 算法模型这些内容之后，下一步需要考虑的就是航 迹规划实现的问题。 本文中的程序实现主要分为参 数初始化设置、航迹搜索和图形绘制 ３ 个主要部分， 其具体每一步如下所述。 １）参数初始化设置 在进行航迹规划实现时，首先要确定的就是各 项参数设置问题。 这些参数设置包括起始点的确 定、主方向的选取、种群数的确定、迭代次数的选择、 航迹搜寻范围选取、信息素初始化设置。 ①起始点的确定。 当把四旋翼无人机放置在规 划空间中的某一点时，放置四旋翼的位置不一定恰 好就是栅格图中栅格的位置，这时就需要把该四旋 翼无人机所在的栅格的栅格坐标作为航迹规划的起 始点。 那么，三维栅格地图原点坐标为 （ＸＧｒｉｄｓｔａｒｔ， ＹＧｒｉｄｓｔａｒｔ，ＺＧｒｉｄｓｔａｒｔ） ，四旋翼的放置位置 （Ｓｌａｔ，Ｓｌｏｎ ，Ｓｈ ） 和其所在栅格的栅格坐标位置 （Ｘｓｔａｒｔ，Ｙｓｔａｒｔ，Ｚｓｔａｒｔ） 的 关系为 Ｘｓｔａｒｔ ＝ ｃｅｉｌ（ Ｓｌａｔ － ＸＧｒｉｄｓｔａｒｔ ＸＧｒｉｄ ） × ＸＧｒｉｄ ＋ ＸＧｒｉｄｓｔａｒｔ （１１） Ｙｓｔａｒｔ ＝ ｃｅｉｌ（ Ｓｌｏｎ － ＹＧｒｉｄｓｔａｒｔ ＹＧｒｉｄ ） × ＹＧｒｉｄ ＋ ＹＧｒｉｄｓｔａｒｔ （１２） 第 ２ 期 莫宏伟，等：基于蚁群算法的四旋翼航迹规划 ·２１９·