CN109612464B - 基于iez框架下的多算法增强的室内导航系统及方法 - Google Patents
基于iez框架下的多算法增强的室内导航系统及方法 Download PDFInfo
- Publication number
- CN109612464B CN109612464B CN201811403479.8A CN201811403479A CN109612464B CN 109612464 B CN109612464 B CN 109612464B CN 201811403479 A CN201811403479 A CN 201811403479A CN 109612464 B CN109612464 B CN 109612464B
- Authority
- CN
- China
- Prior art keywords
- pedestrian
- error
- algorithm
- state
- navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Abstract
本发明公开了一种基于IEZ框架下的多算法增强的室内导航系统及方法,在基于惯性传感器IMU测量以及零速更新算法ZUPT的基础上,融合IHDE、ZARU、ZUNA等算法共同对系统的状态误差量进行处理,且并行输送至扩展卡尔曼滤波器对误差状态进行滤波估计,得到最优导航状态并提高该导航算法的导航精度;设计分解观测矩阵m和系统量测矩阵H为多个子矩阵,每个子矩阵分别对应以上算法,并由对应算法行相应的更新,可以增强导航算法的鲁棒性、可靠性和容错性能。同时本发明针对多种导航环境,借助磁力计和气压计设计了基于多种导航环境下的算法调整方案,极大地提高导航精度。本发明通过多算法增强的方案设计,最终提高了导航算法的精度和可靠性。
Description
技术领域
本发明属于导航技术领域,具体涉及一种基于IEZ框架下的多算法增强的室内导航系统及方法;其借助墙壁走向、磁力分布等多种外界导航因素和惯性自主导航结合辅助导航方法,实现不依赖导航卫星下的连续、精确定位的室内导航算法。
背景技术
随着GPS的发展,现如今打造的GPS导航系统可以向全球各地提供全时段,高精度的三维位置等信息的无线电导航定位系统。然而,在深山密林或者城市室内这些环境中,接收的GPS信号较弱或者缺失,这样用户将无法依靠GPS完成对自己当前位置的定位,以及前往目的地的导航规划。
因此,在卫星信号被密林或建筑物遮挡这些特殊环境下,便可以通过人员自主定位技术进行“补盲”。而惯性导航是一种自主性导航,通过固定于载体上的惯性测量器件,测得载体的角速度信息和加速度信息,并通过积分运算后求得载体的速度与位置信息,从而完成对载体的导航定位功能。由于惯性器件的复杂结构与高昂成本,限制了相关应用的发展。
随着微机电(MEMS)技术的发展,MEMS惯性器件逐渐取代了传统惯性器件,成为较为热门的研究对象。而基于MEMS的惯性测量单元(IMU)因其低廉的成本,小型化体积,功耗低以及极强的环境适用能力等优点,被广泛地用于军用,民用产业之中。因而使用基于MEMS惯性技术为核心的自主导航,便于安装与携带,自主性强,适用于各种野外荒地。并且其不依赖任何外部信息,也不向外辐射能量,具有较高的隐蔽性,也适用于特战,单兵潜入作战等方式。
基于MEMS的惯性测量单元也存在一些缺陷,虽然在短程内具有较高的精度,然而随着时间的增长,因为积分运算以及测量元件的漂移,即使初始化修正完成,之后的误差仍然十分严重,经验上来看,一般位置误差与操作时间的立方成正比。
使用有关系统动力学信息以及对导航信息的约束可以减少立方误差的增长。通常是利用静止时速度与姿态恒定,以此来限制误差的增长,比如零速度更新算法、零角速率更新算法等。也可以借助使用建筑的先验信息,给行人导航提供约束,同样可以限制误差的增长,最终使得导航精度得到保证。
发明内容
针对于上述现有技术的不足,本发明的目的在于提供一种基于IEZ框架下的多算法增强的室内导航系统及方法,以解决现有技术中GPS信号恶劣的环境下,即室内情况下无GPS信号或者信号不好的状态下行人自主导航的问题。本发明考虑适当利用行人所在环境提供的余度信息减小自主导航本身精度不足带来的影响,提高室内导航的精度。
为达到上述目的,本发明采用的技术方案如下:
本发明的一种基于IEZ框架下的多算法增强的室内导航系统,包含:数据测量模块、数据解算模块、姿态行为分析模块、多算法处理模块及数据输出模块;其中,
数据测量模块:六轴惯性测量单元、三轴磁力计和气压计组成的传感器组,采集每个时刻行人的导航数据;
数据解算模块,对传感器组采集到的导航数据进行惯导解算得到行人状态向量;
姿态行为分析模块,对传感器组采集到的导航数据进行处理、判断,得出行人的运动模式;
多算法处理模块,基于数据解算模块解算出的行人状态向量以及姿态行为分析模块得出行人的运动模式,通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器,得到行人状态误差向量的最优估计;
数据输出模块,用于将行人状态误差向量的最优估计与数据解算模块解算出的行人状态向量进行数据融合校准,得到最优的导航信息并输出。
优选地,所述姿态行为分析模块包含:静止状态检测模块和行为模式分析模块;其中,静止状态检测模块包含的三组逻辑判断标准,对六轴惯性测量单元测量得到的加速度和角速度数据进行逻辑判断,确定行人静止或运动状态;行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算,再通过行为模式分析模块内部的三组逻辑判断标准对行人的步伐长度和步伐方向进行判断,确定行人的运动模式;检测到行人静止时,静止状态检测模块触发零速度更新算法、零角速率更新算法、零粗加速度更新算法;检测到行人处于运动状态时,计算出行人步伐长度和步伐方向,并结合建筑物墙体走向,判断并得出行人的运动模式,行为模式分析模块触发改进启发式漂移消除算法。
优选地,所述零速度更新算法(ZUPT算法),将速度误差输出至扩展卡尔曼滤波器,获得速度误差状态量的最优估计;
所述零角速率更新算法(ZARU算法),将角速率误差输出至扩展卡尔曼滤波器,获得角速率误差状态量的最优估计;
所述零粗加速度更新算法(ZUNA算法),将加速度误差输出至扩展卡尔曼滤波器,获得加速度误差状态量的最优估计;
所述改进启发式漂移消除算法(IHDE算法),测量计算得到的步幅方向和步幅大小数据经逻辑判断后输出结果,然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器,获得行人运动方位角误差的最优估计。
优选地,所述多算法处理模块具体包括:
多算法处理模块分为两部分,一部分是由零速度更新算法、零角速率更新算法、零粗加速度更新算法组成,这些算法在静止状态检测模块逻辑判断行人处于静止状态时对行人状态误差向量的速度误差分量、角速率误差分量、加速度误差分量提取处理并输入至扩展卡尔曼滤波器中;另一部分由启发式漂移消除算法组成,当静止状态检测模块逻辑判断行人处于运动状态时,触发行为模式分析模块,行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算,再通过行为模式分析模块内部的逻辑判断对行人的步伐长度和步伐方向进行判断确定行人的运动模式,触发启发式漂移消除算法,该算法根据行人的多种运动模式,选择对应的处理方法对行人状态误差向量的航向角误差分量进行提取处理并输入至扩展卡尔曼滤波器中。
优选地,所述行人状态向量、行人状态误差向量、三轴磁力计测量值和气压计的测量值具体如下:
(1)行人状态向量为:
其中,分别为惯性测量单元测量的载体的x轴、y轴、z轴三个方向上的姿态角;Px、Py、Pz分别为x轴、y轴、z轴的方向上的位置;vx、vy、vz分别为x轴、y轴、z轴的三个方向上的速度;Wx、Wy、Wz分别为x轴、y轴、z轴的角速度;分别为x轴、y轴、z轴的加速度;
简写为;
(2)行人状态误差向量:
其中,分别为惯性测量单元测量的载体的x轴、y轴、z轴的方向上的姿态角误差;δPx、δPy、δPz分别为x轴、y轴、z轴的方向上的位置误差;δvx、δvy、δvz分别x轴、y轴、z轴的方向上的速度误差;δWx、δWy、δWz分别为x轴、y轴、z轴的角速度误差;分别为x轴、y轴、z轴的加速度计零位误差;
简写为:
(3)三轴磁力计测量值为:
XMG=[ψMGx ψMGy ψMGz]
其中,ψMGx、ψMGy、ψMGz为传感器运动方向上的磁分量、水平方向上与运动方向垂直的右手分量和垂向分量;
(4)气压计的测量值为:
XB=δhB
其中,δhB为气压计的高度测量误差。
本发明的一种基于IEZ框架下的多算法增强的室内导航方法,包括步骤如下:
1)采集每个时刻行人的导航数据;
2)对上述采集到的导航数据进行惯导解算得到行人状态向量;
3)对采集到的数据进行分析得出行人的运动模式;
4)通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器,得到行人状态误差向量的最优估计;
5)将行人状态误差向量的最优估计与解算出的行人状态向量进行数据融合校准,得到最优的导航信息并输出;
6)针对多种导航环境,借助三轴磁力计和气压计设计了基于多种导航环境下的算法调整方案,实时监控导航环境,切换增强算法使用方案确保最优的导航精度和可靠性。
优选的,所述步骤4)具体包含:基于扩展卡尔曼滤波器建立的状态方程和量测方程,具体为:
其中,Φk为状态转移矩阵;δxk、δx′k-1分别为采样时间k下估计状态误差矩阵和采样时间k-1下纠正后的状态误差估计矩阵;wk-1为系统过程白噪声矢量;H为系统量测矩阵;Vk为系统噪声量测矢量;Zk为观测误差量;
系统量测矩阵H由多个量测子矩阵组成;系统量测矩阵H以及观测量Zk定义如下:
其中,为第k个采样时间下的角度误差观测矩阵;为第k个采样时间下的位置误差观测矩阵;为第k个采样时间下的速度误差观测矩阵;为第k个采样时间下的角速度误差观测矩阵;为第k个采样时间下的加速度测量误差观测矩阵;Hθ根据行人状态误差向量的角度误差分量定义,为系统量测矩阵H的子矩阵;HP根据行人状态误差向量的位置误差分量定义,为系统量测矩阵H的子矩阵;Hz根据行人状态误差向量的速度误差分量定义,为系统量测矩阵H的子矩阵;HR根据行人状态误差向量的角速度误差分量定义,为系统量测矩阵H的子矩阵;HN根据行人状态误差向量的加速度误差分量定义,为系统量测矩阵H的子矩阵;
最终更新后的观测量和系统量测矩阵组合输入到扩展卡尔曼滤波器,经过滤波后的行人状态误差向量与惯导解算后的粗精度数据相融合最终得到行人状态向量的最优估计。
优选的,所述步骤6)中的基于多种导航环境下的算法调整方案具体为:
在正常对导航数据进行读取和导航解算后,增加了对导航环境进行判定的环节,确认是否有电磁干扰和气压等异常;由发生异常情况选择对应最优增强算法,最大化限度地提高导航精度;实时监控导航环境,切换最优状态以确保整体的导航精度和可靠性。
本发明的有益效果:
1.本发明通过设计姿态行为分析模块,可以判断行人的运动模式:行人是运动或者静止;行人是否沿着直线行走;行人行走方向是否与建筑物墙壁走向一致。
2.本发明使用多种增强算法,对目标的多个导航参数进行约束,从而提高导航精度。
3.本发明基于行人的运动模式,调整选择最佳处理方法,降低增强算法在行人运动模式变化后造成的测量误差,减小负面影响。
4.本发明将观测矩阵和量测矩阵分解为多个子矩阵,并与各个增强算法相对应。当某个增强算法满足条件,则对应子矩阵进行更新,从而增强导航算法的鲁棒性、可靠性和容错性能。
5.本发明针对多种导航环境,设置多增强算法调整的方案设计来选择最优增强算法,改善了增强算法使用策略,可以灵活选择各种增强算法,最大化限度地提高导航精度。并且实时监控导航环境,切换增强算法确保最优的导航精度和可靠性。
附图说明
图1是本发明方法的原理图;
图2是本发明方法的任务流程图;
图3是本发明的静止状态检测的逻辑判断图;
图4是本发明的行为模式分析的模块流程图;
图5是本发明的基于多种导航环境下的算法调整方案设计流程图。
具体实施方式
为了便于本领域技术人员的理解,下面结合实施例与附图对本发明作进一步的说明,实施方式提及的内容并非对本发明的限定。
本发明的一种基于IEZ框架下的多算法增强的室内导航系统,包含:数据测量模块、数据解算模块、姿态行为分析模块、多算法处理模块及数据输出模块;其中,
数据测量模块:六轴惯性测量单元、三轴磁力计和气压计组成的传感器组,采集每个时刻行人的导航数据;
数据解算模块,对传感器组采集到的导航数据进行惯导解算得到行人状态向量;
姿态行为分析模块,对传感器组采集到的导航数据进行处理、判断,得出行人的运动模式;
多算法处理模块,基于数据解算模块解算出的行人状态向量以及姿态行为分析模块得出行人的运动模式,通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器,得到行人状态误差向量的最优估计;
数据输出模块,用于将状态误差向量的最优估计与数据解算模块解算出的行人状态向量进行数据融合校准,得到最优的导航信息并输出。
其中,所述姿态行为分析模块包含:静止状态检测模块和行为模式分析模块;其中,静止状态检测模块包含的三组逻辑判断标准,对六轴惯性测量单元测量得到的加速度和角速度数据进行逻辑判断,确定行人静止或运动状态;行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算,再通过行为模式分析模块内部的三组逻辑判断标准对行人的步伐长度和步伐方向进行判断,确定行人的运动模式;检测到行人静止时,静止状态检测模块触发零速度更新算法、零角速率更新算法、零粗加速度更新算法;检测到行人处于运动状态时,计算出行人步伐长度和步伐方向,并结合建筑物墙体走向,判断并得出行人的运动模式,行为模式分析模块触发改进启发式漂移消除算法。
零速度更新算法(ZUPT算法),将速度误差输出至扩展卡尔曼滤波器,获得速度误差状态量的最优估计;
零角速率更新算法(ZARU算法),将角速率误差输出至扩展卡尔曼滤波器,获得角速率误差状态量的最优估计;
零粗加速度更新算法(ZUNA算法),将加速度误差输出至扩展卡尔曼滤波器,获得加速度误差状态量的最优估计;
改进启发式漂移消除算法(IHDE算法),测量计算得到的步幅方向和步幅大小数据经逻辑判断后输出结果,然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器,获得行人运动方位角误差的最优估计。
所述静止状态检测模块中包含的三组评判标准如下:
1)加速度大小:
设定阈值THmin、THmax进行逻辑判断,则
当加速度大小在设定的阈值内时,则第一组逻辑判断结果C1为1。
2)加速度方差:
定义窗口大小S,则局部加速度方差为
设定阈值THσ进行逻辑判断,则
当加速度方差大于等于设定的阈值时,则第二组逻辑判断结果C2为1。
3)角速度大小:
设定阈值THWmax进行逻辑判断,则
当角速度大小小于等于设定的阈值时,则第三组逻辑判断结果C3为1。
当三组评判标准独立判断后,通过与运算融合三组判断结果得出结论。当与运算结果为1时,判断行人处于静止状态,否则行人处于运动状态。
所述多算法处理模块具体包括:一部分是由零速度更新算法、零角速率更新算法、零粗加速度更新算法组成,这些算法在静止状态检测模块逻辑判断行人处于静止状态时对行人状态误差向量的速度误差分量、角速率误差分量、加速度误差分量提取处理并输入至扩展卡尔曼滤波器中;另一部分由启发式漂移消除算法组成,当静止状态检测模块逻辑判断行人处于运动状态时,触发行为模式分析模块,行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算,再通过行为模式分析模块内部的逻辑判断对行人的步伐长度和步伐方向进行判断确定行人的运动模式,触发启发式漂移消除算法,该算法根据行人的多种运动模式,选择对应的处理方法对行人状态误差向量的航向角误差分量进行提取处理并输入至扩展卡尔曼滤波器中。
参照图1、图2所示,本发明的一种基于IEZ框架下的多算法增强的室内导航方法,包括步骤如下:
采集每个时刻行人的导航数据:六轴惯性测量单元、三轴磁力计和气压计组成的传感器组收集每个时刻行人的运动数据;
对上述采集到的导航数据进行惯导解算:将采集到的行人运动数据进行初步解算得到行人的速度、位置等粗精度导航数据;
姿态行为分析任务包含静止状态检测部分和行为模式分析部分,首先进行静止状态检测,具体如图3所示。静止状态检测部分包含三组评判标准,各自独立判定后再结合三组判断结果得出结论,判断行人是否处于静止状态;若行人判定为静止状态的时候,触发零速度更新算法、零角速率更新算法、零粗加速度更新算法;同时对静止时间进行一个判定,当静止时间超过阈值5s,则表明行人处于完全静止状态,则锁定当前的位置和姿态;若行人处于运动状态,则启用行为模式分析部分,计算出行人步伐长度和步伐方向,并结合建筑物墙体走向,判断并得出行人的运动模式。逻辑判断内容如图4所示,触发改进启发式漂移消除算法;解算滤波任务中,将处理后的行人状态误差向量通过扩展卡尔曼滤波器,得到行人状态误差向量的最优估计;校准输出任务负责将行人状态误差向量的最优估计与行人状态向量进行数据融合校准,得到最优的导航信息并输出。
实时监控导航环境、对导航环境进行判定,确认是否有电磁干扰和气压异常。由异常情况选择对应最优增强算法,最大化限度地提高导航精度,确保最优的导航精度和可靠性。
运动状态判定具体如下:
运动状态判定包含两类数据计算用于逻辑判断;
(11)步伐方向,计算当前行人移动时的方向;
(12)步伐长度,计算当前行人行走一步的长度;
运动状态的逻辑判断如下:
(21)判断行走步伐大小
防止行走步伐较小的时候触发算法造成测量误差,对系统产生负面影响,给出阈值ThSL进行逻辑判断:
(22)判断是否为直线行走
统计最近五次具有相似方向的步幅,将行走轨迹进行归类;并给出阈值Thθ进行逻辑判断:
(23)判断是否沿着墙体方向运动
并给出阈值Thδθ进行逻辑判断:
其中,Mdk表示在第k个采样时间下的逻辑判断值,当行人步伐方向与建筑墙体主要方向之间的角度误差量小于阈值约束角度时,Mdk值为1,表示行人沿着墙体方向行走。
改进启发式漂移消除算法具体如下:
(31)行人为曲线行走时
即WSk=0,此时算法不输出任何信息,保证滤波后的状态不受影响;
(32)行人不沿着墙壁方向直线行走时
即WSk=1,Mdk=0,SSk=1,此时将当前采样时间k下的步伐方向与采样时间k-4下步伐方向做差作为滤波器的观测值输入到扩展卡尔曼滤波器中;
(33)行人沿着墙壁方向行走时
即WSk=1,Mdk=1,SSk=1,此时将采样时间下行人步伐方向与建筑墙体的主要方向之间的角度误差量δθk作为滤波器的观测值输入到扩展卡尔曼滤波器中。
通过多种增强算法并行处理输入到扩展卡尔曼滤波器,得到行人状态误差向量的最优估计;
多种增强算法具体如下:
ZUPT算法,即零速度更新算法,用于对速度误差状态量的最优估计。在行人处于静止运动状态时,理想状态下的速度测量值理应保持为零,实际情况下由于惯性测量模块自身存在的漂移以及测量误差,导致在静止运动状态下行人的速度测量值并不为零。将速度误差输出至扩展卡尔曼滤波器,获得速度误差状态量的最优估计。
ZARU算法,即零角速率更新算法,用于对角速度误差状态量的最优估计。在行人处于静止运动状态时,理想状态下陀螺仪测量的角速度应该保持为零,实际情况下由于陀螺自身存在的漂移误差以及测量误差,导致真实测量下角速度不为零。将角速度误差输出至扩展卡尔曼滤波器,获得角速度误差状态量的最优估计。
ZUNA算法,即零粗加速度更新算法,用于对加速度误差状态量的最优估计。在行人处于静止运动状态时,理想状态下的加速度测量值理应等于起始点处行人处于静止状态时的平均加速度,实际情况下由于惯性测量模块自身存在的漂移以及测量误差,导致在静止运动状态下行人的加速度测量值并不等于起始处的平均加速度。将加速度误差输出至扩展卡尔曼滤波器,获得加速度误差状态量的最优估计。
IHDE算法,即改进启发式漂移消除算法,用于对行人运动方位角误差的最优估计。算法包含逻辑判断模块和处理模块;其中测量计算得到的步幅方向和步幅大小数据经由逻辑判断模块判断输出结果,然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器,获得行人运动方位角误差的最优估计;该算法通过分析行人的运动状态并且适当借助建筑物的墙体走向来修正行人的运动状态。
以上算法分别影响着观测矩阵和系统量测矩阵,当满足触发的条件,相应算法便进行对应更新,未被触发的算法对观测矩阵和系统量测矩阵无影响。
定义行人状态向量、行人状态误差向量、三轴磁力计的测量值和气压计的测量值具体如下:
(41)行人状态向量为:
其中,分别为惯性测量单元测量的载体的x轴、y轴、z轴三个方向上的姿态角;Px、Py、Pz分别为x轴、y轴、z轴的方向上的位置;vx、vy、vz分别为x轴、y轴、z轴的三个方向上的速度;Wx、Wy、Wz分别为x轴、y轴、z轴的角速度;分别为x轴、y轴、z轴的加速度;
简写为;
(42)行人状态误差向量:
其中,分别为惯性测量单元测量的载体的x轴、y轴、z轴的方向上的姿态角误差;δPx、δPy、Pz分别为x轴、y轴、z轴的方向上的位置误差;δvx、δvy、vz分别x轴、y轴、z轴的方向上的速度误差;δWx、δWy、δWz分别为x轴、y轴、z轴的角速度误差;分别为x轴、y轴、z轴的加速度计零位误差;
简写为:
(43)三轴磁力计的测量值为:
XMG=[ψMGx ψMGy ψMGz]
其中,ψMGx、ψMGy、ψMGz为传感器运动方向上的磁分量、水平方向上与运动方向相互垂直的右手分量和垂向分量;
(44)气压计的测量值为:
XB=δhB
其中,δhB为气压计的高度测量误差。
下面建立各增强算法更新下的观测矩阵和系统量测矩阵:
(51)ZUPT零速更新算法
其中,为第k个采样时间下传感器测量得到的速度测量值;为第k个采样时间下的速度误差观测矩阵,即行人在静止状态下传感器测量得到的速度测量矩阵与零矩阵的差值,在实际工作中,这个值虽然很小但是不等于零;Hz根据行人状态误差向量的速度误差分量定义,为系统量测矩阵H的子矩阵。
(52)ZARU零角速率更新算法
其中,为第k个采样时间下三个正交陀螺仪传感器的测量值;为第k个采样时间下的角速度误差观测矩阵,即行人在静止状态下三个正交陀螺仪传感器的测量矩阵与零矩阵的差值,在实际工作中,这个值虽然很小但是不等于零;HR根据行人状态误差向量的角速度误差分量定义,为系统量测矩阵H的子矩阵。
(53)ZUNA零粗加速度更新算法
其中,为起始点处行人处于静止状态时3秒内的平均加速度值;为第k个采样时间下三个正交加速度传感器的测量值;为第k个采样时间下的加速度测量误差观测矩阵,即行人在静止状态下加速度传感器的测量值与初始测量加速度值的差值;HN根据行人状态误差向量的加速度误差分量定义,为系统量测矩阵H的子矩阵。
(54)IHDE改进启发式漂移消除算法:
(541)行人曲线行走时
即WSk=0,此时算法不输出任何信息,保证滤波后的状态不受影响;
(542)行人不沿着墙壁方向直线行走时
(543)行人沿着墙壁方向行走时
即WSk=1,Mdk1,此时将采样时间下行人步伐方向与建筑墙体的主要方向之间的角度误差量δθk作为滤波器的观测值输入到扩展卡尔曼滤波器中;
基于多种导航环境下的算法调整方案具体方法为:
在正常对导航数据进行读取和导航解算后,增加了对导航环境进行判定的环节,确认是否有电磁干扰和气压等异常。由发生异常情况选择对应最优增强算法,最大化限度地提高导航精度。实时监控导航环境,切换最优状态以确保整体的导航精度和可靠性。
在磁清洁环境中几公里范围内区域磁场强度以及磁倾角几乎保持不变,而磁航向角可以通过三维磁力计测量计算出来,如下:
则行人的航向角误差为:
但是在存在铁磁材料区域时,比如室内,磁力计将会受到严重影响。因此将磁力计测量到的磁场强度以及计算得到的倾角与参考值进行比较,即将行人从起始到静止期间的数值进行比较,可以轻松检测磁力计读数是否存在异常。通过磁异常检测,可以确保磁力计数据没有失真,保证行人航向数据不受污染。
由于气压值与温度和高度保持着比例关系,所以理论上行人在上下楼梯或者在楼道上行走时,气压值处于一个小幅度的变化状态。但是如果附近存在火源或者空调,气压值波动相对较大,当波动超过设定阈值,则气压检测出现异常。
(611)当行人正常在楼道行走时,未出现气压和磁异常检测,则利用磁力计和气压计测量数据辅助零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新,如图5所示。
则系统观测矩阵Zk
则系统量测矩阵H
Hz根据行人状态误差向量的速度误差分量定义,为系统量测矩阵H的子矩阵;HR根据行人状态误差向量的角速度误差分量定义,为系统量测矩阵H的子矩阵。
(612)由于铁磁材料分布的原因,会出现磁异常检测,则利用改进启发式漂移消除算法搭配气压计测量数据以及零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新。
则系统观测矩阵Zk
则系统量测矩阵H
HI根据行人状态误差向量的角度误差分量定义,为系统量测矩阵H的子矩阵。当判断行人为直线行走时,系统量测矩阵H:
当判断行人为曲线行走时,系统量测矩阵H:
(621)当出现气压检测异常时,首先通过气压变化均值判断是否处于平地。当处于平地时,对导航数据的高度分量做一个约束,并利用磁力计数据辅助零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新。
则系统观测矩阵Zk
则系统量测矩阵H
(622)若磁异常检测,则利用改进启发式漂移消除算法搭配高度辅助以及零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新。
则系统观测矩阵Zk
则系统量测矩阵H
HI根据行人状态误差向量的角度误差分量定义,为系统量测矩阵H的子矩阵。当判断行人为直线行走时,系统量测矩阵H:
当判断行人为曲线行走时,系统量测矩阵H:
(631)出现气压检测异常且判断行人未处于平地,则利用磁力计数据辅助零速度更新算法、零角速率更新算法、零粗速度更新算法对行人导航数据进行校准更新。
则系统观测矩阵Zk
则系统量测矩阵H
(632)若磁异常检测,则利用改进启发式漂移消除算法搭配零速度更新算法、零角速率更新算法、零粗速度更新算法对行人导航数据进行校准更新,最大化限度地提高导航精度。
则系统观测矩阵Zk
则系统量测矩阵H
HI根据行人状态误差向量的角度误差分量定义,为系统量测矩阵H的子矩阵。当判断行人为直线行走时,系统量测矩阵H:
当判断行人为曲线行走时,系统量测矩阵H:
建立状态方程和量测方程:
其中,Φk为状态转移矩阵;δxk、δx′k-1分别为采样时间k下估计状态误差矩阵和采样时间k-1下纠正后的状态误差估计矩阵;wk-1为系统过程白噪声矢量;H为系统系统量测矩阵;Vk为系统噪声量测矢量;Zk为观测误差量;
系统量测矩阵H由多个量测子矩阵组成;系统量测矩阵H以及观测量Zk定义如下:
为第k个采样时间下的角度测量误差观测矩阵;为第k个采样时间下的位置误差观测矩阵;为第k个采样时间下的速度误差观测矩阵;为第k个采样时间下的角速度测量误差观测矩阵;为第k个采样时间下的加速度测量误差观测矩阵;
Hθ根据行人状态误差向量的角度误差分量定义;HP根据行人状态误差向量的位置误差分量定义;Hz根据行人状态误差向量的速度误差分量定义;HR根据行人状态误差向量的角速度误差分量定义;HN根据行人状态误差向量的加速度误差分量定义;
最终更新后的观测量和系统量测矩阵组合输入到扩展卡尔曼滤波器,经过滤波后的行人状态误差向量与惯导解算后的粗精度数据相融合最终得到行人状态向量的最优估计。
本发明具体应用途径很多,以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以作出若干改进,这些改进也应视为本发明的保护范围。
Claims (6)
1.一种基于IEZ框架下的多算法增强的室内导航系统,其特征在于,包含:数据测量模块、数据解算模块、姿态行为分析模块、多算法处理模块及数据输出模块;
数据测量模块:六轴惯性测量单元、三轴磁力计和气压计组成的传感器组,采集每个时刻行人的导航数据;
数据解算模块,对传感器组采集到的导航数据进行惯导解算得到行人状态向量;
姿态行为分析模块,对传感器组采集到的导航数据进行处理、判断,得出行人的运动模式;
多算法处理模块,基于数据解算模块解算出的行人状态向量以及姿态行为分析模块得出行人的运动模式,通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器,得到行人状态误差向量的最优估计;
数据输出模块,用于将行人状态误差向量的最优估计与数据解算模块解算出的行人状态向量进行数据融合校准,得到最优的导航信息并输出;
所述姿态行为分析模块包含:静止状态检测模块和行为模式分析模块;其中,静止状态检测模块对六轴惯性测量单元测量得到的加速度和角速度数据进行逻辑判断,确定行人静止或运动状态;行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算,对行人的步伐长度和步伐方向进行判断,确定行人的运动模式;检测到行人静止时,静止状态检测模块触发零速度更新算法、零角速率更新算法、零粗加速度更新算法;检测到行人处于运动状态时,计算出行人步伐长度和步伐方向,并结合建筑物墙体走向,判断并得出行人的运动模式,行为模式分析模块触发改进启发式漂移消除算法;
所述零速度更新算法,将速度误差输出至扩展卡尔曼滤波器,获得速度误差状态量的最优估计;
所述零角速率更新算法,将角速率误差输出至扩展卡尔曼滤波器,获得角速率误差状态量的最优估计;
所述零粗加速度更新算法,将加速度误差输出至扩展卡尔曼滤波器,获得加速度误差状态量的最优估计;
所述改进启发式漂移消除算法,测量计算得到的步幅方向和步幅大小数据经逻辑判断后输出结果,然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器,获得行人运动方位角误差的最优估计。
2.根据权利要求1所述的基于IEZ框架下的多算法增强的室内导航系统,其特征在于,所述多算法处理模块具体包括:
多算法处理模块分为两部分,一部分是由零速度更新算法、零角速率更新算法、零粗加速度更新算法组成,这些算法在静止状态检测模块逻辑判断行人处于静止状态时对行人状态误差向量的速度误差分量、角速率误差分量、加速度误差分量提取处理并输入至扩展卡尔曼滤波器中;另一部分由启发式漂移消除算法组成,当静止状态检测模块逻辑判断行人处于运动状态时,触发行为模式分析模块,行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算,再通过行为模式分析模块内部的逻辑判断对行人的步伐长度和步伐方向进行判断确定行人的运动模式,触发启发式漂移消除算法,该算法根据行人的多种运动模式,选择对应的处理方法对行人状态误差向量的航向角误差分量进行提取处理并输入至扩展卡尔曼滤波器中。
3.根据权利要求1所述的基于IEZ框架下的多算法增强的室内导航系统,其特征在于,所述行人状态向量、行人状态误差向量、三轴磁力计测量值和气压计的测量值具体如下:
(1)行人状态向量为:
其中,分别为惯性测量单元测量的载体的x轴、y轴、z轴三个方向上的姿态角;Px、Py、Pz分别为x轴、y轴、z轴的方向上的位置;vx、vy、vz分别为x轴、y轴、z轴的三个方向上的速度;Wx、Wy、Wz分别为x轴、y轴、z轴的角速度;分别为x轴、y轴、z轴的加速度;
简写为;
(2)行人状态误差向量:
其中,分别为惯性测量单元测量的载体的x轴、y轴、z轴的方向上的姿态角误差;δPx、Py、δPz分别为x轴、y轴、z轴的方向上的位置误差;δvx、δvy、δvz分别x轴、y轴、z轴的方向上的速度误差;δWx、δWy、δWz分别为x轴、y轴、z轴的角速度误差;分别为x轴、y轴、z轴的加速度计零位误差;
简写为:
(3)三轴磁力计测量值为:
XMG=[ψMGx ψMGy ψMGz]
其中,ψMGx、ψMGy、ψMGz为传感器运动方向上的磁分量、水平方向上与运动方向垂直的右手分量和垂向分量;
(4)气压计的测量值为:
XB=δhB
其中,δhB为气压计的高度测量误差。
4.一种基于IEZ框架下的多算法增强的室内导航方法,其特征在于,包括步骤如下:
1)采集每个时刻行人的导航数据;
2)对上述采集到的导航数据进行惯导解算得到行人状态向量;
3)对采集到的数据进行分析得出行人的运动模式;
4)通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器,得到行人状态误差向量的最优估计;
5)将行人状态误差向量的最优估计与解算出的行人状态向量进行数据融合校准,得到最优的导航信息并输出;
6)针对多种导航环境,借助三轴磁力计和气压计设计了基于多种导航环境下的算法调整方案,实时监控导航环境,切换增强算法使用方案确保最优的导航精度和可靠性;
所述步骤3)中得出行人的运动模式具体包括:对六轴惯性测量单元测量得到的加速度和角速度数据进行逻辑判断,确定行人静止或运动状态;利用得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算,对行人的步伐长度和步伐方向进行判断,确定行人的运动模式;检测到行人静止时,触发零速度更新算法、零角速率更新算法、零粗加速度更新算法;检测到行人处于运动状态时,计算出行人步伐长度和步伐方向,并结合建筑物墙体走向,判断并得出行人的运动模式,触发改进启发式漂移消除算法;
所述零速度更新算法,将速度误差输出至扩展卡尔曼滤波器,获得速度误差状态量的最优估计;
所述零角速率更新算法,将角速率误差输出至扩展卡尔曼滤波器,获得角速率误差状态量的最优估计;
所述零粗加速度更新算法,将加速度误差输出至扩展卡尔曼滤波器,获得加速度误差状态量的最优估计;
所述改进启发式漂移消除算法,测量计算得到的步幅方向和步幅大小数据经逻辑判断后输出结果,然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器,获得行人运动方位角误差的最优估计。
5.根据权利要求4所述的基于IEZ框架下的多算法增强的室内导航方法,其特征在于,所述步骤4)具体包含:基于扩展卡尔曼滤波器建立的状态方程和量测方程,具体为:
其中,Φk为状态转移矩阵;δxk、δx′k-1分别为采样时间k下估计状态误差矩阵和采样时间k-1下纠正后的状态误差估计矩阵;wk-1为系统过程白噪声矢量;H为系统量测矩阵;Vk为系统噪声量测矢量;Zk为观测误差量;
系统量测矩阵H由多个量测子矩阵组成;系统量测矩阵H以及观测误差量Zk定义如下:
其中,为第k个采样时间下的角度误差观测矩阵的转置;为第k个采样时间下的位置误差观测矩阵的转置;为第k个采样时间下的速度误差观测矩阵的转置;为第k个采样时间下的角速度误差观测矩阵的转置;为第k个采样时间下的加速度测量误差观测矩阵的转置;Hθ根据行人状态误差向量的角度误差分量定义,为系统量测矩阵H的子矩阵;HP根据行人状态误差向量的位置误差分量定义,为系统量测矩阵H的子矩阵;Hz根据行人状态误差向量的速度误差分量定义,为系统量测矩阵H的子矩阵;HR根据行人状态误差向量的角速度误差分量定义,为系统量测矩阵H的子矩阵;HN根据行人状态误差向量的加速度误差分量定义,为系统量测矩阵H的子矩阵;
最终更新后的观测量和系统量测矩阵组合输入到扩展卡尔曼滤波器,经过滤波后的行人状态误差向量与惯导解算后的粗精度数据相融合最终得到行人状态向量的最优估计。
6.根据权利要求4所述的基于IEZ框架下的多算法增强的室内导航方法,其特征在于,所述步骤6)中的基于多种导航环境下的算法调整方案具体为:
在正常对导航数据进行读取和导航解算后,增加了对导航环境进行判定的环节,确认是否有电磁干扰和气压异常;由发生异常情况选择对应最优增强算法,最大化限度地提高导航精度;实时监控导航环境,切换最优状态以确保整体的导航精度和可靠性。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811403479.8A CN109612464B (zh) | 2018-11-23 | 2018-11-23 | 基于iez框架下的多算法增强的室内导航系统及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811403479.8A CN109612464B (zh) | 2018-11-23 | 2018-11-23 | 基于iez框架下的多算法增强的室内导航系统及方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109612464A CN109612464A (zh) | 2019-04-12 |
CN109612464B true CN109612464B (zh) | 2022-06-10 |
Family
ID=66004504
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811403479.8A Active CN109612464B (zh) | 2018-11-23 | 2018-11-23 | 基于iez框架下的多算法增强的室内导航系统及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109612464B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111795694B (zh) * | 2020-04-08 | 2022-05-10 | 应急管理部四川消防研究所 | 一种用于消防救援的室内定位电磁校准方法 |
CN113483753B (zh) * | 2021-06-30 | 2022-11-01 | 北京航空航天大学 | 一种基于环境约束的惯性航向误差消除方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106017461A (zh) * | 2016-05-19 | 2016-10-12 | 北京理工大学 | 基于人体/环境约束的行人导航系统三维空间定位方法 |
CN107655476A (zh) * | 2017-08-21 | 2018-02-02 | 南京航空航天大学 | 基于多信息融合补偿的行人高精度足部导航算法 |
CN108426574A (zh) * | 2018-02-02 | 2018-08-21 | 哈尔滨工程大学 | 一种基于zihr的航向角修正算法的mems行人导航方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20160349057A1 (en) * | 2010-10-13 | 2016-12-01 | Elbit Systems Ltd. | Multiple data sources pedestrian navigation system |
-
2018
- 2018-11-23 CN CN201811403479.8A patent/CN109612464B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106017461A (zh) * | 2016-05-19 | 2016-10-12 | 北京理工大学 | 基于人体/环境约束的行人导航系统三维空间定位方法 |
CN107655476A (zh) * | 2017-08-21 | 2018-02-02 | 南京航空航天大学 | 基于多信息融合补偿的行人高精度足部导航算法 |
CN108426574A (zh) * | 2018-02-02 | 2018-08-21 | 哈尔滨工程大学 | 一种基于zihr的航向角修正算法的mems行人导航方法 |
Non-Patent Citations (3)
Title |
---|
A Foot-mounted PDR System Based On IMU/EKF+HMM+ZUPT+ZARU+HDR+Compass Algorithm;Wenchao Zhang,等;《IEEE:2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN)》;20170930;第1-4页 * |
Evaluation and Comparison of Performance Analysis of Indoor Inertial Navigation system Based on Foot Mounted IMU;Woyano, F, 等;《IEEE:2016 18TH INTERNATIONAL CONFERENCE ON ADVANCED COMMUNICATIONS TECHNOLOGY (ICACT)》;20161214;全文 * |
基于准静止磁场检测的行人导航算法框架iIEZ+;张立强等;《传感技术学报》;20170430;第30卷(第4期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN109612464A (zh) | 2019-04-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
CN104729506B (zh) | 一种视觉信息辅助的无人机自主导航定位方法 | |
CN105628024B (zh) | 基于多传感器融合的单人定位导航器和定位导航方法 | |
CN109099913B (zh) | 一种基于mems惯性器件的穿戴式导航装置和方法 | |
CN105606094B (zh) | 一种基于mems/gps组合系统的信息条件匹配滤波估计方法 | |
CN108036785A (zh) | 一种基于直接法与惯导融合的飞行器位姿估计方法 | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN107490378B (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
CN111024126B (zh) | 一种行人导航定位中的自适应零速修正方法 | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
Leutenegger et al. | Robust state estimation for small unmanned airplanes | |
CN107014376A (zh) | 一种适用于农业机械精准作业的姿态倾角估计方法 | |
CN112611380B (zh) | 基于多imu融合的姿态检测方法及其姿态检测装置 | |
JP2007232443A (ja) | 慣性航法装置およびその誤差補正方法 | |
CN109612464B (zh) | 基于iez框架下的多算法增强的室内导航系统及方法 | |
He et al. | Adaptive error-state Kalman filter for attitude determination on a moving platform | |
CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
JP2021179438A (ja) | 機首方位測定システムにおけるセンサ測定の欠如を補償するシステムと方法 | |
CN109579832A (zh) | 一种人员高度自主定位算法 | |
CN108562289A (zh) | 连续多边几何环境中四旋翼飞行器激光雷达导航方法 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
CN115540860A (zh) | 一种多传感器融合位姿估计算法 | |
Zhu et al. | Orientation estimation by partial-state updating Kalman filter and vectorial magnetic interference detection | |
JP2007232444A (ja) | 慣性航法装置およびその誤差補正方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |