CN102853837B - 一种mimu和gnss信息融合的方法 - Google Patents

Abstract

本发明公开了MIMU和GNSS信息融合的方法，首先基于MIMU角速度和比力数据，分别在ECEF坐标系和NED坐标系下进行惯性导航解算；然后基于ECEF坐标系和NED坐标系状态方程，分别进行滤波，以达到降低滤波器状态方程维数目的；最后综合ECEF坐标系和NED坐标系下导航输出，给出最终导航解。本发明具有原理简单、处理速度快、精度高、容错性强、适用范围广等优点。

Description

一种MIMU和GNSS信息融合的方法

技术领域

本发明主要涉及到导航系统领域，特指一种基于低成本、低精度MIMU的MIMU/GNSS信息融合的方法。

背景技术

众所周知，惯性导航系统以其独特的优点，在航空、航天和航海等领域得到了广泛的应用。例如，在微型飞行器、微型机器人等微小型设备等领域有着大量的应用，这也促使惯性导航系统向低成本、微型化、低功耗的方向发展。近几年来，由于MIMU/GNSS组合导航系统具有成本低、体积小、功耗低等优点，因而受到众多研究者的关注。MIMU(Micro-Electro-Mechanical-Systems based Inertial Measurement Unit)和GNSS(GlobalNavigation Satellite System)组合，构成MIMU/GNSS组合导航系统，两者之间可以克服各自缺点、取长补短，使二者性能得到互补。MIMU/GNSS组合导航系统以MIMU和GNSS输出的信息(如位置、速度、伪距或伪距率等)差值作为观测量，以MIMU线性化的误差方程作为系统方程，通过Kalman滤波器对MIMU的速度、位置、姿态角以及传感器的误差进行最优估计，并根据估计结果对MIMU进行输出或者反馈校正。

对于MIMU/GNSS信息融合的方法，现有技术中有许多研究人员进行了大量的理论研究，取得了很多的进展。目前，MIMU/GNSS组合导航信息融合方法主要采用单一滤波器对MIMU的位置、速度、姿态角以及传感器的误差进行最优估计。这种方法存在滤波器状态方程维数大、计算复杂等缺点。同时，由于MEMS惯性器件性能较差，且受温度影响大等特点，在组合导航滤波器设计时需要更多的考虑惯性器件误差影响，可能会造成滤波器发散。

目前，基于低成本、低精度MIMU的MIMU/GNSS信息融合方法均采用单一滤波器，由于器件误差耦合的影响，存在计算量大、精度不高、可用性较差等不足，难以满足航空制导设备、无人飞行器、机器人和地面车辆等日益迫切的高精度要求。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种原理简单、处理速度快、精度高、容错性强、适用范围广的MIMU和GNSS信息融合的方法。

为解决上述技术问题，本发明采用以下技术方案：

一种MIMU和GNSS信息融合的方法，包括以下步骤：

(1)基于MIMU角速度和比力数据，分别在ECEF坐标系和NED坐标系下进行惯性导航解算；将NED坐标系下惯性导航的姿态角直接赋给ECEF坐标系下的导航解算，并将GNSS接收机解算得到的位置、速度信息作为惯性导航解算初始状态的位置、速度；

(2)建立ECEF坐标系下的位置和速度误差方程，作为ECEF坐标系下的误差状态方程；建立NED坐标系下姿态角误差方程和速度误差方程，作为NED坐标系下的误差状态方程；

(3)利用GNSS伪距和伪距率值建立ECEF坐标系下信息融合的观测方程，利用GNSS北向速度和东向速度值建立NED坐标系下信息融合的观测方程；

(4)基于ECEF坐标系下的误差状态方程，采用卡尔曼滤波器进行位置误差、速度误差、加速度计偏置估计；基于NED坐标系下的误差状态方程，采用卡尔曼滤波器进行姿态角误差和陀螺零偏估计；

(5)对于ECEF坐标系下的惯性导航解算进行位置与速度信息的反馈校正，对于NED坐标系下的惯性导航解算进行姿态角与速度信息的反馈校正，根据估计陀螺零偏和加速度计偏置，分别对IMU输出的角速度和比力进行修正；

(6)由ECEF坐标系和NED坐标系下的惯性导航解算综合向用户输出载体的姿态角、位置、速度、角速度和加速度信息。

作为本发明的进一步改进：

所述步骤(2)中的ECEF坐标系下误差状态矢量为X^e＝[δR^e δV^e δa^b]^T，其中δR^e、δV^e分别为ECEF坐标系下的位置误差和速度误差矢量，δa^b为三个轴的加速度计零偏；NED坐标系下误差状态矢量Xⁿ＝[δVⁿ ε δω^b]₈，其中δVⁿ＝[δV_N δV_E]分别为北向、东向速度误差；ε为姿态角误差；δω^b为微陀螺偏差。

所述步骤(3)中ECEF坐标系下组合导航观测量采用伪距差分(星间差)和多普勒差分(星间差)来抵消接收机钟差和钟差漂移影响；NED坐标系下组合导航观测量为北向速度、东向速度。

所述步骤(4)中的卡尔曼滤波器为线性卡尔曼滤波器或扩展卡尔曼滤波器。

所述步骤(5)包括：

(5.1)对NED坐标系下姿态角对应的方向余弦阵修正为：

$C_b^n=[I+\mathrm{\ϵ}\×]{\hat{C}}_b^n$

其中，ε×为失准角矢量的反对称矩阵，为修正前的方向余弦阵，为修正后的方向余弦阵；

(5.2)NED坐标系下的速度修正为：

$V^n={\hat{V}}^n-\mathrm{\δ}{V}^n$

其中，为NED坐标系下修正前的速度矢量，Vⁿ为NED坐标系下修正后的速度矢量；

(5.3)ECEF坐标系下速度修正为：

$R^e={\hat{R}}^e-\mathrm{\δ}{R}^e$

其中，为修正前的位置矢量，R^e为修正后的位置矢量；

(5.4)ECEF坐标系下速度修正为：

$V^e={\hat{V}}^e-\mathrm{\δ}{V}^e$

其中，为ECEF坐标系下修正前的速度矢量，V^e为ECEF坐标系下修正后的速度矢量。

与现有技术相比，本发明的优点在于：

1、本发明的MIMU和GNSS信息融合的方法，具有计算简单、精度高、容错能力强和应用领域广的优点，可广泛应用于提高航空制导设备、无人飞行器、机器人和地面车辆等用MIMU/GNSS组合导航系统的导航精度。

2、本发明的MIMU和GNSS信息融合的方法针对低精度MIMU，基于误差分离原理，采用双滤波结构，使各种误差耦合误差分离；即，ECEF坐标系下导航滤波器用来估计位置、速度和加速度零偏，而NED坐标系下导航滤波器用来估计姿态角、陀螺零偏，从而提高滤波估计的精度。

3、本发明的MIMU和GNSS信息融合的方法，在ECEF坐标系下组合导航中，采用伪距单差和伪距率单差(星间单差)作为导航滤波器的观测量，这样可以抵消钟差和钟差漂移的影响，从而减少了导航滤波器的维数，提高了计算效率和导航定位的精度。

附图说明

图1是本发明方法的流程示意图。

图2为本发明方法在应用时的框架原理示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

如图1和图2所示，本发明的MIMU和GNSS信息融合的方法，其原理为：首先，基于MIMU角速度和比力数据，分别在ECEF坐标系和NED坐标系下进行惯性导航解算；然后，基于ECEF坐标系和NED坐标系状态方程，分别进行滤波，以达到降低滤波器状态方程维数目的；最后，综合ECEF坐标系和NED坐标系下导航输出，给出最终导航解。

结合具体应用实例，本发明的具体流程为：

1.采集MIMU的角速度和比力数据，分别在ECEF坐标系和NED坐标系下进行惯性导航解算，并且将NED坐标系下惯性导航的姿态角直接赋给ECEF坐标系下的导航解算，并将GNSS接收机解算得到的位置、速度信息作为惯性导航解算初始状态的位置、速度；

2.建立ECEF坐标系下的位置和速度误差方程，作为ECEF坐标系下的误差状态方程；建立NED坐标系下姿态角误差方程和速度误差方程，作为NED坐标系下的误差状态方程。

2.1、ECEF坐标系下误差状态矢量为X^e＝[δR^e δV^e δa^b]^T，其中，δR^e、δV^e分别为ECEF坐标系下的位置误差和速度误差矢量，δa^b为三个轴的加速度计零偏。

误差状态方程为下式(1)

${\stackrel{\·}{X}}^e=F^e{X}^e+G^e{W}^e---\left(\text{1}\right)$

其中：

$F^e=\left[\begin{array}{ccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}\\ N^e& 0_{3\×3}& C_b^e\\ 0_{3\×3}& 0_{3\×3}& -B\end{array}\right]$ 为系统矩阵；I_3×3为3×3的单位阵； $G^e=\left[\begin{array}{c}{0}_{3\×3}\\ 0_{3\×3}\\ I_{3\×3}\end{array}\right]$ 为噪声系数矩阵；

W^e＝[W_a]^T为加速度计噪声W_a向量；B＝diag(β_a，β_a，β_a)为对角阵，其元素为对应的随机过程相关时间倒数；为从载体系至ECEF坐标系的方向余弦阵；

$N^e=\frac{\mathrm{GM}}{r^3}\left[\begin{array}{ccc}-1+\frac{3x^3}{r^2}& \frac{3\mathrm{xy}}{r^2}& \frac{3\mathrm{xz}}{r^2}\\ \frac{3\mathrm{xy}}{r^2}& -1+\frac{{3y}^3}{r^2}& \frac{3\mathrm{yz}}{r^2}\\ \frac{3\mathrm{xz}}{r^2}& \frac{3\mathrm{yz}}{r^2}& -1+\frac{{3z}^3}{r^2}\end{array}\right]+\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{ie}}^2& 0& 0\\ 0& {\mathrm{\ω}}_{\mathrm{ie}}^2& 0\\ 0& 0& 0\end{array}\right],$ GM为万有引力常数与地球质量之积；ω_ie为地球自转角速度；R^e＝(x，y，z)为ECEF坐标系下的位置坐标。

2.2、NED坐标系下误差状态矢量Xⁿ＝[δVⁿ ε δω^b]₈，其中δVⁿ＝[δV_N δV_E]分别为北向和东向速度误差；ε为姿态角误差；δω^b为微陀螺偏差。

在北东地(NED)坐标系下的状态误差方程为下式(2)：

${\stackrel{\·}{X}}^n\left(t\right)=F^n{X}^n\left(t\right)+G^n{W}^n---\left(2\right)$

$F^n=\left[\begin{array}{cccccccc}{F}_{11}& F_{12}& 0& -f_D& f_E& 0& 0& 0\\ F_{21}& F_{22}& f_D& 0& f_N& 0& 0& 0\\ 0& F_{32}& 0& {\mathrm{\ω}}_D& -{\mathrm{\ω}}_E& c_{11}& c_{12}& c_{13}\\ F_{41}& 0& -{\mathrm{\ω}}_D& 0& {\mathrm{\ω}}_N& c_{21}& c_{22}& c_{23}\\ 0& F_{52}& {\mathrm{\ω}}_E& -{\mathrm{\ω}}_N& 0& c_{31}& c_{32}& c_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right],$ $G^n=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ -c_{11}& -c_{12}& -c_{13}\\ -c_{21}& -c_{22}& -c_{23}\\ -c_{31}& -c_{32}& -c_{33}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right],$ $W^n=\left[\begin{array}{c}{w}_{\mathrm{gx}}\\ w_{\mathrm{gy}}\\ w_{\mathrm{gz}}\end{array}\right]$

其中， $F_{11}=\frac{V_D}{R_m};$ $F_{12}=-2{\mathrm{\ω}}_{\mathrm{ie}}\sin \left(L\right)-\frac{2V_e\tan \left(L\right)}{R_n},$ F₁₂＝-F₂₁； $F_{22}=\frac{V_D}{R_m}+\frac{V_N\tan \left(L\right)}{R_n};$

$F_{32}=\frac{1}{R_n};$ $F_{41}=-\frac{1}{R_m};$ $F_{52}=\frac{\tan \left(L\right)}{R_n};$ ω_N＝ω_iecos(L)+V_E/R_n；ω_E＝-V_N/R_n；ω_D＝-ω_iesin(L)-V_Etan(L)/R_n；

ω_ie为地球自转的角速率；L为纬度；f_N，f_E，f_D为载体加速度沿北向、东向和垂直方向的分量；c₁₁，c₁₂，c₁₃，…为方向余弦矩阵的元素；w_gx，w_gy，w_gz为陀螺的噪声分量。

3.利用GNSS伪距和伪距率值建立ECEF坐标系下信息融合的观测方程，利用GNSS北向速度和东向速度值建立NED坐标系下信息融合的观测方程；

3.1、ECEF坐标系下组合导航观测量采用伪距差分和多普勒差分(星间差)来抵消接收机钟差和钟差漂移影响，降低滤波器的维数，设通道1观测的卫星为基准卫星，则基于卫星之间单差的观测方程表示为下式(3)：

$Z^e=\left[\begin{array}{c}{\mathrm{\δ\ρ}}^{j1}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^{j1}\end{array}\right]=\left[\begin{array}{ccccccccc}{e}_1^{21}& e_2^{21}& e_3^{21}& 0& 0& 0& 0& 0& 0\\ e_1^{31}& e_2^{31}& e_3^{31}& 0& 0& 0& 0& 0& 0\\ \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·\\ e_1^{n1}& e_2^{n1}& e_3^{n1}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& e_1^{21}& e_2^{21}& e_3^{21}& 0& 0& 0\\ 0& 0& 0& e_1^{31}& e_2^{31}& e_3^{31}& 0& 0& 0\\ \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·\\ 0& 0& 0& e_1^{n1}& e_2^{n1}& e_3^{n1}& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{R}}^e\\ \mathrm{\δ}{\stackrel{\·}{V}}^e\\ \mathrm{\δ}{\stackrel{\·}{a}}^b\end{array}\right]\left[\begin{array}{c}{\mathrm{\ϵ}}_{\mathrm{\ρ}}^{11}\\ {\mathrm{\ϵ}}_{\mathrm{\ρ}}^{21}\\ \·\\ \·\\ \·\\ {\mathrm{\ϵ}}_{\mathrm{\ρ}}^{n1}\\ {\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^{11}\\ {\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^{21}\\ \·\\ \·\\ \·\\ {\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^{n1}\end{array}\right]---\left(3\right)$

式中，δρ^j1＝δρ^j-δρ¹，n为跟踪卫星的数目；

$e_i^{j1}=e_i^j-e_i^1(i=\mathrm{1,2,3}),$ ${\mathrm{\ϵ}}_{\mathrm{\ρ}}^{j1}={\mathrm{\ϵ}}_{\mathrm{\ρ}}^j-{\mathrm{\ϵ}}_{\mathrm{\ρ}}^1,$ ${\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^{j1}={\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^j-{\mathrm{\ϵ}}_{\stackrel{\·}{\mathrm{\ρ}}}^1.$ 其中， $\mathrm{\δ}{\mathrm{\ρ}}^j={\mathrm{\ρ}}_c^j-{\widetilde{\mathrm{\ρ}}}^j,$ 为惯性导航得到第j号卫星伪距，为卫星接收机计算得到第j号卫星伪距， $e_3^j=\frac{z-z^j}{{\mathrm{\ρ}}^j},$ ${\mathrm{\ρ}}^j={[{(x-x^j)}^2+{(y-y^j)}^2+{(z-z^j)}^2]}^{\frac{1}{2}},$ 载体位置R^e＝(x，y，z)，j号卫星位置为(x^j，y^j，z^j)，为伪距测量噪声，为伪距率测量噪声。

3.2、NED坐标系下组合导航观测量为北向速度、东向速度，测量方程为下式(4)：

$Z^n=\left[\begin{array}{c}{\mathrm{\δV}}_N\\ \mathrm{\δ}{V}_E\end{array}\right]=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]X^n+v---\left(4\right)$

其中， 为MIMU导航解算的北向速度估计值，由GPS测量得到北向速度测量值； 为MIMU导航解算的东向速度估计值，由GPS测量得到东向速度测量值；v为速度观测噪声矢量。

4.基于ECEF坐标系下的误差状态方程，采用卡尔曼滤波器进行位置误差、速度误差、加速度计偏置估计，基于NED坐标系下的误差状态方程，采用卡尔曼滤波器进行姿态角误差和陀螺零偏估计；

基于ECEF坐标系下的误差状态方程和观测方程，基于NED坐标系下的误差状态方程和观测方程，分别进行卡尔曼滤波：

状态一步预测方程将简化为下式(5)：

X_K+1/K＝Φ_K+1，KX_K    (5)

预测误差的方差阵为下式(6)：

$P_{K+1/K}={\mathrm{\Φ}}_{K+1,K}{P}_{K/K}{\mathrm{\Φ}}_{K+1,K}^T+Q_K---\left(6\right)$

滤波增益为下式(7)：

$K_{K+1}=P_{K+1/K}{H}_{K+1}^T{(H_{K+1}{P}_{K+1/K}{H}_{K+1}^T+R_{K+1})}^{-1}---\left(7\right)$

状态的最佳估计为下式(8)：

X_K+1/K+1＝K_K+1·(Z_K+1-H_K+1X_K+1/K)    (8)

估计误差的方差阵为下式(9)：

P_K+1/K+1＝P_K+1/K-K_K+1H_K+1P_K+1/K    (9)

5.对于ECEF坐标系下的惯性导航解算进行位置与速度信息的反馈校正，对于NED坐标系下的惯性导航解算进行姿态角与速度信息的反馈校正，根据估计陀螺零偏和加速度计偏置，分别对IMU输出的角速度和比力进行修正。

在NED坐标系下姿态角对应的方向余弦阵修正为下式(10)：

$C_b^n=[I+\mathrm{\ϵ}\×]{\hat{C}}_b^n---\left(10\right)$

其中，ε×为失准角矢量的反对称矩阵，为修正前的方向余弦阵，为修正后的方向余弦阵；

NED坐标系下速度修正为下式(11)：

$V^n={\hat{V}}^n-\mathrm{\δ}{V}^n---\left(11\right)$

其中，为NED坐标系下修正前的速度矢量，Vⁿ为NED坐标系下修正后的速度矢量。ECEF坐标系下速度修正为下式(12)：

$R^e={\hat{R}}^e-\mathrm{\δ}{R}^e---\left(12\right)$

其中，为修正前的位置矢量，R^e为修正后的位置矢量；

ECEF坐标系下速度修正为下式(13)：

$V^e={\hat{V}}^e-\mathrm{\δ}{V}^e---\left(13\right)$

其中，为ECEF坐标系下修正前的速度矢量，V^e为ECEF坐标系下修正后的速度矢量。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (4)

1.一种MIMU和GNSS信息融合的方法，其特征在于，包括以下步骤：

(1)基于MIMU角速度和比力数据，分别在ECEF坐标系和NED坐标系下进行惯性导航解算；将NED坐标系下惯性导航的姿态角直接赋给ECEF坐标系下的导航解算，并将GNSS接收机解算得到的位置、速度信息作为惯性导航解算初始状态的位置、速度；

(2)建立ECEF坐标系下的位置和速度误差方程，作为ECEF坐标系下的误差状态方程；建立NED坐标系下姿态角误差方程和速度误差方程，作为NED坐标系下的误差状态方程；

(3)利用GNSS伪距和伪距率值建立ECEF坐标系下信息融合的观测方程，利用GNSS北向速度和东向速度值建立NED坐标系下信息融合的观测方程；

(4)基于ECEF坐标系下的误差状态方程，采用卡尔曼滤波器进行位置误差、速度误差、加速度计偏置估计；基于NED坐标系下的误差状态方程，采用卡尔曼滤波器进行姿态角误差和陀螺零偏估计；

(5)对于ECEF坐标系下的惯性导航解算进行位置与速度信息的反馈校正，对于NED坐标系下的惯性导航解算进行姿态角与速度信息的反馈校正，根据估计陀螺零偏和加速度计偏置，分别对IMU输出的角速度和比力进行修正；

所述步骤(5)包括：

(5.1)对NED坐标系下姿态角对应的方向余弦阵修正为：

$C_b^n=[I+\mathrm{\ϵ}\×]{\hat{C}}_b^n$

其中，ε×为失准角矢量的反对称矩阵，为修正前的方向余弦阵，为修正后的方向余弦阵；

(5.2)NED坐标系下的速度修正为：

$V^n={\hat{V}}^n-\mathrm{\δ}{V}^n$

其中，为NED坐标系下修正前的速度矢量，Vⁿ为NED坐标系下修正后的速度矢量；

(5.3)ECEF坐标系下速度修正为：

$R^e={\hat{R}}^e-\mathrm{\δ}{R}^e$

其中，为修正前的位置矢量，R^e为修正后的位置矢量；

(5.4)ECEF坐标系下速度修正为：

$V^e={\hat{V}}^e-\mathrm{\δ}{V}^e$

其中，为ECEF坐标系下修正前的速度矢量，V^e为ECEF坐标系下修正后的速度矢量；

(6)由ECEF坐标系和NED坐标系下的惯性导航解算综合向用户输出载体的姿态角、位置、速度、角速度和加速度信息。

2.根据权利要求1所述MIMU和GNSS信息融合的方法，其特征在于，所述步骤(2)中的ECEF坐标系下误差状态矢量为X^e＝[δR^e δV^e δa^b]^T，其中δR^e、δV^e分别为ECEF坐标系下的位置误差和速度误差矢量，δa^b为三个轴的加速度计零偏；NED坐标系下误差状态矢量Xⁿ＝[δVⁿ ε δω^b]₈，其中δVⁿ＝[δV_N δV_E]分别为北向、东向速度误差；ε为姿态角误差；δω^b为微陀螺偏差。

3.根据权利要求2所述MIMU和GNSS信息融合的方法，其特征在于，所述步骤(3)中ECEF坐标系下组合导航观测量采用伪距差分和多普勒差分来抵消接收机钟差和钟差漂移的影响；NED坐标系下组合导航观测量为北向速度、东向速度。

4.根据权利要求1或2或3所述MIMU和GNSS信息融合的方法，其特征在于，所述步骤(4)中的卡尔曼滤波器为线性卡尔曼滤波器或扩展卡尔曼滤波器。