基于贪心策略下混合蚁群算法的无人机航迹规划

曹建秋,徐 鹏,张广言

(重庆交通大学 信息科学与工程学院，重庆 400074)

0 引 言

智能飞行器已经广泛用于军事侦察[1]，灾害处理[2]，物资配送[3]等领域。复杂环境下航迹快速规划是智能飞行器控制的一个重要课题。航迹规划指飞行器根据任务需要，在复杂环境中选择满足约束的最优路径，要求该路径的综合代价最小。

目前航迹规划的算法主要分为经典算法和群体智能算法两大类。经典算法包括A*算法，栅格法[4-6,14]等。但随着问题复杂度增加，被求解问题往往可以通过规约证明属于NP难问题，此时经典算法的计算复杂度呈指数增长。经典算法也存在自身的局限，如A*算法在实际问题中难以选择恰当的评价函数，也进一步限制了经典算法的应用场景。随着蚁群算法，粒子群算法等[7-13]智能算法的发展，这些情况得到了改善。

智能算法中，初始值多采用随机生成，这降低了算法的收敛速度，算法运行时间较长。为了得到更优解的同时加速算法收敛，实际应用中将经典算法与智能算法融合形成融合算法，能取得较好的效果。文献[7]融合了简化A*算法与模拟退火算法；文献[10]通过使用A*算法取代蚁群算法的信息素随机初始化，增加了蚁群算法的稳定性；文献[11]在稀疏A*全局优化搜索的基础之上融入生物启发神经动力学模型等。融合算法不仅提高了单一算法的可解释性，并且较原始算法有一定性能提升。

目前航迹规划问题的研究难点在于路径规划时需要考虑飞行器的性能，如最小转弯半径，飞行中产生的误差等。为处理飞行器航迹规划中的飞行误差，笔者重点研究了校正点约束下的多目标航迹规划问题，建立了校正点约束的航迹规划模型，提出一种贪心策略下的混合蚁群算法进行求解。该算法基于贪心策略对校正点进行剪枝操作，提升了经典A*算法，粒子群算法，蚁群算法的效果。

1 航迹约束模型

1.1 校正点建模

部分飞行器由于系统结构限制或工作环境复杂，定位系统无法对自身进行精准定位。因此，航迹空间中存在一些可用于误差校正的校正点，当飞行器到达校正点时，飞行器能根据该位置的误差校正类型进行校正，如水平校正点只用于校正水平误差。飞行器定位误差包括垂直和水平误差，误差会随着飞行积累，且只在一定范围内才可以校正。若垂直、水平误差都能得到及时校正，则飞行器可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。

已知航迹空间T内存在两类校正点，分别为水平校正点X,垂直校正点Y。对于飞行器路径L上的校正点{xi},{yi}有：

{xi|xi∈X},{yi|yi∈Y}∈L

(1)

∀xi∈X,Sc<α1,Ss<α2

(2)

∀yi∈Y,Sc<β1,Ss<β2

(3)

式中：Ss为飞行器当前累积的水平误差；Sc为飞行器当前累计的垂直误差。

飞行器受性能限制存在两组误差校正上限，分别为(α1,α2)与(β1,β2)。当飞行器的垂直误差不大于α1，水平误差不大于α2时才能进行垂直误差校正。垂直误差不大于β1，水平误差不大于β2时才能进行水平误差校正。

1.2 转弯半径约束建模

在考虑航迹时，考虑到飞行器在转弯时受到结构和控制系统的限制，无法完成即时转弯，存在最小转向半径RMIN，对飞行器的转弯半径R有：

R≥RMIN

(4)

需要在规划完成后所得航迹进行光滑处理。

1.3 综合代价函数

飞行器航迹规划需满足两点优化目标：①航迹长度尽可能小；②经过校正点次数尽可能少。式(5)表示经过的校正点之和最少，式(6)表示经过的校正点距离之和最少。

(5)

(6)

式中：di表示L中第i段路径的长度。

1.4 航迹规划问题贪心策略

1.4.1 问题的规约

对于给定满足式(1)～(4)的图G(V,E)，V=X,Y。E为满足条件的边(Vi,Vj)的集合。对图G进行规约：①令式(2)，式(3)中的α1,α2,β1,β2>M。M为航迹空间T内航迹误差的最大值；②删除校正点，使航迹空间的校正点类型为同一类校正点(水平或垂直)；③令飞行器航迹起点A与终点B重合；④令RMIN=0。

此时图G(V,E)转化为求解经典的旅行商问题(traveling salesman problem，TSP)。由于a-d的规约均可在多项式时间内完成，TSP又属于经典的NP-完全(non-deterministic polynomial complete problem，NPC)问题。根据NP完全理论，图G(V,E)问题属于NPC问题，即图G(V,E)问题仅能在多项式时间内验证，不能在多项式时间内求解。

1.4.2 贪心策略

对NPC问题，使用贪心思想进行求解。贪心思想指在对问题求解时，只关注当前条件下局部最优解。由于图G(V,E)的求解复杂度较高，笔者提出一种贪心策略对问题进行简化，在实验中取得了较好的效果。该策略核心思想为：基于贪心思想对校正点进行剪枝操作，具体的实现主要分为两步：

步骤1选择当前矫正点。设飞行器路径L上的第i点为水平校正点，此时Sc=k,Ss=0；

步骤2选择下一矫正点。若第i+1点为水平校正点，则下一校正点的选择范围为：min(β1-Sc,β2-Ss)，即min(β1-k,β2)。若第i+1点为垂直校正点，则下一校正点的选择范围为：min{α1-k,α2}。

贪心策略要求每一步能尽量选择更多的点，因此第i+1点的选择范围如式(7)：

max{min(β1-k,β2),min(α1-k,α2)}

(7)

第i点为垂直校正点时处理方式相同。

实际数据中，水平校正点的对水平误差校正能力强，垂直校正点的对垂直误差的校正能力强，若连续经过同一类型校正点，易出现另一类型的误差积累而无法进行校正的情况。此外，根据贪心策略，需要尽量选择更多的可行点。因此在规划路径时，应优先选择与当前矫正点类型不同的矫正点。

2 航迹规划算法

2.1 简化的贪心A*算法

A*算法是静态路网中求解最短路的高效方法之一。作为一种启发式算法，A*算法既具有明显的速度优势又在理论上保证收敛到全局最优解，其代价函数如式(8)：

f(n)=g(n)+h(n)

(8)

式中：f(n)为至当前节点n的代价总和；g(n)为航迹起点到该节点的实际代价；h(n)为该节点到航迹终点的预估代价。

通常情况下，启发函数中的代价使用两点的欧氏距离来表示。

传统的A*算法属于二维栅格法，在搜索下一节点过程中，需使用邻域定义确定搜索范围。在非栅格场景中，若直接将A*算法推广至三维空间，代价函数往往不能精确刻画实际航迹代价，使问题求解变得困难。为使其应用于图G(V,E)表示的三维航迹规划问题，对A*算法的中的下一节点的搜索范围重新定义，如图1。

图1 备选校正点Fig. 1 Alternative correction points

算法在确定下一节点时，从当前节点出发，根据最大搜索半径对下一节点进行筛选作为备选节点。由于图G(V,E)属于NPC问题，为降低时间复杂度，对A*算法进行简化：①根据文献[7]，将下一节点定为当前备选节点中式(8)代价最小的节点；②根据贪心策略由式(7)决定备选点的最大搜索半径，并对备选点进行剪枝操作，最终经贪心策略剪枝后结果如图2，此时缩小了矫正点的选择范围。

图2 贪心策略后的备选校正点Fig. 2 Alternative correction points under greedy strategy

得到图2备选矫正点后，可以使用A*算法快速求解图G(V,E)表示的三维航迹规划问题得到初始路径。

简化后的贪心A*算法如下：

Algorithm:简化贪心A∗算法Input: α1,α2,β1,β2起点A,终点B,栈S,栈L,当前节点TEMP,航迹空间TOutput: LWhile(不能到达终点B)1)If(TEMP==NULL)

2) TEMP =A3) L.Push(TEMP)4) 随机选择下一节点类型;5)Else根据公式(7)确定下一节点类型6)根据公式(2),(3)确定备选点范围7)If (备选点存在)使用公式(8)确定备选范围内各点代价,将各备选点按代价从大至小排序后压入栈S.Push()8)将栈S中代价最小的弹出TEMP=S.Pop(),其余压栈L.Push(TEMP)9)Else TEMP = L.Pop()10)输出:L

2.2 稀疏A*初始化的贪心蚁群算法

蚁群算法是基于蚂蚁之间简单交互的高效智能算法。在优化无人救生船航行路径[8]，解决无人机三维的航迹规划问题[9]，以及在旅行商问题求解[3]上，均获得了较好的结果。

使用蚁群算法求解航迹问题时，将航迹空间定义为所有蚂蚁的可行解空间。通过不断更新路径上的信息素浓度，由于正反馈作用，路径不断优化，最终得到满足规划要求的航迹。

当每次路径搜索完成时，需要更新航迹空间的信息素浓度分布。通过信息素的信息交互方式是蚁群算法的独特优势。信息素更新如式(9)、式(10)：

τi,j=(1-ρ)×φi,j+Δγi,j

(9)

(10)

由于蚁群算法初始搜索策略的随机性，使算法在起始点会搜索许多无效路径，既影响了算法的航迹规划质量，又增加了系统运行时间。同时经典蚁群算法信息素均按预设常数更新，未考虑蚁群整体质量。笔者针对蚁群算法做出改进：①使用A*算法生成的初始航迹替换蚁群算法中的随机初始化方法；② 蚂蚁选择下一节点时，使用式(7)对校正点剪枝后，再使用轮盘赌法进行选择；③借鉴粒子群中的适应度概念，定义一个自适应系数对信息素更新进行修改，此时，式(10)变为式(11)：

(11)

式中：Fk定义第k只蚂蚁航迹距离终点距离的倒数；s为启发值常数，取值范围为[0,1]。

此外，对式(5)的目标，在保证式(6)路径存在的情况下，通过降低初始化路径节点数求得。

稀疏A*初始化的贪心蚁群算法定义如下：

Algorithm: 稀疏A∗初始化的贪心蚁群算法Input:Input: α1,α2,β1,β2起点A,终点B,栈L,蚂蚁数量m,迭代次数n,航迹空间TOutput: min(L′)1)使用简化贪心A∗算法得到初始航迹L2)For i=1:m t = random(length(L));3)将L中t至length(L)的航迹点随机生成4)For j=2:n For i=1:m For k=1: length(L)5)按公式(7)确定校正类型与备选点范围6)从备选点中找到所有满足公式(2),(3)的校正点7)根据信息素浓度,按照经典轮盘赌法选择校正点8)If(航迹能到达终点B) 记录航迹至L′9)按照公式(9)(11)更新航迹空间的信息素浓度10)输出:L′中公式(6)最小的路径

3 三维空间的Dubins曲线平滑算法

在作业中，考虑到飞行器航迹需要满足对最小转弯半径的约束，需对航迹进行光滑处理。目前对航迹的平滑处理已经成为航迹规划研究中的必备步骤。如：文献[10]由于B样条曲线具有参数表达简单，局部修改性，凸包性等优点，使用三次B样条曲线对规划轨迹进行优化；文献[15]针对航带间转弯轨迹进行了分类研究；文献[5]使用对规划航迹进行了优化，此外圆切法等方法也被用于不规则航迹的光滑。

但目前的方法都存在一定缺陷：B样条方法需要对整条航迹进行规划，不能满足航迹最短的要求；圆切法规划的航迹会使路径绕过校正点；经典Dubins曲线仅用于对二维平面曲线进行光滑。

由于目前算法均不能满足三维空间无人机下需经过规划点且轨迹最短的要求。因此，笔者将Dubins曲线算法推广至三维空间，该算法为降低运算复杂度，将曲线的求解过程投影到二维进行求解后再映射回三维计算航迹长度。算法的主要步骤为：①坐标点变换；②圆心计算；③切点计算；④坐标点逆变换；⑤得到光滑后航迹。

以给定的三个矫正点A′(x′1,y′1,z′1)，B′(x′2,y′2,z′2)，C′(x′3,y′3,z′3)为例说明算法的详细实现过程。

3.1 坐标点变换

对坐标系进行变换，将A′,B′,C′ 三个点所在平面作为X′O′Y′平面，如图3。将图3(a)所示的三维问题转化为图3(b)中在X′O′Y′平面中的二维问题。

图3 坐标变换Fig. 3 Coordinate transformation

首先进行坐标点变换。构造向量A′B′，B′C′分别为k1,k2。

(12)

进行施密特正交化：

(13)

对结果进行单位化后得到η1,η2：

(14)

η3=(ηi,ηj,ηk)

(15)

(16)

此时对于坐标系中任意一向量P有:

P(2)=P(3)M

(17)

式中：P(2)为以新坐标系为基的向量；P(3)为以原坐标系为基的向量。

将A′,B′,C′分别代入式(17)进行坐标变换，截取坐标前两个维度后得到：A(x1,y1)，B(x2,y2)，C(x3,y3)。

3.2 圆心计算

根据以上步骤，得到了二维平面上的三个坐标A,B,C。此时需要在二维平面上使用Dubins曲线进行路径的平滑，如图4。

图4 平面Dubins曲线Fig. 4 Dubins curve in plane

直线LAB的两点式为：

(18)

设圆心O坐标为(x0,y0)，将式(18)化为一般式：

(y2-y1)x-(x2-x1)y-(y2-y1)x1-(x2-x1)y1=0

(19)

已知圆心O到LAB的最短距离为R，由点到直线距离公式有：

(20)

又有OB·AB=0，则：

(x2-x1)(x2-x0)-(y2-y1)(y2-y0)=0

(21)

联立式(20)、式(21)得到两个解，圆心O为两个解中距离C(x3,y3)点较近的坐标。

3.3 切点计算

此时已经求得圆心，根据圆心进一步对切点进行计算。

由图4可知，过C(x3,y3)点的直线LCT的直线为：

kx-y-kx3+y3=0

(22)

设切点T坐标为(xT,yT)，切点T到圆心的最短距离为半径R。即：

(23)

联立公式(22)、式(23)并求解，得到斜率不同的两个切点T1,T2。最终切点T坐标取T1,T2中∠BOTi(i=1,2)中角度较小的Ti。

3.4 坐标点逆变换

按照对切点T进行逆变换P(2)M-1得到T′(x′T,y′T,z′T)，此时LA′B′,LT′C′,B′T′,即为光滑后的航迹。

4 仿真实验验证

4.1 实验设计

设计仿真实验验证笔者算法的有效性，：

飞行器起点为S点，终点为E点。在飞行区域内分布有613个校正点(编号0至612)，用于飞行器误差校正，且校正点在空间的坐标已知。校正点类型包括垂直校正点与水平校正点。校正点分布位置依赖于地形，无统一规律。飞行器每飞行1 m，垂直误差和水平误差将各增加δ个专用单位，下简称单位。到达终点时，垂直误差和水平误差均应小于θ个单位。为简化问题，设当垂直和水平误差均小于θ单位时，飞行器仍能按照规划路径飞行。

在出发地A点，飞行器的垂直和水平误差均为0。飞行器在垂直误差校正可将其垂直误差校正为0，水平误差保持不变。在水平误差校正点也只校正水平误差为0。矫正机制设置如1.1节。假设飞行器的最小转弯半径为R。

给定参数如下(数据来自2019年全国研究生数学建模竞赛F题)：

α1=25，α2=15，β1=20，β2=25，

θ=30，δ=0.001，R=200

4.2 实验对比

实验环境：Intel Core i5-7300HQ，GTX 1050Ti显卡，16G内存+128SSD，算法使用MATLAB R2014b编写。

不同算法的路径规划对比结果如表1。粒子群算法参数设置为最大迭代次数为100，粒子数量为20。蚁群算法参数为最大迭代次数为100，蚁群数量为20，启发值常数为0.5，信息素常量为10。笔者算法为：蚁群+贪心+启发值+ A*初始化。

表1 实验对比Table 1 Experiment comparison

表1中为消除算法随机性，将算法运行10次取最优结果作为最终结果实验结果表明，在校正点的航迹规划任务中，使用贪心策略的蚁群算法的最优长度为112.013 km，优于粒子群算法与经典A*算法。启发值融合贪心策略后，能有效降低经典蚁群算法在无效路径的尝试，时间从8.11 降至6.11 s，路径长度为109.798 km。证明启发值能有效优化算法的运行效率与结果。

4.3 参数影响

考虑到算法主要参数对结果的影响，对蚁群算法的迭代次数，蚁群数量，启发值进行消融实验。评价指标为十次算法运行中最优路径。

参数选择时，较大的蚁群数量与较低的启发值常数可以增加有效解数量，而解的数量多说明该组解的鲁棒性较强，因而多次迭代后往往能获得较短的航迹长度。启发值，迭代次数和蚁群数量三个模型参数对路径长度的影响程度，如图5。

图5 模型参数对比Fig. 5 Comparison of model parameters

由图5可知，启发值选择0.1时，蚁群评价对路径的改进效果最好，此时路径长度较短。迭代次数并不总是越多越好，最好的结果在蚁群数量为100时得到。蚁群数量太少削弱了解的多样性，蚁群的数量多则会增加算法复杂度，但根据实验，蚁群数量合理增加有助于生成更高效的解。

为研究各个参数相互影响下路径长度的变化，对比了迭代次数,启发值与蚁群数量对最终结果的影响，如图6。由于图5中，迭代次数和启发值的波动剧烈且蚁群数量最优长度差距较小，为突出参数间的影响，选择蚁群数量差距较大的20和100作为对比。结果显示，蚁群数量固定时，最优解均出现在图像的右下角，即迭代次数和启发值取较小值。又图6可知，随着蚁群数量的增加，较小的启发值常数下算法规划的路径长度更短。

图6 模型复合参数对比Fig. 6 Comparison of model composite parameters

综合考虑图5、图6。笔者在蚁群算法时均选择启发值常数为0.1 蚁群数量为100 ，最大迭代次数100。

使用Dubins曲线平滑后航迹路径如图7。

图7展示了对起点S点至终点E点的规划路径，规划空间为100 km×100 km×10 km，空间内分布有包括起点0，终点612，共计613个规划点。实心圆点为垂直校正点，空心圆点为水平校正点。仅使用贪心A*算法便可以获得较好的规划结果，贪心蚁群算法规划的轨迹更加平滑，加入A*初始化后，贪心蚁群算法规划航迹的前半部分得到了优化，规划航迹进一步缩短。

图7 规划航迹Fig. 7 Planning track

最终规划航迹节点编号为：0，506，294，91，607，540，250，340，277，612，经过8个校正点，航迹总长度为104.06km。改进后结果较原始结果航迹长度减少了约6%，时间减少了约25%。

6 结 语

通过对基于校正点的航迹规划研究，提出了一种使用A*算法优化的自适应蚁群算法，该算法融合多种策略对原始蚁群进行优化，不仅增强了算法在解空间上的搜索效率，也提高了最优解的质量。在参数设置上，当蚁群数量较大时，设置较小的启发值常数能获得更好的结果。迭代次数需要根据具体实验进行适当选择，太小算法难以收敛，太大则会造成算法出现过拟合现象。

对贪心蚁群算法使用A*算法进行初始化，能同时满足多约束下综合代价最小的规划要求。实验仿真表明：改进后航迹长度减少了约6%，时间减少了约25%。使用A*算法优化的自适应蚁群算法能有效的在复杂环境中针对多目标进行航迹规划，保证了无人机在极限条件下完成战术任务的成功率。