CN111912406A - 一种基于改进式hde的室内行人导航航向修正方法及系统 - Google Patents

一种基于改进式hde的室内行人导航航向修正方法及系统 Download PDF

Info

Publication number
CN111912406A
CN111912406A CN202010819613.3A CN202010819613A CN111912406A CN 111912406 A CN111912406 A CN 111912406A CN 202010819613 A CN202010819613 A CN 202010819613A CN 111912406 A CN111912406 A CN 111912406A
Authority
CN
China
Prior art keywords
pedestrian
course
hde
angular rate
difference value
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.)
Granted
Application number
CN202010819613.3A
Other languages
English (en)
Other versions
CN111912406B (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.)
Academy Of Aerospace Science Technology And Communications Technology Co ltd
Original Assignee
Academy Of Aerospace Science Technology And Communications 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 Academy Of Aerospace Science Technology And Communications Technology Co ltd filed Critical Academy Of Aerospace Science Technology And Communications Technology Co ltd
Priority to CN202010819613.3A priority Critical patent/CN111912406B/zh
Publication of CN111912406A publication Critical patent/CN111912406A/zh
Application granted granted Critical
Publication of CN111912406B publication Critical patent/CN111912406B/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
    • 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

本发明公开了一种基于改进式HDE的室内行人导航航向修正方法及系统，至少包括下列步骤：S1,结合典型建筑结构定义主方向，进行运动类型分类；S2,扩展卡尔曼滤波。本发明采用MEMS惯性测量单元，在传统姿态解算和PDR算法基础上，结合改进的启发式偏移消除算法，增加航向信息的长时间稳定性，提高定位系统的精度。

Description

一种基于改进式HDE的室内行人导航航向修正方法及系统
技术领域
本发明涉及行人导航技术领域，并且更具体地，涉及一种基于改进式HDE的室内行人导航航向修正方法。
背景技术
近年来，基于位置服务而衍生的各类应用受到了广泛的关注，如何实现高精度室内行人导航成为科研机构的研究热点。MEMS惯性测量单元(IMU)由三轴加速度计、陀螺仪和磁力计组成。一种方法是惯性导航系统，惯性导航系统利用载体自身的角速率和加速度进行实时测量，基于行人航迹推算算法PDR，由行人的步长、步态、航向等信息解算位置坐标，在短期内能有效实现较高精度的定位效果。但随着定位时间增加，在PDR系统中，航向误差是引起定位误差的主要因素之一。另一种方法是通过磁力计解算航向信息，但由于地磁受周边环境影响较大，对航向信息产生较大误差。为解决惯性导航存在的航向解算缺陷，目前对航向修正方法大多引进了多源信息，如GPS、超宽带、超声波、RFID、Zigbee、蓝牙、红外线、WiFi等，需要提前布置定位基础设施，但存在前期准备工作量大、成本费用高、普适性差等缺点，不能普适用于室内行人导航领域。
发明内容
针对于现有技术中传统惯性导航解算的航向误差存在随时间累积的不足，一方面，本发明提供一种实现室内行人导航的航向修正方法，采用MEMS惯性测量单元，在传统姿态解算和PDR算法基础上，结合改进的启发式偏移消除算法，增加航向信息的长时间稳定性，提高定位系统的精度。
一种基于改进式HDE的室内行人导航航向修正方法，至少包括下列步骤：
S1,结合典型建筑结构定义主方向，进行运动类型分类；
S2,扩展卡尔曼滤波。
作为优选，所述S1包括：
S11，结合典型建筑结构定义主方向，本步骤中，基于启发式偏移消除算法为中心，设定8个主方向，主方向间的角度间隔Δ为45°；以及
S12，运动类型分类，本步骤中，根据行人行走的方向，分为三种运动类型：非直线运动、非主方向的直线运动以及沿主方向的直线运动。
作为优选，所述运动类型分类包括
S121,行人航向判定；
S122,陀螺仪角速率判定；
S123,行人位置判定；以及
S124,主方向判定。
作为优选，所述S121中，行人航向判定的方法为：
Figure BDA0002633991840000021
ψk代表当前k时刻陀螺仪解算的航向，ψj代表前第j时刻陀螺仪解算的航向，Δψk代表当前时刻陀螺仪解算的航向与历史平均值的差值，n代表选取的步数，
Figure BDA0002633991840000022
代表航向差值的阈值；
当航向差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在连续区间的航向发生较大变化，判定为非直线运动；和/或
S122中，陀螺仪角速率判定的方法为：
观察在直行、转圈、左转弯、右转弯下，可发现行人行走过程中的角速率具有明显的变化；
根据角速率变化规律，进行类型判定。
Figure BDA0002633991840000031
m代表加权滑动均值的窗口大小，
Figure BDA0002633991840000032
代表窗口第j个陀螺角速率值，αj代表
Figure BDA0002633991840000033
所对应的权重因子，ΔWarm代表相邻两个窗口的差值，
Figure BDA0002633991840000034
代表所取阈值。
当角速率差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在2个相邻区间的角速率发生较大变化，判定为非直线运动；和/或
S123,行人位置判定的方法为：
为了判断准备，采用前3步的位置和当前位置检测直线运动。基于线性回归通过4步位置的垂直偏移拟合直线，设Rk、Rk-1、Rk-2、Rk-3分别为当前位置和前3步的位置，拟合直线为y＝a+bx；
Figure BDA0002633991840000035
垂直偏移量的平方和：
Figure BDA0002633991840000036
由函数D得到a的偏导：
Figure BDA0002633991840000037
由函数D得到b的偏导：
Figure BDA0002633991840000038
得到函数D的最小值：
Figure BDA0002633991840000041
选择函数D最小化的b值：
Figure BDA0002633991840000042
其中，
Figure BDA0002633991840000043
得到一条最佳拟合直线，利用最小函数值D来判定。当D小于阈值时，判定行人沿直线行走，进行下一步判定；
Figure BDA0002633991840000044
和/或
S124,主方向判定的方法为：
根据8个主方向，使用MOD函数得到离最接近主方向的偏移差值；
Figure BDA0002633991840000045
当
Figure BDA0002633991840000046
时，Δψk-1 *＝Δψk-1 *-Δ
Figure BDA0002633991840000047
Figure BDA0002633991840000048
代表取整数的函数，输出结果为小于
Figure BDA0002633991840000049
的最大整数值，即此算法中MOD函数的取值区间为[0，Δ]，ΔψM代表MOD函数取余差值；
当偏移差值小于阈值时，判定行人沿主方向直线行走。反之，则为沿非主方向直线行走。
作为优选，所述S2中，构建主方向航向偏移差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计从而修正航向；
首先构建9维状态向量：x＝[δψn,δωn,δpn]
其中，δψ＝[δpitch,δroll,δyaw]为3轴姿态角误差，δω＝[δωx,δωy,δωz]为3轴角速率误差，δp＝[δx,δy,δh]为位置误差；量测矩阵为H＝[0,0,1,0,0,0,0,0,0]，由扩展卡尔曼滤波得到最优估计结果
Figure BDA0002633991840000051
可得航向信息
Figure BDA0002633991840000052
该输出航向作为下一时刻的初始值，参与到下一时刻的姿态角结算和行人航迹推算算法中，达到对航向的修正作用。
一方面，本发明还提供一种基于改进式HDE的室内行人导航航向修正系统。
一种基于改进式HDE的室内行人导航航向修正系统，该基于改进式HDE的室内行人导航航向修正系统至少包括：
运动类型分类系统，运动类型分类系统结合典型建筑结构定义主方向，进行运动类型分类；以及
扩展卡尔曼滤波系统，扩展卡尔曼滤波系统根据运动类型分类系统分类得出的运动类型分类构建航向差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波得到航向的最优估计。
作为优选，所述基于改进式HDE的室内行人导航航向修正系统还包括惯性测量单元系统，惯性测量单元系统解算航向、步长和步态信息，行人航迹推算位置信息。
作为优选，所述运动类型分类系统包括主方向模块以及运动类型模块，主方向模块结合典型建筑结构定义主方向，基于启发式偏移消除算法为中心，设定8个主方向，主方向间的角度间隔Δ为45°；运动类型模块根据行人行走的方向，分为三种运动类型：非直线运动、非主方向的直线运动、沿主方向的直线运动。
作为优选，所述运动类型模块包括行人航向判定模块、陀螺仪角速率判定模块、行人位置判定模块以及主方向判定模块，行人航向判定模块中，当航向差值小于阈值时，判定行人沿直线行走，进行下一步判定，反之，由于在连续区间的航向发生较大变化，判定为非直线运动；陀螺仪角速率判定模块中，当角速率差值小于阈值时，判定行人沿直线行走，进行下一步判定，反之，由于在2个相邻区间的角速率发生较大变化，判定为非直线运动；行人位置判定模块中，垂直偏移量的平方和D小于阈值时，判定行人沿直线行走，进行下一步判定；主方向判定模块中，当偏移差值小于阈值时，判定行人沿主方向直线行走，反之，则为沿非主方向直线行走。
与现有技术相比，本发明的有益效果在于：
本发明在HDE算法的基础上，增加了运动类型分类的判别方法，结合行人航向、陀螺仪角速率、行人位置和主方向共同判定，构建主方向航向偏移差值、角速率差值、位置差值的扩展卡尔曼滤波器，通过反馈系统修正航向。本发明提出的改进式HDE的航向修正方法，能够有效地避免传统的惯性导航系统随时间产生的累计误差以及传统的HDE算法出现错误修正航向的情况，增加航向信息的长时间稳定性，提高定位系统的精度。
名词术语解释
HDE-Heuristic Drift Elimination的缩写，即启发式偏移消除算法。
PDR-Pedestrian Dead Reckoning的缩写，即行人航迹推算算法。
EKF-extended kalman filter的缩写，即扩展卡尔曼滤波。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的基于改进式HDE的室内行人导航航向修正方法流程图；
图2为本发明的8个主方向示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
下面以具体地实施例对本发明的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
实施例1
请参考图1，图1为本实施例提供的一种基于改进式HDE的室内行人导航航向修正方法流程图。
一种基于改进式HDE的室内行人导航航向修正方法，该方法包括：
S0,惯性测量单元解算航向、步长和步态信息，行人航迹推算位置信息。
S01,姿态角解算:
选用四元数算法进行姿态解算，地理坐标系(n系)下，依次按照Z-X-Y的次序旋转，设θ，γ，ψ分别为俯仰角、横滚角和航向角。由加速度计和磁力计共同解算得到初始的θ，γ，ψ
Figure BDA0002633991840000081
ax、ay、az分别代表加速度计X、Y、Z轴值，
Figure BDA0002633991840000082
分别代表X、Y轴磁力计值。
根据初始姿态角，通过一阶毕卡法求解四元数微分方程，得到更新后的四元数为：
Figure BDA0002633991840000083
Q(t)代表采样点时刻的四元数，ωx、ωy、ωz代表陀螺仪的三轴角速率，Δt代表前后采样点的时间差。
更新即可得到新的姿态旋转矩阵，进一步求解三个姿态角：
Figure BDA0002633991840000084
S02,步长和PDR：
行人步长跟随身高、步伐改变，采用基于非线性模型的步长估计算法。
Figure BDA0002633991840000085
K代表步长阈值，amax、amin代表该步的加速度最大值、最小值。
在二维面，基于行人航迹推算算法PDR，由行人的步长、步态、航向等信息解算位置坐标，设初始点东、北向位置坐标(E(0),N(0))，得到第k步相对位置(E(k),N(k))。
Figure BDA0002633991840000091
S1,结合典型建筑结构定义主方向，进行运动类型分类。
S11，结合典型建筑结构定义主方向。
请参考图2，图2为本发明的8个主方向示意图。
本发明中，基于启发式偏移消除算法(HDE)为中心，设定8个主方向，主方向间的角度间隔Δ为45°。
S12，运动类型分类。
根据行人行走的方向，分为三种运动类型：非直线运动、直线运动(非主方向)、直线运动(沿主方向)。下面将通过S121-S124进行判定。
S121,行人航向判定。
Figure BDA0002633991840000092
ψk代表当前k时刻陀螺仪解算的航向，ψj代表前第j时刻陀螺仪解算的航向，Δψk代表当前时刻陀螺仪解算的航向与历史平均值的差值，n代表选取的步数，
Figure BDA0002633991840000093
代表航向差值的阈值。
当航向差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在连续区间的航向发生较大变化，判定为非直线运动。
S122,陀螺仪角速率判定。
观察在直行、转圈、左转弯、右转弯下，可发现行人行走过程中的角速率具有明显的变化。
根据角速率变化规律，进行类型判定。
Figure BDA0002633991840000101
m代表加权滑动均值的窗口大小，
Figure BDA0002633991840000102
代表窗口第j个陀螺角速率值，αj代表
Figure BDA0002633991840000103
所对应的权重因子，ΔWarm代表相邻两个窗口的差值，
Figure BDA0002633991840000104
代表所取阈值。
当角速率差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在2个相邻区间的角速率发生较大变化，判定为非直线运动。
S123,行人位置判定。
为了判断准备，采用前3步的位置和当前位置检测直线运动。基于线性回归通过4步位置的垂直偏移拟合直线，设Rk、Rk-1、Rk-2、Rk-3分别为当前位置和前3步的位置，拟合直线为y＝a+bx。
Figure BDA0002633991840000105
垂直偏移量的平方和：
Figure BDA0002633991840000106
由函数D得到a的偏导：
Figure BDA0002633991840000107
由函数D得到b的偏导：
Figure BDA0002633991840000108
得到函数D的最小值：
Figure BDA0002633991840000109
选择函数D最小化的b值：
Figure BDA0002633991840000111
其中，
Figure BDA0002633991840000112
得到一条最佳拟合直线，利用最小函数值D来判定。当D小于阈值时，判定行人沿直线行走，进行下一步判定。
Figure BDA0002633991840000113
S124,主方向判定。
根据8个主方向，使用MOD函数得到离最接近主方向的偏移差值。
Figure BDA0002633991840000114
当
Figure BDA0002633991840000115
时，Δψk-1 *＝Δψk-1 *-Δ
Figure BDA0002633991840000116
Figure BDA0002633991840000117
代表取整数的函数，输出结果为小于
Figure BDA0002633991840000118
的最大整数值，即此算法中MOD函数的取值区间为[0，Δ]，ΔψM代表MOD函数取余差值。
当偏移差值小于阈值时，判定行人沿主方向直线行走。反之，则为沿非主方向直线行走。
S2,扩展卡尔曼滤波。
构建主方向航向偏移差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计从而修正航向。
首先构建9维状态向量：x＝[δψn,δωn,δpn]
其中，δψ＝[δpitch,δroll,δyaw]为3轴姿态角误差，δω＝[δωx,δωy,δωz]为3轴角速率误差，δp＝[δx,δy,δh]为位置误差。量测矩阵为H＝[0,0,1,0,0,0,0,0,0]，由扩展卡尔曼滤波得到最优估计结果
Figure BDA0002633991840000121
可得航向信息
Figure BDA0002633991840000122
该输出航向作为下一时刻的初始值，参与到下一时刻的姿态角结算和行人航迹推算算法中，达到对航向的修正作用。
本发明通过惯性测量单元解算航向、步长和步态信息，行人航迹推算位置信息，结合典型建筑结构定义主方向，运动类型分类，构建航向差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计。
基于上述的基于改进式HDE的室内行人导航航向修正方法，本发明还提供一种基于改进式HDE的室内行人导航航向修正系统。
一种基于改进式HDE的室内行人导航航向修正系统，包括惯性测量单元系统、运动类型分类系统以及扩展卡尔曼滤波系统，惯性测量单元系统解算航向、步长和步态信息，行人航迹推算位置信息，运动类型分类系统根据惯性测量单元系统的信息结合典型建筑结构定义主方向，进行运动类型分类，扩展卡尔曼滤波系统根据运动类型分类系统分类得出的运动类型分类构建航向差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波得到航向的最优估计。
进一步地，惯性测量单元系统包括姿态角解算模块和步长和PDR模块，姿态角解算模块选用四元数算法进行姿态解算，步长和PDR模块中，行人步长跟随身高、步伐改变，采用基于非线性模型的步长估计算法，在二维面，基于行人航迹推算算法PDR。
进一步地，运动类型分类系统包括主方向模块以及运动类型模块，主方向模块结合典型建筑结构定义主方向，基于启发式偏移消除算法(HDE)为中心，设定8个主方向，主方向间的角度间隔Δ为45°。运动类型模块根据行人行走的方向，分为三种运动类型：非直线运动、直线运动(非主方向)、直线运动(沿主方向)。
进一步地，运动类型模块包括行人航向判定模块、陀螺仪角速率判定模块、行人位置判定模块以及主方向判定模块，行人航向判定模块中，当航向差值小于阈值时，判定行人沿直线行走，进行下一步判定，反之，由于在连续区间的航向发生较大变化，判定为非直线运动；陀螺仪角速率判定模块中，当角速率差值小于阈值时，判定行人沿直线行走，进行下一步判定，反之，由于在2个相邻区间的角速率发生较大变化，判定为非直线运动；行人位置判定模块中，垂直偏移量的平方和D小于阈值时，判定行人沿直线行走，进行下一步判定；主方向判定模块中，当偏移差值小于阈值时，判定行人沿主方向直线行走，反之，则为沿非主方向直线行走。
本发明通过惯性测量单元解算航向、步长和步态信息，行人航迹推算位置信息，结合典型建筑结构定义主方向，运动类型分类，构建航向差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计。
本发明结合典型建筑结构定义8个主方向。
本发明结合行人运动特征对非直线运动、直线运动(非主方向)、直线运动(沿主方向)3类运动类型进行分类，判定方法包括：行人航向判定、陀螺仪角速率判定、行人位置判定、主方向判定。
本发明构建主方向航向偏移差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计从而修正航向，并作为下一时刻的初始值，参与到下一时刻的姿态角结算和行人航迹推算算法中。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (10)

1.一种基于改进式HDE的室内行人导航航向修正方法，至少包括下列步骤：
S1,结合典型建筑结构定义主方向，进行运动类型分类；
S2,扩展卡尔曼滤波。
2.根据权利要求1所述的基于改进式HDE的室内行人导航航向修正方法，其特征在于所述S1包括：
S11，结合典型建筑结构定义主方向，本步骤中，基于启发式偏移消除算法为中心，设定8个主方向，主方向间的角度间隔Δ为45°；以及
S12，运动类型分类，本步骤中，根据行人行走的方向，分为三种运动类型：非直线运动、非主方向的直线运动以及沿主方向的直线运动。
3.根据权利要求2所述的基于改进式HDE的室内行人导航航向修正方法，其特征在于：所述运动类型分类包括
S121,行人航向判定；
S122,陀螺仪角速率判定；
S123,行人位置判定；以及
S124,主方向判定。
4.根据权利要求3所述的基于改进式HDE的室内行人导航航向修正方法，其特征在于：
所述S121中，行人航向判定的方法为：
Figure FDA0002633991830000011
ψk代表当前k时刻陀螺仪解算的航向，ψj代表前第j时刻陀螺仪解算的航向，Δψk代表当前时刻陀螺仪解算的航向与历史平均值的差值，n代表选取的步数，
Figure FDA0002633991830000021
代表航向差值的阈值；
当航向差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在连续区间的航向发生较大变化，判定为非直线运动；和/或
S122中，陀螺仪角速率判定的方法为：
观察在直行、转圈、左转弯、右转弯下，可发现行人行走过程中的角速率具有明显的变化；
根据角速率变化规律，进行类型判定。
Figure FDA0002633991830000022
m代表加权滑动均值的窗口大小，
Figure FDA0002633991830000023
代表窗口第j个陀螺角速率值，αj代表
Figure FDA0002633991830000024
所对应的权重因子，ΔWarm代表相邻两个窗口的差值，
Figure FDA0002633991830000025
代表所取阈值。
当角速率差值小于阈值时，判定行人沿直线行走，进行下一步判定；反之，由于在2个相邻区间的角速率发生较大变化，判定为非直线运动；和/或
S123,行人位置判定的方法为：
为了判断准备，采用前3步的位置和当前位置检测直线运动。基于线性回归通过4步位置的垂直偏移拟合直线，设Rk、Rk-1、Rk-2、Rk-3分别为当前位置和前3步的位置，拟合直线为y＝a+bx；
Figure FDA0002633991830000026
垂直偏移量的平方和：
Figure FDA0002633991830000027
由函数D得到a的偏导：
Figure FDA0002633991830000028
由函数D得到b的偏导：
Figure FDA0002633991830000031
得到函数D的最小值：
Figure FDA0002633991830000032
选择函数D最小化的b值：
Figure FDA0002633991830000033
其中，
Figure FDA0002633991830000034
得到一条最佳拟合直线，利用最小函数值D来判定。当D小于阈值时，判定行人沿直线行走，进行下一步判定；
Figure FDA0002633991830000035
S124,主方向判定的方法为：
根据8个主方向，使用MOD函数得到离最接近主方向的偏移差值；
Figure FDA0002633991830000036
当
Figure FDA0002633991830000037
时，Δψk-1 *＝Δψk-1 *-Δ
Figure FDA0002633991830000038
Figure FDA0002633991830000039
代表取整数的函数，输出结果为小于
Figure FDA00026339918300000310
的最大整数值，即此算法中MOD函数的取值区间为[0，Δ]，ΔψM代表MOD函数取余差值；
当偏移差值小于阈值时，判定行人沿主方向直线行走。反之，则为沿非主方向直线行走。
5.根据权利要求1-4任一所述的基于改进式HDE的室内行人导航航向修正方法，其特征在于：所述S2中，构建主方向航向偏移差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF)，得到航向的最优估计从而修正航向；
首先构建9维状态向量：x＝[δψn,δωn,δpn]
其中，δψ＝[δpitch,δroll,δyaw]为3轴姿态角误差，δω＝[δωx,δωy,δωz]为3轴角速率误差，δp＝[δx,δy,δh]为位置误差；量测矩阵为H＝[0,0,1,0,0,0,0,0,0]，由扩展卡尔曼滤波得到最优估计结果
Figure FDA0002633991830000041
可得航向信息
Figure FDA0002633991830000042
该输出航向作为下一时刻的初始值，参与到下一时刻的姿态角结算和行人航迹推算算法中，达到对航向的修正作用。
6.根据权利要求1-5任一所述的基于改进式HDE的室内行人导航航向修正方法，其特征在于：该基于改进式HDE的室内行人导航航向修正方法还包括S0,惯性测量单元解算航向、步长和步态信息，行人航迹推算位置信息，其中S0包括S01姿态角解算以及S02步长和PDR解算。
7.一种基于改进式HDE的室内行人导航航向修正系统，该基于改进式HDE的室内行人导航航向修正系统至少包括：
运动类型分类系统，运动类型分类系统结合典型建筑结构定义主方向，进行运动类型分类；以及
扩展卡尔曼滤波系统，扩展卡尔曼滤波系统根据运动类型分类系统分类得出的运动类型分类构建航向差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波得到航向的最优估计。
8.根据权利要求7所述的基于改进式HDE的室内行人导航航向修正系统，其特征在于：所述基于改进式HDE的室内行人导航航向修正系统还包括惯性测量单元系统，惯性测量单元系统解算航向、步长和步态信息，行人航迹推算位置信息。
9.根据权利要求7-8任一所述的基于改进式HDE的室内行人导航航向修正系统，其特征在于：所述运动类型分类系统包括主方向模块以及运动类型模块，主方向模块结合典型建筑结构定义主方向，基于启发式偏移消除算法为中心，设定8个主方向，主方向间的角度间隔Δ为45°；运动类型模块根据行人行走的方向，分为三种运动类型：非直线运动、非主方向的直线运动、沿主方向的直线运动。
10.根据权利要求7-9任一所述的基于改进式HDE的室内行人导航航向修正系统，其特征在于：所述运动类型模块包括行人航向判定模块、陀螺仪角速率判定模块、行人位置判定模块以及主方向判定模块，行人航向判定模块中，当航向差值小于阈值时，判定行人沿直线行走，进行下一步判定，反之，由于在连续区间的航向发生较大变化，判定为非直线运动；陀螺仪角速率判定模块中，当角速率差值小于阈值时，判定行人沿直线行走，进行下一步判定，反之，由于在2个相邻区间的角速率发生较大变化，判定为非直线运动；行人位置判定模块中，垂直偏移量的平方和D小于阈值时，判定行人沿直线行走，进行下一步判定；主方向判定模块中，当偏移差值小于阈值时，判定行人沿主方向直线行走，反之，则为沿非主方向直线行走。
CN202010819613.3A 2020-08-14 2020-08-14 一种基于改进式hde的室内行人导航航向修正方法及系统 Active CN111912406B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010819613.3A CN111912406B (zh) 2020-08-14 2020-08-14 一种基于改进式hde的室内行人导航航向修正方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010819613.3A CN111912406B (zh) 2020-08-14 2020-08-14 一种基于改进式hde的室内行人导航航向修正方法及系统

Publications (2)

Publication Number Publication Date
CN111912406A true CN111912406A (zh) 2020-11-10
CN111912406B CN111912406B (zh) 2022-04-15

Family

ID=73284121

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010819613.3A Active CN111912406B (zh) 2020-08-14 2020-08-14 一种基于改进式hde的室内行人导航航向修正方法及系统

Country Status (1)

Country Link
CN (1) CN111912406B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113029148A (zh) * 2021-03-06 2021-06-25 西南交通大学 一种基于航向角准确修正的惯性导航室内定位方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104197936A (zh) * 2014-09-10 2014-12-10 长沙格致电子科技有限公司 一种基于建筑物规则结构信息的室内定位方法
EP2827101A1 (en) * 2013-07-18 2015-01-21 Astrium GmbH Method for determing a position of a tracking device
CN107677267A (zh) * 2017-08-22 2018-02-09 重庆邮电大学 基于mems‑imu的室内行人导航航向反馈修正方法
CN108225304A (zh) * 2018-01-26 2018-06-29 青岛美吉海洋地理信息技术有限公司 基于多源传感器室内快速定位方法与系统
CN108444473A (zh) * 2018-03-20 2018-08-24 南京华苏科技有限公司 一种行人室内轨迹定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2827101A1 (en) * 2013-07-18 2015-01-21 Astrium GmbH Method for determing a position of a tracking device
CN104197936A (zh) * 2014-09-10 2014-12-10 长沙格致电子科技有限公司 一种基于建筑物规则结构信息的室内定位方法
CN107677267A (zh) * 2017-08-22 2018-02-09 重庆邮电大学 基于mems‑imu的室内行人导航航向反馈修正方法
CN108225304A (zh) * 2018-01-26 2018-06-29 青岛美吉海洋地理信息技术有限公司 基于多源传感器室内快速定位方法与系统
CN108444473A (zh) * 2018-03-20 2018-08-24 南京华苏科技有限公司 一种行人室内轨迹定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
GUO YING,ET.AL: "Improved Heuristic Drift Elimination Algorithm Optimisation Based on a Smartphone Gyroscope", 《JOURNAL OF NAVIGATION》 *
陈自然: "基于MEMS惯性传感器的多源信息增强人员定位技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113029148A (zh) * 2021-03-06 2021-06-25 西南交通大学 一种基于航向角准确修正的惯性导航室内定位方法

Also Published As

Publication number Publication date
CN111912406B (zh) 2022-04-15

Similar Documents

Publication Publication Date Title
CN109813311B (zh) 一种无人机编队协同导航方法
CN106225784B (zh) 基于低成本多传感器融合行人航位推算方法
Tong et al. A double-step unscented Kalman filter and HMM-based zero-velocity update for pedestrian dead reckoning using MEMS sensors
WO2020253854A1 (zh) 移动机器人姿态角解算方法
CN110274588B (zh) 基于无人机集群信息的双层嵌套因子图多源融合导航方法
CN105043385B (zh) 一种行人自主导航定位的自适应卡尔曼滤波方法
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN109959374B (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
CN110851776B (zh) 一种高动态变转速载体的姿态解算方法
CN113587925B (zh) 一种惯性导航系统及其全姿态导航解算方法与装置
CN107270896A (zh) 一种行人定位与轨迹跟踪方法和系统
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111399023A (zh) 基于李群非线性状态误差的惯性基组合导航滤波方法
CN113029148A (zh) 一种基于航向角准确修正的惯性导航室内定位方法
CN111912406B (zh) 一种基于改进式hde的室内行人导航航向修正方法及系统
CN111307148B (zh) 一种基于惯性网络的行人定位方法
CN110146079A (zh) 一种基于主副imu和气压计的三维行人导航方法
Luo et al. Crawler Robot Indoor Positioning Based on a Combination of Bluetooth and IMU
CN112902962A (zh) 一种稳健的室内定位航向角度估计方法
CN111964671A (zh) 一种基于双轴旋转调制的惯性天文组合导航系统及方法
CN115574816B (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