无人机三维航迹规划_无人机航迹规划

一、无人机三维航迹规划

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

1.1路径最短约束

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

1.2威胁最小约束

无人机通过躲避障碍物来确保安全作业航迹。设定障碍物威胁区为圆柱体形式，其投影如下图所示，记圆柱体中心坐标为 $C_k$，半径为 $R_k$，外围为碰撞威胁区 $D$，则无人机的避障威胁成本与其路径段$\Vert \overrightarrow{W_{ij}{W}_{i,j+1}}\Vert$和障碍物中心 $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(\overrightarrow{W_{ij}{W}_{i,j+1}}\right)$$
其中：
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​(Wij​Wi,j+1​ ​)=⎩ ⎨ ⎧​0γc​((D+Rk​)−dk​)∞​(dk​>D+Rk​)(Rk​<dk​<D+Rk​)(dk​<Rk​)​

1.3飞行高度约束

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

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

1.4飞行转角约束

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

记