CN102589552A - 低成本组合导航系统的数据融合方法和装置 - Google Patents

Abstract

本发明涉及一种低成本组合导航系统的数据融合方法和装置。该方法主要包括：根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψ_G；根据设置的载体航向最优化估计性能指标，将所述ψ_I、ψ_m和ψ_G按照一定的权重进行叠加，得到所述载体航向的最优估计值。本发明实施例在估计方差最小的最优性能指标约束下，能根据载体的动态特性自动调节估计系数，给出载体运动过程中的最优航向估计，具有较强的自适应能力、鲁棒性好，方便工程实际应用。

Description

低成本组合导航系统的数据融合方法和装置

技术领域

本发明涉及惯性导航领域，尤其涉及一种低成本组合导航系统的数据融合方法和装置。

背景技术

低成本组合导航系统通常由MEMS(Micro-Electro-Mechanical Systems，微机电系统)器件构成，其位置和速度精度由GPS(Global Positioning System，全球定位系统)接收机保证，航向和姿态由MIMU(Miniature Inertial MeasurementUnit，微惯性测量单元)提供。

目前，GPS接收机通用芯片基本能保证位置10m、速度0.1m/s的精度，满足低成本导航需求。航向和姿态对于车载导航系统、小型无人机的飞行控制系统等低成本组合导航系统是重要的参数。而MIMU的精度低、易发散的特性导致姿态和航向的估计成为低成本组合导航系统急需解决的难题。

由于低成本组合导航系统中的MEMS陀螺的精度较低，无法敏感到地球自转角速度，导致低成本组合导航系统的捷联惯导解算初始对准时的航向无估计精度，姿态解算时航向误差不断累积，通过捷联惯导解算得到的低成本组合导航系统的航向和姿态数据在动态情况下精度不高。

【发明内容】

本发明的实施例提供了一种低成本组合导航系统的数据融合方法和装置，以实现载体运动过程中的最优航向估计，满足低成本组合导航系统中航向、姿态的工程应用需求。

一种低成本组合导航系统的数据融合方法，包括：

根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m 、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψ_G；

根据设置的载体航向最优化估计性能指标，将所述ψ_I、ψ_m和ψ_G按照一定的权重进行叠加，得到所述载体航向的最优估计值。

一种低成本组合导航系统中的数据融合装置，包括：

载体MEMS惯性器件航向计算模块，用于根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψ_G；

载体最终航向计算模块，用于根据设置的载体的航向最优化估计性能指标，将所述ψ_I、ψ_m和ψ_G按照一定的权重进行叠加，得到所述载体的航向最优估计值。

由上述本发明的实施例提供的技术方案可以看出，本发明实施例在估计方差最小的最优性能指标约束下，能根据载体的动态特性自动调节估计系数，给出载体运动过程中的最优航向估计，具有较强的自适应能力、鲁棒性好，方便工程实际应用。

【附图说明】

图1为本发明实施例一提供的一种低成本组合导航系统的结构图；

图2为本发明实施例一提供的一种低成本组合导航系统的数据融合方法的原理示意图；

图3为本发明实施例一提供的一种低成本组合导航系统的数据融合方法的具体处理流程图；

图4为本发明实施例一提供的一种捷联惯导解算的原理示意图；

图5为本发明实施例一提供的一种载体的航向最优估计的处理流程图；

图6为本发明实施例一提供的一种低成本组合导航系统中的数据融合装置的具体结构图。

【具体实施方式】

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图，以对本发明实施例进行清楚、完整地描述人员。

实施例一

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图，以对本发明实施对脉冲信号进行甄别的方法和装置例进行清楚、完整地描述人员。

实施例一

本发明实施例提供的低成本组合导航系统的结构如图1所示，主要由MIMU、MEMS磁阻传感器、单天线卫星接收机和导航计算机等构成。导航计算机完成多传感器的数据采集、信号处理、捷联惯导解算、数据融合(包括航向最优估计和组合Kalman滤波)等处理。上述卫星接收机可以为GPS卫星接收机或者北斗导航系统的卫星接收机。

基于上述低成本组合导航系统，该实施例提供的一种低成本组合导航系统的数据融合方法的原理示意图如图2所示，具体处理流程如图3所示，包括如下的处理步骤：

步骤31、导航计算机对MIMU、MEMS磁阻传感器的输出信号进行滤波处理。

导航计算机接收MIMU、MEMS磁阻传感器、单天线GPS卫星接收机的输出信号，完成数据采集。上述MIMU、MEMS磁阻传感器等MEMS器件的精度低、输出噪声大，为了降低MEMS器件误差对低成本组合导航系统导航精度的影响，导航计算机需要对上述MIMU、MEMS磁阻传感器的输出信号进行实时滤波，通过设计合理的数字滤波器，可以达到去除噪声、平滑信号的目的。

步骤32、导航计算机根据MIMU的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态。

捷联惯导解算前需要初始对准，即求得载体的初始姿态，初始对准代表了惯性导航的最高精度。对于自主导航系统通常采用MIMU的输出进行自对准，自对准的姿态误差如下：

$\mathrm{\δ\θ}=\frac{\mathrm{\δ}{f}_E}{g}$

$\mathrm{\δ\γ}=-\frac{\mathrm{\δ}{f}_N}{g}$

$\mathrm{\δ\ψ}=\frac{{\mathrm{\ϵ}}_E}{\mathrm{\Ω}\cos L}$

其中，

为载体的水平姿态误差、δψ为航向误差、δf_E、δf_N分别为东向和北向的加速度计偏值、g为地球重力加速度，ε_E为等效东向陀螺漂移、Ω为地球自转角速度、L为载体所在地的纬度。

由上述公式可知，水平姿态误差

与加速度计偏值(δf_E，δf_N)成正比，而航向误差δψ与ε_E、Ω和L有关。加速度计1mg的偏值可以产生0.057°的水平姿态误差，通常MIMU加速度计的偏值可以达到200μg～10mg，则水平姿态误差小于0.6°。而MEMS陀螺的漂移通常为10～100°/h，无法感测地球的自转角速度，航向没有估计精度，因此，捷联惯性导航解算时的航向容易发散。但当载体水平速度变化时，通过捷联惯性导航解算得到的载体的航向ψ_I还是能收敛的，ψ_I的误差δψ_I的计算公式如下：

$\mathrm{\δ}{\mathrm{\ψ}}_I\≈-\frac{\mathrm{\δ}{\stackrel{\·}{V}}_N-\mathrm{\δ}{f}_N-f_U\mathrm{\δ\θ}}{f_E}$ 或 $\mathrm{\δ}{\mathrm{\ψ}}_I\≈\frac{\mathrm{\δ}{\stackrel{\·}{V}}_E-\mathrm{\δ}{f}_E+f_U\mathrm{\δ\γ}}{f_N}$

其中，

为东、北向的速度变化量，δf_E，δf_N为东、北向的加速度变化量，f_N，f_E，f_U为东、北、天向的加速度，

为载体水平姿态误差。

上述捷联惯导解算的原理示意图如图4所示，具体处理过程包括：

(1)载体姿态解算

捷联惯导系统中，姿态是以载体系(b系)到导航系(n系)的方向余弦矩阵

表示的。方向余弦阵

是由下式微分方程解算得到的

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\Ω}}_{\mathrm{nb}}^b$

式中，是载体系相对导航系的角速度

的斜对称矩阵，

由下式得到：

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

其中，

为陀螺检测的载体角速度，

为方向余弦阵的转置，

分别为地球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：

${\mathrm{\ω}}_{\mathrm{ie}}^n={[0,\mathrm{\Ω}\cos L,\mathrm{\Ω}\sin L]}^T,$ ${\mathrm{\ω}}_{\mathrm{en}}^n={[-\frac{v_N}{R+h},-\frac{v_E}{R+h},-\frac{v_N}{R+h}\tan L]}^T,$

其中，Ω为地球自转角速度，L为地理纬度，v_E，v_N为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度。

求得方向余弦阵

后可以由下式计算载体的姿态，该姿态包括：俯仰角横滚角γ、航向角ψ。

$\mathrm{\θ}=-a\sin C_b^n\left(\mathrm{3,2}\right)$

$\mathrm{\γ}=-a\tan \frac{C_b^n\left(\mathrm{3,1}\right)}{C_b^n\left(\mathrm{3,3}\right)}$

$\mathrm{\ψ}=-a\tan \frac{C_b^n\left(\mathrm{1,2}\right)}{C_b^n\left(\mathrm{2,2}\right)}$

其中，

为方向余弦阵的第i行第j列分量。

上述航向角ψ即为载体的航向。

(2)速度解算

载体速度V(v_E，v_N，v_U)的解算公式如下：

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

其中，f^b为加速度计检测的载体加速度，gⁿ＝[0，g，0]^T，g为重力加速度。

(3)位置解算

载体的位置信息(纬度L、经度λ、高度h)的解算公式如下：

$\stackrel{\·}{L}=\frac{v_N}{R+h}$

$\stackrel{\·}{\mathrm{\λ}}=\frac{v_E}{(R+h)\cos L}$

$\stackrel{\·}{h}=v_U$

其中，v_E，v_N，v_U为载体速度在东向、北向、天向的分量，R为地球半径、h为载体距地面的高度。

步骤33、根据MEMS磁阻传感器的测量值和惯导水平姿态求解载体的航向。

采用东北天地理坐标系，右前上载体坐标系(即载体系正交的三个轴分别指向载体的右方、前方、上方)，载体磁航向的解算方法如下：

将磁阻传感器测量的载体坐标系中的磁场

转换到地理坐标系中的磁场H(H_x，H_y，H_z)。转换公式如下：

$\left[\begin{array}{c}{H}_x\\ H_y\\ H_z\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_x^b\\ H_y^b\\ H_z^b\end{array}\right]$

其中，

γ为上述捷联惯性导航解算得到的载体水平姿态角。H_x，H_y，H_z为磁场在相应坐标系中的x，y，z轴分量。

利用最佳椭圆补偿算法，校正磁场测量值H(H_x，H_y，H_z)得到载体当地的水平磁场分量(X，Y)，校正公式如下：。

X＝H_xX_sf+X_off

Y＝H_yY_sf+Y_off

其中，X_sf、Y_sf为磁场分量校正的标度系数，X_off、Y_off为磁场强度的偏移量。磁补偿的模型参数，需要在系统标定时按照椭圆补偿方法的操作方法确定。

根据载体当地的水平磁场分量及磁偏角计算载体的真北航向ψ_m，ψ_m的计算公式如下：

ψ_m＝arctan(Y/X)-θ

其中，θ为载体当地的磁偏角。

步骤34、根据GPS卫星接收机输出的速度信息求解载体的航向。

ψ_G＝arctan(V_E/V_N)

其中，V(V_E，V_N，V_U)为GPS卫星接收机输出的载体速度，V_E，V_N，V_U分别为东向、北向和天向的载体速度分量。ψ_G为载体的速度航向，当载体协调运动时，速度航向与载体航向一致。

步骤35、基于载体动态特性的载体的航向最优估计。

载体的航向最优估计的处理流程如图4所示，包括以下的处理过程：

首先，分析载体速度航向与姿态航向一致的运动特征，引起载体航向变化的因素有两个：一是沿速度水平投影切线方向的力，二是沿竖直方向的力矩。因此，航向变化时MIMU的加速度计或陀螺均会有输出变化。

对于车载体，其航向变化是由车轮与地面的摩擦力提供的向心加速度引起，只要车轮不存在侧滑，则车载体的航向与速度的航向是一致的。

对于微型无人机系统，其基准运动为等速直线平飞状态，协调飞行(侧滑角为零)时，速度航向与载体航向一致。

计算各传感器解算的航向误差；

1：通过GPS接收机解算的载体航向估计误差δψ_G为

$\mathrm{\δ}{\mathrm{\ψ}}_G=\frac{V_N\mathrm{\δ}{V}_E-V_E\mathrm{\δ}{V}_N}{V_E^2+V_N^2}$

上式中，V_E，V_N，为载体速度的东向和北向分量，δV_E，δV_N为相应的速度误差。

由上式可以看出，载体水平面内的速度越大，则速度航向误差越小。反之，当载体静止时，GPS航向没有估计精度。

当载体协调运动时，可以利用速度航向表示姿态航向。对于车载系统(除车轮侧滑的特殊情况)、微型无人机的直线平飞状态均可以利用速度航向估计载体的姿态航向。

2：载体转弯时通过惯导计算的载体航向ψ_I的估计误差δψ_I

$\mathrm{\δ}{\mathrm{\ψ}}_I\≈-\frac{\mathrm{\δ}{\stackrel{\·}{V}}_N-\mathrm{\δ}{f}_N-f_U\mathrm{\δ\θ}}{f_E}$ 或 $\mathrm{\δ}{\mathrm{\ψ}}_I\≈\frac{\mathrm{\δ}{\stackrel{\·}{V}}_E-\mathrm{\δ}{f}_E+f_U\mathrm{\δ\γ}}{f_N}$

当载体航向转弯时，会产生水平方向的向心加速度(f_E或f_N)、引起水平方向速度的变化，这时惯导解算的航向是收敛的。并且通常由于转弯时间较短惯导解算的航向误差较小。

3：通过磁阻传感器计算载体航向的估计误差。

磁场航向的测量误差主要来源于载体的磁场干扰和动态干扰：通常磁场干扰可以通过最佳椭圆补偿方法校正；而低成本磁阻传感器的动态误差较大。因此，本发明实施例将磁阻传感器的航向误差δψ_m设为载体动态的函数：

δψ_m≈m₁f+m₂ω

其中，f、ω为MIMU的比力和角速度输出，m₁、m₂由磁阻传感器的性能确定。

当载体静止或匀速直线运动时，磁航向的误差较小、可信度较高。

本发明实施例基于最优性能指标估计载体的航向，由GPS卫星接收机、惯导、磁阻传感器信息估计载体航向的方法如下：

ψ＝k₁ψ_G+k₂ψ_I+k₃ψ_m

其中，k₁、k₂、k₃为加权系数k₁+k₂+k₃＝1，系数的求解方法如下：

基于最优控制的思想，设定载体的航向最优估计性能指标J为航向估计误差δψ的方差δ²ψ最小，即

$J=\underset{K\⋐R}{\min }{\mathrm{\δ}}^2\mathrm{\ψ}$

在

约束条件下，可以求得满足性能指标J(即方差最小)的加权系数K(k₁，k₂，k₃)，求解公式如下：

$k_1=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_2=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_3=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

式中，δ²ψ_I、δ²ψ_G、δ²ψ_m分别为惯导、GPS、磁估计航向误差的方差。

根据前面的分析可知：当载体直线运动速度较大时GPS的航向误差较小，则航向估计中k₁较大；载体动态转弯时，惯导解算的航向误差较小，则k₂较大；而载体静止或匀速运动时磁航向误差较小，则k₃较大。因此，在估计方差最小的最优性能指标约束下，该航向估计方法能根据载体的动态特性自动调节估计系数，给出特定运动过程中的最优航向估计，具有较强的自适应能力、鲁棒性好，方便工程实际应用。

步骤36、通过Kalman(卡曼)组合滤波，解算载体的位置、速度、姿态等最优估计信息。

Kalman组合滤波是将惯导、GPS接收机、磁阻传感器的信息进行融合，解算步骤如下：

1：根据惯导系统的误差传播模型建立系统的状态方程；

状态向量取惯导解算的载体位置误差δP(δL，δλ，δh)、速度误差δV、平台误差角φ(φ_E，φ_N，φ_U)、陀螺漂移ε(ε_x，ε_y，ε_z)、加速度计漂移

即 $X\left(t\right)=[{\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},\mathrm{\δ}{V}_E,\mathrm{\δ}{V}_N{V}_U,{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\▿}_x,{\▿}_y,{\▿}_z]}^T,$ 确定系统的状态方程如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

其中，F(t)为状态转移矩阵，可以由惯导误差传播方差得到，W(t)为零均值高斯白噪声，W(t)的协方差阵由状态初始值方差、陀螺、加速度计的性能指标确定。

转移矩阵F(t)的各分量如下：

$F\left(\mathrm{1,3}\right)=\frac{v_N}{{(R+h)}^2},$ $F\left(\mathrm{1,5}\right)=\frac{1}{R+h},$

$F\left(\mathrm{2,1}\right)=\frac{v_E\tan L}{(R+h)\mathrm{coL}},$ $F_S\left(\mathrm{2,3}\right)=-\frac{v_E\sec L}{{(R+h)}^2},$ $F\left(\mathrm{2,4}\right)=\frac{1}{R+h}\sec L$

F(3，6)＝1

$F\left(\mathrm{4,1}\right)=2\mathrm{\Ω}(v_U\sin L+v_N\cos L)+\frac{v_E{v}_N}{R+h}{\sec }^{2}L,$ $F\left(\mathrm{4,3}\right)=\frac{v_N{v}_U+v_E^2\tan L}{{(R+h)}^2}$ $F\left(\mathrm{4,4}\right)=\frac{v_N\tan L-v_U}{R+h}$

$F\left(\mathrm{4,5}\right)=2\mathrm{\Ω}\sin L+\frac{v_E\tan L}{R+h}$ $F\left(\mathrm{4,6}\right)=2\mathrm{\Ω}\cos L+\frac{v_E}{R+h}$ F(4，8)＝-f_U   F(4，9)＝f_N

$F\left(\mathrm{5,1}\right)=-2\mathrm{\Ω}{v}_E\cos L-\frac{v_E^2}{R+h}{\sec }^{2}L$ $F\left(\mathrm{5,3}\right)=\frac{v_N{v}_U+v_E^2\tan L}{{(R+h)}^2}$ $F\left(\mathrm{5,4}\right)=-2(\mathrm{\Ω}\sin L+\frac{v_E\tan L}{R+h})$

$F\left(\mathrm{5,5}\right)=-\frac{v_U}{R+h}$ $F\left(\mathrm{5,6}\right)=-\frac{v_N}{R+h}$ F(5，7)＝f_U    F(5，9)＝-f_E   F(6，1)＝-2Ωv_EsinL

$F\left(\mathrm{6,3}\right)=\frac{v_E^2+v_N^2}{{(R+h)}^2}$ $F\left(\mathrm{6,4}\right)=2\left(\text{\ΩcosL+}\frac{v_E}{R+h}\right)$ $F\left(\mathrm{6,5}\right)=\frac{2v_N}{R+h}$ F(6，7)＝-f_N F(6，8)＝f_E

$F\left(\mathrm{7,3}\right)=\frac{v_N}{{(R+h)}^2},$ $F\left(\mathrm{7,5}\right)=-\frac{1}{R+h}$ $F\left(\mathrm{7,8}\right)=\mathrm{\Ω}\sin L+\frac{v_E\tan L}{R+h},$ $F\left(\mathrm{7,8}\right)=-\mathrm{\Ω}\cos L-\frac{v_E}{R+h}$

F(8，1)＝-ΩsinL， $F\left(\mathrm{8,3}\right)=-\frac{v_E}{{(R+h)}^2},$ $F\left(\mathrm{8,4}\right)=\frac{1}{R+h},$

$F\left(\mathrm{8,7}\right)=-\mathrm{\Ω}\sin L+\frac{v_E}{R+h}\tan L,$ $F\left(\mathrm{8,9}\right)=-\frac{v_N}{R+h}$

$F\left(\mathrm{9,1}\right)=\mathrm{\Ω}\cos L+\frac{v_E}{R+h}{\sec }^{2}L,$ $F\left(\mathrm{9,3}\right)=-\frac{v_E\tan L}{{(R+h)}^2},$ $F\left(\mathrm{9,4}\right)=-\frac{1}{R+h}\tan L$

$F\left(\mathrm{9,7}\right)=\mathrm{\Ω}\cos L+\frac{v_E}{R+h},$ $F\left(\mathrm{9,8}\right)=\frac{v_N}{R+h}$

$\left[\begin{array}{ccc}F\left(\mathrm{4,13}\right)& F\left(\mathrm{4,14}\right)& F\left(\mathrm{4,15}\right)\\ F\left(\mathrm{5,13}\right)& F\left(\mathrm{5,14}\right)& F\left(\mathrm{5,15}\right)\\ F\left(\mathrm{6,13}\right)& F\left(\mathrm{6,14}\right)& F\left(\mathrm{6,15}\right)\end{array}\right]=\left[\begin{array}{ccc}F\left(\mathrm{7,13}\right)& F\left(\mathrm{7,14}\right)& F\left(\mathrm{7,15}\right)\\ F\left(\mathrm{8,13}\right)& F\left(\mathrm{8,14}\right)& F\left(\mathrm{8,15}\right)\\ F\left(\mathrm{9,13}\right)& F\left(\mathrm{9,14}\right)& F\left(\mathrm{9,15}\right)\end{array}\right]=C_b^n$

上述表达式中，L为纬度，λ为经度，h为高度，v_E，v_N，v_U为载体速度在东向、北向、天向的分量，f_E，f_N，f_U为载体加速度在东向、北向、天向的分量，R为地球半径，Ω为地球自转角速度，

为载体的方向余弦阵。除上述分量外，矩阵F(t)的其余分量均为0。

2：建立量测方程；

系统量测值取GPS接收机与惯导解算的载体位置误差δP(δL，δλ，δh)、速度误差δV、航向的最优估计ψ与惯导解算航向ψ_I的差值δψ，即Z(t)＝[δL，δλ，δh，δV_E，δV_N，δV_U，δψ]^T，则测量方程如下：

Z(t)＝H(t)X(t)+V(t)

测量矩阵H(t)为：

$H\left(t\right)=\left[\begin{array}{ccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中，V(t)的元素为互不相关的零均值高斯白噪声，V(t)的协方差阵由GPS接收机的性能指标确定。

3：离散化系统方程；

设离散型系统的方程为

X(k+1)＝Φ(k+1，k)X(k)+W(k)

Z(k)＝H(k)X(k)+V(k)

其中，E[W(k)]＝E[V(k)]＝0，E[W(k)W^T(j)]＝Q_kδ_kj，E[V(k)V^T(j)]＝R_kδ_kj，E[W(k)V^T(j)]＝0。上式中，初始状态X(0)的统计特性为：

E[X(0)]＝m₀

E{[X(0)-m₀][X(0)-m₀]^T}＝P₀

4：Kalman滤波估计载体的位置、速度、姿态信息

Kalman滤波是利用递推的方式由系统的初始状态X(0)(位置、速度、姿态误差等)估计新状态

的过程。递推估计的步骤如下：

①由当前状态

预测下一步状态

为

$\stackrel{^^}{X}(k+1,k)=\mathrm{\Φ}(k+1,k)X\left(k\right)$

②步骤①预测的协方差P(k+1，k)为P(k+1，k)＝Φ(k+1，k)p(k)Φ^T(k+1，k)+Q_k

③由估计协方差计算滤波增益K(k+1)为K(k+1)＝P(k+1，k)H^T(k+1)[H(k+1)P(k+1，k)H^T(k+1)+R_k+1]^-1

④新状态

的最优估值为

$\stackrel{^^^}{X}(k+1)=X(k+1,k)+K(k+1)[Z(k+1)-H(k+1)X(k+1,k)]$

⑤新状态估计的均方误差P(k+1)为P(k+1)＝[I-K(k+1)H(k+1)]P(k+1，k)

迭代循环步骤①～⑤，则由系统初始状态X(0)、初始方差P₀、各个时刻的观测量Z(k)，系统误差矩阵Q_k、观测噪声矩阵R_k，可以递推得到最终的新状态

(即载体的位置、速度、姿态等误差)。

根据上述Kalman组合滤波计算得到的误差量对前面捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。

实施例二

该实施例提供了一种低成本组合导航系统中的数据融合装置，该数据融合装置设置在所述图1中的导航计算机中，其具体结构如图6所示，包括如下的模块：

MEMS惯性器件航向计算模块61，用于根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψ_G；

载体最终航向计算模块62，用于根据设置的载体的航向最优化估计性能指标，将所述ψ_I、ψ_m和ψ_G按照一定的权重进行叠加，得到所述载体航向的最优估计值。

具体的，所述的MEMS惯性器件航向计算模块61，还用于在捷联惯导系统中，姿态是以载体系到导航系的方向余弦矩阵

表示的，方向余弦阵

是由下式微分方程解算得到的

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\Ω}}_{\mathrm{nb}}^b$

式中，

是载体系相对导航系的角速度的斜对称矩阵，

由下式得到：

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

其中，

为陀螺检测的载体角速度，

为方向余弦阵

的转置，

分别为地球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：

${\mathrm{\ω}}_{\mathrm{ie}}^n={[0,\mathrm{\Ω}\cos L,\mathrm{\Ω}\sin L]}^T,$ ${\mathrm{\ω}}_{\mathrm{en}}^n={[-\frac{v_N}{R+h},-\frac{v_E}{R+h},-\frac{v_N}{R+h}\tan L]}^T,$

其中，Ω为地球自转角速度，L为地理纬度，v_E，v_N为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度。

求得方向余弦阵

后，可以由下式转换为载体的姿态，该姿态包括：俯仰角

横滚角γ、航向角ψ；

$\mathrm{\θ}=-a\sin C_b^n\left(\mathrm{3,2}\right)$

$\mathrm{\γ}=-a\tan \frac{C_b^n\left(\mathrm{3,1}\right)}{C_b^n\left(\mathrm{3,3}\right)}$

$\mathrm{\ψ}=-a\tan \frac{C_b^n\left(\mathrm{1,2}\right)}{C_b^n\left(\mathrm{2,2}\right)}$

其中，

为方向余弦阵

的第i行第j列分量，上述航向角ψ为根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到的载体航向ψ_I；

还用于将磁阻传感器的测量值从载体坐标系

转换到地理坐标系H(H_x，H_y，H_z)转换公式如下：

$\left[\begin{array}{c}{H}_x\\ H_y\\ H_z\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_x^b\\ H_y^b\\ H_z^b\end{array}\right]$

其中，

γ为上述捷联惯性导航解算得到的载体的水平姿态角，H_x，H_y，H_z为磁场在相应坐标系中的x，y，z轴分量；

利用最佳椭圆补偿算法，校正磁场测量值H(H_x，H_y，H_z)得到载体当地的水平磁场分量(X，Y)，校正公式如下：

X＝H_xX_sf+X_off

Y＝H_yY_sf+Y_off

其中，X_sf、Y_sf为标度系数，X_off、Y_off为载体当地的磁场强度的偏移量，

根据载体当地的水平磁场分量及磁偏角计算载体的航向ψ_m，ψ_m的计算公式如下：

ψ_m＝arctan(Y/X)-θ

其中，θ为载体当地的磁偏角；

所述ψ_m即为根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向；

还用于设V(V_E，V_N，V_U)为卫星接收机输出的载体速度，V_E，V_N，V_U分别为东向、北向和天向的载体速度分量。则由载体速度解算的速度航向ψ_G为

ψ_G＝arctan(V_E/V_N)

当载体协调运动时，载体的航向与载体的速度航向一致，所述ψ_G为根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向。

具体的，所述的载体最终航向计算模块62，还用于获取所述ψ_I、ψ_m和ψ_G的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；

所述载体的航向的最优估计值ψ的计算方法如下：

ψ＝k₁ψ_G+k₂ψ_I+k₃ψ_m

其中，k₁、k₂、k₃为加权系数，

在

约束条件下，求得满足所述载体的航向最优估计性能指标的加权系数K(k₁，k₂，k₃)。

具体的，所述的载体最终航向计算模块62，还用于计算出所述ψ_I的估计误差δψ_I为：

$\mathrm{\δ}{\mathrm{\ψ}}_I\≈-\frac{\mathrm{\δ}{\stackrel{\·}{V}}_N-\mathrm{\δ}{f}_N-f_U\mathrm{\δ\β}}{f_E}$ 或 $\mathrm{\δ}{\mathrm{\ψ}}_I\≈\frac{\mathrm{\δ}{\stackrel{\·}{V}}_E-\mathrm{\δ}{f}_E+f_U\mathrm{\δ\α}}{f_N}$

其中，

为东、北向的速度变化量，δf_E，δf_N为东、北向的加速度变化量，f_N，f_E，f_U为东、北、天向的加速度，为载体水平姿态误差；

计算出所述ψ_m的估计误差δψ_m为：

δψ_m≈m₁f+m₂ω

其中，f、ω为微惯性测量单元MIMU的比力和角速度输出，m₁、m₂为根据磁阻传感器的性能确定的参数；

计算所述ψ_G的估计误差δψ_G为：

$\mathrm{\δ}{\mathrm{\ψ}}_G=\frac{V_N\mathrm{\δ}{V}_E-V_E\mathrm{\δ}{V}_N}{V_E^2+V_N^2}$

上式中，V_E，V_N，为载体速度的东向和北向分量，δV_E，δV_N为相应的速度误差；

设定载体的航向最优估计性能指标为航向的估计误差δψ的方差δ²ψ最小，即

$J=\underset{K\⋐R}{\min }{\mathrm{\δ}}^2\mathrm{\ψ}$

在

约束条件下，可以求得满足所述航向最优估计性能指标J的加权系数K(k₁，k₂，k₃)，求解公式如下：

$k_1=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_2=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_3=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

式中，δ²ψ_I、δ²ψ_G、δ²ψ_m分别为惯导、GPS、磁估计航向误差的方差。

具体的，所述的装置还可以包括：

载体速度、位置和姿态计算模块63，用于采集所述微惯性测量单元、磁阻传感器、卫星接收机的输出信号，并对采集到的数据进行滤波处理；

根据所述微惯性测量单元的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态；

通过卡曼Kalman组合滤波过程计算得到的误差量，对所述捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。

应用本发明实施例的装置进行数据融合的具体处理过程与前述方法实施例类似，此处不再赘述。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random Access Memory，RAM)等。

综上所述，本发明实施例在航向估计方差最小的最优性能指标约束下，能根据载体的动态特性自动调节估计系数，给出载体运动过程中的最优航向估计，具有较强的自适应能力、鲁棒性好，方便工程实际应用。

本发明实施例分析引起磁场干扰的外界因素，将磁航向误差设置为载体动态参数的函数，既符合工程实际应用，又避免了通常对磁信号干扰程度的模糊判断，方便工程应用。

本发明实施例将多传感器的航向误差统一为载体动态特性的函数，基于最优控制理论的思想，将最小方差的最优估计性能指标引入到载体的航向估计中。将航向误差作为观测量引入传统Kalman滤波后，可以加快滤波的收敛。

本发明实施例适用于低精度要求的低成本MEMS组合导航系统，对器件的精度要求不高。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (10)

1.一种低成本组合导航系统的数据融合方法，其特征在于，包括：

根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψ_G；

根据设置的载体的航向最优化估计性能指标，将所述ψ_I、ψ_m和ψ_G按照一定的权重进行叠加，得到所述载体航向的最优估计值。

2.根据权利要求1所述的低成本组合导航系统的数据融合方法，其特征在于：

所述的根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I，包括：

在捷联惯导系统中，姿态是以载体系到导航系的方向余弦矩阵

表示的。方向余弦阵是由下式微分方程解算得到的

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\Ω}}_{\mathrm{nb}}^b$

式中，

是载体系相对导航系的角速度

的斜对称矩阵，

由下式得到：

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

其中，

为陀螺检测的载体角速度，

为方向余弦阵

的转置，

分别为地球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：

${\mathrm{\ω}}_{\mathrm{ie}}^n={[0,\mathrm{\Ω}\cos L,\mathrm{\Ω}\sin L]}^T,$ ${\mathrm{\ω}}_{\mathrm{en}}^n={[-\frac{v_N}{R+h},-\frac{v_E}{R+h},-\frac{v_N}{R+h}\tan L]}^T,$

其中，Ω为地球自转角速度，L为地理纬度，v_E，v_N为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度。

求得方向余弦阵

后，可以由下式转换为载体的姿态，该姿态包括：俯仰角

横滚角γ、航向角ψ；

$\mathrm{\θ}=-a\sin C_b^n\left(\mathrm{3,2}\right)$

$\mathrm{\γ}=-a\tan \frac{C_b^n\left(\mathrm{3,1}\right)}{C_b^n\left(\mathrm{3,3}\right)}$

$\mathrm{\ψ}=-a\tan \frac{C_b^n\left(\mathrm{1,2}\right)}{C_b^n\left(\mathrm{2,2}\right)}$

其中，为方向余弦阵

的第i行第j列分量，上述航向角ψ即为载体的航向ψ_I；

所述的根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m，包括：

将磁阻传感器的测量值从载体坐标系

转换到水平坐标系上H(H_x，H_y，H_z)，转换公式如下：

$\left[\begin{array}{c}{H}_x\\ H_y\\ H_z\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_x^b\\ H_y^b\\ H_z^b\end{array}\right]$

其中，

γ为上述捷联惯性导航解算得到的载体水平姿态角，H_x，H_y，H_z为磁场在相应坐标系中的x，y，z轴分量；

利用最佳椭圆补偿算法，校正磁场测量值H(H_x，H_y，H_z)得到载体当地的水平磁场分量(X，Y)，校正公式如下：

X＝H_xX_sf+X_off

Y＝H_yY_sf+Y_off

其中，X_sf、Y_sf为标度系数，X_off、Y_off为载体当地的磁场强度的偏移量；

根据载体当地的水平磁场分量及磁偏角计算载体的航向ψ_m，ψ_m的计算公式如下：

ψ_m＝arctan(Y/X)-θ

其中，θ为载体当地的磁偏角；

所述的根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψ_G，包括：

设V(V_E，V_N，V_U)为卫星接收机输出的载体速度，V_E，V_N，V_U分别为东向、北向和天向的载体速度分量。则由载体速度解算的速度航向ψ_G为

ψ_G＝arctan(V_E/V_N)

当载体协调运动时，载体的航向与载体的速度航向一致。

3.根据权利要求1或2所述的低成本组合导航系统的数据融合方法，其特征在于，所述的根据设置的载体航向最优化估计性能指标，将所述ψ_I、ψ_m和ψ_G按照一定的权重进行叠加，得到所述载体航向的最优估计值，包括：

获取所述ψ_I、ψ_m和ψ_G的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；

所述载体航向的最优估计值ψ的计算方法如下：

ψ＝k₁ψ_G+k₂ψ_I+k₃ψ_m

其中，k₁、k₂、k₃为加权系数。

在

约束条件下，求得满足所述载体的航向最优估计性能指标的加权系数K(k₁，k₂，k₃)。

4.根据权利要求3所述的低成本组合导航系统的数据融合方法，其特征在于，所述的获取所述ψ_I、ψ_m和ψ_G的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小，包括：

计算出所述ψ_I的估计误差δψ_I为：

$\mathrm{\δ}{\mathrm{\ψ}}_I\≈-\frac{\mathrm{\δ}{\stackrel{\·}{V}}_N-\mathrm{\δ}{f}_N-f_U\mathrm{\δ\β}}{f_E}$ 或 $\mathrm{\δ}{\mathrm{\ψ}}_I\≈\frac{\mathrm{\δ}{\stackrel{\·}{V}}_E-\mathrm{\δ}{f}_E+f_U\mathrm{\δ\α}}{f_N}$

其中，

为东、北向的速度变化量，δf_E，δf_N为东、北向的加速度变化量，f_N，f_E，f_U为东、北、天向的加速度，

为载体水平姿态误差；

计算出所述ψ_m的估计误差δψ_m为：

δψ_m≈m₁f+m₂ω

其中，f、ω为微惯性测量单元MIMU的比力和角速度输出，m₁、m₂为根据磁阻传感器的性能确定的参数；

计算所述ψ_G的估计误差δψ_G为：

$\mathrm{\δ}{\mathrm{\ψ}}_G=\frac{V_N\mathrm{\δ}{V}_E-V_E\mathrm{\δ}{V}_N}{V_E^2+V_N^2}$

上式中，V_E，V_N，为载体速度的东向和北向分量，δV_E，δV_N为相应的速度误差；

设定载体的航向最优估计性能指标J为航向估计误差的方差δ²ψ最小，即

$J=\underset{K\⋐R}{\min }{\mathrm{\δ}}^2\mathrm{\ψ}$

在

约束条件下，可以求得满足所述航向最优估计性能指标J的加权系数K(k₁，k₂，k₃)，求解公式如下：

$k_1=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_2=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_3=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

式中，δ²ψ_I、δ²ψ_G、δ²ψ_m分别为惯导、GPS、磁估计航向误差的方差。

5.根据权利要求1所述的低成本组合导航系统的数据融合方法，其特征在于，所述的方法还包括：

组合导航系统中的导航计算机采集所述微惯性测量单元、磁阻传感器、卫星接收机的输出信号，并对采集的数据进行滤波处理；

所述导航计算机根据所述微惯性测量单元的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态；

通过卡曼Kalman组合滤波过程计算得到的修正值，对所述捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。

6.一种低成本组合导航系统中的数据融合装置，其特征在于，包括：

载体微机电系统MEMS惯性器件航向计算模块，用于根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψ_G；

载体最终航向计算模块，用于根据设置的载体的航向最优化估计性能指标，将所述ψ_I、ψ_m和ψ_G按照一定的权重进行叠加，得到所述载体的航向最优估计值。

7.根据权利要求6所述的低成本组合导航系统的数据融合装置，其特征在于：

所述的载体微机电系统MEMS惯性器件航向计算模块，还用于在捷联惯导系统中，姿态是以载体系到导航系的方向余弦矩阵

表示的，方向余弦阵是由下式微分方程解算得到的

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\Ω}}_{\mathrm{nb}}^b$

式中，

是载体系相对导航系的角速度

的斜对称矩阵，由下式得到：

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

其中，

为陀螺检测的载体角速度，

为方向余弦阵

的转置，

分别为地球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：

${\mathrm{\ω}}_{\mathrm{ie}}^n={[0,\mathrm{\Ω}\cos L,\mathrm{\Ω}\sin L]}^T,$ ${\mathrm{\ω}}_{\mathrm{en}}^n={[-\frac{v_N}{R+h},-\frac{v_E}{R+h},-\frac{v_N}{R+h}\tan L]}^T,$

其中，Ω为地球自转角速度，L为地理纬度，v_E，v_N为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度。

求得方向余弦阵

后，可以由下式转换为载体的姿态，该姿态包括：俯仰角

横滚角γ、航向角ψ；

$\mathrm{\θ}=-a\sin C_b^n\left(\mathrm{3,2}\right)$

$\mathrm{\γ}=-a\tan \frac{C_b^n\left(\mathrm{3,1}\right)}{C_b^n\left(\mathrm{3,3}\right)}$

$\mathrm{\ψ}=-a\tan \frac{C_b^n\left(\mathrm{1,2}\right)}{C_b^n\left(\mathrm{2,2}\right)}$

其中，

为方向余弦阵

的第i行第j列分量，上述航向角ψ为根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψ_I；

还用于将磁阻传感器的测量值从载体坐标系

转换到地理坐标系上H(H_x，H_y，H_z)，转换公式如下：

$\left[\begin{array}{c}{H}_x\\ H_y\\ H_z\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_x^b\\ H_y^b\\ H_z^b\end{array}\right]$

其中，

γ为上述捷联惯性导航解算得到的载体的水平姿态角，H_x，H_y，H_z为磁场在相应坐标系中的x，y，z轴分量；

利用最佳椭圆补偿算法，校正磁场测量值H(H_x，H_y，H_z)得到载体当地的水平磁场分量(X，Y)，校正公式如下：

X＝H_xX_sf+X_off

Y＝H_yY_sf+Y_off

其中，X_sf、Y_sf为标度系数，X_off、Y_off为载体当地的磁场强度的偏移量；

根据载体当地的水平磁场分量及磁偏角计算载体的航向ψ_m，ψ_m的计算公式如下：

ψ_m＝arctan(Y/X)-θ

其中，θ为载体当地的磁偏角；

所述ψ_m为根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψ_m；

还用于设V(V_E，V_N，V_U)为卫星接收机输出的载体速度，V_E，V_N，V_U分别为东向、北向和天向的载体速度分量。则由载体速度解算的速度航向ψ_G为

ψ_G＝arctan(V_E/V_N)

当载体协调运动时，载体的航向与载体的速度航向一致，所述ψ_G为根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向。

8.根据权利要求6或7所述的低成本组合导航系统的数据融合装置，其特征在于：

所述的载体最终航向计算模块，还用于获取所述ψ_I、ψ_m和ψ_G的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；

所述载体的航向的最优估计值ψ的计算方法如下：

ψ＝k₁ψ_G+k₂ψ_I+k₃ψ_m

其中，k₁、k₂、k₃为加权系数；

在约束条件下，求得满足所述载体的航向最优估计性能指标的加权系数K(k₁，k₂，k₃)。

9.根据权利要求8所述的低成本组合导航系统的数据融合装置，其特征在于：

所述的载体最终航向计算模块，还用于计算出所述ψ_I的估计误差δψ_I为：

$\mathrm{\δ}{\mathrm{\ψ}}_I\≈-\frac{\mathrm{\δ}{\stackrel{\·}{V}}_N-\mathrm{\δ}{f}_N-f_U\mathrm{\δ\β}}{f_E}$ 或 $\mathrm{\δ}{\mathrm{\ψ}}_I\≈\frac{\mathrm{\δ}{\stackrel{\·}{V}}_E-\mathrm{\δ}{f}_E+f_U\mathrm{\δ\α}}{f_N}$

其中，为东、北向的速度变化量，δf_E，δf_N为东、北向的加速度变化量，f_N，f_E，f_U为东、北、天向的加速度，为载体水平姿态误差；

计算出所述ψ_m的估计误差δψ_m为：

δψ_m≈m₁f+m₂ω

其中，f、ω为微惯性测量单元MIMU的比力和角速度输出，m₁、m₂为根据磁阻传感器的性能确定的参数；

计算所述ψ_G的估计误差δψ_G为：

$\mathrm{\δ}{\mathrm{\ψ}}_G=\frac{V_N\mathrm{\δ}{V}_E-V_E\mathrm{\δ}{V}_N}{V_E^2+V_N^2}$

上式中V_E，V_N，为载体速度的东向和北向分量，δV_E，δV_N为相应的速度误差；

设定载体的航向最优估计性能指标为航向的估计误差的方差最小，即

$J=\underset{K\⋐R}{\min }{\mathrm{\δ}}^2\mathrm{\ψ}$

在

约束条件下，可以求得满足所述航向最优估计性能指标J的加权系数K(k₁，k₂，k₃)，求解公式如下：

$k_1=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_2=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

$k_3=\frac{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G}{{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_G+{\mathrm{\δ}}^2{\mathrm{\ψ}}_I{\mathrm{\δ}}^2{\mathrm{\ψ}}_m+{\mathrm{\δ}}^2{\mathrm{\ψ}}_G{\mathrm{\δ}}^2{\mathrm{\ψ}}_m}$

式中，δ²ψ_I、δ²ψ_G、δ²ψ_m分别为惯导、GPS、磁估计航向误差的方差。

10.根据权利要求6所述的低成本组合导航系统的数据融合装置，其特征在于，所述的装置还包括：

载体速度、位置和姿态计算模块，用于采集所述微惯性测量单元、磁阻传感器、卫星接收机的输出信号，并对采集的数据进行滤波处理；

根据所述微惯性测量单元的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态；

通过卡曼Kalman组合滤波过程计算得到的修正值，对所述捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。

Images