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

专利名称：：一种低成本组合式定位定向装置及组合定位方法
技术领域：
：本发明涉及的是一种定位定向装置，特别是一种由微惯性导航系统(M五MS/iVS)、全球定位系统(G户5)、电子罗盘、微型压力高度计和微型压力空速计等构成的微型组合式定位定向装置。本发明还涉及一种采用该低成本组合式定位定向装置的定位定向方法。(二)
背景技术：
：惯性导航的基本原理是根据牛顿力学定律，通过测量载体在惯性参考系的加速度和角速度，将它对时间进行积分，且把它变换到导航坐标系中，就能够得到在导航坐标系中的速度、位置和姿态角信息等。所以，它是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统。具有很好的隐蔽性，其工作环境不仅包括空中、地球表面，还可以在水下。比如，1958年美国"皇后鱼"号核潜艇依靠惯性导航系统穿过北极在冰下航行21天；美国"阿波罗"号飞船上也装备了捷联式惯性导航系统。目前，惯性导航在海、陆、空、天等各领域都得到了广泛的应用。惯性导航的主要缺点是它的误差会随时间增长而迅速积累。而且由于启动时需要加温、初始对准，所以进入导航状态的时间较长，比如某型船用平台罗经需要约4小时。美国的全球定位系统(Gf5)是一个高精度、全天候和全球性的多功能无线电导航定位、定时系统，它的显著优点是精度均匀，定位误差不随时间积累。因而，目前G户S的定位导航技术发展非常迅速，应用极为广泛。但是GPS系统存在的问题是在使用时，城市的高大树木或高层建筑对GPS信号有遮挡效应，即城市大峡谷效应，会导致G/W定位失效。而且GPS是非自主式导航，数据更新频率低(一般为1/fe)，不能满足实时控制的要求，故需利用辅助装置与GPS组成组合定位系统。电子罗盘是以地磁北为基准测量物体方位的，在静止或运动状态下均能给出方位信息，且不受高大阻挡物的影响，因此高精度电子罗盘可以对GPS进行有效地补偿，即使在G尸S信号消失后也能正确地给出航向，也能保证导航定位信息的有效性。但是电子罗盘受外环境磁介质的影响较大，如果补偿不当，航向测量误差可达几十度。从上面的分析看到，将/iVS、G尸S和电子罗盘各有自己的长处，也都存在一定的短处，如果能将它们组合起来，构成组合式定位定向系统，各取所长而避其所短，无疑可以提高系统的精度和可靠性，降低对各分系统的要求，从而达到以较低的成本、构成中等精度的定位定向系统的目的。但是如何将它们结合起来，在系统结构设计；M五MS/iVS姿态、速度及位置更新算法；AffiMS/iVS与G尸S、数字电子罗盘州l狄3000、高度计、气速计等信息融合方式以及人机界面及接口等方面还存在许多需要解决的技术问题。
发明内容本发明的目的在于提供一种成本较低、功能较全的定位定向的低成本组合式定位定向装置。本发明的目的是这样实现的本发明的低成本组合式定位定向装置的组成包括由/Mf/和导航计算机组成的M五MS/iVS、G户S接收机、三维数字电子罗盘、高度计、气速计、电源模块、箱体及接插件等，其中AffiM/MC/包括9只M￡M陀螺和6只双轴加速度计。+24V直流电压进入电源模块，通过DC-DC模块转换为士5V和+12V，供给相应的子系统，系统加电启动后，MEMS陀螺和加速度计分别测量出载体运动的角速度和加速度，输出为相应的模拟电压值，通过数据采集电路将该电压值进行^4/￡)转换，然后输入到M五MS/A/5的标定或导航程序中，得到载体运动的姿态、位置和速度等信息，GPS接收机、电子罗盘、气速计和高度计都是通过232串口向导航计算机发送位置、姿态、速度和高度信息。本发明还可以包括这样一些结构特征1、所述的微惯性导航系统(AffiMS/iV5)的9个Affi:MS陀螺选择^aWSA50、6个双轴加速度计选择JDZi:2d9个M￡MS陀螺和6个双轴加速度计安装在一个由4块平整的铜板组成的安装基座上，陀螺和双轴加速度计分布在三个正交面上。2、所述的GPS接收机选择/rmx02，所述的三维数字电子罗盘选择/fM及30卵，所述的高度计选择^D5M。3、M五MS/A^和数字电子罗盘设置在箱体的中间，艏艉线从中穿过。本发明的低成本组合式定位定向装置的组合定位方法为1、选用由微惯性测量单元(Mi;MS/Mf7)和导航计算机组成微惯性导航系统(JkffiMS/7VS)、电源模块以及外界通讯插头、气速计、是高度计、GPS接收机、三维数字电子罗盘以及箱体组成的低成本组合式定位定向装置，ikffiMS/Mt/包括9只MEMS陀螺和6只双轴加速度计；2、微惯性导航系统(AffiM5/iV5)进行姿态、位置和速度更新在三维空间上的每个测量轴上有采用3个MEMS陀螺对同一个输入角速度进行测量，然后设计最优滤波器估计出各陀螺误差的大小，并对量测信息补偿校正，得到对输入角速率的高精度估计值，另一方面，每隔0.05秒采集一次三维数字电子罗盘/ZMiW0卯的姿态角信息，通过设计一个/^/w"w滤波器，估计陀螺的常值漂移，进而对陀螺测量的角速度信息进行定时补偿；3、对各导航单元信息进行融合导航计算机高速采集多路陀螺和加速度计信号，实时计算出载体的姿态、位置和速度后，每隔1秒钟同时接收一次G户S接收机/rmx02的位置、速度和三维数字电子罗盘i/Mi30卵的三维姿态角信息，然后以M五M5/iVS作为参考系统，分别与z丁rox02和//MiW0卯组成子滤波器1和子滤波器2进行联邦滤波，滤波器的参数由预先的专家系统判断载体机动状态后作自适应调整，经过补偿后得到优化的姿态、位置和速度信息。本发明的优点在于整个系统体积小、功耗低、成本低，但是功能齐全，接口丰富，完全能满足地面或空中运载体的中等精度定位定向要求。将M￡MS/A^、/rmx02、/W狄3000和mX550等各子系统的导航信息经A:a/m朋联邦滤波后，系统的定位定向精度超过了其单一子系统的精度。这样通过对低精度的异类传感器进行信息融合，有效地提高了整体的测量精度。已有技术中与本发明相当的定位定向装置采用分立式结构，其中惯性测量组件箱体积为400x400x650wm，导航计算机箱400x400x250wm，而本发明的装置的体积为200x250xl30wm，后者仅为前者的1/22，本系统的最大功耗仅为13W，平均功耗为4W。本发明的对外接口丰富，有2个232串口、2个422串口和1个网络接口等，如果需要，可以很方便地扩展CAN接口，完成姿态、航向、经度、纬度、高度、角速度以及线速度等导航信息的传送。本装置所有的成本不足万元，所采用的J^ffiMS陀螺约几百元，而传统的惯性导航系统的陀螺约十万余元。虽然器件的精度低，但是通过组合后精度有很大的提高，达到了中等精度的水平。并且由于其体积小、功耗低，所以只需调整部分参数后，可以应用于智能轮椅自动导航、高速列车轨道测量、智能炸弹导航和空中摄影等领域中。(四)图1是本发明的低成本组合式定位定向装置的内部结构示意图2是本发明的低成本组合式定位定向装置的各子单元连接关系示意图3是A/五MS陀螺和加速度计的安装示意图4是陀螺测量信息融合原理图5是ikffiMS/iVS姿态、位置和速度更新示意图6是多信息融合联邦滤波器工作原理框图7是组合式定位定向装置的工作流程图8是组合式定位定向装置的人机交互界面图。(五)具体实施例方式下面结合附图举例对本发明做更详细地描述-结合图1。低成本组合式定位定向装置的组成包括微惯性测量单元(JWEM/ML01、气速计2、高度计3、GPS接收机4、维数字电子罗盘5、电源模块6、与外界通讯的14芯航空插头7、4芯电源插头9和导航计算机15，微惯性测量单元包括9只MEMS陀螺和6只双轴加速度计，它与导航计算机组成微惯性导航系统(^ffiMS/A/S)，能实时测量载体的姿态、位置和速度。11是气速计与外界连接的通气孔。高度计选用^D550，IO是与高度计其相应的通气孔，通过测量外界气压，计算载体所在的高度。GPS接收机选用/7)m肌8是GPS接收机的信号接收天线。//0恥，6//三维数字电子罗盘选择/^汉3000。电源模块提供士5V和12V直流电源。与外界通讯的14芯航空插头包括2路232串口，2个422串口和1个网络通讯口。4芯电源插头为装置提供24V直流电源。12是铝合金材7料制成的一体式箱体。13是装置的艏艉线，即中心轴线，为使用时提供安装基准。15是导航计算机，包括模拟信号调理电路，A。转换电路和通讯接口电路等。结合图2。+24V直流电压进入装置以后，通过Z)C-DC模块转换为士5V和+12V,供给各子系统。惯性测量单元(/A/f7)在接通电源后，它内部的陀螺和加速度计开始工作，分别测量出载体运动的角速度和加速度，输出为相应的模拟电压值，通过数据采集电路将该电压值进行^/D转换，然后输入到/A/S的标定或导航程序中做相应的处理，得到载体运动的姿态、位置和速度等信息。其他设备如GPS接收机、电子罗盘、气速计和高度计都是通过232串口按一定的频率向导航计算机发送位置、姿态、速度和高度信息。在系统设计时，考虑到数字电子罗盘//M/卯(W容易受外界环境磁介质的影响，整个装置的箱体釆用铝合金材质。同时，为了保证JUffiA/S/iViS和数字电子罗盘的姿态、航向测量精度，将两者安置在箱体的中间，艏艉线从中穿过。另外，为了提高微惯性导航系统(M五MS/iV5)的精度，陀螺和加速度计都采用了冗余技术，结合图3，9个AffiMS陀螺^M7SA50和6个双轴加速度计安装在一个由4块平整的铜板组成的安装基座上，陀螺和加速度计分布在三个正交面上，其正交性通过机械加工保证。微惯性导航系统(AffiM5/iV5)导航算法由于定位定向装置中的惯性测量组件(/MW选用的是M五M5惯性器件，包括^DZjL202加速度计和^i)XiS"0陀螺，它们的测量分辨率低，噪声大。比如陀螺^ZMKS"0漂移经补偿后约为无法敏感地球自转角速度叫e(-15。//0，也即ft^完全淹没在陀螺噪声里，这样传统的导航更新算法无法适用。结合图4和图5，改进的方案里一方面釆用同类传感器多信息融合技术，在三维空间上的每个测量轴上采用3个陀螺对同一个输入角速度进行测量，然后设计最优滤波器估计出各陀螺误差的大小，并对量测信息补偿校正，得到对输入角速率的高精度估计值。另一方面，根据三维数字电子罗盘/ZA^邓W的数据更新频率为20Hz，所以每隔0.05秒采集一次i/MUW0的姿态角信息，通过设计一个尺"/mcm滤波器，估计陀螺的常值漂移，进而对陀螺测量的角速度信息进行定时补偿。通过这两方面的修正后，大大地提高了陀螺的测量精度，降低了陀螺的随机漂移误差量。各导航单元信息融合方法结合图6和图7。导航计算机高速采集多路陀螺和加速度计信号，实时计算出载体的姿态、位置和速度后，每隔1秒钟同时接收一次G尸S接收机z'7Vox02的位置、速度和//Mi30卯的三维姿态角信息，然后以M￡MS/A/S作为参考系统，分别与/rrox^和/^flW0W组成子滤波器1和子滤波器2进行联邦滤波，滤波器的参数由预先的专家系统判断载体机动状态后作自适应调整，这样经过补偿后得到优化的姿态、位置和速度信息。将其与前面的M五MS/7VS导航计算过程结合在一起，可以总结出系统的整个工作流程。人机交互界面结合图8。导航计算机采用嵌入式工业计算机PC7W，它具有键盘和显示器接口。并在计算机预装了DOS6力系统，然后采用C语言编程，可以实现组合式定位定向装置的人机交互功能。本发明中的/Aft/选用硅微陀螺^Z)^7S/50和硅微加速度计XiMI2M，为提高精度和保证可靠性，三个正交敏感轴的每个轴向各安装了三个陀螺和两个双轴加速度计，这样需要采集的信号包括9路陀螺、12路加速度计和9路陀螺温度共30路模拟信号，数据采集电路中的模/数G4/0)转换芯片选用美国^D/公司的^D7656，它是一种6通道16位高精度、高速JD芯片，工作时由一个16位的凌阳单片机S尸C五06L4控制。将采集的各路数据在导航计算机尸C7似中进行实时导航解算处理，得到的由M5MS/A/iS解算出来的姿态、位置和速度等信息，下面详细介绍姿态更新算法。设载体横滚角y，俯仰角0，航向角v，那么由载体坐标系6到"东北天"地理坐标系w的方向余弦矩阵可以表示为cosycosy-sinysin^siny-cos^sin^sinycosy+cosysin0sin^T=cos^siny+sinysin0cosycos9cosysinysin^/—cosysin0cosy—sinycos^sin^cosycos0对r微分可得到如下关系<formula>formulaseeoriginaldocumentpage9</formula>为了构建ATfl/附fl"滤波方程，取矩阵T的第3行元素[ns，。3力3]作为状态变量，此外，由于陀螺信号的漂移误差会给姿态解算的精度带来很大的影响，因此有必要对陀螺的漂移误差进行实时估计。即取x+31&&&￡yez](2)其中"31、r32、为矩阵r的第3行元素；￡2为三轴陀螺的漂移；当M五MS/iVS在没有加速的情况下，三轴加速度计测量值为0cosycos0取A、fly、flz为观测量，并设其单位为重力加速度g,即7=[","z]。那么，扩展/^/附fl"滤波方程的状态空间模型可表示为(3)式中:爿=0000..0-(化—0o,=r330并且，前3个状态变量满足—&0C=[/3x30]，v为过程噪声向量，w为测量噪声向量。"323=1(4)将式(3)离散化，利用Kfl/ma"滤波递推公式可以估计出r13、r23、^33三个状态变量，从而解算出俯仰角和横滚角。在此过程中，为了提高系统精度，设计一个专家系统，通过微型压力高度计、空速计和i^ffiMS/iVS的信息判断载体的机动状态，以选择不同的/^//^"滤波器参数。由于系统无法进行初始对准，获得精确的航向，为了弥补该不足，利用装置中的电子罗盘，通过测量地球磁场，快速地获取载体的航向角。结合①中所计算得到的两个水平姿态角，传递给A/五MS/iVS，完成姿态传递对准。这中间产生的误差一方面可以通过高度回路予以修正，另一方面将通过设计的联邦滤波器进行误差参数在线辨识和实时补偿，这样将误差降到最小，而且使姿态、位置误差不随时间增长而快速发散。根据组合导航典型联邦滤波器的体系结构，M￡MS/7V5/OPS///Mi0(W组合的联邦滤波器结构如图5示，捷联系统和GPS组合为子滤波器1，捷联系统和电子罗盘组合为子滤波器2，主滤波器对公共状态进行融合。在子滤波器1中,取M￡MS/7VS的10个状态向量和G尸S的3个姿态向量作为组合系统的状态变量x-[坤,说，W￡5<5a,邵,々，f￡fz&g^g々J，其中，G尸S的3个姿态向量用一阶马尔科夫过程表示<formula>formulaseeoriginaldocumentpage11</formula>^为G尸S系统的相关时间的导数，wg为激励的马尔科夫过程的白噪声。组合系统状态方程为<formula>formulaseeoriginaldocumentpage11</formula>(6)(6)式中^=<formula>formulaseeoriginaldocumentpage11</formula>以AffiMS/iVS和G户S的姿态之差作为组合系统的观测量，则M五iWS/iVS/G户S的观测方程为<formula>formulaseeoriginaldocumentpage11</formula>其中巧(0=。利用/ZA/i30W测量姿态时，关键就是航向角的测量，//i^i3WW测量的是沿载体坐标系三轴方向上的磁场强度，由此推算出的航向角是磁航向^。取/^,/^,私,为磁场沿地理坐标系三轴的分量，/^,/^，/4/磁场为沿舰船坐标系的各分量，则它们之间有如下关系<table>tableseeoriginaldocumentpage12</column></row><table>(9)所以^g—1，。设真北与磁北方向差角即磁差为Aa，则真航向角ar为"r="m+A"(10)由//Mi3卵0测量姿态角的原理可知其测量误差主要有2个倾斜角的偏移误差^r、卸r，磁强计测量误差(5ar，将它们都用一阶马尔可夫过程表示=—+wn<5秀=-/cy稱+wr2(11)对M￡M5/iVS///Mi3OO0的组合模式，我们也采用与M￡MS/iVS/G户S相似的处理方法，将&3、邵3、々3扩充为状态变量，状态方程为柳,)+,(12)将//M及3000和M￡MS/iVS输出的姿态信息的差值作为组合模式的测量值，则AffiMS/iVS/7ZMi邓W组合的观测方程为z2=執-祸=i/2;c2+v2(13)其中<formula>formulaseeoriginaldocumentpage12</formula>一l(14)建立了2个子系统M五MS/A^/G/^和AffiMS/A^/Z/M及JW0的数学模型后，将其状态方程和观测方程离散化，就可通过线性离散卡尔曼滤波，估计出组合系统的姿态信息误差。根据A/￡MS/A/S/OPS/Z/il狄3W0组合的特点，以M￡MS7TVS作为参考系统，分别与!7>似02、//M及30卯组成2个子滤波器——AffiM5/AW/GPS组合和M五MS组合，然后根据联邦滤波器的结构组成，设计不同的信息分配系数，运用联邦滤波器算法，进行全局滤波，以得到较优的定位定向信息。/iV5的姿态、位置更新频率为200/fe，而G尸S接收机/rrax^和三维数字电子罗盘Z/Mi3W0的更新频率为1/fe，所以，联邦滤波器中以M五MS/TVS为参考系统，G户S和电子罗盘的导航信息对其进行定时修正，在技术方案所描述的联邦滤波器具体参数设置如下①滤波器状态向量初始值"0/0)=0;②滤波误差方差尸(0/0)^^agW(0/0)，尸2(0/0)，A，户16(0/0))，《(0/0)为相应的状态向量的均方差五(x;2)，不为零的元素为<formula>formulaseeoriginaldocumentpage13</formula>互界面如图6所示，其中16是键盘区，由数字键和功能键组成，17是惯性测量组件(IMU)的陀螺和加速度计信号的实时显示区，18是陀螺的温度显示区，19是装置位置和速度信息显示区，20是三维姿态角图形显示区。为了监控定位定向装置的输出信息，在装置的正面配备了TFT真彩显示器，可以实时显示载体的三维姿态、经纬度、高度和速度等信息。而这些是通过在导航计算机PC104中预装了DOS6.0系统，然后采用C语言编写人机接口程序来实现的。打开计算机电源后，计算机先自检，装入操作系统，然后调入口令识别程序，此时计算机屏幕上显示"Programisreadytorun..."。然后进入口令识别程序，等待用户输入口令，如果输入口令不正确或5秒钟内无键盘操作，系统自动运行组合式定位定向系统程序；通过键盘输入口令，如果口令正确，系统进入功能选择界面，出现以下选项Run(系统运行)；Update(程序更新)，Setup(参数设置)；Quit(退出)。从而可以进行程序更新、参数设置等操作。系统运行主画面如图8所示。另夕卜，装置预留了2个232串口、2个422串口和1个网络接口，它们集中在一个航空接头里，可以与外界进行通信，为保证传输安全和提高抗干扰性能，采用光耦将外设备和定位定向装置进行隔离。总之，本发明是将/A/S、G尸S接收机/rmx02、三维数字电子罗盘i/Mi3WW、微型压力高度计A^D550和微型压力空速计等系统集成到一个微型装置里，搭建一个组合式导航硬件平台，然后通过设计一种数据存储量和计算量较小、实时性好的i^/mfl"滤波器，对各子系统的导航信息进行滤波，经过优化处理后的姿态、位置和速度等信息精度超过了装置中的任一单系统，这样用低成本、任务单一的器件组合成功能全、精度较高的定位定向系统，供外设备实现导航功能。权利要求1、一种低成本组合式定位定向装置，其组成包括由MEMSIMU和导航计算机组成的MEMSINS、GPS接收机、三维数字电子罗盘、高度计、气速计、电源模块、箱体及接插件等，其特征是+24V直流电压进入电源模块后，通过DC-DC模块转换为±5V和+12V，供给相应的子系统，MEMSIMU的9只MEMS陀螺和MEMS加速度计分别测量出载体运动的角速度和加速度，其输出为相应的模拟电压值，通过数据采集电路将该电压值进行A/D转换，然后输入到MEMSINS的标定和导航程序中，得到载体运动的姿态、位置和速度等信息，同时，导航计算机通过232串口定时采集GPS接收机、电子罗盘、气速计和高度计的位置、姿态、速度和高度等信息。2、根据权利要求l所述的低成本组合式定位定向装置，其特征是所述的M￡M5/iVS的9个M五MS陀螺选择^DZiS/50、6个双轴加速度计选择XDv江202，9个陀螺和6个双轴加速度计安装在一个由4块平整的铜板组成的安装基座上，M￡MS陀螺和双轴加速度计分布在三个正交面上。3、根据权利要求1或2所述的低成本组合式定位定向装置，其特征是所述的GPS接收机选择!Trax02，所述的三维数字电子罗盘选择/fM及3卯0，所述的高度计选择AO)550。4、根据权利要求l或2所述的低成本组合式定位定向装置，其特征是:JkffiMS/WS和数字电子罗盘设置在箱体的中间，艏艉线从中穿过。5、根据权利要求3所述的低成本组合式定位定向装置，其特征是:/A/5和数字电子罗盘设置在箱体的中间，艏艉线从中穿过。6、一种采用权利要求1所述的低成本组合式定位定向装置的组合定位方法，其特征是(1)选用由7MC7和导航计算机组成的M￡MS/iVS、电源模块以及外界通讯插头、气速计、是高度计、GPS接收机、三维数字电子罗盘以及箱体组成的低成本组合式定位定向装置，其中M￡MS/Mf/包括9只MEMS陀螺和6只双轴加速度计；(2)A/￡MS/A^姿态、位置和速度更新在三维空间上的每个测量轴上有采用3个MEMS陀螺对同一个输入角速度进行测量，然后设计最优滤波器估计出各陀螺误差的大小，并对量测信息补偿校正，得到对输入角速率的高精度估计值，另一方面，每隔0.05秒采集一次三维数字电子罗盘/ZM及^WO的姿态角信息，通过设计一个〖fl/ma"滤波器，估计陀螺的常值漂移，进而对陀螺测量的角速度信息进行定时补偿；(3)对各导航单元信息进行融合导航计算机高速采集多路陀螺和加速度计信号，实时计算出载体的姿态、位置和速度后，每隔l秒钟同时接收一次GPS接收机irmx02的位置、速度和三维数字电子罗盘//MiW卵0的三维姿态角信息，然后以M￡MS/iVS作为参考系统，分别与!丁rnx02和组成子滤波器1和子滤波器2进行联邦滤波，滤波器的参数由预先的专家系统判断载体机动状态后作自适应调整，经过补偿后得到优化的姿态、位置和速度信息。全文摘要本发明提供的是一种低成本组合式定位定向装置及组合定位定向方法。该装置主要包括由MEMSIMU和导航计算机组成的MEMSINS、GPS接收机、三维数字电子罗盘、高度计、气速计、电源模块、箱体及接插件等，其中MEMSIMU包括9只MEMS陀螺和6只双轴MEMS加速度计，由它们分别测量出运动载体的角速度和加速度，然后输入到导航解算程序中，计算得到载体的姿态、位置和速度等信息。同时导航计算机通过232串口定时采集GPS接收机、电子罗盘、气速计和高度计等导航子单元的信息，通过信息融合处理，得到载体最优的位置、姿态、速度和高度等导航信息，精度优于装置中单一的子系统。本发明体积小、功耗低、成本低，但是功能齐全，接口丰富，完全能满足地面或空中运载体的中等精度定位定向要求。文档编号G01C21/10GK101319902SQ20081006494公开日2008年12月10日申请日期2008年7月18日优先权日2008年7月18日发明者蕾任,何昆鹏,张勤拓,肖吕怡,胡文彬,许德新,颖郝,高延滨申请人:哈尔滨工程大学