CN113267185B - 抗磁干扰的定位方法及装置、系统、电子设备、存储介质 - Google Patents

抗磁干扰的定位方法及装置、系统、电子设备、存储介质 Download PDF

Info

Publication number
CN113267185B
CN113267185B CN202110451962.9A CN202110451962A CN113267185B CN 113267185 B CN113267185 B CN 113267185B CN 202110451962 A CN202110451962 A CN 202110451962A CN 113267185 B CN113267185 B CN 113267185B
Authority
CN
China
Prior art keywords
magnetic field
acceleration
interference
angular velocity
moving object
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
CN202110451962.9A
Other languages
English (en)
Other versions
CN113267185A (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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202110451962.9A priority Critical patent/CN113267185B/zh
Publication of CN113267185A publication Critical patent/CN113267185A/zh
Application granted granted Critical
Publication of CN113267185B publication Critical patent/CN113267185B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; 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/16—Navigation; 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
    • G01C21/165—Navigation; 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 combined with non-inertial navigation instruments
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本申请公开了一种抗磁干扰的定位方法及装置、系统、电子设备、存储介质，该方法包括：接收运动物体的加速度、角速度和磁场；对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。该方法提高行人航位推算系统的定位精度和抗磁干扰能力。

Description

抗磁干扰的定位方法及装置、系统、电子设备、存储介质
技术领域
本申请涉及室内定位导航领域，具体涉及一种基于惯性传感器的行人航位推算系统磁干扰检测和补偿方法及装置、系统、电子设备、存储介质。
背景技术
全球卫星导航系统(GNSS)是行人导航技术中常用的传统方法。然而，在城市、峡谷和室内环境中，卫星信号受到严重的影响，基于GNSS的解决方案在无法在这些情况下进行定位。随着传感器和无线技术的发展，其他定位技术，如红外、超声波、蓝牙、无线局域网等广泛应用于室内定位。但是上述方法的缺点是，需要提前在环境中部署信标或基站以创建和维护网络。此外，新环境的几何特性和信号干扰会导致鲁棒性较差。
随着传感器精度的提高和不断小型化，在卫星定位受限的情况下，基于行人航迹推算(PDR)系统的定位技术应用越来越广泛。基于微机电系统(MEMS)磁力计、加速计和陀螺仪的PDR系统具有携带方便的特点，可以在没有外部信号的情况下为行人提供自主定位服务，因此它已被广泛应用于医疗监测、军事防御、智能环境等领域。MEMS传感器具有成本低、体积小、重量轻、功耗低等优点，常用于PDR系统的行人导航系统。由于PDR系统有加速度和角速度的积分过程，最终位置误差将随着时间继续增加。为了减少积分漂移造成的误差，一种解决方案是将MEMS传感器安装在脚上，采用零速校正(ZUPT)算法和拓展卡尔曼滤波(EKF)算法。当一个人走路时，会有一个站立期，此时一只脚的速度实际上等于0。ZUPT算法主要根据加速度和角速度是否达到阈值来判断姿态是否进入零速度区间。因此ZUPT算法可以消除累积的速度误差，同时根据磁力计测量的地磁场和加速计测量的重力加速度，可以对传感器姿态进行校准。但实际环境中磁场受周围电气设备和铁镍材料的影响，环境中的磁场干扰会导致传感器的姿态估计，尤其是航向角校准产生较大误差，从而导致严重的位置误差。
因此在实现本发明的过程中，发明人发现现有技术中至少存在如下问题：
(1)基于EKF-ZUPT的PDR系统进行室内定位时，磁力计对磁干扰非常敏感，由此产生的误差将影响运动物体姿态和比力加速度的计算，从而导致较大的定位误差；
(2)PDR系统进行室内定位时，环境中的磁干扰系统无法识别；
(3)定位过程中，环境中的磁干扰无法实时校准补偿。
发明内容
本申请实施例的目的是提供一种方法及装置、电子设备，以解决相关技术中存在的基于EKF-ZUPT的PDR系统室内定位过程中，受环境中的磁场干扰导致系统定位位置、姿态误差较大的技术问题。
根据本申请实施例的第一方面，提供一种抗磁干扰的定位方法，该方法包括：
接收运动物体的加速度、角速度和磁场；对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。
根据本申请实施例的第二方面，提供一种抗磁干扰的定位装置，该装置包括：
接收模块，用于接收运动物体的加速度、角速度和磁场；处理模块，用于对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。
根据本申请实施例的第三方面，提供一种抗磁干扰的定位系统，该系统包括：
传感器节点，用于实时采样运动物体的加速度、角速度和磁场；处理器节点，与所述传感器节点相连，用于接收加速度、角速度和磁场，对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则基于递推最小二乘法(RLS)拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。
根据本申请实施例的第四方面，提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。
根据本申请实施例的第五方面，提供一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如第一方面所述方法的步骤。
本申请的实施例提供的技术方案可以包括以下有益效果：
本申请采用了磁场干扰阈值检测的方法，克服了复杂环境中磁干扰无法识别的技术问题，进而达到了实时识别磁场中的干扰的效果。
若存在干扰，则拟合磁干扰函数曲线，解决了环境磁干扰无法量化拟合的技术问题，进而能递推得到后续受干扰磁场的干扰函数，并进行拟合得到校准后的磁场；相较于传统的抗磁干扰方法都无法量化拟合磁干扰函数，本发明不仅能拟合磁干扰函数，并能递推得到后续磁场干扰量，从而补偿干扰得到校准后的磁场，进而帮助系统姿态校准。
本申请采用的磁干扰拟合方法可以实时针对干扰进行拟合补偿，具有低时延性，满足采样频率100Hz及以内的系统实时定位的要求。
本申请的方法可以与北斗定位模块或GPS模块联合定位，系统具有良好的拓展性。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。
图1是根据一示例性实施例示出的一种抗磁干扰的定位方法的流程图。
图2为根据一示例性实施例示出，基于递推最小二乘法(RLS)对磁干扰拟合的仿真效果图。
图3为根据所述的抗磁干扰的定位方法仿真的运动轨迹图。
图4为根据所述的抗磁干扰的定位方法开展实地实验的运动轨迹图。
图5是根据一示例性实施例示出的一种抗磁干扰的定位系统的框图。
图6是根据一示例性实施例示出的一种抗磁干扰的定位装置的流程图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。
图1是根据一示例性实施例示出的一种抗磁干扰的定位方法的流程图，如图1所示，该方法应用于终端或处理器中，可以包括以下步骤：
步骤S11，接收运动物体的加速度、角速度和磁场；
步骤S12，对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。
由上述实施例可知，本申请通过磁干扰阈值检测和基于递推最小二乘法(RLS)拟合磁干扰函数并动态补偿后续受干扰磁场的方法，有效提高了行人航位推算(PDR)系统的抗磁干扰能力和定位精度。
在步骤S11的具体实施中，接收运动物体的加速度、角速度和磁场；
具体地，通过将行人航位推算(PDR)系统的微机电系统(MEMS)磁力计、加速计和陀螺仪固定在行人的一只鞋上，设置采样频率100Hz实时采集脚部在运动过程中的磁场、加速度和角速度，并输入所述传感器连接的Raspberry Pi 4B处理节点；
在步骤S12的具体实施中，对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。可以包括以下子步骤：
具体地，首先通过静态初始对准实验，采样n0次磁场数据均值计算得初始标准磁场
Figure BDA0003039047900000061
其中
Figure BDA0003039047900000062
和
Figure BDA0003039047900000063
分别为第k个采样序列时刻下导航坐标系、载体坐标系下的磁场和载体坐标系转换导航坐标系的旋转矩阵；通过静态实验设定磁场测量阈值μ3、角速度阈值μ1和加速度阈值μ2；
步骤S121，对所述加速度和角速度进行阈值检测。对所述角速度范数ε1＝‖wb‖和所述加速度减去重力加速度的范数ε2＝|‖ab‖-g0|进行阈值检测；角速度阈值μ1和加速度阈值μ2通过传感器噪声和静态实验标定测量经验取值，若ε1<μ1且ε2<μ2则判别载体进入零速区间，并执行步骤S122进行磁干扰检测；若所述加速度和角速度不全在阈值范围内，则执行步骤S125进行航位推算；
步骤S122，对所述磁场进行阈值检测判定是否受到磁干扰；载体位于零速区间时，计算到达第k个序列，时间tk时载体坐标系采样转换到导航坐标系的磁场
Figure BDA0003039047900000064
将磁场
Figure BDA0003039047900000065
与初始标准磁场
Figure BDA0003039047900000066
的矢量差范数
Figure BDA0003039047900000067
Figure BDA0003039047900000068
与阈值μ3比较，若ε3＞μ3则判别受到磁干扰，执行步骤S123，若ε3≤μ3则执行步骤S124；
步骤S123，所述磁场受到干扰后，记录时间tk和受干扰磁场
Figure BDA0003039047900000069
到序列
Figure BDA00030390479000000610
Ω(k)＝(Ω1,Ω2,...,Ωk)，基于递推最小二乘法(RLS)拟合磁干扰函数曲线；通过递推最小二乘法(RLS)拟合磁干扰函数系数；之后递推补偿后续磁场，推算后续时刻tk+m的干扰量
Figure BDA00030390479000000611
以补偿磁场得到校准磁场
Figure BDA00030390479000000612
拟合得到校准磁场后执行步骤S124；
步骤S124，对所述磁场和加速度进行零速校正ZUPT，校准载体的姿态和速度，之后将校准的姿态和速度代入拓展卡尔曼滤波器(EKF)进行滤波；ZUPT过程中运动物体的速度vn校准为[0 0 0]T；将校准的磁场与所述的运动物体的加速度计算，校准载体姿态
Figure BDA0003039047900000071
其中包含航向角
Figure BDA0003039047900000072
俯仰角θ和横滚角γ；
将姿态作为观测量输入拓展卡尔曼滤波器(EKF)，减小磁干扰对定位结果的影响，之后得到所述的运动物体的位置、速度和姿态，执行步骤S126；
步骤S125，对所述加速度、角速度进行航位推算(PDR)，计算得到运动物体的位置、速度及姿态；
步骤S126，输出运动物体的位置p、速度v及姿态
Figure BDA0003039047900000073
循环执行步骤S121。
在本发明实施例中，对所述加速度和角速度进行阈值检测，包括：
设定加速度阈值范围和角速度阈值范围。通过静态初始对准实验，采样n0次磁场数据均值计算得初始标准磁场
Figure BDA0003039047900000074
其中
Figure BDA0003039047900000075
和
Figure BDA0003039047900000076
分别为第k个采样序列时刻下导航坐标系、载体坐标系下的磁场和载体坐标系转换导航坐标系的旋转矩阵；通过静态实验设定磁场测量阈值μ3、角速度阈值μ1和加速度阈值μ2；
将所述加速度与所述加速度阈值范围进行比较；
将所述角速度与所述角速度阈值范围进行比较；
采集传感器加速度、角速度和磁场数据，对所述角速度范数ε1＝‖wb‖和所述加速度减去重力加速度的范数ε2＝|‖ab‖-g0|进行阈值检测，其中wb,ab和g0分别是载体运动的角速度、加速度和地区重力加速度；角速度阈值μ1和加速度阈值μ2通过静态实验标定取值，若ε1<μ1且ε2<μ2则判别载体进入零速区间；
在本发明实施例中，所述磁场进行阈值检测，包括：
运动物体位于零速区间时，计算到达第k个序列，时间tk时载体坐标系采样转换到导航坐标系的磁场
Figure BDA0003039047900000077
将磁场
Figure BDA0003039047900000078
与初始标准磁场
Figure BDA0003039047900000079
的矢量差范数
Figure BDA00030390479000000710
与阈值μ3比较；
在本发明实施例中，所述拟合采用基于递推最小二乘法(RLS)，递推后续干扰量补偿校准磁场，包括：
所述磁场受到干扰后，记录时间tk和受干扰磁场
Figure BDA0003039047900000081
到序列
Figure BDA0003039047900000082
Ω(k)＝(Ω1,Ω2,...,Ωk)，基于递推最小二乘法(RLS)拟合磁干扰函数曲线；设定磁场受到线性一次干扰函数如公式(1)所示，
Figure BDA0003039047900000083
为偏置磁场，Jk为一次干扰函数系数：
Figure BDA0003039047900000084
通过递推最小二乘法(RLS)拟合磁干扰函数系数
Figure BDA0003039047900000085
如公式(2)-(3)所示：
Figure BDA0003039047900000086
Figure BDA0003039047900000087
之后递推补偿后续磁场，推算后续时刻tk+m的干扰量
Figure BDA0003039047900000088
以补偿磁场得到校准磁场
Figure BDA0003039047900000089
如公式(4)-(5)所示。
Figure BDA00030390479000000810
Figure BDA00030390479000000811
如图2所示，为抗磁干扰定位系统基于递推最小二乘法对磁干扰函数进行拟合的结果。为验证所提方法的有效性，设计了抗磁干扰的航位推算PDR仿真定位系统。载体从原点O出发运动回到原点，设置运动参数，载体的初始速度vb和角速度wb分别为2m/s和12°/s。采样时间为30秒，频率为100Hz。运动开始后，在磁场中加入线性的环境磁干扰。基于递推最小二乘法进行磁干扰函数拟合的结果图如图2所示。设定xy轴干扰参数Jk为[0.0222,-0.0111]。拟合的函数参数
Figure BDA00030390479000000812
为[0.0224，-0.0106]，xy轴磁干扰拟合误差分别为1.35％和2.70％。上述实施例证明本申请所提方法拟合磁干扰的有效性和准确性；
在本发明实施例中，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度，包括：
对所述磁场和加速度进行零速校正(ZUPT)，校准载体的姿态和速度，之后将校准的姿态和速度代入拓展卡尔曼滤波器(EKF)进行滤波；ZUPT过程中运动物体的速度vn校准为[0 0 0]T；将校准的磁场与所述的加速度计算，校准载体姿态
Figure BDA0003039047900000091
其中包含航向角
Figure BDA0003039047900000092
俯仰角θ和横滚角γ，如公式(6)-(11)所示：
Figure BDA0003039047900000093
Figure BDA0003039047900000094
Eb＝mb×ab   (8)
Figure BDA0003039047900000095
Figure BDA0003039047900000096
Figure BDA0003039047900000097
其中
Figure BDA0003039047900000098
mb和Eb分别是载体坐标系下的加速度、校准磁场和东方向矢量。将姿态作为观测量输入拓展卡尔曼滤波器(EKF)，减小磁干扰对定位结果的影响，状态方程如公式(12)所示；
Figure BDA0003039047900000099
EKF中
Figure BDA00030390479000000910
为状态矢量，分别表示位置误差、速度误差、姿态误差、加速度误差和陀螺误差。zk＝[δv,δψ]为观测矢量，表示速度误差和姿态误差。Fk和Hk是
Figure BDA00030390479000000911
和h(Xk)的雅克比矩阵。Wk和Vk是独立的随机噪声向量，分别具有协方差矩阵Qk和Rk。Fk，Hk和Sn的方程如公式(13)-(15)所示。基于EKF滤波过程如公式(16)-(20)所示：
Figure BDA0003039047900000101
Figure BDA0003039047900000102
Figure BDA0003039047900000103
Figure BDA0003039047900000104
Figure BDA0003039047900000105
Figure BDA0003039047900000106
Pk＝(I-Kk·Hk)·Pk|k-1   (19)
Figure BDA0003039047900000107
之后得到所述运动物体的位置、速度和姿态；
在本发明实施例中，对所述加速度和角速度进行航位推算，包括：
将所述加速度
Figure BDA0003039047900000108
转化到导航坐标系下
Figure BDA0003039047900000109
减去当地重力加速度
Figure BDA00030390479000001010
通过积分得到序列k时刻的运动物体的速度
Figure BDA00030390479000001011
之后将所述速度进行积分得到所述的运动物体的位置
Figure BDA00030390479000001012
如公式(21)-(24)所示。
Figure BDA00030390479000001013
Figure BDA00030390479000001014
Figure BDA00030390479000001015
Figure BDA00030390479000001016
如图3所示，为基于PDR的抗干扰定位系统的仿真定位结果图。真实轨迹的起点和终点是原点。标准PDR、磁干扰下的PDR和干扰动态补偿的PDR的终点分别为(1.38,0.68)、(5.48,0.44)和(0.25,0.59)。标准PDR、磁干扰下的PDR和磁干扰动态补偿的PDR的轨迹最终位置误差分别为1.54m、5.50m和0.64m，占运动距离误差分别为2.57％,9.16％和1.07％。采用磁干扰补偿方法将定位位置误差降低了8.09％，该仿真结果实施例表明该申请所提方法能够有效检测和抵抗环境磁干扰。
如图4所示，为基于PDR的抗干扰定位系统的实地实验定位结果图。将商用级惯性测量单元IMU MTi-300安装在鞋上进行实验，采样频率为100hz。实验选择了一个20.00m*12.45m的正方形作为实验场地。真实运动轨迹的起点和终点都是原点。实验总运动距离为82.4893m。基于标准PDR、磁干扰下的PDR和干扰动态补偿的PDR轨迹终点分别为(-0.18、-1.65、0.08)、(-8.23、-1.15、-0.14)和(-1.79、-1.55、-0.09)。位置误差分别为2.01％、10.07％和2.87％。通过该申请所提出的抗磁干扰定位方法进行动态补偿，使位置误差减小了7.20％，该实地实验结果实施例表明，该申请所提方法能够有效检测和抵抗环境磁干扰，降低定位误差。
图5是根据一示例性实施例示出的一种抗磁干扰的定位系统的框图。本发明实施例提供一种抗磁干扰的定位系统，该系统包括：
传感器节点，用于实时采样运动物体的加速度、角速度和磁场；
处理器节点，与所述传感器节点相连，用于接收加速度、角速度和磁场，对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则基于递推最小二乘法(RLS)拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。
关于上述实施例中的系统，其中各个节点执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
下面对以上系统进行详细说明。
所述的传感器节点可以包括串口通信模块和惯性传感器模块，所述的惯性传感器包括三轴加速度传感器、三轴陀螺仪传感器和三轴磁场传感器。
进一步的，所述的三轴加速度传感器、三轴陀螺仪传感器和三轴磁场传感器为荷兰Xsens公司生产的高精度惯性传感器MTi-300。
所述处理器节点为一处理器，本实例采用Raspberry Pi 4B节点，以上三个传感器通过串口通讯模块与Raspberry Pi 4B节点进行通讯。
该系统还可以包括无线通讯模块和服务器端，无线通讯模块负责数据的传递，用于将Raspberry Pi 4B节点处理后的数据传递给所述服务器端。
进一步的，所述的无线通讯模块可以包括4G模块和WiFi模块，4G模块选用USR-G401t，WiFi模块选择ESP8266，当然不限于此。
服务器端实时监听Raspberry Pi 4B端的位置p、速度v及姿态ψ信息，ESP8266WiFi模块设置AP接入点，服务器端接入该AP，ESP8266设置为记住该服务器地址，一旦服务器端无法接收到ESP8266模块的信息，就启动4G模块通信。
进一步的，所述的服务器端还可通过python的tkinter模块编写图形界面显示，实时显示施加磁干扰前后载体的定位轨迹及运动姿态。
与前述的一种抗磁干扰的定位方法的实施例相对应，本申请还提供了一种抗磁干扰的定位装置的实施例。
图6是根据一示例性实施例示出的一种抗磁干扰的定位装置框图。参照图6，该装置包括：
接收模块21，用于接收运动物体的加速度、角速度和磁场；
处理模块22，用于对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本申请方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
相应的，本申请还提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述的一种抗磁干扰的定位方法。
相应的，本申请还提供一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如上述的一种抗磁干扰的定位方法。
本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本申请的真正范围和精神由下面的权利要求指出。
应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。

Claims (9)

1.一种抗磁干扰的定位方法，其特征在于，该方法包括：
接收运动物体的加速度、角速度和磁场；
对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度；
其中，所述拟合采用基于递推最小二乘法，递推后续干扰量补偿校准磁场，包括：
所述磁场受到干扰后，记录时间tk和受干扰磁场
Figure FDA0004087538280000011
到序列
Figure FDA0004087538280000012
Ω(k)＝(Ω1,Ω2,...,Ωk)，基于递推最小二乘法拟合磁干扰函数曲线；设定磁场受到线性一次干扰函数如公式(1)所示，
Figure FDA0004087538280000013
为偏置磁场，Jk为一次干扰函数系数：
Figure FDA0004087538280000014
通过递推最小二乘法拟合磁干扰函数系数
Figure FDA0004087538280000015
如公式(2)-(3)所示：
Figure FDA0004087538280000016
Figure FDA0004087538280000017
之后递推补偿后续磁场，推算后续时刻tk+m的干扰量
Figure FDA0004087538280000018
以补偿磁场得到校准磁场
Figure FDA0004087538280000019
如公式(4)-(5)所示：
Figure FDA00040875382800000110
Figure FDA00040875382800000111
2.根据权利要求1所述的方法，其特征在于，对所述加速度和角速度进行阈值检测，包括：
设定加速度阈值范围和角速度阈值范围；
将所述加速度与所述加速度阈值范围进行比较；
将所述角速度与所述角速度阈值范围进行比较。
3.根据权利要求1所述的方法，其特征在于，递推后续干扰量补偿校准磁场，包括：
根据所述拟合得到的干扰函数曲线，递推后续磁场干扰量，补偿得到校准磁场。
4.根据权利要求1所述的方法，其特征在于，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度，包括：
将所述运动物体的速度校准为0m/s；
将所述加速度的重力加速度进行矢量计算，得到所述运动物体姿态的俯仰角和横滚角；
将所述磁场与所述加速度的重力加速度进行矢量计算，得到所述运动物体姿态的航向角；
所述运动物体的位置校准为所述加速度和角速度进行阈值检测前所述运动物体的位置。
5.根据权利要求1所述的方法，其特征在于，对所述加速度和角速度进行航位推算，包括：
将所述角速度通过四元数法进行姿态更新，得到所述运动物体的速度和姿态转换矩阵；
将所述加速度点乘姿态转换矩阵并积分，加上航位推算前的初始速度，得到所述运动物体的速度；
将所述运动物体的速度积分，加上航位推算前的初始位置得到所述运动物体的位置。
6.一种抗磁干扰的定位装置，其特征在于，该装置包括：
接收模块，用于接收运动物体的加速度、角速度和磁场；
处理模块，用于对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度；
其中，所述拟合采用基于递推最小二乘法，递推后续干扰量补偿校准磁场，包括：
所述磁场受到干扰后，记录时间tk和受干扰磁场
Figure FDA0004087538280000031
到序列
Figure FDA0004087538280000032
Figure FDA0004087538280000033
Ω(k)＝(Ω1,Ω2,...,Ωk)，基于递推最小二乘法拟合磁干扰函数曲线；设定磁场受到线性一次干扰函数如公式(1)所示，
Figure FDA0004087538280000034
为偏置磁场，Jk为一次干扰函数系数：
Figure FDA0004087538280000035
通过递推最小二乘法拟合磁干扰函数系数
Figure FDA0004087538280000036
如公式(2)-(3)所示：
Figure FDA0004087538280000037
Figure FDA0004087538280000038
之后递推补偿后续磁场，推算后续时刻tk+m的干扰量
Figure FDA0004087538280000039
以补偿磁场得到校准磁场
Figure FDA00040875382800000310
如公式(4)-(5)所示：
Figure FDA00040875382800000311
Figure FDA00040875382800000312
7.一种抗磁干扰的定位系统，其特征在于，该系统包括：
传感器节点，用于实时采样运动物体的加速度、角速度和磁场；
处理器节点，与所述传感器节点相连，用于接收加速度、角速度和磁场，对所述加速度和角速度进行阈值检测，当所述加速度和角速度均在阈值范围内时，再通过阈值检测判别磁场是否受到干扰，若存在干扰，则基于递推最小二乘法拟合磁干扰函数曲线，递推后续干扰量补偿校准磁场，若不存在干扰，则无需对磁场进行补偿，对判别后的磁场和加速度进行零速校正，得到运动物体的位置、姿态和速度；当所述加速度和角速度不全在阈值范围内时，对所述加速度和角速度进行航位推算，得到运动物体的位置、姿态和速度；
其中，所述拟合采用基于递推最小二乘法，递推后续干扰量补偿校准磁场，包括：
所述磁场受到干扰后，记录时间tk和受干扰磁场
Figure FDA0004087538280000041
到序列
Figure FDA0004087538280000042
Figure FDA0004087538280000043
Ω(k)＝(Ω1,Ω2,...,Ωk)，基于递推最小二乘法拟合磁干扰函数曲线；设定磁场受到线性一次干扰函数如公式(1)所示，
Figure FDA0004087538280000044
为偏置磁场，Jk为一次干扰函数系数：
Figure FDA0004087538280000045
通过递推最小二乘法拟合磁干扰函数系数
Figure FDA0004087538280000046
如公式(2)-(3)所示：
Figure FDA0004087538280000047
Figure FDA0004087538280000048
之后递推补偿后续磁场，推算后续时刻tk+m的干扰量
Figure FDA0004087538280000049
以补偿磁场得到校准磁场
Figure FDA00040875382800000410
如公式(4)-(5)所示：
Figure FDA00040875382800000411
Figure FDA00040875382800000412
8.一种电子设备，其特征在于，包括：
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-5任一项所述的方法。
9.一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如权利要求1-5中任一项所述方法的步骤。
CN202110451962.9A 2021-04-26 2021-04-26 抗磁干扰的定位方法及装置、系统、电子设备、存储介质 Active CN113267185B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110451962.9A CN113267185B (zh) 2021-04-26 2021-04-26 抗磁干扰的定位方法及装置、系统、电子设备、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110451962.9A CN113267185B (zh) 2021-04-26 2021-04-26 抗磁干扰的定位方法及装置、系统、电子设备、存储介质

Publications (2)

Publication Number Publication Date
CN113267185A CN113267185A (zh) 2021-08-17
CN113267185B true CN113267185B (zh) 2023-04-28

Family

ID=77229323

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110451962.9A Active CN113267185B (zh) 2021-04-26 2021-04-26 抗磁干扰的定位方法及装置、系统、电子设备、存储介质

Country Status (1)

Country Link
CN (1) CN113267185B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114279396A (zh) * 2021-12-22 2022-04-05 Oppo广东移动通信有限公司 运动姿态确定方法及装置、计算机可读介质和电子设备

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100612836B1 (ko) * 2003-12-13 2006-08-18 삼성전자주식회사 자장 이용 방법 및 장치
FR2976353B1 (fr) * 2011-06-07 2013-07-05 Movea Procede d'estimation simplifie de l'orientation d'un objet et centrale d'attitude mettant en oeuvre un tel procede
DE102012011632A1 (de) * 2011-07-28 2013-01-31 Memsic Inc. System und Verfahren zum Erfassen einer Raumstellung und Winkelrate unter Verwendung eines Magnetfeldsensors und Akzelerometers für tragbare elektronische Vorrichtungen
CN103630137B (zh) * 2013-12-02 2016-03-23 东南大学 一种用于导航系统的姿态及航向角的校正方法
US20160178657A9 (en) * 2013-12-23 2016-06-23 InvenSense, Incorporated Systems and methods for sensor calibration
CN106017459B (zh) * 2016-05-19 2019-03-19 极翼机器人(上海)有限公司 一种抗磁干扰的方法
CN107014388B (zh) * 2016-12-22 2020-08-07 威海北洋电气集团股份有限公司 一种基于磁干扰检测的步行者轨迹推算方法及装置
FR3070515B1 (fr) * 2017-08-25 2022-04-08 Institut Francais Des Sciences Et Technologies Des Transp De Lamenagement Et Des Reseaux Procede de determination de la trajectoire d'un objet mobile, programme et dispositif aptes a la mise en oeuvre de ce procede
CN109238262B (zh) * 2018-11-05 2020-10-30 珠海全志科技股份有限公司 一种航向姿态解算及罗盘校准抗干扰方法
EP3708953B1 (en) * 2019-03-15 2021-12-22 Dune S.r.l. Inertial positioning and tracking method and system with innovative functionality for magnetometer auto-calibration and for angular drift compensation/correction
CN112683269B (zh) * 2020-12-07 2022-05-03 电子科技大学 一种附有运动加速度补偿的marg姿态计算方法

Also Published As

Publication number Publication date
CN113267185A (zh) 2021-08-17

Similar Documents

Publication Publication Date Title
Bai et al. A high-precision and low-cost IMU-based indoor pedestrian positioning technique
Rohac et al. Calibration of low-cost triaxial inertial sensors
Jiménez et al. Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU
CN110487267B (zh) 一种基于vio＆uwb松组合的无人机导航系统及方法
US8010308B1 (en) Inertial measurement system with self correction
CN110553646B (zh) 基于惯性和磁航向及零速修正的行人导航方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN106908060A (zh) 一种基于mems惯性传感器的高精度室内定位方法
CN109163721A (zh) 姿态测量方法及终端设备
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
Sasani et al. Improving MEMS-IMU/GPS integrated systems for land vehicle navigation applications
CN102445200A (zh) 微小型个人组合导航系统及其导航定位方法
CN112562077B (zh) 一种融合pdr和先验地图的行人室内定位方法
CN110174123B (zh) 一种磁传感器实时标定方法
CN106153069B (zh) 自主导航系统中的姿态修正装置和方法
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN106996780A (zh) 一种航向误差修正方法与装置及磁场检测方法与装置
CN109579836A (zh) 一种基于mems惯性导航的室内行人方位校准方法
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
Wang et al. Improving the navigation performance of the MEMS IMU array by precise calibration
CN113267185B (zh) 抗磁干扰的定位方法及装置、系统、电子设备、存储介质
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
Tao et al. Precise displacement estimation from time-differenced carrier phase to improve PDR performance
Li et al. Unmanned aerial vehicle position estimation augmentation using optical flow sensor
CN117232507A (zh) 基于自适应步长估计的惯性导航定位误差修正方法和系统

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