CN103076015B - 一种基于全面最优校正的sins/cns组合导航系统及其导航方法 - Google Patents

Abstract

本发明提出一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法，属于组合导航技术领域。该组合导航系统包括天文导航子系统、惯性导航子系统和信息融合子系统；该导航方法包括：基于星光折射的解析天文定位、导航系统状态方程建立、导航系统量测方程的建立和基于卡尔曼滤波的组合导航系统信息融合。本发明利用星光折射间接敏感地平的基本原理和大视场星敏感器可以同时观测多颗恒星的特点，将星光折射间接敏感地平方法应用于不满足轨道动力学模型的飞行器，解决了天文导航系统高精度自主地平的问题。且本发明充分利用天文导航系统的位置和姿态信息，对SINS误差进行全面最优校正，显著地提高了组合导航精度。

Description

一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法

技术领域

本发明涉及一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法，属于组合导航技术领域。

背景技术

高空长航时飞行器由于飞行距离远，航行时间长，对导航精度提出了很高的要求。目前适合高空长航时飞行器的导航系统主要有捷联惯导系统（SINS）、GPS、天文导航系统（CNS）、地形匹配导航系统（TAN）和多普勒导航系统等。

捷联惯导系统是一种完全自主的导航技术，具有短时精度高、输出连续、抗干扰能力强、导航信息完整等优点。但是其导航误差随时间积累，难以长时间独立工作，需要与其他导航系统进行组合以提高导航性能。天文导航系统主要利用恒星来进行导航，隐蔽性好、自主性强，并且导航精度不受时间、距离长短的影响，能够提供高精度的姿态与位置信息；但是其输出不连续。由于SINS、CNS各有优缺点，将两者结合起来进行组合导航，可以实现优势互补。惯性/天文组合导航以惯性导航为主体，利用CNS量测信息构造观测量，采用卡尔曼滤波算法对惯导误差进行估计，进而获得高精度的导航信息。

目前，SINS/CNS组合导航由于自主性强、导航精度高，受到了越来越多的关注，并已获得了广泛的应用。SINS/CNS组合导航系统主要采用以下两种工作模式。

（1）简单组合模式。该组合模式是最简单的组合模式，SINS独立工作，提供全面的导航信息，CNS利用SINS的导航信息计算出飞行器的位置和姿态，直接对SINS的位置、姿态输出进行校正；

（2）基于陀螺仪漂移校正的组合模式。该模式下，SINS利用CNS高精度的姿态信息对姿态误差和陀螺仪漂移进行估计和补偿。

简单组合模式中，由于陀螺仪漂移与加速度计零偏无法被估计、校正，所以该模式定位精度较低。相对于简单组合模式，基于陀螺仪漂移校正的组合模式可以有效地校正陀螺仪漂移引起的导航误差，提高了组合导航系统的导航精度；但该组合模式无法消除加速度计零偏累积的导航误差，因此该组合导航方案的误差随时间缓慢发散。

提高CNS导航系统精度的关键是如何获得不随时间漂移的、高精度的地平信息，目前常用的提供地平信息的方法主要有惯导辅助提供地平法、直接敏感地平法和星光折射间接敏感地平法。惯导辅助提供地平法以惯导系统的平台坐标系作为地平信息，该方法简单、易于实现；但该方法提供的地平信息耦合了SINS误差，随着SINS误差的累积，将导致地平信息误差的发散。直接敏感地平法利用红外地平仪直接确定地心矢量方向，但是该方法得到的地平信息精度较低，且由于高度限制无法应用于高空长航时飞行器。星光折射间接敏感地平法是一种低成本、高精度的地平确定方法，该方法利用飞行器的轨道动力学模型、高精度的星敏感器和大气折射模型，精确敏感地平，进而实现精确定位。但是，传统的基于星光折射间接敏感地平的天文导航方法需要飞行器的轨道动力学模型，无法应用于高空长航时飞行器。

目前，随着星敏感器技术的发展，天文导航系统（CNS）可以提供高精度的姿态、位置信息，对SINS导航误差进行全面最优地校正。因此，采用基于全面最优校正的SINS/CNS组合导航方案可以满足高空长航时飞行器对导航性能的要求。

发明内容

针对现有技术中存在的问题，本发明提出一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法，根据星光折射间接敏感地平的基本原理，提出一种基于星光折射的解析天文定位方法，充分利用天文导航系统和惯性导航系统提供的位置、姿态信息，通过卡尔曼滤波方法进行信息融合，显著的提高了组合导航系统的可靠性。

本发明一种基于全面最优校正的SINS/CNS组合导航系统，包括天文导航子系统、惯性导航子系统和信息融合子系统；三者之间的关系是：天文导航子系统和惯性导航子系统为信息融合子系统提供位置、姿态信息，信息融合子系统为惯性导航子系统提供估计误差。

所述天文导航子系统，包括大视场星敏感器、大气折射模型、数字滤波器、天文定位单元和天文定姿单元；其间关系是：大视场星敏感器将观测到的星光折射角提供给大气折射模型；大气折射模型根据星光折射角计算折射恒星的视高度，并将其提供给数字滤波器；数字滤波器对视高度信息进行降噪处理，并将处理后的信息发送给天文定位单元；天文定位单元利用基于星光折射间接敏感地平的解析天文定位方法得到位置信息和地平信息；天文定姿单元利用大视场星敏感器提供的惯性姿态信息和天文定位单元提供的地平信息确定姿态信息。该大视场星敏感器是采用数学仿真可以同时观测多颗恒星，直接输出飞行器的惯性姿态信息和折射恒星的星光折射角；该大气折射模型是国际参考大气（CIRA）1986，利用该大气折射模型可以根据星光折射角计算出折射恒星的视高度；该数字滤波器是二阶数字低通滤波器；该天文定位单元是利用视高度信息计算飞行器的位置；该天文定姿单元是直接利用惯性姿态信息和地平信息计算飞行器相对于导航坐标系的姿态信息；

所述惯性导航子系统，包括惯性测量单元和SINS解算单元，其间关系是：SINS解算单元利用惯性测量元件的输入解算出飞行器的位置和姿态信息；并利用信息融合子系统提供的估计误差对SINS导航误差进行校正。该惯性测量单元是由三个加速度计和三个陀螺仪组成，可以测量飞行器的加速度和角速度；该SINS解算单元是SINS解算过程，利用飞行器的加速度和加速度信息计算飞行器的位置、速度、姿态等导航信息；

所述信息融合子系统，包括失准角计算单元和卡尔曼滤波器，其间关系是：失准角计算单元利用天文导航子系统和惯性导航子系统提供的姿态信息求得平台失准角，并提供给卡尔曼滤波器；卡尔曼滤波器以SINS误差方程为状态方程，以位置误差和平台失准角作为观测量进行卡尔曼滤波，得到平台失准角、位置误差和陀螺仪漂移误差的估计值。该失准角计算单元是根据惯性导航子系统和天文导航子系统姿态输出的误差角计算系统的失准角；该卡尔曼滤波器采用标准卡尔曼滤波算法，以SINS误差方程为状态方程，以位置误差和平台失准角作为观测量，对平台失准角、位置误差和陀螺仪漂移误差进行估计；

本发明一种基于全面最优校正的SINS/CNS组合导航方法，具体包括以下步骤：

步骤一：天文导航信息和惯性导航信息的计算

a、天文导航信息的计算

利用大视场星敏感器观测多颗导航恒星和折射恒星，可以得到星光折射角；大气折射模型利用星光折射角计算出视高度，利用数字滤波器对视高度进行预处理，得到的处理结果可用于天文定位单元；天文定位单元利用基于星光折射的解析天文定位方法实现天文定位，具体过程如下：

根据星光折射的几何原理，可以得到：

$\cos \mathrm{\α}=\sqrt{1-{\left(\frac{R_e+h_a}{r_s}\right)}^2}---\left(1\right)$

其中：α为折射后的星光矢量与飞行器位置矢量之间的夹角；R_e为地球半径；h_a为视高度；r_s为地心距，即飞行器到地心的距离。

当观测到n颗折射恒星时，根据夹角α的定义可得：

$\left\{\begin{array}{c}{\stackrel{\→}{u}}_1\·\stackrel{\→}{r}={\cos \mathrm{\α}}_1\\ {\stackrel{\→}{u}}_2\·\stackrel{\→}{r}=\cos {\mathrm{\α}}_2\\ \·\\ \·\\ \·\\ {\stackrel{\→}{u}}_n\·\stackrel{\→}{r}=\cos {\mathrm{\α}}_n\end{array}\right.---\left(2\right)$

其中：为第i颗折射恒星折射后的星光矢量，可由大视场星敏感器测得；地心单位矢量，即由地心指向飞行器的单位矢量；α_i为和的夹角。

将公式(1)代入方程组(2)，可得

$\left\{\begin{array}{c}{\stackrel{\→}{u}}_1\·\stackrel{\→}{r}=\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ {\stackrel{\→}{u}}_2\·\stackrel{\→}{r}=\sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ \·\\ \·\\ \·\\ {\stackrel{\→}{u}}_n\·\stackrel{\→}{r}=\sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right.---\left(3\right)$

其中：h_ai(i=1,2，...,n)为第i颗折射恒星的视高度。

方程组(3)中，地球半径R_e为已知量，折射后的星光矢量可以由大视场星敏感器获得，视高度h_ai可利用大视场星敏感器、大气折射模型和数字滤波器得到，因此该方程组实际上含有r_x,r_y,r_z和r_s四个未知数。这样，根据的约束条件，如果同时观测三颗或三颗以上的折射恒星，就可以确定地心距r_s和地心单位矢量进而求得飞行器的三维位置信息（经度、纬度和高度）。目前，随着大视场星敏感器技术的发展，同时观测多颗折射恒星变成了可能。当大视场星敏感器观测到n≥3颗折射恒星时，方程组(3)可以写成：

$U\·\stackrel{\·}{r}=Z---\left(4\right)$

其中：

$U=\left[\begin{array}{ccc}{u}_{x1}& u_{y1}& u_{z1}\\ u_{x2}& u_{y2}& u_{z2}\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ u_{\mathrm{xn}}& u_{\mathrm{yn}}& u_{\mathrm{zn}}\end{array}\right],$ $Z\left(r_s\right)=\left[\begin{array}{c}\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ \sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ \·\\ \·\\ \·\\ \sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right]$

利用最小二乘法求解公式(4)，可以得到地心单位矢量的表达式为：

$\stackrel{\→}{r}=B\·Z\left(r_s\right)---\left(5\right)$

式中，B=(U^TU)^-1U^T为矩阵U的广义逆矩阵。

由于所以根据公式(5)可以得到地心距r_s的一元方程为

$F\left(r_s\right)=Z{\left(r_s\right)}^T{B}^T\mathrm{BZ}\left(r_s\right)-1={\stackrel{\→}{r}}^T\stackrel{\→}{r}-1=0---\left(6\right)$

采用牛顿迭代法解算方程(6)，具体迭代步骤如下：

（1）选取一个初始的地心距r_s(0)；

（2）利用迭代公式计算出下一时刻的地心距；

迭代公式为：

$r_s(k+1)=r_s\left(k\right)-\frac{F\left[r_s\left(k\right)\right]}{A}---\left(7\right)$

其中：r_s(k)、r_s(k+1)分别为第k次和第k+1次的地心距估计值；A为F(r_s)对r_s的微分，即：

$A=\frac{\mathrm{dF}\left(r_s\right)}{{\mathrm{dr}}_s}={2Z}^T{B}^{T}B\frac{\∂Z}{{\∂r}_s}$

其中：为Z对地心距r_s的偏微分，即：

$\frac{\∂Z}{{\∂r}_s}=\left[\begin{array}{c}{M}_1\\ M_2\\ \·\\ \·\\ \·\\ M_n\end{array}\right]=\left[\begin{array}{c}\frac{{(h_{a1}+R_e)}^2}{r_s^3\sqrt{1+{((h_{a1}+R_e)/r_s)}^2}}\\ \frac{{(h_{a2}+R_e)}^2}{r_s^3\sqrt{1-{\left((h_{a2}+R_e)r_s\right)}^2}}\\ \·\\ \·\\ \·\\ \frac{{(h_{\mathrm{an}}+R_e)}^2}{r_s^3\sqrt{1-{((h_{\mathrm{an}}+R_e)/r_s)}^2}}\end{array}\right]$

（3）若|r_s(k+1)-r_s(k)|<τ（τ为给定的小量），则迭代结束，且r_s(k+1)为地心距r_s的数值解；否则，以r_s(k+1)作为新的初始条件返回第（2）步重新进行计算。

然后，根据牛顿迭代法解算出地心距r_s的数值解，并将其数值解代入公式(5)，即得到地心单位矢量

根据地心距的定义，由地心距r_s可以确定飞行器的高度h为：

h=r_s-R_e                      (8)

根据地心单位矢量的定义，其可以表示为：

$\stackrel{\&RightArrow;}{r}={[r_x,r_y,r_z]}^T={\left[\begin{array}{ccc}\cos {\mathrm{\&delta;}}_d\cos {\mathrm{\&alpha;}}_d& \cos {\mathrm{\&delta;}}_d\sin {\mathrm{\&alpha;}}_d& \sin {\mathrm{\&delta;}}_d\end{array}\right]}^T---\left(9\right)$

因此，根据公式(9)可以确定飞行器的赤经α_d、赤纬δ_d为

α_d=arctan(r_y/r_x)，δ_d=arcsin(r_z)           (10)其中：α_d∈(0~2π)，δ_d∈(-π/2~π/2)。

将惯性系下的坐标赤经、赤纬(α_d,δ_d)转变为地理系下的经、纬度坐标(λ，L)，即：

λ=α_d-t_G，L=δ_d                (11)其中：(λ，L)为飞行器的经、纬度；t_G为春分点的格林时角，可由时间基准得到。

此外，根据地心单位矢量的定义，还可以得到地平信息

$C_i^n=\left[\begin{array}{ccc}-\frac{r_y}{\sqrt{1-r_z^2}}& \frac{r_x}{\sqrt{1-r_z^2}}& 0\\ -\frac{r_x{r}_z}{\sqrt{1-r_z^2}}& -\frac{r_y{r}_z}{\sqrt{1-r_z^2}}& \sqrt{1-r_z^2}\\ r_x& r_y& r_z\end{array}\right]---\left(12\right)$

这样，天文定位单元可以输出天文位置信息和地平信息。

天文姿态信息可由天文定姿单元求得。天文定姿单元利用惯性姿态信息和地平信息，直接计算出天文姿态信息。

b、惯性导航信息的计算

惯性位置信息和惯性姿态信息可由SINS解算单元求出。惯性导航子系统利用惯性测量单元测量飞行器的加速度信息和角速度信息；SINS解算单元根据加速度信息和角速度信息解算出飞行器的位置信息和姿态信息。

步骤二：组合导航系统状态方程的建立

选择东北天地理坐标系作为导航坐标系，组合导航系统的状态方程为SINS的误差方程，可以表示为：

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+\mathrm{GW}---\left(13\right)$

其中：状态向量 $X={[{\mathrm{\&phi;}}_x,{\mathrm{\&phi;}}_y,{\mathrm{\&phi;}}_z,{\mathrm{\&delta;v}}_x,{\mathrm{\&delta;v}}_y,{\mathrm{\&delta;v}}_z,\mathrm{\&delta;L},\mathrm{\&delta;\&lambda;},\mathrm{\&delta;h},{\mathrm{\&epsiv;}}_{\mathrm{bx}},{\mathrm{\&epsiv;}}_{\mathrm{by}},{\mathrm{\&epsiv;}}_{\mathrm{bz}},{\&dtri;}_{\mathrm{bx}},{\&dtri;}_{\mathrm{by}},{\&dtri;}_{\mathrm{bz}}]}^T,$ 包括平台失准角φ_x,φ_y,φ_z、速度误差δv_x,δv_y,δv_z、位置误差δL,δλ，δh、陀螺仪漂移误差ε_bx,ε_by,ε_bz和加速度计零偏误差F为状态转移矩阵：

$F=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\&times;9}& 0_{6\&times;6}\end{array}\right],$ $F_S=\left[\begin{array}{cc}-C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\end{array}\right]$

F_N是平台失准角误差、速度误差和位置误差对应的状态转移矩阵；G为噪声驱动矩阵：

$G=\left[\begin{array}{cc}-C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right];$

为系统噪声向量，包括陀螺仪随机误差ω_gx,ω_gy,ω_gz和加速度计随机误差ω_dx,ω_dy,ω_dz。

步骤三：组合导航系统量测方程的建立

选取平台失准角误差和位置误差作为系统观测量，建立量测方程；

a、平台失准角误差的量测方程建立

平台失准角可由天文导航子系统和惯性导航子系统输出的姿态信息求得。令表示天文导航子系统和惯性导航子系统的姿态误差角，其可以定义为：

δθ=θ_s-θ_c

δ_γ=γ_s-γ_c

其中：θ_c,γ_c是天文导航子系统输出的姿态信息，θ_s,γ_s是惯性导航子系统输出的姿态信息。

失准角计算单元利用姿态误差角求得平台失准角φ_x,φ_y,φ_z为：

根据公式(14)、(15)，可以根据天文导航子系统和惯性导航子系统的姿态输出求的平台失准角。进而可得平台失准角对应的观测方程为：

Z₁=H₁X+V₁               (16)其中：Z₁=[φ_x φ_y φ_z]^T为平台失准角的观测量；H₁=[I_3×3 0_3×12]为平台失准角对应的观测矩阵；V₁为观测噪声。

b、位置误差的量测方程建立

将天文导航子系统与惯性导航子系统位置输出的差值作为位置误差的观测量，则位置误差对应的观测方程为：

Z₂=H₂X+V₂              (17)

其中：Z₂=[L_s-L_c λ_s-λ_c]^T为位置误差的观测量；L_c,λ_c为天文导航子系统输出的位置信息，L_s,λ_s为惯性导航子系统输出的位置信息；H₂=[0_2×6 I_2×2 0_2×7]为位置误差对应的观测矩阵；V₂为CNS的定位误差。

步骤四：基于卡尔曼滤波的组合导航系统信息融合

卡尔曼滤波器利用捷联惯导的误差方程作为状态方程，将天文导航子系统和惯性导航子系统位置输出的差值和失准角计算单元输出的平台失准角作为观测值，利用卡尔曼滤波算法对导航误差进行实时估计，并将估计误差发送到SINS解算单元，对导航误差进行校正，提高导航精度。

本发明的优点在于：

(1)本发明提出一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法，利用大视场根据星敏感器可同时观测多颗折射恒星的特点，将基于星光折射的间接敏感地平方法应用于不满足轨道动力学特性的高空长航时无人机，实现了基于星光折射的解析天文定位；

(2)本发明提出一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法，利用卡尔曼滤波算法对惯性导航系统和天文导航系统输出的姿态、位置信息进行信息融合，进而对导航误差进行全面最优地估计、校正，提高了组合导航系统的精度；

(3)本发明提出一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法，具有计算量小、可靠性高等优点，并且还可以应用于临近空间飞行器、空天往返飞行器、弹道导弹、变轨航天器等飞行器，具有广阔的应用前景。

附图说明

图1为本发明提出的一种基于全面最优校正的SINS/CNS组合导航系统结构示意图；

图2为本发明组合导航方法的流程框图

图3为本发明中基于星光折射的解析天文定位方法的原理图。

图1中：

1-天文导航子系统      2-惯性导航子系统      3-信息融合子系统

101-大视场星敏感器    201-惯性测量元件      301-失准角计算单元

102-大气折射模型      202-SINS解算单元      302-卡尔曼滤波器

103-数字滤波器        f^b-加速度信息         φ-失准角

104-天文定位单元      -角速度信息

105-天文定姿单元      λ_s,L_s-惯性位置信息

R-星光折射角          θ_s,γ_s-惯性姿态信息

-视高度观测值

-地平信息

-惯性姿态信息

λ_c,L_c-天文位置信息

θ_c,γ_c-天文姿态信息

图3中：

R-星光折射角             h_a-视高度               R_e-地球半径

                                                 -折射恒星折射后的星光

r_s-地心距                -地心单位矢量

                                                   失量

α-与的夹角           δ_d-赤纬                 α_d-赤经

-折射恒星折射后的星光  -折射恒星折射后的星光  -折射恒星折射后的星光

   矢量                    矢量                     矢量

α₁-与的夹角          α₂-与的夹角          α_n-与的夹角

具体实施方式

下面将结合附图对本发明作进一步的详细说明。

本发明提出一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法。根据SINS的误差方程建立系统状态方程；以天文位置信息（L_c,λ_c）、天文姿态信息（θ_c,γ_c）和惯性位置信息（L_s,λ_s）、惯性姿态信息（θ_s,γ_s）输出构造观测值；利用卡尔曼滤波算法估计、校正导航误差，提高组合导航系统的精度。

见图1，本发明提出一种基于全面最优校正的SINS/CNS组合导航系统，包括天文导航子系统1、惯性导航子系统2和信息融合子系统3；

天文导航子系统1包括大视场星敏感器101、大气折射模型102、数字滤波器103、天文定位单元104和天文定姿单元105；大视场星敏感器101将观测到的星光折射角（R）提供给大气折射模型102；星光折射模型102根据星光折射角计算折射恒星的视高度并将其提供给数字滤波器103；数字滤波器103对视高度信息进行降噪处理，并将处理后的信息发送给天文定位单元104；天文定位单元104利用基于星光折射间接敏感地平的解析天文定位方法得到位置信息（L_c,λ_c）和地平信息；天文定姿单元105利用大视场星敏感器101提供的惯性姿态信息和天文定位单元104提供的地平信息确定姿态信息（θ_c,γ_c）。

惯性导航子系统2包括惯性测量单元201和SINS解算单元202，SINS解算单元202利用惯性测量元件201的输入（f^b,）解算出飞行器的位置（L_s,λ_s）和姿态（θ_s,γ_s）信息；并利用信息融合子系统3提供的估计误差对SINS导航误差进行校正。

信息融合子系统3包括失准角计算单元301和卡尔曼滤波器302，失准角计算单元301利用天文导航子系统1和惯性导航子系统2提供的姿态信息求得平台失准角（φ），并提供给卡尔曼滤波器302；卡尔曼滤波器302以SINS误差方程为状态方程，以位置误差和平台失准角作为观测量进行卡尔曼滤波，得到平台失准角、位置误差和陀螺仪漂移误差的估计值。

见图2，本发明提出一种基于全面最优校正的SINS/CNS组合导航方法，具体包括以下步骤：

步骤一：天文导航信息和惯性导航信息的计算

a、天文导航信息的计算

利用大视场星敏感器101观测多颗导航恒星和折射恒星，可以得到星光折射角（R）；大气折射模型102利用星光折射角（R）计算出视高度利用数字滤波器103对视高度进行预处理，得到的处理结果可用于天文定位单元104；天文定位单元104利用基于星光折射的解析天文定位方法实现天文定位，具体过程如下：

如图3所示，根据星光折射的几何原理，可以得到：

$\cos \mathrm{\&alpha;}=\sqrt{1-{\left(\frac{R_e+h_a}{r_s}\right)}^2}---\left(1\right)$

其中：α为折射后的星光矢量与飞行器位置矢量之间的夹角；R_e为地球半径；h_a为视高度；r_s为地心距，即飞行器到地心的距离。

当观测到n颗折射恒星时，根据夹角α的定义可得：

$\left\{\begin{array}{c}{\stackrel{\&RightArrow;}{u}}_1\&CenterDot;\stackrel{\&RightArrow;}{r}={\cos \mathrm{\&alpha;}}_1\\ {\stackrel{\&RightArrow;}{u}}_2\&CenterDot;\stackrel{\&RightArrow;}{r}=\cos {\mathrm{\&alpha;}}_2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\stackrel{\&RightArrow;}{u}}_n\&CenterDot;\stackrel{\&RightArrow;}{r}=\cos {\mathrm{\&alpha;}}_n\end{array}\right.---\left(2\right)$

其中：为第i颗折射恒星折射后的星光矢量，可由大视场星敏感器101测得；地心单位矢量，即由地心指向飞行器的单位矢量；α_i为和的夹角。

将公式(1)代入方程组(2)，可得

$\left\{\begin{array}{c}{\stackrel{\&RightArrow;}{u}}_1\&CenterDot;\stackrel{\&RightArrow;}{r}=\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ {\stackrel{\&RightArrow;}{u}}_2\&CenterDot;\stackrel{\&RightArrow;}{r}=\sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\stackrel{\&RightArrow;}{u}}_n\&CenterDot;\stackrel{\&RightArrow;}{r}=\sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right.---\left(3\right)$

其中：h_ai(i＝1,2，...,n)为第i颗折射恒星的视高度。

方程组(3)中，地球半径R_e为已知量，折射后的星光矢量可以由大视场星敏感器101获得，视高度h_ai可利用大视场星敏感器101、大气折射模型102和数字滤波器103得到，因此该方程组实际上含有r_x,r_y,r_z和r_s四个未知数。这样，根据的约束条件，如果同时观测三颗或三颗以上的折射恒星，就可以确定地心距r_s和地心单位矢量进而求得飞行器的三维位置信息（经度、纬度和高度）。目前，随着大视场星敏感器技术的发展，同时观测多颗折射恒星变成了可能。当大视场星敏感器101观测到n≥3颗折射恒星时，方程组(3)可以写成：

$U\&CenterDot;\stackrel{\&CenterDot;}{r}=Z---\left(4\right)$

其中：

$U=\left[\begin{array}{ccc}{u}_{x1}& u_{y1}& u_{z1}\\ u_{x2}& u_{y2}& u_{z2}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ u_{\mathrm{xn}}& u_{\mathrm{yn}}& u_{\mathrm{zn}}\end{array}\right],$ $Z\left(r_s\right)=\left[\begin{array}{c}\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ \sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ \sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right]$

利用最小二乘法求解公式(4)，可以得到地心单位矢量的表达式为：

$\stackrel{\&RightArrow;}{r}=B\&CenterDot;Z\left(r_s\right)---\left(5\right)$

式中，B=(U^TU)^-1U^T为矩阵U的广义逆矩阵。

由于所以根据公式(5)可以得到地心距r_s的一元方程为

$F\left(r_s\right)=Z{\left(r_s\right)}^T{B}^T\mathrm{BZ}\left(r_s\right)-1={\stackrel{\&RightArrow;}{r}}^T\stackrel{\&RightArrow;}{r}-1=0---\left(6\right)$

采用牛顿迭代法解算方程(6)，具体迭代步骤如下：

（3）选取一个初始的地心距r_s(0)；

（4）利用迭代公式计算出下一时刻的地心距；

迭代公式为：

$r_s(k+1)=r_s\left(k\right)-\frac{F\left[r_s\left(k\right)\right]}{A}---\left(7\right)$

其中：r_s(k)、r_s(k+1)分别为第k次和第k+1次的地心距估计值；A为F(r_s)对r_s的微分，即：

$A=\frac{\mathrm{dF}\left(r_s\right)}{{\mathrm{dr}}_s}={2Z}^T{B}^{T}B\frac{\&PartialD;Z}{{\&PartialD;r}_s}$

其中：为Z对地心距r_s的偏微分，即：

$\frac{\&PartialD;Z}{{\&PartialD;r}_s}=\left[\begin{array}{c}{M}_1\\ M_2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ M_n\end{array}\right]=\left[\begin{array}{c}\frac{{(h_{a1}+R_e)}^2}{r_s^3\sqrt{1+{((h_{a1}+R_e)/r_s)}^2}}\\ \frac{{(h_{a2}+R_e)}^2}{r_s^3\sqrt{1-{\left((h_{a2}+R_e)r_s\right)}^2}}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ \frac{{(h_{\mathrm{an}}+R_e)}^2}{r_s^3\sqrt{1-{((h_{\mathrm{an}}+R_e)/r_s)}^2}}\end{array}\right]$

（3）若|r_s(k+1)-r_s(k)|<τ（τ为给定的小量），则迭代结束，且r_s(k+1)为地心距r_s的数值解；否则，以r_s(k+1)作为新的初始条件返回第（2）步重新进行计算。

然后，根据牛顿迭代法解算出地心距r_s的数值解，并将其数值解代入公式(5)，即得到地心单位矢量

根据地心距的定义，由地心距r_s可以确定飞行器的高度h为：

h=r_s-R_e       (8)

根据地心单位矢量的定义，其可以表示为：

$\stackrel{\&RightArrow;}{r}={[r_x,r_y,r_z]}^T={\left[\begin{array}{ccc}\cos {\mathrm{\&delta;}}_d\cos {\mathrm{\&alpha;}}_d& \cos {\mathrm{\&delta;}}_d\sin {\mathrm{\&alpha;}}_d& \sin {\mathrm{\&delta;}}_d\end{array}\right]}^T---\left(9\right)$

因此，根据公式(9)可以确定飞行器的赤经α_d、赤纬δ_d为

α_d=arctan(r_y/r_x)，δ_d=arcsin(r_z)        (10)其中：α_d∈(0~2π)，δ_d∈(-π/2~π/2)。

将惯性系下的坐标赤经、赤纬(α_d,δ_d)转变为地理系下的经、纬度坐标(λ，L)，即：

λ=α_d-t_G，L=δ_d           (11)

其中：(λ，L)为飞行器的经、纬度；t_G为春分点的格林时角，可由时间基准得到。

此外，根据地心单位矢量的定义，还可以得到地平信息

$C_i^n=\left[\begin{array}{ccc}-\frac{r_y}{\sqrt{1-r_z^2}}& \frac{r_x}{\sqrt{1-r_z^2}}& 0\\ -\frac{r_x{r}_z}{\sqrt{1-r_z^2}}& -\frac{r_y{r}_z}{\sqrt{1-r_z^2}}& \sqrt{1-r_z^2}\\ r_x& r_y& r_z\end{array}\right]---\left(12\right)$

这样，天文定位单元104可以输出天文位置信息（λ_c,L_c）和地平信息

天文姿态信息（θ_c,γ_c）可由天文定姿单元105求得。天文定姿单元105利用惯性姿态信息和地平信息直接计算进而求出天文姿态信息（θ_e,γ_c）。

b、惯性导航信息的计算

惯性位置信息（λ_s,L_s）和惯性姿态信息（θ_c,γ_c）可由SINS解算单元202求出。惯性导航子系统2利用惯性测量单元201测量飞行器的加速度信息（f^b）和角速度信息SINS解算单元202根据加速度信息（f^b）和角速度信息解算出飞行器的位置信息（λ_s,L_s）和姿态信息（θ_s,γ_s）。

步骤二：组合导航系统状态方程的建立

选择东北天地理坐标系作为导航坐标系，组合导航系统的状态方程为SINS的误差方程，可以表示为：

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+\mathrm{GW}---\left(13\right)$

其中：状态向量 $X={[{\mathrm{\&phi;}}_x,{\mathrm{\&phi;}}_y,{\mathrm{\&phi;}}_z,{\mathrm{\&delta;v}}_x,{\mathrm{\&delta;v}}_y,{\mathrm{\&delta;v}}_z,\mathrm{\&delta;L},\mathrm{\&delta;\&lambda;},\mathrm{\&delta;h},{\mathrm{\&epsiv;}}_{\mathrm{bx}},{\mathrm{\&epsiv;}}_{\mathrm{by}},{\mathrm{\&epsiv;}}_{\mathrm{bz}},{\&dtri;}_{\mathrm{bx}},{\&dtri;}_{\mathrm{by}},{\&dtri;}_{\mathrm{bz}}]}^T,$ 包括平台失准角φ_x,φ_y,φ_z、速度误差δv_x,δv_y,δv_z、位置误差δL,δλ，δh、陀螺仪漂移误差ε_bx,ε_by,ε_bz和加速度计零偏误差F为状态转移矩阵：

$F=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\&times;9}& 0_{6\&times;6}\end{array}\right],$ $F_S=\left[\begin{array}{cc}-C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\end{array}\right]$

F_N是平台失准角误差、速度误差和位置误差对应的状态转移矩阵；G为噪声驱动矩阵：

$G=\left[\begin{array}{cc}-C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right];$

为系统噪声向量，包括陀螺仪随机误差ω_gx,ω_gy,ω_gz和加速度计随机误差ω_dx,ω_dy,ω_dz。

步骤三：组合导航系统量测方程的建立

选取平台失准角误差和位置误差作为系统观测量，建立量测方程；

a、平台失准角误差的量测方程建立

平台失准角可由天文导航子系统1和惯性导航子系统2输出的姿态信息求得。令表示天文导航子系统1和惯性导航子系统2的姿态误差角，其可以定义为：

δθ=θ_s-θ_c

δγ=γ_s-γ_c

其中：θ_c,γ_c是天文导航子系统1输出的姿态信息，θ_s,γ_s是惯性导航子系统2输出的姿态信息。

失准角计算单元301利用姿态误差角求得平台失准角φ_x,φ_y,φ_z为：

根据公式(14)、(15)，可以根据天文导航子系统1和惯性导航子系统2的姿态输出求的平台失准角。进而可得平台失准角对应的观测方程为：

Z₁=H₁X+V₁          (16)其中：Z₁=[φ_x φ_y φ_z]^T为平台失准角的观测量；H₁=[I_3×3 0_3×12]为平台失准角对应的观测矩阵；V₁为观测噪声。

b、位置误差的量测方程建立

将天文导航子系统1与惯性导航子系统2位置输出的差值作为位置误差的观测量，则位置误差对应的观测方程为：

Z₂=H₂X+V₂         (17)

其中：Z₂=[L_s-L_c λ_s-λ_c]^T为位置误差的观测量；L_c,λ_c为天文导航子系统1输出的位置信息，L_s,λ_s为惯性导航子系统2输出的位置信息；H₂=[0_2×6 I_2×2 0_2×7]为位置误差对应的观测矩阵；V₂为CNS的定位误差。

步骤四：基于卡尔曼滤波的组合导航系统信息融合

卡尔曼滤波器302利用捷联惯导的误差方程作为状态方程，将天文导航子系统1和惯性导航子系统2位置输出的差值和失准角计算单元301输出的平台失准角作为观测值，利用卡尔曼滤波算法对导航误差进行实时估计，并将估计误差发送到SINS解算单元202，对导航误差进行校正，提高导航精度。

Claims (1)

1.一种基于全面最优校正的SINS/CNS组合导航方法，该方法的实施是利用基于全面最优校正的SINS/CNS组合导航系统，该系统包括天文导航子系统、惯性导航子系统和信息融合子系统；天文导航子系统和惯性导航子系统为信息融合子系统提供位置、姿态信息，信息融合子系统为惯性导航子系统提供估计误差；

所述天文导航子系统，包括大视场星敏感器、大气折射模型、数字滤波器、天文定位单元和天文定姿单元；大视场星敏感器将观测到的星光折射角提供给大气折射模型；大气折射模型根据星光折射角计算折射恒星的视高度，并将其提供给数字滤波器；数字滤波器对视高度信息进行降噪处理，并将处理后的信息发送给天文定位单元；天文定位单元利用基于星光折射间接敏感地平的解析天文定位方法得到位置信息和地平信息；天文定姿单元利用大视场星敏感器提供的惯性姿态信息和天文定位单元提供的地平信息确定姿态信息；该大视场星敏感器是采用数学仿真同时观测多颗恒星，直接输出飞行器的惯性姿态信息和折射恒星的星光折射角；该大气折射模型是国际参考大气CIRA1986，利用该大气折射模型根据星光折射角计算出折射恒星的视高度；该数字滤波器是二阶数字低通滤波器；该天文定位单元是利用视高度信息计算飞行器的位置；该天文定姿单元是直接利用惯性姿态信息和地平信息计算飞行器相对于导航坐标系的姿态信息；

所述惯性导航子系统，包括惯性测量单元和SINS解算单元；SINS解算单元利用惯性测量单元的输入解算出飞行器的位置和姿态信息；并利用信息融合子系统提供的估计误差对SINS导航误差进行校正；该惯性测量单元是由三个加速度计和三个陀螺仪组成，测量飞行器的加速度和角速度；该SINS解算单元是SINS解算过程，利用飞行器的加速度和加速度信息计算飞行器的位置、速度和姿态导航信息；

所述信息融合子系统，包括失准角计算单元和卡尔曼滤波器；失准角计算单元利用天文导航子系统和惯性导航子系统提供的姿态信息求得平台失准角，并提供给卡尔曼滤波器；卡尔曼滤波器以SINS误差方程为状态方程，以位置误差和平台失准角作为观测量进行卡尔曼滤波，得到平台失准角、位置误差和陀螺仪漂移误差的估计值；该失准角计算单元是根据惯性导航子系统和天文导航子系统姿态输出的误差角计算系统的失准角；该卡尔曼滤波器采用标准卡尔曼滤波算法，以SINS误差方程为状态方程，以位置误差和平台失准角作为观测量，对平台失准角、位置误差和陀螺仪漂移误差进行估计；

其特征在于：该方法具体包括以下步骤：

步骤一：天文导航信息和惯性导航信息的计算

a、天文导航信息的计算

利用大视场星敏感器观测多颗导航恒星和折射恒星，得到星光折射角；大气折射模型利用星光折射角计算出视高度，利用数字滤波器对视高度进行预处理，得到的处理结果用于天文定位单元；天文定位单元利用基于星光折射的解析天文定位方法实现天文定位，具体过程如下：

根据星光折射的几何原理，得到：

$\cos \mathrm{\&alpha;}=\sqrt{1-{\left(\frac{R_e+h_a}{r_s}\right)}^2}---\left(1\right)$

其中：α为折射后的星光矢量与飞行器位置矢量之间的夹角；R_e为地球半径；h_a为视高度；r_s为地心距，即飞行器到地心的距离；

当观测到n颗折射恒星时，根据夹角α的定义得：

$\left\{\begin{array}{c}{\stackrel{\&RightArrow;}{u}}_1\&CenterDot;\stackrel{\&RightArrow;}{r}=\cos {\mathrm{\&alpha;}}_1\\ {\stackrel{\&RightArrow;}{u}}_2\&CenterDot;\stackrel{\&RightArrow;}{r}=\cos {\mathrm{\&alpha;}}_2\\ .\\ .\\ .\\ {\stackrel{\&RightArrow;}{u}}_n\&CenterDot;\stackrel{\&RightArrow;}{r}=\cos {\mathrm{\&alpha;}}_n\end{array}\right.---\left(2\right)$

其中：(i＝1,2,...,n)为第i颗折射恒星折射后的星光矢量，由大视场星敏感器测得；地心单位矢量，即由地心指向飞行器的单位矢量；α_i为和的夹角；

将公式(1)代入方程组(2)，得

$\left\{\begin{array}{c}{\stackrel{\&RightArrow;}{u}}_1\&CenterDot;\stackrel{\&RightArrow;}{r}=\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ {\stackrel{\&RightArrow;}{u}}_2\&CenterDot;\stackrel{\&RightArrow;}{r}=\sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ .\\ .\\ .\\ {\stackrel{\&RightArrow;}{u}}_n\&CenterDot;\stackrel{\&RightArrow;}{r}=\sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right.---\left(3\right)$

其中：h_ai(i＝1,2,...,n)为第i颗折射恒星的视高度；

方程组(3)中，地球半径R_e为已知量，折射后的星光矢量由大视场星敏感器获得，视高度h_ai利用大视场星敏感器、大气折射模型和数字滤波器得到，因此该方程组实际上含有r_x,r_y,r_z和r_s四个未知数；这样，根据的约束条件，如果同时观测三颗以上的折射恒星，就可以确定地心距r_s和地心单位矢量进而求得飞行器的三维位置信息即经度、纬度和高度；当大视场星敏感器观测到n≥3颗折射恒星时，方程组(3)写成：

$U\&CenterDot;\stackrel{\&RightArrow;}{r}=Z---\left(4\right)$

其中：

$U=\left[\begin{array}{ccc}{u}_{x1}& u_{y1}& u_{z1}\\ u_{x2}& u_{y2}& u_{z2}\\ .& .& .\\ .& .& .\\ .& .& .\\ u_{\mathrm{xn}}& u_{\mathrm{yn}}& u_{\mathrm{zn}}\end{array}\right],Z\left(r_s\right)=\left[\begin{array}{c}\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ \sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ .\\ .\\ .\\ \sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right]$

利用最小二乘法求解公式(4)，得到地心单位矢量的表达式为：

$\stackrel{\&RightArrow;}{r}=B\&CenterDot;Z\left(r_s\right)---\left(5\right)$

式中，B＝(U^TU)^-1U^T为矩阵U的广义逆矩阵；

由于所以根据公式(5)得到地心距r_s的一元方程为

$F\left(r_s\right)=Z{\left(r_s\right)}^T{B}^T\mathrm{BZ}\left(r_s\right)-1={\stackrel{\&RightArrow;}{r}}^T\stackrel{\&RightArrow;}{r}-1=0---\left(6\right)$

采用牛顿迭代法解算方程(6)，具体迭代步骤如下：

(1)选取一个初始的地心距r_s(0)；

(2)利用迭代公式计算出下一时刻的地心距；

迭代公式为：

$r_s(k+1)=r_s\left(k\right)-\frac{F\left[r_s\left(k\right)\right]}{A}---\left(7\right)$

其中：r_s(k)、r_s(k+1)分别为第k次和第k+1次的地心距估计值；A为F(r_s)对r_s的微分，即：

$A=\frac{\mathrm{dF}\left(r_s\right)}{d{r}_s}=2Z^T{B}^{T}B\frac{\&PartialD;Z}{\&PartialD;r_s}$

其中：为Z对地心距r_s的偏微分，即：

$\frac{\&PartialD;Z}{\&PartialD;r_s}=\left[\begin{array}{c}{M}_1\\ M_2\\ .\\ .\\ .\\ M_n\end{array}\right]=\left[\begin{array}{c}\frac{{(h_{a1}+R_e)}^2}{r_s^3\sqrt{1-{((h_{a1}+R_e)/r_s)}^2}}\\ \frac{{(h_{a2}+R_e)}^2}{r_s^3\sqrt{1-{((h_{a2}+R_e)/r_s)}^2}}\\ .\\ .\\ .\\ \frac{{(h_{\mathrm{an}}+R_e)}^2}{r_s^3\sqrt{1-{((h_{\mathrm{an}}+R_e)/r_s)}^2}}\end{array}\right]$

(3)若|r_s(k+1)-r_s(k)|＜τ，τ为给定的小量，则迭代结束，且r_s(k+1)为地心距r_s的数值解；否则，以r_s(k+1)作为新的初始条件返回第(2)步重新进行计算；

然后，根据牛顿迭代法解算出地心距r_s的数值解，并将其数值解代入公式(5)，即得到地心单位矢量

根据地心距的定义，由地心距r_s确定飞行器的高度h为：

h＝r_s-R_e           (8)

根据地心单位矢量的定义，表示为：

$\stackrel{\&RightArrow;}{r}={[r_x,r_y,r_z]}^T={\left[\begin{array}{ccc}\cos {\mathrm{\&delta;}}_d\cos {\mathrm{\&alpha;}}_d& \cos {\mathrm{\&delta;}}_d\sin {\mathrm{\&alpha;}}_d& \sin {\mathrm{\&delta;}}_d\end{array}\right]}^T---\left(9\right)$

因此，根据公式(9)确定飞行器的赤经α_d、赤纬δ_d为

α_d＝arctan(r_y/r_x)，δ_d＝arcsin(r_z)       (10)

其中：α_d∈(0～2π)，δ_d∈(-π/2～π/2)；

将惯性系下的坐标赤经、赤纬(α_d,δ_d)转变为地理系下的经、纬度坐标(λ,L)，即：

λ＝α_d-t_G，L＝δ_d            (11)

其中：(λ,L)为飞行器的经、纬度；t_G为春分点的格林时角，由时间基准得到；

此外，根据地心单位矢量的定义，还得到地平信息

$C_i^n=\left[\begin{array}{ccc}-\frac{r_y}{\sqrt{1-r_z^2}}& \frac{r_x}{\sqrt{1-r_z^2}}& 0\\ -\frac{r_x{r}_z}{\sqrt{1-r_z^2}}& -\frac{r_y{r}_z}{\sqrt{1-r_z^2}}& \sqrt{1-r_z^2}\\ r_x& r_y& r_z\end{array}\right]---\left(12\right)$

这样，天文定位单元输出天文位置信息和地平信息；

天文姿态信息由天文定姿单元求得，天文定姿单元利用惯性姿态信息和地平信息，直接计算出天文姿态信息；

b、惯性导航信息的计算

惯性位置信息和惯性姿态信息由SINS解算单元求出，惯性导航子系统利用惯性测量单元测量飞行器的加速度信息和角速度信息；SINS解算单元根据加速度信息和角速度信息解算出飞行器的位置信息和姿态信息；

步骤二：组合导航系统状态方程的建立

选择东北天地理坐标系作为导航坐标系，组合导航系统的状态方程为SINS的误差方程，表示为：

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+\mathrm{GW}---\left(13\right)$

其中：状态向量X＝[φ_x,φ_y,φ_z,δv_x,δv_y,δv_z,δL,δλ,δh,ε_bx,ε_by,ε_bz,▽_bx,▽_by,▽_bz]^T，包括平台失准角φ_x,φ_y,φ_z、速度误差δv_x,δv_y,δv_z、位置误差δL,δλ,δh、陀螺仪漂移误差ε_bx,ε_by,ε_bz和加速度计零偏误差▽_bx,▽_by,▽_bz；F为状态转移矩阵：

$F=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\&times;9}& 0_{6\&times;6}\end{array}\right],F_S=\left[\begin{array}{cc}-C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\end{array}\right]$

F_N是平台失准角误差、速度误差和位置误差对应的状态转移矩阵；G为噪声驱动矩阵：

$G=\left[\begin{array}{cc}-C_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right];$

W＝[ω_gx,ω_gy,ω_gz,ω_dx,ω_dy,ω_dz]为系统噪声向量，包括陀螺仪随机误差ω_gx,ω_gy,ω_gz和加速度计随机误差ω_dx,ω_dy,ω_dz；

步骤三：组合导航系统量测方程的建立

选取平台失准角误差和位置误差作为系统观测量，建立量测方程；

a、平台失准角误差的量测方程建立

平台失准角由天文导航子系统和惯性导航子系统输出的姿态信息求得，令表示天文导航子系统和惯性导航子系统的姿态误差角，其定义为：

δθ＝θ_s-θ_c

δγ＝γ_s-γ_c

其中：是天文导航子系统输出的姿态信息，是惯性导航子系统输出的姿态信息；

失准角计算单元利用姿态误差角求得平台失准角φ_x,φ_y,φ_z为：

根据公式(14)、(15)，根据天文导航子系统和惯性导航子系统的姿态输出求的平台失准角，进而得平台失准角对应的观测方程为：

Z₁＝H₁X+V₁          (16)

其中：Z₁＝[φ_x φ_y φ_z]^T为平台失准角的观测量；H₁＝[I_3×3 0_3×12]为平台失准角对应的观测矩阵；V₁为观测噪声；

b、位置误差的量测方程建立

将天文导航子系统与惯性导航子系统位置输出的差值作为位置误差的观测量，则位置误差对应的观测方程为：

Z₂＝H₂X+V₂        (17)

其中：Z₂＝[L_s-L_c λ_s-λ_c]^T为位置误差的观测量；L_c,λ_c为天文导航子系统输出的位置信息，L_s,λ_s为惯性导航子系统输出的位置信息；H₂＝[0_2×6 I_2×2 0_2×7]为位置误差对应的观测矩阵；V₂为CNS的定位误差；

步骤四：基于卡尔曼滤波的组合导航系统信息融合

卡尔曼滤波器利用捷联惯导的误差方程作为状态方程，将天文导航子系统和惯性导航子系统位置输出的差值和失准角计算单元输出的平台失准角作为观测值，利用卡尔曼滤波算法对导航误差进行实时估计，并将估计误差发送到SINS解算单元，对导航误差进行校正，提高导航精度。