CN101706281B - 惯性/天文/卫星高精度组合导航系统及其导航方法 - Google Patents

Abstract

本发明提供一种惯性/天文/卫星高精度组合导航系统及其导航方法。本发明系统由传感器模块依次串接组合导航解算模块、实时显示模块组成，所述传感器模块包括惯性传感器、星敏感器和卫星接收机，所述组合导航解算模块包括四阶龙格库塔捷联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿算法模块，所述实时显示模块由三维实时显示模块构成。本发明方法实现传感器数据的实时采集；利用惯性传感器数据进行基于四阶龙格库塔算法的捷联导航解算；能够对惯性传感器数据、星敏感器数据和卫星接收机数据进行组合导航解算；对组合导航结果利用投影算法进行实时三维显示。

Description

惯性/天文/卫星高精度组合导航系统及其导航方法

技术领域

本发明涉及一种高精度的惯性/天文/卫星信息融合组合导航系统及导航方法，属组合导航领域，其中，“/”表示“和”的关系。

背景技术

导航系统的精度和稳定性是导航系统的首要指标。目前一般采用以惯性导航系统为核心，与卫星导航、天文导航等系统进行组合的组合导航技术，提高系统的精度和稳定性。

捷联惯性导航系统的精度是导航的关键。捷联算法不但要考虑到圆锥误差、划船效应等误差的影响，也要考虑捷联周期对导航精度和实时性的影响。高速高动态的情况对捷联解算的实时性即捷联解算周期提出了很高的要求。目前广泛使用的多子样算法，需要在一个捷联解算周期内多次采样陀螺输出来提高解算精度。因此，随着计算机能力的提高，捷联解算周期必然受限于惯性器件的输出速率。惯性器件的输出速率成为制约传统的捷联算法实时性的重要因素。因此需要提出一种快速的捷联导航算法，降低惯性器件输出速率对捷联解算周期的限制，提高导航的精度和实时性，以满足高速高动态情况下的导航需要。

捷联惯导系统能提供连续的导航信息，但误差随时间累积；星敏感器可以提供不随时间发散的姿态信息，但受环境影响较大；卫星导航能提供准确的不随时间发散的位置和速度信息，但易被电子信号干扰。因此，以惯性导航为核心的采用信息融合方式的组合导航技术得到了重视和迅速发展。目前的组合导航系统将卫星导航系统、天文导航系统等与惯导系统进行组合，取长补短，扩大系统的使用范围、提高导航系统的余度和容错能力，以充分满足现代对导航系统性能长时间稳定可靠工作的要求。

多传感器组合导航系统结构可以采用集中滤波结构，也可以采用联邦滤波结构。由于联邦滤波系统结构复杂，工程实现困难，因此，集中滤波结构在组合导航系统中被大量采用。由于传感器的输出速率各不相同，因此多传感器的输出不同步和量测滞后是集中滤波器必须解决的问题。但目前的惯性/天文/卫星组合导航系统中，对非等间隔输出特性的滤波研究较少。

发明内容

技术问题：

本发明的目的是针对现有技术存在的缺陷与不足，利用四阶龙格库塔算法改进传统的捷联算法，提高捷联导航系统的实时性和精度；设计非等间隔集中滤波器的结构，满足多传感器信息融合的需要；利用投影技术，直观显示三维航迹。

技术方案：为达到上述发明目的，通过以下步骤实现：

惯性/天文/卫星高精度组合导航系统，由传感器模块、组合导航解算模块、实时显示模块依次串接组成，其特征在于所述传感器模块包括惯性传感器、星敏感器和卫星接收机；所述组合导航解算模块包括四阶龙格库塔捷联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿模块；所述实时显示模块由三维实时显示模块构成；其中四阶龙格库塔捷联导航解算模块的输入端分别接惯性传感器的输出端和非等间隔卡尔曼滤波模块的输出端；位置和速度补偿模块的输入端分别接卫星接收机的输出端和四阶龙格库塔捷联导航解算模块的输出端；天文定姿解算模块的输入端分别接星敏感器的输出端和位置和速度补偿模块的输出端；非等间隔卡尔曼滤波模块的输入端分别接四阶龙格库塔捷联导航解算模块的输出端、位置和速度补偿算法模块的输出端和天文定姿解算模块的输出端；三维实时显示模块的输入端接四阶龙格库塔捷联导航解算模块的输出端。

所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述导航方法如下：

四阶龙格库塔捷联导航解算模块：利用惯性传感器输出的角速率和加速度信息进行基于四阶龙格库塔算法的捷联导航解算，并利用非等间隔卡尔曼滤波模块输出的反馈校正信息进行校正，输出导航速度、位置、姿态及加速度信息；

位置和速度补偿模块：利用四阶龙格库塔捷联导航解算模块输出的导航速度和加速度信息计算卫星速度和位置的补偿量，再结合卫星接收机输出的卫星速度和位置信息输出补偿后的卫星速度和位置信息；

天文定姿解算模块：利用星敏传感器输出的信息以及所述补偿后的卫星位置信息解算得到天文导航系统的姿态信息；

非等间隔卡尔曼滤波模块：将四阶龙格库塔捷联导航解算模块输出的导航速度、位置和姿态信息，补偿后的卫星速度和位置信息以及天文导航系统的姿态信息进行卡尔曼滤波得到反馈校正信息；

三维实时显示模块：将方法所述导航位置信息实时进行三维图形显示。

所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述四阶龙格库塔捷联导航解算模块的解算方法如下：

选取“东北天”地理坐标系作为导航坐标系；

速度的微分方程为：

$\stackrel{\·}{v}=C_b^n{f}_{\mathrm{ib}}^b-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+w_{\mathrm{en}}^n)\×v+g---\left(3.1\right)$

位置微分方程为：

$\stackrel{\·}{L}=\frac{v_n}{R_M+h},$ $\stackrel{\·}{\mathrm{\λ}}=\frac{v_e}{(R_N+h)\cos L},$ $\stackrel{\·}{h}=v_u---\left(3.2\right)$

其中，v＝[v_e v_n v_u]，v_e v_n v_u分别表示地理系下东、北、天向的速度；表示v的对时间的导数，以下参数上的符号“·”均表示该参数对时间的导数；f_ib ^b为加速度计输出，C_b ⁿ为载体系到地理系的姿态转移矩阵，ω_ie ⁿ为地球自转角速率在地理系下的投影，ω_en ⁿ为地理坐标系相对地球坐标系的转动角速率在地理系下的投影；且有：

${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_n}{R_M+h}\\ \frac{v_e}{R_N+h}\\ \frac{v_e}{R_N+h}\mathrm{tgL}\end{array}\right]---\left(3.3\right)$

λ，L，h分别为地理系下的经度、纬度、高度；R_N为卯酉圈曲率半径， $R_N=\frac{R_e}{{[{(1-f_r)}^2{\sin }^{2}L+{\cos }^{2}L]}^{1/2}},$ R_e为WGS84大地坐标系地球参考椭球的赤道平面半径，f_r为为WGS84大地坐标系地球参考椭球的椭圆度；R_M为子午圈曲率半径， $R_M=\frac{{(1-f_r)}^2{R}_e}{{[{(1-f_r)}^2{\sin }^{2}L+{\cos }^{2}L]}^{3/2}},$ 下同；g＝[0；0；-g₀]，g₀表示重力加速度，下同；

姿态微分方程为：

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\ω}}_{\mathrm{nb}}^b---\left(3.4\right)$

其中，

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)---\left(3.5\right)$

C_n ^b为c_b ⁿ的转置，ω_ib ^b为陀螺仪的输出；

在某一时刻t，地理系下的速度，位置分别为v(t)＝[v_e(t) v_n(t) v_u(t)]，p(t)＝[λ(t) L(t) h(t)]，载体系到地理系的姿态转移矩阵为C_b ⁿ(t)，经过Δt时间后，在t+Δt时刻地理系的速度，位置分别变为v(t+Δt)＝[v_e(t+Δt) v_n(t+Δt) v_u(t+Δt)]，p(t+Δt)＝[λ(t+Δt) L(t+Δt) h(t+Δt)]，C_b ⁿ(t+Δt)，则由式(3.1)，式(3.2)和式(3.4)，可以得到变化前后的关系为：

$v(t+\mathrm{\Δt})=v\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}\stackrel{\·}{v}\mathrm{dt}---\left(3.6\right)$

$p(t+\mathrm{\Δt})=p\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}\stackrel{\·}{p}\mathrm{dt}---\left(3.7\right)$

$C_b^n(t+\mathrm{\Δt})=C_b^n\left(t\right)+C_b^n\left(t\right)\×\frac{\sin \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}}{\mathrm{\Δ}{\mathrm{\θ}}_0}\×{\∫}_t^{t+\mathrm{\Δt}}{\mathrm{\ω}}_{\mathrm{nb}}^b\mathrm{dt}---\left(3.8\right)$

其中，Δθ₀表示∫_t ^t+Δtω_nb ^bdt的模；

联立式(3.1)，式(3.2)，式(3.4)可以得到：

$\stackrel{\·}{F}=K---\left(3.9\right)$

其中，

$F=[C_b^n\left(1\right),C_b^n\left(2\right),C_b^n\left(3\right),v_e,v_n,v_u,L,\mathrm{\λ},h]$

$K=[C_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(1\right),c_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(2\right),C_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(3\right),{\stackrel{\·}{v}}_e,{\stackrel{\·}{v}}_n,{\stackrel{\·}{v}}_u,\frac{v_n}{(R_M+h)},\frac{v_e}{(R_N+h)\cos L},v_u]$

C_b ⁿ(1)，C_b ⁿ(2)，C_b ⁿ(3)分别表示C_b ⁿ矩阵的第1，2，3行；C_b ⁿ×ω_nb ^b(1)，C_b ⁿ×ω_nb ^b(2)，C_b ⁿ×ω_nb ^b(3)分别表示C_b ⁿ矩阵与ω_nb ^b矩阵乘积的第1，2，3行；

由式(3.6)，(3.7)和(3.8)可以得到式(3.9)在t时刻和t+Δt时刻的关系为：

$F(t+\mathrm{\Δt})=F\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}K\left(t\right)\mathrm{dt}---\left(3.10\right)$

取∫_t ^t+ΔtK(t)dt的一阶解析式，有 ${\∫}_t^{t+\mathrm{\Δt}}K\left(t\right)\mathrm{dt}=K\left(t\right)\mathrm{\Δt},$ 则式(3.10)为：

F(t+Δt)＝F(t)+K(t)×Δt    (3.11)

利用式(3.11)，得到t时刻，两次t+Δt/2时刻，t+Δt时刻对应的F分别为F₀(t)，F₁(t+Δt/2)，F₂(t+Δt/2)，F(t+Δt)，再利用式(3.3)和式(3.8)得到对应时刻的K分别为K₀(t)，K₁(t+Δt/2)，K₂(t+Δt/2)，K(t+Δt)；

利用四阶龙格库塔算法，得到每个Δt时间内的更新式：

$F(t+\mathrm{\Δt})=F\left(t\right)+\frac{\mathrm{\Δt}}{6}[K_0\left(t\right)+2K_1(t+\mathrm{\Δt}/2)+2K_2(t+\mathrm{\Δt}/2)+K(t+\mathrm{\Δt})]---\left(3.12\right)$

令Δt等于捷联解算周期T，则利用式(3.12)，得到一个捷联解算周期内，捷联导航运算的更新式：

$F(t+T)=F\left(t\right)+\frac{T}{6}[K_0\left(t\right)+2K_1(t+T/2)+{2K}_2(t+T/2)+K(t+T)]---\left(3.13\right)$

按照式(3.13)进行迭代计算，得到每个捷联周期的速度、位置和姿态信息；同时，在非等间隔卡尔曼滤波模块输出的时刻，利用输出的反馈校正信息对对应的捷联解算输出的速度、位置和姿态信息进行校正。

所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述位置和速度补偿模块的解算方法如下：

t_kp时刻获得卫星导航信息，t_ks时刻获得星敏感器信息，t_kp时刻到t_ks时刻时间段捷联惯导系统每个周期T输出的信息如下：v_ei、v_ni、v_ui表示每个周期输出的东向、北向和天向速度，a_ei、a_ni、a_ui表示对应的加速度信息，L_i、h_i表示对应的纬度和高度；R_Mi、R_Ni表示对应的子午圈半径和卯酉圈半径，i＝t_kp，t_kp+T，t_kp+2T，…t_ks；

则建立速度补偿量计算公式如下：

$\left\{\begin{array}{c}\mathrm{\Δ}{v}_e=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ei}}\×T\\ \mathrm{\Δ}{v}_n=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ni}}\×T\\ \mathrm{\Δ}{v}_u=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ui}}\×T\end{array}\right.---\left(4.1\right)$

位置补偿量计算公式如下：

$\left\{\begin{array}{c}\mathrm{\Δ\λ}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}\frac{1}{(R_{\mathrm{Ni}}+h_i)\cos L_i}{v}_{\mathrm{ei}}\×T\\ \mathrm{\ΔL}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}\frac{1}{R_{\mathrm{Mi}}+h_i}{v}_{\mathrm{ni}}\×T\\ \mathrm{\ΔH}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{v}_{\mathrm{ui}}\×T\end{array}\right.---\left(4.2\right)$

其中，Δv_e、Δv_n、Δv_u分别表示东向、北向、天向速度在t_kp时刻到t_ks时刻这段时间内的速度补偿量；Δλ，ΔL，ΔH分别表示经度、纬度和高度在t_kp时刻到t_ks时刻这段时间内的位置补偿量；

L_G、λ_G、h_G表示卫星接收机t_kp时刻输出的纬度、经度、高度位置，v_eG、v_nG、v_uG表示卫星接收机t_kp时刻输出的地理坐标系下东向、北向、天向的速度，则t_ks时刻对应的位置和速度补偿模块的输出分别为L_G+ΔL、λ_G+Δλ、h_G+ΔH，v_eG+Δv_e、v_nG+Δv_n、v_uG+Δv_u。

所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述的非等间隔卡尔曼滤波模块的解算方法如下：

选取“东北天”地理坐标系作为导航坐标系：

组合导航系统的状态方程为：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{GW}---\left(5.1\right)$

其中，

$X=\left[\begin{array}{ccccccccc}{\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& \mathrm{\δ}{v}_e& \mathrm{\δ}{v}_n& \mathrm{\δ}{v}_u& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}\end{array}\right.$

$\begin{array}{c}{\left.\begin{array}{ccccccccc}{\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\ϵ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{ry}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_{\mathrm{rx}}& {\▿}_{\mathrm{ry}}& {\▿}_{\mathrm{rz}}\end{array}\right]}^T\end{array}---\left(5.2\right)$

φ_e φ_n φ_u表示东向、北向、天向平台误差角，δv_e δv_n δv_u表示东向、北向、天向速度误差，δL δλ δh表示经度、纬度、高度位置误差，ε_bx ε_by ε_bz表示三轴陀螺的常值漂移，ε_rx ε_ry ε_rz表示三轴陀螺的一阶马尔科夫过程、表示三轴加速度的一阶马尔科夫过程；A表示系统的状态转移矩阵，W表示系统的噪声矢量，G表示系统的噪声系数矩阵；

量测方程为：

$Z=\left[\begin{array}{c}{H}_v\\ H_p\\ H_a\end{array}\right]X+V---\left(5.3\right)$

其中，

Z＝[δv_e δv_n δv_u δL δλ δh δγ δθ δψ]^T    (5.4)

$H_v=\left[\begin{array}{cccc}{0}_{3\×3},& \left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],& 0_{3\×3},& 0_{3\×9}\end{array}\right]---\left(5.5\right)$

$H_p=\left[\begin{array}{cccc}{0}_{3\×3},& 0_{3\×3},& \left[\begin{array}{ccc}{R}_N\cos L& 0& 0\\ 0& R_M& 0\\ 0& 0& 1\end{array}\right],& 0_{3\×9}\end{array}\right]---\left(5.6\right)$

$H_a=\left[\begin{array}{cccc}\frac{-1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\θ}\end{array}\right],& 0_{3\×3},& 0_{3\×3},& 0_{3\×9}\end{array}\right]---\left(5.7\right)$

γ、θ、ψ表示载体真实的横滚角、俯仰角与航向角；δγ，δθ，δψ表示横滚角、俯仰角、航向角误差；V表示系统的测量噪声矢量；0_3×3表示三行三列的全零矩阵；0_3×9表示三行九列的全零矩阵；

根据式(5.7)，当俯仰角θ为90°时，cosθ＝0，H_a矩阵会产生奇点。当俯仰角θ为90°时，将θ＝90°代入下述两式

$C_t^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(5.8\right)$

$C_p^b=\left[\begin{array}{ccc}\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}+\cos {\mathrm{\ψ\γ}}^{\′}\cos {\mathrm{\γ}}^{\′}& \cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\cos {\mathrm{\γ}}^{\′}& -\cos {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ \sin {\mathrm{\ψ}}^{\′}\cos {\mathrm{\θ}}^{\′}& \cos {\mathrm{\ψ}}^{\′}\cos {\mathrm{\θ}}^{\′}& \sin {\mathrm{\θ}}^{\′}\\ \cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}& -\cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\γ}}^{\′}& \cos {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}\end{array}\right]---\left(5.9\right)$

其中，γ′、θ′、ψ′分别为载体实际的横滚、俯仰和航向角，与载体真实的横滚角、俯仰角、航向角γ、θ、ψ之间的关系为：

$\left\{\begin{array}{c}{\mathrm{\γ}}^{\′}=\mathrm{\γ}+\mathrm{\δ\γ}\\ {\mathrm{\θ}}^{\′}=\mathrm{\θ}+\mathrm{\δ\θ}\\ {\mathrm{\ψ}}^{\′}=\mathrm{\ψ}+\mathrm{\δ\ψ}\end{array}\right.---\left(5.10\right)$

C_p ^b为平台坐标系到载体坐标系的姿态转移矩阵，C_t ^b为地理坐标系到载体坐标系的姿态转移矩阵，且满足

$C_p^b=C_t^b{C}_p^t---\left(5.11\right)$

C_p ^t为平台坐标系到地理坐标系的姿态转移矩阵，

$C_p^t=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 1\end{array}\right]---\left(5.12\right)$

由式(5.8)～式(5.12)可以得到：

$\left[\begin{array}{c}{\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\end{array}\right]=\left[\begin{array}{ccc}0& -\cos \mathrm{\ψ}& 0\\ 0& \sin \mathrm{\ψ}& 0\\ 1& 0& -1\end{array}\right]\left[\begin{array}{c}\mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=H_{\mathrm{\φ}}^{\′}\left[\begin{array}{c}\mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]---\left(5.13\right)$

将H′_φ变为：

$H_{\mathrm{\φ}}^{\′}=\left[\begin{array}{ccc}\sin \mathrm{\ψ}& -\cos \mathrm{\ψ}& 0\\ \cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ 1& 0& -1\end{array}\right]---\left(5.14\right)$

则可以求得其逆矩阵为

$H_{\mathrm{\φ}}=\left[\begin{array}{ccc}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ -\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& -1\end{array}\right]---\left(5.15\right)$

则在俯仰角为90°时，

H_a＝[H_φ，0_3×3，0_3×3，0_3×9]    (5.16)

在建立了系统方程和量测方程后，将系统方程和量测方程离散化，再利用卡尔曼滤波器进行滤波。

利用卡尔曼滤波器进行滤波时，四阶龙格库塔捷联解算模块的的输出周期为T，星敏感器的输出周期(即天文姿态解算模块的输出周期)为T_Star，卫星接收机的输出周期为T_GPS，目前T_GPS＜T_Star；卡尔曼滤波离散化周期为T_D。同时满足T_D＝L×T，T_Star＝M×T_D，T_GPS＝N×T_D，ΔT＝(M-N)×T_D，L、M、N为正整数。

当没有星敏感器和卫星接收机信息的输出时，在每个离散化周期T_D只利用系统状态转移矩阵的特性，进行卡尔曼滤波器的时间更新；在接收到卫星接收机信息而没有接收到星敏感器信息的时间段内，同样只进行卡尔曼滤波器的时间更新；在接收到星敏感器信息的时刻，利用四阶龙格库塔捷联解算模块的输出，位置和速度补偿模块的解算的输出，以及天文定姿解算模块的输出，同时进行卡尔曼滤波器的时间更新和量测更新。

所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述实时三维显示方法如下：

首先按正等侧投影关系，绘制出真实的三维航迹，然后将所述导航位置信息按正等侧投影关系动实时绘制出导航后的航迹，通过比较两条航迹得到导航定位的直观比较结果；所述正等侧投影方法如下：

x_2d＝x_3d+z_3dcos45°

；

y_2d＝y_3d+z_3dcos45°

其中(x_3d，y_3d，z_3d)为所述输出的导航三维位置信息在三维直角坐标系内的坐标，(x_2d，y_2d)为将所述输出的导航三维位置信息按正等侧投影方法在二维直角坐标系内的坐标。

有益效果：

本发明从实际的捷联/天文/卫星组合导航系统工程应用角度出发，实现了高精度的捷联/天文/卫星组合导航系统。该系统实现了基于四阶龙格库塔算法的捷联导航解算，提高了捷联解算的实时性和精度；实现了载体垂直机动时的姿态组合；实现了针对非等间隔集中卡尔曼滤波器；利用投影算法实现了组合导航结果实时三维显示。

本发明使用的陀螺等效漂移为0.01°/h，加速度计等效零偏为1×10^-4g。采用四阶龙格库塔算法的捷联解算，在一小时内的位置漂移小于1000米，有效提高了捷联解算的精度。惯性/天文/卫星组合导航系统，在载体垂直机动时，其姿态依然可以快速收敛，精度保持和其他机动状态一致。在使用的星敏感器精度20角秒，GPS位置精度为10米，速度精度为0.2米/秒的情况下，组合导航系统定位精度优于5米，速度精度0.1米/秒，姿态角精度达到15角秒。本发明可以作为惯性/天文/卫星组合导航系统的演示和验证平台，同时，具有很强的工程应用价值。

附图说明

图1惯性/天文/卫星组合导航系统结构图。

图2是四阶龙格库塔捷联算法解算流程图。

图3器件不同步时的组合导航系统时间关系图。

图4惯性/天文/卫星组合导航系统实时显示界面。

具体实施方式

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

本发明惯性/天文/卫星高精度组合导航系统如图1所示。系统由惯性传感器、星敏感器、卫星接收机、导航计算机即组合导航解算模块和显示计算机即实时显示模块组成。系统完全具备实际工程应用能力。

导航计算机包括组合导航解算模块，所述组合导航解算模块包括四阶龙格库塔捷联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿模块；所述实时显示模块由三维实时显示模块构成。其中四阶龙格库塔捷联导航解算模块的输入端分别接惯性传感器的输出端和非等间隔卡尔曼滤波器模块的输出端；位置和速度补偿算法模块的输入端分别接卫星接收机的输出端和四阶龙格库塔捷联导航解算模块的输出端；天文定姿解算模块的输入端分别接星敏感器的输出端和位置和速度补偿算法模块的输出端；非等间隔卡尔曼滤波器模块的输入端分别接四阶龙格库塔捷联导航解算模块的输出端、位置和速度补偿算法模块的输出端和天文定姿解算模块的输出端；三维实时显示模块的输入端接四阶龙格库塔捷联导航解算模块的输入端。

所述的惯性/天文/卫星高精度组合导航系统的导航方法如下：

四阶龙格库塔捷联导航解算模块：利用惯性传感器(包括陀螺仪和加速度计)输出的角速率和加速度信息进行基于四阶龙格库塔算法的捷联导航解算，并利用非等间隔卡尔曼滤波模块输出的反馈校正信息进行校正，输出导航速度、位置、姿态及加速度信息；

位置和速度补偿模块：利用四阶龙格库塔捷联导航解算模块输出的速度和加速度信息计算卫星速度和位置的补偿量，结合卫星接收机输出的卫星速度和位置信息输出补偿后的卫星速度和位置信息；

天文定姿解算模块：利用星敏传感器输出的信息以及所述补偿后的卫星速度和位置信息解算得到天文导航系统的姿态信息；

非等间隔卡尔曼滤波模块：将四阶龙格库塔捷联导航解算模块输出的导航速度、位置、姿态，补偿后的卫星速度和位置信息以及天文导航系统的姿态信息利用设计的非等间隔卡尔曼滤波器进行滤波；

三维实时显示模块：将方法所述输出的三维位置信息实时进行三维图形显示。

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

1 惯性测量器件数据、卫星数据和天文数据的实时采集

实时采集惯性传感器数据、卫星接收机数据和星敏感器数据。

2 基于四阶龙格库塔算法的捷联惯导解算

选取“东北天”地理坐标系作为导航坐标系：

速度的微分方程为：

$\stackrel{\·}{v}=C_b^n{f}_{\mathrm{ib}}^b-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+w_{\mathrm{en}}^n)\×v+g---\left(2.1\right)$

位置微分方程为：

$\stackrel{\·}{L}=\frac{v_n}{R_M+h},$ $\stackrel{\·}{\mathrm{\λ}}=\frac{v_e}{(R_N+h)\cos L},$ $\stackrel{\·}{h}=v_u---\left(2.2\right)$

其中，v＝[v_e v_n v_u]，v_e v_n v_u表示地理系下东、北、天向的速度；表示v的对时间的导数，以下参数上的符号“·”均表示该参数对时间的导数，不再单独说明；f_ib ^b为加速度计输出，C_b ⁿ为载体系到地理系的姿态转移矩阵，ω_ie ⁿ为地球自转角速率在地理系下的投影，ω_en ⁿ为地理坐标系相对地球坐标系的转动角速率在地理系下的投影；，且有

${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_n}{R_M+h}\\ \frac{v_e}{R_N+h}\\ \frac{v_e}{R_N+h}\mathrm{tgL}\end{array}\right]---\left(2.3\right)$

λ，L，h分别为地理系下的经度、纬度、高度；R_N为卯酉圈曲率半径， $R_N=\frac{R_e}{{[{(1-f_r)}^2{\sin }^{2}L+{\cos }^{2}L]}^{1/2}},$ R_e为WGS84大地坐标系地球参考椭球的赤道平面半径，f_r为为WGS84大地坐标系地球参考椭球的椭圆度，R_M为子午圈曲率半径， $R_M=\frac{{(1-f_r)}^2{R}_e}{{[{(1-f_r)}^2{\sin }^{2}L+{\cos }^{2}L]}^{3/2}},$ 下同；g＝[0；0；-g₀]，g₀表示重力加速度，下同；

方向余弦姿态微分方程为：

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\ω}}_{\mathrm{nb}}^b---\left(2.4\right)$

其中，

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)---\left(2.5\right)$

C_n ^b为C_b ⁿ的转置，ω_ib ^b为陀螺仪的输出；

假设某时刻t，地理系下的速度，位置分别为v(t)＝[v_e(t) v_n(t) v_u(t)]，p(t)＝[λ(t) L(t) h(t)]，载体系到地理系的姿态转移矩阵为C_b ⁿ(t)，经过Δt时间后，在t+Δt时刻分别变为v(t+Δt)＝[v_e(t+Δt) v_n(t+Δt) v_u(t+Δt)]，p(t+Δt)＝[λ(t+Δt) L(t+Δt) h(t+Δt)]，C_b ⁿ(t+Δt)，则由式(2.1)，式(2.2)和式(2.4)，可以得到变化前后的关系为：

$v(t+\mathrm{\Δt})=v\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}\stackrel{\·}{v}\mathrm{dt}---\left(2.6\right)$

$p(t+\mathrm{\Δt})=p\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}\stackrel{\·}{p}\mathrm{dt}---\left(2.7\right)$

$C_b^n(t+\mathrm{\Δt})=C_b^n\left(t\right)+C_b^n\left(t\right)\×\frac{\sin \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}}{\mathrm{\Δ}{\mathrm{\θ}}_0}\×{\∫}_t^{t+\mathrm{\Δt}}{\mathrm{\ω}}_{\mathrm{nb}}^b\mathrm{dt}---(2.8)$

其中，Δθ₀表示∫_t ^t+Δtω_nb ^bdt的模。

联立式(2.1)，式(2.2)，式(2.4)可以得到：

$\stackrel{\·}{F}=K---\left(2.9\right)$

其中，

$F=[C_b^n\left(1\right),C_b^n\left(2\right),C_b^n\left(3\right),v_e,v_n,v_u,L,\mathrm{\λ},h],$

$K=[C_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(1\right),c_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(2\right),C_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(3\right),{\stackrel{\·}{v}}_e,{\stackrel{\·}{v}}_n,{\stackrel{\·}{v}}_u,\frac{v_n}{(R_M+h)},\frac{v_e}{(R_N+h)\cos L},v_u]$

C_b ⁿ(1)，C_b ⁿ(2)，C_b ⁿ(3)分别表示C_b ⁿ矩阵的第1，2，3行；C_b ⁿ×ω_nb ^b(1)，C_b ⁿ×ω_nb ^b(2)，C_b ⁿ×ω_nb ^b(3)分别表示C_b ⁿ矩阵与ω_nb ^b矩阵乘积的第1，2，3行。

由式(2.6)，(2.7)和(2.8)可以得到式(2.9)在t时刻和t+Δt时刻的关系为：

$F(t+\mathrm{\Δt})=F\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}K\left(t\right)\mathrm{dt}---\left(2.10\right)$

取∫_t ^t+ΔtK(t)dt的一阶解析式，有 ${\∫}_t^{t+\mathrm{\Δt}}K\left(t\right)\mathrm{dt}=K\left(t\right)\mathrm{\Δt},$ 则式(2.9)可简化为

F(t+Δt)＝F(t)+K(t)×Δt    (2.11)

由K的表达式可知，K可以由对应时刻的F，陀螺仪输出ω_ib ^b和加速度输出f_ib ^b通过式(2.1)，式(2.3)和式(2.5)求得。考虑利用(2.5)式求取ω_nb ^b时，由于每个捷联计算周期只采集一次陀螺仪输出，即只有一个ω_ib ^b值，因此，假设每个捷联周期内角速率是恒定的，利用式(2.3)和式(2.8)通过更新C_b ⁿ，ω_ie ⁿ，ω_en ⁿ来更新ω_nb ^b。

解算流程如图2所示。

按照如图2所示的流程通过计算分别得到t时刻，两次t+Δt/2时刻，t+Δt时刻对应的F分别为F₀(t)，F₁(t+Δt/2)，F₂(t+Δt/2)，F(t+Δt)，以及对应时刻的K分别为K₀(t)，K₁(t+Δt/2)，K₂(t+Δt/2)，K(t+Δt)后，即可利用四阶龙格库塔算法，得到每个Δt时间内的更新式：

$F(t+\mathrm{\Δt})=F\left(t\right)+\frac{\mathrm{\Δt}}{6}[K_0\left(t\right)+2K_1(t+\mathrm{\Δt}/2)+2K_2(t+\mathrm{\Δt}/2)+K(t+\mathrm{\Δt})]---\left(2.12\right)$

令Δt等于捷联解算周期T，则可以利用式(2.11)，得到一个捷联解算周期内，捷联导航运算的更新式：

$F(t+T)=F\left(t\right)+\frac{T}{6}[K_0\left(t\right)+2K_1(t+T/2)+{2K}_2(t+T/2)+K(t+T)]---\left(2.13\right)$

若非等间隔卡尔曼滤波模块无反馈校正信息输出，按照式(3.13)进行计算，即可得到四阶龙格库塔捷联导航解算模块输出的速度、位置和姿态信息；在非等间隔卡尔曼滤波模块输出反馈校正信息的时刻，利用输出的反馈校正信息对按照式(3.13)计算得到的对应时刻的的速度、位置和姿态信息进行校正，即可得到四阶龙格库塔捷联导航解算模块输出的速度、位置和姿态信息；

3.捷联/天文/卫星组合导航卡尔曼滤波器设计步骤

选取“东北天”地理坐标系作为导航坐标系。

组合导航系统的状态方程为：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{GW}---\left(3.1\right)$

其中，

$X=\left[\begin{array}{ccccccccc}{\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& \mathrm{\δ}{v}_e& \mathrm{\δ}{v}_n& \mathrm{\δ}{v}_u& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}\end{array}\right.$

$\begin{array}{c}{\left.\begin{array}{ccccccccc}{\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\ϵ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{ry}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_{\mathrm{rx}}& {\▿}_{\mathrm{ry}}& {\▿}_{\mathrm{rz}}\end{array}\right]}^T\end{array}---\left(3.2\right)$

φ_e φ_n φ_u表示东向、北向、天向平台误差角，δv_e δv_n δv_u表示东向、北向、天向速度误差，δL δλ δh表示东向、北向、天向位置误差，ε_bx ε_by ε_bz表示三轴陀螺的常值漂移，ε_rx ε_ry ε_rz表示三轴陀螺的一阶马尔科夫过程、表示三轴加速度的一阶马尔科夫过程。

A表示系统的状态转移矩阵，W表示系统的噪声矢量，G表示系统的噪声系数矩阵。

定义量测方程为：

$Z=\left[\begin{array}{c}{H}_v\\ H_p\\ H_a\end{array}\right]X+V---\left(3.3\right)$

其中，

Z＝[δv_e δv_n δv_u δL δλ δh δγ δθ δψ]^T    (3.4)

$H_v=\left[\begin{array}{cccc}{0}_{3\×3},& \left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],& 0_{3\×3},& 0_{3\×9}\end{array}\right]---\left(3.5\right)$

$H_p=\left[\begin{array}{cccc}{0}_{3\×3},& 0_{3\×3},& \left[\begin{array}{ccc}{R}_N\cos L& 0& 0\\ 0& R_M& 0\\ 0& 0& 1\end{array}\right],& 0_{3\×9}\end{array}\right]---\left(3.6\right)$

$H_a=\left[\begin{array}{cccc}\frac{-1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\θ}\end{array}\right],& 0_{3\×3},& 0_{3\×3},& 0_{3\×9}\end{array}\right]---\left(3.7\right)$

γ、θ、ψ表示载体真实的横滚角、俯仰角与航向角；δγ，δθ，δψ表示横滚角、俯仰角、航向角误差；V表示系统的测量噪声矢量；0_3×3表示三行三列的全零矩阵；0_3×9表示三行九列的全零矩阵。

根据式(3.7)，当俯仰角θ为90°时，cosθ＝0，H_a矩阵会产生奇点。

设C_p ^b为平台坐标系到载体坐标系的姿态转移矩阵，C_t ^b为地理坐标系到载体坐标系的姿态转移矩阵，C_p ^t为平台坐标系到地理坐标系系的姿态转移矩阵，则各姿态转移矩阵存在如下关系：

$C_p^b=C_t^b{C}_p^t---\left(3.8\right)$

其中， $C_t^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(3.9\right)$

$C_p^b=\left[\begin{array}{ccc}\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}+\cos {\mathrm{\ψ}}^{\′}\cos {\mathrm{\γ}}^{\′}& \cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\cos {\mathrm{\γ}}^{\′}& -\cos {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ \sin {\mathrm{\ψ}}^{\′}\cos {\mathrm{\θ}}^{\′}& \cos {\mathrm{\ψ}}^{\′}\cos {\mathrm{\θ}}^{\′}& \sin {\mathrm{\θ}}^{\′}\\ \cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}& -\cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\γ}}^{\′}& \cos {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}\end{array}\right]---\left(3.10\right)$

$C_p^t=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 1\end{array}\right]---\left(3.11\right)$

γ′、θ′、ψ′分别载体实际的横滚、俯仰和航向角，与载体真实的横滚角、俯仰角与航向角γ、θ、ψ之间的关系为：

$\left\{\begin{array}{c}{\mathrm{\γ}}^{\′}=\mathrm{\γ}+\mathrm{\δ\γ}\\ {\mathrm{\θ}}^{\′}=\mathrm{\θ}+\mathrm{\δ\θ}\\ {\mathrm{\ψ}}^{\′}=\mathrm{\ψ}+\mathrm{\δ\ψ}\end{array}\right.---\left(3.12\right)$

当俯仰角为90度时，将θ＝90°代入式(3.9)和(3.10)，化简后和式(3.11)一起代入(3.8)式，可以得到：

$\left[\begin{array}{c}{\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\end{array}\right]=\left[\begin{array}{ccc}0& -\cos \mathrm{\ψ}& 0\\ 0& \sin \mathrm{\ψ}& 0\\ 1& 0& -1\end{array}\right]\left[\begin{array}{c}\mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=H_{\mathrm{\φ}}^{\′}\left[\begin{array}{c}\mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]---\left(3.13\right)$

由上式可以看出，H′_φ不满秩，因此其逆矩阵是无法求得的。在俯仰角为90°时，考虑到实际上航向角实际是无法准确确定的，因此，为使H′_φ满秩，将H′_φ变为：

$H_{\mathrm{\φ}}^{\′}=\left[\begin{array}{ccc}\sin \mathrm{\ψ}& -\cos \mathrm{\ψ}& 0\\ \cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ 1& 0& -1\end{array}\right]---\left(3.14\right)$

则可以求得其逆矩阵为

$H_{\mathrm{\φ}}=\left[\begin{array}{ccc}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ -\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& -1\end{array}\right]---(3.15)$

则在俯仰角为90°时，

H_a＝[H_φ，0_3×3，0_3×3，0_3×9]    (3.16)

在建立了系统方程和量测方程后，将系统方程和量测方程离散化，即可建立卡尔曼滤波器。

4.速度位置补偿算法设计步骤

在进行卡尔曼滤波时，必须保证所有输入的信息是同一时刻的。天文定姿解算的周期取决于星敏感器的输出周期。目前星敏感器的输出周期一般大于卫星导航的输出周期。假设t_kp时刻获得卫星导航信息，t_ks时刻获得星敏感器信息。为使滤波器在星光输出时刻进行滤波，必须将卫星数据补偿到t_ks时刻。考虑到步骤2中捷联惯导系统的输出在短时间精度较高，因此利用t_kp时刻到t_ks时刻这段时间内的捷联惯导系统输出的速度和加速度信息，将卫星导航信息外推到t_ks时刻。

v_ei、v_ni、v_ui表示t_kp时刻到t_ks时刻时间段步骤2中的捷联惯导系统每个周期T输出的东向、北向和天向速度；a_ei、a_ni、a_ui表示对应的加速度输出信息，i＝t_kp，t_kp+T，t_kp+2T，…t_ks；L_i、h_i表示对应的纬度和高度；R_Mi、R_Ni表示对应的子午圈半径和卯酉圈半径。

则可以建立速度补偿量计算公式如下：

$\left\{\begin{array}{c}\mathrm{\Δ}{v}_e=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ei}}\×T\\ \mathrm{\Δ}{v}_n=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ni}}\×T\\ \mathrm{\Δ}{v}_u=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ui}}\×T\end{array}\right.---\left(4.1\right)$

位置补偿量计算公式如下：

$\left\{\begin{array}{c}\mathrm{\Δ\λ}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}\frac{1}{(R_{\mathrm{Ni}}+h_i)\cos L_i}{v}_{\mathrm{ei}}\×T\\ \mathrm{\ΔL}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}\frac{1}{R_{\mathrm{Mi}}+h_i}{v}_{\mathrm{ni}}\×T\\ \mathrm{\ΔH}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{v}_{\mathrm{ui}}\×T\end{array}\right.---\left(4.2\right)$

其中，Δv_e、Δv_n、Δv_u分别表示东向、北向、天向速度在t_kp时刻到t_ks时刻这段时间内的速度补偿量；Δλ，ΔL，ΔH分别表示经度、纬度和高度在t_kp时刻到t_ks时刻这段时间内的位置补偿量。

假设L_G、λ_G、h_G表示卫星接收机t_kp时刻输出的纬度、经度、高度位置，v_eG、v_nG、v_uG表示卫星接收机t_kp时刻输出的地理坐标系下东向、北向、天向的速度，则利用式(4.1)和式(4.2)t_ks时刻对应的输出分别为L_G+ΔL、λ_G+Δλ、h_G+ΔH，v_eG+Δv_e、v_nG+Δv_n、v_uG+Δv_u。

5.器件不同步的组合滤波步骤

利用卡尔曼滤波器进行滤波时，必须保证输入信息是同一时刻的。假设步骤2中捷联解算模块的的输出周期为T，星敏感器的输出周期(即天文姿态解算模块的输出周期)为T_Star，卫星接收机的输出周期为T_GPS，目前T_GPS＜T_Star；卡尔曼滤波离散化周期为T_D。同时满足T_D＝L×T，T_Star＝M×T_D，T_GPS＝N×T_D，ΔT＝(M-N)×T_D，L、M、N为正整数。

以一个离散周期的处理过程为例。假设此次滤波周期的起始时刻为t_k，从图3可以清楚地看出，当星光仪测量输出时，此时卫星接收机的输出信息超前于星光仪的输出。

当没有星敏感器和卫星接收机信息的输出时，即在t_k+m·T_D时刻，其中，m＝1，2…N，只利用系统状态转移矩阵的特性，进行卡尔曼滤波器的时间更新；在接收到卫星接收机信息而没有接收到星敏感器信息的ΔT时间段内，同样只进行卡尔曼滤波器的时间更新；在接收到星敏感器信息的t_k+M·T_D时刻，首先利用步骤3将t_k+N·T_D得到的卫星接收机数据利用步骤4的方法外推到t_k+M·T_D时刻，同时，结合步骤2以及天文定姿解算模块的输出，同时进行卡尔曼滤波器的时间更新和量测更新。

6.实时显示步骤

对任意一个由多面体组成的三维图形，在三维直角坐标系内，可以分解为许多条由空间的两个点组成的直线。而PC机屏幕是一个平面，它上面只有二维坐标。因此利用正等侧投影方法将三维坐标投影到二维平面坐标内。导航航迹的绘制采用的是VC++6.0MFC中的绘图类库。

假设空间某点的空间坐标为(x_3d，y_3d，z_3d)，则其正等侧投影后的平面坐标为：

x_2d＝x_3d+z_3dcos45°

y_2d＝y_3d+z_3dcos45°

绘制曲线时，首先根据选择的航迹数据在屏幕上的三维地理坐标系内画出一条真实的航迹，然后以组合导航解算模块输出的数据为基础作动画显示，画出另一条航迹，通过比较这两条航迹，可以非常直观地看出导航定位的结果。

下面说明在Visual C++中程序的具体实现步骤：

首先要建立一个位图对象，再定义并创建一个与位图对象相兼容的内存设备描述对象。

1.按正等侧投影关系，先将设置的三维真实航迹画好。为了具有通用性，使载体无论运行多大区域，始终能处于三维坐标系的中间，首先将全部航迹读入，确定三维方向的最大值和最小值，然后根据指定区域的大小，计算出比例系数，再绘制出真实航迹曲线。

2.同样按照正等侧投影关系，将组合导航解算模块输出的数据实时显示在平面坐标系中，绘制出实时航迹曲线。同时，为了显示出航迹的实时运动，采用符号“☆”指示载体当前时刻所在的位置。符号“☆”在运行中根据组合导航解算模块输出的数据实时刷新，就形成了载体的连续运动。

设计完成的实时显示界面如图4所示。

本发明说明书中，四阶龙格库塔捷联解算模块中利用非等间隔卡尔曼滤波模块输出的反馈校正信息进行校正，以及非等间隔卡尔曼滤波模块中对状态方程和量测方程进行离散化的方法，都属于本领域专业技术人员公知的现有技术。

Claims (6)

1.一种惯性/天文/卫星高精度组合导航系统，由传感器模块、组合导航解算模块、实时显示模块依次串接组成，其特征在于所述传感器模块包括惯性传感器、星敏感器和卫星接收机；所述组合导航解算模块包括四阶龙格库塔捷联导航解算模块、天文定姿解算模块、非等间隔卡尔曼滤波模块以及位置和速度补偿模块；所述实时显示模块由三维实时显示模块构成；其中四阶龙格库塔捷联导航解算模块的输入端分别接惯性传感器的输出端和非等间隔卡尔曼滤波模块的输出端；位置和速度补偿模块的输入端分别接卫星接收机的输出端和四阶龙格库塔捷联导航解算模块的输出端；天文定姿解算模块的输入端分别接星敏感器的输出端和位置和速度补偿模块的输出端；非等间隔卡尔曼滤波模块的输入端分别接四阶龙格库塔捷联导航解算模块的输出端、位置和速度补偿算法模块的输出端和天文定姿解算模块的输出端；三维实时显示模块的输入端接四阶龙格库塔捷联导航解算模块的输出端。

2.一种如权利要求1所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述导航方法如下：

采用四阶龙格库塔捷联导航解算模块利用惯性传感器输出的角速率和加速度信息进行基于四阶龙格库塔算法的捷联导航解算，并利用非等间隔卡尔曼滤波模块输出的反馈校正信息进行校正，输出导航速度、位置、姿态及加速度信息；

采用位置和速度补偿模块利用四阶龙格库塔捷联导航解算模块输出的导航速度和加速度信息计算卫星速度和位置的补偿量，结合卫星接收机输出的卫星速度和位置信息输出补偿后的卫星速度和位置信息；

采用天文定姿解算模块利用星敏感器输出的信息以及所述补偿后的卫星位置信息解算得到天文导航系统的姿态信息；

采用非等间隔卡尔曼滤波模块将四阶龙格库塔捷联导航解算模块输出的导航速度、位置和姿态信息，补偿后的卫星速度和位置信息以及天文导航系统的姿态信息进行滤波得到反馈校正信息；

采用三维实时显示模块将所述导航位置信息实时进行三维图形显示。

3.根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述四阶龙格库塔捷联导航解算模块方法如下：

选取“东北天”地理坐标系作为导航坐标系：

速度微分方程为：

$\stackrel{\·}{v}=C_b^n{f}_{\mathrm{ib}}^b-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×v+g---\left(3.1\right)$

位置微分方程为：

$\stackrel{\·}{L}=\frac{v_n}{R_M+h},$ $\stackrel{\·}{\mathrm{\λ}}=\frac{v_e}{(R_N+h)\cos L},$ $\stackrel{\·}{h}=v_u---\left(3.2\right)$

其中，v＝[v_e v_n v_u]，v_e v_n v_u分别表示地理系下东、北、天向的速度；表示v对时间的导数，以下参数上的符号“·”均表示该参数对时间的导数；

为加速度计输出，

为载体系到地理系的姿态转移矩阵，

为地球自转角速率在地理系下的投影，

为地理坐标系相对地球坐标系的转动角速率在地理系下的投影；且有：

${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{v_n}{R_M+h}\\ \frac{v_w}{R_N+h}\\ \frac{v_e}{R_N+h}\mathrm{tgL}\end{array}\right]---\left(3.3\right)$

λ，L，h分别为地理系下的经度、纬度、高度；R_N为卯酉圈曲率半径，R_e为WGS84大地坐标系地球参考椭球的赤道平面半径，f_r为WGS84大地坐标系地球参考椭球的椭圆度；R_M为子午圈曲率半径，

下同；g＝[0；0；-g₀]，g₀表示重力加速度，下同；

姿态微分方程为：

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\ω}}_{\mathrm{nb}}^b---\left(3.4\right)$

其中，

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^n-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)---\left(3.5\right)$

为

的转置，

为陀螺仪的输出；

在某一时刻t，地理系下的速度，位置分别为v(t)＝[v_e(t)v_n(t)v_u(t)]，p(t)＝[λ(t)L(t)h(t)]，载体系到地理系的姿态转移矩阵为

经过Δt时间后，在t+Δt时刻地理系的速度，位置分别变为v(t+Δt)＝[v_e(t+Δt)v_n(t+Δt)v_u(t+Δt)]，p(t+Δt)＝[λ(t+Δt)L(t+Δt)h(t+Δt)]，

则由式(3.1)，式(3.2)和式(3.4)，变化前后的关系为：

$v(t+\mathrm{\Δt})=v\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}\stackrel{\·}{v}\mathrm{dt}---\left(3.6\right)$

$p(t+\mathrm{\Δt})=p\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}\stackrel{\·}{p}\mathrm{dt}---\left(3.7\right)$

$C_b^n(t+\mathrm{\Δt})=C_b^n\left(t\right)+C_b^n\left(t\right)\×\frac{\sin \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}}{\mathrm{\Δ}{\mathrm{\θ}}_0}\×{\∫}_t^{t+\mathrm{\Δt}}{\mathrm{\ω}}_{\mathrm{nb}}^b\mathrm{dt}---\left(3.8\right)$

其中，Δθ₀表示

的模；

联立式(3.1)，式(3.2)，式(3.4)则有：

$\stackrel{\·}{F}=K---\left(3.9\right)$

其中，

$F=[C_b^n\left(1\right),C_b^n\left(2\right),C_b^n\left(3\right),v_e,v_n,v_u,L,\mathrm{\λ},h]$

$K=[C_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(1\right),C_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(2\right),C_b^n\×{\mathrm{\ω}}_{\mathrm{nb}}^b\left(3\right),{\stackrel{\·}{v}}_e,{\stackrel{\·}{v}}_n,{\stackrel{\·}{v}}_u,\frac{v_n}{(R_M+h)},\frac{v_e}{(R_N+h)\cos L},v_u]$

分别表示

矩阵的第1，2，3行；

分别表示

矩阵与

矩阵乘积的第1，2，3行；

由式(3.6)，(3.7)和(3.8)得到式(3.9)在t时刻和t+Δt时刻的关系为：

$F(t+\mathrm{\Δt})=F\left(t\right)+{\∫}_t^{t+\mathrm{\Δt}}K\left(t\right)\mathrm{dt}---\left(3.10\right)$

取的一阶解析式，有则式(3.10)变为：

F(t+Δt)＝F(t)+K(t)×Δt                  (3.11)

利用式(3.11)，得到t时刻，两次t+Δt/2时刻，t+Δt时刻对应的F分别为F₀(t)，F₁(t+Δt/2)，F₂(t+Δt/2)，F(t+Δt)，再利用式(3.3)和式(3.8)，得到对应时刻的K即K₀(t)，K₁(t+Δt/2)，K₂(t+Δt/2)，K(t+Δt)；

利用四阶龙格库塔算法，得到每个Δt时间内的更新式：

$F(t+\mathrm{\Δt})=F\left(t\right)+\frac{\mathrm{\Δt}}{6}[K_0\left(t\right)+2K_1(t+\mathrm{\Δt}/2)+2K_2(t+\mathrm{\Δt}/2)+K(t+\mathrm{\Δt})]---\left(3.12\right)$

令Δt等于捷联解算周期T，则利用式(3.12)，得到一个捷联解算周期内，捷联导航运算的更新式：

$F(t+T)=F\left(t\right)+\frac{T}{6}[K_0\left(t\right)+2K_1(t+T/2)+2K_2(t+T/2)+K(t+T)]---\left(3.13\right).$

按照式(3.13)进行迭代计算，得到每个捷联周期的速度、位置和姿态信息；同时，在非等间隔卡尔曼滤波模块输出的时刻，利用输出的反馈校正信息对对应的捷联解算输出的速度、位置和姿态信息进行校正。

4.根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述位置和速度补偿模块的解算方法如下：

t_kp时刻获得卫星导航信息，t_ks时刻获得星敏感器信息，t_kp时刻到t_ks时刻时间段捷联惯导系统每个周期T输出的信息如下：v_ei、v_ni、v_ui表示每个周期输出的东向、北向和天向速度，a_ei、a_ni、a_ui表示对应的加速度信息，L_i、h_i表示对应的纬度和高度；R_Mi、R_Ni表示对应的子午圈半径和卯酉圈半径，i＝t_kp，t_kp+T，t_kp+2T，…t_ks；

则建立速度补偿量计算公式如下：

$\left\{\begin{array}{c}\mathrm{\Δ}{v}_e=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ei}}\×T\\ \mathrm{\Δ}{v}_n=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ni}}\×T\\ \mathrm{\Δ}{v}_u=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{a}_{\mathrm{ui}}\×T\end{array}\right.---\left(4.1\right)$

位置补偿量计算公式如下：

$\left\{\begin{array}{c}\mathrm{\Δ\λ}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}\frac{1}{(R_{\mathrm{Ni}}+h_i)\cos L_i}{v}_{\mathrm{ei}}\×T\\ \mathrm{\ΔL}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}\frac{1}{R_{\mathrm{Mi}}+h_i}{v}_{\mathrm{ni}}\×T\\ \mathrm{\ΔH}=\underset{i=t_{\mathrm{kp}}}{\overset{t_{\mathrm{ks}}}{\mathrm{\Σ}}}{v}_{\mathrm{ui}}\×T\end{array}\right.---\left(4.2\right)$

其中，Δv_e、Δv_n、Δv_u分别表示东向、北向、天向速度在t_kp时刻到t_ks时刻这段时间内的速度补偿量；Δλ，ΔL，ΔH分别表示经度、纬度和高度在t_kp时刻到t_ks时刻这段时间内的位置补偿量；

L_G、λ_G、h_G表示卫星接收机t_kp时刻输出的纬度、经度、高度位置，v_eG、v_nG、v_uG表示卫星接收机t_kp时刻输出的地理坐标系下东向、北向、天向的速度，则t_ks时刻对应的位置和速度补偿模块的输出分别为L_G+ΔL、λ_G+Δλ、h_G+ΔH，v_eG+/Δv_e、v_nG+Δv_n、v_uG+Δv_u。

5.根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述的非等间隔卡尔曼滤波模块的解算方法如下：

选取“东北天”地理坐标系作为导航坐标系：

组合导航系统的状态方程为：

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{GW}---\left(5.1\right)$

其中，

$X=\left[\begin{array}{ccccccccc}{\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& \mathrm{\δ}{v}_e& \mathrm{\δ}{v}_n& \mathrm{\δ}{v}_u& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}\end{array}\right.$ (5.2)

${\left.\begin{array}{ccccccccc}{\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\ϵ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{ry}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_{\mathrm{rx}}& {\▿}_{\mathrm{ry}}& {\▿}_{\mathrm{rz}}\end{array}\right]}^T$

φ_e φ_n φ_u表示东向、北向、天向平台误差角，δv_e δv_n δv_u表示东向、北向、天向速度误差，δL δλ δh表示经度、纬度、高度位置误差，ε_bx ε_by ε_bz表示三轴陀螺的常值漂移，ε_rx ε_ry ε_rz表示三轴陀螺的一阶马尔科夫过程、

表示三轴加速度的一阶马尔科夫过程；A表示系统的状态转移矩阵，W表示系统的噪声矢量，G表示系统的噪声系数矩阵；

量测方程为：

$Z=\left[\begin{array}{c}{H}_v\\ H_p\\ H_a\end{array}\right]X+V---\left(5.3\right)$

其中，

Z＝[δv_e δv_n δv_u δL δλ δh δγ δθ δψ]^T    (5.4)

$H_v=[0_{3\×3},\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],0_{3\×3},0_{3\×9}]---\left(5.5\right)$

$H_p=[0_{3\×3},0_{3\×3},\left[\begin{array}{ccc}{R}_N\cos L& 0& 0\\ 0& R_M& 0\\ 0& 0& 1\end{array}\right],0_{3\×9}]---\left(5.6\right)$

$H_a=[\frac{-1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}& -\cos \mathrm{\θ}\end{array}\right],0_{3\×3},0_{3\×3},0_{3\×9}]---\left(5.7\right)$

γ、θ、ψ表示载体真实的横滚角、俯仰角与航向角；δγ，δθ，δψ表示横滚角、俯仰角、航向角误差；V表示系统的测量噪声矢量；0_3×3表示三行三列的全零矩阵；0_3×9表示三行九列的全零矩阵；

根据式(5.7)，当俯仰角θ为90°时，cosθ＝0，H_a矩阵产生奇点：

当俯仰角θ为90°时，将θ＝90°代入下述两式：

$C_t^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(5.8\right)$

$C_p^b=\left[\begin{array}{ccc}\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}+\cos {\mathrm{\ψ}}^{\′}\cos {\mathrm{\γ}}^{\′}& \cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\cos {\mathrm{\γ}}^{\′}& -\cos {\mathrm{\θ}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ \sin {\mathrm{\ψ}}^{\′}\cos {\mathrm{\θ}}^{\′}& \cos {\mathrm{\ψ}}^{\′}\cos {\mathrm{\θ}}^{\′}& \sin {\mathrm{\θ}}^{\′}\\ \cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}& -\cos {\mathrm{\ψ}}^{\′}\sin {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}-\sin {\mathrm{\ψ}}^{\′}\sin {\mathrm{\γ}}^{\′}& \cos {\mathrm{\θ}}^{\′}\cos {\mathrm{\γ}}^{\′}\end{array}\right]---\left(5.9\right)$

其中，γ′、θ′、ψ′分别为载体实际的横滚、俯仰和航向角，与载体真实的横滚角、俯仰角与航向角γ、θ、ψ之间的关系为：

$\left\{\begin{array}{c}{\mathrm{\γ}}^{\′}=\mathrm{\γ}+\mathrm{\δ\γ}\\ {\mathrm{\θ}}^{\′}=\mathrm{\θ}+\mathrm{\δ\θ}\\ {\mathrm{\ψ}}^{\′}=\mathrm{\ψ}+\mathrm{\δ\ψ}\end{array}\right.---\left(5.10\right)$

为平台坐标系到载体坐标系的姿态转移矩阵，

为地理坐标系到载体坐标系的姿态转移矩阵，且满足：

$C_p^b=C_t^b{C}_p^t---\left(5.11\right)$

为平台坐标系到地理坐标系的姿态转移矩阵，

$C_p^t=\left[\begin{array}{ccc}1& -{\mathrm{\φ}}_u& {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u& 1& -{\mathrm{\φ}}_e\\ -{\mathrm{\φ}}_n& {\mathrm{\φ}}_e& 1\end{array}\right]---\left(5.12\right)$

由式(5.8)～式(5.12)可以得到：

$\left[\begin{array}{c}{\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\end{array}\right]=\left[\begin{array}{ccc}0& -\cos \mathrm{\ψ}& 0\\ 0& \sin \mathrm{\ψ}& 0\\ 1& 0& -1\end{array}\right]\left[\begin{array}{c}\mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=H_{\mathrm{\φ}}^{\′}\left[\begin{array}{c}\mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]---\left(5.13\right)$

将H′_φ变为：

$H_{\mathrm{\φ}}^{\′}=\left[\begin{array}{ccc}\sin \mathrm{\ψ}& -\cos \mathrm{\ψ}& 0\\ \cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ 1& 0& -1\end{array}\right]---\left(5.14\right)$

则可以求得其逆矩阵为

$H_{\mathrm{\φ}}=\left[\begin{array}{ccc}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ -\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& -1\end{array}\right]---\left(5.15\right)$

则在俯仰角为90°时，

H_a＝[H_φ，0_3×3，0_3×3，0_3×9]            (5.16)

在建立了系统方程和量测方程后，将系统方程和量测方程离散化，再利用卡尔曼滤波器进行滤波；

利用卡尔曼滤波器进行滤波时，四阶龙格库塔捷联解算模块的的输出周期为T，星敏感器的输出周期即天文姿态解算模块的输出周期为T_Star，卫星接收机的输出周期为T_GPS，目前T_GPS＜T_Star；卡尔曼滤波离散化周期为T_D，同时满足T_D＝L×T，T_Star＝M×T_D，T_GPS＝N×T_D，ΔT＝(M-N)×T_D，L、M、N为正整数；

当没有星敏感器和卫星接收机信息的输出时，在每个离散化周期T_D只利用系统状态转移矩阵的特性，进行卡尔曼滤波器的时间更新；在接收到卫星接收机信息而没有接收到星敏感器信息的时间段内，同样只进行卡尔曼滤波器的时间更新；在接收到星敏感器信息的时刻，利用四阶龙格库塔捷联解算模块的输出，位置和速度补偿模块的解算的输出，以及天文定姿解算模块的输出，同时进行卡尔曼滤波器的时间更新和量测更新。

6.根据权利要求2所述的惯性/天文/卫星高精度组合导航系统的导航方法，其特征在于所述实时三维显示方法如下：

首先按正等侧投影关系，绘制出真实的三维航迹，然后将所述导航位置信息按正等侧投影关系实时绘制出导航后的航迹，通过比较两条航迹得到导航定位的比较结果；所述正等侧投影方法如下：

x_2d＝x_3d+z_3d cos45°

                    ；

y_2d＝y_3d+z_3d cos45°

其中(x_3d，y_3d，z_3d)为所述的导航位置信息在三维直角坐标系内的坐标，(x_2d，y_2d)为将所述导航位置信息按正等侧投影方法在二维直角坐标系内的坐标。

Images