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

摘要

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

主权项

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}{\gamma }_M-{\gamma }_E\\ {\theta }_M-{\theta }_E\\ {\psi }_M-{\psi }_E\end{array}\right]=\left[\begin{array}{c}{\delta }_{\gamma }\\ {\delta }_{\theta }\\ {\delta }_{\psi }\end{array}\right]=H_1\mathrm{\delta X}+w_1$其中，γ_E、θ_E、ψ_E对应电子罗盘输出的东向、北向、天向姿态；γ_M、θ_M、ψ_M对应微惯导输出的东向、北向、天向姿态；w₁对应电子罗盘部分的观测噪声矩阵，H₁对应电子罗盘部分的观测矩阵、其表达形式为：$H_1=\left[\begin{array}{ccccc}{0}_{1\times 6}& \frac{\sin {\psi }_E}{\cos {\theta }_E}& -\frac{\cos {\psi }_E}{\cos {\theta }_E}& 0& 0_{1\times 6}\\ 0_{1\times 6}& -\cos {\psi }_E& -{\sin \psi }_E& 0& 0_{1\times 6}\\ 0_{1\times 6}& -\frac{\sin {\theta }_E\sin {\psi }_E}{\cos {\theta }_E}& \frac{\sin {\theta }_E\cos {\psi }_E}{\cos {\theta }_E}& -1& 0_{1\times 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\times 3}& 0_{1\times 3}& q{C}_n^e& 0_{1\times 6}\\ 0_{1\times 3}& q{C}_n^e& 0_{1\times 3}& 0_{1\times 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{\delta X}+\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right];$步骤8、利用步骤3和步骤7提供的状态方程和观测方程构成扩展卡尔曼滤波器，实时估测出微惯导系统失准角步骤9、利用步骤8估测出的微惯导系统失准角修正步骤1中微惯导系统的姿态矩阵，得到修正后的姿态矩阵，再利用步骤2中的计算方法实时地计算出微惯导系统的姿态值。