CN102519470A - 多级嵌入式组合导航系统及导航方法 - Google Patents

Abstract

本发明公布了一种多级嵌入式组合导航系统及导航方法，属于组合导航技术领域。该方法包括以下步骤：首先建立了航空机载惯性导航系统惯导误差状态量方程和惯性/卫星组合量测模型，随后定义了嵌入式惯性/卫星组合系统的导航误差状态量并建立惯性/星敏感器组合量测模型；在此基础上，进行了嵌入式惯性/卫星组合系统内部组合导航解算及闭环滤波修正，最后进行外部惯性/星敏感器组合导航解算及开环滤波修正，获得多信息融合导航结果。本方法能够在保留嵌入式惯性/卫星组合系统完整独立集成结构的同时，实现惯性/卫星/天文异步多传感器信息的多级融合和对惯导的分步校正，有效提高综合导航性能，适合工程应用。

Description

多级嵌入式组合导航系统及导航方法

技术领域

本发明公布了一种多级嵌入式组合导航系统及导航方法，属于组合导航技术领域。

背景技术

近年来，由惯性导航系统、卫星导航接收机和卡尔曼滤波计算单元构成的嵌入式惯性/卫星组合导航系统工程化产品已日渐成熟，并在机载和弹载等环境下得到了广泛应用。惯性/卫星组合导航系统根据卫星导航接收机的位置、速度测量信息，通过滤波算法对惯导误差进行在线估计和校正，能够获得较高的位置和速度测量精度；在缺乏对载体姿态直接测量的情况下，惯性/卫星组合导航系统只能通过间接观测修正陀螺漂移，输出的姿态精度还较为有限。

随着新型飞行器的研制，对导航系统性能要求的日益提高，特别是对于执行遥感、测绘及侦查任务等的飞行器，对其姿态测量精度均提出了较高的要求，需要进一步提高嵌入式惯性/卫星组合系统的姿态测量精度。

在目前已有的姿态测量设备中，星敏感器在测量精度方面具有突出的优势。它通过探测天球上不同位置的恒星，提供飞行器的三轴姿态，目前星敏感器的指向精度和姿态测量精度最高可达到角秒的量级，同时能够克服惯性陀螺姿态测量精度随时间漂移的缺点。

因此，研究在既有的嵌入式惯性/卫星组合系统基础上，基于星敏感器进行姿态量测的多级嵌入式组合导航系统及导航方法，将能够有效地提高姿态测量精度，将具有突出的应用价值。

发明内容

技术问题：

嵌入式惯性/卫星组合系统根据卫星导航的位置、速度量测信息，通过滤波算法对惯导误差进行在线估计和校正，能够获得较高的位置和速度测量精度；在缺乏对载体姿态直接测量的情况下，嵌入式惯性/卫星组合系统只能通过间接观测修正平台误差角和陀螺漂移，输出的姿态精度还较为有限。在组合导航系统中采用星敏感器进行姿态量测，将能够有效地提高姿态测量精度，但同时嵌入式惯性/卫星组合系统内已存在独立工作的组合滤波和惯导闭环校正回路，需要维持其完整性。

本发明目的在于克服嵌入式惯性/卫星组合系统姿态精度低的不足，提供了以嵌入式惯性/卫星组合系统为基础，外加星敏感器实现多级嵌入式组合导航的方法。

技术方案：

本发明多级嵌入式组合导航系统及方法，为达到上述发明目的，通过以下方案实现：

本发明所述多级嵌入式组合导航系统，其特征在于由内部传感器部分、内部组合导航解算部分、外部传感器部分、外部组合导航解算部分构成；所述内部传感器部分由惯性测量单元和卫星导航接收机组成；所述内部组合导航解算部分由惯性导航解算模块、误差补偿A模块、状态预测A模块、状态估计A模块组成；所述外部传感器部分由星敏感器组成；所述外部组合导航解算部分由协方差转换模块、状态估计B模块、误差补偿B模块组成；惯性/卫星/天文多级嵌入式组合导航系统中，惯性测量单元的输出端接误差补偿A模块的输入端，卫星导航接收机的输出端接状态估计A模块的输入端；误差补偿A模块的输出端接惯性导航解算模块的输入端，惯性导航解算模块的输出端接状态估计A模块、状态预测A模块、状态估计B模块和误差补偿B模块的输入端，状态预测A模块的输出端接状态估计A模块和协方差转化模块的输入端，状态估计A模块的输出端接误差补偿A模块和协方差转换模块的输入端；协方差转换模块的输出端接状态估计B模块的输入端、状态估计B模块的输出端接误差补偿B模块的输入端，误差补偿B模块的输出端输出导航结果。

本发明所述多级嵌入式组合导航方法，其特征在于包括以下步骤：

(1)选取东北天地理坐标系，定义航空机载惯性导航系统惯导误差状态量为：

$X_1={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\φ}}_{E1},& {\mathrm{\φ}}_{N1},& {\mathrm{\φ}}_{U1},& {\mathrm{\δv}}_{E1},& {\mathrm{\δv}}_{N1},& {\mathrm{\δv}}_{U1},& {\mathrm{\δL}}_1,& {\mathrm{\δ\λ}}_1,& {\mathrm{\δh}}_1,& {\mathrm{\ϵ}}_{\mathrm{bx}1},& {\mathrm{\ϵ}}_{\mathrm{by}1},& {\mathrm{\ϵ}}_{\mathrm{bz}1},& {\mathrm{\ϵ}}_{\mathrm{rx}1},& {\mathrm{\ϵ}}_{\mathrm{ry}1},& {\mathrm{\ϵ}}_{\mathrm{rz}1},& {\▿}_{x1},& {\▿}_{y1},& {\▿}_{z1}\end{array}\right]}^T$

φ_E，φ_N，φ_U分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E1，δv_N1，δv_U1分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL₁，δλ₁，δh₁分别表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx1，ε_by1，ε_bz1，ε_rx1，ε_ry1，ε_rz1分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，^T为转置；

建立航空机载惯性导航系统惯导误差状态方程，如式(15)所示

X₁(k)＝Φ₁(k，k-1)X₁(k-1)+T₁(k，k-1)W₁(k-1)           (1)

其中，Φ₁(k，k-1)为t_k-1时刻至t_k时刻系统的状态转移矩阵，W₁(k-1)为t_k时刻系统的噪声矢量，Γ₁(k，k-1)为t_k-1时刻至t_k时刻系统的噪声驱动矩阵；X₁(k-1)为t_k-1时刻航空机载惯性导航系统误差状态量，X₁(k)为t_k时刻航空机载惯性导航系统误差状态量；

(2)采用地理系下位置和速度线性化观测原理，建立地理系下卫星导航位置速度观测量和被估计的步骤(1)所述的惯性导航系统误差状态量之间的线性化量测方程，作为状态估计A模块的惯性/卫星组合量测模型；

状态估计A模块的量测值为惯导系统输出的位置、速度和卫星导航接收机输出的位置、速度的差值，可以建立如下形式的状态估计A模块的量测方程

Z₁(k)＝H₁(k)X₁(k)+V_G(k)            (2)

其中 $Z_1\left(k\right)=\left[\begin{array}{c}{v}_I\left(k\right)\\ p_I\left(k\right)\end{array}\right]-\left[\begin{array}{c}{v}_G\left(k\right)\\ p_G\left(k\right)\end{array}\right],$ v_I(k)为t_k时刻惯导输出的飞行器速度矢量，p_I(k)为t_k时刻惯导输出的飞行器位置矢量，v_G(k)为t_k时刻卫星导航接收机输出的飞行器速度矢量，p_G(k)为t_k时刻卫星导航接收机输出的飞行器位置矢量；

H₁(k)为t_k时刻速度位置量测矩阵；

$V_G\left(k\right)=\left[\begin{array}{c}{V}_v\left(k\right)\\ V_p\left(k\right)\end{array}\right],$ V_v(k)为t_k时刻卫星导航接收机的速度量测噪声向量，V_p(k)为t_k时刻卫星导航接收机的位置量测噪声向量；

(3)选取东北天地理坐标系，定义内部组合导航解算部分的惯性/卫星组合导航误差状态量X₂

X₂＝[φ_E2，φ_N2，φ_U2，ε_bx2，ε_by2，ε_bz2，ε_rx2，ε_ry2，ε_rz2]^T

φ_E2，φ_N2，φ_U2分别表示内部组合导航解算部分误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；ε_bx2，ε_by2，ε_bz2，ε_rx2，ε_ry2，ε_rz2分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

(4)采用地理系下姿态线性化观测原理，建立地理系下星敏感器姿态观测量和被估计的步骤(3)所述的内部组合导航解算部分误差状态量的之间的线性化量测方程，作为状态估计B模块惯性/星敏感器组合量测模型；

根据星敏感器姿态量测信息与惯性导航系统误差状态量之间的关系，建立状态估计B模块的量测方程

Z₂(k)＝H₂(k)X₂(k)+V_s(k)                 (3)

其中Z₂(k)＝a_I(k)-a_S(k)

a_I(k)为惯导输出的飞行器速度矢量，a_S(k)为星敏感器输出的飞行器位置矢量，H₂(k)为姿态量测矩阵，V_S(k)为星敏感器的姿态量测噪声向量，X₂(k)为t_k时刻内部组合导航解算部分的惯性/卫星组合导航误差状态量；

(5)内部组合导航解算部分使用步骤(1)的惯性导航误差状态量方程和步骤(2)的惯性/卫星组合滤波模型进行惯性/卫星组合滤波，输出惯性/卫星组合导航结果及协方差信息到外部组合导航解算部分；

(6)外部组合导航解算部分接收步骤(5)的惯性/卫星组合导航结果及协方差信息，外部组合导航解算部分使用步骤(4)的惯性/星敏感器组合滤波模型进行惯性/星敏感器组合滤波，输出导航结果；

有益效果：

本发明能够在保留嵌入式惯性/卫星组合系统完整独立集成结构的同时，实现外加星敏感器构成惯性/卫星/天文多级嵌入式组合导航系统，和未采用本发明的嵌入式惯性/卫星组合系统相比，采用本发明的姿态误差可以显著减小，本发明可以获得高精度的姿态、速度和位置信息，适合工程应用。

附图说明

图1为本发明的惯性/卫星/天文多级嵌入式组合导航系统结构图；

图2为仿真飞行航迹图；

图3惯性/卫星组合导航和本发明的惯性/卫星/天文多级嵌入式组合导航横滚角误差曲线对比图；

图4惯性/卫星组合导航和本发明的惯性/卫星/天文多级嵌入式组合导航俯仰角误差曲线对比图；

图5惯性/卫星组合导航和本发明的惯性/卫星/天文多级嵌入式组合导航航向角误差曲线对比图；

图6为本发明的惯性/卫星/天文多级嵌入式组合导航方法的工作流程图。

具体实施方式

下面结合附图对发明的技术方案进行详细说明：

如图1所示，本发明所述惯性/卫星/天文多级嵌入式组合导航系统，其特征在于由内部传感器部分、内部组合导航解算部分、外部传感器部分、外部组合导航解算部分构成。

所述内部传感器部分由惯性测量单元和卫星导航接收机组成；所述内部组合导航解算部分由惯性导航解算模块、误差补偿A模块、状态预测A模块、状态估计A模块组成；所述外部传感器部分由星敏感器组成；所述外部组合导航解算部分由协方差转换模块、状态估计B模块、误差补偿B模块组成；

惯性/卫星/天文多级嵌入式组合导航系统中，惯性测量单元的输出端接误差补偿A模块的输入端，卫星导航接收机的输出端接状态估计A模块的输入端；误差补偿A模块的输出端接惯性导航解算模块的输入端，惯性导航解算模块的输出端接状态估计A模块、状态预测A模块、状态估计B模块和误差补偿B模块的输入端，状态预测A模块的输出端接状态估计A模块和协方差转化模块的输入端，状态估计A模块的输出端接误差补偿A模块和协方差转换模块的输入端；协方差转换模块的输出端接状态估计B模块的输入端、状态估计B模块的输出端接误差补偿B模块的输入端，误差补偿B模块的输出端输出导航结果。

本发明所述惯性/卫星/天文多级嵌入式组合导航方法的原理是：从机载地理系导航的角度入手，分析了嵌入式惯性/卫星组合系统的工作流程和特点，在嵌入式惯性/卫星组合系统为既有的集成系统，其内部的内层卡尔曼滤波器独立完成惯导和卫星导航信息的组合，并在此基础上对惯导进行闭环反馈校正，获得嵌入式惯性/卫星组合系统的导航输出。在此基础上，嵌入式惯性/卫星组合系统的外部构建的外层滤波器与内层滤波器协同工作，进一步融合天文姿态敏感器的量测信息，并通过开环校正的方式修正嵌入式惯性/卫星组合系统导航输出中的姿态误差，获得多信息融合导航结果，获得高精度的姿态、速度和位置信息。

以下结合附图具体叙述本发明实施方法的过程：

1.建立航空机载惯性导航系统惯导误差状态量方程

选取东北天地理坐标系，定义航空机载惯性导航系统惯导误差状态量为：

$X_1={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\φ}}_{E1},& {\mathrm{\φ}}_{N1},& {\mathrm{\φ}}_{U1},& {\mathrm{\δv}}_{E1},& {\mathrm{\δv}}_{N1},& {\mathrm{\δv}}_{U1},& {\mathrm{\δL}}_1,& {\mathrm{\δ\λ}}_1,& {\mathrm{\δh}}_1,& {\mathrm{\ϵ}}_{\mathrm{bx}1},& {\mathrm{\ϵ}}_{\mathrm{by}1},& {\mathrm{\ϵ}}_{\mathrm{bz}1},& {\mathrm{\ϵ}}_{\mathrm{rx}1},& {\mathrm{\ϵ}}_{\mathrm{ry}1},& {\mathrm{\ϵ}}_{\mathrm{rz}1},& {\▿}_{x1},& {\▿}_{y1},& {\▿}_{z1}\end{array}\right]}^T$

φ_E，φ_N，φ_U分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E1，δv_N1，δv_U1分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL₁，δλ₁，δh₁分别表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx1，ε_by1，ε_bz1，ε_rx1，ε_ry1，ε_rz1分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，^T为转置；

建立航空机载惯性导航系统惯导误差状态方程，如式(18)所示

X₁(k)＝Φ₁(k，k-1)X₁(k-1)+Γ₁(k，k-1)W₁(k-1)             (4)

其中，Φ₁(k，k-1)为t_k-1时刻至t_k时刻系统的状态转移矩阵，W₁(k-1)为t_k时刻系统的噪声矢量，Γ₁(k，k-1)为t_k-1时刻至t_k时刻系统的噪声驱动矩阵；X₁(k-1)为t_k-1时刻航空机载惯性导航系统误差状态量，X₁(k)为t_k时刻航空机载惯性导航系统误差状态量；

2.建立惯性/卫星组合量测模型

采用地理系下位置和速度线性化观测原理，建立地理系下卫星导航位置速度观测量和被估计惯性导航系统误差状态量之间的线性化量测方程，作为状态估计A模块的惯性/卫星组合量测模型；

状态估计A模块的量测值为惯导系统输出的位置、速度和卫星导航接收机输出的位置、速度的差值，可以建立如下形式的状态估计A模块的量测方程

Z₁(k)＝H₁(k)X₁(k)+V_G(k)                (5)

其中 $Z_1\left(k\right)=\left[\begin{array}{c}{v}_I\left(k\right)\\ p_I\left(k\right)\end{array}\right]-\left[\begin{array}{c}{v}_G\left(k\right)\\ p_G\left(k\right)\end{array}\right],$ v_I(k)为t_k时刻惯导输出的飞行器速度矢量，p_I(k)为t_k时刻惯导输出的飞行器位置矢量，v_G(k)为t_k时刻卫星导航接收机输出的飞行器速度矢量，p_G(k)为t_k时刻卫星导航接收机输出的飞行器位置矢量；

H₁(k)为t_k时刻速度位置量测矩阵；

$V_G\left(k\right)=\left[\begin{array}{c}{V}_v\left(k\right)\\ V_p\left(k\right)\end{array}\right],$ V_v(k)为t_k时刻卫星导航接收机的速度量测噪声向量，V_p(k)为t_k时刻卫星导航接收机的位置量测噪声向量；

3.定义嵌入式惯性/卫星组合系统的导航误差状态量

根据图1给出的惯性/卫星/天文多级嵌入式组合导航系统结构图，外部组合导航解算部分的状态估计，实质上是通过星敏感器的测量来估计嵌入式惯性/卫星组合系统导航结果中的误差状态量。因此，基于惯性/星敏感器组合可观测性的分析，取式(20)中的平台误差角和陀螺误差状态作为嵌入式惯性/卫星组合系统的导航误差状态量，定义内部组合导航解算部分的惯性/卫星组合导航误差状态量X₂

X₂＝[φ_E2，φ_N2，φ_U2，ε_bx2，ε_by2，ε_bz2，ε_rx2，ε_ry2，ε_rz2]^T    (6)

其中，φ_E2，φ_N2，φ_U2分别表示内部组合导航解算部分误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；ε_bx2，ε_by2，ε_bz2，ε_rx2，ε_ry2，ε_rz2分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

4.建立惯性/星敏感器组合量测模型

采用地理系下姿态线性化观测原理，建立地理系下星敏感器姿态观测量和被估计的内部组合导航解算部分误差状态量的之间的线性化量测方程，作为状态估计B模块惯性/星敏感器组合量测模型；

根据星敏感器姿态量测信息与惯性导航系统误差状态量之间的关系，建立状态估计B模块的量测方程

Z₂(k)＝H₂(k)X₂(k)+V_S(k)              (7)

其中Z₂(k)＝a_I(k)-a_S(k)

a_I(k)为惯导输出的飞行器速度矢量，a_S(k)为星敏感器输出的飞行器位置矢量，H₂(k)为姿态量测矩阵，V_S(k)为星敏感器的姿态量测噪声向量，X₂(k)为t_k时刻内部组合导航解算部分的惯性/卫星组合导航误差状态量；

5.进行嵌入式惯性/卫星组合系统内部组合导航解算及滤波修正

内部组合导航解算部分使用惯性导航误差状态量方程和惯性/卫星组合滤波模型进行惯性/卫星组合滤波，输出惯性/卫星组合导航结果及协方差信息到外部组合导航解算部分，具体按照以下子步骤进行：

5.1)惯导系统进行解算，解出惯导导航结果；

5.2)状态预测A模块按照式错误！未找到引用源。进行惯性/卫星组合滤波中惯导误差状态量和协方差信息的时间更新；

P₁(k，k-1)＝Φ₁(k，k-1)P₁(k-1，k-1)Φ₁(k，k-1)^T             (8)

            +Γ₁(k，k-1)Q₁(k)Γ₁(k，k-1)^T

其中P₁(k，k-1)为t_k-1时刻至t_k时刻惯性/卫星组合滤波一步预测协方差矩阵，P₁(k-1，k-1)为t_k-1时刻惯性/卫星组合滤波状态估计协方差矩阵，Q₁(k)为t_k时刻系统的噪声协方差矩阵；

5.3)判断是否有新的卫星导航测量信息：如果是，则执行子步骤5.4)；如果否，则按照式错误！未找到引用源。置协方差阵暂存值，并执行子步骤5.5)；

M＝P₁(k，k-1)                               (9)

5.4)状态估计A模块接收新的卫星导航测量信息，按照式(24)、式(25)、式(26)对子步骤5.2)部分的惯导误差状态量和协方差信息的量测更新，误差补偿A模块用状态估计A模块估计出的惯性导航系统的估计误差状态量X₁(k，k)对惯导进行闭环校正；

K₁(k)＝P₁(k，k-1)H₁(k)^T[H₁(k)P₁(k，k-1)H₁(k)^T+R₁(k)]^-1        (10)

X₁(k)＝K₁(k)Z₁(k)                                             (11)

P₁(k，k)＝[I-K₁(k)H₁(k)]P₁(k，k-1)                            (12)

置协方差阵暂存值

M＝P₁(k，k)                                                   (13)

其中K₁(k)为t_k时刻惯性/卫星组合滤波增益矩阵，H₁(k)为t_k时刻速度位置量测矩阵，R₁(k)为t_k时刻速度位置量测噪声协方差阵，P₁(k，k)为t_k时刻惯性/卫星组合滤波状态估计协方差矩阵，^-1为求逆；

误差补偿A模块用估计出的惯性导航的估计误差状态量X₁(k)对子步骤5.1)的惯性导航结果进行开环校正。

5.5)将惯导导航结果作为惯性/卫星组合系统导航结果输出到外部组合导航解算部分，将协方差信息M输出到协方差转换模块，返回子步骤5.1)；

信息M存储到协方差阵暂存单元，返回子步骤5.1)。

6.进行外部惯性/星敏感器组合导航解算及滤波修正

外部组合导航解算部分接收惯性/卫星组合导航结果及协方差信息，外部组合导航解算部分使用惯性/星敏感器组合滤波模型进行惯性/星敏感器组合滤波，输出导航结果，具体包括如下子步骤：

6.1)协方差转换模块接收惯性/卫星组合导航结果及协方差信息M，并将M按照维数关系分解为式(28)的形式

$M_{18\×18}=\left[\begin{array}{cccc}{S}_{\mathrm{\φ}3\×3}& L& S_{\mathrm{\ϵ\φ}3\×6}& \\ M& S_{p6\×6}& M& M\\ S_{\mathrm{\φ\ϵ}6\×3}& L& S_{\mathrm{\ϵ}6\×6}& \\ & L& & S_{\▿3\×3}\end{array}\right]---\left(14\right)$

其中_n×n代表矩阵的维数为n×n维；

协方差转换模块在式(14)的基础上，按照式(29)进行协方差转换，并输出到状态估计B模块

$N_{9\×9}=\left[\begin{array}{cc}{S}_{\mathrm{\φ}3\×3}& S_{\mathrm{\ϵ\φ}3\×6}\\ S_{\mathrm{\φ\ϵ}6\×3}& S_{\mathrm{\ϵ}6\×6}\end{array}\right]---\left(15\right)$

6.2)判断是否有新的星敏感器测量信息：如果是，则执行子步骤6.3)；如果否，则执行子步骤6.4)；

6.3)状态估计B模块接收新的星敏感器测量信息，按照式(30)、式(31)、式(32)对子步骤6.1)部分的协方差信息的量测更新；

协方差赋值

P₂(k)＝N                                               (16)

计算外层滤波增益矩阵

K₂(k)＝P₂(k)H₂(k)^T[H₂(k)P₂(k)H₂(k)^T+R₂(k)]^-1           (17)

计算惯性导航误差状态量估计值

X₂(k)＝K₂(k)Z₂(k)                                      (18)

其中P₂(k)为t_k时刻惯性/星敏感器组合滤波状态估计协方差矩阵，K₂(k)为t_k时刻惯性/星敏感器组合滤波增益矩阵，H₂(k)为t_k时刻姿态量测矩阵，R₂(k)为t_k时刻姿态量测噪声协方差阵；

误差补偿B模块用估计出的惯性/卫星组合导航的估计误差状态量X₂(k)对5.部分的惯性/卫星组合导航结果进行开环校正。

6.4)将步骤6.3)开环校正后的惯性/卫星组合导航结果作为多信息融合导航结果输出，返回子步骤6.1)。

为了验证发明所提出的惯性/卫星/天文多级嵌入式组合导航系统和方法的性能，本文分别对未采用本发明方法的惯性/卫星组合系统和本发明的系统方法进行了对比，图2为验证时采用的飞行航迹，两种情况下姿态误差曲线分别如图3～图5所示。

通过图3～图5的仿真结果可以看出，本发明能够有效提高基于嵌入式惯性/卫星组合系统的姿态精度。同时本发明在保留嵌入式惯性/卫星组合系统完整独立集成结构和独立工作流程的同时，实现星敏感器对导航姿态的辅助校正，提高导航精度，具有有益的工程应用价值。

本发明的惯性/卫星/天文多级嵌入式组合导航系统和方法工作流程如图6所示。

Claims (4)

1.一种多级嵌入式组合导航系统，其特征在于由内部传感器部分、内部组合导航解算部分、外部传感器部分、外部组合导航解算部分构成；所述内部传感器部分由惯性测量单元和卫星导航接收机组成；所述内部组合导航解算部分由惯性导航解算模块、误差补偿A模块、状态预测A模块、状态估计A模块组成；所述外部传感器部分由星敏感器组成；所述外部组合导航解算部分由协方差转换模块、状态估计B模块、误差补偿B模块组成；多级嵌入式组合导航系统中，惯性测量单元的输出端接误差补偿A模块的输入端，卫星导航接收机的输出端接状态估计A模块的输入端；误差补偿A模块的输出端接惯性导航解算模块的输入端，惯性导航解算模块的输出端接状态估计A模块、状态预测A模块、状态估计B模块和误差补偿B模块的输入端，状态预测A模块的输出端接状态估计A模块和协方差转化模块的输入端，状态估计A模块的输出端接误差补偿A模块和协方差转换模块的输入端；协方差转换模块的输出端接状态估计B模块的输入端、状态估计B模块的输出端接误差补偿B模块的输入端，误差补偿B模块的输出端输出导航结果。

2.一种多级嵌入式组合导航方法，其特征在于包括以下步骤：

(1)选取东北天地理坐标系，航空机载惯性导航系统惯导误差状态量为：

$X_1={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\φ}}_{E1},& {\mathrm{\φ}}_{N1},& {\mathrm{\φ}}_{U1},& {\mathrm{\δv}}_{E1},& {\mathrm{\δv}}_{N1},& {\mathrm{\δv}}_{U1},& {\mathrm{\δL}}_1,& {\mathrm{\δ\λ}}_1,& {\mathrm{\δh}}_1,& {\mathrm{\ϵ}}_{\mathrm{bx}1},& {\mathrm{\ϵ}}_{\mathrm{by}1},& {\mathrm{\ϵ}}_{\mathrm{bz}1},& {\mathrm{\ϵ}}_{\mathrm{rx}1},& {\mathrm{\ϵ}}_{\mathrm{ry}1},& {\mathrm{\ϵ}}_{\mathrm{rz}1},& {\▿}_{x1},& {\▿}_{y1},& {\▿}_{z1}\end{array}\right]}^T$

φ_E，φ_N，φ_U分别表示航空机载惯性导航系统误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；δv_E1，δv_N1，δv_U1分别表示航空机载惯性导航系统误差状态量中的东向速度误差状态量、北向速度误差状态量和天向速度误差状态量；δL₁，δλ₁，δh₁分别表示航空机载惯性导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx1，ε_by1，ε_bz1，ε_rx1，ε_ry1，ε_rz1分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴和Z轴方向加速度计零偏，^T为转置；

建立航空机载惯性导航系统惯导误差状态方程，如式(1)所示

X₁(k)＝Φ₁(k，k-1)X₁(k-1)+Γ₁(k，k-1)W₁(k-1)            (1)

其中，Φ₁(k，k-1)为t_k-1时刻至t_k时刻系统的状态转移矩阵，W₁(k-1)为t_k时刻系统的噪声矢量，Γ₁(k，k-1)为t_k-1时刻至t_k时刻系统的噪声驱动矩阵；X₁(k-1)为t_k-1时刻航空机载惯性导航系统误差状态量，X₁(k)为t_k时刻航空机载惯性导航系统误差状态量；

(2)采用地理系下位置和速度线性化观测原理，建立地理系下卫星导航位置速度观测量和被估计的步骤(1)所述的惯性导航系统误差状态量之间的线性化量测方程，作为状态估计A模块的惯性/卫星组合量测模型；

状态估计A模块的量测值为惯导系统输出的位置、速度和卫星导航接收机输出的位置、速度的差值，建立如下形式的状态估计A模块的量测方程：

Z₁(k)＝H₁(k)X₁(k)+V_G(k)                                    (2)

其中 $Z_1\left(k\right)=\left[\begin{array}{c}{v}_I\left(k\right)\\ p_I\left(k\right)\end{array}\right]-\left[\begin{array}{c}{v}_G\left(k\right)\\ p_G\left(k\right)\end{array}\right],$ v_I(k)为t_k时刻惯导输出的飞行器速度矢量，p_I(k)为t_k时刻惯导输出的飞行器位置矢量，v_G(k)为t_k时刻卫星导航接收机输出的飞行器速度矢量，p_G(k)为t_k时刻卫星导航接收机输出的飞行器位置矢量；

H₁(k)为t_k时刻速度位置量测矩阵；

$V_G\left(k\right)=\left[\begin{array}{c}{V}_v\left(k\right)\\ V_p\left(k\right)\end{array}\right],$ V_v(k)为t_k时刻卫星导航接收机的速度量测噪声向量，V_p(k)为t_k时刻卫星导航接收机的位置量测噪声向量；

(3)选取东北天地理坐标系，定义内部组合导航解算部分的惯性/卫星组合导航误差状态量X₂：

X₂＝[φ_E2，φ_N2，φ_U2，ε_bx2，ε_by2，ε_bz2，ε_rx2，ε_ry2，ε_rz2]^T

φ_E2，φ_N2，φ_U2分别表示内部组合导航解算部分误差状态量中的东向平台误差角状态量、北向平台误差角状态量和天向平台误差角状态量；ε_bx2，ε_by2，ε_bz2，ε_rx2，ε_ry2，ε_rz2分别表示航空机载惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量和X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂移误差状态量；

(4)采用地理系下姿态线性化观测原理，建立地理系下星敏感器姿态观测量和被估计的步骤(3)所述的内部组合导航解算部分误差状态量的之间的线性化量测方程，作为状态估计B模块惯性/星敏感器组合量测模型；

根据星敏感器姿态量测信息与惯性导航系统误差状态量之间的关系，建立状态估计B模块的量测方程：

Z₂(k)＝H₂(k)X₂(k)+V_S(k)                                     (3)

其中Z₂(k)＝a_I(k)-a_S(k)

a_I(k)为惯导输出的飞行器速度矢量，a_S(k)为星敏感器输出的飞行器位置矢量，H₂(k)为姿态量测矩阵，V_S(k)为星敏感器的姿态量测噪声向量，X₂(k)为t_k时刻内部组合导航解算部分的惯性/卫星组合导航误差状态量；

(5)内部组合导航解算部分使用步骤(1)的惯性导航误差状态量方程和步骤(2)的惯性/卫星组合滤波模型进行惯性/卫星组合滤波，输出惯性/卫星组合导航结果及协方差信息到外部组合导航解算部分；

(6)外部组合导航解算部分接收步骤(5)的惯性/卫星组合导航结果及协方差信息，外部组合导航解算部分使用步骤(4)的惯性/星敏感器组合滤波模型进行惯性/星敏感器组合滤波，输出导航结果。

3.根据权利要求2所述的多级嵌入式组合导航方法，其特征在于步骤(5)中内部组合导航解算部分的惯性/卫星组合滤波包含如下子步骤：

5.1)惯导系统进行解算，解出惯导导航结果；

5.2)状态预测A模块按照式(4)进行惯性/卫星组合滤波中惯导误差状态量和协方差信息的时间更新；

P₁(k，k-1)＝Φ₁(k，k-1)P₁(k-1，k-1)Φ₁(k，k-1)^T

                                                          (4)

            +Γ₁(k，k-1)Q₁(k)Γ₁(k，k-1)^T

其中P₁(k，k-1)为t_k-1时刻至t_k时刻惯性/卫星组合滤波一步预测协方差矩阵，P₁(k-1，k-1)为t_k-1时刻惯性/卫星组合滤波状态估计协方差矩阵，Q₁(k)为t_k时刻系统的噪声协方差矩阵；

5.3)判断是否有新的卫星导航测量信息：如果是，则执行子步骤5.4)；如果否，则按照式(5)置协方差阵暂存值，并执行子步骤5.5)；

M＝P₁(k，k-1)                                              (5)

5.4)状态估计A模块接收新的卫星导航测量信息，按照式(6)、式(7)、式(8)对子步骤5.2)部分的惯导误差状态量和协方差信息的量测更新，误差补偿A模块用状态估计A模块估计出的惯性导航系统的估计误差状态量X₁(k，k)对惯导进行闭环校正；

K₁(k)＝P₁(k，k-1)H₁(k)^T[H₁(k)P₁(k，k-1)H₁(k)^T+R₁(k)]^-1         (6)

X₁(k)＝K₁(k)Z₁(k)                                              (7)

P₁(k，k)＝[I-K₁(k)H₁(k)]P₁(k，k-1)                             (8)

置协方差阵暂存值

M＝P₁(k，k)                                                    (9)

其中K₁(k)为t_k时刻惯性/卫星组合滤波增益矩阵，H₁(k)为t_k时刻速度位置量测矩阵，R₁(k)为t_k时刻速度位置量测噪声协方差阵，P₁(k，k)为t_k时刻惯性/卫星组合滤波状态估计协方差矩阵，^-1为求逆；

误差补偿A模块用估计出的惯性导航的估计误差状态量X₁(k)对子步骤5.1)的惯性导航结果进行开环校正；

5.5)将惯导导航结果作为惯性/卫星组合系统导航结果输出到外部组合导航解算部分，将协方差信息M输出到协方差转换模块，返回子步骤5.1)。

4.根据权利要求2所述的多级嵌入式组合导航方法，其特征在于步骤(6)中外部组合导航解算部分的惯性/星敏感器组合滤波包含如下子步骤：

6.1)协方差转换模块接收子骤(4)中输出的惯性/卫星组合导航结果及协方差信息M，并将M按照维数关系分解为式(10)的形式

$M_{18\×18}=\left[\begin{array}{cccc}{S}_{\mathrm{\φ}3\×3}& L& S_{\mathrm{\ϵ\φ}3\×6}& \\ M& S_{p6\×6}& M& M\\ S_{\mathrm{\φ\ϵ}6\×3}& L& S_{\mathrm{\ϵ}6\×6}& \\ & L& & S_{\▿3\×3}\end{array}\right]---\left(10\right)$

其中_n×n代表矩阵的维数为n×n维；

协方差转换模块在式(10)的基础上，按照式(11)进行协方差转换，并输出到状态估计B模块

$N_{9\×9}=\left[\begin{array}{cc}{S}_{\mathrm{\φ}3\×3}& S_{\mathrm{\ϵ\φ}3\×6}\\ S_{\mathrm{\φ\ϵ}6\×3}& S_{\mathrm{\ϵ}6\×6}\end{array}\right]---\left(11\right)$

6.2)判断是否有新的星敏感器测量信息：如果是，则执行子步骤6.3)；如果否，则执行子步骤6.4)；

6.3)状态估计B模块接收新的星敏感器测量信息，按照式(12)、式(13)、式(14)对子步骤6.1)部分的协方差信息的量测更新；

协方差赋值

P₂(k)＝N                         (12)

计算外层滤波增益矩阵

K₂(k)＝P₂(k)H₂(k)^T[H₂(k)P₂(k)H₂(k)^T+R₂(k)]^-1               (13)

计算惯性导航误差状态量估计值

X₂(k)＝K₂(k)Z₂(k)                                          (14)

其中P₂(k)为t_k时刻惯性/星敏感器组合滤波状态估计协方差矩阵，K₂(k)为t_k时刻惯性/星敏感器组合滤波增益矩阵，H₂(k)为t_k时刻姿态量测矩阵，R₂(k)为t_k时刻姿态量测噪声协方差阵；

误差补偿B模块用估计出的惯性/卫星组合导航的估计误差状态量X₂(k)对子步骤6.1)的惯性/卫星组合导航结果进行开环校正；

6.4)将步骤6.3)开环校正后的惯性/卫星组合导航结果作为多信息融合导航结果输出，返回子步骤6.1)。

Images