CN103135546B - 基于航段距离和相对位置向量点积的无人机飞行航段切换方法 - Google Patents

Abstract

本发明公开了基于航段距离和相对位置向量点积的无人机的飞行航段切换方法。无人机以航段切换实现整个航线飞行。根据飞控计算机实时计算出的待飞距离和侧偏距可得到此刻的航段距离。当GPS信号全程正常时，采用基于航段距离的航段切换方法。在GPS信号无效时，不进行航段切换。在GPS信号恢复后，根据无人机当前位置和航段的从点/到点计算出相对位置向量点积，与已计算出的航段距离形成复合判断条件，实现以上所有情况下无人机飞行航段的正确切换。

Description

基于航段距离和相对位置向量点积的无人机飞行航段切换方法

技术领域

本发明涉及一种无人机的飞行航段切换方法，该方法基于航段距离和相对位置向量点积，属于无人机飞行控制领域。

背景技术

无人机自主飞行的过程如下所述：

航线信息存储在飞控计算机中，无人机沿航线飞行。一条航线中有n个航路点，相邻两个航路点用直线连接，这条直线称为航段。航段的第一个航路点称为从点，航段的第二个航路点称为到点，无人机所在的位置成为当前点。航线由n-1个航段组成，图1给出了航线和航段的定义。航段切换过程如图1所示，无人机由航段P_i-1P_i的从点P_i-1出发飞向到点P_i，完成航段P_i-1P_i飞行后，更新从点和到点，沿航段P_iP_i+1飞行，此时，从点更新为P_i，到点更新为P_i+1，即完成航段切换。无人机从航段P₁P₂开始飞行，按照以上方法逐次完成剩余的航段飞行，即完成整条航线飞行。

无人机在GPS正常情况下，一般按照一定的方法进行航段切换管理。当GPS信号无效时，无人机无法导航，进行等滚转角盘旋飞行（如图2，无人机在五角星位置出现GPS信号故障），等待GPS信号恢复，等滚转角盘旋飞行时不进行航段切换，但飞控计算机记录GPS信号故障前在飞的航段信息，在GPS信号恢复正常后，无人机将恢复航段切换管理。

目前常采用的航段切换管理方法为基于待飞距离的方法。该切换方法的思想是，当待飞距离小于航段切换门限值时，认为无人机完成了该航段飞行，更新从点和到点，切换到下一个航段飞行。图3给出了待飞距离的定义，图4给出了基于待飞距离的航段切换示意图（其中DtogoG为航段切换条件门限值）。基于待飞距离的方法以无人机与航段切换圆的相对位置进行航段切换管理。当无人机进入航段切换圆后（待飞距离小于DtogoG），进行航段切换，在飞航段由航段P_i-1P_i切为航段P_iP_i+1；当无人机在航段切换圆外时，不进行航段切换。无人机在航段切换圆外有三种情况，第一种情况，无人机正常沿在飞航段飞行，没有进入航段切换圆；第二种情况，无人机侧偏距较大（图4中的方框位置，较大侧偏距通常由气动不对称性、螺旋桨反扭距或风干扰等因素引起），无人机将从航段切换圆旁飞过，进入不了航段切换圆；第三种情况，GPS信号故障时，无人机盘旋等待飞行，在GPS信号恢复时无人机正好在航段切换圆外（图4中三角形位置）。在上述第二情况下或第三种情况下的某些位置（如图4所示三角形位置），基于待飞距离的方法将不能实现正确的航段切换（本应进行航段切换但由于在航段切换圆外而不进行航段切换），无人机将沿航段P_i-1P_i的延长线飞行。

因此，如何保证无人机在GPS信号全程正常时和飞行过程中GPS信号由无效恢复正常时，都能正确地实现航段切换，这对于自主飞行的无人机是非常必要的。

发明内容

本发明的目的是提出一种无人机的飞行航段切换方法，根据无人机当前位置点经纬度，计算出航段距离和相对位置向量点积，准确地判断航段切换条件，使无人机在GPS信号全程正常或由故障恢复后都能正确地实现航段切换。

本发明的航段切换方法通过以下步骤实现：

步骤一：判断GPS信号是否正常，当GPS信号正常时，进入步骤二；当GPS信号无效时，进入步骤五；

步骤二：根据当前点的经纬度计算出待飞距离D_togo和侧偏距D_z，由直角三角形勾股定理计算出航段距离D_sw。

当前点P的经纬度分别为L和B；从点P_i-1的经纬度分别为L_i-1和B_i-1；到点P_i的经纬度分别为L_i和B_i。

侧偏距表达式：

式中地球长半轴R_长＝6378137.0m，地球椭偏度E_D＝0.0033523，

$\mathrm{\α}=\arccos [\left(\frac{k_1\×k_0}{|k_1\×k_0|}\right)\•k]$

其中，k₁为折算为地心纬度的到点P_i在地球坐标系中的天向分量：

$k_1=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}1}\right)\·\cos \left(L_i\right)\\ \cos \left(B_{\mathrm{EC}1}\right)\·\sin \left(L_i\right)\\ \sin \left(B_{\mathrm{EC}1}\right)\end{array}\right]$

k₀为折算为地心纬度的从点P_i-1在地球坐标系中的天向分量：

$k_0=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}0}\right)\·\cos \left(L_{i-1}\right)\\ \cos \left(B_{\mathrm{EC}0}\right)\·\sin \left(L_{i-1}\right)\\ \sin \left(B_{\mathrm{EC}0}\right)\end{array}\right]$

k为折算为地心纬度的当前点P在地球坐标系中的天向分量：

$k=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}}\right)\·\cos \left(L\right)\\ \cos \left(B_{\mathrm{EC}}\right)\·\sin \left(L\right)\\ \sin \left(B_{\mathrm{EC}}\right)\end{array}\right]$

当前点P地心纬度单位（rad）；

从点P_i-1地心纬度单位（rad）

到点P_i地心纬度单位（rad）

h：无人机当前高度；R_短：地球短半轴；R_长：地球长半轴；

待飞距离表达式：

$D_{\mathrm{togo}}=\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2}$

其中，Δx＝R_p·cosB_EC1·(L-L_i)，Δy＝R_p·(B_EC-B_EC1)

航段距离表达式：

$D_{\mathrm{sw}}=\sqrt{D_{\mathrm{togo}}^2-D_z^2}$

步骤三：计算无人机当前点至到点的向量与从点至到点的向量的相对位置向量点积。

当前点P至到点P_i的向量表述形式为：

$\stackrel{\→}{{\mathrm{PP}}_i}\left(\begin{array}{cc}{L}_i-L& B_i-B\end{array}\right)$

从点邗1至到点坪的向量表述形式为：

$\stackrel{\→}{P_{i-1}{P}_i}=\left(\begin{array}{cc}{L}_i-L_{i-1}& B_i-B_{i-1}\end{array}\right)$

向量和向量作点积

$\stackrel{\→}{{\mathrm{PP}}_i}\•\stackrel{\→}{P_{i-1}{P}_i}=(L_i-L)\·(L_i-L_{i-1})+(B_i-B)\·(B_i-B_{i-1})$

步骤四：设置航段切换判断条件如下：

D_sw≤DswG或

其中，DswG为航段切换门限值，取值范围为300m～700m，当上述条件成立时，无人机进行切换航段；否则不进行切换航段；

步骤五：无人机进入固定滚转角盘旋等待飞行状态，不进行航段切换。

本发明基于航段距离和相对位置向量点积的航段切换方法的优点在于：

避免了基于待飞距离航段切换法中第二和第三种情况下航段不切换的问题，实现了无人机在GPS信号全程正常或由故障恢复后都能正确地实现航段切换。

附图说明

图1：航线航段定义/航段切换过程示意图；

图2：待飞距离/侧偏距/航段距离的定义示意图；

图3：GPS信号无效后无人机固定滚转角盘旋示意图；

图4：基于待飞距离的航段切换示意图；

图5：本发明提供的航段切换流程图；

图6：基于航段距离的航段切换示意图；

图7：GPS信号恢复时基于航段距离的航段切换示意图；

图8：向量夹角示意图；

图9：基于相对位置向量点积的航段切换示意图；

图10：GPS信号恢复时基于航段距离和相对位置向量点积的航段切换示意图。

具体实施方式

下面将结合附图对本发明的具体实施步骤作进一步说明。

本发明提出一种无人机的飞行航段切换方法，该方法基于航段距离和相对位置向量点积，使无人机在GPS信号全程正常或由故障恢复后都能正确地实现航段切换，其航段切换流程图如图5所示，具体实施步骤如下：

步骤一：判断GPS信号是否正常，当GPS信号正常时，进入步骤二；当GPS信号无效时，进入步骤五；

步骤二：根据无人机当前点的经纬度计算出待飞距离D_togo和侧偏距D_z，由直角三角形勾股定理计算出航段距离D_sw。待飞距离、侧偏距和航段距离的几何意义如图3所示（其中，飞机位于在飞航段右侧时侧偏距为正，待飞距离和航段距离始终为正）。

当前点P的经纬度分别为L和B；从点P_i-1的经纬度分别为L_i-1和B_i-1；到点P_i的经纬度分别为L_i和B_i。

侧偏距表达式：

式中地球长半轴R_长＝6378137.0m，地球椭偏度E_D＝0.0033523，

$\mathrm{\α}=\arccos [\left(\frac{k_1\×k_0}{|k_1\×k_0|}\right)\•k]$

其中，k₁为折算为地心纬度的到点P_i在地球坐标系中的天向分量：

$k_1=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}1}\right)\·\cos \left(L_i\right)\\ \cos \left(B_{\mathrm{EC}1}\right)\·\sin \left(L_i\right)\\ \sin \left(B_{\mathrm{EC}1}\right)\end{array}\right]$

k₀为折算为地心纬度的从点P_i-1在地球坐标系中的天向分量：

$k_0=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}0}\right)\·\cos \left(L_{i-1}\right)\\ \cos \left(B_{\mathrm{EC}0}\right)\·\sin \left(L_{i-1}\right)\\ \sin \left(B_{\mathrm{EC}0}\right)\end{array}\right]$

k为折算为地心纬度的当前点P在地球坐标系中的天向分量：

$k=\left[\begin{array}{c}\cos \left(B_{\mathrm{EC}}\right)\·\cos \left(L\right)\\ \cos \left(B_{\mathrm{EC}}\right)\·\sin \left(L\right)\\ \sin \left(B_{\mathrm{EC}}\right)\end{array}\right]$

当前点P地心纬度单位（rad）

从点P_i-1地心纬度单位（rad）

到点P_i地心纬度单位（rad）

h：无人机当前高度；R_短：地球短半轴；R_长：地球长半轴；

待飞距离表达式：

$D_{\mathrm{togo}}=\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2}$

其中，Δx＝R_p·cosB_EC1·(L-L_i)，Δy＝R_p·(B_EC-B_EC1)，

航段距离表达式：

$D_{\mathrm{sw}}=\sqrt{D_{\mathrm{togo}}^2-D_z^2}$

步骤三：计算当前点至到点的向量与从点至到点的向量的相对位置向量点积。当前点P至到点P_i的向量表述形式为：

$\stackrel{\→}{{\mathrm{PP}}_i}\left(\begin{array}{cc}{L}_i-L& B_i-B\end{array}\right)$

从点P_i-1至到点P_i的向量表述形式为：

$\stackrel{\→}{P_{i-1}{P}_i}=\left(\begin{array}{cc}{L}_i-L_{i-1}& B_i-B_{i-1}\end{array}\right)$

向量和向量作点积

$\stackrel{\→}{{\mathrm{PP}}_i}\•\stackrel{\→}{P_{i-1}{P}_i}=(L_i-L)\·(L_i-L_{i-1})+(B_i-B)\·(B_i-B_{i-1})$

步骤四：设置航段切换判断条件。

条件一：设置基于航段距离的航段切换条件：

D_sw≤DswG，其中，DswG为航段切换门限值，

当上述条件成立时，无人机进行切换航段；否则不进行切换航段。

用条件一作为航段切换条件。当GPS信号全程正常时，飞行轨迹如图6所示，理想情况下，无人机在图中黑色圆处穿过航线墙时满足航段切换条件，转入航段P_iP_i+1飞行。但是，当GPS信号由无效恢复正常时，如果无人机在如图7所示的三角形位置，将不满足航段切换条件，无人机将继续沿航段P_i-1P_i的延长线飞行。

条件二：设置基于相对位置向量点积的航段切换条件：

向量点积为：

$\stackrel{\→}{{\mathrm{PP}}_i}\•\stackrel{\→}{P_{i-1}{P}_i}$

$=\left|\stackrel{\→}{{\mathrm{PP}}_i}\right|\·\left|\stackrel{\→}{P_{i-1}{P}_i}\right|\·(\stackrel{\→}{{\mathrm{PP}}_i}\·\stackrel{\→}{P_{i-1}{P}_i})$

$=\left|\stackrel{\→}{{\mathrm{PP}}_i}\right|\·\left|\stackrel{\→}{P_{i-1}{P}_i}\right|\·\cos \left(\mathrm{\α}\right)$

式中，α为向量和向量的夹角，

当α＜90°时， $\stackrel{\→}{{\mathrm{PP}}_i}\•\stackrel{\→}{P_{i-1}{P}_i}>0;$

当α≥90°时， $\stackrel{\→}{{\mathrm{PP}}_i}\•\stackrel{\→}{P_{i-1}{P}_i}\≤0;$

规定基于相对位置向量点积的航段切换条件为：

当时，进行航段切换；

当时，不进行航段切换。

在图8中，向量和向量的夹角为锐角，不满足航段切换条件，无人机继续沿航段P_i-1P_i飞行；向量和向量的夹角为钝角，满足航段切换条件，无人机转入航段P_iP_i+1飞行。

用作为无人机的航段切换条件。在GPS信号正常时，当向量夹角α≥90°时，认为完成航段P_i-1P_i飞行，更新从点和到点，转入航段P_iP_i+1飞行，飞行轨迹如图9所示。当GPS信号全程正常时，相同环境下，对比图6与图9，基于相对位置向量点积的航段切换时刻晚于基于航段距离的航段切换时刻，但是，其优点在于当GPS信号由无效恢复正常时无人机在任何位置都能实现正确地航段切换。

综合条件一和条件二各自的优点，设置复合式航段切换判断条件如下：

D_sw≤DswG或

当上述条件成立时，无人机进行切换航段；否则不进行切换航段。

复合式航段切换判断条件，既满足了GPS信号全程正常时的航段切换，也满足了GPS信号由无效恢复正常时的航段切换。图10给出了GPS信号恢复正常时，如果无人机在三角形位置，经判断满足航段切换条件，更新从点和到点，转入了航段邗邗小飞行。

步骤五：无人机进入固定滚转角盘旋等待飞行状态，不进行航段切换。

Claims (1)

1.基于航段距离和相对位置向量点积的无人机飞行航段切换方法，其特征在于：

步骤一：判断GPS信号是否正常，当GPS信号正常时，进入步骤二；当GPS信号无效时，进入步骤五；

步骤二：根据当前位置的经纬度计算出待飞距离D_togo和侧偏距D_z，由直角三角形勾股定理计算出航段距离D_sw；

步骤三：计算当前点至到点的向量与从点至到点的向量的相对位置向量点积；当前点P至到点P_i的向量表述形式为：

$\stackrel{\→}{{\mathrm{PP}}_i}=\left(\begin{array}{cc}{L}_i-L& B_i-B\end{array}\right)$

从点P_i-1至到点P_i的向量表述形式为：

$\stackrel{\→}{P_{i-1}{P}_i=}\left(\begin{array}{cc}{L}_i-L_{i-1}& B_i-B_{i-1}\end{array}\right)$

向量和向量作点积

步骤四：设置航段切换判断条件如下：

D_sw≤DswG或

其中，DswG为航段切换门限值；为向量和向量的点积，P代表当前点，P_i代表到点，P_i-1代表从点；

当上述条件成立时，无人机进行切换航段；否则不进行切换航段；所述的航段切换门限值的取值范围为300m～700m；

步骤五：无人机进入固定滚转角盘旋等待飞行状态，不进行航段切换；

所述的侧偏距表达式：

当前点P的经纬度分别为L和B；式中地球长半轴R_长＝6378137.0m，地球椭偏度E_D＝0.0033523，其中，k₁为折算为地心纬度的到点P_i在地球坐标系中的天向分量：

k₁＝(cos(B_EC1)·cos(L_i),cos(B_EC1)·sin(L_i),sin(B_EC1))

k₀为折算为地心纬度的从点P_i-1在地球坐标系中的天向分量：

k₀＝(cos(B_EC0)·cos(L_i-1),cos(B_EC0)·sin(L_i-1),sin(B_EC0))

k为折算为地心纬度的当前点P在地球坐标系中的天向分量：

k＝(cos(B_EC)·cos(L),cos(B_EC)·sin(L),sin(B_EC))

当前点P地心纬度单位(rad)；

从点P_i-1地心纬度单位(rad)；

到点P_i地心纬度单位(rad)；

h为无人机当前高度；R_短为地球短半轴；R_长为地球长半轴；从点P_i-1的经纬度分别为L_i-1和B_i-1；到点P_i的经纬度分别为L_i和B_i；

待飞距离表达式：

$D_{\mathrm{togo}}=\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2}$

其中，Δx＝R_p·cosB_EC1·(L-L_i)，Δy＝R_p·(B_EC-B_EC1)，

航段距离表达式：

$D_{\mathrm{sw}}=\sqrt{D_{\mathrm{togo}}^2-D_z^2}.$