一、无人机三维航迹规划
三维航迹规划是无人机在执行任务过程中的非常关键的环节,三维航迹规划的主要目的是在满足任务需求和自主飞行约束的基础上,计算出发点和目标点之间的最佳航路。
1.1路径最短约束
无人机航迹规划的首要目标是寻找起飞点和目标点之间最短路程的飞行路径方案。一般地,记无人机的飞行路径点为
W
i
j
=
(
x
i
j
,
y
i
j
,
z
i
j
)
W_{i j}=\left(x_{i j}, y_{i j}, z_{i j}\right)
Wij=(xij,yij,zij)即在第
i
i
i 条飞行路径中第
j
j
j个路径点的无人机三维空间位置,则整条飞行路径
X
i
X_{i}
Xi 可表示为包含
n
n
n 个路径点的三维数组。将 2 个路径点之间的欧氏距离记作路径段
∥
W
i
j
W
i
,
j
+
1
→
∥
\left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\|
WijWi,j+1
,则与无人机飞行路径相关的成本函数
F
1
F_{1}
F1 为:
F
1
(
X
i
)
=
∑
j
=
1
n
−
1
∥
W
i
j
W
i
,
j
+
1
→
∥
F_{1}\left(X_{i}\right)=\sum_{j=1}^{n-1}\left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\|
F1(Xi)=j=1∑n−1
WijWi,j+1
1.2威胁最小约束
无人机通过躲避障碍物来确保安全作业航迹。设定障碍物威胁区为圆柱体形式,其投影如下图所示,记圆柱体中心坐标为
C
k
C_{k}
Ck,半径为
R
k
R_{k}
Rk,外围为碰撞威胁区
D
D
D,则无人机的避障威胁成本与其路径段
∥
W
i
j
W
i
,
j
+
1
→
∥
\left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\|
WijWi,j+1
和障碍物中心
C
k
C_{k}
Ck的距离
d
k
d_{k}
dk 成反比。
将飞行环境下的障碍物威胁区集合记作
K
K
K,障碍物威胁成本惩罚系数记作
γ
c
γ_{c}
γc ,则与无人机避障威胁相关的成本函数
F
2
F_{2}
F2为:
F
2
(
X
i
)
=
∑
j
=
1
n
−
1
∑
k
=
1
K
T
k
(
W
i
j
W
i
,
j
+
1
→
)
F_{2}\left(X_{i}\right)=\sum_{j=1}^{n-1} \sum_{k=1}^{K} T_{k}\left(\overrightarrow{W_{i j} W_{i, j+1}}\right)
F2(Xi)=j=1∑n−1k=1∑KTk(WijWi,j+1)
其中:
T
k
(
W
i
j
W
i
,
j
+
1
→
)
=
{
0
(
d
k
>
D
+
R
k
)
γ
c
(
(
D
+
R
k
)
−
d
k
)
(
R
k
<
d
k
<
D
+
R
k
)
∞
(
d
k
<
R
k
)
T_{k}\left(\overrightarrow{W_{i j} W_{i, j+1}}\right)=\left\{\begin{array}{ll} 0 & \left(d_{k}>D+R_{k}\right) \\ \gamma_{c}\left(\left(D+R_{k}\right)-d_{k}\right) & \left(R_{k}<d_{k}<D+R_{k}\right) \\ \infty & \left(d_{k}<R_{k}\right) \end{array}\right.
Tk(WijWi,j+1)=⎩
⎨
⎧0γc((D+Rk)−dk)∞(dk>D+Rk)(Rk<dk<D+Rk)(dk<Rk)
1.3飞行高度约束
无人机的飞行高度通常受到最小高度
h
m
i
n
h_{min}
hmin 和最大高度
h
m
a
x
h_{max}
hmax 的约束限制,如下图 所示,其中
T
i
j
T_{ij}
Tij 为地形的高度,
Z
i
j
Z_{ij}
Zij为无人机相对于海平面的高度。
将无人机在路径点
W
i
j
W_{ij}
Wij处距离基准地形地面的高度记作
h
i
j
h_{ij}
hij,即
Z
i
j
Z_{ij}
Zij和
T
i
j
T_{ij}
Tij 的差,则与无人机当前路径点
W
i
j
W_{ij}
Wij相关的成本函数
H
i
j
H_{ij}
Hij 为:
H
i
j
=
{
γ
h
(
h
i
j
−
h
max
)
(
h
i
j
>
h
max
)
0
(
h
min
<
h
i
j
<
h
max
)
γ
h
(
h
min
−
h
i
j
)
(
0
<
h
i
j
<
h
min
)
∞
(
h
i
j
<
0
)
H_{i j}=\left\{\begin{array}{ll} \gamma_{h}\left(h_{i j}-h_{\max }\right) & \left(h_{i j}>h_{\max }\right) \\ 0 & \left(h_{\min }<h_{i j}<h_{\max }\right) \\ \gamma_{h}\left(h_{\min }-h_{i j}\right) & \left(0<h_{i j}<h_{\min }\right) \\ \infty & \left(h_{i j}<0\right) \end{array}\right.
Hij=⎩
⎨
⎧γh(hij−hmax)0γh(hmin−hij)∞(hij>hmax)(hmin<hij<hmax)(0<hij<hmin)(hij<0)
同时,将无人机飞行高度超出约束限制条件的惩罚系数记作
γ
h
γ_{h}
γh,则与无人机飞行路径相关的成本函数
F
3
F_{3}
F3为:
F
3
(
X
i
)
=
∑
j
=
1
n
H
i
j
F_{3}\left(X_{i}\right)=\sum_{j=1}^{n} H_{i j}
F3(Xi)=j=1∑nHij
1.4飞行转角约束
无人机的飞行转角控制参数主要包括水平转弯角和竖直俯仰角,这 2 个参数变量必须符合无人机的实际转角约束限制,否则航迹规划模型无法生成具有可行性的飞行路径。如下图所示,
∥
W
i
j
W
i
,
j
+
1
→
∥
\left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\|
WijWi,j+1
和
∥
W
i
j
+
1
W
i
,
j
+
2
→
∥
\left\|\overrightarrow{W_{i j+1} W_{i, j+2}}\right\|
Wij+1Wi,j+2
表示无人机飞行路径中的 2 个连续路径段,
W
i
j
′
W
i
,
j
+
1
′
→
\overrightarrow{W_{i j}^{\prime} W_{i, j+1}^{\prime}}
Wij′Wi,j+1′和
W
i
j
+
1
′
W
i
,
j
+
2
′
→
\overrightarrow{W_{i j+1}^{\prime} W_{i, j+2}^{\prime}}
Wij+1′Wi,j+2′是其在xoy 平面的投影。
记𝒌为轴正方向的单位向量,则
W
i
j
+
1
′
W
i
,
j
+
2
′
→
\overrightarrow{W_{i j+1}^{\prime} W_{i, j+2}^{\prime}}
Wij+1′Wi,j+2′的计算式和水平转弯角
α
i
j
α_{ij}
αij、竖直俯仰角
β
i
,
j
+
1
β_{i,j+1}
βi,j+1 计算式为:
W
i
j
′
W
i
,
j
+
1
′
→
=
k
×
(
W
i
j
W
i
,
j
+
1
→
×
k
)
α
i
j
=
arctan
(
W
i
j
′
W
i
,
j
+
1
′
→
×
W
i
,
j
+
1
′
W
i
,
j
+
2
′
‾
W
i
j
′
W
i
,
j
+
1
′
→
⋅
W
i
,
j
+
1
′
W
i
,
j
+
2
′
‾
)
β
i
j
=
arctan
(
z
i
,
j
+
1
−
z
i
j
∥
W
i
j
′
W
i
,
j
+
1
′
→
∥
)
\begin{array}{c} \overrightarrow{W_{i j}^{\prime} W_{i, j+1}^{\prime}}=\boldsymbol{k} \times\left(\overrightarrow{W_{i j} W_{i, j+1}} \times \boldsymbol{k}\right) \\ \alpha_{i j}=\arctan \left(\frac{\overrightarrow{W_{i j}^{\prime} W_{i, j+1}^{\prime}} \times \overline{W_{i, j+1}^{\prime} W_{i, j+2}^{\prime}}}{\overrightarrow{W_{i j}^{\prime} W_{i, j+1}^{\prime}} \cdot \overline{W_{i, j+1}^{\prime} W_{i, j+2}^{\prime}}}\right) \\ \beta_{i j}=\arctan \left(\frac{z_{i, j+1}-z_{i j}}{\left\|\overrightarrow{W_{i j}^{\prime} W_{i, j+1}^{\prime}}\right\|}\right) \end{array}
Wij′Wi,j+1′=k×(WijWi,j+1×k)αij=arctan(Wij′Wi,j+1′⋅Wi,j+1′Wi,j+2′Wij′Wi,j+1′×Wi,j+1′Wi,j+2′)βij=arctan
Wij′Wi,j+1′
zi,j+1−zij
同时,将无人机的水平转弯角和竖直俯仰角超出约束限制条件的惩罚系数分别记作
a
1
a_{1}
a1和
a
2
a_{2}
a2,则与无人机飞行转角相关的成本函数
F
4
F_{4}
F4 为:
F
4
(
X
i
)
=
a
1
∑
j
=
1
n
−
2
α
i
j
+
a
2
∑
j
=
1
n
−
1
∣
β
i
j
−
β
i
,
j
−
1
∣
F_{4}\left(X_{i}\right)=a_{1} \sum_{j=1}^{n-2} \alpha_{i j}+a_{2} \sum_{j=1}^{n-1}\left|\beta_{i j}-\beta_{i, j-1}\right|
F4(Xi)=a1j=1∑n−2αij+a2j=1∑n−1∣βij−βi,j−1∣
3.5多因素约束的飞行成本函数
综合考虑与无人机飞行路径
X
i
X_{i}
Xi 相关的最短路径、最小威胁,以及飞行高度和飞行转角等限制,基于多因素约束的飞行成本函数
F
F
F 为:
F
(
X
i
)
=
∑
k
=
1
4
b
k
F
k
(
X
i
)
F\left(X_{i}\right)=\sum_{k=1}^{4} b_{k} F_{k}\left(X_{i}\right)
F(Xi)=k=1∑4bkFk(Xi)
式中
b
k
b_{k}
bk为各因素的权重系数。
参考文献:
[1]吕石磊,范仁杰,李震,陈嘉鸿,谢家兴.基于改进蝙蝠算法和圆柱坐标系的农业无人机航迹规划[J/OL].农业机械学报:1-19
[2]褚宏悦,易军凯.无人机安全路径规划的混沌粒子群优化研究[J/OL].控制工程:1-8
[3]MD Phung, Ha Q P . Safety-enhanced UAV Path Planning with Spherical Vector-based Particle Swarm Optimization: arXiv, 10.1016/j.asoc.2021.107376[P]. 2021.
二、蜣螂优化算法
蜣螂优化算法(Dung beetle optimizer,DBO)由Jiankai Xue和Bo Shen于2022年提出,该算法主要受蜣螂的滚球、跳舞、觅食、偷窃和繁殖行为的启发所得。
2.1蜣螂滚球
(1)当蜣螂前行无障碍时,蜣螂在滚粪球过程中会利用太阳进行导航,下图中红色箭头表示滚动方向
本文假设光源的强度会影响蜣螂的位置,蜣螂在滚粪球过程中位置更新如下:
x
i
(
t
+
1
)
=
x
i
(
t
)
+
α
×
k
×
x
i
(
t
−
1
)
+
b
×
Δ
x
,
Δ
x
=
∣
x
i
(
t
)
−
X
w
∣
\begin{aligned} x_{i}(t+1) &=x_{i}(t)+\alpha \times k \times x_{i}(t-1)+b \times \Delta x, \\ \Delta x &=\left|x_{i}(t)-X^{w}\right| \end{aligned}
xi(t+1)Δx=xi(t)+α×k×xi(t−1)+b×Δx,=∣xi(t)−Xw∣
其中,
t
t
t表示当前迭代次数,
x
i
(
t
)
x_{i}(t)
xi(t)表示第
i
i
i次蜣螂在第t次迭代中的位置信息,
k
∈
(
0
,
0.2
]
k∈(0,0.2]
k∈(0,0.2]为扰动系数,
b
b
b为
(
0
,
1
)
(0,1)
(0,1) 之间的随机数,
α
\alpha
α取 -1 或 1 ,
X
w
X^{w}
Xw表示全局最差位置,
Δ
x
\Delta x
Δx用于模拟光的强度变化。
其中,
α
\alpha
α的取值采用算法1:
(2)当蜣螂遇到障碍物无法前进时,它需要通过跳舞来重新调整自己,以获得新的路线。本文使用切线函数来模仿跳舞行为,以此获得新的滚动方向,滚动方向仅考虑为
[
0
,
π
]
[0,π]
[0,π]之间。
蜣螂一旦成功确定新的方向,它应该继续向后滚动球。蜣螂的位置更新如下:
x
i
(
t
+
1
)
=
x
i
(
t
)
+
tan
(
θ
)
∣
x
i
(
t
)
−
x
i
(
t
−
1
)
∣
x_{i}(t+1)=x_{i}(t)+\tan (\theta)\left|x_{i}(t)-x_{i}(t-1)\right|
xi(t+1)=xi(t)+tan(θ)∣xi(t)−xi(t−1)∣
其中,
θ
\theta
θ为偏转角,其取值为
[
0
,
π
]
[0,π]
[0,π],采用算法2:
2.2蜣螂繁殖
在自然界中,雌性蜣螂将粪球被滚到适合产卵的安全地方并将其隐藏起来,以此为后代提供一个安全的环境。受此启发,因而提出了一种边界选择策略以此模拟雌性蜣螂产卵的区域:
L
b
∗
=
max
(
X
∗
×
(
1
−
R
)
,
L
b
)
U
b
∗
=
min
(
X
∗
×
(
1
+
R
)
,
U
b
)
\begin{array}{l} L b^{*}=\max \left(X^{*} \times(1-R), L b\right) \\ U b^{*}=\min \left(X^{*} \times(1+R), U b\right) \end{array}
Lb∗=max(X∗×(1−R),Lb)Ub∗=min(X∗×(1+R),Ub)
其中,
X
∗
X^{*}
X∗表示当前最优位置,
L
b
∗
L b^{*}
Lb∗和
U
b
∗
U b^{*}
Ub∗分别表示产卵区的下限和上限,
R
=
1
−
t
/
T
m
a
x
R=1−t/T_{max}
R=1−t/Tmax,
T
m
a
x
T_{max}
Tmax表示最大迭代次数,
L
b
Lb
Lb和
U
b
Ub
Ub分别表示优化问题的下限和上限。
雌性蜣螂一旦确定了产卵区,就会选择在该区域育雏球产卵。每只雌性蜣螂在每次迭代中只产生一个卵,可以看出,产卵区的边界范围是动态变化的,主要由R值决定。因此,育雏球的位置在迭代过程中也是动态的,其定义如下:
B
i
(
t
+
1
)
=
X
∗
+
b
1
×
(
B
i
(
t
)
−
L
b
∗
)
+
b
2
×
(
B
i
(
t
)
−
U
b
∗
)
B_{i}(t+1)=X^{*}+b_{1} \times\left(B_{i}(t)-L b^{*}\right)+b_{2} \times\left(B_{i}(t)-U b^{*}\right)
Bi(t+1)=X∗+b1×(Bi(t)−Lb∗)+b2×(Bi(t)−Ub∗)
其中,
B
i
(
t
)
B_{i}(t)
Bi(t)表示第t次迭代中第 i个育雏球的位置信息,
b
1
b_{1}
b1和
b
2
b_{2}
b2均为1×D的随机向量,D表示优化问题的维度。
产卵区的选择如算法3所示:
2.3蜣螂觅食
雌性蜣螂所产的卵会逐渐长大,一些已经成熟的小蜣螂会从地下出来寻找食物,小蜣螂的最佳觅食区建模如下:
L
b
b
=
max
(
X
b
×
(
1
−
R
)
,
L
b
)
U
b
b
=
min
(
X
b
×
(
1
+
R
)
,
U
b
)
\begin{array}{l} L b^{b}=\max \left(X^{b} \times(1-R), L b\right) \\ U b^{b}=\min \left(X^{b} \times(1+R), U b\right) \end{array}
Lbb=max(Xb×(1−R),Lb)Ubb=min(Xb×(1+R),Ub)
其中,
X
b
X^{b}
Xb表示全局最优位置,
L
b
b
L b^{b}
Lbb和
U
b
b
U b^{b}
Ubb分别表示最佳觅食区的下限和上限。
小蜣螂的位置更新如下:
x
i
(
t
+
1
)
=
x
i
(
t
)
+
C
1
×
(
x
i
(
t
)
−
L
b
b
)
+
C
2
×
(
x
i
(
t
)
−
U
b
b
)
x_{i}(t+1)=x_{i}(t)+C_{1} \times\left(x_{i}(t)-L b^{b}\right)+C_{2} \times\left(x_{i}(t)-U b^{b}\right)
xi(t+1)=xi(t)+C1×(xi(t)−Lbb)+C2×(xi(t)−Ubb)
其中,
x
i
(
t
)
x_{i}(t)
xi(t)表示第t次迭代中第i只小蜣螂在的位置,
C
1
C_{1}
C1是服从正态分布的随机数,
C
2
C_{2}
C2为(0,1)的随机向量。
2.4蜣螂偷窃
另一方面,一些蜣螂从其他蜣螂那里偷粪球,盗贼蜣螂的位置更新如下:
x
i
(
t
+
1
)
=
X
b
+
S
×
g
×
(
∣
x
i
(
t
)
−
X
∗
∣
+
∣
x
i
(
t
)
−
X
b
∣
)
x_{i}(t+1)=X^{b}+S \times g \times\left(\left|x_{i}(t)-X^{*}\right|+\left|x_{i}(t)-X^{b}\right|\right)
xi(t+1)=Xb+S×g×(∣xi(t)−X∗∣+
xi(t)−Xb
)
其中,
x
i
(
t
)
x_{i}(t)
xi(t)表示在第t次迭代中第i个盗贼蜣螂的位置,g为服从正态分布的1×D随机向量,S为常数。
2.5DBO描述
滚球蜣螂、繁殖蜣螂、觅食蜣螂和偷窃蜣螂的比例分布如下:
DBO算法描述如下:
参考文献:Xue, J., Shen, B. Dung beetle optimizer: a new meta-heuristic algorithm for global optimization. J Supercomput (2022). https://doi.org/10.1007/s11227-022-04959-6
三、实验结果
将蜣螂优化算法(DBO)与白鲸优化算法(BWO)、北方苍鹰优化算法(NGO)、鹈鹕优化算法(POA)、鲸鱼优化算法(WOA)参与比较,部分代码如下:
close all
clear
clc
dbstop if all error
global model
model = CreateModel(); % 创建模型
F='F1';
[Xmin,Xmax,dim,fobj] = fun_info(F);%获取函数信息
pop=100;%种群大小(可以自己修改)
maxgen=200;%最大迭代次数(可以自己修改)
[fMin1,bestX1,ConvergenceCurve1] = BWO(pop, maxgen,Xmin,Xmax,dim,fobj);
[fMin2,bestX2,ConvergenceCurve2] = NGO(pop, maxgen,Xmin,Xmax,dim,fobj);
[fMin3,bestX3,ConvergenceCurve3] = POA(pop, maxgen,Xmin,Xmax,dim,fobj);
[fMin4,bestX4,ConvergenceCurve4] = WOA(pop, maxgen,Xmin,Xmax,dim,fobj);
[fMin5,bestX5,ConvergenceCurve5] = DBO(pop, maxgen,Xmin,Xmax,dim,fobj);
四、完整MATLAB代码
代码获取见博客底端博主联系方式。