CN103900569A - 微惯导与dgps和电子罗盘组合导航姿态测量方法 - Google Patents

Abstract

本发明提供的是一种微惯导与DGPS和电子罗盘组合导航姿态测量方法。首先利用微惯导、电子罗盘对组合系统进行初始对准，得到载体坐标系b到导航坐标系n的初始姿态矩阵；进而可以计算出载体的初始姿态值；利用微惯导系统的位置、速度、姿态及惯性传感器的误差方程，建立扩展卡尔曼滤波器的状态方程；利用电子罗盘和GPS分别建立的观测方程组成扩展卡尔曼滤波器的观测方程；利用扩展卡尔曼滤波器进行实时估测微惯导系统姿态误差；利用得到的姿态误差进行修正姿态矩阵，并计算出微惯导系统新的姿态值。本发明的方法是利用电子罗盘和GPS辅助微惯导系统来提高导航姿态精度的方法。

Description

微惯导与DGPS和电子罗盘组合导航姿态测量方法

技术领域

本发明涉及的是一种姿态测量方法，具体地说是一种微惯导/DGPS/电子罗盘组合导航姿态测量方法。

背景技术

GPS载波相位能够被利用计算出非常精确的位置、速度信息，但是其连续定位能力差、动态性能低，特别是在高楼区、山区、隧道、立交桥等恶劣的环境下，卫星信号容易受到遮挡导致失效，从而使GPS不能进行定位；微惯导系统具有独立自主工作的优点，但其导航误差随时间积累。上述单一的导航系统由于自身存在的不足--GPS载波相位不能够提供载体的姿态而微惯导系统虽能够提供载体的姿态但其误差随时间积累，无法提供高精度的载体姿态的要求；而电子罗盘以其具有完全自主、结构简单、启动速度快、并能提供姿态值等优点被常选作为一种姿态测量的辅助设备，因此本发明是将上述的三种导航测量系统组合起来使用，以提供高精度的载体姿态。

在利用微惯导/GPS载波相位/电子罗盘组合进行确定姿态的研究，目前已有的文章，如上海交通大学的硕士论文《组合式车载导航系统研究》是将微惯导、GPS、电子罗盘三种测量方式分成两组，即微惯导/GPS和微惯导/电子罗盘，进行组合测量，然后建立联合卡尔曼滤波器进行导航测量，且微惯导/电子罗盘的组合仅仅利用了微惯导和电子罗盘提供的姿态之间的差，没有得到姿态差与失准角之间的关系。而中北大学的硕士论文《MIMU/GPS/电子罗盘组合导航系统关键技术研究》虽将微惯导、GPS、电子罗盘三者进行了直接的组合并利用得到的姿态差与失准角之间的关系建立了电子罗盘部分的观测方程，但是GPS部分的观测方程却是位置、速度之差，因此得到姿态精度不高。本发明不仅将三种测量方式直接进行组合，并利用姿态差与失准角之间的关系建立了电子罗盘部分的观测方程，GPS部分的观测方程是利用GPS接收机的原始数据进行站际星际双差得到的载波相位误差和多普勒速度误差，因此能够得到高精度的姿态值。

发明内容

本发明的目的在于提供一种能提高导航姿态精度的微惯导与DGPS和电子罗盘组合导航姿态测量方法。

本发明的目的是这样实现的：

步骤1、利用微惯导、电子罗盘对组合系统进行初始对准，得到载体坐标系b到导航坐标系n的初始姿态矩阵

$C_b^n\left(0\right)=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

其中，Ci_j是姿态矩阵

的元素，i,j=1,2,3；

步骤2、根据步骤1中的初始姿态矩阵计算出载体的初始姿态值

步骤3、利用微惯导系统的位置误差方程、速度误差方程、平台失准角方程及惯性传感器误差方程构成扩展卡尔曼滤波器的状态方程，其状态变量为15维：

其中，δL、δλ、δh分别为SINS解算得到的经度、纬度、高度误差，δv_E、δv_N、δv_U分别为SINS解算得到的东向、北向、天向速度误差，分别为SINS解算得到的东、北、天三个方向的平台失准角，ε_E、ε_N、ε_U分别为东、北、天向陀螺漂移，

分别为东、北、天向加速度计零偏；

步骤4、利用电子罗盘输出的姿态γ_E、θ_E、ψ_E和微惯导系统输出的姿态γ_M、θ_M、ψ_M之间的差值δγ、δθ、δψ与平台失准角

之间的关系建立电子罗盘部分的观测方程，即为

$Z_1=\left[\begin{array}{c}{\mathrm{\γ}}_M-{\mathrm{\γ}}_E\\ {\mathrm{\θ}}_M-{\mathrm{\θ}}_E\\ {\mathrm{\ψ}}_M-{\mathrm{\ψ}}_E\end{array}\right]=\left.\begin{array}{c}{\mathrm{\δ}}_{\mathrm{\γ}}\\ {\mathrm{\δ}}_{\mathrm{\θ}}\\ {\mathrm{\δ}}_{\mathrm{\ψ}}\end{array}\right]=H_1\mathrm{\δX}+w_1$

其中，γ_E、θ_E、ψ_E对应电子罗盘输出的东向、北向、天向姿态；γ_M、θ_M、ψ_M对应微惯导输出的东向、北向、天向姿态；w₁对应电子罗盘部分的观测噪声矩阵，H₁对应电子罗盘部分的观测矩阵、其表达形式为：

$H_1=\left[\begin{array}{ccccc}{0}_{1\×6}& \frac{\sin {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& -\frac{\cos {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& 0& 0_{1\×6}\\ 0_{1\×6}& -\cos {\mathrm{\ψ}}_E& -{\sin \mathrm{\ψ}}_E& 0& 0_{1\×6}\\ 0_{1\×6}& -\frac{\sin {\mathrm{\θ}}_E\sin {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& \frac{\sin {\mathrm{\θ}}_E\cos {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& -1& 0_{1\×6}\end{array}\right];$

步骤5、利用GPS接收机在t₁、t₂时刻接收到的l卫星和m卫星的载波相位信号，在地球坐标系下对其进行站际、星际双差过程，得到单位向量差q；

步骤6、根据步骤5得到GPS部分的观测方程：

Z₂=H₂δX+w₂

其中，w₂对应GPS部分的观测噪声矩阵，δX表示的是步骤3中扩展卡尔曼滤波器状态方程的15维状态变量，H₂对应的是观测转换矩阵，即

$H_2=\left[\begin{array}{cccc}{0}_{1\×3}& 0_{1\×3}& q{C}_n^e& 0_{1\×6}\\ 0_{1\×3}& q{C}_n^e& 0_{1\×3}& 0_{1\×6}\end{array}\right]$

其中，表示的是由微惯导解算中得到的导航坐标系到地球坐标系的转换矩阵；

步骤7、根据步骤4得到的电子罗盘部分的观测方程和步骤6得到的GPS部分的观测方程建立组合系统的扩展卡尔曼滤波器观测方程为：

$\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]\mathrm{\δX}+\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right];$

步骤8、利用步骤3和步骤7提供的状态方程和观测方程构成扩展卡尔曼滤波器，实时估测出微惯导系统失准角

步骤9、利用步骤8估测出的微惯导系统失准角修正步骤1中微惯导系统的姿态矩阵，得到修正后的姿态矩阵，再利用步骤2中的计算方法实时地计算出微惯导系统的姿态值。

本发明是一种由微惯导系统提供滤波器的状态方程，电子罗盘的姿态值与GPS的站际、星际双差组成的滤波器观测方程，然后利用扩展卡尔曼滤波器进行滤波实时地估测出载体失准角，然后进行实时地修正载体姿态的测量方法。

本发明的微惯导/DGPS/电子罗盘组合导航姿态测量方法，是由微惯导提供滤波器的状态方程，滤波器的观测方程则由GPS和电子罗盘两部分构成，利用扩展卡尔曼滤波器进行实时估测微惯导系统失准角，利用得到的平台失准角修正姿态矩阵，并计算出微惯导系统新的姿态值。在算法上将微惯导/DGPS/电子罗盘组合，可以有效提高导航姿态精度。

附图说明

图1为本发明的流程图。

具体实施方式

下面结合图1对本发明做更详细地描述：

步骤1、通过对微惯导系统进行初始对准，得到载体坐标系b到导航坐标系n的初始姿态矩阵 $C_b^n\left(0\right):$

$C_b^n\left(0\right)=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

其中，Ci_j是姿态矩阵

的元素，i,j=1,2,3。

步骤2、根据步骤1中的初始姿态矩阵计算出载体的初始姿态值；

首先计算

的主值公式为：

则载体的姿态值为：

步骤3、利用微惯导系统的位置误差、速度误差、平台失准角及惯性传感器误差的方程构成扩展卡尔曼滤波器的状态方程，其状态变量为15维：

其中，δL、δλ、δh分别为SINS解算得到的经度、纬度、高度误差，δv_E、δv_N、δv_U分别为SINS解算得到的东向、北向、天向速度误差，分别为SINS解算得到的东、北、天三个方向的平台失准角，ε_E、ε_N、ε_U分别为东、北、天向陀螺漂移，

分别为东、北、天向加速度计零偏。

步骤4、利用电子罗盘输出的姿态γ_E、θ_E、ψ_E和微惯导系统输出的姿态γ_M、θ_M、ψ_M之间的差值δγ、δθ、δψ与平台失准角

之间的关系建立电子罗盘部分的观测方程，即为

$Z_1=\left[\begin{array}{c}{\mathrm{\γ}}_M-{\mathrm{\γ}}_E\\ {\mathrm{\θ}}_M-{\mathrm{\θ}}_E\\ {\mathrm{\ψ}}_M-{\mathrm{\ψ}}_E\end{array}\right]=\left.\begin{array}{c}{\mathrm{\δ}}_{\mathrm{\γ}}\\ {\mathrm{\δ}}_{\mathrm{\θ}}\\ {\mathrm{\δ}}_{\mathrm{\ψ}}\end{array}\right]=H_1\mathrm{\δX}+w_1$

其中，γ_E、θ_E、ψ_E对应电子罗盘输出的东向、北向、天向姿态；γ_M、θ_M、ψ_M对应微惯导输出的东向、北向、天向姿态；w₁为电子罗盘部分的观测噪声矩阵，H₁为电子罗盘部分的观测矩阵，其表达形式为：

$H_1=\left[\begin{array}{ccccc}{0}_{1\×6}& \frac{\sin {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& -\frac{\cos {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& 0& 0_{1\×6}\\ 0_{1\×6}& -\cos {\mathrm{\ψ}}_E& -{\sin \mathrm{\ψ}}_E& 0& 0_{1\×6}\\ 0_{1\×6}& -\frac{\sin {\mathrm{\θ}}_E\sin {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& \frac{\sin {\mathrm{\θ}}_E\cos {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& -1& 0_{1\×6}\end{array}\right]$

步骤5、利用GPS接收机在t₁、t₂时刻接收到的l卫星和m卫星的载波相位信号，在地球坐标系下对其进行站际、星际双差过程中，得到单位向量差q。

步骤6、根据步骤5得到GPS部分的观测方程：

Z₂=H₂δX+w₂

其中，w₂对应GPS部分的观测噪声矩阵，δX表示的是步骤3中扩展卡尔曼滤波器状态方程的15维状态变量，H₂对应的是观测转换矩阵，即

$H_2=\left[\begin{array}{cccc}{0}_{1\×3}& 0_{1\×3}& q{C}_n^e& 0_{1\×6}\\ 0_{1\×3}& q{C}_n^e& 0_{1\×3}& 0_{1\×6}\end{array}\right]$

其中，

表示的是由微惯导解算中得到的导航坐标系到地球坐标系的转换矩阵。

步骤7、根据步骤4得到的电子罗盘部分的观测方程和步骤6得到的GPS部分的观测方程建立组合系统的扩展卡尔曼滤波器观测方程为：

$\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]\mathrm{\δX}+\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right]$

步骤8、利用步骤3和步骤7提供的状态方程和观测方程构成扩展卡尔曼滤波器，实时估测出微惯导系统失准角

步骤9、利用步骤8估测出的微惯导系统平台失准角修正步骤1中微惯导系统的姿态矩阵，得到修正后的姿态矩阵，再利用步骤2中的计算方法就可以实时地计算出微惯导系统的姿态值。

姿态矩阵修正方法：

其中，

表示第k次微惯导系统解算得到的姿态矩阵，

表示的是第k次修正得到的姿态矩阵；

表示的是步骤6中第k次估测出微惯导系统的平台失准角

的反对称矩阵。

本发明还包括如下的技术特征：

1、在步骤3中，扩展卡尔曼滤波器的状态方程为：

$\mathrm{\δ}\stackrel{\·}{X}=\mathrm{A\δX}+\mathrm{BW}$

其中，δX表示15维状态变量，A是15×15维的一步转移矩阵，B为15×15维单位阵，W为系统噪声，为15×1维的白噪声序列，

一步转移阵A的形式如下：

$A=\left[\begin{array}{ccccc}{A}_1& A_2& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ A_3& A_4& A_5& 0_{3\×3}& C_{b3\×3}^n\\ A_6& A_7& A_8& C_{b3\×3}^n& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}\end{array}\right]$

其中

$A_1=\left[\begin{array}{ccc}0& 0& \frac{v_N}{{(r+h)}^2}\\ \frac{v_E}{r+h}\tan L\sec L& 0& -\frac{v_E}{{(r+h)}^2}\\ 0& 0& 0\end{array}\sec L\right],A_2=\left[\begin{array}{ccc}0& \frac{1}{(r+h)}& .0\\ \frac{\sec L}{r+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$A_3=\left[\begin{array}{ccc}(2\mathrm{\Ω}\cos {\mathrm{Lv}}_N+2\mathrm{\Ω}\sin {\mathrm{Lv}}_U+\frac{v_E{v}_N}{r+h}{\sec }^{2}L)& 0& \frac{v_E{v}_U-v_E{v}_N\tan L}{{(r+h)}^2}\\ -(2\mathrm{\Ω}\cos {\mathrm{Lv}}_E+\frac{v_E^2}{r+h}{\sec }^{2}L)& 0& \frac{v_N{v}_U+v_E^2\tan L}{{(r+h)}^2}\\ -2\mathrm{\Ω}\sin {\mathrm{Lv}}_E& 0& -\frac{v_E^2+v_N^2}{{(r+h)}^2}\end{array}\right]$

$A_4=\left[\begin{array}{ccc}(\frac{v_N}{r+h}\tan L-\frac{v_U}{r+h})& (2\mathrm{\Ω}\sin L+\frac{v_E}{r+h}\tan L)& -(2\mathrm{\Ω}\cos L+\frac{v_E}{r+h})\\ -(2\mathrm{\Ω}\sin L+\frac{{2v}_E}{r+h}\tan L)& -\frac{v_U}{r+h}& -\frac{v_N}{r+h}\\ (2\mathrm{\Ω}\cos L+\frac{v_E}{r+h}\tan L)& \frac{{2v}_N}{r+h}& 0\end{array}\right]$

$A_5=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right],A_6=\left[\begin{array}{ccc}0& 0& \frac{v_N}{{(r+h)}^2}\\ -\mathrm{\Ω}\sin L& 0& -\frac{v_E}{{(r+h)}^2}\\ (\mathrm{\Ω}\cos L+\frac{v_E}{r+h}{\sec }^{2}L)& 0& -\frac{v_E\tan L}{{(r+h)}^2}\end{array}\right],A_7\left[\begin{array}{ccc}0& -\frac{1}{r+h}& 0\\ \frac{1}{r+h}& 0& 0\\ \frac{\tan L}{r+h}& 0& 0\end{array}\right]$

$A_8=\left[\begin{array}{ccc}0& (\mathrm{\Ω}\sin L+\frac{v_E\tan L}{r+h})& -(\mathrm{\Ω}\cos L+\frac{v_E}{r+h})\\ -(\mathrm{\Ω}\sin L+\frac{v_E\tan L}{r+h})& 0& -\frac{v_N}{r+h}\\ (\mathrm{\Ω}\cos L+\frac{v_E}{r+h})& \frac{v_N}{r+h}& 0\end{array}\right]$

其中，f_E、f_N、f_U分别是东、北、天向的加速度计的输出比力；v_E,v_N,v_U分别是载体东向、北向、天向的速度，地球自转角速率Ω=7.2921158×10^-5rad/s，地球半径r=6378393m，h为载体的高度。

2、在步骤4中电子罗盘输出的姿态γ_E、θ_E、ψ_E和微惯导系统输出的姿态γ_M、θ_M、ψ_M之间的差值δγ、δθ、δψ可以描述为：

$\left[\begin{array}{c}{\mathrm{\γ}}_M\\ {\mathrm{\θ}}_M\\ {\mathrm{\ψ}}_M\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ\γ}+{\mathrm{\γ}}_E\\ \mathrm{\δ\θ}+{\mathrm{\θ}}_E\\ \mathrm{\δ\ψ}+{\mathrm{\ψ}}_E\end{array}\right]$

微矩阵可写为：

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}\\ \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

其中，

表示载体坐标系（b系）到导航坐标系（n系）的姿态矩阵，γ、θ、ψ对应b系相对于n系的东北天向姿态角。

在微惯性系统的解算过程中，导航坐标系（n系）和平台坐标系（p系）之间存在着误差，即为平台失准角，其向量形式为：

那么n系到p系转换矩阵

为

根据坐标系的变换理论可知：

$C_b^p=C_n^p{C}_b^n$

其中，

$C_b^p=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_M\cos {\mathrm{\ψ}}_M-\sin {\mathrm{\γ}}_M\sin {\mathrm{\θ}}_M\sin {\mathrm{\ψ}}_M& -\cos {\mathrm{\θ}}_M\sin {\mathrm{\ψ}}_M& \sin {\mathrm{\γ}}_M\cos {\mathrm{\ψ}}_M+\cos {\mathrm{\γ}}_M\sin {\mathrm{\θ}}_M\sin {\mathrm{\ψ}}_M\\ \cos {\mathrm{\γ}}_M\sin {\mathrm{\ψ}}_M+\sin {\mathrm{\γ}}_M\sin {\mathrm{\θ}}_M\cos {\mathrm{\ψ}}_M& \cos {\mathrm{\θ}}_M\cos {\mathrm{\ψ}}_M& \sin {\mathrm{\γ}}_M\sin {\mathrm{\ψ}}_M-\cos {\mathrm{\γ}}_M\sin {\mathrm{\θ}}_M\cos {\mathrm{\ψ}}_M\\ -\sin {\mathrm{\γ}}_M\cos {\mathrm{\θ}}_M& \sin {\mathrm{\θ}}_M& \cos {\mathrm{\γ}}_M\cos {\mathrm{\θ}}_M\end{array}\right]$

根据三角函数的积化和差公式，并考虑到姿态角误差δγ,δθ,δψ和平台失准角

都是小量，忽略其二阶量，可以得到姿态角误差和平台失准角之间的变换关系：

那么就可以得到步骤4中利用电子罗盘输出的姿态γ_E、θ_E、ψ_E和微惯导系统输出的姿态γ_M、θ_M、ψ_M之间的差值δγ、δθ、δψ与平台失准角

之间的关系建立电子罗盘部分的观测方程。

3、所述单位向量差q的计算方法为：

步骤1、计算GPS卫星位置到GPS接收机位置（由微惯导系统解算得到）的单位向量u，计算公式如下：

$\left[\begin{array}{ccc}\frac{-(X-x)}{R}& \frac{-(Y-y)}{R}& \frac{-(Z-z)}{R}\end{array}\right]$

其中，(XYZ)表示是广播星历给出的GPS卫星在地球坐标下的位置坐标；(xyz)表示微惯导系统解算得到的GPS接收机在地球坐标系下的位置坐标；R表示GPS卫星位置到GPS接收机位置的距离：

$R=\sqrt{{(X-x)}^2+{(Y-y)}^2+{(Z-z)}^2};$

步骤2、计算l卫星和m卫星的位置到GPS接收机位置的单位向量之间的差值u^(lm)，

u^(lm)=u^l-u^m

其中，u^l表示卫星l到GPS接收机位置的单位向量；u^m表示卫星m到GPS接收机位置的单位向量；

步骤3、计算^u(lm)在t₁、t₂两时刻之间的差值

$u_{t_{12}}^{\mathrm{lm}}=u_{t_1}^{\mathrm{lm}}-u_{t_2}^{\mathrm{lm}}$

其中，

表示t₁时刻u^(lm)的值；

表示t₂时刻u^(lm)的值；

得到单位向量差

本发明说明书中未作详细描述的内容属于本领域专业技术人员所公知的现有技术。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (2)

1.一种微惯导与DGPS和电子罗盘组合导航姿态测量方法，其特征是：

步骤1、利用微惯导、电子罗盘对组合系统进行初始对准，得到载体坐标系b到导航坐标系n的初始姿态矩阵

$C_b^n\left(0\right)=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

其中，Ci_j是姿态矩阵

的元素，i,j=1,2,3；

步骤2、根据步骤1中的初始姿态矩阵

计算出载体的初始姿态值

步骤3、利用微惯导系统的位置误差方程、速度误差方程、平台失准角方程及惯性传感器误差方程构成扩展卡尔曼滤波器的状态方程，其状态变量为15维：

其中，δL、δλ、δh分别为SINS解算得到的经度、纬度、高度误差，δv_E、δv_N、δv_U分别为SINS解算得到的东向、北向、天向速度误差，

分别为SINS解算得到的东、北、天三个方向的平台失准角，ε_E、ε_N、ε_U分别为东、北、天向陀螺漂移，

分别为东、北、天向加速度计零偏；

步骤4、利用电子罗盘输出的姿态γ_E、θ_E、ψ_E和微惯导系统输出的姿态γ_M、θ_M、ψ_M之间的差值δγ、δθ、δψ与平台失准角

之间的关系建立电子罗盘部分的观测方程，即为

$Z_1=\left[\begin{array}{c}{\mathrm{\γ}}_M-{\mathrm{\γ}}_E\\ {\mathrm{\θ}}_M-{\mathrm{\θ}}_E\\ {\mathrm{\ψ}}_M-{\mathrm{\ψ}}_E\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δ}}_{\mathrm{\γ}}\\ {\mathrm{\δ}}_{\mathrm{\θ}}\\ {\mathrm{\δ}}_{\mathrm{\ψ}}\end{array}\right]=H_1\mathrm{\δX}+w_1$

其中，γ_E、θ_E、ψ_E对应电子罗盘输出的东向、北向、天向姿态；γ_M、θ_M、ψ_M对应微惯导输出的东向、北向、天向姿态；w₁对应电子罗盘部分的观测噪声矩阵，H₁对应电子罗盘部分的观测矩阵、其表达形式为：

$H_1=\left[\begin{array}{ccccc}{0}_{1\×6}& \frac{\sin {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& -\frac{\cos {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& 0& 0_{1\×6}\\ 0_{1\×6}& -\cos {\mathrm{\ψ}}_E& -{\sin \mathrm{\ψ}}_E& 0& 0_{1\×6}\\ 0_{1\×6}& -\frac{\sin {\mathrm{\θ}}_E\sin {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& \frac{\sin {\mathrm{\θ}}_E\cos {\mathrm{\ψ}}_E}{\cos {\mathrm{\θ}}_E}& -1& 0_{1\×6}\end{array}\right];$

步骤5、利用GPS接收机在t₁、t₂时刻接收到的l卫星和m卫星的载波相位信号，在地球坐标系下对其进行站际、星际双差过程，得到单位向量差q；

步骤6、根据步骤5得到GPS部分的观测方程：

Z₂=H₂δX+w₂

其中，w₂对应GPS部分的观测噪声矩阵，δX表示的是步骤3中扩展卡尔曼滤波器状态方程的15维状态变量，H₂对应的是观测转换矩阵，即

$H_2=\left[\begin{array}{cccc}{0}_{1\×3}& 0_{1\×3}& q{C}_n^e& 0_{1\×6}\\ 0_{1\×3}& q{C}_n^e& 0_{1\×3}& 0_{1\×6}\end{array}\right]$

其中，

表示的是由微惯导解算中得到的导航坐标系到地球坐标系的转换矩阵；

步骤7、根据步骤4得到的电子罗盘部分的观测方程和步骤6得到的GPS部分的观测方程建立组合系统的扩展卡尔曼滤波器观测方程为：

$\left[\begin{array}{c}{Z}_1\\ Z_2\end{array}\right]=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]\mathrm{\δX}+\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right];$

步骤8、利用步骤3和步骤7提供的状态方程和观测方程构成扩展卡尔曼滤波器，实时估测出微惯导系统失准角

步骤9、利用步骤8估测出的微惯导系统失准角修正步骤1中微惯导系统的姿态矩阵，得到修正后的姿态矩阵，再利用步骤2中的计算方法实时地计算出微惯导系统的姿态值。

2.根据权利要求1所述的微惯导与DGPS和电子罗盘组合导航姿态测量方法，其特征是所述单位向量差q的计算方法为：

步骤1、计算GPS卫星位置到GPS接收机位置的单位向量u，计算公式如下：

$\left[\begin{array}{ccc}\frac{-(X-x)}{R}& \frac{-(Y-y)}{R}& \frac{-(Z-z)}{R}\end{array}\right]$

其中，(XYZ)表示是广播星历给出的GPS卫星在地球坐标下的位置坐标；(xyz)表示微惯导系统解算得到的GPS接收机在地球坐标系下的位置坐标；R表示GPS卫星位置到GPS接收机位置的距离：

$R=\sqrt{{(X-x)}^2+{(Y-y)}^2+{(Z-z)}^2};$

步骤2、计算l卫星和m卫星的位置到GPS接收机位置的单位向量之间的差值u^(lm)，

u^(lm)=u^l-u^m

其中，u^l表示卫星l到GPS接收机位置的单位向量；u^m表示卫星m到GPS接收机位置的单位向量；

步骤3、计算u^(lm)在t₁、t₂两时刻之间的差值

$u_{t_{12}}^{\mathrm{lm}}=u_{t_1}^{\mathrm{lm}}-u_{t_2}^{\mathrm{lm}}$

其中，表示t₁时刻u^(lm)的值；

表示t₂时刻u^(lm)的值；

得到单位向量差

Images