一种行人导航定位中的自适应零速修正方法技术

本发明专利技术一种行人导航定位中的自适应零速修正方法，在行人足跟部穿戴导航模块，所述导航模块由陀螺仪和加速度计组成，本发明专利技术方法使用MEMS单元获得的信息，根据阈值判定行走零速状态。在零速状态时，构成全维度状态观测量，设计卡尔曼滤波器，得到行人更精确的速度位置信息。在缺少外部导航信息的情况下，消除惯性导航累积误差，提高定位精度。

An adaptive zero speed correction method for pedestrian navigation and positioning

【技术实现步骤摘要】
一种行人导航定位中的自适应零速修正方法
本专利技术涉及一种行人导航定位中的自适应零速修正方法，属于惯性导航

技术介绍
个人导航系统能够实时跟踪人员的速度、姿态、位置等信息，通过无线通讯传递给终端进行显示，实时监控人员运动状态和位置变化。能够应用于战场作战指挥、人员搜救、战场侦察，提高作战实时反应，增加作战成功率。目前的行人导航系统主要依赖卫星、UWB、WIFI等外部导航信息来实现，卫星定位能实现较好的定位精度，但在有遮挡的室内情况下定位失效。UWB、WIFI等定位方法虽然能够实现室内定位，但是需要预先铺设基站，使定位区域受到限制。基于MEMS惯性器件的个人导航系统体积小、自主性高、价格低、适合规模量产，在军事行动、反恐、消防等领域有很好的应用。基于惯性导航的人员定位系统存在定位误差随时间累积的缺点。本专利技术提出了一种零速修正方法，能够更精准的判别人行走中的零速状态，通过卡尔曼滤波消除累积误差，实现利用自身传感器信息的自主行人导航。惯性导航系统技术已经非常成熟，广泛地应用飞机、车辆、船舶等大型载体，由于传统惯性器件的体积较大、成本较高，不能够应用于人员的定位跟踪。现阶段由于MEMS技术的突飞猛进，解决了设备重量、体积和造价的问题，使人员定位惯性导航系统成为了可能。同时惯性导航特有的自主性克服了卫星导航易受遮挡干扰的缺点。
技术实现思路
本专利技术解决的技术问题是：克服现有技术的不足，提出一种行人导航零速修正的方法，有效地抑制了惯性导航系统导航信息的累积误差，实现各种环境下人员的精确定位。本专利技术解决技术的方案是：一种行人导航定位中的自适应零速修正方法，行人足跟部穿戴有导航模块，所述导航模块由陀螺仪和加速度计组成，包括步骤如下：1)以5℃为间隔，选取-40-+60℃温度段中20个温度点；在每个温度点下，以100Hz为采样频率，持续采集陀螺仪输出的角速度和加速度计输出的加速度数据5min，根据每个温度点下陀螺仪输出的角速度和加速度计输出的加速度，确定每个温度点对应的角速度均值和加速度均值；以各温度点的温度值为输入，以各温度点对应的角速度均值和加速度均值为输出进行二阶拟合，获得陀螺仪的拟合方程参数ag、bg、cg和加速度计的拟合方程参数aa、ba、ca；2)根据步骤1)获得的拟合方程参数ag、bg、cg、aa、ba、ca；并根据行人在导航定位过程中，当前陀螺仪实时的温度Tε和当前加速度计实时的温度确定角速度补偿量ε和加速度补偿量3)根据步骤2)所述的角速度补偿量ε，补偿当前行人在导航定位过程中，陀螺仪输出的角速度ω，获得补偿后的角速度ω′；同时，根据步骤2)所述的加速度补偿量补偿当前行人在导航定位过程中，加速度计输出的加速度f，获得补偿后的加速度f′；4)根据当前实时陀螺仪输出的角速度和加速度输出角加速度，判定当前导航模块是否进入零速状态，如果未进入零速状态，则执行步骤5)，如果进入零速状态，则执行步骤6)；5)根据步骤3)计算的当前实时补偿后的角速度ω′和加速度f′，确定行人当前k+1时刻的位置相对于起始点的位移作为最终结果输出给显示终端；6)根据当前k+1时刻导航模块的速度确定速度误差以速度误差作为输入，通过卡尔曼滤波计算位置误差、姿态误差，根据速度误差、位置误差和姿态误差，获得更新后的速度、位置和姿态角，然后进入步骤7)；7)根据步骤6)所述更新后的速度、位置和姿态角，确定行人当前k+1时刻的位置相对于起始点的位移作为最终结果输出给显示终端。步骤2)所述确定角速度补偿量ε和加速度补偿量具体如下：ε＝agTε2+bgTε+cg；步骤3)所述确定补偿后的角速度ω′和补偿后的加速度f′，具体为：ω′＝ω-ε；本专利技术与现有技术相比的有益效果是：本专利技术使用行走时的陀螺仪和加速度计输出信息检测零速状态，并采用零速修正算法消除惯性导航单元的累积误差，在不使用GPS等外界导航信息辅助的条件下进行完全自主导航。设备采用MEMS器件具有便携、低成本的优势，适合批量地应用于行人导航。1)采用零速卡尔曼滤波技术手段，起到了抑制惯性误差累积作用，提高了连续导航的精度；2)采用自适应零速阈值判定技术手段，使零速状态的判断更加准确，减少了零速状态的误判，提高导航精度；附图说明图1为本专利技术惯性导航系统的系统工作原理图。具体实施方式下面结合实施例对本专利技术作进一步阐述。本专利技术的MEMS模块包括：三轴加速度计、三轴陀螺仪、三轴磁力计、气压计。通过陀螺仪和加速度计的输出检测行走过程的零速状态，进行零速修正。以速度作为观测量设计卡尔曼滤波器对速度误差、位置误差、姿态误差进行估计，补偿到惯性导航单元的导航信息提升精度。系统的工作原理图如图1所示，通过三轴陀螺仪测量的角速度积分得到载体的姿态角，通过姿态角计算捷联矩阵将加速度计测量的比力信息投影到导航坐标系下进行积分，获得导航坐标系下的速度，再次积分得到行人的位置信息。该部分为纯导航下的导航信息输出，根据行走时的步态特征，结合陀螺仪和加速度计的输出信息判定脚部零速状态，进行卡尔曼滤波，获得纠正后的姿态、速度、位置信息。所述的零速修正卡尔曼滤波算法，根据零速时脚部速度为零的特点，选取速度误差作为观测量，设计简化的全维状态方程，对速度误差、位置误差、姿态误差进行估计，反馈补偿给惯性导航单元对累积误差进行消除。导航模块系统运行分为初始化和正常工作两个模式，系统上电后，静止一段时间等待系统实现初始化，然后进入正常工作模式。初始化要完成对系统的初始姿态和初始位置的确定，以及温度检测和温度补偿，包括对MEMS陀螺仪进行温度补偿，对加速度计进行温度补偿，根据加速度计输出计算姿态角。其中陀螺仪和加速度计温度补偿采用二阶多项式拟合姿态角的计算公式如下其中，f为加速度计三轴输出的平均值，由于MEMS器件的精度有限，航向角无法确定，本案采取初始方位朝东放置。零速判定的阈值所需的值也需要在初始化时刻确定。取初始化时间内的加速度计和陀螺仪输出的方差作为零速判定的条件。使用RF算法对运动状态进行分类并根据运动状态自适应地调整判定阈值。对于单兵导航系统，位置变化对状态方程的影响可以忽略，为了提高导航的实时性我们采用简化的误差方程。离散化的状态方程和量测方程为δxk|k-1＝Akδxk-1|k-1+wk-1Zk＝Hkδxk|k+nk卡尔曼滤波的过程可表示为以上五个方程组成一个循环递推最优估计，通过递推估计能够消除惯性导航计算随时间的积累的误差。本专利技术中的MEMS惯性器件安装在鞋底，通过捷联解算计算行人姿态速度位置信息，并使用步态检测的算法检测零速，进行卡尔曼滤波估计消除累积误差，使用气压计控制速度发散。行人足跟部穿戴有导航模块，所述导航模块由陀螺仪和加速度计组成，本发本文档来自技高网...

【技术保护点】
1.一种行人导航定位中的自适应零速修正方法，行人足跟部穿戴有导航模块，所述导航模块由陀螺仪和加速度计组成，其特征在于，包括步骤如下：/n1)以5℃为间隔，选取-40-+60℃温度段中20个温度点；在每个温度点下，以100Hz为采样频率，持续采集陀螺仪输出的角速度和加速度计输出的加速度数据5min，根据每个温度点下陀螺仪输出的角速度和加速度计输出的加速度，确定每个温度点对应的角速度均值和加速度均值；以各温度点的温度值为输入，以各温度点对应的角速度均值和加速度均值为输出进行二阶拟合，获得陀螺仪的拟合方程参数a

【技术特征摘要】
1.一种行人导航定位中的自适应零速修正方法，行人足跟部穿戴有导航模块，所述导航模块由陀螺仪和加速度计组成，其特征在于，包括步骤如下：
1)以5℃为间隔，选取-40-+60℃温度段中20个温度点；在每个温度点下，以100Hz为采样频率，持续采集陀螺仪输出的角速度和加速度计输出的加速度数据5min，根据每个温度点下陀螺仪输出的角速度和加速度计输出的加速度，确定每个温度点对应的角速度均值和加速度均值；以各温度点的温度值为输入，以各温度点对应的角速度均值和加速度均值为输出进行二阶拟合，获得陀螺仪的拟合方程参数ag、bg、cg和加速度计的拟合方程参数aa、ba、ca；
2)根据步骤1)获得的拟合方程参数ag、bg、cg、aa、ba、ca；并根据行人在导航定位过程中，当前陀螺仪实时的温度Tε和当前加速度计实时的温度确定角速度补偿量ε和加速度补偿量
3)根据步骤2)所述的角速度补偿量ε，补偿当前行人在导航定位过程中，陀螺仪输出的角速度ω，获得补偿后的角速度ω′；同时，根据步骤2)所述的加速度补偿量补偿当前行人在导航定位过程中，加速度计输出的加速度f，获得补偿后的加速度f′；
4)根据当前实时陀螺仪输出的角速度和加速度输出角加速度，判定当前导航模块是否进入零速状态，如果未进入零速状态，则执行步骤5)，如果进入零速状态，则执行步骤6)；
5)根据步骤3)计算的当前实时补偿后的角速度ω′和加速度f′，确定行人当前k+1时刻的位置相对于起始点的位移作为最终结果输出给显示终端；
6)根据当前k+1时刻导航模块的速度确定速度误差以速度误差作为输入，通过卡尔曼滤波计算位置误差、姿态误差，根据速度误差、位置误差和姿态误差，获得更新后的速度、位置和姿态角，然后进入步骤7)；
7)根据步骤6)所述更新后的速度、位置和姿态角，确定行人当前k+1时刻的位置相对于起始点的位移作为最终结果输出给显示终端。


2.根据权利要求1所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤2)所述确定角速度补偿量ε和加速度补偿量具体如下：
ε＝agTε2+bgTε+cg；





3.根据权利要求1所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤3)所述确定补偿后的角速度ω′和补偿后的加速度f′，具体为：
ω′＝ω-ε；





4.根据权利要求1所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤5)所述确定行人当前k+1时刻的位置相对于起始点的位移的方法，具体为：






其中，为k+1时刻行人位置相对起始点的位移，为k+1时刻导航模块的速度，Rk为k时刻根据陀螺仪的输出确定的导航模块的姿态矩阵；k＝1时，Rk由初始时刻的姿态角确定，为重力加速度，为导航坐标系相对于惯性坐标系的旋转矢量。


5.根据权利要求1～4任意之一所述的一种行人导航定位中的自适应零速修正方法，其特征在于，步骤4)所述当前导航模块是否进入零速状态的方法，具体为：
41)按100HZ频率分别采集K组静...

【专利技术属性】
技术研发人员：侯旭阳，罗涛，徐浩，张浩然，杨明，张益民，杨登，王宏建，马杰，杨亮，魏超，高赛，
申请(专利权)人：北京航天控制仪器研究所，
类型：发明
国别省市：北京;11