CN108362282A - 一种基于自适应零速区间调整的惯性行人定位方法 - Google Patents
一种基于自适应零速区间调整的惯性行人定位方法 Download PDFInfo
- Publication number
- CN108362282A CN108362282A CN201810084487.4A CN201810084487A CN108362282A CN 108362282 A CN108362282 A CN 108362282A CN 201810084487 A CN201810084487 A CN 201810084487A CN 108362282 A CN108362282 A CN 108362282A
- Authority
- CN
- China
- Prior art keywords
- pedestrian
- foot
- zero
- speed
- information
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 8
- 230000004807 localization Effects 0.000 title abstract 2
- 238000001514 detection method Methods 0.000 claims abstract description 59
- 230000033001 locomotion Effects 0.000 claims abstract description 50
- 230000001133 acceleration Effects 0.000 claims abstract description 45
- 230000008569 process Effects 0.000 claims abstract description 15
- 239000011159 matrix material Substances 0.000 claims description 30
- 230000003068 static effect Effects 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 8
- 230000036461 convulsion Effects 0.000 claims description 6
- 230000001174 ascending effect Effects 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 4
- 230000010354 integration Effects 0.000 claims description 4
- 238000012850 discrimination method Methods 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 abstract description 20
- 238000004364 calculation method Methods 0.000 abstract description 6
- 238000005516 engineering process Methods 0.000 abstract description 4
- 230000008859 change Effects 0.000 description 12
- 238000012937 correction Methods 0.000 description 12
- 230000005021 gait Effects 0.000 description 5
- 238000005259 measurement Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000001629 suppression Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 102100023121 Ninein Human genes 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 238000004321 preservation Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
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/18—Stabilised platforms, e.g. by gyroscope
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于自适应零速区间调整的惯性行人定位方法,属于依托惯导系统的行人定位技术领域。本发明将惯性器件固定在行人脚部,根据测量得的加速度和角速度实时解算行人的定位信息,同时,结合加加速度和角加速度幅值检测算法设计了自适应滑动窗口检测法,在系统解算过程中自适应地调整零速检测窗口长度,提高零速检测精度。再利用零速校正算法对零速区间内的行人位姿信息进行补偿,补偿后的位姿信息作为最终定位信息并输出。本发明增强了任意运动速度下惯性行人定位系统的适用性,减小了因行人运动速度变化而导致的误差补偿不充分的问题,并且不需任何外界辅助信息就可提高定位精度,计算量小,简单易操作。
Description
技术领域
本发明属于依托惯导系统的行人定位技术领域,具体涉及一种基于自适应零速区间调整的惯性行人定位方法。
背景技术
目前,最常见的行人定位系统是全球定位系统(Global Positioning System,GPS),但是在高楼林立的街道和相对封闭的室内等环境中,行人无法正常接收GPS信号,进而无法定位。而惯性导航系统(inertial navigation system,INS,以下简称惯导)是一种不依赖于外部信息、也不向外辐射能量的自主式导航系统,适用于复杂多变的行人运动环境。正是由于微惯性传感器的快速发展,有体积小、重量轻、易集成、抗电磁干扰、可靠性高等优点,才使得将惯导系统应用于行人定位成为可能。惯导系统的基本工作原理是以牛顿力学定律为基础,通过实时测量载体的加速度和角速度,计算得载体的定位信息。但是,由于生产加工工艺的限制,惯导系统会不可避免的存在噪声,使得系统的定位误差随时间发散而不断增大,这是制约惯性行人定位系统定位精度的重要因素之一。
为了提高系统定位精度,可以采用高精度惯性元件或者先进的惯性行人定位系统误差抑制算法。但是,无限制的提高元件精度必然会导致制作成本的大幅度增加,考虑到行人定位系统的核心要求为低成本、高精度,采用先进的误差抑制算法才是最佳途径。通过采取先进的惯性行人定位系统定位误差抑制算法,自动抵消惯性器件的误差对系统精度的影响,这样就可以应用现有精度的惯性元件构成较高精度的惯性行人定位系统。
零速校正方法是一种惯性行人定位系统的误差自补偿方法,该方法需要将惯性器件与行人足部固联,实时检测行人足部的运动状态,当足部离地时,对足部的位置、速度、姿态信息(以下简称位姿信息)进行解算;当足部触地时,理论上认为足部与地面是相对静止的,并将这一段相对静止的时间称为“零速区间”,通过对零速区间内的位姿信息进行解算与误差补偿,可以抵消器件噪声对系统的影响,进而提高系统定位精度。但是,零速校正方法的有效性建立在零速区间检测的准确性上,只有系统能精确地检测出行的足部的零速区间,零速校正才能取得理想的效果。常见的零速检测方法是滑动窗口检测法,即对固定长度窗口内的数据进行检测从而判定足部是否处于零速状态。但是行人运动具有随意性,据参考相关文献及发明人不完全试验统计,当行人慢速行进时,每步时长可达2.7秒,其中零速时长可达1.15秒;当行人快速行进时,每步时长短达0.69秒,其中零速时长短达0.05秒。可见行人运动过程中零速区间的长度有很大差异,如果仍然利用固定窗长的滑动窗口来检测行人运动状态,这会导致零速检测不准确,即漏检或误检,从而导致零速校正算法的适用性降低,进而无法完全补偿定位误差,导致定位精度显著下降,无法满足行人定位的需求。
公告号为CN104296750A的中国发明专利在2015年1月21日公开的《一种零速检测方法和装置以及行人导航方法和系统》,在传统滑动窗口检测法的基础上,结合聚类分析方法自适应的调整零速检测的阈值,进而检测出行人足部运动状态,在一定程度上提高了零速检测的可靠性。《导航与控制》2016年15卷第4期由孟祥宾等人撰写的《一种优化的零速检测行人导航算法》,该文章主要针对常用零速校正算法对步速变化、转弯等一些异常情况实用性低的问题,提出了用多种零速检测方法相结合的方式来检测行人足部运动状态的方法,提高了部分步态下惯性行人定位系统的精度。《TRANSACTIONS ON BIOMEDICALENGINEERING》2010年57卷11期由Skog等人撰写的《Zero-Velocity Detection-AnAlgorithm Evaluation》,该文章提出了三种不同的基于滑动窗口的零速检测方法,并指出滑动窗口的长度应由惯导系统的采样频率和行人足部的真实触地时间决定。《Sensors》2016年16卷第10期由Xiaochun Tian等人撰写的《A Novel Zero Velocity IntervalDetection Algorithm for Self-Contained Pedestrian Navigation System withInertial Sensors》,该文章提出了一种基于平滑伪Wigner-Ville分布的自适应零速检测算法,该算法采用SPWVD-RMFI方法提取行人步态频率,并通过建立阈值与步态频率之间的函数关系,实时调整零速检测阈值,在一定程度上提高了零速检测的精度。《IEEE SENSORSJOURNAL》2017年17卷第7期由Zhang Rui等人撰写的《Adaptive Zero Velocity UpdateBased On Velocity Classification For Pedestrian Tracking》,该文章对行人步态与胸部运动特征的物理关系进行了分析,提出了一种胸部加速度辅助校准零速检测阈值的方案,实现了不同步速下零速检测阈值的自适应调节。哈尔滨工程大学2013年由刘峰丽撰写的《MIMU/GPS/磁力计单兵系统组合导航技术研究》,该文章提出了一种基于隐马尔科夫模型的零速校正技术,它利用陀螺仪输出数据进行零速检测,在一定程度上提高了快速运动时零速校正算法的适用性,但同时导致行人定位系统在慢速运动时定位精度下降。以上文献都在于自适应调整行人不同运动速度下的零速检测阈值,通过调整阈值来提高零速检测精度,或针对某一特定步态来改进零速检测算法,以此提高零速检测精度,综上,所有文献都没有提及零速检测窗口长度对任意步态下系统解算位姿信息精度及适用性的影响,也没有提出自适应调整零速检测窗口长度的方法。
发明内容
本发明的目的在于提供提高定位精度,增强不同运动速度下系统解算行人定位信息的适用性的一种基于自适应零速区间调整的惯性行人定位方法。
本发明通过如下技术方案来实现:
一种基于自适应零速区间调整的惯性行人定位方法,包括以下步骤:
步骤一:将惯性器件固定在行人足部,打开电源并初始化系统;
步骤二:系统实时采集惯性器件的输出数据,包括加速度计输出的加速度信息和陀螺仪输出的角速度信息;
步骤三:利用步骤二中采集到的角速度信息,解算行人足部的姿态信息,得到其中b表示载体坐标系,n表示导航坐标系,表示b系到n系的转换矩阵;
步骤四:根据步骤二中采集的加速度f和步骤三中解算得的转换矩阵计算行人足部位置信息和姿态信息:利用将加速度计输出的加速度信息投影到导航坐标系后,有其中,fb表示载体坐标系下的加速度信息,fn表示导航坐标系下的加速度信息;加速度信息经过一次积分求解出速度信息:νn=∫fndt,再次积分求解出位置信息:pn=∫vndt;其中ν表示速度信息,p表示位置信息,pn表示导航坐标系下的位置信息,t表示时间;
步骤五:系统利用加加速度和角加速度幅值判别法实时检测行人的运动状态,将行人运动状态分为慢走、快走和跑步;
步骤六:根据步骤五中检测得的行人运动状态,判断零速检测窗口长度N;
步骤七:读取步骤二中采集的足部加速度f和足部角速度ω,根据步骤六中零速窗口长度N,利用基于滑动窗口的加速度和角速度方差检测法判断行人足部运动状态,并用ZUPT表示足部运动状态。当足部处于离地状态时,记作ZUPT=0;当行人足部触地时,记作ZUPT=1;
步骤八:根据步骤五中零速检测结果,如果ZUPT=0,则将步骤四中解算的定位信息作为最终导航信息并输出;如果ZUPT=1,则转至步骤九;
步骤九:根据步骤七中加速度和角速度信息,解算行人的速度和位置由于理论上行人足部触地时有ν=0,所以其中δν为速度误差。
步骤十:以速度误差δν、位置误差δp和姿态误差δε为状态量,以速度误差为观测量,对零速区间内数据进行Kalman滤波;
步骤十一:利用步骤十中解算出的δν,δp,δε补偿并将补偿后的结果作为最终导航输出,其中,分别为系统解算的ν,p,ε的值;
步骤十二:重复步骤二到步骤十一,直至惯性行人定位系统导航结束。
本发明针对行人运动速度变化导致零速检测不准确的问题,在惯性行人定位系统解算行人位姿信息过程中,根据行人运动快慢自适应的调整零速检测窗口长度,并利用滑动窗口检测法实时检测行人足部的运动状态。在得到零速检测结果的基础上,通过零速校正算对行人的位姿信息进行补偿,提高定位精度,增强不同运动速度下系统解算行人定位信息的适用性。
本发明的有益效果在于:
本发明根据惯性行人定位系统位姿误差形式,建立了相应的误差传播模型。结合加加速度和角加速度幅值检测算法设计了自适应滑动窗口检测法,在系统解算过程中自适应地调整零速检测窗口长度,提高零速检测精度。再利用零速校正算法对零速区间内的行人位姿信息进行补偿,提高了不同运动速度下零速校正算法的适用性。零速校正后的定位信息作为最终导航解算输出信息。
增强了不同运动速度下零速校正算法的适用性,减小了因行人运动速度变化而导致的误差补偿不充分的问题;不需任何外界辅助信息就可提高定位精度;计算量小,满足实践中的实时性要求。
附图说明
图1为本发明的方法流程图;
图2为本发明中利用MATLAB仿真得到有无自适应零速区间调整算法的轨迹曲线。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明:
一种惯性行人定位系统定位误差抑制方法,方法流程如图1所示,包括以下步骤:
步骤一:将MIMU器件水平固定在行人足部,其x、y、z轴分别指向行人的右、前、上方向。上电,运行。
导航初始时刻,需初始化系统:(1)初始化惯性定位系统初值:行人三轴位置信息 (单位均为m),三轴速度信息(单位均为ms),以及三个姿态角信息φ0、θ0、ψ0((单位均为rad)),初始转换矩阵初始四元数q0;(2)初始化常值参量:当地重力海拔h(单位为m),当地纬度λ(单位为rad),运动状态检测窗口长度m,运动状态检测阈值γ′f、γ″f、γ′ω、γ″ω,零速检测阈值γ1、γ2,陀螺仪白噪声误差σω,采样时间T(单位为s);(3)Kalman滤波器参数初值:状态变量初值均方误差阵P0,系统噪声方差阵Q,量测噪声方差阵R,量测阵H;将以上初始化信息装订至导航计算机中。
通常情况下,
姿态角φ0、θ0计算如下:
其中,φ0表示横滚角初始值,θ0表示俯仰角初始值,分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量。
初始转换矩阵计算如下:
其中,b表示载体坐标系,n表示导航坐标系,表示b系到n系的转换矩阵。
初始四元数q0计算如下:
令则
其中,cij(i=1,2,3,j=1,2,3)表示中第i行第j列矩阵元素,[·]T表示矩阵的转置;
当地重力加速度计算如下:
g=9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10- 9sin2λ)×h+7.2×10-14×h2
其中,0为三行三列的零矩阵;
其余初值(例如h)需根据实际情况设定;
定位过程中,利用该初始信息进行更新,得到任意时刻行人的位置、速度和姿态信息;
步骤二:系统实时采集惯性器件的输出数据,包括加速度计输出的三轴加速度 分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为m/s2);陀螺仪输出的三轴角速度 分别表示陀螺仪测量的角速度在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为rads);
步骤三:利用步骤二中采集到的角速度信息,解算行人足部的姿态信息,得到其中b表示载体坐标系,n表示导航坐标系,表示b系到n系的转换矩阵;具体过程如下:
在行人运动过程中,通过ωb更新转换矩阵具体为:
四元数姿态矩阵更新:
设任意时刻载体系相对导航坐标系的转动四元数为:
q=[q0 q1 q2 q3]T (1)
其中,q为四元数;q0为q的实数部分,q1、q2、q3为q的虚数部分。
四元数q的及时修正:
其中,分别表示q0、q1、q2、q3的变化率;
根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k),求取k时刻转动四元数的变化率为:
则k时刻载体的转动四元数具体为:
其中,I为单位阵,T为采样时间,式中ωx、ωy、ωz均省略了上角标b,当k=1时,q(k-1)为步骤一中初始化系统时获得的载体初始四元数。
利用得到的q(k)中元素q0(k)、q1(k)、q2(k)、q3(k),更新捷联矩阵
其中,(5)式中的qi(i=1,2,3,4)为(4)式中qi(k)(i=1,2,3,4),(5)式中省略了下角标k。
更新载体姿态信息,具体为:
步骤四:根据步骤二中采集的加速度f和步骤三中解算得的转换矩阵计算行人足部位置信息和姿态信息:利用将加速度计输出的加速度信息投影到导航坐标系后,有其中,fb表示载体坐标系下的加速度信息,fn表示导航坐标系下的加速度信息;加速度信息经过一次积分求解出速度信息:νn=∫fndt,再次积分求解出位置信息:pn=∫vndt;其中ν表示速度信息,p表示位置信息,pn表示导航坐标系下的位置信息,t表示时间;具体过程如下:
将加速度计沿载体坐标系测量的比力信息,通过捷联矩阵进行投影转换:
利用下列微分方程求解载体运动速度:
其中,分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量; 表示vx、vy、vz的变化率,即载体沿导航系oxn轴、oyn轴、ozn轴的运动加速度;g为当地重力加速度。
根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k),求取k时刻载体速度变化率为:
在k时刻载体速度和位置分别为:
其中,分别表示载体位置在导航系oxn轴、oyn轴、ozn轴上的分量,当k=1时,vx(1)、vy(1)、vz(1)为步骤一中初始化系统时获得的载体初始速度,为步骤一中初始化系统时获得的载体初始位置。
至此,根据(6)、(10)、(11)式,得到了行人的姿态、速度、位置。
步骤五:在工程上,加加速度又名“急动度”,它可以用来描述运动过程中力的变化快慢;在行人运动过程中,足部角加速度会因运动速度的不同而具有明显的差异性,故行人定位系统可以利用加加速度和角加速度幅值检测法实时检测行人的运动状态,将行人运动状态分为慢走(包含静止)、快走和跑步;具体过程如下:
k时刻,行人足部加加速度幅值为角加速度幅值为其中,||·||表示向量的2范数。
记m为行人运动状态检测窗口长度,对前m时刻的数据进行分析,若满足且则可认为行人处于跑步状态;若且则可认为行人处于快走状态;否则,认为行人处于慢走状态。其中,γ′f、γ″f、γ′ω、γ′ω为运动状态检测阈值,当k<m时,属于行人运动初始时刻,此时行人足部一般的静止的,将其归类为慢走状态;
步骤六:根据步骤五中检测得的行人运动状态,判断零速检测窗口长度N;
若行人处于跑步状态,则零速检测窗口长度选取为N1;若行人处于快走状态,则零速检测窗口长度选取为N2;若行人处于慢走状态,则零速检测窗口长度选取为N3;
步骤七:读取步骤二中采集的足部加速度f和足部角速度ω,根据步骤六中零速窗口长度N,利用基于滑动窗口的加速度和角速度方差检测法判断行人足部运动状态,并用ZUPT表示足部运动状态。当足部处于离地状态时,记作ZUPT=0;当行人足部触地时,记作ZUPT=1,理论上认为此时足部与地面相对静止,即足部运动速度为0,故将这一段相对静止的时间称为“零速区间”。具体过程如下:
若k时刻,满足且则认为足部处于触地状态,记作ZUPT(k)=1,理论上认为此时足部与地面相对静止,即足部运动速度为0,并将这一段相对静止的时间称为“零速区间”;反之,则认为足部处于离地状态,记作ZUPT(m)=0。其中,γ1、γ2为零速检测阈值,σω为陀螺仪白噪声误差;
步骤八:根据步骤五中零速检测结果,如果ZUPT=0,则将步骤四中解算的定位信息作为最终导航信息,并输出;如果ZUPT=1,则转至步骤九;
步骤九:根据k-1时刻行人足部位姿信息,解算k时刻行人的速度和位置具体过程如下:
理论上行人足部触地时,位置保持不变,k时刻载体位置为:
速度为:
其中,分别为ν,p的计算值。
由于理论上行人足部触地时有ν=0,所以其中δν为速度误差。
步骤十:以速度误差δν、位置误差δp和姿态误差δε为状态量,以速度误差δν为观测量,对零速区间内数据进行Kalman滤波,具体过程如下:
利用下列微分方程建立载体运动速度、位置和姿态的误差模型:
其中:δp为载体位置误差,且δν为载体速度误差,且δε为载体姿态角误差并且有δε=[δφ δθ δψ]。分别为δp,δν,δε的变化率,ωb为载体角速度,且[fn×]为由加速度信息构成的反对称阵,且03×1为三行一列的零向量。
根据k时刻的载体位置误差δp(k),载体速度误差δν(k),载体姿态角误差δε(k),求取k时刻载体位置误差变化率、速度误差变化率、姿态角误差变化率为:
在k时刻载体位置误差δp(k)、速度误差δν(k)、姿态角误差δε(k)分别为:
其中,式(17)和式(18)中省去了上角标n。
由于理论上行人足部触地时,应有ν=0,故此时系统解算出的速度信息ν为误差量,即ν=δν,其中δν为速度误差。
根据上述误差模型,利用卡尔曼滤波器,对行人的位姿信息进行误差修正,具体算法如下:
Kk=PkHT(HPkHT+R)-1 (18)
其中,Pk为k时刻的估计均方误差阵,Fk,k-1为k-1时刻到k时刻的为状态转移矩阵,且I为单位阵,0为零矩阵,为k-1时刻的噪声驱动阵,Kk为k时刻的滤波增益阵。
Xk=-KkZk (19)
更新Pk:
其中,Xk为k时刻的状态变量,当k=1时,状态量X=[δp(1)δν(1)δε(1)]为步骤一中初始化系统时获得的载体初始状态量X0,Pknew为更新后的Pk。
保存Pk与Xk,并将其作为下一次滤波的输入。
式(17)应当在整个定位过程中实时结算,而非只针对零速区间。
步骤十一:利用步骤十中解算出的δν,δp,δε补偿并将补偿后的结果作为最终导航输出,其中,分别为系统解算的ν,p,ε的值。具体过程如下:
利用步骤九中解算出的δν,δp,δε补偿步骤八中的更新四元数qk,并将补偿后的结果作为最终导航输出:
位置、速度信息补偿:
姿态角补偿:
令利用公式 更新转换矩阵利用式(6)求出更新后的φ,θ,ψ;
其中,[·]-1为矩阵的逆运算,为步骤三中的计算值。
更新四元数:
令则
至此,k时刻行人足部的位置、速度和姿态信息误差补偿完毕。将补偿后的位姿信息作为最终导航结果,并输出。
步骤十二:重复步骤二到步骤十一,直至惯性行人定位系统导航结束。
实施例:
对本发明的有益效果如下方式得以验证:
采用MTi-710系列MIMU构建惯性行人定位系统,进行惯性行人定位系统的变速行走试验。
行人预设路线为一条直线,行人变速(先快速再慢速)行进22.69秒,行进路程长度为48.4米;
系统初始化参数如下:
载体三轴位置信息:
载体三轴速度信息:
载体航向角信息:ψ0=0rad;
当地重力海拔:h=100m;
当地纬度:λ=0.7988rad;
采样时间:T=0.01s;
陀螺仪白噪声误差:σω=0.00506;
运动状态检测窗口长度:m=3;
运动状态检测阈值:γ′f=15;
γ″f=6.15、
γ′ω=2;
γ′ω=0.45,
零速检测阈值:γ1=8500;
γ2=0.3;
Kalman滤波器初始参数:
初始状态量:
初始均方误差阵:
系统噪声方差阵:
量测噪声方差阵:
利用发明所述方法,得到行人变速行走时有、无自适应零速区间调整算法的定位轨迹比较曲线,其中实线、虚线分别表示有自适应零速区间调整算法的定位轨迹曲线,无自适应零速区间调整算法的定位轨迹曲线。结果表明本发明抑制行人变速运动时定位误差能力较好,可以满足实际需求。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (4)
1.一种基于自适应零速区间调整的惯性行人定位方法,其特征在于,包括以下步骤:
步骤一:将惯性器件固定在行人足部,打开电源并初始化系统;
步骤二:系统实时采集惯性器件的输出数据,包括加速度计输出的加速度信息和陀螺仪输出的角速度信息;
步骤三:利用步骤二中采集到的角速度信息,解算行人足部的姿态信息,得到其中b表示载体坐标系,n表示导航坐标系,表示b系到n系的转换矩阵;
步骤四:根据步骤二中采集的加速度f和步骤三中解算得的转换矩阵计算行人足部位置信息和姿态信息:利用将加速度计输出的加速度信息投影到导航坐标系后,有其中,fb表示载体坐标系下的加速度信息,fn表示导航坐标系下的加速度信息;加速度信息经过一次积分求解出速度信息:νn=∫fndt,再次积分求解出位置信息:pn=∫vndt;其中ν表示速度信息,p表示位置信息,pn表示导航坐标系下的位置信息,t表示时间;
步骤五:系统利用加加速度和角加速度幅值判别法实时检测行人的运动状态,将行人运动状态分为慢走、快走和跑步;
步骤六:根据步骤五中检测得的行人运动状态,判断零速检测窗口长度N;
步骤七:读取步骤二中采集的足部加速度f和足部角速度ω,根据步骤六中零速窗口长度N,利用基于滑动窗口的加速度和角速度方差检测法判断行人足部运动状态,并用ZUPT表示足部运动状态,当足部处于离地状态时,记作ZUPT=0;当行人足部触地时,记作ZUPT=1;
步骤八:根据步骤五中零速检测结果,如果ZUPT=0,则将步骤四中解算的定位信息作为最终导航信息并输出;如果ZUPT=1,则转至步骤九;
步骤九:根据步骤七中加速度和角速度信息,解算行人的速度和位置由于理论上行人足部触地时有ν=0,所以其中δν为速度误差;
步骤十:以速度误差δν、位置误差δp和姿态误差δε为状态量,以速度误差为观测量,对零速区间内数据进行Kalman滤波;
步骤十一:利用步骤十中解算出的δν,δp,δε补偿并将补偿后的结果作为最终导航输出,其中,分别为系统解算的ν,p,ε的值;
步骤十二:重复步骤二到步骤十一,直至惯性行人定位系统导航结束。
2.根据权利要求1所述的一种基于自适应零速区间调整的惯性行人定位方法,其特征在于,所述的步骤五具体为:
(5.1)k时刻,行人足部加加速度幅值为角加速度幅值为其中,||·||表示向量的2范数;
(5.2)记m为行人运动状态检测窗口长度,对前m时刻的数据进行分析,若满足且则可认为行人处于跑步状态;若且则可认为行人处于快走状态;否则,认为行人处于慢走状态,其中,γf′、γ″f、γ′ω、γ″ω为运动状态检测阈值,当k<m时,属于行人运动初始时刻,此时行人足部一般的静止的,将其归类为慢走状态。
3.根据权利要求1所述的一种基于自适应零速区间调整的惯性行人定位方法,其特征在于,所述的步骤六具体为:
若行人处于跑步状态,则零速检测窗口长度选取为N1;若行人处于快走状态,则零速检测窗口长度选取为N2;若行人处于慢走状态,则零速检测窗口长度选取为N3。
4.根据权利要求1所述的一种基于自适应零速区间调整的惯性行人定位方法,其特征在于,所述的步骤七具体为:
(7.1)读取步骤二中采集的足部加速度f和足部角速度ω,根据步骤六中零速窗口长度N,利用基于滑动窗口的加速度和角速度方差检测法判断行人足部运动状态,并用ZUPT表示足部运动状态;当足部处于离地状态时,记作ZUPT=0;当行人足部触地时,记作ZUPT=1,理论上认为此时足部与地面相对静止,将这一段相对静止的时间定义为“零速区间”;
(7.2)具体过程如下:
若k时刻,满足且则认为足部处于触地状态,记作ZUPT(k)=1,理论上认为此时足部与地面相对静止,即足部运动速度为0,并将这一段相对静止的时间称为“零速区间”;反之,则认为足部处于离地状态,记作ZUPT(m)=0,其中,γ1、γ2为零速检测阈值,σω为陀螺仪白噪声误差。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810084487.4A CN108362282B (zh) | 2018-01-29 | 2018-01-29 | 一种基于自适应零速区间调整的惯性行人定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810084487.4A CN108362282B (zh) | 2018-01-29 | 2018-01-29 | 一种基于自适应零速区间调整的惯性行人定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108362282A true CN108362282A (zh) | 2018-08-03 |
CN108362282B CN108362282B (zh) | 2020-10-27 |
Family
ID=63007224
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810084487.4A Active CN108362282B (zh) | 2018-01-29 | 2018-01-29 | 一种基于自适应零速区间调整的惯性行人定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108362282B (zh) |
Cited By (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109085376A (zh) * | 2018-08-20 | 2018-12-25 | 合肥优控科技有限公司 | 一种目标速度自适应估计方法 |
CN109374001A (zh) * | 2018-11-20 | 2019-02-22 | 中国电子科技集团公司第五十四研究所 | 一种结合行人运动场景约束的方位角校准算法 |
CN109540133A (zh) * | 2018-09-29 | 2019-03-29 | 中国科学院自动化研究所 | 基于微惯性技术的自适应步态划分方法、系统 |
CN109708647A (zh) * | 2019-03-08 | 2019-05-03 | 哈尔滨工程大学 | 一种基于融合特征元素的室内拓扑地图行人定位方法 |
CN109751997A (zh) * | 2018-12-26 | 2019-05-14 | 南京威惯传感科技有限公司 | 一种结合rfid标定的自主行人惯性定位系统 |
CN110455316A (zh) * | 2019-08-27 | 2019-11-15 | 哈尔滨工业大学 | 一种自适应零速区间检测方法 |
CN110553643A (zh) * | 2019-09-17 | 2019-12-10 | 电子科技大学 | 一种基于神经网络的行人自适应零速更新点选取方法 |
CN110579212A (zh) * | 2019-08-13 | 2019-12-17 | 湘潭大学 | 室内定位方法及装置 |
CN111024126A (zh) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | 一种行人导航定位中的自适应零速修正方法 |
CN111024070A (zh) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | 一种基于航向自观测的惯性足绑式行人定位方法 |
CN111272170A (zh) * | 2020-03-17 | 2020-06-12 | 电子科技大学 | 一种基于SoC的实时行人定位系统及方法 |
CN111735478A (zh) * | 2020-08-19 | 2020-10-02 | 中国人民解放军国防科技大学 | 基于lstm的行人实时导航零速检测方法 |
CN111904427A (zh) * | 2019-05-08 | 2020-11-10 | 株式会社比弗雷斯 | 通过头部加速度传感器的步行因素检测方法及装置 |
CN112762944A (zh) * | 2020-12-25 | 2021-05-07 | 上海商汤临港智能科技有限公司 | 零速区间检测及零速更新方法 |
CN113029136A (zh) * | 2021-03-12 | 2021-06-25 | 北京百度网讯科技有限公司 | 定位信息处理的方法、设备、存储介质及程序产品 |
CN113092819A (zh) * | 2021-04-14 | 2021-07-09 | 东方红卫星移动通信有限公司 | 足部加速度计动态零速校准方法及系统 |
CN114034297A (zh) * | 2021-10-22 | 2022-02-11 | 北京自动化控制设备研究所 | 一种自适应零速区间判定方法 |
CN114088090A (zh) * | 2021-12-03 | 2022-02-25 | 山东大学 | 一种足绑式行人足部零速度检测方法及系统 |
CN114279441A (zh) * | 2021-12-15 | 2022-04-05 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
CN114440867A (zh) * | 2021-12-17 | 2022-05-06 | 际络科技(上海)有限公司 | 重卡的零速检测方法及装置 |
WO2023108498A1 (zh) * | 2021-12-15 | 2023-06-22 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103616030A (zh) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | 基于捷联惯导解算和零速校正的自主导航系统定位方法 |
CN104296750A (zh) * | 2014-06-27 | 2015-01-21 | 大连理工大学 | 一种零速检测方法和装置以及行人导航方法和系统 |
CN104457781A (zh) * | 2014-12-22 | 2015-03-25 | 北京航空航天大学 | 一种基于单轴加速度计的自适应步数检测方法 |
CN105806340A (zh) * | 2016-03-17 | 2016-07-27 | 孙红星 | 一种基于窗口平滑的自适应零速更新算法 |
CN106908060A (zh) * | 2017-02-15 | 2017-06-30 | 东南大学 | 一种基于mems惯性传感器的高精度室内定位方法 |
-
2018
- 2018-01-29 CN CN201810084487.4A patent/CN108362282B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103616030A (zh) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | 基于捷联惯导解算和零速校正的自主导航系统定位方法 |
CN104296750A (zh) * | 2014-06-27 | 2015-01-21 | 大连理工大学 | 一种零速检测方法和装置以及行人导航方法和系统 |
CN104457781A (zh) * | 2014-12-22 | 2015-03-25 | 北京航空航天大学 | 一种基于单轴加速度计的自适应步数检测方法 |
CN105806340A (zh) * | 2016-03-17 | 2016-07-27 | 孙红星 | 一种基于窗口平滑的自适应零速更新算法 |
CN106908060A (zh) * | 2017-02-15 | 2017-06-30 | 东南大学 | 一种基于mems惯性传感器的高精度室内定位方法 |
Non-Patent Citations (1)
Title |
---|
SKOG ETAL.: "Zero-Velocity Detection—An Algorithm Evaluation", 《IEEE TRANSACTIONS ON BIOMEDICAL ENGINEERING 》 * |
Cited By (29)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109085376A (zh) * | 2018-08-20 | 2018-12-25 | 合肥优控科技有限公司 | 一种目标速度自适应估计方法 |
CN109085376B (zh) * | 2018-08-20 | 2020-09-18 | 东阳市维创工业产品设计有限公司 | 一种目标速度自适应估计方法 |
CN109540133A (zh) * | 2018-09-29 | 2019-03-29 | 中国科学院自动化研究所 | 基于微惯性技术的自适应步态划分方法、系统 |
CN109374001A (zh) * | 2018-11-20 | 2019-02-22 | 中国电子科技集团公司第五十四研究所 | 一种结合行人运动场景约束的方位角校准算法 |
CN109751997A (zh) * | 2018-12-26 | 2019-05-14 | 南京威惯传感科技有限公司 | 一种结合rfid标定的自主行人惯性定位系统 |
CN109708647A (zh) * | 2019-03-08 | 2019-05-03 | 哈尔滨工程大学 | 一种基于融合特征元素的室内拓扑地图行人定位方法 |
CN111904427A (zh) * | 2019-05-08 | 2020-11-10 | 株式会社比弗雷斯 | 通过头部加速度传感器的步行因素检测方法及装置 |
CN110579212A (zh) * | 2019-08-13 | 2019-12-17 | 湘潭大学 | 室内定位方法及装置 |
CN110579212B (zh) * | 2019-08-13 | 2022-11-29 | 湘潭大学 | 室内定位方法及装置 |
CN110455316A (zh) * | 2019-08-27 | 2019-11-15 | 哈尔滨工业大学 | 一种自适应零速区间检测方法 |
CN110553643A (zh) * | 2019-09-17 | 2019-12-10 | 电子科技大学 | 一种基于神经网络的行人自适应零速更新点选取方法 |
CN110553643B (zh) * | 2019-09-17 | 2021-12-21 | 电子科技大学 | 一种基于神经网络的行人自适应零速更新点选取方法 |
CN111024070A (zh) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | 一种基于航向自观测的惯性足绑式行人定位方法 |
CN111024126B (zh) * | 2019-12-26 | 2022-03-22 | 北京航天控制仪器研究所 | 一种行人导航定位中的自适应零速修正方法 |
CN111024126A (zh) * | 2019-12-26 | 2020-04-17 | 北京航天控制仪器研究所 | 一种行人导航定位中的自适应零速修正方法 |
CN111272170A (zh) * | 2020-03-17 | 2020-06-12 | 电子科技大学 | 一种基于SoC的实时行人定位系统及方法 |
CN111735478A (zh) * | 2020-08-19 | 2020-10-02 | 中国人民解放军国防科技大学 | 基于lstm的行人实时导航零速检测方法 |
CN111735478B (zh) * | 2020-08-19 | 2020-11-13 | 中国人民解放军国防科技大学 | 基于lstm的行人实时导航零速检测方法 |
CN112762944A (zh) * | 2020-12-25 | 2021-05-07 | 上海商汤临港智能科技有限公司 | 零速区间检测及零速更新方法 |
CN112762944B (zh) * | 2020-12-25 | 2024-03-22 | 上海商汤临港智能科技有限公司 | 零速区间检测及零速更新方法 |
CN113029136A (zh) * | 2021-03-12 | 2021-06-25 | 北京百度网讯科技有限公司 | 定位信息处理的方法、设备、存储介质及程序产品 |
CN113029136B (zh) * | 2021-03-12 | 2024-06-04 | 北京百度网讯科技有限公司 | 定位信息处理的方法、设备、存储介质及程序产品 |
CN113092819A (zh) * | 2021-04-14 | 2021-07-09 | 东方红卫星移动通信有限公司 | 足部加速度计动态零速校准方法及系统 |
CN114034297A (zh) * | 2021-10-22 | 2022-02-11 | 北京自动化控制设备研究所 | 一种自适应零速区间判定方法 |
CN114088090A (zh) * | 2021-12-03 | 2022-02-25 | 山东大学 | 一种足绑式行人足部零速度检测方法及系统 |
WO2023108498A1 (zh) * | 2021-12-15 | 2023-06-22 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
CN114279441B (zh) * | 2021-12-15 | 2023-12-29 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
CN114279441A (zh) * | 2021-12-15 | 2022-04-05 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
CN114440867A (zh) * | 2021-12-17 | 2022-05-06 | 际络科技(上海)有限公司 | 重卡的零速检测方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
CN108362282B (zh) | 2020-10-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108362282B (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
CN110207697B (zh) | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 | |
CN107289930B (zh) | 基于mems惯性测量单元的纯惯性车辆导航方法 | |
CN107560613B (zh) | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 | |
CN108007477B (zh) | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 | |
CN106342284B (zh) | 一种飞行载体姿态确定方法 | |
CN104406586B (zh) | 基于惯性传感器的行人导航装置和方法 | |
CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
CN111024070A (zh) | 一种基于航向自观测的惯性足绑式行人定位方法 | |
CN103196445B (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
CN107490378B (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
CN108132053B (zh) | 一种行人轨迹构建方法、系统及惯性测量装置 | |
CN106153069B (zh) | 自主导航系统中的姿态修正装置和方法 | |
CN106662443A (zh) | 用于垂直轨迹确定的方法和系统 | |
CN110207692B (zh) | 一种地图辅助的惯性预积分行人导航方法 | |
CN106643715A (zh) | 一种基于bp神经网络改善的室内惯性导航方法 | |
CN108981694A (zh) | 基于小波神经网络与ekf的姿态解算方法及系统 | |
CN104215262A (zh) | 一种惯性导航系统惯性传感器误差在线动态辨识方法 | |
CN110567492A (zh) | 低成本mems惯性传感器系统级标定方法 | |
CN109579832A (zh) | 一种人员高度自主定位算法 | |
CN107255474A (zh) | 一种融合电子罗盘和陀螺仪的pdr航向角确定方法 | |
WO2018214226A1 (zh) | 一种无人车实时姿态测量方法 | |
CN113340298A (zh) | 一种惯导和双天线gnss外参标定方法 | |
CN112362057A (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 |