惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法与流程

本发明属于惯性卫导组合导航技术领域，尤其是一种惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法。

背景技术：

长航时无人机的长时间滞空特性要求其导航系统的精度和稳定性很高，针对这一需求普遍采用惯性卫导组合导航系统作为长航时无人机的导航系统。惯性导航系统具有高度自主性、抗干扰能力强、输出实时性高等优势，但存在误差随时间累积、价格昂贵等缺点。卫星导航系统具有定位精度高、误差不随时间发散、价格低廉等优势，但也存在输出实时性差、易受外界干扰等缺点。由二者构成的组合导航系统可以综合它们各自的优势，以较低的成本实现长时间、高精度的导航信息实时输出。

在复杂的工作环境中，卫星导航系统易受到各种环境干扰，使得卫导系统的导航信息精度下降或产生突变等故障现象，此时如不加鉴别的直接利用故障信息进行组合滤波，将极大降低组合导航系统的导航精度。

技术实现要素：

本发明的目地在于克服现有技术的不足，提出一种设计合理、导航精度高且性能稳定的惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法。

本发明解决其技术问题是采取以下技术方案实现的：

一种惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法，包括以下步骤：

步骤1、在惯导系统中，进行惯导解算输出姿态航向、速度和位置信息并更新；

步骤2、采用卫星定位导航系统的位置信息作为参考信息，与惯导系统的位置信息进行数据融合，融合策略是依据惯导系统的误差传播规律确定数学模型，并利用卡尔曼滤波器对惯导系统导航信息的误差状态进行在线估计和校准。

在步骤2后包括如下步骤：在性卫导组合导航系统抗中进行卫导系统故障诊断与隔离。

所述步骤1采用三只陀螺仪和三只加速度构成惯性导航系统进行惯导解算，具体方法为：通过采集陀螺仪的输出角速度数据并扣除地球自转角速度以及牵连角速度，计算得到姿态角；加速度计测量的加速度通过方向余弦阵进行坐标变换到地理坐标系的北向、东向、天向轴上，积分地理坐标系上的加速度并扣除重力和有害加速度的影响得到地理坐标系的北向、东向和天向速度；利用地理坐标系的北向、东向速度，并考虑地球的曲率半径计算得到经度和纬度。

所述步骤2的具体实现方法包括：

(1)卡尔曼滤波一步预测步骤：

(2)当滤波时间到时进行滤波更新步骤；

(3)当系统计时器到达修正时刻，以修正后的姿态四元数、速度、位置为初始值进入自主导航步骤。

所述步骤(1)包括状态转移阵的计算、输入噪声方差阵的计算和状态预测与误差方差预测的计算。

所述步骤(2)包括量测计算、滤波增益计算、状态估计更新和误差方差更新。

所述步骤(3)包括姿态四元数修正、速度修正和位置修正过程。

所述卫导系统故障诊断与隔离的方法为：以组合惯性卫导组合导航系统的卡尔曼滤波器作为诊断工具，采用一致性检验检验法，通过隔离源自卫导系统故障的测量值，实现抗干扰的惯导系统姿态航向校准；卡尔曼滤波器的测量新息指示测量与状态估计是否一致；采用新息监测即基于残差的检验法检测大的突变的卫导系统故障，采用新息序列监测检测小的慢变的卫导系统故障。

本发明的优点和积极效果是：

本发明针对惯导系统误差随时间漂移和卫导系统易受外界环境干扰的缺点，利用卡尔曼滤波器对惯导系统的导航信息与卫导系统的导航信息进行组合滤波，得到最优的载机导航信息，并在恶劣环境中对卫导系统的故障进行诊断隔离，即使在卫导系统受干扰条件下也能完成惯导系统的长航时姿态航向误差校准，解决惯导系统在长时间工作情况下姿态角误差校准问题，保证惯导系统姿态角误差估计准确可靠。

附图说明

图1是本发明的导航解算流程图；

图2是本发明的惯性卫导组合导航流程图。

具体实施方式

以下结合附图对本发明实施例做进一步详述。

一种惯性卫导组合导航系统的长航时抗干扰姿态航向校准方法，是以三只陀螺仪和三只加速度计组成捷联惯性导航系统，进行惯导解算输出姿态航向、速度和位置信息。采用卫星定位导航系统的位置信息作为参考信息，与惯导系统的位置信息进行数据融合，融合策略是依据惯导系统的误差传播规律确定数学模型，利用卡尔曼滤波器对惯导系统导航信息的误差状态进行在线估计和校准。具体包括如下步骤：

步骤1、在捷联惯性导航系统中，进行惯导解算输出姿态航向、速度和位置信息并更新。

在本步骤中，在三只陀螺仪和三只加速度构成惯性导航系统中，进行捷联惯导解算。通过采集陀螺的输出角速度数据并扣除地球自转角速度以及牵连角速度，计算得到姿态角。加速度计测量的加速度通过方向余弦阵进行坐标变换到地理坐标系的北向、东向、天向轴上，积分地理坐标系上的加速度并扣除重力和有害加速度的影响得到地理坐标系的北向、东向和天向速度。利用地理坐标系的北向、东向速度，并考虑地球的曲率半径计算得到经度和纬度。导航解算流程如图1所示，具体方法为：

记初始姿态角对应的方向余弦矩阵对应的姿态四元数记为将转成姿态四元数形式：

k＝0，1，2，…时，利用四元数更新算法计算t_k+1时刻的姿态四元数

其中

Δθ＝[Δθ_x Δθ_y Δθ_z]^T为(t_k,t_k+1]采样周期内的陀螺输出角增量，单位：rad；

为(t_k,t_k+1]更新周期内n系相对于i系转过的角度。

为e系相对于i系的自转角速率在n系的投影，为n系相对于e系的角速率在n系上的投影，由以下两式计算：

式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果。最后进行四元数归一化处理

得到更新后的姿态四元数

加速度计输出的速度增量Δv经由坐标变换得地理坐标系上的速度分量

其中，Δv＝[Δv_x Δv_y Δv_z]^T为加速度计输出的速度增量。

地理坐标系上的加速度分量为

fⁿ＝Δvⁿ/h

其中，记fⁿ＝[f_E f_N f_U]^T。

速度更新初始时刻，vⁿ(0)＝[0 0 0]^T；

k＝0，1，2，…时，在一个速度更新周期(t_k,t_k+1]内，速度更新为

其中，

δA为有害加速度，计算如下：

初始时刻，L(0)、λ(0)和S(0)为装订的纬度、经度和高程；

k＝0，1，2，…时，在一个速度更新周期(tk,t_k+1]内，位置更新为

步骤2、采用卫星定位导航系统的位置信息作为参考信息，与惯导系统的位置信息进行数据融合，融合策略是依据惯导系统的误差传播规律确定数学模型，利用卡尔曼滤波器对惯导系统导航信息的误差状态进行在线估计和校准

惯性卫导组合导航系统是在纯惯性导航解算的基础上利用外部信息(卫导)对姿态失准角进行估计与修正，并重调位置和速度的过程。在组合导航状态下，设备可保持最优精度。组合导航采用卫导系统的位置作为卡尔曼滤波器的量测信息，流程如图2所示。

卡尔曼滤波一步预测分为状态转移阵Φ_k,k-1的计算，输入噪声方差阵的计算，状态预测与误差方差预测P_k,k-1的计算三个阶段：

(1)状态转移阵Φ_k,k-1的计算

记(t_k-1,t_k]为一个预测周期，h＝t_k-t_k-1。预测周期h一般较短，状态转移阵计算如下

其中，

矩阵中各项元素的变量均可由比力坐标变换、速度更新、位置更新计算而得，其中，

L为纬度，单位：rad；；

v_E为东向速度，单位：m/s；

v_N为北向速度，单位：m/s；

v_U为垂向速度，单位：m/s；

R_E为子午面垂直的法线平面的曲率半径，单位：m；

R_N为子午面上的曲率半径，单位：m；

f_E为地理坐标系东向加速度，单位：m/s2；

f_N为地理坐标系北向加速度，单位：m/s2；

f_U为地理坐标系垂向加速度，单位：m/s2；

ω_ie为地球自转角速度，ω_ie＝7.292115×10-5rad/s。

(2)输入噪声方差阵的计算

连续系统的系统噪声即三个陀螺和三个加速度计向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为

Q_q＝G(t)Q(t)G^T(t)

其中，Q(t)为常量，G(t)为噪声输入矩阵，重写如下：

得到连续系统的输入噪声方差Q_q后计算Kalman滤波的输入噪声方差如下：

(3)状态预测与误差方差预测P_k,k-1的计算

初始时刻当k＝0时，对和P₀进行初始化；

k＝0，1，2，…时，递推计算

当滤波更新周期未到时，不进行滤波更新，继续进行预测，令

P_k＝P_k,k-1

当滤波更新周期到时，P_k按下一节的滤波更新计算。

当滤波时间到时进行滤波更新，滤波更新周期一般为外参考信息更新周期，滤波更新周期设为1s，分为四步计算：

(1)量测计算：

按下式计算量测值：

下标s表示捷联惯导系统解算输出，下标r表示参考基准输出。

v_sE、v_sN、v_sU为解算东向速度、北向速度、垂向速度，单位：m/s；

L_s、λ_s为解算纬度、经度，单位：rad；

S_s为解算高度，单位：m；

v_rE、v_rN、v_rU为参考速度，单位：m/s；

L_r、λ_r为装订纬度、经度，单位rad；

S_r为参考高度，单位：m。

(2)滤波增益计算

按下式计算滤波增益K_k：

其中，

P_k,k-1为误差方差预测计算得；

H_k＝[0_6×3 I_6×6 0_6×6]；

R_k＝diag[(R_ve)² (R_vn)² (R_vu)²

(R_lat)² (R_lon)² (R_h)²]。

(3)状态估计更新

按下式计算状态估计

其中，为状态预测计算得。

(4)误差方差更新

按下式计算误差方差P_k：

当系统计时器到达修正时刻，以修正后的姿态四元数、速度、位置为初始值进入自主导航，假设t_k时刻为修正时间点：

(1)姿态四元数修正

t_k时刻，利用Kalman滤波估计得到失准角φ_E、φ_N、φ_U，对应的第1、2、3个元素，修正角记为φ_c＝[φ_E φ_N φ_U]^T，采用四元数法修正t_k时刻的姿态四元数

其中，

(2)速度修正

t_k时刻，利用Kalman滤波估计得到速度误差量δv_E、δv_N、δv_U，对应的第4、5、6个元素，修正t_k时刻的解算速度：

(3)位置修正

t_k时刻，利用Kalman滤波估计得到位置误差量δL、δλ、δS，对应的第7、8、9个元素，修正t_k时刻的解算位置：

步骤3、抗干扰组合导航系统中的卫导系统故障诊断与隔离

组合导航系统的故障诊断与隔离主要针对卫导系统在受到外界干扰时产生的故障进行在线诊断并将其隔离。采用组合导航系统的卡尔曼滤波器作为诊断工具。采用一致性检验χ²检验法，通过隔离源自卫导系统故障的测量值，实现抗干扰的惯导系统姿态航向校准。卡尔曼滤波器的测量新息也即残差可以指示测量与状态估计是否一致。采用新息监测即基于残差的χ²检验法检测大的突变的卫导系统故障，采用新息序列监测检测小的慢变的卫导系统故障。具体方法如下：

(1)新息监测

新息监测算法流程为：若k-1步以前(包括k-1步)系统无故障，则经过卡尔曼滤波得到的第k-1步的估计值应是正确的，由系统的状态方程可得到第k时刻系统状态的一步预测值即

由此可得k时刻系统量测的预测值

若k时刻外部实际量测信息z_k是正确的，则它与量测的预测值之间的残差δz_k^-应服从零均值高斯白噪声分布

其方差为

当卫导系统发生故障时，残差δz_k^-就不再是零均值的高斯白噪声序列。通过检测δz_k^-的统计特性可以判别卫导系统是否发生故障。

构造统计量当量测信息正常时，λ_z(k)服从自由度为m的χ²分布，而当量测信息异常时，λ_z(k)不再服从自由度为m的χ²分布。采用如下准则：

a)当λ_z(k)≤χ_α²时，认为外部量测信息正常；

b)当λ_z(k)＞χ_α²时，认为外部量测信息故障。

其中χ_α²为检测阈值，α为误警率。

(2)新息序列监测

测量与状态估计之间更小的和缓慢建立的偏差，可以通过最新的N个测量构成统计检验来辨识，新息检验统计量为：

其中

只要协方差C_μ^-正确，该新息检验统计量就是满足具有m个自由度的χ²分布，这里m是量测向量的维数，将s_δz,k与阈值的比较就是信息序列的χ²检验。

上述两种检验结果作为图2中量测是否有效的判据，当χ²检验中的统计量超出阈值时，则可判定卫导系统出现故障，此时量测信息不可用，卡尔曼滤波器不进行量测更新，将故障信息进行隔离，避免惯导系统的姿态航向被故障信息污染。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。