一、无人机三维航迹规划

三维航迹规划是无人机在执行任务过程中的非常关键的环节，三维航迹规划的主要目的是在满足任务需求和自主飞行约束的基础上，计算出发点和目标点之间的最佳航路。

1.1路径最短约束

无人机航迹规划的首要目标是寻找起飞点和目标点之间最短路程的飞行路径方案。一般地，记无人机的飞行路径点为Wij=(xij,yij,zij)W_{i j}=\left(x_{i j}, y_{i j}, z_{i j}\right)Wij=(xij,yij,zij)即在第 iii 条飞行路径中第 jjj个路径点的无人机三维空间位置，则整条飞行路径 XiX_{i}Xi 可表示为包含 nnn 个路径点的三维数组。将 2 个路径点之间的欧氏距离记作路径段∥WijWi,j+1→∥\left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\|WijWi,j+1，则与无人机飞行路径相关的成本函数 F1F_{1}F1 为:
F1(Xi)=∑j=1n−1∥WijWi,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−1WijWi,j+1

1.2威胁最小约束

无人机通过躲避障碍物来确保安全作业航迹。设定障碍物威胁区为圆柱体形式，其投影如下图所示，记圆柱体中心坐标为 CkC_{k}Ck，半径为 RkR_{k}Rk，外围为碰撞威胁区 DDD，则无人机的避障威胁成本与其路径段∥WijWi,j+1→∥\left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\|WijWi,j+1和障碍物中心 CkC_{k}Ck的距离dkd_{k}dk 成反比。
在这里插入图片描述

将飞行环境下的障碍物威胁区集合记作KKK，障碍物威胁成本惩罚系数记作γcγ_{c}γc ，则与无人机避障威胁相关的成本函数 F2F_{2}F2为:
F2(Xi)=∑j=1n−1∑k=1KTk(WijWi,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)
其中：
Tk(WijWi,j+1→)={0(dk>D+Rk)γc((D+Rk)−dk)(RkD+R_{k}\right) \\ \gamma_{c}\left(\left(D+R_{k}\right)-d_{k}\right) & \left(R_{k})=⎩⎨⎧0γc((D+Rk)−dk)∞(dk>D+Rk)(Rk

1.3飞行高度约束

无人机的飞行高度通常受到最小高度 hminh_{min}hmin 和最大高度hmaxh_{max}hmax 的约束限制，如下图所示，其中TijT_{ij}Tij 为地形的高度，ZijZ_{ij}Zij为无人机相对于海平面的高度。
在这里插入图片描述

将无人机在路径点 WijW_{ij}Wij处距离基准地形地面的高度记作 hijh_{ij}hij，即 ZijZ_{ij}Zij和 TijT_{ij}Tij 的差，则与无人机当前路径点 WijW_{ij}Wij相关的成本函数 HijH_{ij}Hij 为:
Hij={γh(hij−hmax⁡)(hij>hmax⁡)0(hmin⁡h_{\max }\right) \\ 0 & \left(h_{\min }⎨⎧γh(hij−hmax)0γh(hmin−hij)∞(hij>hmax)(hmin 同时，将无人机飞行高度超出约束限制条件的惩罚系数记作 γhγ_{h}γh，则与无人机飞行路径相关的成本函数 F3F_{3}F3为:
F3(Xi)=∑j=1nHijF_{3}\left(X_{i}\right)=\sum_{j=1}^{n} H_{i j}F3(Xi)=j=1∑nHij

1.4飞行转角约束

无人机的飞行转角控制参数主要包括水平转弯角和竖直俯仰角，这 2 个参数变量必须符合无人机的实际转角约束限制，否则航迹规划模型无法生成具有可行性的飞行路径。如下图所示，∥WijWi,j+1→∥\left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\|WijWi,j+1和∥Wij+1Wi,j+2→∥\left\|\overrightarrow{W_{i j+1} W_{i, j+2}}\right\|Wij+1Wi,j+2表示无人机飞行路径中的 2 个连续路径段，Wij′Wi,j+1′→\overrightarrow{W_{i j}^{\prime} W_{i, j+1}^{\prime}}Wij′Wi,j+1′和Wij+1′Wi,j+2′→\overrightarrow{W_{i j+1}^{\prime} W_{i, j+2}^{\prime}}Wij+1′Wi,j+2′是其在xoy 平面的投影。
在这里插入图片描述

记𝒌为轴正方向的单位向量，则Wij+1′Wi,j+2′→\overrightarrow{W_{i j+1}^{\prime} W_{i, j+2}^{\prime}}Wij+1′Wi,j+2′的计算式和水平转弯角 αijα_{ij}αij、竖直俯仰角βi,j+1β_{i,j+1}βi,j+1 计算式为：
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⁡(zi,j+1−zij∥Wij′Wi,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=arctanWij′Wi,j+1′zi,j+1−zij
同时，将无人机的水平转弯角和竖直俯仰角超出约束限制条件的惩罚系数分别记作a1a_{1}a1和a2a_{2}a2，则与无人机飞行转角相关的成本函数F4F_{4}F4 为:
F4(Xi)=a1∑j=1n−2αij+a2∑j=1n−1∣βij−β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多因素约束的飞行成本函数

综合考虑与无人机飞行路径 XiX_{i}Xi 相关的最短路径、最小威胁，以及飞行高度和飞行转角等限制，基于多因素约束的飞行成本函数 FFF 为:
F(Xi)=∑k=14bkFk(Xi)F\left(X_{i}\right)=\sum_{k=1}^{4} b_{k} F_{k}\left(X_{i}\right)F(Xi)=k=1∑4bkFk(Xi)
式中 bkb_{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求解三维无人机路径规划：

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);

一次结果如下：