目录
- 1.无人机路径规划模型
- 2.粒子群算法PSO原理
- 3.结果展示
- 4.参考文献
- 5.代码获取
1.无人机路径规划模型
路径最优性
为了实现UAV的高效运行，计划的路径需要在某一特定标准上达到最优。UAV飞行路径Xi表示为UAV需要飞过的一系列n个航路点，每个航路点对应搜索地图中的一个路径节点，其坐标为Pij = (xij, yij, zij)。通过计算两个节点之间的欧几里得距离，计算出路径长度相关的成本F1:
F 1 ( X i ) = ∑ j = 1 n − 1 ∥ P i j P i , j + 1 → ∥ F_1\left(X_i\right)=\sum_{j=1}^{n-1}\left\|\overrightarrow{P_{ij}P_{i,j+1}}\right\| F1(Xi)=j=1∑n−1 PijPi,j+1
安全性和可行性约束
除了最优性外，计划的路径还需要通过引导UAV避开操作空间中出现的障碍来确保UAV的安全运行。设K为所有威胁的集合，每个威胁假设为一个圆柱体，其投影中心坐标为Ck，半径为Rk。对于给定的路径段 ∥ P i j P i , j + 1 → ∥ \left\|\overrightarrow{P_{ij}P_{i,j+1}}\right\| PijPi,j+1 ，与威胁相关的成本与其到Ck的距离dk成正比。考虑UAV的直径D和碰撞区的危险距离S，可以计算威胁成本F2:
F 2 ( X i ) = ∑ j = 1 n − 1 ∑ k = 1 K T k ( P i j P i , j + 1 → ) F_2(X_i)=\sum_{j=1}^{n-1}\sum_{k=1}^KT_k(\overrightarrow{P_{ij}P_{i,j+1}}) F2(Xi)=j=1∑n−1k=1∑KTk(PijPi,j+1)
T k ( P i j , P i j + 1 ) = { 0 , if d k > S + D + R k , ( S + D + R k ) − d k , if D + R k < d k ≤ S + D + R k , ∞ , if d k ≤ D + R k . T_k(P_{ij}, P_{ij+1}) = \begin{cases} 0, & \text{if } d_k > S + D + R_k, \\ (S + D + R_k) - d_k, & \text{if } D + R_k < d_k \leq S + D + R_k, \\ \infty, & \text{if } d_k \leq D + R_k. \end{cases} Tk(Pij,Pij+1)=⎩ ⎨ ⎧0,(S+D+Rk)−dk,∞,if dk>S+D+Rk,if D+Rk<dk≤S+D+Rk,if dk≤D+Rk.
飞行高度约束
UAV飞行高度通常受到两个极限的约束，即最小和最大高度。设最小和最大高度分别为hmin和hmax。与航点Pij相关的高度成本计算:
H i j = { ∣ h i j − ( h max + h min ) 2 ∣ , if h min ≤ h i j ≤ h max ∞ , otherwise , (3) H_{ij}=\begin{cases}|h_{ij}-\frac{(h_{\max}+h_{\min})}{2}|,&\text{if}h_{\min}\leq h_{ij}\leq h_{\max}\\\infty,&\text{otherwise},\end{cases}\tag{3} Hij={∣hij−2(hmax+hmin)∣,∞,ifhmin≤hij≤hmaxotherwise,(3)
其中，hij表示相对于地面的飞行高度。将所有航路点的Hij求和，得到飞行高度成本F3:
F 3 ( X i ) = ∑ j = 1 n H i j (4) F_3(X_i)=\sum_{j=1}^nH_{ij}\tag{4} F3(Xi)=j=1∑nHij(4)
平滑度约束
UAV平滑度是对UAV方位角和俯仰角约束，这些是生成可行路径的关键。
UAV转弯角度:
ϕ 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 ′ → ) (5) \phi_{ij}=\arctan\left(\frac{\left\|\overrightarrow{P_{ij}^{\prime}P_{i,j+1}^{\prime}}\times\overrightarrow{P_{i,j+1}^{\prime}P_{i,j+2}^{\prime}}\right\|}{\overrightarrow{P_{ij}^{\prime}P_{i,j+1}^{\prime}}.\overrightarrow{P_{i,j+1}^{\prime}P_{i,j+2}^{\prime}}}\right)\tag{5} ϕij=arctan Pij′Pi,j+1′.Pi,j+1′Pi,j+2′ Pij′Pi,j+1′×Pi,j+1′Pi,j+2′ (5)
UAV俯仰角度:
ψ i j = arctan ( z i , j + 1 − z i j ∥ P i j ′ P i , j + 1 ′ → ∥ ) (6) \psi_{ij}=\arctan\left(\frac{z_{i,j+1}-z_{ij}}{\left\|\overrightarrow{P_{ij}^{\prime}P_{i,j+1}^{\prime}}\right\|}\right)\tag{6} ψij=arctan Pij′Pi,j+1′ zi,j+1−zij (6)
计算平滑成本F4为:
F 4 ( X i ) = a 1 ∑ j = 1 n − 2 ϕ i j + a 2 ∑ j = 1 n − 1 ∣ ψ i j − ψ i , j − 1 ∣ (7) F_4(X_i)=a_1\sum_{j=1}^{n-2}\phi_{ij}+a_2\sum_{j=1}^{n-1}\mid\psi_{ij}-\psi_{i,j-1}\mid\tag{7} F4(Xi)=a1j=1∑n−2ϕij+a2j=1∑n−1∣ψij−ψi,j−1∣(7)
考虑路径Xi的最优性、安全性和可行性约束，总体成本函数可以定义为:
F ( X i ) = ∑ k = 1 4 b k F k ( X i ) (8) F(X_i)=\sum_{k=1}^4b_kF_k(X_i)\tag{8} F(Xi)=k=1∑4bkFk(Xi)(8)
三维坐标 P i j = ( x i j , y i j , z i j ) ∈ X i P_{ij}=(x_{ij},y_{ij},z_{ij})\in X_{i} Pij=(xij,yij,zij)∈Xi到球形坐标 u i j = ( ρ i j , ψ i j , ϕ i j ) ∈ Ω i u_{ij}=(\rho_{ij}, \psi_{ij}, \phi_{ij}) \in \Omega_{i} uij=(ρij,ψij,ϕij)∈Ωi转换:
x i j = x i , j − 1 + ρ i j s i n ψ i j c o s ϕ i j , y i j = y i , j − 1 + ρ i j s i n ψ i j s i n ϕ i j , z i j = z i , j − 1 + ρ i j c o s ψ i j . (9) \begin{aligned}x_{ij}&=x_{i,j-1}+\rho_{ij}sin\psi_{ij}cos\phi_{ij},\\y_{ij}&=y_{i,j-1}+\rho_{ij}sin\psi_{ij}sin\phi_{ij},\\z_{ij}&=z_{i,j-1}+\rho_{ij}cos\psi_{ij}.\end{aligned}\tag{9} xijyijzij=xi,j−1+ρijsinψijcosϕij,=yi,j−1+ρijsinψijsinϕij,=zi,j−1+ρijcosψij.(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.
