CN101319902A - 一种低成本组合式定位定向装置及组合定位方法 - Google Patents

Abstract

本发明提供的是一种低成本组合式定位定向装置及组合定位定向方法。该装置主要包括由MEMS IMU和导航计算机组成的MEMS INS、GPS接收机、三维数字电子罗盘、高度计、气速计、电源模块、箱体及接插件等，其中MEMS IMU包括9只MEMS陀螺和6只双轴MEMS加速度计，由它们分别测量出运动载体的角速度和加速度，然后输入到导航解算程序中，计算得到载体的姿态、位置和速度等信息。同时导航计算机通过232串口定时采集GPS接收机、电子罗盘、气速计和高度计等导航子单元的信息，通过信息融合处理，得到载体最优的位置、姿态、速度和高度等导航信息，精度优于装置中单一的子系统。本发明体积小、功耗低、成本低，但是功能齐全，接口丰富，完全能满足地面或空中运载体的中等精度定位定向要求。

Description

一种低成本组合式定位定向装置及组合定位方法

(一)技术领域

本发明涉及的是一种定位定向装置，特别是一种由微惯性导航系统(MEMSINS)、全球定位系统(GPS)、电子罗盘、微型压力高度计和微型压力空速计等构成的微型组合式定位定向装置。本发明还涉及一种采用该低成本组合式定位定向装置的定位定向方法。

(二)背景技术

惯性导航的基本原理是根据牛顿力学定律，通过测量载体在惯性参考系的加速度和角速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、位置和姿态角信息等。所以，它是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统。具有很好的隐蔽性，其工作环境不仅包括空中、地球表面，还可以在水下。比如，1958年美国“皇后鱼”号核潜艇依靠惯性导航系统穿过北极在冰下航行21天；美国“阿波罗”号飞船上也装备了捷联式惯性导航系统。目前，惯性导航在海、陆、空、天等各领域都得到了广泛的应用。

惯性导航的主要缺点是它的误差会随时间增长而迅速积累。而且由于启动时需要加温、初始对准，所以进入导航状态的时间较长，比如某型船用平台罗经需要约4小时。

美国的全球定位系统(GPS)是一个高精度、全天候和全球性的多功能无线电导航定位、定时系统，它的显著优点是精度均匀，定位误差不随时间积累。因而，目前GPS的定位导航技术发展非常迅速，应用极为广泛。但是GPS系统存在的问题是：在使用时，城市的高大树木或高层建筑对GPS信号有遮挡效应，即城市大峡谷效应，会导致GPS定位失效。而且GPS是非自主式导航，数据更新频率低(一般为1Hz)，不能满足实时控制的要求，故需利用辅助装置与GPS组成组合定位系统。

电子罗盘是以地磁北为基准测量物体方位的，在静止或运动状态下均能给出方位信息，且不受高大阻挡物的影响，因此高精度电子罗盘可以对GPS进行有效地补偿，即使在GPS信号消失后也能正确地给出航向，也能保证导航定位信息的有效性。但是电子罗盘受外环境磁介质的影响较大，如果补偿不当，航向测量误差可达几十度。

从上面的分析看到，将INS、GPS和电子罗盘各有自己的长处，也都存在一定的短处，如果能将它们组合起来，构成组合式定位定向系统，各取所长而避其所短，无疑可以提高系统的精度和可靠性，降低对各分系统的要求，从而达到以较低的成本、构成中等精度的定位定向系统的目的。但是如何将它们结合起来，在系统结构设计；MEMSINS姿态、速度及位置更新算法；MEMSINS与GPS、数字电子罗盘HMR3000、高度计、气速计等信息融合方式以及人机界面及接口等方面还存在许多需要解决的技术问题。

(三)发明内容

本发明的目的在于提供一种成本较低、功能较全的定位定向的低成本组合式定位定向装置。

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

本发明的低成本组合式定位定向装置的组成包括由MEMS IMU和导航计算机组成的MEMSINS、GPS接收机、三维数字电子罗盘、高度计、气速计、电源模块、箱体及接插件等，其中MEMSIMU包括9只MEMS陀螺和6只双轴MEMS加速度计。+24V直流电压进入电源模块，通过DC-DC模块转换为±5V和+12V，供给相应的子系统，系统加电启动后，MEMS陀螺和加速度计分别测量出载体运动的角速度和加速度，输出为相应的模拟电压值，通过数据采集电路将该电压值进行A/D转换，然后输入到MEMS INS的标定或导航程序中，得到载体运动的姿态、位置和速度等信息，GPS接收机、电子罗盘、气速计和高度计都是通过232串口向导航计算机发送位置、姿态、速度和高度信息。

本发明还可以包括这样一些结构特征：

1、所述的微惯性导航系统(MEMSINS)的9个MEMS陀螺选择ADXRS150、6个双轴加速度计选择ADXL202，9个MEMS陀螺和6个双轴加速度计安装在一个由4块平整的铜板组成的安装基座上，MEMS陀螺和双轴加速度计分布在三个正交面上。

2、所述的GPS接收机选择iTrax02，所述的三维数字电子罗盘选择HMR3000，所述的高度计选择AXD550。

3、MEMSINS和数字电子罗盘设置在箱体的中间，艏艉线从中穿过。

本发明的低成本组合式定位定向装置的组合定位方法为：

1、选用由微惯性测量单元(MEMS IMU)和导航计算机组成微惯性导航系统(MEMS INS)、电源模块以及外界通讯插头、气速计、是高度计、GPS接收机、三维数字电子罗盘以及箱体组成的低成本组合式定位定向装置，MEMS IMU包括9只MEMS陀螺和6只双轴加速度计；

2、微惯性导航系统(MEMSINS)进行姿态、位置和速度更新

在三维空间上的每个测量轴上有采用3个MEMS陀螺对同一个输入角速度进行测量，然后设计最优滤波器估计出各陀螺误差的大小，并对量测信息补偿校正，得到对输入角速率的高精度估计值，另一方面，每隔0.05秒采集一次三维数字电子罗盘HMR3000的姿态角信息，通过设计一个Kalman滤波器，估计陀螺的常值漂移，进而对陀螺测量的角速度信息进行定时补偿；

3、对各导航单元信息进行融合

导航计算机高速采集多路陀螺和加速度计信号，实时计算出载体的姿态、位置和速度后，每隔1秒钟同时接收一次GPS接收机iTrax02的位置、速度和三维数字电子罗盘HMR3000的三维姿态角信息，然后以MEMS INS作为参考系统，分别与iTrax02和HMR3000组成子滤波器1和子滤波器2进行联邦滤波，滤波器的参数由预先的专家系统判断载体机动状态后作自适应调整，经过补偿后得到优化的姿态、位置和速度信息。

本发明的优点在于整个系统体积小、功耗低、成本低，但是功能齐全，接口丰富，完全能满足地面或空中运载体的中等精度定位定向要求。

将MEMS INS、iTrax02、HMR3000和AXD550等各子系统的导航信息经Kalman联邦滤波后，系统的定位定向精度超过了其单一子系统的精度。这样通过对低精度的异类传感器进行信息融合，有效地提高了整体的测量精度。

已有技术中与本发明相当的定位定向装置采用分立式结构，其中惯性测量组件箱体积为400×400×650mm，导航计算机箱400×400×250mm，而本发明的装置的体积为200×250×130mm，后者仅为前者的1/22，本系统的最大功耗仅为13W，平均功耗为4W。

本发明的对外接口丰富，有2个232串口、2个422串口和1个网络接口等，如果需要，可以很方便地扩展CAN接口，完成姿态、航向、经度、纬度、高度、角速度以及线速度等导航信息的传送。

本装置所有的成本不足万元，所采用的MEMS陀螺约几百元，而传统的惯性导航系统的陀螺约十万余元。虽然器件的精度低，但是通过组合后精度有很大的提高，达到了中等精度的水平。并且由于其体积小、功耗低，所以只需调整部分参数后，可以应用于智能轮椅自动导航、高速列车轨道测量、智能炸弹导航和空中摄影等领域中。

(四)附图说明

图1是本发明的低成本组合式定位定向装置的内部结构示意图；

图2是本发明的低成本组合式定位定向装置的各子单元连接关系示意图；

图3是MEMS陀螺和加速度计的安装示意图；

图4是陀螺测量信息融合原理图；

图5是MEMSINS姿态、位置和速度更新示意图；

图6是多信息融合联邦滤波器工作原理框图；

图7是组合式定位定向装置的工作流程图；

图8是组合式定位定向装置的人机交互界面图。

(五)具体实施方式

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

结合图1。低成本组合式定位定向装置的组成包括微惯性测量单元(MEMSIMU)1、气速计2、高度计3、GPS接收机4、维数字电子罗盘5、电源模块6、与外界通讯的14芯航空插头7、4芯电源插头9和导航计算机15，微惯性测量单元包括9只MEMS陀螺和6只双轴加速度计，它与导航计算机组成微惯性导航系统(MEMS INS)，能实时测量载体的姿态、位置和速度。11是气速计与外界连接的通气孔。高度计选用AXD550，10是与高度计其相应的通气孔，通过测量外界气压，计算载体所在的高度。GPS接收机选用iTrax02，8是GPS接收机的信号接收天线。Honeywell三维数字电子罗盘选择HMR3000。电源模块提供±5V和12V直流电源。与外界通讯的14芯航空插头包括2路232串口，2个422串口和1个网络通讯口。4芯电源插头为装置提供24V直流电源。12是铝合金材料制成的一体式箱体。13是装置的艏艉线，即中心轴线，为使用时提供安装基准。15是导航计算机，包括模拟信号调理电路，A/D转换电路和通讯接口电路等。

结合图2。+24V直流电压进入装置以后，通过DC-DC模块转换为±5V和+12V，供给各子系统。惯性测量单元(IMU)在接通电源后，它内部的陀螺和加速度计开始工作，分别测量出载体运动的角速度和加速度，输出为相应的模拟电压值，通过数据采集电路将该电压值进行A/D转换，然后输入到MEMS INS的标定或导航程序中做相应的处理，得到载体运动的姿态、位置和速度等信息。其他设备如GPS接收机、电子罗盘、气速计和高度计都是通过232串口按一定的频率向导航计算机发送位置、姿态、速度和高度信息。

在系统设计时，考虑到数字电子罗盘HMR3000容易受外界环境磁介质的影响，整个装置的箱体采用铝合金材质。同时，为了保证MEMSINS和数字电子罗盘的姿态、航向测量精度，将两者安置在箱体的中间，艏艉线从中穿过。

另外，为了提高微惯性导航系统(MEMSINS)的精度，陀螺和加速度计都采用了冗余技术，结合图3，9个MEMS陀螺ADXRS150和6个双轴加速度计ADXL202安装在一个由4块平整的铜板组成的安装基座上，陀螺和加速度计分布在三个正交面上，其正交性通过机械加工保证。

微惯性导航系统(MEMS INS)导航算法

由于定位定向装置中的惯性测量组件(IMU)选用的是MEMS惯性器件，包括ADXL202加速度计和ADXRS150陀螺，它们的测量分辨率低，噪声大。比如陀螺ADXRS150漂移经补偿后约为80°/h，无法敏感地球自转角速度ω_ie(≈15°/h)，也即ω_ie完全淹没在陀螺噪声里，这样传统的导航更新算法无法适用。

结合图4和图5，改进的方案里一方面采用同类传感器多信息融合技术，在三维空间上的每个测量轴上采用3个陀螺对同一个输入角速度进行测量，然后设计最优滤波器估计出各陀螺误差的大小，并对量测信息补偿校正，得到对输入角速率的高精度估计值。另一方面，根据三维数字电子罗盘HMR3000的数据更新频率为20Hz，所以每隔0.05秒采集一次HMR3000的姿态角信息，通过设计一个Kalman滤波器，估计陀螺的常值漂移，进而对陀螺测量的角速度信息进行定时补偿。通过这两方面的修正后，大大地提高了陀螺的测量精度，降低了陀螺的随机漂移误差量。

各导航单元信息融合方法

结合图6和图7。导航计算机高速采集多路陀螺和加速度计信号，实时计算出载体的姿态、位置和速度后，每隔1秒钟同时接收一次GPS接收机iTrax02的位置、速度和HMR3000的三维姿态角信息，然后以MEMSINS作为参考系统，分别与iTrax02和HMR3000组成子滤波器1和子滤波器2进行联邦滤波，滤波器的参数由预先的专家系统判断载体机动状态后作自适应调整，这样经过补偿后得到优化的姿态、位置和速度信息。将其与前面的MEMS INS导航计算过程结合在一起，可以总结出系统的整个工作流程。

人机交互界面

结合图8。导航计算机采用嵌入式工业计算机PC104，它具有键盘和显示器接口。并在计算机预装了DOS6.0系统，然后采用C语言编程，可以实现组合式定位定向装置的人机交互功能。

本发明中的MEMSIMU选用硅微陀螺ADXRS150和硅微加速度计ADXL202，为提高精度和保证可靠性，三个正交敏感轴的每个轴向各安装了三个陀螺和两个双轴加速度计，这样需要采集的信号包括9路陀螺、12路加速度计和9路陀螺温度共30路模拟信号，数据采集电路中的模/数(A/D)转换芯片选用美国ADI公司的AD7656，它是一种6通道16位高精度、高速AD芯片，工作时由一个16位的凌阳单片机SPCE061A控制。将采集的各路数据在导航计算机PC104中进行实时导航解算处理，得到的由MEMS INS解算出来的姿态、位置和速度等信息，下面详细介绍姿态更新算法。

设载体横滚角γ，俯仰角θ，航向角ψ，那么由载体坐标系b到“东北天”地理坐标系n的方向余弦矩阵可以表示为

$T=\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]$

对T微分可得到如下关系：

T^&＝Ω(ω)gT            (1)

其中， $\mathrm{\Ω}\left(\mathrm{\ω}\right)=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right],$ ω_x，ω_y，ω_z机体三轴角速度。

为了构建Kalman滤波方程，取矩阵T的第3行元素[r₁₃，r₂₃，r₃₃]作为状态变量，此外，由于陀螺信号的漂移误差会给姿态解算的精度带来很大的影响，因此有必要对陀螺的漂移误差进行实时估计。即取：

x＝[r₃₁ r₃₂ r₃₃ ω_x ε_y ω_z]              (2)

其中：r₃₁、r₃₂、r₃₃为矩阵T的第3行元素；ω_x，ω_y，ω_z为三轴陀螺的漂移；

当MEMS INS在没有加速的情况下，三轴加速度计测量值为：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T^{-1}g\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}-\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\\ \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]g$

取a_x、a_y、a_z为观测量，并设其单位为重力加速度g，即y＝[a_x a_y a_z]。那么，扩展Kalman滤波方程的状态空间模型可表示为：

x&＝Ax+v

y＝Cx+w               (3)

式中：

$A=\left[\begin{array}{cc}\stackrel{\‾}{\mathrm{\Ω}}\left(\mathrm{\ω}\right)& \mathrm{\Ω}\left(R\right)\\ 0& 0\end{array}\right],$ $\stackrel{\‾}{\mathrm{\Ω}}\left(\mathrm{\ω}\right)=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_z-{\mathrm{\Δ\ω}}_z& -({\mathrm{\ω}}_y-{\mathrm{\Δ\ω}}_y)\\ -({\mathrm{\ω}}_z-{\mathrm{\Δ\ω}}_z)& 0& {\mathrm{\ω}}_x-{\mathrm{\Δ\ω}}_x\\ {\mathrm{\ω}}_y-{\mathrm{\Δ\ω}}_y& -({\mathrm{\ω}}_x-{\mathrm{\Δ\ω}}_x)& 0\end{array}\right],$

$\mathrm{\Ω}\left(R\right)=\left[\begin{array}{ccc}0& {-r}_{33}& r_{32}\\ r_{33}& 0& -r_{31}\\ -r_{32}& r_{31}& 0\end{array}\right],$ C＝[I_3×3 0]，v为过程噪声向量，w为测量噪声向量。并且，前3个状态变量满足

$r_{31}^2+r_{32}^2+r_{33}^2=1---\left(4\right)$

将式(3)离散化，利用Kalman滤波递推公式可以估计出r₁₃、r₂₃、r₃₃三个状态变量，从而解算出俯仰角和横滚角。在此过程中，为了提高系统精度，设计一个专家系统，通过微型压力高度计、空速计和MEMSINS的信息判断载体的机动状态，以选择不同的Kalman滤波器参数。

由于系统无法进行初始对准，获得精确的航向，为了弥补该不足，利用装置中的电子罗盘，通过测量地球磁场，快速地获取载体的航向角。结合①中所计算得到的两个水平姿态角，传递给MEMS INS，完成姿态传递对准。这中间产生的误差一方面可以通过高度回路予以修正，另一方面将通过设计的联邦滤波器进行误差参数在线辨识和实时补偿，这样将误差降到最小，而且使姿态、位置误差不随时间增长而快速发散。

根据组合导航典型联邦滤波器的体系结构，MEMS INS/GPS/HMR3000组合的联邦滤波器结构如图5示，捷联系统和GPS组合为子滤波器1，捷联系统和电子罗盘组合为子滤波器2，主滤波器对公共状态进行融合。

在子滤波器1中，取MEMSINS的10个状态向量和GPS的3个姿态向量作为组合系统的状态变量其中，GPS的3个姿态向量用一阶马尔科夫过程表示：

k_g为GPS系统的相关时间的导数，w_g为激励的马尔科夫过程的白噪声。

组合系统状态方程为

(6)式中 $F_1=\left[\begin{array}{cc}{F}_I& 0\\ 0& F_g\end{array}\right],$ $w_1\left(t\right)=\left[\begin{array}{cc}{w}_I\left(t\right)& 0\\ 0& w_g\left(t\right)\end{array}\right].$

以MEMS INS和GPS的姿态之差作为组合系统的观测量，则MEMS INS/GPS的观测方程为

$z_1\left(t\right)=\left[\begin{array}{c}{\mathrm{\δ\α}}_I-{\mathrm{\δ\α}}_g\\ {\mathrm{\δ\β}}_I-{\mathrm{\δ\β}}_g\\ {\mathrm{\δ\γ}}_I-{\mathrm{\δ\γ}}_g\end{array}\right]=H_1\left(t\right)x_1\left(t\right)+v_1\left(t\right)---\left(7\right)$

其中H₁(t)＝[0_3×4 I_3×3 0_3×3 -I_3×3 0_3×5]。

利用HMR3000测量姿态时，关键就是航向角的测量，HMR3000测量的是沿载体坐标系三轴方向上的磁场强度，由此推算出的航向角是磁航向α_m。

取H_xt，H_yt，H_zt为磁场沿地理坐标系三轴的分量，H_xd，H_vd，H_zd磁场为沿舰船坐标系的各分量，则它们之间有如下关系

$\left[\begin{array}{c}{H}_{\mathrm{xt}}\\ H_{\mathrm{yt}}\\ H_{\mathrm{zt}}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& \sin \mathrm{\γ}\\ 0& -\sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_{\mathrm{xd}}\\ H_{\mathrm{yd}}\\ H_{\mathrm{zd}}\end{array}\right]---\left(8\right)$

这里的α角就是磁航向角α_m，因为β，γ已由倾斜仪测出，所以有

$\left[\begin{array}{c}{H}_{\mathrm{xt}}^{\′}\\ H_{\mathrm{yt}}^{\′}\\ H_{\mathrm{zt}}^{\′}\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_m& -\sin {\mathrm{\α}}_m& 0\\ \sin {\mathrm{\α}}_m& \cos {\mathrm{\α}}_m& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{H}_{\mathrm{xd}}\\ H_{\mathrm{yd}}\\ H_{\mathrm{zd}}\end{array}\right]---\left(9\right)$

所以 ${\mathrm{\α}}_m={\mathrm{tg}}^{-1}\frac{-H_{\mathrm{yt}}^{\′}}{H_{\mathrm{xt}}^{\′}}.$

设真北与磁北方向差角即磁差为Δα，则真航向角α_T为

α_T＝α_m+Δα                       (10)

由HMR3000测量姿态角的原理可知其测量误差主要有：2个倾斜角的偏移误差δβ_T、δγ_T，磁强计测量误差δα_T，将它们都用一阶马尔可夫过程表示

对MEMS INS/HMR3000的组合模式，我们也采用与MEMS INS/GPS相似的处理方法，将δα₃、δβ₃、δγ₃扩充为状态变量，状态方程为

将HMR3000和MEMSINS输出的姿态信息的差值作为组合模式的测量值，则MEMS INS/HMR3000组合的观测方程为

$z_2=\left[\begin{array}{c}{\mathrm{\δ\α}}_I-{\mathrm{\δ\α}}_3\\ {\mathrm{\δ\β}}_I-{\mathrm{\δ\β}}_3\\ {\mathrm{\δ\γ}}_I-{\mathrm{\δ\γ}}_3\end{array}\right]=H_2{x}_2+v_2---\left(13\right)$

其中

$H_2=\left[\begin{array}{ccccc}{0}_{1\×4}& 1& 0_{1\×5}& -1& 0_{1\×2}\\ 0_{1\×5}& 1& 0_{1\×5}& -1& 0\\ 0_{1\×6}& 1& 0_{1\×4}& 0& -1\end{array}\right]---\left(14\right)$

建立了2个子系统MEMS INS/GPS和MEMS INS/HMR3000的数学模型后，将其状态方程和观测方程离散化，就可通过线性离散卡尔曼滤波，估计出组合系统的姿态信息误差。

根据MEMS INS/GPS/HMR3000组合的特点，以MEMS INS作为参考系统，分别与iTrax02、HMR3000组成2个子滤波器——MEMS INS/GPS组合和MEMSINS/HMR3000组合，然后根据联邦滤波器的结构组成，设计不同的信息分配系数，运用联邦滤波器算法，进行全局滤波，以得到较优的定位定向信息。

MEMS INS的姿态、位置更新频率为200Hz，而GPS接收机iTrax02和三维数字电子罗盘HMR3000的更新频率为1Hz，所以，联邦滤波器中以MEMS INS为参考系统，GPS和电子罗盘的导航信息对其进行定时修正，在技术方案所描述的联邦滤波器具体参数设置如下：

①滤波器状态向量初始值 $\hat{x}(0/0)=0;$

②滤波误差方差P(0/0)＝diag(P₁(0/0)，P₂(0/0)，Λ，P₁₆(0/0))，P_i(0/0)为相应的状态向量的均方差E(x_i ²)，不为零的元素为

E(x₁ ²)＝E(δλ²)＝(0.5′)²；

E(x₃ ²)＝E(δv_E ²)＝(0.01m/s)²；        E(x₄ ²)＝E(δv_N ²)＝(0.01m/s)²；

E(x₅ ²)＝E(δα_I ²)＝(3′)²；           E(x₆ ²)＝E(δβ_I ²)＝(3′)²；

E(x₇ ²)＝E(δγ_I ²)＝(6′)²；           E(x₈ ²)＝E(δε_E ²)＝(0.01°)²；

E(x₉ ²)＝E(δε_N ²)＝(0.01°)²；        E(x₁₀ ²)＝E(δε_Z ²)＝(0.01°)²；

E(x₁₁ ²)＝E(δα_g ²)＝(4′)²；          E(x₁₂ ²)＝E(δβ_g ²)＝(4′)²；

E(x₁₃ ²)＝E(δγ_g ²)＝(5′)²；          E(x₁₄ ²)＝E(δα_T ²)＝(6′)²；

E(x₁₅ ²)＝E(δβ_T ²)＝(6′)²；          E(x₁₆ ²)＝E(δγ_T ²)＝(6′)²

③系统噪声方差阵Q₀＝diag(Q_1，1，Q_2，2，Λ_16，16)，不为零的元素为

Q_8，8＝Q_9，9＝Q_10，10＝2×(0.001°)²×k_I；   Q_11，11＝Q_12，13＝Q_14，14＝2×(0.02′)²×k_g；

Q_14，14＝Q_15，15＝Q_16，16＝2×(0.04′)²×k_T；k_I＝1/1800，k_g＝1/600，k_T＝1/300。

④观测噪声方差阵R(0)＝diag(R_1，1，R_2，2ΛR_6，6)，不为零的元素为

R_1，1＝R_2，2＝R_3，3＝(0.1′)²；  R_4，4＝R_5，5＝R_6，6＝(0.4′)²

定位定向装置的人机交互界面如图6所示，其中16是键盘区，由数字键和功能键组成，17是惯性测量组件(IMU)的陀螺和加速度计信号的实时显示区，18是陀螺的温度显示区，19是装置位置和速度信息显示区，20是三维姿态角图形显示区。为了监控定位定向装置的输出信息，在装置的正面配备了TFT真彩显示器，可以实时显示载体的三维姿态、经纬度、高度和速度等信息。而这些是通过在导航计算机PC104中预装了DOS6.0系统，然后采用C语言编写人机接口程序来实现的。

打开计算机电源后，计算机先自检，装入操作系统，然后调入口令识别程序，此时计算机屏幕上显示“Program is ready to run...”。然后进入口令识别程序，等待用户输入口令，如果输入口令不正确或5秒钟内无键盘操作，系统自动运行组合式定位定向系统程序；通过键盘输入口令，如果口令正确，系统进入功能选择界面，出现以下选项：Run(系统运行)；Update(程序更新)，Setup(参数设置)；Quit(退出)。从而可以进行程序更新、参数设置等操作。系统运行主画面如图8所示。

另外，装置预留了2个232串口、2个422串口和1个网络接口，它们集中在一个航空接头里，可以与外界进行通信，为保证传输安全和提高抗干扰性能，采用光耦将外设备和定位定向装置进行隔离。

总之，本发明是将MEMS INS、GPS接收机iTrax02、三维数字电子罗盘HMR3000、微型压力高度计AXD550和微型压力空速计等系统集成到一个微型装置里，搭建一个组合式导航硬件平台，然后通过设计一种数据存储量和计算量较小、实时性好的Kalman滤波器，对各子系统的导航信息进行滤波，经过优化处理后的姿态、位置和速度等信息精度超过了装置中的任一单系统，这样用低成本、任务单一的器件组合成功能全、精度较高的定位定向系统，供外设备实现导航功能。

Claims (6)

1、一种低成本组合式定位定向装置，其组成包括由MEMS IMU和导航计算机组成的MEMS INS、GPS接收机、三维数字电子罗盘、高度计、气速计、电源模块、箱体及接插件等，其特征是：+24V直流电压进入电源模块后，通过DC-DC模块转换为±5V和+12V，供给相应的子系统，MEMS IMU的9只MEMS陀螺和MEMS加速度计分别测量出载体运动的角速度和加速度，其输出为相应的模拟电压值，通过数据采集电路将该电压值进行A/D转换，然后输入到MEMSINS的标定和导航程序中，得到载体运动的姿态、位置和速度等信息，同时，导航计算机通过232串口定时采集GPS接收机、电子罗盘、气速计和高度计的位置、姿态、速度和高度等信息。

2、根据权利要求1所述的低成本组合式定位定向装置，其特征是：所述的MEMS INS的9个MEMS陀螺选择ADXRS150、6个双轴加速度计选择ADXL202，9个MEMS陀螺和6个双轴加速度计安装在一个由4块平整的铜板组成的安装基座上，MEMS陀螺和双轴加速度计分布在三个正交面上。

3、根据权利要求1或2所述的低成本组合式定位定向装置，其特征是：所述的GPS接收机选择iTrax02，所述的三维数字电子罗盘选择HMR3000，所述的高度计选择AXD550。

4、根据权利要求1或2所述的低成本组合式定位定向装置，其特征是：MEMSINS和数字电子罗盘设置在箱体的中间，艏艉线从中穿过。

5、根据权利要求3所述的低成本组合式定位定向装置，其特征是：MEMS INS和数字电子罗盘设置在箱体的中间，艏艉线从中穿过。

6、一种采用权利要求1所述的低成本组合式定位定向装置的组合定位方法，其特征是：

(1)选用由MEMS IMU和导航计算机组成的MEMS INS、电源模块以及外界通讯插头、气速计、是高度计、GPS接收机、三维数字电子罗盘以及箱体组成的低成本组合式定位定向装置，其中MEMS IMU包括9只MEMS陀螺和6只双轴加速度计；

(2)MEMS INS姿态、位置和速度更新

在三维空间上的每个测量轴上有采用3个MEMS陀螺对同一个输入角速度进行测量，然后设计最优滤波器估计出各陀螺误差的大小，并对量测信息补偿校正，得到对输入角速率的高精度估计值，另一方面，每隔0.05秒采集一次三维数字电子罗盘HMR3000的姿态角信息，通过设计一个Kalman滤波器，估计陀螺的常值漂移，进而对陀螺测量的角速度信息进行定时补偿；

(3)对各导航单元信息进行融合

导航计算机高速采集多路陀螺和加速度计信号，实时计算出载体的姿态、位置和速度后，每隔1秒钟同时接收一次GPS接收机iTrax02的位置、速度和三维数字电子罗盘HMR3000的三维姿态角信息，然后以MEMS INS作为参考系统，分别与iTrax02和HMR3000组成子滤波器1和子滤波器2进行联邦滤波，滤波器的参数由预先的专家系统判断载体机动状态后作自适应调整，经过补偿后得到优化的姿态、位置和速度信息。

Images