一种基于航向自观测的惯性足绑式行人定位方法技术

本发明专利技术公开一种基于航向自观测的惯性足绑式行人定位方法，属于行人定位领域；本发明专利技术在惯性足绑式行人定位系统解算行人足部位姿过程中实时检测行人足部运动状态；在检测到零速状态时，对加速度计噪声进行判断；当加速度计噪声小于阈值时，根据加速度计输出信息得到水平姿态：横滚角、俯仰角，经过进一步解算得到姿态失准角：横滚失准角、俯仰失准角、航向失准角；以速度误差和姿态失准角为观测量，导航误差为状态量，利用卡尔曼滤波对导航误差进行估计、补偿，提高航向角可观测性，通过航向校正提高惯性足绑式行人定位系统精度。

An inertial foot bound pedestrian positioning method based on course self observation

【技术实现步骤摘要】
一种基于航向自观测的惯性足绑式行人定位方法
本专利技术属于行人定位领域，具体涉及一种基于航向自观测的惯性足绑式行人定位方法。
技术介绍
惯性足绑式行人定位系统是一种以惯性组件(陀螺仪和加速度计)为主，并将惯性组件固定在行人足部，为行人提供实时精确位置信息的定位系统。由于微惯性测量单元(MicroInertialMeasurementUnit，MIMU)体积小、成本低、功耗低，MIMU是惯性足绑式行人定位系统中最常用惯性组件。惯性足绑式行人定位系统通过采集MIMU输出足部运动的角速度和线速度，经过导航解算，实时输出行人位置信息。全球卫星导航系统是目前使用最为广泛的定位系统，其通过对接收到的卫星信号进行位置解算，为用户提供实时定位信息。而惯性足绑式行人定位系统不需任何外部信息，是一种全自主定位系统；且由于其体积小、重量轻、成本低、易集成等优点，是行人定位中的主要方式。在消防和军事行动中具有极其重要的作用。然而，受加工工艺限制，MIMU测量噪声大，导致系统的定位误差随时间迅速累积发散，这是制约足绑式惯性行人定位系统长时间导航的重要因素之一。零速校正算法是一种常用足绑式惯性行人定位误差自补偿方法。该方法通过实时检测行人足部运动状态，当足部与地面存在相对运动时，对足部的位置、速度、姿态(横滚角、俯仰角、航向角)信息(简称位姿信息)进行解算；当足部与地面相对静止时，速度理论值为0，解算速度即为速度误差，以此为观测量，通过卡尔曼滤波完成对足部位姿信息的估计、补偿，来抵消器件噪声对系统的影响，进而提高系统定位精度。然而，由于零速校正算法不能对航向角进行有效估计、补偿，进而导致由航向失准角引起的定位误差累积，导致足绑式惯性行人定位精度降低，无法满足行人定位需求。针对航向角不可观测问题，公开号为CN107655476A的中国专利技术专利在2018年2月2日公开的《基于多信息融合补偿的行人高精度足部导航算法》中，提出了磁力计提供航向失准角信息的方法，提高行人定位精度。《兵工学报》期刊2017年38卷10期，由黄欣等人撰写的《基于零速/航向自观测/地磁匹配的行人导航算法研究》一文中，提出了行人初始静态下磁航向误差观测算法及行人运动状态下的零速航向误差自观测算法，解决行人长时间行走航向发散问题。《中国惯性技术学报》期刊2016年24卷2期，由刘春燕撰写的《基于多源信息融合的行人航位推算室内定位方法》一文中，提出了一种借助建筑几何信息实现行人航向的实时补偿方案，通过提高定向精度来抑制定位误差的累积传递。《IEEESensorsJournal》期刊2017年17卷10期，由Yu-LiangHsu等人撰写的《AWearableInertialPedestrianNavigationSystemWithQuaternion-BasedExtendedKalmanFilterforPedestrianLocalization》一文中，提出了基于二阶四元数的扩展卡尔曼滤波方法对加速度计、陀螺仪和磁力计信息进行融合，减小惯性导航的累积误差。《Sensors》期刊2017年17卷2期，由BaoShuDi等人撰写的《FusionofInertial/MagneticSensorMeasurementsandMapInformationforPedestrianTracking》一文中，在零速校正算法基础上，利用地图信息，进一步校正加速度漂移对定为精度影响，提高估计精度。《JournalofNavigation》期刊2011年64卷2期，由AbdulrahimK等人撰写的《AidingLowCostInertialNavigationwithBuildingHeadingforPedestrianNavigation》一文中，提出了基于行人行走建筑物基本方位知识的航向测量算法，消除低成本行人定位系统中航向漂移问题。以上文献都需借助外界信息或外传感器解决由航向角发散引起的定位误差累积问题，增加了系统的复杂度。
技术实现思路
本专利技术针对零速校正算法中由航向角发散引起的定位误差累积问题，提出了一种基于航向自观测的惯性足绑式行人定位方法。在惯性足绑式行人定位系统解算行人足部位姿过程中实时检测行人足部运动状态；在检测到零速状态时，对加速度计噪声进行判断；当加速度计噪声小于阈值时，根据加速度计输出信息得到水平姿态(横滚角、俯仰角)，经过进一步解算得到姿态失准角(横滚失准角、俯仰失准角、航向失准角)；以速度误差和姿态失准角为观测量，导航误差为状态量，利用卡尔曼滤波对导航误差进行估计、补偿，提高航向角可观测性，通过航向校正提高惯性足绑式行人定位系统精度。本专利技术的目的是这样实现的：一种基于航向自观测的惯性足绑式行人定位方法，包含以下步骤：步骤一：将微惯性测量单元固定在行人足部，打开电源并初始化系统；步骤二：实时采集MIMU输出的角速度ω和加速度f，即实时采集MIMU加速度计输出的三轴加速度f＝[fxfyfz]和陀螺输出的三轴角速度ω＝[ωxωyωz]；其中，fi(i＝x,y,z)表示加速度f在载体系oxb轴、oyb轴、ozb轴上的分量，单位均为m/s2；ωi(i＝x,y,z)分别表示角速度ω在载体系oxb轴、oyb轴、ozb轴上的分量，单位均为rad/s；步骤三：对步骤二中采集的微惯性测量单元输出的ω和f进行导航解算，得到行人的位置速度姿态步骤四：利用f和ω对行人足部运动状态进行实时检测：当且时，则认为行人足部触地处于零速状态，则转至步骤五；反之，则认为行人足部离地，不处于零速状态，则直接将步骤三中解算的行人位姿信息作为输出；其中，fl、ωl分别表示l时刻加速度计和陀螺仪的输出值，||·||表示向量的模；步骤五：获取速度观测量；当行人足部触地状态时，理论上行人速度为0，则其中，δv表示速度误差，δv＝[δvxδvyδvz]T，角标T表示矩阵转置；步骤六：加速度计噪声判断；比较|||f||-g|与阈值Tmd，当|||f||-g|＜Tmd时，则转至步骤七；当|||f||-g|≥Tmd时，则转至步骤十；其中，g表示当地重力加速度；||·||表示向量模值；|·|表示取绝对值；步骤七：根据步骤二中的f得到水平姿态角即其中，表示水平姿态的横滚角，表示水平姿态的俯仰角；fi(i＝x,y,z)分别表示加速度f在载体系oxb轴、oyb轴、ozb轴上的分量；arctan表示三角函数中的反正切函数；步骤八：根据步骤七中得到的水平姿态角得到旋转矩阵Cx、Cy，结合步骤二中解算过程中得到的载体系到导航系的转换矩阵得到矩阵转换关系为：则载体的姿态失准角为：其中，表示中第i行、第j列矩阵元素；φi(i＝x,y,z)分别表示横滚、俯仰、航向失准角；步骤九：构造1号卡尔曼滤波器，以位置误差δp、速度误差δv、姿态失准角Φ、加速度计零偏Δ、陀螺零偏ε为状态量，以步骤五中的速度误差和步骤八中的姿态失准角为观测量，对零速状态数据进行滤波，转至步骤十一；步骤十：构造2号卡尔曼滤波器，以位置误本文档来自技高网...

【技术保护点】
1.一种基于航向自观测的惯性足绑式行人定位方法，其特征在于，包含以下步骤：/n步骤一：将微惯性测量单元固定在行人足部，打开电源并初始化系统；/n步骤二：实时采集MIMU输出的角速度ω和加速度f，即实时采集MIMU加速度计输出的三轴加速度f＝[f

【技术特征摘要】
1.一种基于航向自观测的惯性足绑式行人定位方法，其特征在于，包含以下步骤：
步骤一：将微惯性测量单元固定在行人足部，打开电源并初始化系统；
步骤二：实时采集MIMU输出的角速度ω和加速度f，即实时采集MIMU加速度计输出的三轴加速度f＝[fxfyfz]和陀螺输出的三轴角速度ω＝[ωxωyωz]；其中，fi(i＝x,y,z)表示加速度f在载体系oxb轴、oyb轴、ozb轴上的分量，单位均为m/s2；ωi(i＝x,y,z)分别表示角速度ω在载体系oxb轴、oyb轴、ozb轴上的分量，单位均为rad/s；
步骤三：对步骤二中采集的微惯性测量单元输出的ω和f进行导航解算，得到行人的位置速度姿态
步骤四：利用f和ω对行人足部运动状态进行实时检测：
当且时，则认为行人足部触地处于零速状态，则转至步骤五；反之，则认为行人足部离地，不处于零速状态，则直接将步骤三中解算的行人位姿信息作为输出；其中，fl、ωl分别表示l时刻加速度计和陀螺仪的输出值，||·||表示向量的模；
步骤五：获取速度观测量；当行人足部触地状态时，理论上行人速度为0，则其中，δv表示速度误差，δv＝[δvxδvyδvz]T，角标T表示矩阵转置；
步骤六：加速度计噪声判断；比较|||f||-g|与阈值Tmd，当|||f||-g|＜Tmd时，则转至步骤七；当|||f||-g|≥Tmd时，则转至步骤十；其中，g表示当地重力加速度；||·||表示向量模值；|·|表示取绝对值；
步骤七：根据步骤二中的f得到水平姿态角即



其中，表示水平姿态的横滚角，表示水平姿态的俯仰角；fi(i＝x,y,z)分别表示加速度f在载体系oxb轴、oyb轴、ozb轴上的分量；arctan表示三角函数中的反正切函数；
步骤八：根据步骤七中得到的水平姿态角得到旋转矩阵Cx、Cy，结合步骤二中解算过程中得到的载体系到导航系的转换矩阵得到矩阵转换关系为：则载体的姿态失准角为：



其中，表示中第i行、第j列矩阵元素；φi(i＝x,y,z)分别表示横滚、俯仰、航向失准角；
步骤九：构造1号卡尔曼滤波器，以位置误差δp、速度误差δv、姿态失准角Φ、加速度计零偏Δ、陀螺零偏ε为状态量，以步骤五中的速度误差和步骤八中的姿态失准角为观测量，对零速状态数据进行滤波，转至步骤十一；
步骤十：构造2号卡尔曼滤波器，以位置误差δp、速度误差δv、姿态失准角Φ、加速度计零偏Δ、陀螺零偏ε为状态量，以步骤五中的速度误差为观测量，对零速状态数据进行滤波；
步骤十一：根据解算得到的δp、δv、Φ补偿得到行人的p、v、
步骤十二：将步骤十一补偿后行人的位置信息输出。


2.根据权利要求1所述的一种基于航向自观测的惯性足绑式行人定位方法，其特征在于，所述的步骤一包含以下步骤：
步骤一：初始化MIMU导航解算初值：行人三维位置信息px0、py0、pz0，单位均为m；三维速度信息vx0、vy0、vz0，单位均为m/s；以及三个姿态角信息单位均为rad；初始转换矩阵
初始转换矩阵计算如下：



其中，b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；
步骤二：初始化常值参量：零速检测阈值γ1、γ2，零速检测窗长度W，加速度计白噪声误差σa，陀螺仪白噪声误差σω，加速度计噪声检测阈值Tmd，采样时间T，单位为s，当地重力海拔h，单位为m，当地纬度λ，单位rad；
当地重力加速度g计算公式如下：
g＝9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10-9sin2λ)×h+7.2×10-14×h2

【专利技术属性】
技术研发人员：王秋滢，刘凯悦，孙志国，崔旭飞，郭铮，孙希君，吴应为，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙;23