CN106996780B - 一种航向误差修正方法与装置及磁场检测方法与装置 - Google Patents

一种航向误差修正方法与装置及磁场检测方法与装置 Download PDF

Info

Publication number
CN106996780B
CN106996780B CN201710270156.5A CN201710270156A CN106996780B CN 106996780 B CN106996780 B CN 106996780B CN 201710270156 A CN201710270156 A CN 201710270156A CN 106996780 B CN106996780 B CN 106996780B
Authority
CN
China
Prior art keywords
magnetic field
course
gyroscope
person
resolving
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
Application number
CN201710270156.5A
Other languages
English (en)
Other versions
CN106996780A (zh
Inventor
方楚雄
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan yunjiangna micro Information Technology Co.,Ltd.
Original Assignee
Hunan Glonavin Information Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hunan Glonavin Information Technology Co ltd filed Critical Hunan Glonavin Information Technology Co ltd
Priority to CN201710270156.5A priority Critical patent/CN106996780B/zh
Publication of CN106996780A publication Critical patent/CN106996780A/zh
Application granted granted Critical
Publication of CN106996780B publication Critical patent/CN106996780B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01RMEASURING ELECTRIC VARIABLES; MEASURING MAGNETIC VARIABLES
    • G01R33/00Arrangements or instruments for measuring magnetic variables
    • G01R33/02Measuring direction or magnitude of magnetic fields or magnetic flux

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Condensed Matter Physics & Semiconductors (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明提供一种航向误差修正方法,包括如下步骤:(a)采集磁场测量数据、陀螺仪测量数据、加速度计测量数据;(b)计算检验统计量;(c)判定在人员行进中第s步到第s+m‑1步之间对应的时间区间内磁场是否处于稳态;(d)若磁场处于稳态,则将磁场解算航向作为观测量,利用卡尔曼滤波对航向角误差进行估计,利用航向角误差对人员的航向进行修正。本发明还提供一种航向误差修正装置,包括加速度计、陀螺仪、磁力计、信号采集单元、信号处理单元。本发明还提供一种磁场检测方法与装置。

Description

一种航向误差修正方法与装置及磁场检测方法与装置
技术领域
本发明涉及一种航向误差修正方法,尤其涉及一种陀螺仪航向漂移误差修正方法。本发明属于室内人员自主定位技术领域,具体说是一种利用稳态磁场修正陀螺仪航向漂移误差的方法与装置及磁场检测方法与装置。
背景技术
在基于微惯导的人员自主定位技术中,利用固定在人员足部微机电系统(Micro-electromechanical Systems,MEMS)MEMS的惯性测量单元(包括微型加速度计和微型陀螺仪)对人体运动进行测量,积分推演获得人员运动轨迹。该技术无需在定位场所预先安装任何设备,具有自主性、低功耗及便携性等优势,已成为卫星导航定位不可靠情况下重要的“补盲”手段。
受制造工艺水平的限制,微型陀螺仪在实际的应用中会产生漂移,导致陀螺仪解算航向误差随时间不断积累,导致定位误差不断增大。地磁场是一种稳定的地理信息,通常被用于辅助修正陀螺仪航向误差。但在室内场景中,由于地磁场受墙体、电子设备等的干扰比较严重,导致地磁场解算航向存在较大误差。
考虑到地磁场在某些地点或者时间段受到的干扰很小,受到干扰很小的磁场称为稳态磁场。在稳态磁场中,解算出的航向误差可以忽略,因此可用来修正陀螺仪航向误差。
从受干扰磁场中提取稳态磁场依赖于稳态磁场检测方法。传统的稳态磁场检测方法通过磁场幅度的变换率作为判断磁场受干扰程度的依据,当磁场幅度变化率小于某一阈值时,判定该时刻磁场处于稳态。若磁场在短时间内受到比较稳定的干扰,则该方法很容易造成虚警检测,即误将受干扰磁场判定为稳态磁场。若用其解算航向值修正陀螺仪航向,反而会引入额外的误差,极大地影响测量精度。在受到干扰的情况下,磁场在导航坐标系(东-北-天)三个轴向的受干扰程度往往是不相同的,即各轴向磁场变化比例不同,导致解算出来的航向存在较大的波动,而这样的磁场航向角无法有效的被用来修正陀螺仪航向角的长期漂移特性。
发明内容
本发明要解决的问题是针对现有技术中利用稳态磁场的解算航向值修正陀螺仪航向会引入额外的误差的问题,提供一种新的航向误差修正方法与装置及磁场检测方法与装置。
为解决上述技术问题,本发明采用的技术方案是:一种航向误差修正方法,包括如下步骤:
(a)人员行进中,以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据;
(b)利用下式计算检验统计量T
Figure BDA0001277142020000021
其中,α1、α2、α3、α4分别为第一权值、第二权值、第三权值、第四权值,且α1、α2、α3、α4的取值范围均为[0,1],且α1234=1,m为人员行进的步数间隔,m>2,s为人员步数,Bi
Figure BDA0001277142020000022
θi、ξi分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差,
Figure BDA0001277142020000023
分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差,Bi
Figure BDA0001277142020000024
θi、ξi
Figure BDA0001277142020000025
均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到;
(c)若T<γ,则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态,并跳转到步骤(d),否则跳转到步骤(a),其中,γ为磁场稳态检测门限;
(d)在由步骤(c)得到的人员行进中第s步到第s+m-1步之间对应的时间区间内,将磁场解算航向作为观测量,利用卡尔曼滤波对航向角误差进行估计,利用航向角误差对人员的航向进行修正。
在受到干扰的情况下,磁场在导航坐标系(东-北-天)三个轴向的受干扰程度往往是不相同的,即各轴向磁场变化比例不同,导致解算出来的航向存在较大的波动,而这样的磁场航向角无法有效的被用来修正陀螺仪航向角的长期漂移特性。本发明中,利用构造检验统计量T检验磁场是否为稳态,即判断磁场解算航向的可信度,若磁场为稳态,则利用磁场航向角修正陀螺仪航向角的漂移。
若α4=0,则利用磁场解算航向的相对变化、磁场幅度的相对变化、陀螺仪短时航向的相对变化构造检验统计量,则对直线行走的效果较好;若α1、α2、α3、α4均不为0,则由于加入了磁场解算航向与陀螺仪解算航向的偏差值的变化量Δξs,因此可以在连续转弯环境下确定磁场角度连续变化以及陀螺连续角度变化的之间的差异,如果磁场解算航向与陀螺仪解算航向两者之间的相对角度变化偏差是稳定的,则对于连续转弯的行走线路的磁场的稳态策略仍然可以对陀螺起到修正作用,因此对曲线行走线路甚至是连续曲线的行走线路的修正较好。
上述技术方案中,所述步骤(b)中,Bi
Figure BDA0001277142020000031
θi分别为从人员行进中第i步的静止时间区间中任取的时刻在导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向,其中,第i步的静止时间区间为人员行进中第i步对应的人员足部与地接触的时间区间。
上述技术方案中,所述步骤(b)中,第i步的静止时间区间通过如下步骤计算:
(b1)计算人员足部在k时刻的加速度模值
Figure BDA0001277142020000032
其中(ax(k),ay(k),az(k))为k时刻人员足部在导航坐标系下的加速度;
(b2)利用下式计算偏差σk和方差λk
Figure BDA0001277142020000033
其中M1的取值范围为3≤M1≤10,M2的取值范围为3≤M2≤10,且M1、M2均为整数;
(b3)设置偏差门限ηBIAS、方差门限ηv,若式子λk≥ηv和σk≥ηBIAS中至少有一项成立,则判断人员足部在k时刻为运动状态,反之,则判断人员足部在k时刻为静止状态;
(b4)若检测到人员足部在时间区间[k1i,k2i]内的每个时刻均为静止状态,而检测到人员足部在k1i-1时刻、k2i+1时刻均为运动状态,则认为时间区间[k1i,k2i]为人员行进中的第i步对应的静止时间区间。
上述技术方案中,所述步骤(b)中,θi通过姿态四元数计算得到,Bi
Figure BDA0001277142020000034
ξi通过下式计算得到
Figure BDA0001277142020000035
其中,(Bi,x,Bi,y,Bi,z)为人员行进中的第i步时人员足部在导航坐标系下的磁场强度,D为地磁偏角。
上述技术方案中,所述的步骤(b)中,m=5。
上述技术方案中,所述的步骤(c)中,磁场稳态检测门限γ通过接收器操作特性曲线确定。
上述技术方案中,所述步骤(d)中,利用航向角误差对人员行进中第s+m-1步对应的航向进行修正或对第s步到第s+m-1步中对应的各个航向进行修正。
本发明还提供一种实现上述航向误差方法的航向误差修正装置,包括设置于人员足部且具有相同载体坐标系的加速度计、陀螺仪、磁力计,还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元,还包括与所述信号采集单元连接的信号处理单元。
本发明还提供一种磁场检测方法,在人员足部设置具有相同载体坐标系的加速度计、陀螺仪、磁力计,所述磁场检测方法包括如下步骤:
(a)人员行进中,以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据;
(b)利用下式计算检验统计量T
Figure BDA0001277142020000041
其中,α1、α2、α3、α4分别为第一权值、第二权值、第三权值、第四权值,且α1、α2、α3、α4的取值范围均为[0,1],且α1234=1,m为人员行进的步数间隔,m>2,s为人员步数,Bi
Figure BDA0001277142020000042
θi、ξi分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差,
Figure BDA0001277142020000043
分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差,Bi
Figure BDA0001277142020000044
θi、ξi
Figure BDA0001277142020000045
均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到;
(c)若T<γ,则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态。
本发明还提供一种磁场检测装置,包括设置于人员足部且具有相同载体坐标系的加速度计、陀螺仪、磁力计,还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元,还包括与所述信号采集单元连接的信号处理单元。
本发明具有的优点和积极效果是:本发明可以极大改善利用磁力计航向角修正惯导航向误差的精度,降低室内外环境因素对磁力计干扰的影响;新的检测策略在保证有效磁场向角检测,降低检测误检率的同时,通过融合修正抑制了惯导航向的累积误差。本发明直线行走的效果较好,而且对曲线行走线路甚至是连续曲线的行走线路的修正也有较好的效果。相比较传统检测方法,本发明可大幅降低稳态磁场的检测虚警率,提高检测精度。采用本发明的方法,导航定位轨迹的航向误差明显降低。
附图说明
图1为本发明的航向误差修正方法的处理流程示意图;
图2为本发明的航向误差修正装置的结构示意图;
图3为本发明的磁场检测方法处理流程示意图;
图4为检测统计量曲线,图中横坐标为步数,纵坐标为检测统计量,图中采用的参数为:M1、M2取值为5,σθ=0.56rad/s,
Figure BDA0001277142020000051
图5为定位效果对比图,图中外围实线表示墙体,实心方形曲线为采用传统检测方法解算轨迹,空心圆形曲线为采用本发明的航向误差修正方法的解算轨迹;
图中,1、人员足部,2、加速度计,3、陀螺仪,4、磁力计,5、信号采集单元,6、信号处理单元,7、数据传输单元。
具体实施方式
下面结合附图对本发明作进一步说明。
本发明中,若判定人员行进过程中某一步的磁场为稳态磁场,则对相应的静止时间区间内的各个时刻的磁场数据求出的航向角进行修正。
稳态磁场是对地磁场稳定性的判断性描述,由于在复杂的室内或者室外环境下地磁场会受到不同程度的污染干扰,从而导致依据其计算出来的航向角存在干扰,并间接的影响后续陀螺航向角修正。稳态磁场中,在固定时间间隔内,磁传感器获取的多个当地地磁场总强度采样值保持稳定,或者说磁传感器获取到的磁场强度随时间的变化率约等于零,磁传感器可采用磁力计。
本发明的基本思路是:在受到干扰的情况下,磁场在导航坐标系(东-北-天)三个轴向的受干扰程度往往是不相同的,即各轴向磁场变化比例不同,导致解算出来的航向存在较大的波动,而这样的磁场航向角无法有效的被用来修正陀螺仪航向角的长期漂移特性。因此本发明利用磁场解算航向的相对变化、磁场幅度的相对变化以及陀螺仪短时航向的相对变化共同作为磁场解算航向可信度的一个判断,即发明中提出的稳态磁场检测。
如图1—图5所示,将足部模块安装在鞋尖处,其中足部模块融合了加速度计2、陀螺仪3、磁力计4,且加速度计2、陀螺仪3、磁力计4具有相同方向的载体坐标系和导航坐标系。测量坐标系即为载体坐标系。其中载体指人员足部1。实验中,人员沿着某办公楼一楼内墙行走一圈后回到出发点,路径总长约626米。本专利实验所用模块采样率为400Hz。磁场测量数据即为磁力计4测量得到的数据。磁场解算航向即为由磁场测量数据解算得到的航向。利用其它磁场测量装置测量得到磁场测量数据也可实现本发明的方法。
本发明的航向误差修正方法包括以下处理步骤:
步骤(a):数据采集
人员行进中,设置采样周期,以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据,采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的固定周期可根据实际需要或由各个产品的实际参数确定,例如可设置为0.001-5秒的任意数值或其他数值,k为采样时刻,k=0,1,2,3……;s=1,2,3……;三轴加速度计在k时刻测得人员足部1在载体坐标系下的加速度计测量数据为(ax(k),ay(k),az(k)),磁力计4在k时刻测得人员足部1部在载体坐标系下的磁场强度为(hx(k),hy(k),hz(k)),k时刻人员足部1在导航坐标系下的磁场强度为(Bx(k),By(k),Bz(k));
步骤(b):参数计算
利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算人员行进中第i步在导航坐标系下的磁场幅度Bi、磁场解算航向
Figure BDA0001277142020000061
陀螺仪解算航向θi以及人员行进中第i步的导航坐标系下的磁场幅度Bi、磁场解算航向
Figure BDA0001277142020000062
陀螺仪解算航向θi、磁场解算航向与陀螺仪解算航向的偏差ξi以及磁场幅度的噪声方差
Figure BDA0001277142020000063
磁场解算航向的噪声方差
Figure BDA0001277142020000064
陀螺仪解算航向的噪声方差
Figure BDA0001277142020000065
磁场解算航向与陀螺仪解算航向的偏差的方差
Figure BDA0001277142020000066
其中s为人员步数,s=1,2,3……,i∈[s,s+m-1]。本实施例中,利用步态静止检测方法确定人员各个步子的静止时间区间,然后利用第i步的静止时间区间中任取的时刻在导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向得到Bi
Figure BDA0001277142020000067
θi、ξi。实际过程中,也可不计算静止时间区间,通过其他类似方法得到Bi
Figure BDA0001277142020000068
θi、ξi
Figure BDA0001277142020000069
等各个参数。本实施例的静止检测方法中,将固定长度时间窗口内的加速计模值和模值方差作为检测依据,且时间窗口随着时间变化滑动。第i步的静止时间区间为人员行进中第i步对应的足部与地接触的时间区间。
(b1)计算人员足部1在k时刻的加速度模值
Figure BDA00012771420200000610
(b2)利用下式计算偏差σk和方差λk,将方差λk取为短时间计算区间(k-M1,k+M2),从而避免了判断延迟,
Figure BDA00012771420200000611
其中M1的取值范围为3≤M1≤10,M2的取值范围为3≤M2≤10,且M1、M2均为整数,本实施例中,M1、M2的优选值为5;
(b3)设置偏差门限ηBIAS、方差门限ηv,若式子λk≥ηv和σk≥ηBIAS中至少有一项成立,则判断人员足部1在k时刻为运动状态,反之,则判断人员足部1在k时刻为静止状态。本步骤中,考虑了人员行走的规律,判断偏差和方差须同时满足条件,才能判断人员足部为静止状态。偏差门限ηBIAS和方差门限ηv根据加速度计测量数据的统计特性来确定,例如采集加速度计测量数据,通过试验确定偏差门限ηBIAS和方差门限ηv的取值。此处的加速度统计特性是依据实际应用是所使用的加速度传感器的型号不同,它的硬件性能不同,以及器件安装使用的不同,而确定加速计统计特性,本领域的人员可以理解;所述偏差门限ηBIAS和方差门限ηv通过加速计测量数据的统计特性计算得到,本实施例中,方差门限ηv可设置为0.05(m/s2)2到0.8(m/s2)2之间的数值,偏差门限ηBIAS可设置为小于0.15m/s2的数值;
(b4)若检测到人员足部1在时间区间[k1i,k2i]内的每个时刻均为静止状态,而检测到人员足部1在k1i-1时刻、k2i+1时刻均为运动状态,则认为时间区间[k1i,k2i]为人员行进中的第i步对应的静止时间区间,同理即可得到人员行进中每一步对应的静止时间区间,第i步的静止时间区间即为人员行进中第i步对应的足部与地接触的时间区间,其中k1i<k2i
(b5)将静止时间区间中的磁场数据由载体坐标系转换到导航坐标系
设k时刻磁力计的载体坐标系与导航坐标系之间的欧拉角为(r(k),p(k),y(k)),其中r(k)为横滚角,p(k)为俯仰角,y(k)为航向角。在足部着地时的静止时间区间内,r(k)和p(k)可由人员足部1的加速度数据计算得出,即:
Figure BDA0001277142020000071
其中,g为重力加速度。
由于在k时刻测得的人员足部1的磁场强度(hx(k),hy(k),hz(k))是在载体坐标系中的数据,而通常定位是在导航坐标系下进行,因此,需在进行解算航向前将载体坐标系下的磁场数据(hx(k),hy(k),hz(k))转换为导航坐标系下的磁场强度(Bx(k),By(k),Bz(k)),转换通过下式进行:
Figure BDA0001277142020000081
(b6)计算静止时间区间内磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差
磁场幅度的噪声方差
Figure BDA0001277142020000082
磁场解算航向的噪声方差
Figure BDA0001277142020000083
陀螺仪解算航向的噪声方差
Figure BDA0001277142020000084
磁场解算航向与陀螺仪解算航向的偏差
Figure BDA0001277142020000085
可通过长时间进行实验,利用统计方法计算得出,可将足部模块静止放置,连续采集两小时的磁场幅度的噪声数据、磁场解算航向的噪声数据及陀螺仪解算航向的噪声数据,经过计算得到
Figure BDA0001277142020000086
本实施例中的采集时间不对本发明作出限制,数据的采集时间根据实际需要确定。
Figure BDA0001277142020000087
根据磁场解算航向的噪声数据及陀螺仪解算航向的噪声数据计算,因为这个变量是与这两个变量相关的。
在人员足部1行进中某一步的静止时间区间内,人员足部1的磁场强度基本保持稳定,即磁场强度基本不发生变化,因此,计算该步磁场幅度时,可取静止时间区间中的任意采样点或根据一小段采样点的平均数据得到。若在人员行进中第i步的静止时间区间[k1i,k2i]内选取采样点,则可任取时刻k3i,其中k3i∈[k1i,k2i]。若在人员行进中第i步的静止时间区间[k1i,k2i]内选取一小段采样点的平均值,则可选取时间区间[k11i,k12i]并将各个采样点求平均值,其中k1i、k12i均为采样时刻,且k11i≥k1i、k12i≤k2i
当下一步(第i+1步)静止时间区间到来时,用相同方法计算第i+1步的陀螺仪航向、磁场幅度及磁场解算航行,并分别计算相邻两步(第i和第i+1步之间)的变化量。
人员行进过程中,每一步都对应静止时间区间和运动时间区间,且两步分别对应的静止时间区间之间的时间区间即为人员的运动时间区间。在本发明中,人员开始行进,即利用上述步骤(b1)-(b4)检测各个静止时间区间,由此可以将人员行进中第i步与第i步对应的静止时间区间相对应。
人员行进中第i步的导航坐标系下的磁场幅度Bi由下式计算:
Figure BDA0001277142020000088
同理计算得到第i+1步的磁场幅度Bi+1,则相邻两步(第i和第i+1步之间)的磁场幅度变化量ΔBi为:
ΔBi=|Bi+1-Bi|,
初始值ΔB1=0。人员行进中第i步的导航坐标系下的磁场解算航向
Figure BDA0001277142020000089
由下式计算:
Figure BDA0001277142020000091
其中D为地磁偏角,可通过查阅资料获得。
同理计算得到第i+1步的磁场解算航向
Figure BDA0001277142020000092
则相邻两步(第i+1和第i步之间)的磁场解算航向变化量
Figure BDA0001277142020000093
为:
Figure BDA0001277142020000094
人员行进中,利用陀螺仪测量数据,通过姿态四元数解算获取陀螺航向角,人员行进中姿态四元数的计算本领域的人员是可以理解的。人员行进中第i步的陀螺仪解算航向表示为θi
同理计算得到第i+1步的陀螺仪解算航向θi+1,则相邻两步(第i+1和第i步之间)的陀螺仪解算航向变化量Δθi为:
Δθi=|θi+1i|
人员行进中第i步的磁场解算航向与陀螺仪解算航向的偏差表示为
Figure BDA0001277142020000095
同理计算得到第i+1步的磁场解算航向与陀螺仪解算航向的偏差ξi+1,则相邻两步(第i+1和第i步之间)的磁场解算航向与陀螺仪解算航向的偏差变化量Δξi为:
Δξi=|ξi+1i|。
(b7)计算检验统计量
结合似然比检测,令检测统计量T为
Figure BDA0001277142020000096
为提高鲁棒性,在上式的磁场稳态检测中计算相邻m步的磁场变化量,其中m的取值范围为m>2,优选取值范围为2<m≤7,m优选取值为5。s为人员步数。
α1、α2、α3、α4分别为第一权值、第二权值、第三权值、第四权值,且α1、α2、α3、α4的取值范围均为[0,1],且α1234=1,α1、α2、α3、α4为分别与磁场幅度变化量、磁场解算航向变化量、陀螺仪解算航向变化量、磁场解算航向与陀螺仪解算航向的偏差变化量对应的权值。
α1、α2、α3、α4根据实际试验或实际工程需要进行计算,或可根据实际效果进行调整,或可利用现有的数学方法,对α1、α2、α3、α4的最优值进行计算。
若α4=0,则检验统计量T仅由磁场幅度变化量、磁场解算航向变化量、陀螺仪解算航向变化量决定。
若α1、α2、α3、α4均不为0,则由于加入了磁场解算航向与陀螺仪解算航向的偏差值的变化量Δξs,则可以在连续转弯环境下确定磁场角度连续变化以及陀螺连续角度变化的之间的差异,如果磁场解算航向与陀螺仪解算航向两者之间的相对角度变化偏差是稳定的,则对于连续转弯的环境下磁场的稳态策略依旧可以对陀螺起到修正作用,因此对曲线行走线路甚至是连续曲线的行走线路的修正较好。
检测统计量曲线如图2所示。图中横坐标为步数,纵坐标为检测统计量,其中M1、M2取值为5,σθ=0.56rad/s,
Figure BDA0001277142020000101
γ=554mGs。
步骤(c):磁场稳态检测
磁场稳态检测门限γ可通过接收器操作特性曲线(Receiver OperatingCharacteristic curve,或称ROC曲线)确定,接收器操作特性曲线通过改变磁场稳态检测门限,计算该磁场稳态检测门限下样本数据的检测概率和虚警概率,以虚警概率为横坐标,检测概率为纵坐标绘制得出的曲线。磁场稳态检测门限γ的值不恒定。γ的单位为毫高斯(mGs)。本实施例中,γ的优选取值为850mGs,此值根据所在实验环境,考虑了周围磁场干扰的情况下确定的磁场稳态检测门限γ。在实际专利应用中,可以根据需要改变γ值,但γ的变动幅度应参考标准地球磁场标准值。
将T与磁场稳态检测门限比较γ,若T<γ,则判定磁场处于稳态。若磁场处于稳态,则跳转到步骤(d),否则跳转到步骤(a),继续进行数据采集。
步骤(d):利用磁场解算航向对陀螺仪航向误差进行修正
若在步骤(b)中检测得到磁场处于稳态,则将步骤(b)中的磁场解算航向作为零速更新辅助的卡尔曼滤波的观测量对陀螺仪航向误差进行修正。状态转移矩阵Φk、过程噪声wk-1、观测矩阵H、观测噪声nk、速度误差vk的计算为本领域公知常识。在本专利中,使用现有的卡尔曼滤波方法或卡尔曼滤波方法的变形也可以实现。通过本专利的卡尔曼滤波方法,可以求解得到陀螺和加速度误差,并在后续的计算加以修正。本专利中,在满足磁场稳态条件的人员行进中第s步到第s+m-1步之间的静止时间区间(足部与地接触的时间区间)内进行卡尔曼滤波,卡尔曼滤波是一直进行的过程。
令卡尔曼滤波的状态向量δxk|k为:
Figure BDA0001277142020000111
其中δφk为人员足部1的姿态角误差,人员足部1的姿态角误差包括横滚角误差、俯仰角误差、航向角误差,δωk为人员足部1的三轴角速度误差,δrk为人员足部1的三维位置误差,δvk为人员足部1的三轴速度误差,δak为人员足部1的三轴加速度误差。卡尔曼滤波的状态方程为:
δxk|k-1=Φkδxk-1|k-1+wk-1
其中δxk|k-1为一步预测误差状态向量,δxk-1|k-1为上一步滤波后状态向量,Φk为状态转移矩阵,wk-1为过程噪声,噪声协方差矩阵
Figure BDA0001277142020000112
卡尔曼滤波量测方程为:
zk=Hδxk|k+nk
其中zk为预测误差观测,H为观测矩阵,nk为观测噪声,其协方差矩阵
Figure BDA0001277142020000113
当磁场稳态检测结果为稳态时,磁场解算航向可作为参考校准陀螺仪解算误差。此时真实误差观测为:
mk=[Δψk,vk],
其中vk为速度误差,Δψk为陀螺仪解算航向误差。其中
Figure BDA0001277142020000115
Figure BDA0001277142020000116
为陀螺仪解算航向,θk为磁场解算航向值。
卡尔曼滤波的估计误差协方差矩阵Pk|k-1可表示为:
Figure BDA0001277142020000114
令Gk为卡尔曼滤波增益,则有:
Gk=Pk|k-1HT(HPk|k-1HT+Rk)-1
估计误差状态量可通过下式更新:
δxk|k=δxk|k-1+Gk·[mk-Hδxk|k-1];
其中,δxk|k包含航向角误差估计,因此获得δxk|k后可对航向角进行校正。
本发明还提供一种实现上述航向误差修正方法的航向误差修正装置,航向误差修正装置包括设置于人员足部1且具有相同载体坐标系的加速度计2、陀螺仪3、磁力计4,还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元5,还包括与所述信号采集单元5连接的信号处理单元6。可利用数据传输单元7将信号处理单元6处理得到的数据进行传输。在信号处理单元6中执行本发明中的航向误差修正方法。信号处理单元6可由单片机、DSP、FPGA等构成。
本发明中,利用稳态磁场航向角对惯导航向角进行修正时,可仅修正人员行进中第s+m-1步对应的惯导航向角,也可修正第s步到第s+m-1步中对应的人员的航向。例如,若人员行进中第4-7步时,磁场为稳态,且第3步为非稳态,则可仅修正第7步对应的人员的航向,即可满足要求。如果需要达到更高的定位精度,也可对第4、5、6、7步对应的人员的各个航向均进行修正,也可仅修正第6、7步或第5、6、7步对应的人员的各个航向。实际工作过程中,根据运算效率或实际需要决定。本发明提出的航向误差方法可用于室内定位,也可应用于其他类似的磁场易受干扰的环境的定位中。
图4为实施例的检测统计量的曲线图。从图4中可以看出,检测统计量的波动很大,从侧面反映出室内磁场的干扰比较严重。另外,由图中结果可知,磁场受干扰较小的情况同样存在,可用来修正陀螺仪航向误差。
图5为实施例的定位效果对比图。图中,矩形方框分别代表建筑物中的各个房间结构。从图5中可以看出,采用了传统检测方法解算轨迹航向发散较严重,定位已出现偏差,与真实运动区域完全偏离,定位结果已经不可信,从图中可以看出纯惯导运动轨迹相对实际运动建筑物存在较大的误差,解算轨迹已穿透墙体,定位误差达到4m,航向误差超过11°。采用本发明的航向误差修正方法解算轨迹航向误差明显减小,定位轨迹和建筑物走向重合度明显提高,解算轨迹与实际行走路径基本吻合,达到终点时定位误差不超过0.5m,航向误差仅为2°,且定位的轨迹未出现穿过墙体的情况。
本发明还提供一种磁场检测方法,在人员足部1设置具有相同载体坐标系的加速度计、陀螺仪、磁力计,所述磁场检测方法包括如下步骤:
(a)人员行进中,以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据;
(b)利用下式计算检验统计量T
Figure BDA0001277142020000121
其中,α1、α2、α3、α4分别为第一权值、第二权值、第三权值、第四权值,且α1、α2、α3、α4的取值范围均为[0,1],且α1234=1,m为人员行进的步数间隔,m>2,s为人员步数,Bi
Figure BDA0001277142020000122
θi、ξi分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差,
Figure BDA0001277142020000131
分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差,Bi
Figure BDA0001277142020000132
θi、ξi
Figure BDA0001277142020000133
均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到;
(c)若T<γ,则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态。
本发明还提供一种实现上述磁场检测方法的磁场检测装置,包括设置于人员足部1且具有相同载体坐标系的加速度计2、陀螺仪3、磁力计4,还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元5,还包括与所述信号采集单元5连接的信号处理单元6。
以上对本发明的实施例进行了详细说明,但所述内容仅为本发明的较佳实施例,不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等,均应仍归属于本专利涵盖范围之内。

Claims (10)

1.一种航向误差修正方法,其特征在于:包括如下步骤:
(a)人员行进中,以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据;
(b)利用下式计算检验统计量T
Figure FDA0002381308930000011
其中,α1、α2、α3、α4分别为第一权值、第二权值、第三权值、第四权值,α1234=1,α1、α2、α3、α4的取值范围分别为0<α1<1、0<α2<1、0<α3<1、0<α4<1或α1、α2、α3、α4的取值范围分别为0<α1<1、0<α2<1、0<α3<1、α4=0,m为人员行进的步数间隔,m>2,s为人员步数,Bi
Figure FDA0002381308930000012
θi、ξi分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差,
Figure FDA0002381308930000013
分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差,Bi
Figure FDA0002381308930000014
θi、ξi
Figure FDA0002381308930000015
均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到;
(c)若T<γ,则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态,并跳转到步骤(d),否则跳转到步骤(a),其中,γ为磁场稳态检测门限;
(d)在由步骤(c)得到的人员行进中第s步到第s+m-1步之间对应的时间区间内,将磁场解算航向作为观测量,利用卡尔曼滤波对航向角误差进行估计,利用航向角误差对人员的航向进行修正。
2.根据权利要求1所述的航向误差修正方法,其特征在于:所述步骤(b)中,Bi
Figure FDA0002381308930000016
θi分别为从人员行进中第i步的静止时间区间中任取的时刻在导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向,其中,第i步的静止时间区间为人员行进中第i步对应的人员足部(1)与地接触的时间区间。
3.根据权利要求2所述的航向误差修正方法,其特征在于:所述步骤(b)中,第i步的静止时间区间通过如下步骤计算:
(b1)计算人员足部(1)在k时刻的加速度模值
Figure FDA0002381308930000017
其中(ax(k),ay(k),az(k))为k时刻人员足部(1)在导航坐标系下的加速度;
(b2)利用下式计算偏差σk和方差λk
Figure FDA0002381308930000021
其中M1的取值范围为3≤M1≤10,M2的取值范围为3≤M2≤10,且M1、M2均为整数;
(b3)设置偏差门限ηBIAS、方差门限ηv,若式子λk≥ηv和σk≥ηBIAS中至少有一项成立,则判断人员足部(1)在k时刻为运动状态,反之,则判断人员足部(1)在k时刻为静止状态;
(b4)若检测到人员足部(1)在时间区间[k1i,k2i]内的每个时刻均为静止状态,而检测到人员足部(1)在k1i-1时刻、k2i+1时刻均为运动状态,则认为时间区间[k1i,k2i]为人员行进中的第i步对应的静止时间区间。
4.根据权利要求1-3中任一项所述的航向误差修正方法,其特征在于:所述步骤(b)中,Bi
Figure FDA0002381308930000022
ξi通过下式计算得到
Figure FDA0002381308930000023
其中,(Bi,x,Bi,y,Bi,z)为人员行进中的第i步时人员足部(1)在导航坐标系下的磁场强度,D为地磁偏角。
5.根据权利要求1-3中任一项所述的航向误差修正方法,其特征在于:所述的步骤(b)中,m=5。
6.根据权利要求1-3中任一项所述的航向误差修正方法,其特征在于:所述的步骤(c)中,磁场稳态检测门限γ通过接收器操作特性曲线确定。
7.根据权利要求1-3中任一项所述的航向误差修正方法,其特征在于:所述步骤(d)中,利用航向角误差对人员行进中第s+m-1步对应的航向进行修正或对第s步到第s+m-1步对应的各个航向进行修正。
8.一种实现权利要求1-7中任一项所述的航向误差修正方法的航向误差修正装置,其特征在于:包括设置于人员足部(1)且具有相同载体坐标系的加速度计(2)、陀螺仪(3)、磁力计(4),还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元(5),还包括与所述信号采集单元(5)连接的信号处理单元(6)。
9.一种磁场检测方法,其特征在于:包括如下步骤:
(a)人员行进中,以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据;
(b)利用下式计算检验统计量T
Figure FDA0002381308930000031
其中,α1、α2、α3、α4分别为第一权值、第二权值、第三权值、第四权值,α1234=1,α1、α2、α3、α4的取值范围分别为0<α1<1、0<α2<1、0<α3<1、0<α4<1或α1、α2、α3、α4的取值范围分别为0<α1<1、0<α2<1、0<α3<1、α4=0,m为人员行进的步数间隔,m>2,s为人员步数,Bi
Figure FDA0002381308930000032
θi、ξi分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差,
Figure FDA0002381308930000033
分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差,Bi
Figure FDA0002381308930000034
θi、ξi
Figure FDA0002381308930000035
均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到;
(c)若T<γ,则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态。
10.一种实现权利要求9所述的磁场检测方法的磁场检测装置,其特征在于:包括设置于人员足部(1)且具有相同载体坐标系的加速度计(2)、陀螺仪(3)、磁力计(4),还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元(5),还包括与所述信号采集单元(5)连接的信号处理单元(6)。
CN201710270156.5A 2017-04-24 2017-04-24 一种航向误差修正方法与装置及磁场检测方法与装置 Active CN106996780B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710270156.5A CN106996780B (zh) 2017-04-24 2017-04-24 一种航向误差修正方法与装置及磁场检测方法与装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710270156.5A CN106996780B (zh) 2017-04-24 2017-04-24 一种航向误差修正方法与装置及磁场检测方法与装置

Publications (2)

Publication Number Publication Date
CN106996780A CN106996780A (zh) 2017-08-01
CN106996780B true CN106996780B (zh) 2020-05-05

Family

ID=59434442

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710270156.5A Active CN106996780B (zh) 2017-04-24 2017-04-24 一种航向误差修正方法与装置及磁场检测方法与装置

Country Status (1)

Country Link
CN (1) CN106996780B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108387233B (zh) * 2018-01-30 2021-04-23 北京理工大学 一种基于模糊逻辑的行人运动模式判断方法
CN108444473B (zh) * 2018-03-20 2020-08-28 南京华苏科技有限公司 一种行人室内轨迹定位方法
CN109541499B (zh) * 2018-10-16 2020-08-18 天津大学 多源传感器融合中磁场干扰检测方法
CN109883423B (zh) * 2019-02-25 2021-02-09 广州市香港科大霍英东研究院 基于卡尔曼滤波的定位方法、系统、设备及存储介质
CN110779553A (zh) * 2019-12-03 2020-02-11 中国科学院电子学研究所 磁力计数据的校准方法
WO2021127508A1 (en) * 2019-12-19 2021-06-24 Immersion Networks, Inc. Systems and methods for stabilizing magnetic field of inertial measurement unit
CN112197791B (zh) * 2020-09-15 2022-09-30 武汉华中天纬测控有限公司 一种隔离开关监测设备漂移误差修正方法
CN113375694A (zh) * 2021-05-25 2021-09-10 南京航空航天大学 一种静基座条件下低成本陀螺全部零偏快速估计方法
CN113483753B (zh) * 2021-06-30 2022-11-01 北京航空航天大学 一种基于环境约束的惯性航向误差消除方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1570661A (zh) * 2003-07-16 2005-01-26 张奔牛 一种测量微磁场的方法及其高灵敏传感器
US8018669B2 (en) * 2009-07-16 2011-09-13 Kabushiki Kaisha Toshiba Proximity detection method for magnetic head and recording medium
CN104296776A (zh) * 2013-07-15 2015-01-21 霍尼韦尔国际公司 用于磁力计校准和补偿的系统和方法
CN104335061A (zh) * 2012-06-08 2015-02-04 株式会社藤仓 磁元件控制装置、磁元件控制方法以及磁检测装置
CN104880201A (zh) * 2015-03-26 2015-09-02 武汉大学 Mems陀螺自动标定方法
CN106123900A (zh) * 2016-06-20 2016-11-16 南京航空航天大学 基于改进型互补滤波的室内行人导航磁航向解算方法
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7400142B2 (en) * 2003-11-06 2008-07-15 Stephen John Greelish Dynamic magnetic anomaly compensation
US9863784B2 (en) * 2014-02-23 2018-01-09 PNI Sensor Corporation Orientation estimation utilizing a plurality of adaptive filters

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1570661A (zh) * 2003-07-16 2005-01-26 张奔牛 一种测量微磁场的方法及其高灵敏传感器
US8018669B2 (en) * 2009-07-16 2011-09-13 Kabushiki Kaisha Toshiba Proximity detection method for magnetic head and recording medium
CN104335061A (zh) * 2012-06-08 2015-02-04 株式会社藤仓 磁元件控制装置、磁元件控制方法以及磁检测装置
CN104296776A (zh) * 2013-07-15 2015-01-21 霍尼韦尔国际公司 用于磁力计校准和补偿的系统和方法
CN104880201A (zh) * 2015-03-26 2015-09-02 武汉大学 Mems陀螺自动标定方法
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置
CN106123900A (zh) * 2016-06-20 2016-11-16 南京航空航天大学 基于改进型互补滤波的室内行人导航磁航向解算方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
"Estimating three-dimensional orientation of human body parts by inertial/magnetic sensing";Sabatini AM;《Sensors (Basel)》;20111231;第11卷(第2期);1489-1525 *
"Testing a Modern Gyro-Magnetic Compass";J. D. Ackerman;《Journal of Navigation》;19651231;第18卷(第1期);56-64 *
"一种基于转子磁链t轴分量的感应电机转子时间常数在线校正方法";沈汉林等;《微电机》;20151231;第48卷(第12期);36-39 *
"无人机中磁航向自修正应用";刘程等;《压电与声光》;20140630;第36卷(第3期);389-392 *
"磁航向传感器的误差补偿";李智;《弹箭与制导学报》;20041231;第24卷(第4期);207-208 *
"磁航向测量系统误差修正方法研究";杨新勇等;《仪器仪表学报》;20040831;第25卷(第4期);466-469 *

Also Published As

Publication number Publication date
CN106996780A (zh) 2017-08-01

Similar Documents

Publication Publication Date Title
CN106996780B (zh) 一种航向误差修正方法与装置及磁场检测方法与装置
CN109827577B (zh) 基于运动状态检测的高精度惯性导航定位算法
Jiménez et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU
CN110553646B (zh) 基于惯性和磁航向及零速修正的行人导航方法
CN104061934B (zh) 基于惯性传感器的行人室内位置跟踪方法
CN106225784B (zh) 基于低成本多传感器融合行人航位推算方法
EP3933166B1 (en) Attitude measurement method
EP2657647A1 (en) Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian
CN112362057B (zh) 基于零速修正与姿态自观测的惯性行人导航算法
CN108957510B (zh) 基于惯性/零速/gps的行人无缝组合导航定位方法
CN111721288B (zh) 一种mems器件零偏修正方法、装置及存储介质
Chen et al. IMU/GPS based pedestrian localization
US9677888B2 (en) Determining sensor orientation in indoor navigation
CN106153069B (zh) 自主导航系统中的姿态修正装置和方法
Tao et al. An enhanced foot-mounted PDR method with adaptive ZUPT and multi-sensors fusion for seamless pedestrian navigation
Zhang et al. The improved constraint methods for foot-mounted PDR system
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN107544074A (zh) 一种无人机识别虚假gps信号的方法
CN111649742B (zh) 一种基于anfis辅助的高程估计方法
CN105547291B (zh) 室内人员自主定位系统的自适应静止检测方法
Tao et al. Precise displacement estimation from time-differenced carrier phase to improve PDR performance
Kim et al. Performance improvement and height estimation of pedestrian dead-reckoning system using a low cost MEMS sensor
WO2016165336A1 (zh) 一种导航的方法和终端
CN113483753B (zh) 一种基于环境约束的惯性航向误差消除方法
Bravo et al. Comparison of step length and heading estimation methods for indoor environments

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
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province

Applicant after: Hunan Wagner micro Mdt InfoTech Ltd

Address before: 410205 Room 1610, 16th Floor, Headquarters Building, Zhongdian Software Park, 39 Jianshan Road, Changsha High-tech Development Zone, Hunan Province

Applicant before: Hunan Wagner micro Mdt InfoTech Ltd

GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address
CP03 Change of name, title or address

Address after: 410205 1705, building 1b, Hunan military civilian integration science and Technology Innovation Industrial Park, No. 699, Qingshan Road, Changsha high tech Development Zone, Changsha, Hunan

Patentee after: Hunan yunjiangna micro Information Technology Co.,Ltd.

Address before: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province

Patentee before: HUNAN GLONAVIN INFORMATION TECHNOLOGY CO.,LTD.