一、NMOPSO介绍
基于导航变量的多目标粒子群优化算法（Navigation Variable-based Multi-Objective Particle Swarm Optimization, NMOPSO）是一种专门用于无人机三维路径规划的先进算法。该算法通过将路径规划问题建模为一个多目标优化问题，并利用粒子群优化（PSO）技术来寻找满足无人机运动学约束的帕累托最优路径。NMOPSO算法的核心在于引入导航变量来表示无人机的路径，从而更好地考虑无人机的运动学约束，并通过多目标优化方法生成一组非支配解，以满足不同的应用需求。
二. 无人机路径规划问题描述
2.1 运动学模型和约束
无人机在三维空间中的运动学模型可以用以下方程描述：
{ x ˙ = V cos α cos β y ˙ = V cos α sin β z ˙ = V sin α \begin{cases} \dot{x} = V \cos \alpha \cos \beta \ \dot{y} = V \cos \alpha \sin \beta \ \dot{z} = V \sin \alpha \end{cases} {x˙=Vcosαcosβ y˙=Vcosαsinβ z˙=Vsinα
其中：
无人机的运动学约束条件如下：
{ V min ≤ V ≤ V max ∣ Δ α ∣ = ∣ θ ∣ ≤ θ max ∣ Δ β ∣ = ∣ ψ ∣ ≤ ψ max \begin{cases} V_{\min} \leq V \leq V_{\max} \ |\Delta \alpha| = |\theta| \leq \theta_{\max} \ |\Delta \beta| = |\psi| \leq \psi_{\max} \end{cases} {Vmin≤V≤Vmax ∣Δα∣=∣θ∣≤θmax ∣Δβ∣=∣ψ∣≤ψmax
其中：
- V min V_{\min} Vmin 和 V max V_{\max} Vmax 分别表示无人机的最小和最大线速度。
- θ max \theta_{\max} θmax 和 ψ max \psi_{\max} ψmax 分别表示无人机的最大爬升角和最大转向角。
- θ \theta θ 和 ψ \psi ψ 分别表示无人机的爬升角和转向角的变化量。
这些约束条件确保无人机在飞行过程中不会超出其物理能力范围，从而保证飞行的安全性和可行性。
2.2 路径规划的目标函数
无人机路径规划问题可以描述为在三维空间中找到一条从起点到终点的路径，该路径需要满足以下要求：
- 路径长度最短：减少飞行时间和能量消耗。
- 避免障碍物：确保飞行安全。
- 飞行高度稳定：减少能量消耗并提高飞行稳定性。
- 路径平滑：减少转向角度的变化，降低能量消耗。
这些要求可以通过以下四个目标函数来量化：
2.2.1 路径长度 (F1)
路径长度的目标函数 F 1 F_1 F1 用于衡量路径的总长度。路径长度越短，无人机的飞行效率越高。目标函数 F 1 F_1 F1 定义为：
F 1 = { 1 − ∥ P i 1 P i n → ∥ ∑ j = 1 n − 1 ∥ P i j P i , j + 1 → ∥ , if ∥ P i j P i , j + 1 → ∥ ≥ R min ∞ , otherwise F_1 = \begin{cases} 1 - \frac{\| \overrightarrow{P_{i1}P_{in}} \|}{\sum_{j=1}^{n-1} \| \overrightarrow{P_{ij}P_{i,j+1}} \|}, & \text{if } \| \overrightarrow{P_{ij}P_{i,j+1}} \| \geq R_{\min} \\ \infty, & \text{otherwise} \end{cases} F1=⎩ ⎨ ⎧1−∑j=1n−1∥PijPi,j+1∥∥Pi1Pin∥,∞,if ∥PijPi,j+1∥≥Rminotherwise
其中：
- R min R_{\min} Rmin 是两个航点之间的最小距离。
- ∥ P i j P i , j + 1 → ∥ \| \overrightarrow{P_{ij}P_{i,j+1}} \| ∥PijPi,j+1∥ 表示两个连续航点之间的欧几里得距离。
2.2.2 避碰 (F2)
避碰的目标函数 F 2 F_2 F2 用于确保无人机在飞行过程中避免与障碍物碰撞。目标函数 F 2 F_2 F2 定义为：
F 2 = 1 K ( n − 1 ) ∑ j = 1 n − 1 ∑ k = 1 K T k ( P i j P i , j + 1 → ) F_2 = \frac{1}{K(n-1)} \sum_{j=1}^{n-1} \sum_{k=1}^{K} T_k(\overrightarrow{P_{ij}P_{i,j+1}}) F2=K(n−1)1j=1∑n−1k=1∑KTk(PijPi,j+1)
其中：
- K K K 是工作区域中的障碍物数量。
-
T
k
T_k
Tk 计算如下：
T k ( P i j P i , j + 1 → ) = { 0 , if d k ≥ D + R k + S 1 − d k − D − R k S , if D + R k < d k < D + R k + S ∞ , otherwise T_k(\overrightarrow{P_{ij}P_{i,j+1}}) = \begin{cases} 0, & \text{if } d_k \geq D + R_k + S \\ 1 - \frac{d_k - D - R_k}{S}, & \text{if } D + R_k < d_k < D + R_k + S \\ \infty, & \text{otherwise} \end{cases} Tk(PijPi,j+1)=⎩ ⎨ ⎧0,1−Sdk−D−Rk,∞,if dk≥D+Rk+Sif D+Rk<dk<D+Rk+Sotherwise - d k d_k dk 是障碍物 k k k 的中心到路径段 P i j P i , j + 1 P_{ij}P_{i,j+1} PijPi,j+1 的距离。
- D D D 是无人机的尺寸。
- R k R_k Rk 是障碍物 k k k 的半径。
- S S S 是无人机与障碍物之间的安全距离。
2.2.3 飞行高度 (F3)
飞行高度的目标函数 F 3 F_3 F3 用于确保无人机在飞行过程中保持稳定的飞行高度，以减少能量消耗。目标函数 F 3 F_3 F3 定义为：
F 3 = 1 n ∑ j = 1 n H i j F_3 = \frac{1}{n} \sum_{j=1}^{n} H_{ij} F3=n1j=1∑nHij
其中：
H
i
j
=
{
2
∣
h
i
j
−
h
mean
∣
h
max
−
h
min
,
if
h
min
≤
h
i
j
≤
h
max
∞
,
otherwise
H_{ij} = \begin{cases} \frac{2|h_{ij} - h_{\text{mean}}|}{h_{\max} - h_{\min}}, & \text{if } h_{\min} \leq h_{ij} \leq h_{\max} \\ \infty, & \text{otherwise} \end{cases}
Hij={hmax−hmin2∣hij−hmean∣,∞,if hmin≤hij≤hmaxotherwise
- h max h_{\max} hmax 和 h min h_{\min} hmin 分别是最大和最小相对飞行高度。
- h mean = h max + h min 2 h_{\text{mean}} = \frac{h_{\max} + h_{\min}}{2} hmean=2hmax+hmin 是优选的相对飞行高度。
- h i j h_{ij} hij 是无人机在航点 P i j P_{ij} Pij 处的高度。
2.2.4 平滑度 (F4)
平滑度的目标函数 F 4 F_4 F4 用于衡量无人机飞行路径的平滑程度，以减少转向角度的变化，从而降低能量消耗。目标函数 F 4 F_4 F4 定义为：
F 4 = 1 n − 2 ∑ j = 1 n − 2 ∣ η i j ∣ π F_4 = \frac{1}{n-2} \sum_{j=1}^{n-2} \frac{|\eta_{ij}|}{\pi} F4=n−21j=1∑n−2π∣ηij∣
其中：
η
i
j
=
arctan
(
∥
P
i
j
P
i
,
j
+
1
→
×
P
i
,
j
+
1
P
i
,
j
+
2
→
∥
P
i
j
P
i
,
j
+
1
→
⋅
P
i
,
j
+
1
P
i
,
j
+
2
→
)
\eta_{ij} = \arctan \left( \frac{\| \overrightarrow{P_{ij}P_{i,j+1}} \times \overrightarrow{P_{i,j+1}P_{i,j+2}} \|}{\overrightarrow{P_{ij}P_{i,j+1}} \cdot \overrightarrow{P_{i,j+1}P_{i,j+2}}} \right)
ηij=arctan(PijPi,j+1⋅Pi,j+1Pi,j+2∥PijPi,j+1×Pi,j+1Pi,j+2∥)
- η i j \eta_{ij} ηij 是两个连续路径段之间的转向角。
三. 多目标优化方法求解无人机路径规划介绍
3.1. 导航变量表示
NMOPSO算法采用导航变量来表示无人机的路径，每个路径段由三个参数描述：
通过导航变量，可以将无人机的路径表示为一系列有序的航点，每个航点由导航变量确定其在三维空间中的位置。
由于上述目标函数之间可能存在冲突，因此采用多目标优化方法来寻找帕累托最优解。
3.2 帕累托最优解
帕累托最优解的定义如下：
- 一个解 X X X 是非支配的，如果不存在其他解 X ′ X' X′ 使得 F i ( X ′ ) ≤ F i ( X ) F_i(X') \leq F_i(X) Fi(X′)≤Fi(X) 对所有 i i i 成立，并且至少有一个 j j j 使得 F j ( X ′ ) < F j ( X ) F_j(X') < F_j(X) Fj(X′)<Fj(X)。
- 一个解 X ∗ X^* X∗ 是帕累托最优的，如果它是非支配的。
- 帕累托最优集 P ∗ P^* P∗ 是所有非支配解的集合。
3.3 导航变量的多目标粒子群优化算法（NMOPSO）
NMOPSO算法基于粒子群优化（PSO）技术，通过模拟粒子在搜索空间中的运动来寻找最优解。每个粒子代表一个可能的无人机路径，粒子的位置和速度根据一定的规则进行更新。算法通过评估每个粒子的适应度，并根据适应度值来指导粒子的搜索方向，从而逐步逼近最优解。
无人机路径规划问题涉及多个目标函数，这些目标函数可能相互冲突。NMOPSO算法通过多目标优化方法来寻找帕累托最优解，这些目标函数包括：
- 路径长度（ F 1 F_1 F1）：最小化路径总长度，以减少飞行时间和能量消耗。
- 避碰（ F 2 F_2 F2）：确保无人机在飞行过程中避免与障碍物碰撞。
- 飞行高度（ F 3 F_3 F3）：维持稳定的飞行高度，以减少能量消耗并提高飞行稳定性。
- 路径平滑度（ F 4 F_4 F4）：最小化路径中的转向角度变化，以降低能量消耗。
NMOPSO算法通过以下步骤实现：
-
初始化：
- 设置粒子群的大小、最大迭代次数、惯性权重、学习因子等参数。
- 随机生成一组路径，作为粒子群的初始位置。每个路径由导航变量表示，包括路径段的长度、爬升角和转向角。
- 初始化粒子的物理变量，如速度和位置，并根据约束条件进行调整。
- 计算每个粒子当前路径的适应度，根据目标函数 F 1 , F 2 , F 3 , F 4 F_1, F_2, F_3, F_4 F1,F2,F3,F4。
- 初始化非支配解集 P P P，将初始粒子群中的非支配解加入 P P P。
- 建立超网格，根据非支配解集 P P P 中各解的目标函数值，为后续的领导者选择做准备。
-
迭代更新：
- 选择领导者：
- 遍历超网格，计算每个超立方体的拥挤度。
- 根据拥挤度随机选择一个领导者，作为粒子更新的参考点。
- 更新粒子的速度和位置：
- 根据粒子的当前位置、个人最好位置和领导者的位置，更新粒子的速度：
v t + 1 i = w ⋅ v t i + c 1 ⋅ r 1 ⋅ ( p b e s t t i − x t i ) + c 2 ⋅ r 2 ⋅ ( l b e s t t i − x t i ) v_{t+1}^i = w \cdot v_t^i + c_1 \cdot r_1 \cdot (pbest_t^i - x_t^i) + c_2 \cdot r_2 \cdot (lbest_t^i - x_t^i) vt+1i=w⋅vti+c1⋅r1⋅(pbestti−xti)+c2⋅r2⋅(lbestti−xti) - 根据更新后的速度，更新粒子的位置：
x t + 1 i = x t i + v t + 1 i x_{t+1}^i = x_t^i + v_{t+1}^i xt+1i=xti+vt+1i
- 根据粒子的当前位置、个人最好位置和领导者的位置，更新粒子的速度：
- 应用变异机制：
- 随机选择一个粒子的导航变量，按照区域变异机制进行变异：
x t , i j i = x t , i j i + N i j ⋅ G t ⋅ x t , p b e s t , i i x_{t,ij}^i = x_{t,ij}^i + N_{ij} \cdot G_t \cdot x_{t,pbest,i}^i xt,iji=xt,iji+Nij⋅Gt⋅xt,pbest,ii - 其中， N i j N_{ij} Nij 是服从正态分布的随机数， G t G_t Gt 是变异增益。
- 随机选择一个粒子的导航变量，按照区域变异机制进行变异：
- 评估新路径：
- 将变异后的导航变量转换为笛卡尔坐标，生成新的飞行路径。
- 根据目标函数 F 1 , F 2 , F 3 , F 4 F_1, F_2, F_3, F_4 F1,F2,F3,F4 计算新路径的适应度。
- 更新非支配解集
P
P
P：
- 将新生成的路径加入非支配解集 P P P，并去除被支配的解。
- 根据需要进行剪枝操作，保持非支配解集的规模在合理范围内。
- 更新超网格：
- 根据更新后的非支配解集 P P P，重新建立超网格，为下一次迭代的领导者选择做准备。
- 终止条件判断：
- 如果达到最大迭代次数或满足其他终止条件，停止算法，输出非支配解集 P P P。
- 否则，继续进行下一次迭代。
- 选择领导者：
-
输出结果：
- 从非支配解集 P P P 中提取所有路径，作为帕累托最优解。
- 根据应用需求，对帕累托最优路径进行进一步筛选和优化，生成最终的飞行路径。
3.4算法流程
1. 初始化
- 设置参数：确定粒子群的大小、最大迭代次数、惯性权重、学习因子等参数。
- 生成初始路径：随机生成一组路径，作为粒子群的初始位置。每个路径由导航变量表示，包括路径段的长度、爬升角和转向角。
- 初始化粒子的物理变量：为每个粒子初始化速度和位置，并根据约束条件进行调整。
- 计算适应度：根据目标函数 F 1 , F 2 , F 3 , F 4 F_1, F_2, F_3, F_4 F1,F2,F3,F4 计算每个粒子当前路径的适应度。
- 初始化非支配解集：将初始粒子群中的非支配解加入非支配解集。
- 建立超网格：根据非支配解集中的解的目标函数值，建立超网格，为后续的领导者选择做准备。
2. 迭代更新
-
选择领导者：
- 遍历超网格，计算每个超立方体的拥挤度。
- 根据拥挤度随机选择一个领导者，作为粒子更新的参考点。
-
更新粒子的速度和位置：
- 根据粒子的当前位置、个人最好位置和领导者的位置，更新粒子的速度：
v t + 1 i = w ⋅ v t i + c 1 ⋅ r 1 ⋅ ( p b e s t t i − x t i ) + c 2 ⋅ r 2 ⋅ ( l b e s t t i − x t i ) v_{t+1}^i = w \cdot v_t^i + c_1 \cdot r_1 \cdot (pbest_t^i - x_t^i) + c_2 \cdot r_2 \cdot (lbest_t^i - x_t^i) vt+1i=w⋅vti+c1⋅r1⋅(pbestti−xti)+c2⋅r2⋅(lbestti−xti) - 根据更新后的速度，更新粒子的位置：
x t + 1 i = x t i + v t + 1 i x_{t+1}^i = x_t^i + v_{t+1}^i xt+1i=xti+vt+1i
- 根据粒子的当前位置、个人最好位置和领导者的位置，更新粒子的速度：
-
应用变异机制：
- 随机选择一个粒子的导航变量，按照区域变异机制进行变异：
x t , i j i = x t , i j i + N i j ⋅ G t ⋅ x t , p b e s t , i i x_{t,ij}^i = x_{t,ij}^i + N_{ij} \cdot G_t \cdot x_{t,pbest,i}^i xt,iji=xt,iji+Nij⋅Gt⋅xt,pbest,ii - 其中， N i j N_{ij} Nij 是服从正态分布的随机数， G t G_t Gt 是变异增益。
- 随机选择一个粒子的导航变量，按照区域变异机制进行变异：
-
评估新路径：
- 将变异后的导航变量转换为笛卡尔坐标，生成新的飞行路径。
- 根据目标函数 F 1 , F 2 , F 3 , F 4 F_1, F_2, F_3, F_4 F1,F2,F3,F4 计算新路径的适应度。
-
更新非支配解集：
- 将新生成的路径加入非支配解集，并去除被支配的解。
- 根据需要进行剪枝操作，保持非支配解集的规模在合理范围内。
-
更新超网格：
- 根据更新后的非支配解集，重新建立超网格，为下一次迭代的领导者选择做准备。
-
终止条件判断：
- 如果达到最大迭代次数或满足其他终止条件，停止算法，输出非支配解集。
- 否则，继续进行下一次迭代。
3. 输出结果
- 生成帕累托最优路径：从非支配解集中提取所有路径，作为帕累托最优解。
- 路径后处理：根据应用需求，对帕累托最优路径进行进一步筛选和优化，生成最终的飞行路径。
四、部分代码及部分结果
%% Problem Definition
model = CreateModel(); % Create search map and parameters
model_name = 6;
nVar=model.n; % Number of Decision Variables = searching dimension of PSO = number of path nodes
VarSize=[1 nVar]; % Size of Decision Variables Matrix
% Lower and upper Bounds of particles (Variables)
VarMin.x=model.xmin;
VarMax.x=model.xmax;
VarMin.y=model.ymin;
VarMax.y=model.ymax;
VarMin.z=model.zmin;
VarMax.z=model.zmax;
VarMax.r=3*norm(model.start-model.end)/nVar; % r is distance
VarMin.r=VarMax.r/9;
% Inclination (elevation)
AngleRange = pi/4; % Limit the angle range for better solutions
VarMin.psi=-AngleRange;
VarMax.psi=AngleRange;
% Azimuth
VarMin.phi=-AngleRange;
VarMax.phi=AngleRange;
% Lower and upper Bounds of
alpha=0.5;
VelMax.r=alpha*(VarMax.r-VarMin.r);
VelMin.r=-VelMax.r;
VelMax.psi=alpha*(VarMax.psi-VarMin.psi);
VelMin.psi=-VelMax.psi;
VelMax.phi=alpha*(VarMax.phi-VarMin.phi);
VelMin.phi=-VelMax.phi;
