CN103994766A - 一种抗gps失效固定翼无人机定向方法 - Google Patents

Abstract

一种抗GPS失效固定翼无人机定向方法，本发明涉及一种适用于固定翼无人机低成本组合导航***的GPS失效后定向定姿方法。在每个计算周期，惯性测量数据经过捷联导航算法，更新当前固定翼无人机航向姿态；再根据固定翼无人机当前运动特性判断采取不同的信息融合滤波器：静止或者低动态机动情况下以三轴磁传感器和三轴加速度计确定航向姿态最优四元数为观测值进行Kalman滤波；大机动情况下采用三轴磁传感器测量数据为观测值实施Kalman滤波。依据Kalman滤波结果校正惯性测量单元误差，更新导航数据和滤波器参数；进入下一计算周期。本发明可用于任何包含GPS、惯性测量单元和三轴磁传感器的固定翼无人机导航***中。

Description

一种抗GPS失效固定翼无人机定向方法

技术领域

本发明涉及一种抗GPS失效固定翼无人机定向方法，该方法以低成本惯性测量单元和三轴正交磁传感器为定向定姿传感器，采用双滤波器信息融合的思路，为固定翼无人机在GPS失效情况提供可信的定向定姿信息。本发明可用于任何包含GPS、惯性测量单元和三轴磁传感器的固定翼无人机导航***中。

背景技术

GPS是固定翼无人机的重要导航设备，不仅提供固定翼无人机位置和速度信息，而且还与惯性测量单元信息融合提供固定翼无人机航向和姿态信息。但以无线电测距测速为主要技术特征的GPS难以避免受到大气不稳定、信号传输遮挡、多径干扰等诸多随机因素的影响，可靠性和精度常常降低甚至于失效。定位测速信息不准确必然影响GPS+惯性测量单元融合定向定姿的精度，而定向定姿精度和可靠性直接关系无人机飞行控制安全。因此开发抗GPS失效的定向定姿方法成为提高固定翼无人机可靠性的必要途径。

固定翼无人机除了GPS一般还安装惯性测量单元和三轴磁传感器两类导航设备。惯性测量单元中的陀螺获取三维角速度信息，惯性测量单元中的加速度计获取三维加速度信息；三轴磁传感器感测地磁场在三维方向的投影分量。但目前两类导航设备分立工作，在GPS失效情况下惯性测量单元误差因缺乏校正信息而随时间快速积累，无法提供可信的航向姿态；三轴磁传感器也因为缺乏可信的姿态信息而无法给出有效的磁航向。本发明针对惯性测量单元和三轴磁传感器分立工作的问题，提出采用基于固定翼无人机运动特性的双Kalman滤波器信息融合方法，在GPS失效情况下依然能够校正惯性测量单元误差，使其提供可信的姿态，同时支持三轴磁传感器给出有效的磁航向。满足固定翼无人机安全飞行控制的需求。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种抗GPS失效固定翼无人机定向方法。

本发明的技术解决方案为：一种抗GPS失效固定翼无人机定向方法，其特征在于包括以下步骤：

(1)基于GPS收星数量、速度精度和位置精度，固定翼无人机飞行期间不断检测GPS是否失效，若GPS未失效，继续正常的导航程序；若GPS失效，进入GPS失效定向定姿工作模式。

(2)GPS失效后立即以GPS有效期间导航数据确定GPS失效定向定姿工作模式初始状态，保证固定翼无人机在GPS失效前后导航数据连续性。

(3)在每个计算周期，首先利用惯性测量单元提供的三维正交角速度和加速度测量数据，经过捷联导航算法，实时更新当前固定翼无人机航向姿态、速度和位置。

(4)根据固定翼无人机当前运动特性判断采取不同的信息融合方法，主要考虑固定翼无人机三轴运动加速度的大小：运动加速度超过设定门限值则认为固定翼无人机运动属于大机动情况，反之则认为固定翼无人机处于静止或者低动态情况。

(5)如果固定翼无人机静止或者低动态机动情况，以三轴磁传感器和三轴加速度计数据为基础，采用优化搜索的方式确定航向姿态最优四元数，以该最优四元数为观测量，构建四维观测量Kalman滤波器模型。

(6)如果固定翼无人机做大机动，以三轴磁传感器为观测量，构建三维观测量Kalman滤波器模型。

(7)用两种Kalman滤波器状态变量估计值校正惯性测量单元的器件误差和导航误差。

(8)上述步骤迭代重复计算。

本发明的原理是：GPS在固定翼无人机中发挥的作用包括直接提供位置、速度信息，间接与惯性测量单元信息融合提供航向姿态信息。当GPS失效后惯性测量单元独立工作，误差随时间快速积累，无法提供可信的航向姿态信息。本发明利用了GPS和惯性测量单元之外的运动传感器——三轴磁传感器，感测地球磁场作为观测量，通过信息融合滤波器为惯性测量单元提供了误差抑制的途径。信息融合滤波器的具体形式随着固定翼无人机的运动特性而变化，固定翼无人机静止或者低动态机动情况下以三轴磁传感器和三轴加速度计数据为基础确定航向姿态最优四元数，采用最优四元数为观测值进行Kalman滤波；固定翼无人机大机动情况下采用三轴磁传感器测量数据为观测值进行Kalman滤波；依据Kalman滤波结果校正惯性测量单元误差，更新导航数据和滤波器参数。实现在GPS失效情况下定向和定姿功能。

本发明与现有技术相比的优点在于：以低成本、MEMS工艺惯性测量单元和低成本、MEMS工艺三轴正交磁传感器为定向定姿传感器，满足无人机机载电子设备低功耗、小体积、轻质量的限制。GPS失效后立即以GPS有效期间导航数据确定GPS失效定向定姿工作模式的初始状态，保证固定翼无人机在GPS失效前后的数据连续性。根据固定翼无人机当前运动特性判断采取不同的信息融合方法，主要考虑固定翼无人机三轴运动加速度的大小：运动加速度超过设定门限值则认为固定翼无人机运动属于大机动情况，反之则认为固定翼无人机处于静止或者低动态情况。在同一套固定翼无人机导航***中，同时运行两个滤波器：每个迭代计算步骤中，针对不同运动特性选择采用适当的滤波器形式，提高了惯性测量单元和三轴磁传感器信息融合的精度，为固定翼无人机提供了可信的定向定姿信息。

附图说明

图1为本发明的抗GPS失效固定翼无人机双滤波器定向定姿方法流程图。

具体实施方式

如图1所示，本发明的具体方法如下：

1、固定翼无人机飞行期间不断检测GPS是否失效：

设μ为GPS失效门限，K,δl,δv分别为收星颗数、定位误差、测速误差，若K≥4且则认为GPS正常工作，反之则认为GPS失效。

2、GPS失效后判断无人机机动特性：a_x,a_y,a_z表示无人机三轴加速度计测量值，则无人机处于大机动情况；否则，无人机处于静止或者低动态情况，1.125为归一化的机动判断门限。

3、GPS失效后立即以GPS有效期间导航数据确定GPS失效定向定姿工作模式的初始状态，保证固定翼无人机在GPS失效前后的数据连续性：

L_noGPS、λ_noGPS、H_noGPS、VE_noGPS、VN_noGPS、VU_noGPS、θ_noGPS、γ_noGPS表示GPS失效后的纬度、经度、高度、东向速度、北向速度、天向速度、航向角、俯仰角、横滚角等导航参数，

L_GPS、λ_GPS、H_GPS、VE_GPS、VN_GPS、VU_GPS、θ_GPS、γ_GPS表示GPS正常情况的对应导航参数。失效后导航参数初始化为

L_noGPS＝L_GPS,λ_noGPS＝λ_GPS,λ_noGPS＝λ_GPS

VE_noGPS＝VE_GPS,VN_noGPS＝VN_GPS,VU_noGPS＝VU_GPS

θ_noGPS＝θ_GPS,γ_noGPS＝γ_GPS

4、利用惯性测量单元提供的三维正交角速度和加速度测量数据，经过标准捷联惯性导航算法，实时更新当前固定翼无人机航向姿态、速度和位置，同时更新姿态对应的方向余弦矩阵

5、设定Kalman滤波器状态变量为

其中ε_x,ε_y,ε_z表示机体坐标系三个方向的陀螺零偏。两个滤波器的状态方程形式一致：X＝FX+GW，W＝W[3×1]为***噪声，状态转移矩阵F＝F_6×6和***噪声系数阵G＝G[6×3]中除了以下元素外均为零元素：

F₁₂＝ω_iesin(L_noGPS)+VE_noGPStan(L_noGPS)/(R_n+h)

F₁₃＝-ω_iecos(L_noGPS)-VE_noGPS/(R_n+h)

F₂₁＝-F₁₂

F₂₃＝-VN_noGPS/(R_m+h)

F₃₁＝-F₁₃

F₃₂＝-F₂₃

$F_{14}=C_b^n\left[\mathrm{1,1}\right]$

$F_{15}=C_b^n[1,2]$

$F_{16}=C_b^n\left[\mathrm{1,3}\right]$

$F_{24}=C_n^n\left[\mathrm{2,1}\right]$

$F_{25}=C_b^n\left[\mathrm{2,2}\right]$

$F_{26}=C_b^n\left[\mathrm{2,3}\right]$

$F_{34}=C_b^n\left[\mathrm{3,1}\right]$

$F_{35}=C_b^n\left[\mathrm{3,2}\right]$

$F_{36}=C_b^n\left[\mathrm{3,3}\right]$

$G\left[\mathrm{1,1}\right]=C_b^n\left[\mathrm{1,1}\right]$

$G[1,2]=C_b^n\left[\mathrm{1,2}\right]$

$G\left[\mathrm{1,3}\right]=C_b^n\left[\mathrm{1,3}\right]$

$G\left[\mathrm{2,1}\right]=C_b^n\left[\mathrm{2,1}\right]$

$G\left[\mathrm{2,2}\right]=C_b^n\left[\mathrm{2,2}\right]$

$G\left[\mathrm{2,3}\right]=C_b^n\left[\mathrm{2,3}\right]$

$G\left[\mathrm{3,1}\right]=C_b^n\left[\mathrm{3,1}\right]$

$G\left[\mathrm{3,2}\right]=C_b^n\left[\mathrm{3,2}\right]$

$G\left[\mathrm{3,3}\right]=C_b^n\left[\mathrm{3,3}\right]$

6、两个滤波器观测方程采用不同形式：Z_A＝H_AX+V_A和Z_B＝H_BX+V_B。

7、针对无人机静止或者低动态运动情况，基于三轴磁罗盘和三轴加速度计数据，采用标准牛顿梯度寻优算法获得A滤波器的观测量Z_A＝Z_A[4×1]＝[q₀,q₁,q₂,q₃]^T，q_i,i＝0,...,3为最优姿态四元数。

8、针对无人机大机动情况，h_bx,h_by,h_bz为机体坐标系三轴磁罗盘的测量值，B滤波器的观测量为Z_B＝Z_B[3×1]＝[h_bx,h_by,h_bz]^T。

9、A、B滤波器采用标准卡尔曼滤波器估计状态变量X，获得固定翼无人机航向、姿态和陀螺零偏。

10、在GPS失效期间迭代重复上述过程。

Claims (10)

1.一种抗GPS失效固定翼无人机定向方法，其特征在于：以低成本惯性测量单元和三轴正交磁传感器为定向定姿传感器；在每个计算周期，惯性测量单元数据经过捷联导航算法，更新当前固定翼无人机航向姿态和其他导航数据；根据固定翼无人机当前运动特性判断采取不同的信息融合方法：固定翼无人机静止或者低动态机动情况下以三轴磁传感器和三轴加速度计数据为基础确定航向姿态最优四元数，采用最优四元数为观测值进行Kalman滤波；固定翼无人机大机动情况下采用三轴磁传感器测量数据为观测值进行Kalman滤波；依据Kalman滤波结果校正惯性测量单元误差，更新导航数据和滤波器参数；进入下一个重复计算周期。 

2.根据权利要求1所述一种抗GPS失效固定翼无人机定向方法，其特征在于：固定翼无人机飞行期间不断检测GPS是否失效，GPS失效与否的检测标准不仅仅考虑收星数量，还包括速度精度和位置精度。若GPS未失效，继续进入正常的导航程序；若GPS失效，进入本发明方法所述步骤。 

3.根据权利要求1所述一种抗GPS失效固定翼无人机定向方法，其特征在于：GPS失效后立即以GPS有效期间导航数据确定GPS失效定向定姿工作模式的初始状态，保证固定翼无人机在GPS失效前后的数据连续性。 

4.根据权利要求1所述一种抗GPS失效固定翼无人机定向 方法，其特征在于：以低成本、MEMS工艺惯性测量单元和低成本、MEMS工艺三轴正交磁传感器为定向定姿传感器，满足无人机机载电子设备低功耗、小体积、轻质量的限制。 

5.根据权利要求1所述一种抗GPS失效固定翼无人机定向方法，其特征在于：在每个计算周期，首先利用惯性测量单元提供的三维正交角速度和加速度测量数据，经过捷联导航算法，实时更新当前固定翼无人机航向姿态、速度和位置。 

6.根据权利要求1所述一种抗GPS失效固定翼无人机定向方法，其特征在于：根据固定翼无人机当前运动特性判断采取不同的信息融合方法，主要考虑固定翼无人机三轴运动加速度的大小：运动加速度超过设定门限值则认为固定翼无人机运动属于大机动情况，反之则认为固定翼无人机处于静止或者低动态情况。 

7.根据权利要求1所述一种抗GPS失效固定翼无人机定向方法，其特征在于：如果固定翼无人机静止或者低动态机动情况，以三轴磁传感器和三轴加速度计数据为基础，采用优化搜索的方式确定航向姿态最优四元数，以该最优四元数为四维观测值，构建四维观测量Kalman滤波器模型。 

8.根据权利要求1所述一种抗GPS失效固定翼无人机定向方法，其特征在于：如果固定翼无人机做大机动，此时加速度计数据不适用于确定航向姿态，仅以三轴磁传感器为三维观测值，构建三维观测量Kalman滤波器模型。 

9.根据权利要求1所述一种抗GPS失效固定翼无人机定向 方法，其特征在于：两种Kalman滤波器状态变量相同，无论采用何种信息融合方法都能够统一估计并校正惯性测量单元的器件误差和导航误差。 

10.根据权利要求1所述一种抗GPS失效固定翼无人机定向方法，其特征在于：本发明所述方法在GPS失效期间始终迭代重复计算。