CN106123900B - 基于改进型互补滤波的室内行人导航磁航向解算方法 - Google Patents
基于改进型互补滤波的室内行人导航磁航向解算方法 Download PDFInfo
- Publication number
- CN106123900B CN106123900B CN201610442719.XA CN201610442719A CN106123900B CN 106123900 B CN106123900 B CN 106123900B CN 201610442719 A CN201610442719 A CN 201610442719A CN 106123900 B CN106123900 B CN 106123900B
- Authority
- CN
- China
- Prior art keywords
- angle
- state
- complementary filter
- magnetic heading
- pedestrian
- 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
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
本发明公开了一种基于改进型互补滤波的室内行人导航磁航向解算方法,从时域角度设计互补滤波器参数,建立改进型互补滤波器算法模型,构建行人运动模态辨识算法模型,进而利用磁传感器二维椭圆标定实现室内行人磁航向角的实时解算,采用低精度的消费级MEMS传感器芯片,无论室内、室外,航向角均能保证较高的精度,实用性较强,采用非固连模式安装,安装灵活性较高,使用环境不受限制,适用场景广阔,使用方式简易便捷,短时间内即能够保证较高精度的输出。
Description
技术领域
本发明涉及一种室内行人导航磁航向解算算法,尤其涉及一种基于改进型互补滤波的室内行人导航磁航向解算方法,属于室内行人导航技术领域。
背景技术
磁航向传感器凭借对地球磁场的测量,可以长期有效地解算出当前载体的航向角,被广泛的应用于各个领域。目前,室内行人导航较为流行,磁航向传感器则是其中的关键模块,但是当下的磁传感器均采取与人体固连模式(穿戴式),大大约束了其适用条件,实用性不强。对于捷联惯性导航系统,姿态解算是其重要环节,但当前捷联惯性导航姿态解算算法(包括欧拉角法、方向余弦法、四元数法等等)存在三角函数计算问题和方程奇异现象等,复杂性较高。本专利提出一种改进型互补滤波器,从时域角度研究利用最优加权平均参数估计完成对姿态角的解算,同时研究剔除行人自带引入加速度误差;在行人稳态与非稳态辨识上,引入天向陀螺每秒绝对值积分进行模式判别。本方法适用于行人导航使用低精度的惯性传感器,在室内仍能够较为有效及较强适应性的磁航向解算算法,具有极高的工程应用和商业价值。
发明内容
本发明所要解决的技术问题是针对背景技术的不足提供了一种基于改进型互补滤波的室内行人导航磁航向解算方法。
本发明为解决上述技术问题采用以下技术方案
基于改进型互补滤波的室内行人导航磁航向解算方法,具体包含如下步骤,
步骤1,从时域角度获得互补滤波器参数k1和k2,具体计算如下:
k1=1-k2
其中,是陀螺仪积分获得的角度的方差,是加速度计获得的角度的方差,k1为陀螺仪的权重,k2为加速度计的权重;
步骤2,根据步骤1获取的互补滤波器参数建立改进型互补滤波器算法模型,进而计算出姿态角,具体计算如下:
fk1=fk-Δfk-1
其中Δfk-1为k-1时刻行人引入的加速度,fk为k时刻实测加速度计数据,fk1为修正误差后的加速度数据,T为采样周期,w为陀螺仪输出的角速度,θk为k时刻滤波后的姿态角,为陀螺仪在k-1时刻通过积分解算出的角度,加速度计在k时刻解算出来的角度;
步骤3,利用步骤2计算出的姿态角构建投影矩阵,将磁传感器数据投影到水平面:利用天向陀螺每秒绝对值积分进行模式判别,若行人步行、跑步时的模态为非稳态状态,则直接置航向角为零,不输出磁航向角;若行人步行、跑步时的模态为稳态状态,则补偿磁传感器误差,计算出精确地磁航向角,并更新Δfk-1:
其中为k-1时刻坐标转换矩阵,为重力矢量。
作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案,在步骤1中,从时域角度利用最优加权平均估计分析获得互补滤波器参数k1和k2。
作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案,在步骤3中,行人手持载体在步行、跑步时的模态判别如下:
若θz<ε
则此时判为行人步行、跑步时的模态为稳态状态,否则为非稳态状态;
其中,w为z轴方向陀螺仪输出的角速度,T为采样周期,θz为z轴方向每秒绝对值积分计算出得角度,ε为通过统计学方法计算出来的参数值。
作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案,在步骤3中,利用最小二乘法标定好的二维椭圆参数补偿磁传感器误差。
作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案,在步骤3中,利用递推的思想,若行人步行、跑步时的模态为稳态状态,则实时更新人为引入的加速度,利用修正误差后的加速度计算姿态角。
作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案,若行人步行、跑步时的模态为稳态状态,则补偿磁传感器误差,计算出磁航向角,磁航向角的具体计算如下:
其中,为磁航向角,Y为补偿后的横向磁传感器信息,X为补偿后的纵向磁传感器信息。
本发明采用以上技术方案与现有技术相比,具有以下技术效果:
1.本发明采用低精度的消费级MEMS传感器芯片,无论室内、室外,航向角均能保证较高的精度,实用性较强;
2.本发明采用非固连模式安装,安装灵活性较高,使用环境不受限制,适用场景广阔;
3.本发明使用方式简易便捷,短时间内即能够保证较高精度的输出。
附图说明
图1为改进型互补滤波器原理图;
图2为基于改进型互补滤波的室内行人导航磁航向解算方法原理图。
具体实施方式
下面结合附图对本发明的技术方案做进一步的详细说明:
如图2所示,基于改进型互补滤波的室内行人导航磁航向解算方法,具体包含如下步骤,
步骤1,从时域角度获得互补滤波器参数k1和k2,具体计算如下:
k1=1-k2
其中,是陀螺仪积分获得的角度的方差,是加速度获得的角度的方差,k1为陀螺仪的权重,k2为加速度计的权重;
步骤2,根据步骤1获取的互补滤波器参数建立改进型互补滤波器算法模型,进而计算出姿态角,具体计算如下:
fk1=fk-Δfk-1
其中Δfk-1为k-1时刻行人引入的加速度,fk为k时刻实测加速度计数据,fk1为修正误差后的加速度数据,T为采样周期,w为陀螺仪输出的角速度,θk为k时刻滤波后的姿态角,为陀螺仪在k-1时刻通过积分解算出的角度,加速度计在k时刻解算出来的角度;
步骤3,利用步骤2计算出的姿态角构建投影矩阵,将磁传感器数据投影到水平面:利用天向陀螺每秒绝对值积分进行模式判别,若行人步行、跑步时的模态为非稳态状态,则直接置航向角为零,不输出磁航向角;若行人步行、跑步时的模态为稳态状态,则补偿磁传感器误差,计算出精确地磁航向角,并更新Δfk-1:
其中为k-1时刻坐标转换矩阵,为重力矢量。
具体步骤如下:
步骤一,从时域角度获得互补滤波器参数k1和k2:
假设陀螺仪与加速度计在水平静止状态下同时测量一段时间,由陀螺仪积分获得的角度与加速度计获得的角度,其方差分别为和
假设陀螺仪的权重为k1,加速度计的权重为k2,则对真值的估计为:
且
k1+k2=1
则估计值的误差(方差)为
对其求偏导,可以求得最小值
此时,最优解参数为
通过最优参数可以求得此刻最优姿态角。
步骤二:建立改进型互补滤波器参数模型
利用加速度计采集的信息按下面公式所示可以解算出姿态角,陀螺仪通过积分也可以解算出姿态角。
其中,θ、γ分别为俯仰角与横滚角。
其中k1,k2为从时域角度利用最优加权平均估计分析获得的权重参数,则可得姿态角:
θ=k1*θw+k2*θf
其中θ为滤波后姿态角,θw为陀螺仪积分解算出的角度,θf为加速度计解算出来的角度。
又因为:
则
其中T为采样周期,此即为姿态角互补滤波方程。
如图1所示,改进型互补滤波器即如下表述:由k-1时刻计算得到的横滚角、俯仰角、航向角构成投影矩阵将重力矢量投影到机体三轴,计算其与k-1时刻加速度计输出的差值,即为k-1时刻人为引入的三轴加速度。在k时刻,将k时刻加速度计三轴输出量与k-1时刻人为引入的的加速度的差值作为k时刻互补滤波器加速度计的输入量计算得到k时刻的横滚角与俯仰角,然后计算k时刻航向角,如此循环。由此,消除了行人自带加速度的干扰。即
fk=fk-Δfk-1
步骤三:构建行人运动模态辨识算法
行人手持载体在步行、跑步场景下,磁航向角解算主要分为两种模态,一种模态为行人行走、跑步时人体手臂摆动下的非稳态状态,一种模态为行人行走、跑步时的稳态查看状态。通过观测z轴陀螺每秒角速度绝对值积分对两种模态进行判别,可以有效辨别出磁航向角解算的必要性。在非稳态状态下,磁航向角计算无意义;稳态状态下,磁航向角实时输出。模式判别如下所示:
其中,w为z轴方向陀螺仪输出的角速度,T为采样周期。对陀螺仪输出的角速度绝对值进行积分可以避免人体摆动周期性相消。
当满足
θz<ε
此时判为手持稳态查看状态,否则为非稳态状态,其中ε为通过统计学方法计算出来的参数值,θz为z轴方向每秒绝对值积分计算出得角度。
步骤四:构建磁航向实时解算算法:
在步行、跑步前,在水平方向上静止,利用最优加权平均估计法设计出互补滤波器参数,然后实时采集陀螺仪加速度计数据,通过对每秒z轴陀螺绝对值进行积分,判断此时行人步行、跑步时的模态,如果判为非稳态状态,则直接置航向角为0,此时输出无意义,同时人为引入的额外加速度不进行更新;如果判为稳态状态,则每秒利用改进型互补滤波器获得此时姿态角,对磁传感器数据进行坐标投影,并利用最小二乘法标定好的二维椭圆参数,补偿磁传感器误差,实时输出磁航向角。
磁航向角可由下式获得:
其中,为磁航向角,Y为补偿后的横向磁传感器信息,X为补偿后的纵向磁传感器信息。
Claims (1)
1.基于改进型互补滤波的室内行人导航磁航向解算方法,其特征在于:具体包含如下步骤,
步骤1,从时域角度获得互补滤波器参数k1和k2,具体计算如下:
k1=1-k2
其中,是陀螺仪积分获得的角度的方差,是加速度获得的角度的方差,k1为陀螺仪的权重,k2为加速度计的权重;
步骤2,根据步骤1获取的互补滤波器参数建立改进型互补滤波器算法模型,进而计算出姿态角,具体计算如下:
fk1=fk-Δfk-1
θ=k1*θw+k2*θf
又因为:
则
其中θ为滤波后姿态角,Δfk-1为k-1时刻行人引入的加速度,fk为k时刻实测加速度计数据,fk1为修正误差后的加速度数据,θw为陀螺仪积分解算出的角度,θf为加速度计解算出来的角度;T为采样周期,w为陀螺仪输出的角速度,θk为k时刻滤波后的姿态角,为陀螺仪在k-1时刻通过积分解算出的角度,为加速度计在k时刻解算出来的角度;
步骤3,利用步骤2计算出的姿态角构建投影矩阵,将磁传感器数据投影到水平面:利用天向陀螺每秒绝对值积分进行模式判别,若行人步行、跑步时的模态为非稳态状态,则直接置航向角为零,不输出磁航向角;若行人步行、跑步时的模态为稳态状态,则补偿磁传感器误差,计算出精确地磁航向角,并更新Δfk-1:
其中为k-1时刻坐标转换矩阵,为重力矢量;
行人手持载体在步行、跑步场景下,磁航向角解算主要分为两种模态,一种模态为行人行走、跑步时人体手臂摆动下的非稳态状态,一种模态为行人行走、跑步时的稳态查看状态;通过观测z轴陀螺每秒角速度绝对值积分对两种模态进行判别,可以有效辨别出磁航向角解算的必要性;在非稳态状态下,磁航向角计算无意义;稳态状态下,磁航向角实时输出;模式判别如下所示:
其中,w为z轴方向陀螺仪输出的角速度,T为采样周期,对陀螺仪输出的角速度绝对值进行积分可以避免人体摆动周期性相消;
当满足
θz<ε
此时判为手持稳态查看状态,否则为非稳态状态,其中ε为通过统计学方法计算出来的参数值,θz为z轴方向每秒绝对值积分计算出的角度;
在步行、跑步前,在水平方向上静止,利用最优加权平均估计法设计出互补滤波器参数,然后实时采集陀螺仪加速度计数据,通过对每秒z轴陀螺绝对值进行积分,判断此时行人步行、跑步时的模态,如果判为非稳态状态,则直接置航向角为0,此时输出无意义,同时人为引入的额外加速度不进行更新;如果判为稳态状态,则每秒利用改进型互补滤波器获得此时姿态角,对磁传感器数据进行坐标投影,并利用最小二乘法标定好的二维椭圆参数,补偿磁传感器误差,实时输出磁航向角;
磁航向角由下式获得:
其中,为磁航向角,Y为补偿后的横向磁传感器信息,X为补偿后的纵向磁传感器信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610442719.XA CN106123900B (zh) | 2016-06-20 | 2016-06-20 | 基于改进型互补滤波的室内行人导航磁航向解算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610442719.XA CN106123900B (zh) | 2016-06-20 | 2016-06-20 | 基于改进型互补滤波的室内行人导航磁航向解算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106123900A CN106123900A (zh) | 2016-11-16 |
CN106123900B true CN106123900B (zh) | 2019-05-31 |
Family
ID=57469887
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610442719.XA Active CN106123900B (zh) | 2016-06-20 | 2016-06-20 | 基于改进型互补滤波的室内行人导航磁航向解算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106123900B (zh) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106646569B (zh) * | 2016-11-18 | 2020-04-14 | 华为技术有限公司 | 一种导航定位方法及设备 |
CN106767789B (zh) * | 2017-01-12 | 2019-12-24 | 南京航空航天大学 | 一种基于自适应卡尔曼滤波的行人航向最优融合方法 |
CN106996780B (zh) * | 2017-04-24 | 2020-05-05 | 湖南格纳微信息科技有限公司 | 一种航向误差修正方法与装置及磁场检测方法与装置 |
CN110020445B (zh) * | 2017-07-27 | 2023-08-11 | 阿里巴巴(中国)有限公司 | 一种分配互补权重参数的方法及装置 |
CN109753841B (zh) * | 2017-11-01 | 2023-12-12 | 比亚迪股份有限公司 | 车道线识别方法和装置 |
CN108444467B (zh) * | 2017-11-17 | 2021-10-12 | 西北工业大学 | 一种基于反馈互补滤波和代数逼近的行人定位方法 |
CN108398128B (zh) * | 2018-01-22 | 2021-08-24 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
CN108917756A (zh) * | 2018-06-12 | 2018-11-30 | 四川斐讯信息技术有限公司 | 一种航向估计方法及系统 |
CN112985384B (zh) * | 2019-12-15 | 2023-04-14 | 北京原子机器人科技有限公司 | 一种抗干扰磁航向角优化系统 |
CN111044072A (zh) * | 2019-12-31 | 2020-04-21 | 重庆金康特智能穿戴技术研究院有限公司 | 基于智能穿戴设备的人体运动状态判断方法和计步方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104020444A (zh) * | 2014-06-23 | 2014-09-03 | 西北工业大学 | 一种双轮差动式机器人室内组合定位方法 |
CN104237952A (zh) * | 2014-10-09 | 2014-12-24 | 广州市香港科大霍英东研究院 | 一种墙内物体的识别方法 |
CN105043385A (zh) * | 2015-06-05 | 2015-11-11 | 北京信息科技大学 | 一种行人自主导航定位的自适应卡尔曼滤波方法 |
CN105652306A (zh) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7890260B2 (en) * | 2005-11-01 | 2011-02-15 | Honeywell International Inc. | Navigation system with minimal on-board processing |
-
2016
- 2016-06-20 CN CN201610442719.XA patent/CN106123900B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104020444A (zh) * | 2014-06-23 | 2014-09-03 | 西北工业大学 | 一种双轮差动式机器人室内组合定位方法 |
CN104237952A (zh) * | 2014-10-09 | 2014-12-24 | 广州市香港科大霍英东研究院 | 一种墙内物体的识别方法 |
CN105043385A (zh) * | 2015-06-05 | 2015-11-11 | 北京信息科技大学 | 一种行人自主导航定位的自适应卡尔曼滤波方法 |
CN105652306A (zh) * | 2016-01-08 | 2016-06-08 | 重庆邮电大学 | 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法 |
Non-Patent Citations (1)
Title |
---|
基于互补滤波器的HDE算法在室内行人航迹定位中的研究;胡东旭;《吕梁学院院报》;20160430;第6卷(第2期);全文 |
Also Published As
Publication number | Publication date |
---|---|
CN106123900A (zh) | 2016-11-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106123900B (zh) | 基于改进型互补滤波的室内行人导航磁航向解算方法 | |
WO2020253854A1 (zh) | 移动机器人姿态角解算方法 | |
US10184797B2 (en) | Apparatus and methods for ultrasonic sensor navigation | |
CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
AU2011374152B2 (en) | Moving direction determination with noisy signals from inertial navigation systems on mobile devices | |
Racko et al. | Pedestrian dead reckoning with particle filter for handheld smartphone | |
CN106885566B (zh) | 一种可穿戴式运动传感器及其抗磁场干扰的方法 | |
CN106052685B (zh) | 一种两级分离融合的姿态和航向估计方法 | |
KR20150108402A (ko) | 디바이스와 보행자 사이의 오정렬 결정을 위한 방법 및 장치 | |
CN108225324B (zh) | 一种基于智能终端的地磁匹配与pdr融合的室内定位方法 | |
CN109099913B (zh) | 一种基于mems惯性器件的穿戴式导航装置和方法 | |
US10652696B2 (en) | Method and apparatus for categorizing device use case for on foot motion using motion sensor data | |
CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
CN107255474B (zh) | 一种融合电子罗盘和陀螺仪的pdr航向角确定方法 | |
CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
CN109459028A (zh) | 一种基于梯度下降的自适应步长估计方法 | |
CN109612463B (zh) | 一种基于侧向速度约束优化的行人导航定位方法 | |
CN109724602A (zh) | 一种基于硬件fpu的姿态解算系统及其解算方法 | |
CN107490378A (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
CN109764870B (zh) | 基于变换估计量建模方案的载体初始航向估算方法 | |
CN103712598A (zh) | 一种小型无人机姿态确定系统与确定方法 | |
Wu et al. | Indoor positioning system based on inertial MEMS sensors: Design and realization | |
CN103557866A (zh) | 一种基于地磁技术的虚拟陀螺仪及算法 | |
Tian et al. | Pedestrian navigation system using MEMS sensors for heading drift and altitude error correction | |
CN113532477A (zh) | 一种骑行码表设备及骑行码表初始姿态自动校准方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |