CN113483753B - 一种基于环境约束的惯性航向误差消除方法 - Google Patents

一种基于环境约束的惯性航向误差消除方法 Download PDF

Info

Publication number
CN113483753B
CN113483753B CN202110762717.XA CN202110762717A CN113483753B CN 113483753 B CN113483753 B CN 113483753B CN 202110762717 A CN202110762717 A CN 202110762717A CN 113483753 B CN113483753 B CN 113483753B
Authority
CN
China
Prior art keywords
error
straight line
dimensional
inertial
representing
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
CN202110762717.XA
Other languages
English (en)
Other versions
CN113483753A (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.)
Beihang University
Original Assignee
Beihang University
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 Beihang University filed Critical Beihang University
Priority to CN202110762717.XA priority Critical patent/CN113483753B/zh
Publication of CN113483753A publication Critical patent/CN113483753A/zh
Application granted granted Critical
Publication of CN113483753B publication Critical patent/CN113483753B/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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

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

Abstract

本发明公开了一种基于环境约束的惯性航向误差消除方法，利用了室内行走轨迹受建筑物楼向约束的特点来消除航向误差，有效提高了行人惯性定位系统的精度和稳定性，对于火场等复杂环境下人员定位具有十分重要的意义；不需要从航空影像图、建筑图纸等中来获取方向信息、算法复杂度低，易于工程实现。

Description

一种基于环境约束的惯性航向误差消除方法
技术领域
本发明涉及行人自主定位领域，特别涉及一种基于环境约束的惯性航向误差消除方法。
背景技术
随着经济的不断发展，城市建筑的规模持续扩大，并且结构日益复杂。当楼宇内发生火灾时，消防搜救工作面临严峻挑战，消防员在应急救援过程中屡有伤亡。为保障消防员救火时的生命安全，推进“智慧消防”中的个人装备建设，以及提高消防救援的信息化水平，实时定位跟踪消防战士的运动轨迹具有十分重要的意义。
由于室内火灾现场遮挡严重，无法基于卫星定位来实现人员位置的准确跟踪。依赖基础设施的室内定位技术，如UWB，WiFi，RFID，Zigbee等，需要预先部署网络设备或采集指纹库，相对电磁干扰强，可见度低，甚至断电的火场环境，这类技术的运用具有诸多限制。
随着微机电系统(Micro-Electro-Mechanical System,MEMS)技术的发展，基于惯性测量单元(Inertial Measurement Unit,IMU)的定位方法具有自主性、低成本、低功耗等特点，受到广泛关注。惯性定位是一种相对定位方法，短时精度高，但是，长时间运行时累积误差严重。一直以来，零速更新(Zero Velocity Update,ZUPT)是行人惯性定位系统中应用最广泛的误差消除方法，可将随时间三次方变化的误差转化为线性时间误差。然而，航向误差对ZUPT具有弱可观性，由陀螺偏置不稳定引起的航向误差，仍是影响行人惯性定位系统水平精度的主要原因之一。研究人员基于边缘检测技术，从航空影像图、建筑图纸等来获取建筑物的主方向或者朝向(最小地图信息)，通过建筑物的朝向作为约束来修正航向误差，这类方法需预先获取相关建筑物的地图信息，工作量大，并且消耗时间，不能很好地满足消防应急救援的要求。长期以来，消防室内定位具有迫切需求，但是其定位精度和稳定性还难以满足实际需求。
发明内容
为解决现有技术中的上述问题，本发明提出一种基于环境约束的惯性航向误差消除方法，有效提高了行人惯性定位系统的精度和稳定性，对于火场等复杂环境下人员定位具有十分重要的意义。
本发明提出的一种基于环境约束的惯性航向误差消除方法，主要包括以下步骤：
1、从安装于足部的MEMS-IMU传感器中采集三维加速度和三维角速度数据，并进行捷联解算，如式(1)所示
Figure GDA0003398634680000021
其中，上标n和b分别表示导航坐标系和载体坐标系；Vn和
Figure GDA0003398634680000022
分别表示导航坐标系下的三维速度及其微分；
Figure GDA0003398634680000023
表示导航坐标系下三维位置的微分；
Figure GDA0003398634680000024
和
Figure GDA0003398634680000025
分别表示由b系到n系转换的姿态矩阵及其微分；Ωb表示由陀螺仪输出角速度构成的反对称矩阵；fb表示载体坐标系下的比力；gn表示地球重力场矢量；
2、构造式(2)所示的统计值，进行零速更新检测(ZUPT)，即对行走过程中传感器固定脚触地进行周期性检测，
Figure GDA0003398634680000026
其中，a表示加速度；ω表示角速度；W表示窗长；k表示采样序号，取值为从W开始的整数；l依次取k-W+1到k的整数；g表示重力系数，为9.8；
Figure GDA0003398634680000027
表示加速度的方差；
Figure GDA0003398634680000028
表示角速度的方差；||·||表示L-2范数；
Figure GDA0003398634680000029
表示窗长W内的平均加速度，
γ为ZUPT静止态的判定阈值，当T(k)小于γ时，表示当前状态为ZUPT的静止态；反之，则为运动态，检测为静止态时，实际行走速度为零，而基于步骤一解算出来的三维速度不为零时，即可计算静止态的速度误差；
3、基于步骤二所计算的速度误差作为观测量，采用扩展卡尔曼滤波(EKF)对步骤一中所求的定位结果三维速度、三维位置、姿态进行修正；
定义EKF的15维误差状态向量如下：
Figure GDA00033986346800000210
其中，δPn、δVn、δφn、εb和
Figure GDA00033986346800000211
分别代表为三维位置误差向量、三维速度误差向量、三维姿态误差向量、三维陀螺漂移和三维加速度偏置，
EKF状态模型和观测模型分别如下所示：
δxk+1＝Fkδxk+wk (4)
δzk＝Hkδxk+vk (5)
其中，δxk是k时刻的系统状态；δzk是k时刻的误差观测量；Fk是转移矩阵，Hk量测矩阵；wk为过程噪声；vk为量测噪声；
4、根据步骤一至三所估计的水平位置，对当前及其之前连续m步的轨迹坐标进行直线拟合，所拟合的直线满足以下要求：
Figure GDA0003398634680000031
其中，x(i)和y(i)为捷联解算的定位结果，分别表示m步中第i步的横坐标和纵坐标；y_fit(i)表示x(i)与拟合直线对应的拟合值；α表示m步轨迹与拟合直线间的均值阈值；β表示m步的轨迹与拟合直线间的最大值阈值；
5、在步骤四中拟合的直线具有如下所示：
y＝r(1)x+r(2) (7)
其中，r(1)和r(2)分别表示直线斜率和纵轴上的截距；
6、在步骤五中，由于惯性定位短时精度高，累积误差小，当首次检测到直线时，认为是真实无偏差的，并以直线的倾角Ψ为基准，再以方向差为pi/2为间隔，构建四基准方向{Ψ,Ψ-pi,Ψ-pi/2,Ψ+pi/2}，针对部分非常规建筑中八个基准方向的情况，在四基准方向的基础上等间隔再增加四个基准方向，pi的取值为3.14。
7、当步骤五中非首次检测到直线时，根据拟合直线的倾角计算行走方向yaw_fitline：
yaw_fitline＝atan2(y_fit(m)-y_fit(1),x(m)-x(1)) (8)
其中，atan2函数的取值范围是[-pi,pi]；(x(1),y_fit(1))和(x(m),y_fit(m))分别为拟合直线时的第一个和最后一个点的坐标；
8、将步骤七中求得的行走方向与步骤六中的四个或八个楼向分别进行比较，将绝对最小的差值记作delta_yaw，
9、当步骤8中计算的delta_yaw小于阈值γ时，delta_yaw被视为EKF的航向误差，并构造EKF的量测矩阵H：
Figure GDA0003398634680000032
其中，03×3和I3×3分别表示3行3列的零矩阵和单位矩阵；01×3表示取值均为零的三维行向量；[001]表示行向量。
10、将EKF估计出的误差对惯性定位结果进行修正。
11、根据MEMS-IMU是否继续输出数据来判断定位是否结束，如果是，结束运行；如果不是，回转执行步骤2。
现有技术相比，本发明所述的一种基于环境约束的惯性航向误差消除方法，具有如下优点：
本发明所提基于环境约束的惯性航向误差消除方法，不需要从航空影像图、建筑图纸等中来获取方向信息。
本发明所提的方法利用了室内行走轨迹受建筑物楼向约束的特点来消除航向误差。
本发明所提算法复杂度低，易于工程实现。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请系统总体框图。
图2为基于环境约束的惯性航向误差消除方法流程图。
图3为识别示例。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
本发明旨在利用行走时，将学习到的建筑物的楼向信息用于抑制惯性定位系统的航向误差，以提高水平定位精度。下面通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。
本发明中的MEMS-IMU嵌入鞋跟位置，基于IEZ(Inertial Navigation System-Extended Kalman Filter-Zero Velocity Update,INS-EKF-ZUPT)，对传感器输出到的加速度和角速度信息进行惯性解算。在此基础上，结合行人步行的周期性特点，利用当前及过去连续多个步周期静止态下的坐标，检测直线行走轨迹。当首次检测到直线轨迹时，基于该直线的倾角构造建筑物的楼向。建筑物楼向此后将被用于估计具有直线特征轨迹的航向误差，并进行EKF滤波修正，从而消除航向漂移，提高水平定位精度。系统总体框图如图1所示。
一种基于环境约束的惯性航向误差消除方法流程图如图2所示，可通过以下具体步骤实现：
1、从安装于足部的MEMS-IMU传感器中采集三维加速度和三维角速度数据，并进行捷联解算，如式(1)所示
Figure GDA0003398634680000051
其中，上标n和b分别表示导航坐标系和载体坐标系；Vn和
Figure GDA0003398634680000052
分别表示导航坐标系下的三维速度及其微分；
Figure GDA0003398634680000053
表示导航坐标系下三维位置的微分；
Figure GDA0003398634680000054
和
Figure GDA0003398634680000055
分别表示由b系到n系转换的姿态矩阵及其微分；Ωb表示由陀螺仪输出角速度构成的反对称矩阵；fb表示载体坐标系下的比力；gn表示地球重力场矢量；
2、构造式(2)所示的统计值，进行零速更新检测(ZUPT)，即对行走过程中传感器固定脚触地进行周期性检测，
Figure GDA0003398634680000056
其中，a表示加速度；ω表示角速度；W表示窗长；k表示采样序号，取值为从W开始的整数；l依次取k-W+1到k的整数；g表示重力系数，为9.8；
Figure GDA0003398634680000057
表示加速度的方差；
Figure GDA0003398634680000058
表示角速度的方差；||·||表示L-2范数；
Figure GDA0003398634680000059
表示窗长W内的平均加速度，
γ为ZUPT静止态的判定阈值，当T(k)小于γ时，表示当前状态为ZUPT的静止态；反之，则为运动态，检测为静止态时，实际行走速度为零，而基于步骤一解算出来的三维速度不为零时，即可计算静止态的速度误差；
3、基于步骤二所计算的速度误差作为观测量，采用扩展卡尔曼滤波(EKF)对步骤一中所求的定位结果三维速度、三维位置、姿态进行修正；
定义EKF的15维误差状态向量如下：
Figure GDA00033986346800000510
其中，δPn、δVn、δφn、εb和
Figure GDA00033986346800000511
分别代表为三维位置误差向量、三维速度误差向量、三维姿态误差向量、三维陀螺漂移和三维加速度偏置，
EKF状态模型和观测模型分别如下所示：
δxk+1＝Fkδxk+wk (4)
δzk＝Hkδxk+vk (5)
其中，δxk是k时刻的系统状态；δzk是k时刻的误差观测量；Fk是转移矩阵，Hk量测矩阵；wk为过程噪声；vk为量测噪声；
4、根据步骤一至三所估计的水平位置，对当前及其之前连续m步的轨迹坐标进行直线拟合，所拟合的直线满足以下要求：
Figure GDA0003398634680000061
其中，x(i)和y(i)为捷联解算的定位结果，分别表示m步中第i步的横坐标和纵坐标；y_fit(i)表示x(i)与拟合直线对应的拟合值；α表示m步轨迹与拟合直线间的均值阈值；β表示m步的轨迹与拟合直线间的最大值阈值；
5、在步骤四中拟合的直线具有如下所示：
y＝r(1)x+r(2) (7)
其中，r(1)和r(2)分别表示直线斜率和纵轴上的截距；
6、在步骤五中，由于惯性定位短时精度高，累积误差小，当首次检测到直线时，认为是真实无偏差的，并以直线的倾角Ψ为基准，再以方向差为pi/2为间隔，构建四基准方向{Ψ,Ψ-pi,Ψ-pi/2,Ψ+pi/2}，针对部分非常规建筑中八个基准方向的情况，在四基准方向的基础上等间隔再增加四个基准方向，pi的取值为3.14。
7、当步骤五中非首次检测到直线时，根据拟合直线的倾角计算行走方向yaw_fitline：
yaw_fitline＝atan2(y_fit(m)-y_fit(1),x(m)-x(1)) (8)
其中，atan2函数的取值范围是[-pi,pi]；(x(1),y_fit(1))和(x(m),y_fit(m))分别为拟合直线时的第一个和最后一个点的坐标；
8、将步骤七中求得的行走方向与步骤六中的四个或八个楼向分别进行比较，将绝对最小的差值记作delta_yaw，
9、当步骤8中计算的delta_yaw小于阈值γ时，delta_yaw被视为EKF的航向误差，并构造EKF的量测矩阵H：
Figure GDA0003398634680000062
其中，03×3和I3×3分别表示3行3列的零矩阵和单位矩阵；01×3表示取值均为零的三维行向量；[001]表示行向量。
10、将EKF估计出的误差对惯性定位结果进行修正。
11、根据MEMS-IMU是否继续输出数据来判断定位是否结束，如果是，结束运行；如果不是，回转执行步骤2。
一种基于环境约束的惯性航向误差消除方法平面定位示例如图3所示。实验场景为：测试者绕室内长方形走廊行走一圈后回到起点，传感器采样频率为200Hz，总的行走路程为155.53米。如图3所示，实线是基于传统IEZ方法得到定位结果，由于传统IEZ框架下，航向漂移对零速检测具有弱观测，导致航向误差发散。IEZ方法得到的轨迹中，起点与终点的绝对误差为3.73米，平面运动轨迹闭合误差(起点与终点的绝对误差/总的行走距离)为2.40％；虚线代表采用本发明所提供的方法估计的定位结果，起点与终点的绝对误差为1.04米，平面运动轨迹闭合误差(起点与终点的绝对误差/总的行走距离)为0.67％。该实验表明：本发明方法在无外部地图与网络布设的前提下，有效地消除了航向误差，显著提高了平面定位精度。
本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于环境约束的惯性航向误差消除方法，其特征在于，包括如下方法步骤：
步骤一、从安装于足部的MEMS-IMU传感器中采集三维加速度和三维角速度数据，并进行捷联解算，如式(1)所示
Figure FDA0003845798570000011
其中，上标n和b分别表示导航坐标系和载体坐标系；Vn和
Figure FDA0003845798570000012
分别表示导航坐标系下的三维速度及其微分；
Figure FDA0003845798570000013
表示导航坐标系下三维位置的微分；
Figure FDA0003845798570000014
和
Figure FDA0003845798570000015
分别表示由b系到n系转换的姿态矩阵及其微分；Ωb表示由陀螺仪输出角速度构成的反对称矩阵；fb表示载体坐标系下的比力；gn表示地球重力场矢量；
步骤二、构造式(2)所示的统计值，进行零速更新检测(ZUPT)，即对行走过程中传感器固定脚触地进行周期性检测，
Figure FDA0003845798570000016
其中，a表示加速度；ω表示角速度；W表示窗长；k表示采样序号，取值为从W开始的整数；l依次取k+W―1到k的整数；g表示重力系数，为9.8；
Figure FDA0003845798570000017
表示加速度的方差；
Figure FDA0003845798570000018
表示角速度的方差；||·||表示L-2范数；
Figure FDA0003845798570000019
表示窗长W内的平均加速度，
γ为ZUPT静止态的判定阈值，当T(k)小于γ时，表示当前状态为ZUPT的静止态；反之，则为运动态，检测为静止态时，实际行走速度为零，而基于步骤一解算出来的三维速度不为零时，即可计算静止态的速度误差；
步骤三、基于步骤二所计算的速度误差作为观测量，采用扩展卡尔曼滤波(EKF)对步骤一中所求的定位结果三维速度、三维位置、姿态进行修正；
定义EKF的15维误差状态向量如下：
Figure FDA00038457985700000110
其中，δPn、δVn、δφn、εb和
Figure FDA00038457985700000111
分别代表为三维位置误差向量、三维速度误差向量、三维姿态误差向量、三维陀螺漂移和三维加速度偏置，
EKF状态模型和观测模型分别如下所示：
δxk+1＝Fkδxk+wk (4)
δzk＝Hkδxk+vk (5)
其中，δxk是k时刻的系统状态；δzk是k时刻的误差观测量；Fk是转移矩阵，Hk量测矩阵；wk为过程噪声；vk为量测噪声；
步骤四、根据步骤一至三所估计的水平位置，对当前及其之前连续m步的轨迹坐标进行直线拟合，所拟合的直线满足以下要求：
Figure FDA0003845798570000021
其中，x(i)和y(i)为捷联解算的定位结果，分别表示m步中第i步的横坐标和纵坐标；y_fit(i)表示x(i)与拟合直线对应的拟合值；α表示m步轨迹与拟合直线间的均值阈值；β表示m步的轨迹与拟合直线间的最大值阈值；
基于IEZ，对传感器输出到的加速度和角速度信息进行惯性解算，在此基础上，结合行人步行的周期性特点，利用当前及过去连续多个步周期静止态下的坐标，检测直线行走轨迹，当首次检测到直线轨迹时，基于该直线的倾角构造建筑物的楼向，建筑物楼向此后将被用于估计具有直线特征轨迹的航向误差，并进行EKF滤波修正，从而消除航向漂移。
2.根据权利要求1所述的基于环境约束的惯性航向误差消除方法，还包括步骤五：在步骤四中拟合的直线具有如下所示：
y＝r(1)x+r(2) (7)
其中，r(1)和r(2)分别表示直线斜率和纵轴上的截距。
3.根据权利要求2所述的基于环境约束的惯性航向误差消除方法，还包括步骤六，在步骤五中，由于惯性定位短时精度高，累积误差小，当首次检测到直线时，认为是真实无偏差的，并以直线的倾角Ψ为基准，再以方向差为pi/2为间隔，构建四个基准方向{Ψ,Ψ―pi,Ψ―pi/2,Ψ+pi/2}，针对部分非常规建筑中八个基准方向的情况，在四个基准方向的基础上等间隔再增加四个基准方向，pi的取值为3.14。
4.根据权利要求3所述的基于环境约束的惯性航向误差消除方法，还包括步骤七，当步骤五中非首次检测到直线时，根据拟合直线的倾角计算行走方向yaw_fitline：
yaw_fitline＝atan2 (y_fit(m)―y_fit(1),x(m)―x(1)) (8)
其中，atan2函数的取值范围是[―pi,pi]；(x(1),y_fit(1))和(x(m),y_fit(m))分别为拟合直线时的第一个和最后一个点的坐标。
5.根据权利要求4所述的基于环境约束的惯性航向误差消除方法，还包括步骤八，将步骤七中求得的行走方向与步骤六中的四个或八个基准方向分别进行比较，将绝对最小的差值记作delta_yaw。
6.根据权利要求5所述的基于环境约束的惯性航向误差消除方法，当步骤八中计算的delta_yaw小于阈值γ时，delta_yaw被视为EKF的航向误差，并构造EKF的量测矩阵H：
Figure FDA0003845798570000031
其中，03×3和I3×3分别表示3行3列的零矩阵和单位矩阵；01×3表示取值均为零的三维行向量；[001]表示行向量。
7.根据权利要求6所述的基于环境约束的惯性航向误差消除方法，将EKF估计出的误差对惯性定位结果进行修正。
8.根据权利要求7所述的基于环境约束的惯性航向误差消除方法，根据MEMS-IMU传感器是否继续输出数据来判断定位是否结束，如果是，结束运行；如果不是，回转执行步骤二。
9.一种可读存储介质，其特征在于，包括程序或指令，当所述程序或指令在计算机上运行时，如权利要求1至8中任一项所述的方法被执行。
CN202110762717.XA 2021-06-30 2021-06-30 一种基于环境约束的惯性航向误差消除方法 Active CN113483753B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110762717.XA CN113483753B (zh) 2021-06-30 2021-06-30 一种基于环境约束的惯性航向误差消除方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110762717.XA CN113483753B (zh) 2021-06-30 2021-06-30 一种基于环境约束的惯性航向误差消除方法

Publications (2)

Publication Number Publication Date
CN113483753A CN113483753A (zh) 2021-10-08
CN113483753B true CN113483753B (zh) 2022-11-01

Family

ID=77940558

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110762717.XA Active CN113483753B (zh) 2021-06-30 2021-06-30 一种基于环境约束的惯性航向误差消除方法

Country Status (1)

Country Link
CN (1) CN113483753B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115690205B (zh) * 2022-10-09 2023-12-05 北京自动化控制设备研究所 基于点线综合特征的视觉相对位姿测量误差估计方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103616030A (zh) * 2013-11-15 2014-03-05 哈尔滨工程大学 基于捷联惯导解算和零速校正的自主导航系统定位方法
CN106996780A (zh) * 2017-04-24 2017-08-01 湖南格纳微信息科技有限公司 一种航向误差修正方法与装置及磁场检测方法与装置
CN108387233A (zh) * 2018-01-30 2018-08-10 北京理工大学 一种基于模糊逻辑的行人运动模式判断方法
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
CN109520494A (zh) * 2017-09-19 2019-03-26 北京自动化控制设备研究所 一种基于室内步行微惯性自主导航方法
CN109612464A (zh) * 2018-11-23 2019-04-12 南京航空航天大学 基于iez框架下的多算法增强的室内导航系统及方法
CN110631589A (zh) * 2019-09-29 2019-12-31 广东星舆科技有限公司 一种实时修正定位轨迹的方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10210285B2 (en) * 2016-08-10 2019-02-19 Indoor Reality Inc. Methods for generating a floor plan from a building scan

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103616030A (zh) * 2013-11-15 2014-03-05 哈尔滨工程大学 基于捷联惯导解算和零速校正的自主导航系统定位方法
CN106996780A (zh) * 2017-04-24 2017-08-01 湖南格纳微信息科技有限公司 一种航向误差修正方法与装置及磁场检测方法与装置
CN109520494A (zh) * 2017-09-19 2019-03-26 北京自动化控制设备研究所 一种基于室内步行微惯性自主导航方法
CN108387233A (zh) * 2018-01-30 2018-08-10 北京理工大学 一种基于模糊逻辑的行人运动模式判断方法
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
CN109612464A (zh) * 2018-11-23 2019-04-12 南京航空航天大学 基于iez框架下的多算法增强的室内导航系统及方法
CN110631589A (zh) * 2019-09-29 2019-12-31 广东星舆科技有限公司 一种实时修正定位轨迹的方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Performance Enhancement of Pedestrian Navigation Systems Based on Low-Cost Foot-Mounted MEMS-IMU/Ultrasonic Sensor";Ming Xia et al;《sensors》;20190117;第19卷(第2期);第364-1-19页 *
"基于MIMU的单兵室内导航算法研究";马晓龙;《中国优秀硕士学位论文全文数据库信息科技辑》;20160415(第4期);I136-767-第51-57页 *

Also Published As

Publication number Publication date
CN113483753A (zh) 2021-10-08

Similar Documents

Publication Publication Date Title
Tong et al. A double-step unscented Kalman filter and HMM-based zero-velocity update for pedestrian dead reckoning using MEMS sensors
JP6783751B2 (ja) 地図情報支援の質を高められたポータブルナビゲーションを使用する方法および装置
CN113074739B (zh) 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法
US10267646B2 (en) Method and system for varying step length estimation using nonlinear system identification
Romanovas et al. A study on indoor pedestrian localization algorithms with foot-mounted sensors
US20130110397A1 (en) Method and System for Detection of a Zero Velocity State of an Object
CN111683337B (zh) 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质
Khan et al. Investigating effective methods for integration of building's map with low cost inertial sensors and wifi-based positioning
Zhang et al. The improved constraint methods for foot-mounted PDR system
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN113483753B (zh) 一种基于环境约束的惯性航向误差消除方法
CN112362057A (zh) 基于零速修正与姿态自观测的惯性行人导航算法
Tao et al. Precise displacement estimation from time-differenced carrier phase to improve PDR performance
CN111649742A (zh) 一种基于anfis辅助的高程估计方法
Ashkar et al. A low-cost shoe-mounted inertial navigation system with magnetic disturbance compensation
Köping et al. Probabilistic step and turn detection in indoor localization
CN116448103A (zh) 基于uwb测距辅助的行人足绑式惯性导航系统误差修正方法
Liu et al. Pedestrian indoor navigation using foot-mounted IMU with multi-sensor data fusion
CN112611385A (zh) 一种惯性导航与地图特征匹配的室内人员定位方法
CN112902962A (zh) 一种稳健的室内定位航向角度估计方法
CN113483763B (zh) 一种具有自主性的室内人员高程估计方法
Lin et al. A Cnn-Speed-Based Gnss/Pdr Integrated System For Smartwatch
Hsu et al. A particle filter approach for pedestrian dead reckoning using wearable sensors
CN114608576B (zh) 室内定位方法及装置
Wang et al. 3D reconstruction of pedestrian trajectory with moving direction learning and optimal gait recognition

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