基于改进量子粒子群算法的无人机三维航迹规划

Three-Dimensional UAV Trajectory Planning Based on Improved QPSO Algorithm

摘要: 为提升无人机航迹规划的质量，提出了一种改进的量子粒子群优化(QPSO)算法。通过构建威胁山峰地形图，并考虑路径长度、飞行高度和路径平滑度等因素建立了适应度函数。首先采用Logistic混沌映射初始化粒子位置，增强初始种群的随机性，并结合蜂群算法中的“侦查蜂”策略，避免陷入局部最优，再经三次样条插值平滑路径。最后通过多次实验与不同算法进行比对，结果表明改进的QPSO算法能够规划出质量更高的路径。

Abstract: To increase the quality of UAV trajectory planning, an enhanced version of the quantum particle swarm optimization (QPSO) technique is proposed. By creating a topographic map of dangerous mountain peaks and taking flight height, path length, and path smoothness into account, the fitness function is determined. Prior to obtaining a smooth path through cubic spline interpolation, the particle positions are initialized using Logistic Chaos Mapping, which increases the initial population’s randomness and works in tandem with the swarm algorithm’s “scout bee” strategy to prevent falling into the local optimum. Finally, a number of tests are conducted to compare the enhanced QPSO algorithm with other algorithms. The simulation results indicate that the upgraded QPSO algorithm designs a better path.

1. 引言

随着无人机在众多领域的广泛应用，包括军事领域的战场侦察[1]和对地面目标的打击[2]，以及民用领域的农业活动[3]、物资配送[4]和地形测绘[5]等。如何高效规划其飞行路径，以提高任务执行的效率和安全性，已经成为一个备受关注的研究领域。航迹规划技术对于提升无人机在复杂环境中的操作性能至关重要，它涉及到避开障碍物、优化飞行路线、降低能耗和完成任务目标等多方面的考量。

现有诸多经典优化算法以解决该问题。如Dijkstra算法[6]、A*算法[7]、RRT算法[8]等传统算法，但这类算法存在收敛慢、难以面向复杂环境寻优等问题。此外是各类群智能算法，如粒子群算法[9]、蜂群算法[10]、蚁群算法[11]等。

为解决传统粒子群优化(PSO)算法在面临威胁环境时的路径优化质量不足以及易陷入局部最优的问题，本文首先构建了威胁山峰地形图，并考虑了路径长度、飞行高度限制及路径平滑度建立了适应度函数。接着采用量子粒子群(QPSO)优化算法结合高斯变异算子开展了无人机三维航迹规划，该算法通过引入Logistic混沌映射以进行粒子的初始位置设定，替代了传统PSO算法中的初始化方法，实现了种群初始化的混沌性。这种改进有助于提高算法的全局搜索能力，并提升寻优的精确度。并在迭代过程中结合了蜂群算法的“侦查蜂”的跳出局部最优行为，即在个体极值陷入局部最优时对地图进行大范围搜索，从而提高了规划质量。

2. 模型建立

2.1. 环境建模

山峰这类地形障碍构成了飞行路径上的主要威胁。山峰作为一种无法逾越的障碍，无人机必须采取规避策略，要么绕过山峰，要么在确保安全的前提下从山顶上方飞越。因此，在进行无人机路径规划时，将山峰这类地形障碍模拟为不可穿越的尖锐峰顶，以确保无人机能够安全有效地规避这些障碍。这种地形建模方法有助于无人机在实际飞行中采取正确的行动策略，避免与山峰发生碰撞，同时优化飞行路径。

山区地形高度可用式(1)模拟[12]：

$z\left(x,y\right)={\displaystyle \sum _{i=1}^n{h}_i}\exp \left[-{\left(\frac{x-x_i}{x_{si}}\right)}^2-{\left(\frac{y-y_i}{y_{si}}\right)}^2\right]$ (1)

其中$n$ 表示山峰个数。$\left(x_i,y_i\right)$ 代表第$i$ 个山峰的中心坐标；$h_i$ 为地形参数，控制高度。$x_{si}$ 和$y_{si}$ 分别是第$i$ 个山峰沿两轴方向的衰减量、控制坡度，$x_{si}$ 和$y_{si}$ 值越大山峰越平坦，反之越陡峭，如图1所示。

Figure 1. Schematic diagram of two-dimensional section of mountain peak

图1. 山峰二维切面示意图

在任务场景空间随机生成多个山峰，其中所有山峰的高度、坡度和中心位置等均随机生成，任务场景三维建模如图2所示。

Figure 2. Schematic diagram of 3D task scene

图2. 三维任务场景示意图

2.2. 适应度函数设计

在进行无人机的三维路径规划时，不仅要确保路径能够安全地避开障碍物进行飞行，还应对路径的整体质量进行评估。因此，本文采取了一种多目标优化函数，该函数综合考虑了路径长度、飞行高度、转弯角度多个因素，以实现对路径质量的全面评价。通过这种多维度的评价方法，可以更有效地指导无人机在复杂环境中进行高效且安全的路径规划。假定路径由$m$ 个节点组成，其中$\left(x_i,y_i,z_i\right)$ 表示第$i$ 个节点位置。

在评估算法性能的过程中，路径长度是一个关键的指标。路径长度越表示完成任务所需的时间更短，这通常表示路径规划的质量更高。在构建路径长度函数时，首先需要确定路径的起点和终点。接下来，在起点和终点之间随机选择5个中间点。通过三次样条插值方法可以生成一条平滑的连续路径。这条路径的长度通过计算样条曲线的弧长来得到，从而为评估路径规划算法提供了一个量化的指标。

路径长度的计算公式如下式所示：

$G_i={\displaystyle \sum _{i=1}^m\sqrt{{\left(x_{i+1}-x_i\right)}^2+{\left(y_{i+1}-y_i\right)}^2+{\left(z_{i+1}-z_i\right)}^2}}$ (2)

确保无人机在指定区域内的安全飞行，对飞行高度的限制至关重要。此外，对飞行高度进行精确控制，可以有效减少因高度波动带来的能量损耗和操作难度。因此，本文将飞行高度作为一个关键的目标函数来考虑，如式(3)所示。

$G_h=\sqrt{\frac{1}{m}{\displaystyle \sum _{i=1}^m{\left(\text{max}\left(\text{0,}{z}_i-h_{\max }\right)\right)}^2+\text{max}{\left(\text{0,}{h}_{\min }-z_i\right)}^2}}$ (3)

式中：$h_{\min }$ 和$h_{\max }$ 分别为无人机三维路径规划的最低飞行高度和最高飞行高度。

在无人机的三维路径规划中，路径平滑度是维持飞行稳定性和安全性的关键因素。特别是在复杂的城市环境中，平滑的路径可以减少因急剧转弯或快速爬升或下降引起的风险。因此，路径规划应当综合考虑水平转弯角度和垂直爬升角度两个方向的平缓性，目标函数定义为式(7)。

${\theta }_i=\arccos \left(\frac{D_{i-1}•D_i}{\Vert D_{i-1}\Vert \cdot \Vert D_i\Vert }\right)$ (4)

$D_i=\left(x_{i+1}-x_i,y_{i+1}-y_i\right)$ (5)

${\gamma }_i=\arctan \left(\frac{z_{i+1}-z_i}{\sqrt{{\left(x_{i+1}-x_i\right)}^2+{\left(y_{i+1}-y_i\right)}^2}}\right)$ (6)

$G_t={\displaystyle \sum _{i=2}^m{w}_{\theta }\cdot {\theta }_i}+{\displaystyle \sum _{i=1}^{m-1}{w}_{\gamma }}\cdot {\gamma }_i$ (7)

式中：$w_{\theta }$ 和$w_{\gamma }$ 分别为无人机三维路径规划的水平转弯角度${\theta }_i$ 和垂直爬升角度${\gamma }_i$ 的权重。

在对航迹规划效果进行评估时，针对路径长度、飞行高度及路径平滑度采用了多目标函数的加权综合方法。总目标函数表示为：

$fitness=w_1{G}_i+w_2{G}_h+w_3{G}_t$ (8)

式中：$w_1$ 、$w_2$ 及$w_3$ 分别表示各目标函数的权重系数。

3. 传统粒子群算法

粒子群算法通过适应度函数评价粒子位置。粒子能够记忆个体与群体的最佳位置。粒子速度决定移动方向和距离，速度随个体最优和群体最优动态变化，通过大量迭代更新位置，最终找到最优解。

对第$i$ 个粒子而言，位置矢量为$x_{i,j}$ ，速度矢量为$v_{i,j}$ 。

粒子位置更新表达式：

$x_{i,j}=x_{i-1,j}+v_{i-1,j}$ (9)

粒子速度更新表达式：

$v_{i,j}=w{v}_{i-1,j}+c_1{r}_1\left(pbes{t}_i-x_{i-1,j}\right)+c_2{r}_2\left(gbes{t}_i-x_{i-1,j}\right)$ (10)

$w$ 为惯性权重，表示对当前速度方向的信任程度；$c_1$ 、$c_2$ 表示个体学习因子以及群体学习因子；$r_1$ 、$r_2$ 为[0, 1]内的随机数。

传统粒子群算法(PSO)流程如图3所示。

Figure 3. Particle swarm optimization algorithm flowchart

图3. 粒子群算法流程图

4. 改进量子粒子群算法

4.1. 高斯变异的量子粒子群算法

粒子群优化(PSO)是一种群体智能算法，与进化计算有许多相似之处。然而，PSO是由鸟类和其他社会生物体的集体行为驱动，而不是适者生存的个体。受经典PSO方法和量子力学理论的启发，文献[13]提出了新的利用高斯概率分布突变算子的量子行为PSO (QPSO)方法，在QPSO中应用高斯突变算子代替随机序列能提高QPSO性能以防止过早收敛到局部最优。本文基于文献[13]来研究无人机三维航迹规划问题。

以往大多数粒子群算法使用均匀概率分布来生成随机数，而这里利用高斯概率分布给出了QPSO中突变算子的新结果。

种群中的粒子数目为$N$ ，在第$t$ 次迭代时，第$n$ 个粒子的位置为$p_n(t)$ ，势场为$p$ ，个体最优位置表示为$pbest$ 。群体最优位置为$gbest$ 。

现在粒子按照以下公式进行迭代：

$\{\begin{array}{cc}{p}_n\left(t+1\right)=p+\beta \left|Mbes{t}_n-p_n\left(t\right)\right|\cdot \ln \left(1/G\right)& k\le 0.5\\ p_n\left(t+1\right)=p-\beta \left|Mbes{t}_n-p_n\left(t\right)\right|\cdot \ln \left(1/G\right)& else\end{array}$ (11)

$Mbes{t}_n=\frac{1}{N}{\displaystyle \sum _{d=1}^{N}pbest}\left(t\right)$ (12)

$p=\frac{G\cdot pbest\left(t\right)+g\cdot gbest}{G+g}$ (13)

式中：$Mbest$ 被定义为所有粒子最佳位置的平均值；其中$\beta$ 为收缩膨胀系数，$G$ 和$g$ 是在[0, 1]范围内使用均值为零、单位方差的高斯分布生成的值[13]。$k$ 是在(0, 1)之间采用均匀分布生成的随机数[13]。

文献[14]指出$\beta$ 取为固定或递减的调节策略在路径规划中效果不佳，而递增策略能提升寻优和规划表现。故本文采用式(14)作为收缩膨胀系数$\beta$ 的表达式：

$\beta ={\beta }_{\min }+\left({\beta }_{\min }+{\beta }_{\max }\right)\cdot t/t_{\max }$ (14)

式中，${\beta }_{\min }$ 与${\beta }_{\max }$ 分别为$\beta$ 的最小值及最大值。$t_{\max }$ 为最大迭代次数。

4.2. 粒子位置初始化的改进

本文引入Logistic混沌映射以提升粒子位置的随机性。混沌系统以其对初值的敏感和结果的不可预知性，常用于生成随机数列。Logistic映射因其结构简单和易于编程，在多个领域包括路径规划和模拟生物种群增长中被广泛使用，其迭代公式见式(15)。

$x_{n+1}=\mu x_n\left(1-x_n\right)$ (15)

式中$x_n$ 表示迭代的状态值，取值区间为(0, 1)。$\mu$ 为混沌系数，决定混沌程度。当取值为3.57时，存在混沌现象，当取值为4时，表现为完全混沌。

但由于0、1等不动点的存在容易使得方程陷入循环，于是本文基于文献[15]引入了改进的Logistic混沌映射以生成$i\times j\times k$ 个散点，如式(16)所示。

$\{\begin{array}{cc}{u}_{i,j,k}=\mu \cdot u_{i-1,j,k}\cdot \left(1-u_{i-1,j,k}\right)& 0.03\le u\le 0.97\\ u_{i,j,k}=0.01+rand\cdot 0.98& else\end{array}$ (16)

最后，采用改进的Logistic混沌映射初始化种群位置，初始位置见式(17)。

$po{s}_{i,j,k}=\left(ma{p}_{\max ,j}-ma{p}_{\min ,j}\right)\cdot u_{i,j,k}+ma{p}_{\min ,j}$ (17)

式中$po{s}_{i,j,k}$ 表示第$i$ 个粒子在第$j$ 个维度下的第$k$ 个散点的位置；$ma{p}_{\max ,j}$ 和$ma{p}_{\min ,j}$ 分别表示在第$j$ 维下，地图范围的最大值和最小值。

4.3. 本算法与蜂群算法的结合

为帮助算法尽可能防止陷入局部最优，本文将蜂群算法中的侦查蜂引入算法，并设置个体迭代次数，当个体最优粒子更新时，个体迭代次数变为1，否则个体迭代次数加1。当个体迭代次数大于设置的搜索阈值时，此时考虑已经陷入局部最优，则调用侦查蜂采用较大步长的方式随机生成新的蜜源位置，如式(18)所示[10]。

$p_{n,j}\left(t\right)=p_{n,j}\left(t\right)+\left(2\cdot rand-1\right)\cdot 0.5\cdot map{}_{\max ,j}$ (17)

之后，对新生成的粒子点计算适应度，并重置该粒子的个体迭代次数，并与个体最优值与群体最优值进行比较更新。

4.4. 改进量子粒子群算法的实现步骤

综上，本算法的实现步骤为：

Step 1：基于式(1)建立三维山峰威胁地形模型，以模仿飞行路径上的主要威胁；

Step 2：设置算法中迭代次数、粒子数量、适应度权重、角度权重、飞行高度、收缩膨胀系数、位置界限与速度界限等参数；

Step 3：利用改进的Logistic混沌映射，通过式(16)和式(17)初始化种群位置，提升粒子初始化的随机性；

Step 4：基于式(2)，式(3)，式(7)及式(8)计算初始粒子的适应度，并进行碰撞检测。倘若无人机飞行高度低于地图在那一点处的地形高度，则判定其与山峰相撞并将该粒子适应度值放大1000倍。并比较得出个体最优及全局最优；

Step 5：进入主循环，经过式(12)计算出所有粒子最佳位置的平均值，并通过式(13)计算出迭代所需的参数$p$ ；

Step 6：在Step 5的基础上经式(11)得出迭代后的粒子位置；

Step 7：如果粒子位置超出界限，则将地图边界设为该粒子此次迭代的位置。

Step 8：基于式(2)，式(3)，式(7)及式(8)计算粒子的适应度，并进行碰撞检测，计算出本次迭代的适应度值，若产生碰撞，则适应度放大1000倍；

Step 9：更新个体粒子最优位置，如果进行更新，个体迭代次数变为1，繁殖个体迭代次数加1；

Step 10：若粒子个体迭代次数达到搜索阈值，经式(18)进行粒子位置全局搜索，计算粒子的新适应度与粒子的最优适应度进行比较，然后将个体迭代次数变为1。若粒子个体迭代次数未达搜索阈值，跳到Step 11；

Step 11：根据适应度的比较结果，更新种群内的个体极值和全局极值；

Step 12：三次样条插值构建平滑路径；

Step 13：判断迭代次数是否达到上限：若达到，输出最优解。若未达到，返回Step 5。

5. 仿真实验

采用MATLAB R2024a软件构建100 × 100 × 100的三维地图进行仿真实验，将本文算法与传统PSO算法、文献[15]的改进PSO算法及传统QPSO算法比较。设置粒子群数量规模为50，迭代次数为100，路径长度适应度权重为0.4，高度与角度适应度均为0.3，转弯角与爬升角权重均为0.5，混沌系数取4，最高飞行高度为100，最低飞行高度为1，收缩膨胀系数最大与最小分别取1和0.6，搜索阈值取3。任意选择某次实验结果，路径对比如图4~6所示，适应度对比如图7所示，多次试验适应度均值和方差对比如表1所示。

Figure 4. Comparison of path planning (Main View)

图4. 路径规划对比(主视图)

Figure 5. Comparison of path planning (Side View)

图5. 路径规划对比(侧视图)

Figure 6. Comparison of path planning (Vertical View)

图6. 路径规划对比(俯视图)

由图4~6，在三维地形环境下，四种算法都能够从起点规划出到达终点的路径，但改进的QPSO算法规划出的路径质量更佳。

根据图7，文献[15]的改进PSO算法最终适应度值约为69.91，PSO算法最终适应度值约为75.37，改进的QPSO算法最终适应度值约为66.51，QPSO算法最终适应度值约为72.81。

通过对本次数据的分析，本文的算法由于增大了初始粒子的混沌性，并采用跳出局部最优的方法加以改进，改进后的算法在全局搜索和收敛性能上都有明显提高，能够规划出更优的路径。

为减少随机性对算法性能的影响，并检验改进后的粒子群优化算法的稳健性，本文对四种不同的算法各进行了50次的仿真实验。通过多次实验得到适应度值的对比结果，这些结果被汇总在图8中展示。同时计算四种算法适应度的均值及方差，结果见表1。

Figure 7. Comparison chart of fitness value curves for a single experiment

图7. 单次实验适应度值曲线对比图

由图8知，改进QPSO适应度值曲线适应度的起伏趋势相对于对比的算法的适应度曲线较缓，并且对比算法所在区间范围明显大于改进QPSO算法。

Figure 8. Comparison chart of fitness value curves for multiple experiments

图8. 多次实验适应度值曲线对比图

由表1可知，改进的QPSO算法适应度均值为68.6397，PSO算法适应度均值为71.9797，QPSO算法适应度均值为71.1989，文献[15]的改进PSO算法适应度均值为70.8312。本文提出的算法在适应度均值方面显著低于对比算法。且方差较其他两种对比算法更低，表明其稳定性相对更高，进一步证实了该算法在收敛性能上的稳定性。

Table 1. Comparison of average fitness and fitness variance of four algorithms in 50 experiments

表1. 四种算法50次试验适应度均值与方差对比

	改进的QPSO	PSO	QPSO	改进的PSO
适应度均值	68.6397	71.9797	71.1989	70.8312
适应度方差	3.8162	8.4477	5.3682	3.9714

6. 结论

本文面向复杂环境中无人机的路径规划问题，提出一种改进的量子粒子群优化算法。通过构建威胁山峰地形图并综合考虑路径长度、飞行高度和路径平滑度来建立适应度函数。采用混沌映射初始化粒子位置，增强了初始种群的随机性。同时，结合蜂群算法中的“侦查蜂”策略，有效避免了算法陷入局部最优，增强了路径规划的质量和算法的鲁棒性。再经三次样条插值构建平滑的路径。多次实验的适应度值对比显示，本文的算法具有更低的适应度均值和方差，说明其在航迹规划中不仅表现更优，而且结果更加稳健。