【算法应用】基于粒子群算法PSO求解无人机集群路径规划问题

1.无人机路径规划模型

路径最优性

为了实现UAV的高效运行，计划的路径需要在某一特定标准上达到最优。UAV飞行路径Xi表示为UAV需要飞过的一系列n个航路点，每个航路点对应搜索地图中的一个路径节点，其坐标为Pij = (xij, yij, zij)。通过计算两个节点之间的欧几里得距离，计算出路径长度相关的成本F1:
$$F_1\left(X_i\right)=\sum _{j=1}^{n-1}\Vert \overrightarrow{P_{ij}{P}_{i,j+1}}\Vert$$

安全性和可行性约束

除了最优性外，计划的路径还需要通过引导UAV避开操作空间中出现的障碍来确保UAV的安全运行。设K为所有威胁的集合，每个威胁假设为一个圆柱体，其投影中心坐标为Ck，半径为Rk。对于给定的路径段$\Vert \overrightarrow{P_{ij}{P}_{i,j+1}}\Vert$，与威胁相关的成本与其到Ck的距离dk成正比。考虑UAV的直径D和碰撞区的危险距离S，可以计算威胁成本F2:
$$F_2(X_i)=\sum _{j=1}^{n-1}\sum _{k=1}^K{T}_k(\overrightarrow{P_{ij}{P}_{i,j+1}})$$
$$T_k(P_{ij},P_{ij+1})=\{\begin{array}{ll}0,& \text{if }{d}_k>S+D+R_k,\\ (S+D+R_k)-d_k,& \text{if }D+R_k<d_k\le S+D+R_k,\\ \infty ,& \text{if }{d}_k\le D+R_k.\end{array}$$
飞行高度约束

UAV飞行高度通常受到两个极限的约束，即最小和最大高度。设最小和最大高度分别为hmin和hmax。与航点Pij相关的高度成本计算:
$$H_{ij}=\{\begin{array}{ll}|h_{ij}-\frac{(h_{\max }+h_{\min })}{2}|,& \text{if}{h}_{\min }\le h_{ij}\le h_{\max }\\ \infty ,& \text{otherwise},\end{array}\text{\hspace{1em}(3)}$$
其中，hij表示相对于地面的飞行高度。将所有航路点的Hij求和，得到飞行高度成本F3:
$$F_3(X_i)=\sum _{j=1}^n{H}_{ij}\text{\hspace{1em}(4)}$$

平滑度约束

UAV平滑度是对UAV方位角和俯仰角约束，这些是生成可行路径的关键。
UAV转弯角度:
$${\varphi }_{ij}=\arctan \left(\frac{\Vert \overrightarrow{P_{ij}^{\prime }{P}_{i,j+1}^{\prime }}\times \overrightarrow{P_{i,j+1}^{\prime }{P}_{i,j+2}^{\prime }}\Vert }{\overrightarrow{P_{ij}^{\prime }{P}_{i,j+1}^{\prime }}.\overrightarrow{P_{i,j+1}^{\prime }{P}_{i,j+2}^{\prime }}}\right)\text{\hspace{1em}(5)}$$
UAV俯仰角度:
$${\psi }_{ij}=\arctan \left(\frac{z_{i,j+1}-z_{ij}}{\Vert \overrightarrow{P_{ij}^{\prime }{P}_{i,j+1}^{\prime }}\Vert }\right)\text{\hspace{1em}(6)}$$
计算平滑成本F4为:
$$F_4(X_i)=a_1\sum _{j=1}^{n-2}{\varphi }_{ij}+a_2\sum _{j=1}^{n-1}\mid {\psi }_{ij}-{\psi }_{i,j-1}\mid \text{\hspace{1em}(7)}$$

考虑路径Xi的最优性、安全性和可行性约束，总体成本函数可以定义为:
$$F(X_i)=\sum _{k=1}^4{b}_k{F}_k(X_i)\text{\hspace{1em}(8)}$$

三维坐标$P_{ij}=(x_{ij},y_{ij},z_{ij})\in X_i$到球形坐标$u_{ij}=({\rho }_{ij},{\psi }_{ij},{\varphi }_{ij})\in {\Omega }_i$转换:
$$\begin{array}{rl}{x}_{ij}& =x_{i,j-1}+{\rho }_{ij}sin{\psi }_{ij}cos{\varphi }_{ij},\\ y_{ij}& =y_{i,j-1}+{\rho }_{ij}sin{\psi }_{ij}sin{\varphi }_{ij},\\ z_{ij}& =z_{i,j-1}+{\rho }_{ij}cos{\psi }_{ij}.\end{array}\text{\hspace{1em}(9)}$$

2.粒子群算法PSO原理

【智能算法】粒子群算法（PSO）原理及实现

3.结果展示

4.参考文献

[1] Phung M D, Ha Q P. Safety-enhanced UAV path planning with spherical vector-based particle swarm optimization[J]. Applied Soft Computing, 2021, 107: 107376.

5.代码获取