CN104075713B - 一种惯性/天文组合导航方法 - Google Patents

Abstract

一种惯性/天文组合导航方法，本发明涉及一种适用于惯性/天文组合导航系统的信息融合方法，主要以天文导航量测信息来校正惯性导航系统的姿态误差；并通过滤波器估计陀螺常值漂移。同时利用上一时刻的天文导航量测信息和当前时刻陀螺输出值对当前时刻的天文导航量测信息进行野值检测；利用已经估计出的陀螺常值漂移来校准陀螺输出，使得即使在天文导航系统失效后，惯性导航系统的姿态依旧可以保持在很高的精度。本发明大幅提升了组合导航系统滤波收敛速度，同时可以精确估计陀螺常值漂移，提高组合导航系统的精度，可用于任何包含天文和惯性的组合导航系统中。

Description

一种惯性/天文组合导航方法

技术领域

本发明涉及一种适用于惯性/天文组合导航系统的信息融合方法，主要以天文导航量测信息来校正惯性导航系统的姿态误差，并通过滤波器估计陀螺的常值漂移。同时利用上一时刻的天文导航量测信息和当前时刻陀螺输出值对当前时刻的天文导航量测信息进行野值检测。利用已经估计出的陀螺常值漂移来校准惯性传感器的陀螺输出，使得即使在天文导航系统失效后，惯性导航系统的姿态依旧可以保持在很高的精度。本发明大幅提升了组合导航系统滤波收敛速度，同时可以精确估计陀螺误差的常值漂移，提高组合导航系统的精度，可用于任何包含天文和惯性的组合导航系统中。

背景技术

天文导航是通过测量自然天体相对运动载体的矢量方向来实现定位导航。天文导航主要有以下几个优势：1)完全自主式导航；2)导航精度高；3)抗干扰能力强，可靠性高；4)导航误差不随时间积累。

惯性/天文组合导航系统主要以天文导航系统的信息来修正由陀螺漂移和初始失准角引起的误差，获得高精度的位置、速度和姿态导航信息。卡尔曼滤波是目前组合导航系统中使用最广泛的一种信息融合方法，其主要采用信号与噪声的状态空间模型，利用前一时刻的估计值和新时刻的观测值来对状态变量进行估计，求出新时刻的估计值。

标准离散卡尔曼滤波算法主要由时间更新方程和量测更新方程 组成：

1)时间更新方程

${\hat{X}}_{k/k-1}=A_k{\hat{X}}_{k-1}$

P_k/k-1＝A_k-1P_k-1A^T _k-1+Q_k-1

2)量测更新方程

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

其中代表k时刻系统的状态估计量,A_k为一步转移阵；P_k/k-1为一步预测均方误差阵；H_k为量测阵；Q_k-1为系统噪声序列方差阵；R_k为量测噪声序列方差阵；K_k为滤波增益阵；P_k为估计均方误差阵。

本发明涉及一种适用于惯性/天文组合导航系统的信息融合方法，主要以天文导航量测信息来校正惯性导航系统姿态误差，并通过滤波器估计陀螺常值漂移。当天文导航失效后，利用已经估计出的陀螺常值漂移来校准惯性传感器的输出，使得惯性导航系统的姿态依旧可以保持在很高的精度。本发明相对于其他惯性/天文组合导航方法，状态变量的选取仅为七维，大大降低了滤波器的计算量，状态变量中载体姿态误差主要以四元数乘性误差表示，更加符合实际。同时卡尔曼滤波器的一步预测值与一步预测均方误差随惯性导航系统频率更新，使得一步预测更加准确。此外组合导航系统可以精确估计陀螺误差的常值漂移，可用于任何包含天文和惯性的组合导航系统中。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种适用于惯性/天文组合导航的方法。

本发明的技术解决方案为：一种惯性/天文组合导航方法，其特征 在于包括以下步骤：

(1)在天文导航量测信息不可用时，利用惯性测量单元的量测信息进行惯性导航系统解算，获得载体的位置、速度和姿态参数。在天文导航量测信息可用时，建立惯性/天文组合导航系统的状态方程和量测方程，采用卡尔曼滤波信息融合方法，利用组合导航量测信息去修正惯性导航系统的误差，实现高精度的导航。

(2)利用上一时刻的天文导航量测信息和当前时刻陀螺输出值对当前时刻的天文导航量测信息进行估计，并将估计值与当前时刻的天文导航实际量测信息进行比较，若偏离超过一定的阈值，则判定天文导航量测信息为野值，将其剔除。

(3)利用卡尔曼滤波器的振荡程度来判断陀螺常值漂移的估计是否收敛。若卡尔曼滤波器状态量估计值振荡衰减，则表明卡尔曼滤波器的估计值逐渐趋向于收敛。若判断陀螺常值漂移估计值收敛后，则利用估计出的陀螺常值漂移校正惯性测量单元的陀螺输出。

本发明的原理是：在惯性空间里恒星的方位基本保持不变，因此天文导航系统相当于没有漂移的陀螺，可以实现高精度的定姿，所以可以用天文导航量测信息修正惯性器件误差。本发明选取载体姿态乘性四元数误差和三轴陀螺的常值漂移作为状态变量，建立系统状态方程和量测方程进行组合导航系统信息融合。在组合导航过程中，滤波器状态矩阵只与陀螺测量值相关，因而滤波器一步预测值与一步预测均方误差随惯性导航系统频率更新。以当前时刻的陀螺输出值作为天文导航周期内陀螺输出均值，以上一时刻的天文导航量测信息为起点，采用毕卡逼近法估计当前时刻的天文导航量测信息。将估计的天文导航量测信息与当前时刻实际的天文导航量测信息进行比较，若两者偏离超过一定的阙值，则判定当前时刻的天文导航量测信息为野值，将 其剔除。以最近30次的卡尔曼滤波器陀螺常值漂移的估计值作为样本进行统计分析，当标准差小于均值的一百倍时，即可认为陀螺常值漂移估计值趋于收敛，并以此均值作为陀螺的常值漂移。最后在每次系统状态变量估计值去修正惯性导航系统后，将系统状态变量中的载体姿态乘性四元数误差重置成姿态无误差状态，作为下一次滤波迭代的初值。

本发明与现有技术相比的优点在于：状态变量的选取仅为七维，大大降低了滤波器的计算量，状态变量中载体姿态误差主要以四元数乘性误差表示，更加符合实际。同时卡尔曼滤波器的一步预测值与一步预测均方误差随惯性导航系统频率更新，使得一步预测更加准确。此外组合导航系统可以精确估计陀螺误差的常值漂移。

附图说明

图1为本发明的惯性/天文组合导航方法流程图。

具体实施方式

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

(1)在地心惯性坐标系下，建立惯性/天文组合导航系统数学模型。

仅考虑陀螺的常值漂移误差和白噪声误差，所以陀螺的量测方程为：

ω_m＝ω+ε+h

其中，ω为载体真实角速度，ε为陀螺的常值漂移误差，h为陀螺的高斯白噪声，ω_m为载体测得的实际角速度。

定义乘性误差四元数dq为:

q_m＝q_tοdq

其中，q_t为载体真实的四元数姿态，q_m为载体经解算获得的含误 差的四元数姿态。

状态变量X(t)选取载体姿态误差四元数dq₀、dq₁、dq₂、dq₃以及陀螺三轴常值漂移ε_x、ε_y、ε_z，即卡尔曼滤波器的状态变量X(t)为：

X(t)＝[dq₀ dq₁ dq₂ dq₃ ε_x ε_y ε_z]^T

则卡尔曼滤波器的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中：

$F\left(t\right)=\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& {\widehat{\mathrm{\ω}}}_{\mathrm{ibz}}^b& -{\widehat{\mathrm{\ω}}}_{\mathrm{iby}}^b& 0.5& 0& 0\\ 0& -{\widehat{\mathrm{\ω}}}_{\mathrm{ibz}}^b& 0& {\widehat{\mathrm{\ω}}}_{\mathrm{ibx}}^b& 0& 0.5& 0\\ 0& {\widehat{\mathrm{\ω}}}_{\mathrm{iby}}^b& -{\widehat{\mathrm{\ω}}}_{\mathrm{ibx}}^b& 0& 0& 0& 0.5\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

$G\left(t\right)=\left[\begin{array}{c}{0}_{1\×3}\\ \frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}\end{array}\right],W\left(t\right)=\left[\begin{array}{c}{\mathrm{\η}}_x\\ {\mathrm{\η}}_y\\ {\mathrm{\η}}_z\end{array}\right]$

星敏感器测得的载体姿态四元数Q_CNS相对于惯性导航系统计算出载体姿态四元数Q_INS精度更高，且不随时间发散。所以可以认为Q_CNS为组合导航系统的姿态真值，Q_INS为组合导航系统的姿态估计值。则由上面误差四元数的定义可知：

则卡尔曼滤波器的量测方程为：

Z(t)＝H(t)X(t)+V(t)

其中：

(2)每一步惯性导航解算后，利用陀螺输出的数据可求得当前时刻卡尔曼滤波的状态矩阵F(t)，利用卡尔曼滤波离散化方程，将F(t)进行离散化可以求得卡尔曼滤波一步转移阵A_k，继而求得卡尔曼滤波的状态量一步预测和一步预测均方误差阵若当前天文导航数据不可用，则令状态量一步预测等于状态估计量一步预测均方误差阵等于估计均方误差P_k；若当前天文导航数据可用，则使用标准离散卡尔曼滤波方法进行系统状态估计量和一步均方误差阵P_k的计算。

(3)将当前时刻的陀螺输出值视为两次天文导航量测之间的陀螺输出均值，采用毕卡逼近法，可以从上一时刻的天文导航量测信息Q1估计当前时刻的天文导航量测信息记当前时刻天文导航实际量测信息为Q₂，则和Q2的误差四元数δQ₂为：

若误差四元数δQ₂大于一定的阙值，则判定当前时刻天文导航量测信息为野值，应将其剔除。

(4)实时在线估计卡尔曼滤波系统状态变量中陀螺常值漂移的收敛情况。陀螺常值漂移估计值是否收敛的判断依据是卡尔曼滤波器的振荡程度是否趋于稳定。以最近30次的卡尔曼滤波器陀螺常值漂移的估计值作为样本，当其标准差小于均值的一百倍时，即可认为陀螺常值漂移估计值趋于收敛。

(5)若某轴陀螺常值漂移估计值判断为收敛后，利用最近30次的陀螺常值漂移估计值的平均值作为陀螺常值漂移，然后对该轴惯性测量单元的陀螺的输出值进行误差的修正。

(6)利用卡尔曼滤波得到的系统状态变量的最优估计值去修 正惯性导航系统后，需将状态变量中姿态的乘性四元数误差重置成无姿态误差状态，即：

${\hat{X}}_k={\left[\begin{array}{ccccccc}{\mathrm{\δq}}_0& \mathrm{\δ}{q}_1& \mathrm{\δ}{q}_2& {\mathrm{\δq}}_3& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T={\left[\begin{array}{ccccccc}1& 0& 0& 0& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T$ 。

Claims (7)

1.一种惯性/天文组合导航方法，其特征在于：包括以下步骤：

(1)在天文导航量测信息不可用时，利用惯性测量单元的量测信息进行惯性导航解算，获得载体的位置、速度和姿态参数；在天文导航量测信息可用时，建立惯性/天文组合导航系统的状态方程和量测方程，采用卡尔曼滤波信息融合方法，利用组合导航量测信息去修正惯性导航系统的误差，实现高精度的导航；

(2)利用上一时刻的天文导航量测信息和当前时刻陀螺输出值对当前时刻的天文导航量测信息进行估计，并将估计值与当前时刻的天文实际量测信息进行比较，若偏离超过一定的阈值，则判定天文导航量测信息为野值，将其剔除；

(3)利用卡尔曼滤波器的振荡程度来判断陀螺常值漂移的估计是否收敛；若卡尔曼滤波器状态量估计值振荡衰减，则表明卡尔曼滤波器的估计值逐渐趋向于收敛；若判断陀螺常值漂移估计值收敛后，则利用估计出的陀螺常值漂移校正陀螺输出。

2.根据权利要求1所述的一种惯性/天文组合导航方法，其特征在于：组合导航系统的状态变量为七维，状态变量前面四维是载体姿态误差的四元数乘性表示，状态变量后面三维为惯性导航系统三轴陀螺的常值漂移。

3.根据权利要求1所述的一种惯性/天文组合导航方法，其特征在于：卡尔曼滤波器状态方程中状态矩阵只与陀螺测量值相关，所以卡尔曼滤波器的一步预测值与一步预测均方误差随惯性导航系统频率更新。

4.根据权利要求1所述的一种惯性/天文组合导航方法，其特征在于：将当前时刻的陀螺输出值作为天文导航周期内陀螺输出均值，以上一时刻的天文导航量测信息为起点，采用毕卡逼近法估测当前时刻的天文导航量测信息；将估测的天文导航量测信息与当前时刻实际的天文导航量测信息进行比较，若两者偏离超过一定的阙值，则判定当前时刻的天文导航量测信息为野值，将其剔除。

5.根据权利要求1所述的一种惯性/天文组合导航方法，其特征在于：陀螺常值漂移估计值收敛的判断依据是卡尔曼滤波器的振荡程度；以最近30次的卡尔曼滤波器陀螺常值漂移的估计值作为样本，当其标准差小于均值的一百倍时，即可认为陀螺常值漂移估计值趋于收敛。

6.根据权利要求5所述的一种惯性/天文组合导航方法，其特征在于：所述陀螺常值漂移估计值判断为收敛后，利用最近30次的陀螺常值漂移估计值的平均值作为陀螺常值漂移；对陀螺常值漂移误差的修正是独立分轴进行的。

7.根据权利要求1所述的一种惯性/天文组合导航方法，其特征在于：利用卡尔曼滤波得到的系统状态变量的最优估计值去修正惯性导航系统后，需要将状态变量中的载体姿态乘性四元数误差重置成无姿态误差状态。