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

权利要求书

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

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

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

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

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

D_sw≤DswG或

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

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

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

2.根据权利要求1所述的基于航段距离和相对位置向量点积的无人机飞行航段切换方法，其特征在于：所述的侧偏距表达式：

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

k1=cos(BEC1)·cos(Li)cos(BEC1)·sin(Li)sin(BEC1)]]>

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

k0=cos(BEC0)·cos(Li-1)cos(BEC0)·sin(Li-1)sin(BEC0)]]>

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

k=cos(BEC)·cos(L)cos(BEC)·sin(L)sin(BEC)]]>

当前点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；

待飞距离表达式：

Dtogo=Δx2+Δy2]]>

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

航段距离表达式：

Dsw=Dtogo2-Dz2.]]>