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

1.一种低成本组合导航系统的数据融合方法，其特征在于，包括：
根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψI、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψm、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψG；
根据设置的载体的航向最优化估计性能指标，将所述ψI、ψm和ψG按照一定的权重进行叠加，得到所述载体航向的最优估计值；具体的：获取所述ψI、ψm和ψG的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；所述载体航向的最优估计值ψ的计算方法如下：
ψ＝k1ψG+k2ψI+k3ψm；
其中，k1、k2、k3为加权系数；
在 约束条件下，求得满足所述载体的航向最优估计性能指标的
加权系数K(k1,k2,k3)。
2.根据权利要求1所述的低成本组合导航系统的数据融合方法，其特征在于：
所述的根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψI，包括：
在捷联惯导系统中，姿态是以载体系到导航系的方向余弦矩阵 表示的；方向余弦阵是由下式微分方程解算得到的
式中， 是载体系相对导航系的角速度 的斜对称矩阵， 由下式得到：
其中， 为陀螺检测的载体角速度， 为方向余弦阵 的转置， 分别为地
球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：
其中，Ω为地球自转角速度，L为地理纬度，vE,vN为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度；
求得方向余弦阵 后，由下式转换为载体的姿态，该姿态包括：俯仰角 横滚角γ、航向角ψ；

其中， 为方向余弦阵 的第i行第j列分量，上述航向角ψ即为载体的航向ψI；
所述的根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψm，包括：
将磁阻传感器的测量值从载体坐标系 转换到水平坐标系上
H(Hx,Hy,Hz)，转换公式如下：
其中， γ分别为所述捷联惯导解算得到的俯仰角与横滚角，Hx,Hy,Hz为磁场在相应坐标系中的x,y,z轴分量；
利用最佳椭圆补偿算法，校正磁场测量值H(Hx,Hy,Hz)得到载体当地的水平磁场分量(X,Y)，校正公式如下：
X＝HxXsf+Xoff
Y＝HyYsf+Yoff
其中，Xsf、Ysf为标度系数，Xoff、Yoff为载体当地的磁场强度的偏移量；
根据载体当地的水平磁场分量及磁偏角计算载体的航向ψm，ψm的计算公式如下：
ψm＝arctan(Y/X)-θ
其中，θ为载体当地的磁偏角；
所述的根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψG，包括：
设V(VE,VN,VU)为卫星接收机输出的载体速度，VE,VN,VU分别为东向、北向和天向的载体速度分量；则由载体速度解算的速度航向ψG为
ψG＝arctan(VE/VN)
当载体协调运动时，载体的航向与载体的速度航向一致。
3.根据权利要求1所述的低成本组合导航系统的数据融合方法，其特征在于，所述的获取所述ψI、ψm和ψG的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小，包括：
计算出所述ψI的估计误差δψI为：
或
其中， 为东、北向的速度变化量，δfE,δfN为东、北向的加速度变化量，fN,fE,fU为东、北、天向的加速度， 为载体水平姿态误差；
计算出所述ψm的估计误差δψm为：
δψm≈m1f+m2ω
其中，f、ω为微惯性测量单元MIMU的比力和角速度输出，m1、m2为根据磁阻传感器的性能确定的参数；
计算所述ψG的估计误差δψG为：
上式中，VE,VN,为载体速度的东向和北向分量，δVE,δVN为相应的速度误差；
2
设定载体的航向最优估计性能指标J为航向估计误差的方差δψ最小，即在 约束条件下，可以求得满足所述航向最优估计性能指标J的加
权系数K(k1,k2,k3)，求解公式如下：
2 2 2
式中，δψI、δψG、δψm分别为惯导、GPS、磁估计航向误差的方差。
4.根据权利要求1所述的低成本组合导航系统的数据融合方法，其特征在于，所述的方法还包括：
组合导航系统中的导航计算机采集所述微惯性测量单元、磁阻传感器、卫星接收机的输出信号，并对采集的数据进行滤波处理；
所述导航计算机根据所述微惯性测量单元的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态；
通过卡曼Kalman组合滤波过程计算得到的修正值，对所述捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。
5.一种低成本组合导航系统中的数据融合装置，其特征在于，包括：
载体微机电系统MEMS惯性器件航向计算模块，用于根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψI、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψm、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψG；
载体最终航向计算模块，用于根据设置的载体的航向最优化估计性能指标，将所述ψI、ψm和ψG按照一定的权重进行叠加，得到所述载体的航向最优估计值；还用于获取所述ψI、ψm和ψG的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；所述载体的航向的最优估计值ψ的计算方法如下：
ψ＝k1ψG+k2ψI+k3ψm；
其中，k1、k2、k3为加权系数；
在 约束条件下，求得满足所述载体的航向最优估计性能指标的加
权系数K(k1,k2,k3)。
6.根据权利要求5所述的低成本组合导航系统中的数据融合装置，其特征在于：
所述的载体微机电系统MEMS惯性器件航向计算模块，还用于在捷联惯导系统中，姿态是以载体系到导航系的方向余弦矩阵 表示的，方向余弦阵 是由下式微分方程解算得到的
式中， 是载体系相对导航系的角速度 的斜对称矩阵， 由下式得到：
其中， 为陀螺检测的载体角速度， 为方向余弦阵 的转置， 分别为地球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：
其中，Ω为地球自转角速度，L为地理纬度，vE,vN为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度；
求得方向余弦阵 后，由下式转换为载体的姿态，该姿态包括：俯仰角 横滚角γ、航向角ψ；

其中， 为方向余弦阵 的第i行第j列分量，上述航向角ψ为根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψI；
还用于将磁阻传感器的测量值从载体坐标系 转换到地理坐标系上
H(Hx,Hy,Hz)，转换公式如下：
其中， γ分别为所述捷联惯导解算得到的俯仰角与横滚角，Hx,Hy,Hz为磁场在相应坐标系中的x,y,z轴分量；
利用最佳椭圆补偿算法，校正磁场测量值H(Hx,Hy,Hz)得到载体当地的水平磁场分量(X,Y)，校正公式如下：
X＝HxXsf+Xoff
Y＝HyYsf+Yoff
其中，Xsf、Ysf为标度系数，Xoff、Yoff为载体当地的磁场强度的偏移量；
根据载体当地的水平磁场分量及磁偏角计算载体的航向ψm，ψm的计算公式如下：
ψm＝arctan(Y/X)-θ
其中，θ为载体当地的磁偏角；
所述ψm为根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψm；
还用于设V(VE,VN,VU)为卫星接收机输出的载体速度，VE,VN,VU分别为东向、北向和天向的载体速度分量；则由载体速度解算的速度航向ψG为
ψG＝arctan(VE/VN)
当载体协调运动时，载体的航向与载体的速度航向一致，所述ψG为根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向。
7.根据权利要求5所述的低成本组合导航系统中的数据融合装置，其特征在于：
所述的载体最终航向计算模块，还用于计算出所述ψI的估计误差δψI为：
或
其中， 为东、北向的速度变化量，δfE,δfN为东、北向的加速度变化量，fN,fE,fU为东、北、天向的加速度， 为载体水平姿态误差；
计算出所述ψm的估计误差δψm为：
δψm≈m1f+m2ω
其中，f、ω为微惯性测量单元MIMU的比力和角速度输出，m1、m2为根据磁阻传感器的性能确定的参数；
计算所述ψG的估计误差δψG为：
上式中VE,VN,为载体速度的东向和北向分量，δVE,δVN为相应的速度误差；
设定载体的航向最优估计性能指标为航向的估计误差的方差最小，即
在 约束条件下，可以求得满足所述航向最优估计性能指标J的加
权系数K(k1,k2,k3)，求解公式如下：
2 2 2
式中，δψI、δψG、δψm分别为惯导、GPS、磁估计航向误差的方差。
8.根据权利要求5所述的低成本组合导航系统中的数据融合装置，其特征在于，所述的装置还包括：
载体速度、位置和姿态计算模块，用于采集所述微惯性测量单元、磁阻传感器、卫星接收机的输出信号，并对采集的数据进行滤波处理；
根据所述微惯性测量单元的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态；
通过卡曼Kalman组合滤波过程计算得到的修正值，对所述捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。

低成本组合导航系统的数据融合方法和装置 \n技术领域\n[0001] 本发明涉及惯性导航领域，尤其涉及一种低成本组合导航系统的数据融合方法和装置。\n背景技术\n[0002] 低成本组合导航系统通常由MEMS(Micro-Electro-Mechanical Systems，微机电系统)器件构成，其位置和速度精度由GPS(Global Positioning System，全球定位系统)接收机保证，航向和姿态由MIMU(Miniature Inertial Measurement Unit，微惯性测量单元)提供。\n[0003] 目前，GPS接收机通用芯片基本能保证位置10m、速度0.1m/s的精度，满足低成本导航需求。航向和姿态对于车载导航系统、小型无人机的飞行控制系统等低成本组合导航系统是重要的参数。而MIMU的精度低、易发散的特性导致姿态和航向的估计成为低成本组合导航系统急需解决的难题。\n[0004] 由于低成本组合导航系统中的MEMS陀螺的精度较低，无法敏感到地球自转角速度，导致低成本组合导航系统的捷联惯导解算初始对准时的航向无估计精度，姿态解算时航向误差不断累积，通过捷联惯导解算得到的低成本组合导航系统的航向和姿态数据在动态情况下精度不高。\n【发明内容】\n[0005] 本发明的实施例提供了一种低成本组合导航系统的数据融合方法和装置，以实现载体运动过程中的最优航向估计，满足低成本组合导航系统中航向、姿态的工程应用需求。\n[0006] 一种低成本组合导航系统的数据融合方法，包括：\n[0007] 根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψI、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψm、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψG，\n[0008] 根据设置的载体航向最优化估计性能指标，将所述ψI、ψm和ψG按照一定的权重进行叠加，得到所述载体航向的最优估计值；具体的：获取所述ψI、ψm和ψG的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；所述载体航向的最优估计值ψ的计算方法如下：\n[0009] ψ＝k1ψG+k2ψI+k3ψm；\n[0010] 其中，k1、k2、k3为加权系数；\n[0011] 在 约束条件下，求得满足所述载体的航向最优估计性能指标\n的加权系数K(k1,k2,k3)。\n[0012] 一种低成本组合导航系统中的数据融合装置，包括：\n[0013] 载体MEMS惯性器件航向计算模块，用于根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψI、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψm、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψG；\n[0014] 载体最终航向计算模块，用于根据设置的载体的航向最优化估计性能指标，将所述ψI、ψm和ψG按照一定的权重进行叠加，得到所述载体的航向最优估计值；还用于获取所述ψI、ψm和ψG的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；所述载体的航向的最优估计值ψ的计算方法如下：\n[0015] ψ＝k1ψG+k2ψI+k3ψm；\n[0016] 其中，k1、k2、k3为加权系数；\n[0017] 在 约束条件下，求得满足所述载体的航向最优估计性能指标\n的加权系数K(k1,k2,k3)。\n[0018] 由上述本发明的实施例提供的技术方案可以看出，本发明实施例在估计方差最小的最优性能指标约束下，能根据载体的动态特性自动调节估计系数，给出载体运动过程中的最优航向估计，具有较强的自适应能力、鲁棒性好，方便工程实际应用。\n[0019] 【附图说明】\n[0020] 图1为本发明实施例一提供的一种低成本组合导航系统的结构图；\n[0021] 图2为本发明实施例一提供的一种低成本组合导航系统的数据融合方法的原理示意图；\n[0022] 图3为本发明实施例一提供的一种低成本组合导航系统的数据融合方法的具体处理流程图；\n[0023] 图4为本发明实施例一提供的一种捷联惯导解算的原理示意图；\n[0024] 图5为本发明实施例一提供的一种载体的航向最优估计的处理流程图；\n[0025] 图6为本发明实施例一提供的一种低成本组合导航系统中的数据融合装置的具体结构图。\n【具体实施方式】\n[0026] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图，以对本发明实施例进行清楚、完整地描述人员。\n[0027] 实施例一\n[0028] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图，以对本发明实施对脉冲信号进行甄别的方法和装置例进行清楚、完整地描述人员。\n[0029] 实施例一\n[0030] 本发明实施例提供的低成本组合导航系统的结构如图1所示，主要由MIMU、MEMS磁阻传感器、单天线卫星接收机和导航计算机等构成。导航计算机完成多传感器的数据采集、信号处理、捷联惯导解算、数据融合(包括航向最优估计和组合Kalman滤波)等处理。\n上述卫星接收机可以为GPS卫星接收机或者北斗导航系统的卫星接收机。\n[0031] 基于上述低成本组合导航系统，该实施例提供的一种低成本组合导航系统的数据融合方法的原理示意图如图2所示，具体处理流程如图3所示，包括如下的处理步骤：\n[0032] 步骤31、导航计算机对MIMU、MEMS磁阻传感器的输出信号进行滤波处理。\n[0033] 导航计算机接收MIMU、MEMS磁阻传感器、单天线GPS卫星接收机的输出 信号，完成数据采集。上述MIMU、MEMS磁阻传感器等MEMS器件的精度低、输出噪声大，为了降低MEMS器件误差对低成本组合导航系统导航精度的影响，导航计算机需要对上述MIMU、MEMS磁阻传感器的输出信号进行实时滤波，通过设计合理的数字滤波器，可以达到去除噪声、平滑信号的目的。\n[0034] 步骤32、导航计算机根据MIMU的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态。\n[0035] 捷联惯导解算前需要初始对准，即求得载体的初始姿态，初始对准代表了惯性导航的最高精度。对于自主导航系统通常采用MIMU的输出进行自对准，自对准的姿态误差如下：\n[0036] \n[0037] \n[0038] \n[0039] 其中， 为载体的水平姿态误差、δψ为航向误差、δfE、δfN分别为东向和北向的加速度计偏值、g为地球重力加速度，εE为等效东向陀螺漂移、Ω为地球自转角速度、L为载体所在地的纬度。\n[0040] 由上述公式可知，水平姿态误差 与加速度计偏值(δfE，δfN)成正比，而航向误差δψ与εE、Ω和L有关。加速度计1mg的偏值可以产生0.057°的水平姿态误差，通常MIMU加速度计的偏值可以达到200μg～10mg，则水平姿态误差小于0.6°。而MEMS陀螺的漂移通常为10～100°/h，无法感测地球的自转角速度，航向没有估计精度，因此，捷联惯性导航解算时的航向容易发散。但当载体水平速度变化时，通过捷联惯性导航解算得到的载体的航向ψI还是能收敛的，ψI的误差δψI的计算公式如下：\n[0041] 或\n[0042] 其中， 为东、北向的速度变化量，δfE，δfN为东、北向的加速度变化量，fN，fE，fU为东、北、天向的加速度， 为载体水平姿态误差。\n[0043] 上述捷联惯导解算的原理示意图如图4所示，具体处理过程包括：\n[0044] (1)载体姿态解算\n[0045] 捷联惯导系统中，姿态是以载体系(b系)到导航系(n系)的方向余弦矩阵 表示的。方向余弦阵 是由下式微分方程解算得到的\n[0046] \n[0047] 式中， 是载体系相对导航系的角速度 的斜对称矩阵， 由下式得到：\n[0048] \n[0049] 其中， 为陀螺检测的载体角速度， 为方向余弦阵 的转置， 分别为地球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：\n[0050] \n[0051] 其中，Ω为地球自转角速度，L为地理纬度，vE，vN为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度。\n[0052] 求得方向余弦阵 后可以由下式计算载体的姿态，该姿态包括：俯仰角 横滚角γ、航向角ψ。\n[0053] \n[0054] \n[0055] \n[0056] 其中， 为方向余弦阵 的第i行第j列分量。\n[0057] 上述航向角ψ即为载体的航向。\n[0058] (2)速度解算\n[0059] 载体速度V(vE，vN，vU)的解算公式如下：\n[0060] \n[0061] 其中，fb为加速度计检测的载体加速度，gn＝[0，g，0]T，g为重力加速度。\n[0062] (3)位置解算\n[0063] 载体的位置信息(纬度L、经度λ、高度h)的解算公式如下：\n[0064] \n[0065] \n[0066] \n[0067] 其中，vE，vN，vU为载体速度在东向、北向、天向的分量，R为地球半径、h为载体距地面的高度。\n[0068] 步骤33、根据MEMS磁阻传感器的测量值和惯导水平姿态求解载体的航向。\n[0069] 采用东北天地理坐标系，右前上载体坐标系(即载体系正交的三个轴分别指向载体的右方、前方、上方)，载体磁航向的解算方法如下：\n[0070] 将磁阻传感器测量的载体坐标系中的磁场 转换到地理坐标系中\n的磁场H(Hx，Hy，Hz)。转换公式如下：\n[0071] \n[0072] 其中， γ为上述捷联惯性导航解算得到的载体水平姿态角。Hx，Hy，Hz为磁场在相应坐标系中的x，y，z轴分量。\n[0073] 利用最佳椭圆补偿算法，校正磁场测量值H(Hx，Hy，Hz)得到载体当地的水平磁场分量(X，Y)，校正公式如下：。\n[0074] X＝HxXsf+Xoff\n[0075] Y＝HyYsf+Yoff\n[0076] 其中，Xsf、Ysf为磁场分量校正的标度系数，Xoff、Yoff为磁场强度的偏移量。磁 补偿的模型参数，需要在系统标定时按照椭圆补偿方法的操作方法确定。\n[0077] 根据载体当地的水平磁场分量及磁偏角计算载体的真北航向ψm，ψm的计算公式如下：\n[0078] ψm＝arctan(Y/X)-θ\n[0079] 其中，θ为载体当地的磁偏角。\n[0080] 步骤34、根据GPS卫星接收机输出的速度信息求解载体的航向。\n[0081] ψG＝arctan(VE/VN)\n[0082] 其中，V(VE，VN，VU)为GPS卫星接收机输出的载体速度，VE，VN，VU分别为东向、北向和天向的载体速度分量。ψG为载体的速度航向，当载体协调运动时，速度航向与载体航向一致。\n[0083] 步骤35、基于载体动态特性的载体的航向最优估计。\n[0084] 载体的航向最优估计的处理流程如图4所示，包括以下的处理过程：\n[0085] 首先，分析载体速度航向与姿态航向一致的运动特征，引起载体航向变化的因素有两个：一是沿速度水平投影切线方向的力，二是沿竖直方向的力矩。因此，航向变化时MIMU的加速度计或陀螺均会有输出变化。\n[0086] 对于车载体，其航向变化是由车轮与地面的摩擦力提供的向心加速度引起，只要车轮不存在侧滑，则车载体的航向与速度的航向是一致的。\n[0087] 对于微型无人机系统，其基准运动为等速直线平飞状态，协调飞行(侧滑角为零)时，速度航向与载体航向一致。\n[0088] 计算各传感器解算的航向误差；\n[0089] 1：通过GPS接收机解算的载体航向估计误差δψG为\n[0090] \n[0091] 上式中，VE，VN，为载体速度的东向和北向分量，δVE，δVN为相应的速度误差。\n[0092] 由上式可以看出，载体水平面内的速度越大，则速度航向误差越小。反之，当载体静止时，GPS航向没有估计精度。\n[0093] 当载体协调运动时，可以利用速度航向表示姿态航向。对于车载系统(除车轮侧滑的特殊情况)、微型无人机的直线平飞状态均可以利用速度航向估计载体的姿态航向。\n[0094] 2：载体转弯时通过惯导计算的载体航向ψI的估计误差δψI\n[0095] 或\n[0096] 当载体航向转弯时，会产生水平方向的向心加速度(fE或fN)、引起水平方向速度的变化，这时惯导解算的航向是收敛的。并且通常由于转弯时间较短惯导解算的航向误差较小。\n[0097] 3：通过磁阻传感器计算载体航向的估计误差。\n[0098] 磁场航向的测量误差主要来源于载体的磁场干扰和动态干扰：通常磁场干扰可以通过最佳椭圆补偿方法校正；而低成本磁阻传感器的动态误差较大。因此，本发明实施例将磁阻传感器的航向误差δψm设为载体动态的函数：\n[0099] δψm≈m1f+m2ω\n[0100] 其中，f、ω为MIMU的比力和角速度输出，m1、m2由磁阻传感器的性能确定。\n[0101] 当载体静止或匀速直线运动时，磁航向的误差较小、可信度较高。\n[0102] 本发明实施例基于最优性能指标估计载体的航向，由GPS卫星接收机、惯导、磁阻传感器信息估计载体航向的方法如下：\n[0103] ψ＝k1ψG+k2ψI+k3ψm\n[0104] 其中，k1、k2、k3为加权系数k1+k2+k3＝1，系数的求解方法如下：\n[0105] 基于最优控制的思想，设定载体的航向最优估计性能指标J为航向估计误差δψ\n2\n的方差δψ最小，即\n[0106] \n[0107] 在 约束条件下，可以求得满足性能指标J(即方差最小)的\n加权系数K(k1，k2，k3)，求解公式如下：\n[0108] \n[0109] \n[0110] \n2 2 2\n[0111] 式中，δψI、δψG、δψm分别为惯导、GPS、磁估计航向误差的方差。\n[0112] 根据前面的分析可知：当载体直线运动速度较大时GPS的航向误差较小，则航向估计中k1较大；载体动态转弯时，惯导解算的航向误差较小，则k2较大；而载体静止或匀速运动时磁航向误差较小，则k3较大。因此，在估计方差最小的最优性能指标约束下，该航向估计方法能根据载体的动态特性自动调节估计系数，给出特定运动过程中的最优航向估计，具有较强的自适应能力、鲁棒性好，方便工程实际应用。\n[0113] 步骤36、通过Kalman(卡曼)组合滤波，解算载体的位置、速度、姿态等最优估计信息。\n[0114] Kalman组合滤波是将惯导、GPS接收机、磁阻传感器的信息进行融合，解算步骤如下：\n[0115] 1：根据惯导系统的误差传播模型建立系统的状态方程；\n[0116] 状态向量取惯导解算的载体位置误差δP(δL，δλ，δh)、速度误差δV、平台误差角φ(φE，φN，φU)、陀螺漂移ε(εx，εy，εz)、加速度计漂移 即 确定系统的状态方程如下：\n[0117] \n[0118] 其中，F(t)为状态转移矩阵，可以由惯导误差传播方差得到，W(t)为零均值高斯白噪声，W(t)的协方差阵由状态初始值方差、陀螺、加速度计的性能指标确定。\n[0119] 转移矩阵F(t)的各分量如下：\n[0120] \n[0121] \n[0122] F(3，6)＝1\n[0123] \n[0124] \nF(4，8)＝-fU F(4，9)＝fN\n[0125] \n[0126] \nF(5，7)＝fU F(5，9)＝-fE F(6，1)＝-2ΩvEsinL\n[0127] \nF(6，7)＝-fN F(6，8)＝fE\n[0128] \n[0129] F(8，1)＝-ΩsinL，\n[0130] \n[0131] \n[0132] \n[0133] \n[0134] 上述表达式中，L为纬度，λ为经度，h为高度，vE，vN，vU为载体速度在东向、北向、天向的分量，fE，fN，fU为载体加速度在东向、北向、天向的分量，R为地球半径，Ω为地球自转角速度， 为载体的方向余弦阵。除上述分量外，矩阵F(t)的其余分量均为0。\n[0135] 2：建立量测方程；\n[0136] 系统量测值取GPS接收机与惯导解算的载体位置误差δP(δL，δλ，δh)、速度误差δV、航向的最优估计ψ与惯导解算航向ψI的差值δψ，即Z(t)＝[δL，δλ，δh，T\nδVE，δVN，δVU，δψ]，则测量方程如下：\n[0137] Z(t)＝H(t)X(t)+V(t)\n[0138] 测量矩阵H(t)为：\n[0139] \n[0140] 其中，V(t)的元素为互不相关的零均值高斯白噪声，V(t)的协方差阵由GPS接收机的性能指标确定。\n[0141] 3：离散化系统方程；\n[0142] 设离散型系统的方程为\n[0143] X(k+1)＝Φ(k+1，k)X(k)+W(k)\n[0144] Z(k)＝H(k)X(k)+V(k)\nT T\n[0145] 其中，E[W(k)]＝E[V(k)]＝0，E[W(k)W(j)]＝Qkδkj，E[V(k)V(j)]＝Rkδkj，T\nE[W(k)V(j)]＝0。上式中，初始状态X(0)的统计特性为：\n[0146] E[X(0)]＝m0\nT\n[0147] E{[X(0)-m0][X(0)-m0]}＝P0\n[0148] 4：Kalman滤波估计载体的位置、速度、姿态信息\n[0149] Kalman滤波是利用递推的方式由系统的初始状态X(0)(位置、速度、姿态误差等)估计新状态 的过程。递推估计的步骤如下：\n[0150] ①由当前状态 预测下一步状态 为\n[0151] \n[0152] ②步骤①预测的协方差P(k+1，k)为P(k+1，k)＝Φ(k+1，k)p(k)ΦT(k+1，k)+Qk[0153] ③由估计协方差计算滤波增益K(k+1)为K(k+1)＝P(k+1，k)HT(k+1)[H(k+1)P(k+1，k)HT(k+1)+Rk+1]-1\n[0154] ④新状态 的最优估值为\n[0155] \n[0156] ⑤新状态 估计的均方误差P(k+1)为P(k+1)＝[I-K(k+1)H(k+1)]P(k+1，k)\n[0157] 迭代循环步骤①～⑤，则由系统初始状态X(0)、初始方差P0、各个时刻的观测量Z(k)，系统误差矩阵Qk、观测噪声矩阵Rk，可以递推得到最终的新状态 (即载体的位置、速度、姿态等误差)。\n[0158] 根据上述Kalman组合滤波计算得到的误差量对前面捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。\n[0159] 实施例二\n[0160] 该实施例提供了一种低成本组合导航系统中的数据融合装置，该数据融合装置设置在所述图1中的导航计算机中，其具体结构如图6所示，包括如下的模块：\n[0161] MEMS惯性器件航向计算模块61，用于根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到载体的航向ψI、根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向ψm、根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向ψG；\n[0162] 载体最终航向计算模块62，用于根据设置的载体的航向最优化估计性能指标，将所述ψI、ψm和ψG按照一定的权重进行叠加，得到所述载体航向的最优估计值。\n[0163] 具体的，所述的MEMS惯性器件航向计算模块61，还用于在捷联惯导系统中，姿态是以载体系到导航系的方向余弦矩阵 表示的，方向余弦阵 是由下式微分方程解算得到的\n[0164] \n[0165] 式中， 是载体系相对导航系的角速度 的斜对称矩阵， 由下式得到：\n[0166] \n[0167] 其中， 为陀螺检测的载体角速度， 为方向余弦阵 的转置， 分别为地球的自转角速度和导航系相对地球系的旋转角速度，由下式计算：\n[0168] \n[0169] 其中，Ω为地球自转角速度，L为地理纬度，vE，vN为载体的东向和北向速度分量，R为地球半径、h为载体距地面的高度。\n[0170] 求得方向余弦阵 后，可以由下式转换为载体的姿态，该姿态包括：俯仰角 横滚角γ、航向角ψ；\n[0171] \n[0172] \n[0173] \n[0174] 其中， 为方向余弦阵 的第i行第j列分量，上述航向角ψ为根据组合导航系统中的微惯性测量单元的输出数据进行捷联惯导解算得到的载体航向ψI；\n[0175] 还用于将磁阻传感器的测量值从载体坐标系 转换到地理坐标系\nH(Hx，Hy，Hz)转换公式如下：\n[0176] \n[0177] 其中， γ为上述捷联惯性导航解算得到的载体的水平姿态角，Hx，Hy，Hz为磁场在相应坐标系中的x，y，z轴分量；\n[0178] 利用最佳椭圆补偿算法，校正磁场测量值H(Hx，Hy，Hz)得到载体当地的水平磁场分量(X，Y)，校正公式如下：\n[0179] X＝HxXsf+Xoff\n[0180] Y＝HyYsf+Yoff\n[0181] 其中，Xsf、Ysf为标度系数，Xoff、Yoff为载体当地的磁场强度的偏移量，[0182] 根据载体当地的水平磁场分量及磁偏角计算载体的航向ψm，ψm的计算公式如下：\n[0183] ψm＝arctan(Y/X)-θ\n[0184] 其中，θ为载体当地的磁偏角；\n[0185] 所述ψm即为根据组合导航系统中的磁阻传感器的输出数据解算得到载体的航向；\n[0186] 还用于设V(VE，VN，VU)为卫星接收机输出的载体速度，VE，VN，VU分别为东向、北向和天向的载体速度分量。则由载体速度解算的速度航向ψG为\n[0187] ψG＝arctan(VE/VN)\n[0188] 当载体协调运动时，载体的航向与载体的速度航向一致，所述ψG为根据组合导航系统中的卫星接收机的输出数据解算得到载体的航向。\n[0189] 具体的，所述的载体最终航向计算模块62，还用于获取所述ψI、ψm和ψG的估计误差，设定载体的航向最优估计性能指标为航向的估计误差的方差最小；\n[0190] 所述载体的航向的最优估计值ψ的计算方法如下：\n[0191] ψ＝k1ψG+k2ψI+k3ψm\n[0192] 其中，k1、k2、k3为加权系数，\n[0193] 在 约束条件下，求得满足所述载体的航向最优估计性能指\n标的加权系数K(k1，k2，k3)。\n[0194] 具体的，所述的载体最终航向计算模块62，还用于计算出所述ψI的估计误差δψI为：\n[0195] 或\n[0196] 其中， 为东、北向的速度变化量，δfE，δfN为东、北向的加速度变化量，fN，fE，fU为东、北、天向的加速度， 为载体水平姿态误差；\n[0197] 计算出所述ψm的估计误差δψm为：\n[0198] δψm≈m1f+m2ω\n[0199] 其中，f、ω为微惯性测量单元MIMU的比力和角速度输出，m1、m2为根据磁阻传感器的性能确定的参数；\n[0200] 计算所述ψG的估计误差δψG为：\n[0201] \n[0202] 上式中，VE，VN，为载体速度的东向和北向分量，δVE，δVN为相应的速度误差；\n[0203] 设定载体的航向最优估计性能指标为航向的估计误差δψ的方差δ2ψ最小，即[0204] \n[0205] 在 约束条件下，可以求得满足所述航向最优估计性能指标J\n的加权系数K(k1，k2，k3)，求解公式如下：\n[0206] \n[0207] \n[0208] \n[0209] 式中，δ2ψI、δ2ψG、δ2ψm分别为惯导、GPS、磁估计航向误差的方差。\n[0210] 具体的，所述的装置还可以包括：\n[0211] 载体速度、位置和姿态计算模块63，用于采集所述微惯性测量单元、磁阻传感器、卫星接收机的输出信号，并对采集到的数据进行滤波处理；\n[0212] 根据所述微惯性测量单元的陀螺和加速度计信号进行捷联惯导解算，求得载体的航向、速度、位置、姿态；\n[0213] 通过卡曼Kalman组合滤波过程计算得到的误差量，对所述捷联惯导解算得到的载体的速度、位置和姿态进行修正，得到载体的速度、位置和姿态的最优估计值。\n[0214] 应用本发明实施例的装置进行数据融合的具体处理过程与前述方法实施例类似，此处不再赘述。\n[0215] 本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random Access Memory，RAM)等。\n[0216] 综上所述，本发明实施例在航向估计方差最小的最优性能指标约束下，能根据载体的动态特性自动调节估计系数，给出载体运动过程中的最优航向估计，具有较强的自适应能力、鲁棒性好，方便工程实际应用。\n[0217] 本发明实施例分析引起磁场干扰的外界因素，将磁航向误差设置为载体动态参数的函数，既符合工程实际应用，又避免了通常对磁信号干扰程度的模糊判断，方便工程应用。\n[0218] 本发明实施例将多传感器的航向误差统一为载体动态特性的函数，基于最优控制理论的思想，将最小方差的最优估计性能指标引入到载体的航向估计中。将航向误差作为观测量引入传统Kalman滤波后，可以加快滤波的收敛。\n[0219] 本发明实施例适用于低精度要求的低成本MEMS组合导航系统，对器件的精度要求不高。\n[0220] 以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。