CN104034329B - 采用发射惯性系下的多组合导航处理装置的导航方法 - Google Patents

Abstract

本发明公布了一种发射惯性系下的多组合导航处理装置及其导航方法，属于飞行器组合导航技术领域。导航处理装置包括传感器模块、导航处理装置和显示模块。导航方法包括以下步骤：首先直接建立导弹在发射惯性系下的组合导航系统状态模型和量测模型，研制发射惯性系下的SINS/GPS/CNS组合导航方法，采用联邦滤波进行多信息最优融合处理，最终直接输出导弹在发射惯性系下的姿态、位置和速度参数。本发明能够有效实现发射惯性系下的SINS/GPS/CNS多信息融合处理，获取高精度的导航结果，同时为SINS/GPS/CNS组合导航系统的工程化实现和应用提供了有效的支撑。

Description

采用发射惯性系下的多组合导航处理装置的导航方法

技术领域

本发明公布了一种采用发射惯性系下的多组合导航处理装置的导航方法，属于飞行器组合导航技术领域。

背景技术

近年来，基于嵌入式操作系统的组合导航处理机工程化产品已日渐成熟，并在机载和弹载等环境下得到了广泛应用，但其核心导航方法主要局限于惯性/卫星、惯性/天文组合，应用于实际的多组合导航系统尚未见报道。针对远程导弹和高空巡航导弹飞行距离远、航行时间长等特点，典型的以惯性导航为基础的惯性/卫星单一组合导航系统，由于卫星导航系统容易受到欺骗干扰，从而无法满足导弹对导航参数的高可靠、高精度的测量要求。因此，将捷联惯性导航系统(SINS)、卫星定位系统(GPS)和天文导航系统(CNS)等多种导航系统的信息进行最优融合，构建SINS/GPS/CNS多组合导航系统是实现远程导弹、高空巡航导弹高精度导航的保证，具有重要意义。

考虑到卫星定位系统和天文导航系统的量测输出参考坐标系有较大的差异，因此，需要解决在统一的时空导航坐标参考系下实现和SINS信息最优融合的问题。传统的SINS/GPS/CNS多组合导航系统方法主要采用了当地地理参考坐标系作为统一的导航参考坐标系，其组合方法模型通常采用传统的航空组合导航方法模型，在此基础上，将组合获得的导航参数经过一定的参数转换变换为发射惯性系下的导航参数进行输出。该方法实现复杂、需要经过参数的多种转换，不可避免带来转换计算误差，影响系统精度。鉴于导弹在发射过程中，通常需要直接获得发射惯性系下的高精度导航参数这一需求现状，对于发射惯性系下的SINS/GPS/CNS多组合导航系统还需要进一步的研究。

发明内容

为解决导弹在发射惯性系下的多组合导航问题，本发明提出了一种采用发射惯性系下的多组合导航处理装置的导航方法，能有效实现发射惯性系下的SINS/GPS/CNS多信息融合处理，获取高精度的导航结果，同时为SINS/GPS/CNS组合导航系统的工程化实现和应用提供了有效的支撑，具有突出的应用价值。

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

一种采用发射惯性系下的多组合导航处理装置的导航方法，该方法所采用的多组合导航处理装置包括传感器模块、导航处理装置和显示模块，其中传感器模块包括惯性传感器、卫星导航接收机和星敏感器，导航处理装置包括接口模块、导航计算机、电源模块；惯性传感器、卫星导航接收机和星敏感器分别与接口模块连接，接口模块与电源模块连接，接口模块与导航计算机连接，导航计算机与显示模块连接；该导航方法包括以下步骤：

(1)对于SINS/GPS/CNS多组合导航系统，其发射惯性系下的捷联惯导解算过程如下，速度计算公式为：

$C_b^f{f}_b=\stackrel{\·}{v}-C_i^{f}g---\left(2.1\right)$

式中，v为发射惯性系下的速度值，为v对时间的导数，g为地球的万有引力，为发射惯性系相对于本体系的姿态矩阵，为发射惯性系相对于地心惯性系的姿态矩阵，f_b为加速度真值；

位置计算公式为：

$\stackrel{\·}{p}=v---\left(2.2\right)$

式中，p为发射惯性系下的位置值，为p对时间的导数；

姿态运动有四元数描述，计算公式为：

式中，q为姿态四元数，为q对时间的导数，ω_b为为角速度真值；

陀螺和加速度计的测量模型如下：

$\left\{\begin{array}{c}{\mathrm{\ω}}_c={\mathrm{\ω}}_b+{\mathrm{\ω}}_r+{\mathrm{\ω}}_{\mathrm{\ϵ}}\\ f_c=f_b+f_r+f_{\mathrm{\ϵ}}\\ {\stackrel{\·}{\mathrm{\ω}}}_r={\mathrm{\ω}}_n\\ {\stackrel{\·}{f}}_r=f_n\end{array}\right.---\left(2.4\right)$

式中，ω_b为角速度真值，ω_r为陀螺随机游走误差，ω_ε为陀螺测量噪声，f_r为加速度计随机游走误差，f_ε为加速度计测量噪声，ω_n为陀螺随机游走驱动噪声，f_n为加速度计随机游走驱动噪声，f_c为加速度的实际量测值，ω_c为角速度的实际量测值，为ω_r对时间的导数，为f_r对时间的导数；

组合导航系统为非线性对象，采用扩展卡尔曼滤波进行数值计算，根据公式(2.1)、(2.2)、(2.3)和(2.4)建立发射惯性系下的误差增量方程如下：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{q}}_{13}=-\[{\widehat{\mathrm{\ω}}}_b\×\]\·{\mathrm{\δq}}_{13}-0.5\·{\mathrm{\δ\ω}}_r-0.5\·{\mathrm{\ω}}_{\mathrm{\ϵ}}\\ \mathrm{\δ}\stackrel{\·}{p}=\mathrm{\δ}\mathrm{\υ}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\υ}}=2C_b^f\·\[{\hat{f}}_b\×\]\·{\mathrm{\δq}}_{13}-C_b^f\·{\mathrm{\δf}}_r-C_b^f\·f_{\mathrm{\ϵ}}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ω}}}_r={\mathrm{\ω}}_n\\ \mathrm{\δ}{\stackrel{\·}{f}}_r=f_n\end{array}\right.---\left(2.5\right)$

式中，δq₁₃为数学平台误差的四元数矢量部分，δp为位置误差状态量，δv为速度误差状态量，为δq₁₃对时间的导数，为δp对时间的导数，为δv对时间的导数；表示陀螺角速率的反对称矩阵；为加速度的反对称矩阵， ω_bx为角速度真值在X轴的分量；ω_by为角速度真值在Y轴的分量；ω_bz为角速度真值在Z轴的分量；f_bx为加速度真值在X轴的分量；f_by为加速度真值在Y轴的分量；f_bz为加速度真值在Z轴的分量，δω_r为陀螺随机游走误差增量，为δω_r对时间的导数，δf_r为加速度计随机游走误差增量，为δf_r对时间的导数；

(2)基于上述，获得SINS/GPS/CNS多组合导航系统状态方程为：

$\stackrel{\·}{X}{\left(t\right)}_{15\×1}=A{\left(t\right)}_{15\×15}X\left(t\right)+G{\left(t\right)}_{15\×12}W{\left(t\right)}_{12\×1}---\left(2.6\right)$

其中，状态变量为：

X＝[δq₁ δq₂ δq₃ δp_x δp_y δp_z δv_x δv_y δv_z δω_rx δω_ry δω_rz δf_rx δf_ry δf_rz]^T

δq₁,δq₂,δq₃表示惯性导航系统误差状态量中数学平台误差的四元数矢量部分；δp_x,δp_y,δp_z分别表示惯性导航系统误差状态量中的X轴位置误差状态量、Y轴位置误差状态量和Z轴位置误差状态量；δv_x,δv_y,δv_z分别表示惯性导航系统误差状态量中的X轴速度误差状态量、Y轴速度误差状态量和Z轴速度误差状态量；δω_rx,δω_ry,δω_rz分别表示惯性导航系统误差状态量中的X轴陀螺随机游走误差增量状态量、Y轴陀螺随机游走误差增量状态量和Z轴陀螺随机游走误差增量状态量；δf_rx,δf_ry,δf_rz分别表示惯性导航系统误差状态量中的X轴加速度计随机游走误差增量状态量、Y轴加速度计随机游走误差增量状态量和Z轴加速度计随机游走误差增量状态量；上标T为转置；A(t)_15×15为系统的状态转移矩阵；G(t)_15×12为姿态噪声系数矩阵；W(t)_12×1为系统的白噪声矢量；

(3)根据各子系统不同工作特性，建立发射惯性系下各子系统的量测方程，SINS/GPS子系统量测为捷联惯性导航系统给出的三轴位置参数和GPS卫星接收机给出的相应信息的差值；SINS/CNS子系统量测为惯导给出的姿态信息和天文星敏感器给出的姿态差值；GPS和CNS的输出信息需要先转换成发射惯性系下的相关参数，再和惯导信息相减；

a、SINS/GPS子系统

定义观测矢量为：

$Z_G\left(t\right)=\left[\begin{array}{c}{x}_G-x_I\\ y_G-y_I\\ z_G-z_I\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}x\\ \mathrm{\δ}y\\ \mathrm{\δ}z\end{array}\right]+\left[\begin{array}{c}{p}_{\mathrm{\ϵ}x}\\ p_{\mathrm{\ϵ}y}\\ p_{\mathrm{\ϵ}z}\end{array}\right]=H_p\left(t\right)X\left(t\right)+p_{\mathrm{\ϵ}}---\left(2.7\right)$

式中：x_I和x_G分别表示惯导系统和GPS的X轴位置参数，y_I和y_G分别表示惯导系统和GPS的Y轴位置参数，z_I和z_G分别表示惯导系统和GPS的Z轴位置参数，p_ε为GPS量测白噪声，观测矩阵H_p(t)＝[0_3×3 eye(3) 0_3×9]_3×15，δx为X轴的位置误差量测值；δy为Y轴的位置误差量测值；δz为Z轴的位置误差量测值；p_εx为GPS量测误差在X轴的分量；p_εy为GPS量测误差在Y轴的分量；p_εz为GPS量测误差在Z轴的分量；

b、SINS/CNS子系统

定义子系统观测矢量为姿态四元数的矢量部分，如下：

Z_s(t)＝δq₁₃+q_η,13＝H_a(t)X(t)+N_S(t) (2.8)

式中：观测矩阵H_a(t)＝[I_3×3 0_3×3 0_3×3 0_3×6]_3×15，N_S(t)为量测噪声，δq₁₃为数学平台误差四元数的矢量部分，q_η,13为量测白噪声；

(4)将步骤(3)所述的各子系统量测方程中的子系统误差状态量进行卡尔曼滤波，并将子系统卡尔曼滤波结果送入联邦滤波模块；

(5)联邦主滤波模块对步骤(4)中子系统送来的滤波结果进行数据融合，输出全局最优估计值，从而对惯性导航系统的导航误差进行修正。

所述步骤(3)中GPS和CNS的输出信息需要先转换成发射惯性系下的相关参数，其转换过程如下：

对于GPS，位置转换为

$pos\_f=C_e^f\·pos\_e=C_i^f\·C_e^i\·pos\_e---\left(3.1\right)$

式中，pos_f为GPS在发射惯性系下的位置量测值，pos_e为GPS在地固系下的原始位置量测值，为地心惯性系到发射惯性系的姿态矩阵，为地固系到地心惯性系的姿态矩阵，为地固系到发射惯性系的姿态矩阵；

对CNS，姿态转换为

$C_f^b=C_s^b\·C_i^s\·C_f^i---\left(3.2\right)$

式中，为地心惯性系到星敏测量系的姿态矩阵，根据星敏感器的输出求得，为星敏测量系到本体系的姿态矩阵，为发射惯性系到地心惯性系的姿态矩阵，根据公式(3.1)求得的求出星敏感器在发射惯性系下的姿态量测值。

本发明的有益效果如下：

(1)整个组合导航处理机内部核心方法是建立在发射惯性系下SINS/GPS/CNS多组合导航方法，能直接获得导弹在发射惯性系下的高精度导航参数，有效地提高整个导航系统的工作性能。

(2)采用了分布式半物理仿真验证架构，系统中各个传感器模块在逻辑上属于并列关系，多信息融合方法采用相对独立处理的联邦滤波模块结构实现，硬件平台之间通过高速串行接口实现，这样从软件和硬件两个方面都有利于系统未来的扩展。

(3)导航处理机硬件的研制采用基于PC104总线的嵌入式计算机系统，具有强大的数据处理能力和丰富的外围接口，而且处理机体积小，重量轻，便于携带与安装，非常适合工程应用。

附图说明：

图1为本发明的组合导航方法原理图。

图2为本发明的系统硬件模块化构成图。

图3为本发明系统验证时采用的导弹航迹。

图4(a)为纯捷联惯导模式下的姿态误差曲线对比图,图4(b)为SINS/GPS组合模式下的姿态误差曲线对比图,图4(c)为SINS/CNS组合模式下的姿态误差曲线对比图,图4(d)为SINS/GPS/CNS组合模式下的姿态误差曲线对比图。

图5(a)为纯捷联惯导模式下的位置误差曲线对比图,图5(b)为SINS/GPS组合模式下的位置误差曲线对比图,图5(c)为SINS/CNS组合模式下的位置误差曲线对比图,图5(d)为SINS/GPS/CNS组合模式下的位置误差曲线对比图。

图6(a)为纯捷联惯导模式下的速度误差曲线对比图,图6(b)为SINS/GPS组合模式下的速度误差曲线对比图,图6(c)为SINS/CNS组合模式下的速度误差曲线对比图,图6(d)为SINS/GPS/CNS组合模式下的速度误差曲线对比图。

具体实施方式

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

如图1所示，本发明的导航方法原理是：直接建立导弹在发射惯性系下的组合导航系统模型，研制基于联邦滤波的发射惯性系下的SINS/GPS/CNS组合导航方法。具体实施方法如下：

1、发射惯性系下的SINS/GPS/CNS组合导航系统建模：

(1)组合导航系统惯导误差状态量方程建模

对于SINS/GPS/CNS多组合导航系统，其发射惯性系下的捷联惯导解算过程如下，速度计算公式为：

$C_b^f{f}_b=\stackrel{\·}{v}-C_i^{f}g---\left(2.1\right)$

式中，v为发射惯性系下的速度值，为v对时间的导数，g为地球的万有引力，为发射惯性系相对于本体系的姿态矩阵，为发射惯性系相对于地心惯性系的姿态矩阵，f_b为加速度真值；

位置计算公式为：

$\stackrel{\·}{p}=v---\left(2.2\right)$

式中，p为发射惯性系下的位置值，为p对时间的导数，v为发射惯性系下的速度值；

姿态运动有四元数描述，计算公式为：

式中，q为姿态四元数，为q对时间的导数，ω_b为为角速度真值；陀螺和加速度计的测量模型如下：

$\left\{\begin{array}{c}{\mathrm{\ω}}_c={\mathrm{\ω}}_b+{\mathrm{\ω}}_r+{\mathrm{\ω}}_{\mathrm{\ϵ}}\\ f_c=f_b+f_r+f_{\mathrm{\ϵ}}\\ {\stackrel{\·}{\mathrm{\ω}}}_r={\mathrm{\ω}}_n\\ {\stackrel{\·}{f}}_r=f_n\end{array}\right.---\left(2.4\right)$

式中，ω_b为角速度真值，ω_r为陀螺随机游走误差，ω_ε为陀螺测量噪声，f_r为加速度计随机游走误差，f_ε为加速度计测量噪声，ω_n为陀螺随机游走驱动噪声，f_n为加速度计随机游走驱动噪声，f_c为加速度的实际量测值，ω_c为角速度的实际量测值，为ω_r对时间的导数，为f_r对时间的导数；

组合导航系统为非线性对象，采用扩展卡尔曼滤波模块进行数值计算，根据公式(2.1)、(2.2)、(2.3)和(2.4)建立发射惯性系下的误差增量方程如下：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{q}}_{13}=-\[\widehat{\mathrm{\ω}}\×\]\·{\mathrm{\δq}}_{13}-0.5\·{\mathrm{\δ\ω}}_r-0.5\·{\mathrm{\ω}}_{\mathrm{\ϵ}}\\ \mathrm{\δ}\stackrel{\·}{p}=\mathrm{\δ}\mathrm{\υ}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\υ}}=2C_b^f\·\[{\hat{f}}_b\×\]\·{\mathrm{\δq}}_{13}-C_b^f\·{\mathrm{\δf}}_r-C_b^f\·f_{\mathrm{\ϵ}}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ω}}}_r={\mathrm{\ω}}_n\\ \mathrm{\δ}{\stackrel{\·}{f}}_r=f_n\end{array}\right.---\left(2.5\right)$

式中，δq₁₃为数学平台误差的四元数矢量部分，δp为位置误差状态量，δv为速度误差状态量，为δq₁₃对时间的导数，为δp对时间的导数，为δv对时间的导数；表示陀螺角速率的反对称矩阵；为加速度的反对称矩阵， 为角速度真值在X轴的分量；ω_by为角速度真值在Y轴的分量；ω_bz为角速度真值在Z轴的分量；f_bx为加速度真值在X轴的分量；f_by为加速度真值在Y轴的分量；f_bz为加速度真值在Z轴的分量，δω_r为陀螺随机游走误差增量，为δω_r对时间的导数，δf_r为加速度计随机游走误差增量，为δf_r对时间的导数。

基于上述分析，可以获得SINS/GPS/CNS多组合导航系统状态方程为：

$\stackrel{\·}{X}{\left(t\right)}_{15\×1}=A{\left(t\right)}_{15\×15}X\left(t\right)+G{\left(t\right)}_{15\×12}W{\left(t\right)}_{12\×1}---\left(2.6\right)$

其中，状态变量为：

X＝[δq₁ δq₂ δq₃ δp_x δp_y δp_z δv_x δv_y δv_z δω_rx δω_ry δω_rz δf_rx δf_ry δf_rz]^T

δq₁,δq₂,δq₃表示惯性导航系统误差状态量中数学平台误差的四元数矢量部分；δp_x,δp_y,δp_z分别表示惯性导航系统误差状态量中的X轴位置误差状态量、Y轴位置误差状态量和Z轴位置误差状态量；δv_x,δv_y,δv_z分别表示惯性导航系统误差状态量中的X轴速度误差状态量、Y轴速度误差状态量和Z轴速度误差状态量；δω_rx,δω_ry,δω_rz分别表示惯性导航系统误差状态量中的X轴陀螺随机游走误差增量状态量、Y轴陀螺随机游走误差增量状态量和Z轴陀螺随机游走误差增量状态量；δf_rx,δf_ry,δf_rz分别表示惯性导航系统误差增量状态量中的X轴加速度计随机游走误差状态量、Y轴加速度计随机游走误差增量状态量和Z轴加速度计随机游走误差增量状态量；上标T为转置；A(t)_15×15为系统的状态转移矩阵；G(t)_15×12为姿态噪声系数矩阵；W(t)_12×1为系统的白噪声矢量；

(2)组合导航系统量测方程建模

采用发射惯性系下位置、速度、姿态线性化观测原理，根据各子系统不同工作特性，建立发射惯性系下各子系统的量测方程，SINS/GPS子系统量测为捷联惯性导航系统给出的三轴位置参数和GPS卫星接收机给出的相应信息的差值；SINS/GSINS子系统量测为惯导给出的姿态信息和天文星敏感器给出的姿态差值。

a、SINS/GPS子系统

定义观测矢量为：

$Z_G\left(t\right)=\left[\begin{array}{c}{x}_G-x_I\\ y_G-y_I\\ z_G-z_I\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}x\\ \mathrm{\δ}y\\ \mathrm{\δ}z\end{array}\right]+\left[\begin{array}{c}{p}_{\mathrm{\ϵ}x}\\ p_{\mathrm{\ϵ}y}\\ p_{\mathrm{\ϵ}z}\end{array}\right]=H_p\left(t\right)X\left(t\right)+p_{\mathrm{\ϵ}}---\left(2.7\right)$

式中：x_I和x_G分别表示惯导系统和GPS的X轴位置参数，y_I和y_G分别表示惯导系统和GPS的Y轴位置参数，z_I和z_G分别表示惯导系统和GPS的Z轴位置参数，p_ε为GPS量测白噪声，观测矩阵H_p(t)＝[0_3×3 eye(3) 0_3×9]_3×15，δx为X轴的位置误差量测值；δy为Y轴的位置误差量测值；δz为Z轴的位置误差量测值；p_εx为GPS量测误差在X轴的分量；p_εy为GPS量测误差在Y轴的分量；p_εz为GPS量测误差在Z轴的分量。

b、SINS/CNS子系统

定义子系统观测矢量为姿态四元数的矢量部分，如下：

Z_s(t)＝δq₁₃+q_η,13＝H_a(t)X(t)+N_S(t) (2.8)

式中：观测矩阵H_a(t)＝[I_3×3 0_3×3 0_3×3 0_3×6]_3×15，N_S(t)为量测噪声，δq₁₃为数学平台误差四元数的矢量部分，q_η,13为量测白噪声。

2、GPS和星敏感器的参数输出转换

GPS和星敏感器的量测输出都要转换成发射惯性系下的相关参数，转换过程如下：

对GPS，位置转换为

$pos\_f=C_e^f\·pos\_e=C_i^f\·C_e^i\·pos\_e---\left(3.1\right)$

式中，pos_f为GPS在发射惯性系下的位置量测值，pos_e为GPS在地固系下的原始位置量测值，为地心惯性系到发射惯性系的姿态矩阵，为地固系到地心惯性系的姿态矩阵，为地固系到发射惯性系的姿态矩阵。

对CNS，姿态转换为

$C_f^b=C_s^b\·C_i^s\·C_f^i---\left(3.2\right)$

式中，为地心惯性系到星敏测量系的姿态矩阵，根据星敏感器的输出求得，为星敏测量系到本体系的姿态矩阵，为发射惯性系到地心惯性系的姿态矩阵。根据公式(2.1)求得的可以求出星敏感器在发射惯性系下的姿态量测值。

3、SINS/GPS/CNS组合导航系统联邦滤波模块模型建立

结合对导航精度和容错性能的综合考虑，发射惯性系下的SINS/GPS/CNS多组合导航系统考虑采用有重置的联邦滤波模块模型，并平均分配信息，如图1中所示，其中P_i(i＝1,2)分别表示第i个子滤波模块的估计值和方差阵，和P_f分别表示全局估计值和方差阵。

各子滤波模块进行卡尔曼滤波，过程如下：

${\hat{X}}_{i,k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{i,k-1/k-1}$

$P_{i,k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{i,k-1/k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k/k-1}{Q}_{i,k-1}{\mathrm{\Γ}}_{k/k-1}^T$

$K_{i,k}=P_{i,k/k-1}{H}_{i,k}^T{(H_{i,k}{P}_{i,k/k-1}{H}_{i,k}^T+R_{i,k})}^{-1}$

${\hat{X}}_{i,k/k}={\hat{X}}_{i,k/k-1}+K_{i,k}(Z_{i,k}-H_{i,k}{\hat{X}}_{i,k/k-1})$

$P_{i,k/k}=(I-K_{i,k}{H}_{i,k})P_{i,k/k-1}{(I-K_{i,k}{H}_{i,k})}^T+K_{i,k}{R}_{i,k}{K}_{i,k}^T$

上式中，表示第i个子滤波模块t_k-1时刻的状态对t_k时刻的状态的最优估计值，又称一步预测估值，Φ_k/k-1表示t_k-1时刻至t_k时刻系统的状态转移矩阵，表示第i个子滤波模块t_k-1时刻的系统状态估计值，K_i,k表示第i个子滤波模块的增益矩阵，H_i,k表示第i个子滤波模块t_k时刻的观测系数矩阵，P_i,k/k-1表示第i个子滤波模块的最优预测估计误差协方差阵，P_i,k/k表示第i个子滤波模块t_k时刻的系统误差协方差阵，Q_i,k-1表示第i个子滤波模块t_k-1时刻的噪声方差矩阵，Γ_k/k-1表示t_k-1时刻的噪声矢量对t_k时刻状态矢量影响的噪声系数矩阵，R_i,k表示第i个子滤波模块t_k时刻的量测方差矩阵，I为单位矩阵。

主滤波模块进行全局最优融合，以得到高精度导航参数，方法如下：

$\left\{\begin{array}{c}{\hat{X}}_f=P_f(P_1^{-1}{\hat{X}}_1+P_2^{-1}{\hat{X}}_2)\\ P_f=P_1^{-1}+P_2^{-1}\end{array}\right.$

式中，为全局估计值，为第一个子滤波模块的估计值，为第二个子滤波模块的估计值，P_f为全局估计误差方差阵，P₁为第一个子滤波模块估计误差方差阵，P₂为第二个子滤波模块估计误差方差阵。

为解决各子滤波模块估计相关问题，联邦滤波采用信息分配的方法使得各子滤波模块估计状态可以按照不相关的方式处理，方法如下：

$\left\{\begin{array}{c}{\hat{X}}_i={\hat{X}}_f\\ P_i={\mathrm{\β}}_i^{-1}{P}_f\\ P_m^{-1}+\underset{i=1}{\overset{n}{\Σ}}{P}_i^{-1}=P_f^{-1}\end{array}\right.$

式中，为全局估计值，为第i个子滤波模块的估计值，P_f为全局估计误差方差阵，P_i为第i个子滤波模块估计误差方差阵，P_m为主滤波模块估计误差方差阵，β_i为第i个子滤波模块的信息分配系数。

图2为本发明的系统模块化硬件构成图。整个验证系统硬件分为传感器模块、导航计算机、接口模块、电源模块和显示模块。导航计算机内部软件模块包括捷联解算模块、量测转换模块和联邦滤波模块，其中捷联解算模块包括四元数方程求解模块、位置解算模块和速度解算模块，导航计算机硬件研制采用PC104嵌入式主板，主要对多个子系统的仿真数据或实物数据进行滤波处理，并将导航结果传输到显示模块显示出来；接口模块采用八串口扩展板，与仿真子系统和导航计算机相连，向仿真子系统发送同步指令，并接受其仿真数据，再把数据传送给导航计算机；电源模块采用PSI104模块，与PC104相配套，给导航计算机和八串口扩展板供电；传感器模块包括惯性传感器、卫星导航接收机和星敏感器，采用通用PC机，主要负责将模拟生成的三个传感器数据传送给八串口扩展板或导航计算机；显示器模块采用通用PC机显示屏。

图3为验证时所采用的导弹飞行轨迹曲线，图4-图6是系统仿真误差曲线图，仿真中共4种工作模式：纯捷联惯导、SINS/GPS组合、SINS/CNS组合和SINS/GPS/CNS三组合。从仿真误差曲线图可以看出，SINS/CNS组合导航系统能直接修正姿态误差，提高姿态精度；SINS/GPS组合导航，通过对位置误差直接观测和对速度误差间接观测修正，使位置和速度误差大大减小，保证弹着点的导航精度要求；最终的SINS/GPS/CNS三组合导航系统能同时修正姿态、位置和速度误差，大大提高导航系统整体精度。结果表明，本发明提出的发射惯性系下的SINS/GPS/CNS多组合导航系统能大大提高系统整体导航精度，同时也表明整个导航系统的软硬件实现符合要求，设计合理，便于实际应用。

Claims (2)

1.一种采用发射惯性系下的多组合导航处理装置的导航方法，该方法所采用的多组合导航处理装置包括传感器模块、导航处理装置和显示模块，其中传感器模块包括惯性传感器、卫星导航接收机和星敏感器，导航处理装置包括接口模块、导航计算机、电源模块；惯性传感器、卫星导航接收机和星敏感器分别与接口模块连接，接口模块与电源模块连接，接口模块与导航计算机连接，导航计算机与显示模块连接；其特征在于，该导航方法包括以下步骤：

(1)对于SINS/GPS/CNS多组合导航系统，其发射惯性系下的捷联惯导解算过程如下，速度计算公式为：

$C_b^f{f}_b=\stackrel{\·}{v}-C_i^{f}g---\left(2.1\right)$

式中，v为发射惯性系下的速度值，为v对时间的导数，g为地球的万有引力，为发射惯性系相对于本体系的姿态矩阵，为发射惯性系相对于地心惯性系的姿态矩阵，f_b为加速度真值；

位置计算公式为：

$\stackrel{\·}{p}=v---\left(2.2\right)$

式中，p为发射惯性系下的位置值，为p对时间的导数；

姿态运动有四元数描述，计算公式为：

式中，q为姿态四元数，为q对时间的导数，ω_b为为角速度真值；

陀螺和加速度计的测量模型如下：

$\left\{\begin{array}{c}{\mathrm{\ω}}_c={\mathrm{\ω}}_b+{\mathrm{\ω}}_r+{\mathrm{\ω}}_{\mathrm{\ϵ}}\\ f_c=f_b+f_r+f_{\mathrm{\ϵ}}\\ {\stackrel{\·}{\mathrm{\ω}}}_r={\mathrm{\ω}}_n\\ {\stackrel{\·}{f}}_r=f_n\end{array}\right.---\left(2.4\right)$

式中，ω_b为角速度真值，ω_r为陀螺随机游走误差，ω_ε为陀螺测量噪声，f_r为加速度计随机游走误差，f_ε为加速度计测量噪声，ω_n为陀螺随机游走驱动噪声，f_n为加速度计随机游走驱动噪声，f_c为加速度的实际量测值，ω_c为角速度的实际量测值，为ω_r对时间的导数，为f_r对时间的导数；

组合导航系统为非线性对象，采用扩展卡尔曼滤波进行数值计算，根据公式(2.1)、(2.2)、(2.3)和(2.4)建立发射惯性系下的误差增量方程如下：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{q}}_{13}=-\[{\widehat{\mathrm{\ω}}}_b\×\]\·{\mathrm{\δq}}_{13}-0.5\·{\mathrm{\δ\ω}}_r-0.5\·{\mathrm{\ω}}_{\mathrm{\ϵ}}\\ \mathrm{\δ}\stackrel{\·}{p}=\mathrm{\δ}\mathrm{\υ}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\υ}}=2C_b^f\·\[\hat{f}\×\]\·{\mathrm{\δq}}_{13}-C_b^f\·{\mathrm{\δf}}_r-C_b^f\·f_{\mathrm{\ϵ}}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ω}}}_r={\mathrm{\ω}}_n\\ \mathrm{\δ}{\stackrel{\·}{f}}_r=f_n\end{array}\right.---\left(2.5\right)$

式中，δq₁₃为数学平台误差的四元数矢量部分，δp为位置误差状态量，δv为速度误差状态量，为δq₁₃对时间的导数，为δp对时间的导数，为δv对时间的导数；表示陀螺角速率的反对称矩阵；为加速度的反对称矩阵， ω_bx为角速度真值在X轴的分量；ω_by为角速度真值在Y轴的分量；ω_bz为角速度真值在Z轴的分量；f_bx为加速度真值在X轴的分量；f_by为加速度真值在Y轴的分量；f_bz为加速度真值在Z轴的分量，δω_r为陀螺随机游走误差增量，为δω_r对时间的导数，δf_r为加速度计随机游走误差增量，为δf_r对时间的导数；

(2)基于上述，获得SINS/GPS/CNS多组合导航系统状态方程为：

$\stackrel{\·}{X}{\left(t\right)}_{15\×1}=A{\left(t\right)}_{15\×15}X\left(t\right)+G{\left(t\right)}_{15\×12}W{\left(t\right)}_{12\×1}---\left(2.6\right)$

其中，状态变量为：

X＝[δq₁ δq₂ δq₃ δp_x δp_y δp_z δv_x δv_y δv_z δω_rx δω_ry δω_rz δf_rx δf_ry δf_rz]^T

δq₁,δq₂,δq₃表示惯性导航系统误差状态量中数学平台误差的四元数矢量部分；δp_x,δp_y,δp_z分别表示惯性导航系统误差状态量中的X轴位置误差状态量、Y轴位置误差状态量和Z轴位置误差状态量；δv_x,δv_y,δv_z分别表示惯性导航系统误差状态量中的X轴速度误差状态量、Y轴速度误差状态量和Z轴速度误差状态量；δω_rx,δω_ry,δω_rz分别表示惯性导航系统误差状态量中的X轴陀螺随机游走误差增量状态量、Y轴陀螺随机游走误差增量状态量和Z轴陀螺随机游走误差增量状态量；δf_rx,δf_ry,δf_rz分别表示惯性导航系统误差状态量中的X轴加速度计随机游走误差增量状态量、Y轴加速度计随机游走误差增量状态量和Z轴加速度计随机游走误差增量状态量；上标T为转置；A(t)_15×15为系统的状态转移矩阵；G(t)_15×12为姿态噪声系数矩阵；W(t)_12×1为系统的白噪声矢量；

(3)根据各子系统不同工作特性，建立发射惯性系下各子系统的量测方程，SINS/GPS子系统量测为捷联惯性导航系统给出的三轴位置参数和GPS卫星接收机给出的相应信息的差值；SINS/CNS子系统量测为惯导给出的姿态信息和天文星敏感器给出的姿态差值；GPS和CNS的输出信息需要先转换成发射惯性系下的相关参数，再和惯导信息相减；

a、SINS/GPS子系统

定义观测矢量为：

$Z_G\left(t\right)=\left[\begin{array}{c}{x}_G-x_I\\ y_G-y_I\\ z_G-z_I\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}x\\ \mathrm{\δ}y\\ \mathrm{\δ}z\end{array}\right]+\left[\begin{array}{c}{p}_{\mathrm{\ϵ}x}\\ p_{\mathrm{\ϵ}y}\\ p_{\mathrm{\ϵ}z}\end{array}\right]=H_p\left(t\right)X\left(t\right)+p_{\mathrm{\ϵ}}---\left(2.7\right)$

式中：x_I和x_G分别表示惯导系统和GPS的X轴位置参数，y_I和y_G分别表示惯导系统和GPS的Y轴位置参数，z_I和z_G分别表示惯导系统和GPS的Z轴位置参数，p_ε为GPS量测白噪声，观测矩阵H_p(t)＝[0_3×3 eye(3) 0_3×9]_3×15，δx为X轴的位置误差量测值；δy为Y轴的位置误差量测值；δz为Z轴的位置误差量测值；p_εx为GPS量测误差在X轴的分量；p_εy为GPS量测误差在Y轴的分量；p_εz为GPS量测误差在Z轴的分量；

b、SINS/CNS子系统

定义子系统观测矢量为姿态四元数的矢量部分，如下：

Z_s(t)＝δq₁₃+q_η,13＝H_a(t)X(t)+N_S(t) (2.8)

式中：观测矩阵H_a(t)＝[I_3×3 0_3×3 0_3×3 0_3×6]_3×15，N_S(t)为量测噪声，δq₁₃为数学平台误差四元数的矢量部分，q_η,13为量测白噪声；

(4)将步骤(3)所述的各子系统量测方程中的子系统误差状态量进行卡尔曼滤波，并将子系统卡尔曼滤波结果送入联邦滤波模块；

(5)联邦主滤波模块对步骤(4)中子系统送来的滤波结果进行数据融合，输出全局最优估计值，从而对惯性导航系统的导航误差进行修正。

2.根据权利要求1中所述的采用发射惯性系下的多组合导航处理装置的导航方法，其特征在于所述步骤(3)中GPS和CNS的输出信息需要先转换成发射惯性系下的相关参数，其转换过程如下：

对于GPS，位置转换为

$pos\_f=C_e^f\·pos\_e=C_i^f\·C_e^i\·pos\_e---\left(3.1\right)$

式中，pos_f为GPS在发射惯性系下的位置量测值，pos_e为GPS在地固系下的原始位置量测值，为地心惯性系到发射惯性系的姿态矩阵，为地固系到地心惯性系的姿态矩阵，为地固系到发射惯性系的姿态矩阵；

对CNS，姿态转换为

$C_f^b=C_s^b\·C_i^s\·C_f^i---\left(3.2\right)$

式中，为地心惯性系到星敏测量系的姿态矩阵，根据星敏感器的输出求得，为星敏测量系到本体系的姿态矩阵，为发射惯性系到地心惯性系的姿态矩阵，根据公式(3.1)求得的求出星敏感器在发射惯性系下的姿态量测值。