专利名称：微小型个人组合导航系统及其导航定位方法
技术领域：
本发明属于惯性与组合导航领域，特别是一种基于MEMS惯性传感器与其他辅助导航设备的个人组合导航系统的结构设计及其导航定位方法。
背景技术：
随着导航定位技术的迅速发展，个人导航定位已发展成为导航定位技术中的一个重要分支。近年来随着应用环境的不断拓展，个人导航定位技术的性能与环境适应性被提出了新的要求。目前较为成熟的个人导航定位方案多数是基于卫星导航系统实现定位功能，例如，美国、英国、法国为等多个军事发达国家所实施“陆地勇士”、“FIST”、“FELIN”等多种高技术士兵装备计划中，均以卫星导航系统为作战士兵提供定位信息。但在城市、室内环境中，卫星导航接收机由于受到信号遮挡、电磁干扰或多路径效应等因素影响而无法正常工作，个人导航系统的性能将无法得到保障，而传统的惯性/卫星组合导航方案又由于功耗、 体积、成本等原因无法应用于个人导航定位。因此，近年来国外较多的研究机构对于城市、 室内等无卫星导航环境中的个人导航定位进行了深入的研究，其中大部分研究内容均与 MEMS惯性导航技术有紧密的联系。美国Draper实验室在本世纪初提出并设计了基于微机电惯性测量组件的个人导航定位方案，并研究了行进中的零速修正与静止状态下的零姿态修正方法，实时修正惯性导航系统与微惯性传感器的误差；瑞士 Vectronix公司的研究部门研制的个人导航系统将磁传感器与MEMS陀螺仪相结合，以克服环境中的磁场干扰对航向信息的影响，并采用高精度计步器探测人体的运动状态与加速度，通过优化后的多传感器信息融合算法来实现个人导航。国外某些研究机构正致力于研究基于空间定位理论的红外光束扫描、激光测距、光学成像等室内导航定位的方法，以及通过无线电通信、无线网络等技术实现个人实时定位的理论，丰富了个人导航定位的途径与手段。在国内，南京航空航天大学导航研究中心与香港理工大学联合研究了以视觉障碍者紧急避障、个人定位、语音导航等服务为应用背景、结合MEMS惯性技术的行人导航方法，; 西北工业大学电子信息学院研究了一种基于多组MEMSIMU的人体动作检测实现士兵虚拟训练中动作识别的方法，该方法通过在士兵头部、枪部、腿部各安装一组MEMS惯性测量组件感知人体各部位的力学特征信息，从而实现对人体动作的识别；复旦大学微电子系与美国模拟器件公司联合研制了一种基于人体行走模型的高精度计步器；上海交通大学仪器科学与工程系研究了一种基于人体动作识别和步幅估计的步行者航位推算方法，并研制了蓝牙传感器终端；此外，哈尔滨工业大学、解放军理工大学、中北大学等高校的相关研究部门也针对个人导航定位相关技术进行了深入的理论研究，部分研究成果已进入工程应用阶段。上述个人导航技术的相关研究，均无法在室内外复杂电磁环境中实现长时间连续、实时的姿态、速度、位置全参数导航。
发明内容
本发明的目的在于提供一种一种分布式结构的个人导航定位系统，将MEMS惯性传感器安装于人体足部，通过MEMS微惯性导航系统与其他多种量测方法进行信息融合来提高导航定位性能。实现本发明目的的技术解决方案为一种微小型个人组合导航系统，该系统由安装在人体足部的分系统与安装在躯干部位的分系统构成，足部分系统包括微机电MEMS IMU (Micro Electro Mechanical System,简称为MEMS)惯性测量单元(Inertial Measurement Unit，简称为IMU)、磁强计与第一通讯模块，其中磁强计的方位轴与MEMS IMU的方位轴重合安装，其输出端并分别与第一通讯模块相连接，躯干部位的分系统包括卫星导航接收机、 多普勒测速仪、气压高度计、第二通讯模块以及微型导航计算机，其中卫星导航接收机、多普勒测速仪、气压高度计分别与微型导航计算机相连接，微型导航计算机与第二通讯模块相连接，第二通讯模块通过无线通讯的方式与第一通讯模块连接并接收其发送的数据。一种基于微小型个人组合导航系统的导航定位方法，由采集MEMS IMU信号、陀螺仪与加速度计的随机误差建模与校正、惯性导航系统初始对准、捷联惯性导航解算、惯性导航系统零速修正、MEMS惯性导航系统/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航五部分组成，导航定位过程中首先通过导航计算机采集MEMS IMU中陀螺仪与加速度计信号，对陀螺仪与加速度计进行随机误差建模与校正、并采用校正后的数据完成惯性导航系统初始对准，在此基础上进行捷联惯性导航解算，并在足部着地时间段进行惯性导航系统零速修正，将经过修正的捷联惯性导航结果与卫星导航接收机、多普勒测速仪、 磁强计、气压高度计完成多信息融合组合导航，具体步骤如下
(1)MEMSIMU信号采集步骤采集MEMS IMU中三轴MEMS陀螺仪与三轴MEMS加速度计的输出信号，得到捷联惯性导航解算所需的角速度和比力，并将角速度和比力信号通过安装于足部与躯干的通讯模块传输给微型导航计算机；
(2)MEMS惯性器件的随机误差的建模与误差校正步骤在微型导航计算机中对步骤 (1)采集的MEMS陀螺仪与MEMS加速度计数据分别建立求和自回归滑动平均模型即ARIMA 模型，将MEMS陀螺仪与MEMS加速度计实际输出作为观测量，将MEMS陀螺仪与MEMS加速度计的理论输出作为状态量，建立基于ARIMA模型的卡尔曼滤波器，实时估计MEMS陀螺仪与 MEMS加速度计的理论输出，并将理论输出估计值作为初始对准步骤、捷联惯性导航步骤、零速修正步骤以及组合导航的数据源；
(3)惯性导航系统初始对准步骤该步骤在微型导航计算机中进行，微小型个人组合导航系统启动后在静态条件下，利用步骤(2)中MEMS加速度计的理论输出估计值，在微型导航计算机中通过水平自对准得到MEMSIMU的初始横滚角与俯仰角，并将磁强计的信号输出通过通讯模块传送至微型导航计算机以得到MEMS IMU的初始航向角；
(4)捷联惯性导航解算步骤该步骤在微型导航计算机中进行， 由姿态、速度、位置解算三步组成，惯导系统姿态解算首先利用公式 < =^计算机体相对导航坐标系的角速率 ^,其中为MEMS陀螺仪的理论输出估计值，·β^采用多普勒测速仪输出、地球自转角速度·2^以及上个周期的姿态转移矩阵Cf求得，然后采用四元数微分方程求解姿态转移矩阵C1■对应的四元数，并将得到的四元数规范化后生成姿态转移矩阵Cf，最后从(，中提取横滚角y、俯仰角&航向角#，MEMS惯性导航系统的速度、位置解算首先将加速度汁理论输出估计值通过C^的转置矩阵Ci从载体坐标系转换到导航坐标系，并通过解算惯性比力微分方程得到MEMS惯性导航系统的速度信肩· t再将* ^ 行一次积分得到系统的位置信息；
(5)惯性导航系统零速修正步骤该步骤在微型导航计算机中进行，求解MEMS加速度计输出经步骤(2)误差建模与校正后的估计值的三轴矢量和，通过判断该值是否接近重力来检测人体足部是否处于着地时间段，并利用着地时间段MEMSIMU相对地面静止的特点进行间断式的零速修正，即建立惯性导航系统误差状态方程，以足部着地时间段MEMS惯性系统的水平速度输出为量测量建立量测方程，运用卡尔曼滤波器估计惯性导航系统的平台误 ■ 『Μη，MEMS陀螺仪随机常值误差4、6、S,以及MEMS加速度计随机常值误差Uj, 并以反馈校正的方式提高惯性导航系统的水平姿态精度与惯性器件精度；
(6)MEMS惯性导航系统/卫星导航系统多普勒测速仪/磁强计/气压高度计组合导航步骤该步骤在导航计算机中进行，1)构建MEMS惯性/卫星组合导航子系统即子系统I， 将MEMS惯性导航系统与卫星导航系统输出传送至微型导航计算机构建组合导航子系统， 根据惯性导航系统误差方程与惯性器件误差特性建立状态方程，以惯性导航系统与卫星导航系统所测量的位置、速度信息之间的差值为量测量建立量测方程，采用卡尔曼滤波实时估计惯性导航系统与惯性器件的误差；2)构建MEMS惯性/多普勒组合导航子系统即子系统II，将MEMS惯性导航系统与多普勒测速系统输出传送至微型导航计算机构建组合导航子系统，根据惯性导航系统误差方程与惯性器件误差特性建立状态方程，以惯性导航系统与多普勒测速系统所测量的速度信息之间的差值为量测量建立量测方程，采用卡尔曼滤波实时估计惯性导航系统与惯性器件的误差；3)构建惯性/气压高度计组合导航子系统III 将MEMS惯性导航系统与气压高度计输出传送至微型导航计算机构建组合导航子系统，根据惯性导航系统误差方程与惯性器件误差特性建立状态方程，以惯性导航系统与气压高度计所测量的高度信息之间的差值为量测量建立量测方程，采用卡尔曼滤波实时估计惯性导航系统与惯性器件的误差；4)利用组合导航子系统I、II、III建立联邦滤波器，进一步估计惯性导航系统与惯性器件误差，从而提高惯性导航系统与惯性器件误差的估计精度，并通过闭环反馈校正提高惯性导航系统的位置、速度与姿态精度。本发明与现有技术相比，其显著优点(1)本发明将个人导航系统设计为由足部分系统与躯干分系统构成的分布式结构，两个分系统之间通过无线通讯模块进行通讯，有效减轻了导航系统中各类传感器之间的电磁干扰，并使人体躯干与足部分担系统的重量， 更适宜于个人便携；
(2)本发明将MEMSIMU与磁强计安装于人体足部，在进行捷联惯性导航解算的同时，可利用足部着地时间段对惯性导航系统进行零速修正，提高了惯性导航系统的性能；
(3)本发明采用了MEMS惯性导航系统/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航方案，可在复杂电磁环境中实现连续、实时、全参数的导航定位功能。
图1为个人组合导航系统结构图1 ； 图2为个人组合导航系统结构图2 ；图3为组合导航方法流程图。
具体实施例方式下面结合附图对本发明作进一步详细描述。本发明旨在便携式的个人装备中，安装MEMSIMU、卫星导航接收机、多普勒测速仪、 气压高度计等设备，探索个人便携式导航定位的新途径，为行人、作业人员、作战士兵、救援人员等提供实时、精确、完整的导航定位信息。本发明一种微小型个人组合导航系统，该组合导航系统由安装在人体足部的分系统与安装在躯干部位的分系统构成，足部分系统包括MEMS IMU1、磁强计2与第一通讯模块 3，其中磁强计的方位轴与MEMS IMU的方位轴重合安装，其输出端并分别与第一通讯模块相连接，躯干部位的分系统包括卫星导航接收机4、多普勒测速仪5、气压高度计6、第二通讯模块7以及微型导航计算机8，其中卫星导航接收机、多普勒测速仪、气压高度计分别与微型导航计算机相连接，微型导航计算机与第二通讯模块相连接，第二通讯模块通过无线通讯的方式与第一通讯模块连接并接收其发送的数据。组合导航系统的结构如图1与图2所
7J\ ο本发明导航定位方法由采集MEMS IMU信号、陀螺仪与加速度计的随机误差建模与校正、惯性导航系统初始对准、捷联惯性导航解算、惯性导航系统零速修正、MEMS惯性导航系统/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航五部分组成，导航定位过程中首先通过导航计算机采集MEMS IMU中陀螺仪与加速度计信号，对陀螺仪与加速度计进行随机误差建模与校正、并采用校正后的数据完成惯性导航系统初始对准，在此基础上进行捷联惯性导航解算，并在足部着地时间段进行惯性导航系统零速修正，将经过修正的捷联惯性导航结果与卫星导航接收机、多普勒测速仪、磁强计、气压高度计完成多信息融合组合导航，组合导航方法流程如图3所示，通过以下步骤实现
(1)MEMSIMU信号采集步骤采集MEMS IMU中MEMS陀螺与MEMS加速度计的输出信号， 得到惯导系统的角速度和比力，并将信号通过安装于足部与躯干的通讯模块传输给微型导航计算机；
(2)MEMS惯性器件的随机误差的建模与误差校正步骤在微型导航计算机中对步骤 (1)采集的MEMS陀螺仪与MEMS加速度计数据分别建立求和自回归滑动平均模型即ARIMA 模型，将MEMS陀螺仪与MEMS加速度计实际输出作为观测量，将MEMS陀螺仪与MEMS加速度计的理论输出作为状态量，建立基于ARIMA模型的卡尔曼滤波器，实时估计MEMS陀螺仪与 MEMS加速度计的理论输出，并将理论输出估计值作为初始对准步骤、捷联惯性导航步骤、零速修正步骤以及组合导航的数据源；
(3)惯性导航系统初始对准步骤该步骤在微型导航计算机中进行，系统启动后在静态条件下，利用步骤(2)中MEMS加速度计的理论输出估计值，在微型导航计算机中通过水平自对准得到MEMS IMU初始横滚角与俯仰角，并将磁强计的信号输出通过通讯模块1与2传送至微型导航计算机以得到MEMS IMU的初始航向角，水平自对准的公式为
θ =Brcsm(Jpg) , y=arCSm(-，其中ρ力俯仰角，；κ为横滚角，g为重力加速度， 与f 分别为加速度计理论输出估计值在载体坐标系中投影的χ与1轴分量；(4)捷联惯性导航解算步骤该步骤在导航计算机中进行，由姿态解算与速度、位置解算组成，惯导系统姿态解算首先利用公式=计算机体相对导航坐标系角
速本 ^式中为MEMS陀螺仪的理论输出估计值，可采用行进中的速度在导航坐标系中的投影、地球自转角速度辦-以及上个周期的姿态转移矩阵(才求得，然后采用四元
权利要求
1.一种微小型个人组合导航系统，其特征在于该系统由安装在人体足部的分系统与安装在躯干部位的分系统构成，足部分系统包括MEMS IMU[1]、磁强计[2]与第一通讯模块 [3]，其中磁强计[2]的方位轴与MEMS IMU的方位轴重合安装，其输出端并分别与第一通讯模块[3]相连接，躯干部位的分系统包括卫星导航接收机W]、多普勒测速仪[5]、气压高度计W]、第二通讯模块[7]以及微型导航计算机[8]，其中卫星导航接收机W]、多普勒测速仪[5]、气压高度计[6]分别与微型导航计算机[8]相连接，微型导航计算机[8]与第二通讯模块[7]相连接，第二通讯模块[7]通过无线通讯的方式与第一通讯模块[3]连接并接收其发送的数据。
2.一种基于微小型个人组合导航系统的导航定位方法，其特征在于由采集MEMS IMU 信号、陀螺仪与加速度计的随机误差建模与校正、惯性导航系统初始对准、捷联惯性导航解算、惯性导航系统零速修正、MEMS惯性导航系统/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航五部分组成，导航定位过程中首先通过导航计算机采集MEMS IMU 中陀螺仪与加速度计信号，对陀螺仪与加速度计进行随机误差建模与校正、并采用校正后的数据完成惯性导航系统初始对准，在此基础上进行捷联惯性导航解算，并在足部着地时间段进行惯性导航系统零速修正，将经过修正的捷联惯性导航结果与卫星导航接收机、多普勒测速仪、磁强计、气压高度计完成多信息融合组合导航，具体步骤如下(1)MEMSIMU信号采集步骤采集MEMS IMU中三轴MEMS陀螺仪与三轴MEMS加速度计的输出信号，得到捷联惯性导航解算所需的角速度和比力，并将角速度和比力信号通过安装于足部与躯干的通讯模块传输给微型导航计算机；(2)MEMS惯性器件的随机误差的建模与误差校正步骤在微型导航计算机中对步骤 (1)采集的MEMS陀螺仪与MEMS加速度计数据分别建立求和自回归滑动平均模型即ARIMA 模型，将MEMS陀螺仪与MEMS加速度计实际输出作为观测量，将MEMS陀螺仪与MEMS加速度计的理论输出作为状态量，建立基于ARIMA模型的卡尔曼滤波器，实时估计MEMS陀螺仪与 MEMS加速度计的理论输出，并将理论输出估计值作为初始对准步骤、捷联惯性导航步骤、零速修正步骤以及组合导航的数据源；(3)惯性导航系统初始对准步骤该步骤在微型导航计算机中进行，微小型个人组合导航系统启动后在静态条件下，利用步骤(2)中MEMS加速度计的理论输出估计值，在微型导航计算机中通过水平自对准得到MEMSIMU的初始横滚角与俯仰角，并将磁强计的信号输出通过通讯模块传送至微型导航计算机以得到MEMS IMU的初始航向角；(4)捷联惯性导航解算步骤该步骤在微型导航计算机中进行， 由姿态、速度、位置解算三步组成，惯导系统姿态解算首先利用公式= - ^计算机体相对导航坐标系的角速率其中为MEMS陀螺仪的理论输出估计值采用多普勒测速仪输出、地球自转角速度以及上个周期的姿态转移矩阵Cf求得，然后采用四元数微分方程求解姿态转移矩阵C^对应的四元数，并将得到的四元数规范化后生成姿态转移矩阵Cf，最后从Cf中提取横滚角y、俯仰角&航向角#，MEMS惯性导航系统的速度、位置解算首先将加速度计理论输出估计值通过Cf的转置矩阵从载体坐标系转换到导航坐标系，并通过解算惯性比力微分方程得到MEMS惯性导航系统的速度信息再将进行一次积分得到系统的位置信息；(5)惯性导航系统零速修正步骤该步骤在微型导航计算机中进行，求解MEMS加速度计输出经步骤(2)误差建模与校正后的估计值的三轴矢量和，通过判断该值是否接近重力来检测人体足部是否处于着地时间段，并利用着地时间段MEMSIMU相对地面静止的特点进行间断式的零速修正，即建立惯性导航系统误差状态方程，以足部着地时间段MEMS惯性系统的水平速度输出为量测量建立量测方程，运用卡尔曼滤波器估计惯性导航系统的平台误差角‘、各、各,MEMS陀螺仪随机常值误差以及MEMS加速度计随机常值误差并以反馈校正的方式提高惯性导航系统的水平姿态精度与惯性器件精度；(6)MEMS惯性导航系统/卫星导航系统多普勒测速仪/磁强计/气压高度计组合导航步骤该步骤在导航计算机中进行，1)构建MEMS惯性/卫星组合导航子系统即子系统I， 将MEMS惯性导航系统与卫星导航系统输出传送至微型导航计算机构建组合导航子系统， 根据惯性导航系统误差方程与惯性器件误差特性建立状态方程，以惯性导航系统与卫星导航系统所测量的位置、速度信息之间的差值为量测量建立量测方程，采用卡尔曼滤波实时估计惯性导航系统与惯性器件的误差；2)构建MEMS惯性/多普勒组合导航子系统即子系统II，将MEMS惯性导航系统与多普勒测速系统输出传送至微型导航计算机构建组合导航子系统，根据惯性导航系统误差方程与惯性器件误差特性建立状态方程，以惯性导航系统与多普勒测速系统所测量的速度信息之间的差值为量测量建立量测方程，采用卡尔曼滤波实时估计惯性导航系统与惯性器件的误差；3)构建惯性/气压高度计组合导航子系统III 将MEMS惯性导航系统与气压高度计输出传送至微型导航计算机构建组合导航子系统，根据惯性导航系统误差方程与惯性器件误差特性建立状态方程，以惯性导航系统与气压高度计所测量的高度信息之间的差值为量测量建立量测方程，采用卡尔曼滤波实时估计惯性导航系统与惯性器件的误差；4)利用组合导航子系统I、II、III建立联邦滤波器，进一步估计惯性导航系统与惯性器件误差，从而提高惯性导航系统与惯性器件误差的估计精度，并通过闭环反馈校正提高惯性导航系统的位置、速度与姿态精度。
全文摘要
本发明公开了一种微小型个人组合导航系统及其导航定位方法，该系统由安装在人体足部的分系统与安装在躯干部位的分系统构成，足部分系统包括MEMSIMU、磁强计与通讯模块，躯干部位的分系统包括卫星导航接收机、多普勒测速仪、气压高度计、通讯模块与微型导航计算机；定位方法为采集MEMSIMU信号；陀螺仪与加速度计的随机误差建模与校正；MEMS惯性导航系统初始对准；捷联惯性导航解算与零速修正；MEMS惯性导航系统/卫星导航接收机/多普勒测速仪/磁强计/气压高度计组合导航，导航结果实时显示。本发明便于个人携带，可实现个人实时的定姿、测速与定位，能为行人、作业人员、士兵、救援人员等在复杂电磁环境中提供实时、完整、较精确的导航定位信息。
文档编号G01C21/00GK102445200SQ201110291899
公开日2012年5月9日 申请日期2011年9月30日 优先权日2011年9月30日
发明者朱欣华, 苏岩, 钱伟行 申请人:南京理工大学