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

说明书

技术领域

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

背景技术

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

航线信息存储在飞控计算机中，无人机沿航线飞行。一条航线中有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，