CN107036598A - 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法 - Google Patents

Abstract

本发明公开了一种基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，属于飞行器组合导航技术领域。该方法首先建立了陀螺与加速度计误差模型；随后，将陀螺和加速度计误差扩展为***状态量，建立基于对偶四元数的惯性/天文组合导航状态方程与量测方程，并采用基于奇异值分解的可观测度分析方法对***进行降维处理；最后,采用卡尔曼滤波方法对各状态量进行开环估计，利用估计值对陀螺原始输出进行在线修正。本方法在保证导航精度的同时提高了组合导航***的实时性，且在天文星敏感器信息丢失的情况下，惯性导航***仍能在一段时间内保持较高的精度，从而提高了惯性导航***性能。

Description

基于陀螺误差修正的对偶四元数惯性/天文组合导航方法

技术领域

本发明涉及一种基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，属于飞行器组合导航技术领域。

背景技术

近年来，随着高超声速飞行器、空天飞行器等高动态飞行器的研制发展，导航***性能面临着更大的挑战。纯惯导***由于陀螺漂移等误差的存在，其导航误差会随着时间发散，因此需要误差不随时间增长的外部信息源对其进行校正。天文导航***利用天文敏感器对天体进行观测，根据天体的方位信息解算出载体姿态，其精度可达到角秒级，具有自主性强、测量精度高、隐蔽性好、误差不随时间累积的优点。惯性/天文组合将惯性导航和天文导航进行优势互补，利用天文导航的高精度姿态信息对惯导进行校正，而且还可以有效修正陀螺漂移，在远程长航时飞行器、高超声速飞行器、空天飞行器、近空间飞行器等领域都具有重要应用价值。

在高动态情况下，基于对偶四元数的捷联惯性导航算法相较于传统惯性导航算法具有更高的精度。已有的基于对偶四元数的惯性/天文组合导航***维数较高，增加了计算机的计算负担，实时性较差，而空天飞行器等高动态飞行器对导航解算的实时性具有较高的要求。因此，如何降低基于对偶四元数的惯性/天文组合导航***维数并保证惯性/天文组合导航精度具有重要的研究意义。

发明内容

本发明提出了一种基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，在保证飞行器导航精度的同时减少了滤波器计算量，提高组合导航***的实时性，利用开环估计对陀螺常值漂移误差进行在线补偿，可以保证在天文星敏感器信息丢失的情况下，惯导仍能在一段时间内保持较高的精度，从而显著提高惯性导航***精度。

本发明为解决上述技术问题采用以下技术方案：

一种基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，包括如下步骤：

步骤1，将陀螺误差和加速度计误差均建模为常值误差与高斯白噪声随机误差，并将陀螺和加速度计的常值误差扩展为***状态量，由此得到基于加性对偶四元数的惯性/天文组合导航状态方程；

步骤2，天文星敏感器的输出值为机体系相对于惯性系的旋转四元数，将其输出误差建模为加性四元数，并利用陀螺输出的原始信息计算出机体系相对于惯性系的旋转四元数，结合两者的信息，构建基于加性对偶四元数的惯性/天文组合导航量测方程；

步骤3，在步骤1、2的组合导航状态方程和量测方程的基础上，采用基于奇异值分解的可观测度分析方法对该状态方程中的状态变量进行可观测度分析，根据其分析结果，剔除可观测性差和不可观的状态分量，得到降维后的基于加性对偶四元数的惯性/天文组合导航状态方程；

步骤4，对***状态方程和量测方程进行离散化处理，并采用卡尔曼滤波方法对各状态变量进行开环估计，利用开环估计得到的陀螺常值漂移误差估计值对陀螺原始输出信息进行在线修正，然后基于修正后的陀螺输出信息进行对偶四元数捷联惯性导航解算。

步骤1所述陀螺和加速度计误差模型为：

其中，为陀螺误差，为陀螺常值漂移误差，为陀螺高斯白噪声；δf^B为加速度计误差，为加速度计常值误差，为加速度高斯白噪声。

步骤1所述的基于加性对偶四元数的惯性/天文组合导航状态方程：

其中X∈R^24×1为***状态量，F∈R^24×24为***矩阵，G∈R^24×8为噪声系数矩阵，w∈R^{8 ×1}为***噪声向量，为X的一阶导数，各矩阵分别表示为：

***矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q₀q₁q₂q₃]^T的形式，我们定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可表示为：

式(3)～(6)中0均为四阶零矩阵，I₄为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数；δq_IT为推力速度对偶四元数误差的实数部分，δq_I′_T为推力速度对偶四元数误差的对偶部分，δq_I′_G为引力速度对偶四元数误差的对偶部分，δq_I′_U为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘法中的后乘矩阵；q_IT为推力速度对偶四元数的实数部分，为q_IT在四元数乘法中的前乘矩阵；q′_IT为推力对偶四元数的对偶部分，为q_I′_T在四元数乘法中的前乘矩阵；为地球自转角速度，为在四元数乘法中的后乘矩阵；q_IU为位置对偶四元数的实数部分，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；q^* _IT为q_IT的共轭四元数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；f_B为加速度计输出信息，为其在四元数乘法中的后乘矩阵。

步骤2所述的天文星敏感器加性四元数输出误差模型为：

q_c＝q_IB-δq_IB，

其中，q_c为天文星敏感器输出的机体系相对于惯性系的旋转四元数，q_IB为真实的机体系相对于惯性系的旋转四元数，δq_IB为天文星敏感器量测误差，将其建模为高斯白噪声。

步骤2所述的基于加性对偶四元数的惯性/天文组合导航量测方程为：

Z＝HX+v，

其中，Z＝q_IT-q_c为量测向量，H＝[I_4×4 0_4×20]为量测系数矩阵，X为***状态量，v＝δq_IB为***量测噪声。

所述步骤3的具体过程为：

设N为某时间段内基于对偶四元数的惯性/天文组合导航***的可观测性矩阵，X(t₀)为其初始状态，Z₁为量测值，则：

Z₁＝N·X(t₀)，

其中某一时间段的可观测性矩阵N为：

N＝[H^T (HF)^T ... (HF^n-1)^T]，

其中：H^T为量测系数矩阵H的转置；(HF)^T为量测系数矩阵H与***矩阵F乘积的转置；F^n-1为F的n-1次幂，(HF^n-1)^T为H与F^n-1乘积的转置；

对N阵进行奇异值分解，可得：

N＝U·S·V^T，

式中，U＝[u₁ u₂ ... u_m]，V＝[v₁ v₂ ... v_m]均为正交矩阵，u₁为矩阵U第一个列向量，u₂为矩阵U第二个列向量，u_m为矩阵U第m个列向量，v₁为矩阵V第一个列向量，v₂为矩阵V第二个列向量，v_m为矩阵V第m个列向量，S中Λ_r×r＝diag(σ₁σ₂...σ_r)，σ₁＞σ₂＞...＞σ_r＞0为矩阵N的奇异值；

由上式可知：

其中：σ_i为矩阵N的第i个奇异值；v_i为矩阵V的第i个列向量，为v_i的转置；u_i为矩阵U的第i个列向量，t₀表示初始时刻；

则：

其中，r为奇异值的总个数，为u_i的转置，根据上式计算出每一个奇异值σ_i所对应的初始状态向量，σ_i为初始状态向量中绝对值最大的状态分量所对应的奇异值，各状态分量对应的奇异值的相对大小就反映了各状态分量的相对可观测性；

根据上述基于奇异值分解的可观测度分析方法对步骤1中的状态变量进行可观测度分析，具有可观测性的状态变量为四元数δq_IT及四元数实际上是标量部分为0的四元数，其中分别为陀螺x、y、z轴方向的常值漂移误差分量；四元数δq_IT可写作δq_IT＝[δq_IT0 δq_IT1 δq_IT2 δq_IT3]^T，δq_IT0为δq_IT的标量部分，δq_IT1、δq_IT2、δq_IT3为δq_IT的矢量部分，将原24维的组合导航***降维为7维，降维后的基于对偶四元数的惯性/天文组合导航状态方程为：

其中，X₁∈R^7×1为降维后的***状态量，F₁∈R^7×7为降维后的***矩阵，G₁∈R^7×3为降维后的噪声系数矩阵，w₁∈R^3×1为降维后的***噪声向量，为X₁的一阶导数，各矩阵分别表示为：

其中，分别为在陀螺x、y、z轴方向上的分量；将四元数q_IT写作q_IT＝[q_IT0 q_IT1 q_IT2 q_IT3]^T,则和M′(q_IT)为：

其中，分别为在x、y、z轴方向的分量。

所述步骤4的具体过程为：

将***状态方程和量测方程进行离散化处理：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k为t_k时刻***状态量,X_k-1为t_k-1时刻***状态量，Φ_k,k-1为t_k-1时刻至t_k时刻***的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻***的噪声驱动矩阵，W_k-1为t_k-1时刻***的噪声矩阵，Z_k为t_k时刻***的量测矩阵,H_k为t_k时刻的量测系数矩阵，V_k为t_k时刻的噪声矩阵；

采用卡尔曼滤波对状态量进行开环跟踪估计：

其中，是状态量X_k-1的一步预测估计值,P_k-1为t_k-1时刻滤波状态估计协方差矩阵,Q_k-1为t_k-1时刻***噪声协方差矩阵,为Φ_k,k-1的转置，为Γ_k,k-1的转置，P_k,k-1为t_k-1时刻到t_k时刻的状态一步预测协方差矩阵,R_k为t_k时刻的量测噪声协方差矩阵,K_k为t_k时刻滤波增益矩阵,为H_k的转置，为状态量X_k的卡尔曼滤波估值，I为单位矩阵，为K_k的转置，P_k为t_k时刻滤波状态估计协方差矩阵；

利用上述方法得到陀螺常值漂移误差的估计值后，在线对陀螺原始输出信息进行修正：

其中,为陀螺常值漂移误差估计值，则为陀螺经过误差在线修正后的输出信息，基于此,利用对偶四元数捷联惯导算法进行导航解算即得到地理系的位置、速度、姿态等导航信息。

本发明的有益效果如下：

1、本发明对陀螺和加速度计误差进行建模，包括常值误差和高斯白噪声误差模型，并将陀螺和加速度计的常值误差扩展为卡尔曼滤波器***状态量，利用基于奇异值分解的可观测度分析方法对***状态方程进行降维，在保证导航精度的同时减少了滤波器计算量，提高了组合导航***的实时性。

2、本方法利用开环估计对陀螺漂移误差进行在线标定补偿，可保证在天文星敏感器信息丢失的情况下，惯性导航***仍能在一段时间内保持较高的精度，从而提高了惯性导航***性能，适用于工程应用。

附图说明

图1是本发明基于陀螺误差修正的对偶四元数惯性/天文组合导航方法的架构图。

具体实施方式

下面结合附图对本发明创造做进一步详细说明。

如图1所示，本发明所述的基于陀螺误差修正的对偶四元数惯性/天文组合导航方法的原理是：利用基于奇异值分解的可观测度分析方法对对偶四元数惯性/天文组合导航***状态方程进行可观测度分析，根据其分析结果得到降维后的卡尔曼滤波状态方程及量测方程。陀螺的测量输出为利用该陀螺输出信息进行推力速度对偶四元数计算，并将其实数部分q_IT与天文星敏感器输出的转换四元数q_c通过卡尔曼滤波器进行融合，在线估计得到陀螺常值漂移误差估计值利用该信息对陀螺原始输出信息进行在线修正，修正后的陀螺信息进入对偶四元数捷联惯性导航解算模块，以提高惯性导航***性能。

本发明的具体实施方式如下：

1、建立对偶四元数惯性/天文组合导航***状态方程

(1.1)陀螺、加速度计误差建模

陀螺误差和加速度计误差包括常值误差及高斯白噪声随机误差，其误差模型可以看作各误差源组成的线性模型，因此陀螺和加速度计误差模型为：

式(1)中，为陀螺误差，为陀螺常值漂移误差，为陀螺高斯白噪声；δf^B为加速度计误差，为加速度计常值误差，为加速度高斯白噪声。

(1.2)基于陀螺、加速度计误差模型的组合导航***状态方程

将陀螺和加速度计的常值误差扩展为***状态量，由此得到对偶四元数惯性/天文组合导航***状态方程：

式(2)中，X∈R^24×1为***状态量，F∈R^24×24为***矩阵，G∈R^24×8为噪声系数矩阵，w∈R^8×1为***噪声向量，为X的一阶导数，各矩阵分别表示为：

***矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q₀q₁q₂q₃]^T的形式，我们定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可表示为：

式(3)～(6)中0均为四阶零矩阵，I₄为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数。δq_IT为推力速度对偶四元数误差的实数部分，δq_I′_T为推力速度对偶四元数误差的对偶部分，δq_I′_G为引力速度对偶四元数误差的对偶部分，δq_I′_U为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘法中的后乘矩阵；q_IT为推力速度对偶四元数的实数部分，为q_IT在四元数乘法中的前乘矩阵；q_I′_T为推力对偶四元数的对偶部分，为q_I′_T在四元数乘法中的前乘矩阵；为地球自转角速度，为在四元数乘法中的后乘矩阵；q_IU为位置对偶四元数的实数部分，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；为q_IT的共轭四元数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；f_B为加速度计输出信息，为其在四元数乘法中的后乘矩阵。

2、建立对偶四元数的惯性/天文组合导航***量测方程

(2.1)天文星敏感器输出误差建模

由于所建立的对偶四元数惯性/天文组合导航***状态方程是基于加性对偶四元数的，因此将天文星敏感器的输出误差模型建模为加性四元数误差模型：

q_c＝q_IB-δq_IB (8)

其中，q_c为天文星敏感器输出的机体系相对于惯性系的旋转四元数，q_IB为真实的机体系相对于惯性系的旋转四元数，δq_IB为天文星敏感器量测误差，将其建模为高斯白噪声。

(2.2)基于天文量测信息的组合导航***量测方程

将利用陀螺输出计算得到的q_IT与天文星敏感器的输出信息q_c的差值作为量测量，其量测方程为：

Z＝HX+v (9)

其中，Z＝q_IT-q_c为量测向量，H＝[I_4×4 0_4×20]为量测系数矩阵，X为***状态量，v＝δq_IB为***量测噪声。

3、基于可观测度分析的降维滤波器设计

(3.1)基于奇异值分解的状态可观测度分析

设N(m₁×n维)为某时间段内基于对偶四元数的惯性/天文组合导航***的可观测性矩阵，X(t₀)(n维)为其初始状态，Z₁(m₁维)为量测值，则：

Z₁＝N·X(t₀) (10)

其中某一时间段的可观测性矩阵N为：

N＝[H^T (HF)^T ...(HF^n-1)^T] (11)

其中：H^T为量测系数矩阵H的转置；(HF)^T为量测系数矩阵H与***矩阵F乘积的转置；F^n-1为F的n-1次幂，(HF^n-1)^T为H与F^n-1乘积的转置；

对N阵进行奇异值分解，可得：

N＝U·S·V^T (12)

式(12)中，U＝[u₁ u₂ ... u_m]，V＝[v₁ v₂ ... v_m]均为正交矩阵，u₁为矩阵U第一个列向量，u₂为矩阵U第二个列向量，u_m为矩阵U第m个列向量，v₁为矩阵V第一个列向量，v₂为矩阵V第二个列向量，v_m为矩阵V第m个列向量，V^T为V的转置；S中Λ_r×r＝diag(σ₁σ₂...σ_r)，σ₁＞σ₂＞...＞σ_r＞0为矩阵N的奇异值。

由上式可知：

其中：σ_i为矩阵N的第i个奇异值；v_i为矩阵V的第i个列向量，为v_i的转置；u_i为矩阵U的第i个列向量，t₀表示初始时刻；

则：

其中，r为奇异值的总个数，为u_i的转置。根据式(14)可以计算出每一个奇异值σ_i所对应的初始状态向量，σ_i为初始状态向量中绝对值最大的状态分量所对应的奇异值，各状态分量对应的奇异值的相对大小就反映了各状态分量的相对可观测性。

(3.2)***降维滤波器设计

根据上述基于奇异值分解的可观测度分析方法对(1.2)中的状态变量进行可观测度分析，具有可观测性的状态变量为四元数δq_IT及四元数实际上是标量部分为0的四元数，其中分别为陀螺x、y、z轴方向的常值漂移误差分量。四元数δq_IT可写作δq_IT＝[δq_IT0 δq_IT1 δq_IT2 δq_IT3]^T，其中δq_IT0为δq_IT的标量部分，δq_IT1、δq_IT2、δq_IT3为δq_IT的矢量部分。因此可将原24维的组合导航***降维为7维，降维后的基于对偶四元数的惯性/天文组合导航状态方程为：

其中，X₁∈R^7×1为降维后的***状态量，F₁∈R^7×7为降维后的***矩阵，G₁∈R^7×3为降维后的噪声系数矩阵，w₁∈R^3×1为降维后的***噪声向量，为X₁的一阶导数，各矩阵可分别表示为：

式(17)中，分别为在陀螺x、y、z轴方向上的分量。将四元数q_IT写作q_IT＝[q_IT0 q_IT1 q_IT2 q_IT3]^T,则式(18)中和M′(q_IT)为：

其中，分别为在x、y、z轴方向的分量。

4、陀螺常值漂移误差在线估计与修正

(4.1)***状态方程、量测方程离散化处理

将***状态方程和量测方程进行离散化处理：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1 (22)

Z_k＝H_kX_k+V_k (23)

其中，X_k为t_k时刻***状态量,X_k-1为t_k-1时刻***状态量，Φ_k,k-1为t_k-1时刻至t_k时刻***的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻***的噪声驱动矩阵，W_k-1为t_k-1时刻***的噪声矩阵，Z_k为t_k时刻***的量测矩阵,H_k为t_k时刻的量测系数矩阵，V_k为t_k时刻的噪声矩阵；

(4.2)卡尔曼滤波开环估计

采用卡尔曼滤波对状态量进行开环跟踪估计：

其中，是状态量X_k-1的一步预测估计值,P_k-1为t_k-1时刻滤波状态估计协方差矩阵,Q_k-1为t_k-1时刻***噪声协方差矩阵,为Φ_k,k-1的转置，为Γ_k,k-1的转置，P_k,k-1为t_k-1时刻到t_k时刻的状态一步预测协方差矩阵,R_k为t_k时刻的量测噪声协方差矩阵,K_k为t_k时刻滤波增益矩阵,为H_k的转置，为状态量X_k的卡尔曼滤波估值，I为单位矩阵，为K_k的转置，P_k为t_k时刻滤波状态估计协方差矩阵；

(4.3)陀螺误差在线修正

利用(4.2)得到陀螺常值漂移误差的估计值后，在线对陀螺原始输出信息进行修正：

其中,为陀螺常值漂移误差估计值，则为陀螺经过误差在线修正后的输出信息，基于此,利用对偶四元数捷联惯性导航算法进行导航解算即可得到地理系的位置、速度、姿态等导航信息。

Claims (7)

1.一种基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，包括如下步骤：

步骤1，将陀螺误差和加速度计误差均建模为常值误差与高斯白噪声随机误差，并将陀螺和加速度计的常值误差扩展为***状态量，由此得到基于加性对偶四元数的惯性/天文组合导航状态方程；

步骤2，天文星敏感器的输出值为机体系相对于惯性系的旋转四元数，将其输出误差建模为加性四元数，并利用陀螺输出的原始信息计算出机体系相对于惯性系的旋转四元数，结合两者的信息，构建基于加性对偶四元数的惯性/天文组合导航量测方程；

步骤3，在步骤1、2的组合导航状态方程和量测方程的基础上，采用基于奇异值分解的可观测度分析方法对该状态方程中的状态变量进行可观测度分析，根据其分析结果，剔除可观测性差和不可观的状态分量，得到降维后的基于加性对偶四元数的惯性/天文组合导航状态方程；

步骤4，对***状态方程和量测方程进行离散化处理，并采用卡尔曼滤波方法对各状态变量进行开环估计，利用开环估计得到的陀螺常值漂移误差估计值对陀螺原始输出信息进行在线修正，然后基于修正后的陀螺输出信息进行对偶四元数捷联惯性导航解算。

2.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，步骤1所述陀螺和加速度计误差模型为：

其中，为陀螺误差，为陀螺常值漂移误差，为陀螺高斯白噪声；δf^B为加速度计误差，δf_c ^B为加速度计常值误差，δf_s ^B为加速度高斯白噪声。

3.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，步骤1所述的基于加性对偶四元数的惯性/天文组合导航状态方程：

其中X∈R^24×1为***状态量，F∈R^24×24为***矩阵，G∈R^24×8为噪声系数矩阵，w∈R^8×1为***噪声向量，为X的一阶导数，各矩阵分别表示为：

1

***矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q₀ q₁ q₂ q₃]^T的形式，我们定义矩阵为q在四元数乘法中的前乘矩阵，为q在四元数乘法中的后乘矩阵，其具体可表示为：

式(3)～(6)中0均为四阶零矩阵，I₄为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数；δq_IT为推力速度对偶四元数误差的实数部分，δq′_IT为推力速度对偶四元数误差的对偶部分，δq′_IG为引力速度对偶四元数误差的对偶部分，δq′_IU为位置对偶四元数误差的对偶部分；为陀螺输出信息，为在四元数乘法中的后乘矩阵；q_IT为推力速度对偶四元数的实数部分，为q_IT在四元数乘法中的前乘矩阵；q′_IT为推力对偶四元数的对偶部分，为q′_IT在四元数乘法中的前乘矩阵；为地球自转角速度，为在四元数乘法中的后乘矩阵；q_IU为位置对偶四元数的实数部分，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；q^* _IT为q_IT的共轭四元数，为其在四元数乘法中的前乘矩阵，为其在四元数乘法中的后乘矩阵；f_B为加速度计输出信息，为其在四元数乘法中的后乘矩阵。

4.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，步骤2所述的天文星敏感器加性四元数输出误差模型为：

q_c＝q_IB-δq_IB，

其中，q_c为天文星敏感器输出的机体系相对于惯性系的旋转四元数，q_IB为真实的机体系相对于惯性系的旋转四元数，δq_IB为天文星敏感器量测误差，将其建模为高斯白噪声。

5.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，步骤2所述的基于加性对偶四元数的惯性/天文组合导航量测方程为：

Z＝HX+v，

其中，Z＝q_IT-q_c为量测向量，H＝[I_4×4 0_4×20]为量测系数矩阵，X为***状态量，v＝δq_IB为***量测噪声。

6.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，所述步骤3的具体过程为：

设N为某时间段内基于对偶四元数的惯性/天文组合导航***的可观测性矩阵，X(t₀)为其初始状态，Z₁为量测值，则：

Z₁＝N·X(t₀)，

其中某一时间段的可观测性矩阵N为：

N＝[H^T (HF)^T ... (HF^n-1)^T]，

其中：H^T为量测系数矩阵H的转置；(HF)^T为量测系数矩阵H与***矩阵F乘积的转置；F^n-1为F的n-1次幂，(HF^n-1)^T为H与F^n-1乘积的转置；

对N阵进行奇异值分解，可得：

N＝U·S·V^T，

式中，U＝[u₁ u₂ ... u_m]，V＝[v₁ v₂ ... v_m]均为正交矩阵，u₁为矩阵U第一个列向量，u₂为矩阵U第二个列向量，u_m为矩阵U第m个列向量，v₁为矩阵V第一个列向量，v₂为矩阵V第二个列向量，v_m为矩阵V第m个列向量，S中Λ_r×r＝diag(σ₁ σ₂...σ_r)，σ₁＞σ₂＞...＞σ_r＞0为矩阵N的奇异值；

由上式可知：

其中：σ_i为矩阵N的第i个奇异值；v_i为矩阵V的第i个列向量，为v_i的转置；u_i为矩阵U的第i个列向量，t₀表示初始时刻；

则：

其中，r为奇异值的总个数，为u_i的转置，根据上式计算出每一个奇异值σ_i所对应的初始状态向量，σ_i为初始状态向量中绝对值最大的状态分量所对应的奇异值，各状态分量对应的奇异值的相对大小就反映了各状态分量的相对可观测性；

根据上述基于奇异值分解的可观测度分析方法对步骤1中的状态变量进行可观测度分析，具有可观测性的状态变量为四元数δq_IT及四元数实际上是标量部分为0的四元数，其中分别为陀螺x、y、z轴方向的常值漂移误差分量；四元数δq_IT可写作δq_IT＝[δq_IT0 δq_IT1 δq_IT2 δq_IT3]^T，δq_IT0为δq_IT的标量部分，δq_IT1、δq_IT2、δq_IT3为δq_IT的矢量部分，将原24维的组合导航***降维为7维，降维后的基于对偶四元数的惯性/天文组合导航状态方程为：

其中，X₁∈R^7×1为降维后的***状态量，F₁∈R^7×7为降维后的***矩阵，G₁∈R^7×3为降维后的噪声系数矩阵，w₁∈R^3×1为降维后的***噪声向量，为X₁的一阶导数，各矩阵分别表示为：

其中，分别为在陀螺x、y、z轴方向上的分量；将四元数q_IT写作q_IT＝[q_IT0 q_IT1 q_IT2 q_IT3]^T,则和M′(q_IT)为：

其中，分别为在x、y、z轴方向的分量。

7.根据权利要求1所述基于陀螺误差修正的对偶四元数惯性/天文组合导航方法，其特征在于，所述步骤4的具体过程为：

将***状态方程和量测方程进行离散化处理：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k为t_k时刻***状态量,X_k-1为t_k-1时刻***状态量，Φ_k,k-1为t_k-1时刻至t_k时刻***的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻***的噪声驱动矩阵，W_k-1为t_k-1时刻***的噪声矩阵，Z_k为t_k时刻***的量测矩阵,H_k为t_k时刻的量测系数矩阵，V_k为t_k时刻的噪声矩阵；

采用卡尔曼滤波对状态量进行开环跟踪估计：

其中，是状态量X_k-1的一步预测估计值,P_k-1为t_k-1时刻滤波状态估计协方差矩阵,Q_k-1为t_k-1时刻***噪声协方差矩阵,为Φ_k,k-1的转置，为Γ_k,k-1的转置，P_k,k-1为t_k-1时刻到t_k时刻的状态一步预测协方差矩阵,R_k为t_k时刻的量测噪声协方差矩阵,K_k为t_k时刻滤波增益矩阵,为H_k的转置，为状态量X_k的卡尔曼滤波估值，I为单位矩阵，为K_k的转置，P_k为t_k时刻滤波状态估计协方差矩阵；

利用上述方法得到陀螺常值漂移误差的估计值后，在线对陀螺原始输出信息进行修正：

其中,为陀螺常值漂移误差估计值，则为陀螺经过误差在线修正后的输出信息，基于此,利用对偶四元数捷联惯导算法进行导航解算即得到地理系的位置、速度、姿态等导航信息。