一、无人机三维航迹规划
三维航迹规划是无人机在执行任务过程中的非常关键的环节，三维航迹规划的主要目的是在满足任务需求和自主飞行约束的基础上，计算出发点和目标点之间的最佳航路。
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.
二、蜣螂优化算法DBO
蜣螂优化算法原理
三、麻雀搜索算法SSA
麻雀搜索算法原理
四、求解结果
将蜣螂优化算法DBO和麻雀搜索算法SSA求解三维无人机路径规划：
完整MATLAB代码路径：https://mbd.pub/o/bread/mbd-Y56WlZxy
dbstop if all error
global model
model = CreateModel(); % 创建模型
F='F1';
[Xmin,Xmax,dim,fobj] = fun_info(F);%获取函数信息
pop=100;%种群大小(可以自己修改)
maxgen=500;%最大迭代次数(可以自己修改)
[fMin4,bestX4,ConvergenceCurve4] = SSA(pop, maxgen,Xmin,Xmax,dim,fobj);
[fMin5,bestX5,ConvergenceCurve5] = DBO(pop, maxgen,Xmin,Xmax,dim,fobj);
一次结果如下：
4.1收敛曲线图
4.2三维路径规划图
4.3二维航线平面图
五、参考代码
完整MATLAB代码路径：https://mbd.pub/o/bread/mbd-Y56WlZxy
文件夹内包含完整代码及使用说明，点击main.m即可运行。
![SSM整合案例[企业权限管理系统]-学习笔记01【SVN的基本介绍】](https://img-blog.csdnimg.cn/5eaf7bb30a474bac9da267b9986d3ba6.png)
