无人机三维航迹规划

一、无人机三维航迹规划

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

无人机路径

MATLAB无人机

无人机路径规划

无人机路径规划MATLAB

1.1路径最短约束

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

1.2威胁最小约束

无人机通过躲避障碍物来确保安全作业航迹。设定障碍物威胁区为圆柱体形式，其投影如下图所示，记圆柱体中心坐标为 $C_k$，半径为 $R_k$，外围为碰撞威胁区 $D$，则无人机的避障威胁成本与其路径段 ∥ W i j W i , j + 1 → ∥ \left\|\overrightarrow{W_{i j} W_{i, j+1}}\right\| ​Wij​Wi,j+1​ ​ ​和障碍物中心 $C_k$的距离$d_k$ 成反比。

将飞行环境下的障碍物威胁区集合记作$K$，障碍物威胁成本惩罚系数记作${\gamma }_c$ ，则与无人机避障威胁相关的成本函数 $F_2$为:
$$F_2\left(X_i\right)=\sum _{j=1}^{n-1}\sum _{k=1}^K{T}_k\left(W_{ij}{W}_{i,j+1}\right)$$
其中：
$$T_k\left(W_{ij}{W}_{i,j+1}\right)=\{\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}$$

1.3飞行高度约束

无人机的飞行高度通常受到最小高度 $h_{min}$ 和最大高度$h_{max}$ 的约束限制，如下图 所示，其中$T_{ij}$ 为地形的高度，$Z_{ij}$为无人机相对于海平面的高度。

将无人机在路径点 $W_{ij}$处距离基准地形地面的高度记作 $h_{ij}$，即 $Z_{ij}$和 $T_{ij}$ 的差，则与无人机当前路径点 $W_{ij}$相关的成本函数 $H_{ij}$ 为:
$$H_{ij}=\{\begin{array}{ll}{\gamma }_h\left(h_{ij}-h_{\max }\right)& \left(h_{ij}>h_{\max }\right)\\ 0& \left(h_{\min }<h_{ij}<h_{\max }\right)\\ {\gamma }_h\left(h_{\min }-h_{ij}\right)& \left(0<h_{ij}<h_{\min }\right)\\ \infty & \left(h_{ij}<0\right)\end{array}$$
同时，将无人机飞行高度超出约束限制条件的惩罚系数记作 ${\gamma }_h$，则与无人机飞行路径相关的成本函数 $F_3$为:
$$F_3\left(X_i\right)=\sum _{j=1}^n{H}_{ij}$$

1.4飞行转角约束

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

记为轴正方向的单位向量，则$W_{ij+1}^{\prime }{W}_{i,j+2}^{\prime }$的计算式和水平转弯角 ${\alpha }_{ij}$、竖直俯仰角${\beta }_{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×(Wij​Wi,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_2$，则与无人机飞行转角相关的成本函数$F_4$ 为:
$$F_4\left(X_i\right)=a_1\sum _{j=1}^{n-2}{\alpha }_{ij}+a_2\sum _{j=1}^{n-1}\left|{\beta }_{ij}-{\beta }_{i,j-1}\right|$$

3.5多因素约束的飞行成本函数

综合考虑与无人机飞行路径 $X_i$ 相关的最短路径、最小威胁，以及飞行高度和飞行转角等限制，基于多因素约束的飞行成本函数 $F$ 为:
$$F\left(X_i\right)=\sum _{k=1}^4{b}_k{F}_k\left(X_i\right)$$
式中 $b_k$为各因素的权重系数。
参考文献：
[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.