目录
1.算法原理
2.数学模型
路径最优性
为了实现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
→
∥
(1)
F_1(X_i)=\sum_{j=1}^{n-1}\left\|\overrightarrow{P_{ij}P_{i,j+1}}\right\|\tag{1}
F1(Xi)=j=1∑n−1
PijPi,j+1
(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
→
)
,
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
.
(2)
\begin{cases}&F_2(X_i)=\sum_{j=1}^{n-1}\sum_{k=1}^KT_k(\overrightarrow{P_{ij}P_{i,j+1}}),\\&T_k(\overrightarrow{P_{ij}P_{i,j+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}\end{cases}\tag{2}
⎩
⎨
⎧F2(Xi)=∑j=1n−1∑k=1KTk(PijPi,j+1
),Tk(PijPi,j+1
)=⎩
⎨
⎧0,(S+D+Rk)−dk,∞,ifdk>S+D+RkifD+Rk<dk≤S+D+Rkifdk≤D+Rk.(2)
飞行高度约束
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)
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.