CN108007477B - 一种基于正反向滤波的惯性行人定位***误差抑制方法 - Google Patents
一种基于正反向滤波的惯性行人定位***误差抑制方法 Download PDFInfo
- Publication number
- CN108007477B CN108007477B CN201711220837.7A CN201711220837A CN108007477B CN 108007477 B CN108007477 B CN 108007477B CN 201711220837 A CN201711220837 A CN 201711220837A CN 108007477 B CN108007477 B CN 108007477B
- Authority
- CN
- China
- Prior art keywords
- pedestrian
- carrier
- error
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000001914 filtration Methods 0.000 title claims abstract description 30
- 230000002441 reversible effect Effects 0.000 title claims abstract description 19
- 230000001629 suppression Effects 0.000 title claims abstract description 13
- 230000001133 acceleration Effects 0.000 claims abstract description 28
- 238000004364 calculation method Methods 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 48
- 230000008859 change Effects 0.000 claims description 16
- 230000009466 transformation Effects 0.000 claims description 14
- 238000001514 detection method Methods 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 7
- 230000001174 ascending effect Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 6
- 230000010354 integration Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000004321 preservation Methods 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 238000012937 correction Methods 0.000 description 11
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000005021 gait Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000000452 restraining effect Effects 0.000 description 2
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 230000002829 reductive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- 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)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
- Acyclic And Carbocyclic Compounds In Medicinal Compositions (AREA)
Abstract
本发明公开了一种基于正反向滤波的惯性行人定位***误差抑制方法,属于行人定位技术领域。将惯性器件固定在行人脚部,根据测量得的加速度和角速度解算行人的定位信息;同时,定位***检测并记录人脚部的运动状态,当脚部触地时,认为脚部处于“零速状态”,此时需储存加速度和角速度,零速状态结束后,计算零速区间长度;之后构造Kalman滤波器,对扩展后的零速区间内的数据进行正反向滤波,并利用滤波结果校正行人的定位信息,将补偿后的结果作为最终导航输出。本发明增强了不同运动速度下惯性行人定位***的适用性,减小了因行人运动速度加快而导致的误差补偿不充分的问题,并且不需任何外界辅助信息就可提高定位精度,计算量小,简单易操作。
Description
技术领域
本发明属于依托惯性传感器的行人定位技术领域,具体涉及一种基于正反向滤波的惯性行人定位***误差抑制方法。
背景技术
惯性行人定位***是一种利用以惯性组件为主的专用设备,可以为行人提供精确、实时位置信息。微惯性测量单元(Micro Inertial Measurement Unit,MIMU)是惯性行人定位***中最常用的惯性器件,它的核心是组件是加速度计和陀螺仪。惯性行人定位***是将MIMU直接安装在载体上,并将载体与行人足部固连,通过采集MIMU测量载体运动的线运动和角运动信息,经导航解算连续输出行人的速度、位置和姿态(俯仰角、横滚角、航向角)信息。由于惯性行人定位***不需要任何外界信息,所以是一种全自主定位***。由于其具有体积小、重量轻、易集成、无辐射等优点,在抢险救灾、单兵作战中等背景下具有极其重要的作用。但是,由于MIMU器件噪声的存在,***定位误差随时间发散而不断增大,这是制约惯性行人定位***长时间导航的重要因素之一。
为了提高***定位精度,一方面可以提高惯性元件精度,但是由于受加工技术水平的限制,无限制的提高元件精度是很难实现的;另一方面就是采取惯性行人定位***的误差抑制技术,自动抵消惯性器件的误差对***精度的影响。这样就可以应用现有精度的惯性元件构成较高精度的惯性行人定位***。
零速校正方法是一种惯性行人定位***的误差自补偿方法,该方法通过实时检测行人脚部的运动状态,当脚部与地面存在相对运动时,对脚部的位置、速度、姿态信息(简称位姿信息)进行解算;当脚部与地面相对静止时,对脚部位姿信息进行解算与误差补偿,来抵消器件噪声对***的影响,进而提高***定位精度。虽然零速校正方法能够较好的抑制行人正常行进时定位误差的发散,但是当行人行进速度加快时,由于脚部与地面相对静止时间缩短(约为0.05秒),即零速区间缩短,进而导致零速校正时间短,进而无法完全补偿定位误差,导致定位精度显著下降,无法满足行人定位的需求。
对于该方向的研究,在CNKI库中公开报道有:1.公告号为CN103616030A的中国发明专利在2014年3月5日公开的《基于捷联惯导解算和零速校正的自主导航***定位方法》,针对零速校正过程中部分观测量不可观的问题,设计了磁力计辅助零速校正算法的惯性行人定位方案。2.《测绘工程》2017年26卷第5期由朱彩杰等人撰写的《基于MEMS的室内定位误差修正方法研究》,该文章主要对消除行人运动过程中的无效振动进行了研究,针对行走过程中存在无效振动,导致MIMU输出信号中包含明显噪声的问题,提出了一种滤除MIMU输出信号中高频噪声以提高零速校正精度的方案。3.《中国惯性技术学报》2013年22卷第4期由黄志远等人撰写的《多条件约束的行人导航零速区间检测算法》,该文章主要针对MIMU存在漂移大、器件精度低的问题,在分析行人运动步态的基础上,设计了一种改进的零速区间检测算法。在SCI库中公开检索有:1.《Information Fusion》2017年由Sen Qiu等人撰写的《Inertial/magnetic sensors based pedestrian dead reckoning by means of multi-sensor fusion》,该文章主要提出了一种惯性行人定位***误差补偿技术,对MIMU安装误差和***噪声进行了研究。2.《Journal of Chemical Information&Computer Sciences》2016年44卷第5期由Yuan Xu等人撰写的《Two-mode navigation method for low-costinertial measurement unit-based indoor pedestrian navigation》,该文章提出了一种抑制惯性行人定位***误差传播的方案,对行人运动过程中不同的步态特征进行了分析,并建立了相应的数学模型。3.《IEEE Sensors Journal》2016年16卷第17期由ANorrdine等人撰写的《Step Detection for ZUPT-Aided Inertial PedestrianNavigation System Using Foot-Mounted Permanent Magnet》,该文章提出了一种改进的零速校正方法,分析了MIMU器件误差对惯性行人定位***精度的影响,并对MIMU器件误差进行了估计。以上文献都在于抑制行人慢速运动时的定位误差,并没有提及行人快速运动对***解算位姿信息精度及适用性的影响。
发明内容
本发明的目的在于提供可以提高定位精度,增强不同运动速度下***解算行人定位信息适用性的一种基于正反向滤波的惯性行人定位***误差抑制方法。
本发明的目的通过如下技术方案来实现:
在惯性行人定位***解算行人脚部位姿(位置、速度和姿态)的过程中实时检测行人脚部的零速状态,根据零速检测的结果,扩展零速区间,通过基于正反向滤波的零速校正方法对行人的位姿信息进行补偿。
一种基于正反向滤波惯性行人定位***定位误差抑制方法,包括以下步骤:
(1)将MIMU器件固定在行人脚部,打开电源并初始化***;
(2)***实时采集MIMU的输出数据;
(3)***实时检测并记录行人脚部的运动状态,用ZUPT(m)表示第m时刻脚步运动状态。当第m时刻脚部处于离地状态时,记作ZUPT(m)=0;当第m时刻行人脚部触地时,记作ZUPT(m)=1,理论上认为此时脚部与地面相对静止,即脚部运动速度为0,故将这一段相对静止的时间称为“零速区间”;
(4)行人定位***对MIMU(陀螺仪和加速度计)输出信息进行解算,由陀螺仪输出的角速度计算行人姿态信息,得到其中b表示载体坐标系,n表示导航坐标系,表示b系到n系的转换矩阵;利用将加速度计输出的加速度信息投影到导航坐标系后,有其中f表示加速度信息,fb表示载体坐标系下的加速度信息,fn表示导航坐标系下的加速度信息;加速度信息经过一次积分求解出速度信息:νn=∫fndt,再次积分求解出位置信息:pn=∫vndt;其中ν表示速度信息,p表示位置信息,pn表示导航坐标系下的位置信息,t表示时间;
(5)根据步骤(3)中零速检测的结果,如果当前时刻ZUPT=1,则转至步骤(6);如果当前时刻ZUPT=0,判断前N时刻是否一直有ZUPT=1,如果是,则转至步骤(7);如果不是,直接将步骤(4)中解算的行人位姿信息作为最终的导航输出;
(6)记录行人的加速度和角速度信息,并记录零速区间长度。
(7)读取步骤(6)中加速度f、角速度ω和零速区间长度L,并扩展零速区间;
(8):根据步骤(7)中加速度和角速度信息,解算行人的姿态ε和速度ν。由于理论上行人脚部触地时有ν=0,所以ν=δν,其中δν为速度误差。
(9)构造Kalman滤波器,以速度误差δν、位置误差δp和姿态误差δε为状态量,以速度误差为观测量,对零速区间内数据进行滤波;
(11)重复步骤(1)到步骤(10),直至惯性行人定位***导航结束。
本发明的有益效果在于:
本根据惯性行人定位***解算定位信息中位姿误差形式,在得出行人运动过程中的运动状态后,结合零速校正算法设计了正反向滤波算法,消除运动速度对误差补偿的影响,滤波后的定位信息作为最终导航解算输出信息。
增强了不同运动速度下惯性行人定位***的适用性,减小了因行人运动速度加快而导致的误差补偿不充分的问题;不需任何外界辅助信息就可提高定位精度;计算量小,简单易操作。
附图说明
图1是本发明的方法流程图;
图2为本发明中利用MATLAB仿真得到有无正反向滤波的定位轨迹比较曲线;
图3为行人预设路线。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明:
一种基于正反向滤波的惯性行人定位***误差抑制方法,方法流程如图1所示,包括以下步骤:
步骤一:将MIMU器件水平固定在行人脚部,其x、y、z轴分别指向行人的右、前、上方向。上电,运行。
导航初始时刻,需初始化***:(1)初始化惯性定位***初值:行人三轴位置信息 (单位均为m),三轴速度信息单位均为m/s),以及三个姿态角信息φ0、θ0、ψ0((单位均为rad)),初始转换矩阵初始四元数q0;(2)初始化常值参量:当地重力海拔h(单位为m),当地纬度λ(单位为rad),零速检测阈值γ,加速度计白噪声误差σa,陀螺仪白噪声误差σω,采样时间T(单位为s);(3)Kalman滤波器参数初值:状态变量初值均方误差阵P0,***噪声方差阵Q,量测噪声方差阵R,量测阵H;将以上初始化信息装订至导航计算机中。
初始四元数q0计算如下:
当地重力加速度计算如下:
g=9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10- 9sin2λ)×h+7.2×10-14×h2
其中,0为三行三列的零矩阵;
定位过程中,利用该初始信息进行更新,得到任意时刻行人的位置、速度和姿态信息;
步骤二:***实时采集MIMU的输出数据,包括加速度计输出的三轴加速度 分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为m/s2);陀螺仪输出的三轴角速度 分别表示陀螺仪测量的角速度在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为rads);
步骤三:***利用fb和ωb实时检测并记录行人脚部的运动状态,并用ZUPT表示运动状态。
在m时刻,若则认为脚部处于离地状态,记作ZUPT(m)=0;反之,则认为脚部处于触地状态,记作ZUPT(m)=1,理论上认为此时脚部与地面相对静止,即脚部运动速度为0,故将这一段相对静止的时间称为“零速区间”。其中,表示向量的2范数,γ为零速检测阈值,σa为加速度计白噪声误差,σω为陀螺仪白噪声误差;
步骤四:***根据步骤二中采集到的加速度fb和角速度ωb进行导航解算,由ωb计算行人姿态信息,得到利用将加速度计输出的加速度信息投影到导航坐标系后,有加速度信息经过一次积分求解出速度信息:νn=∫fndt,再次积分求解出位置信息:pn=∫vndt;其中ν表示速度信息,p表示位置信息,pn表示导航坐标系下的位置信息,t表示时间;
四元数姿态矩阵更新:
设任意时刻载体系相对导航坐标系的转动四元数为:
q=[q0 q1 q2 q3]T (1)
其中,q为四元数;q0为q的实数部分,q1、q2、q3为q的虚数部分。
四元数q的及时修正:
根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k),求取k时刻转动四元数的变化率为:
在k+1时刻载体的转动四元数具体为:
其中,(5)式中的qi(i=1,2,3,4)为(4)式中qi(k+1)(i=1,2,3,4),(5)式中省略了(k+1)。
更新载体姿态信息,具体为:
利用下列微分方程求解载体运动速度:
根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k),求取k时刻载体速度变化率为:
在k+1时刻载体速度和位置分别为:
其中,分别表示载***置在导航系oxn轴、oyn轴、ozn轴上的分量,当k=1时,vx(1)、vy(1)、vz(1)为步骤一中初始化***时获得的载体初始速度,为步骤一中初始化***时获得的载体初始位置。
至此,根据(6)、(10)、(11)式,得到了行人的姿态、速度、位置。
步骤五:记当前时刻为第k时刻,根据步骤三中零速检测的结果,如果ZUPT(k)=1,则转至步骤六;如果ZUPT(k)=0,判断前N时刻是否一直满足ZUPT(i)≡1,(i=k-N,k-N+1,…,k-1),如果是,表示前N时刻脚部处于零速状态,则转至步骤七;如果不是,表示前N时刻脚部不处于零速状态,直接将步骤四中解算的行人位姿信息作为最终的导航输出。N的一般取1~2;
步骤六:记录行人的加速度fb和角速度ωb,直到ZUPT=0停止记录,并计算零速区间长度L。
步骤七:读取步骤六中零速区间长度L,读取区间内的数据f、ω;记第i组数据为ui=[fi ωi]T,则零速区间中数据可写作U=[u1 u2 … uL];
理论上行人脚部触地时,位置保持不变,k+1时刻载体速度和位置分别为:
步骤九:对Data进行正反向滤波,滤波状态量为X=[δpn δνn δεn],观测量为z=δνn,滤波过程如下:
利用下列微分方程建立载体运动速度、位置和姿态的误差模型:
其中:δp为载***置误差,且δν为载体速度误差,且δε为载体姿态角误差并且有δε=[δφ δθ δψ]。分别为δp,δν,δε的变化率,ωb为载体角速度,且[fn×]为由加速度信息构成的反对称阵,且03×1为三行一列的零向量。
根据k时刻的载***置误差δp(k),载体速度误差δν(k),载体姿态角误差δε(k),求取k时刻载***置误差变化率、速度误差变化率、姿态角误差变化率为:
在k+1时刻载***置误差δp(k+1)、速度误差δν(k+1)、姿态角误差δε(k+1)分别为:
其中,式(17)和式(18)中省去了上角标n。
由于理论上行人脚部触地时,应有ν=0,故此时***解算出的速度信息ν为误差量,即ν=δν,其中δν为速度误差。
根据上述误差模型,利用卡尔曼滤波器,对行人的位姿信息进行误差修正,具体算法如下:
Kk=PkHT(HPkHT+R)-1 (18)
Xk=-KkZk (19)
更新Pk:
保存Pk与Xk,并将其作为下一次滤波的输入。
式(17)应当在整个定位过程中实时结算,而非只针对零速区间。
位置、速度信息补偿:
姿态角补偿:
更新四元数:
步骤十一:重复步骤一到步骤十,直至惯性行人定位***导航结束。
实施例:
对本发明的有益效果如下方式得以验证:
采用MTi-710系列MIMU构建惯性行人定位***,进行惯性行人定位***的快速行走试验。根据图3,其中,黑色方框为行人起始位置。
***初始化参数如下:
当地重力海拔:h=100m;
当地纬度:λ=0.7988rad;
采样时间:T=0.01s;
加速度计白噪声误差:σa=0.008;
陀螺仪白噪声误差:σω=0.00506;
零速检测阈值:γ=30000;
最短零速区间长度:N=2;
零速区间扩展倍数:s=5;
Kalman滤波器初始参数:
初始均方误差阵:
***噪声方差阵:
利用发明所述方法,得到行人快速行走时有、无正反向滤波的定位轨迹比较曲线,其中(a)、(b)图分别表示有正反向滤波的定位轨迹曲线,无正反向滤波的定位轨迹曲线。结果表明本发明抑制行人快速运动时定位误差能力较好,可以满足实际需求。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (6)
1.一种基于正反向滤波的惯性行人定位***误差抑制方法,其特征在于,包括以下步骤:
(1)将惯性器件固定在行人脚部,打开电源并初始化***;
(2)***实时采集惯性器件的输出数据;
(3)***实时检测并记录行人脚部的运动状态,用ZUPT(m)表示第m时刻脚步运动状态,当第m时刻脚部处于离地状态时,记作ZUPT(m)=0;当第m时刻行人脚部触地时,记作ZUPT(m)=1,理论上认为此时脚部与地面相对静止,即脚部运动速度为0,故将这一段相对静止的时间称为零速区间;
(4)行人定位***对惯性器件输出信息进行解算,由陀螺仪输出的角速度计算行人姿态信息,得到其中b表示载体坐标系,n表示导航坐标系,表示b系到n系的转换矩阵;利用将加速度计输出的加速度信息投影到导航坐标系后,有其中f表示加速度信息,fb表示载体坐标系下的加速度信息,fn表示导航坐标系下的加速度信息;加速度信息经过一次积分求解出速度信息:νn=∫fndt,再次积分求解出位置信息:pn=∫vndt;其中ν表示速度信息,p表示位置信息,pn表示导航坐标系下的位置信息,t表示时间;
(5)根据步骤(3)中零速检测的结果,如果当前时刻ZUPT=1,则转至步骤(6);如果当前时刻ZUPT=0,判断前N时刻是否一直有ZUPT=1,如果是,则转至步骤(7);如果不是,直接将步骤(4)中解算的行人位姿信息作为最终的导航输出;
(6)记录行人的加速度和角速度信息,并记录零速区间长度;
(7)读取步骤(6)中加速度f、角速度ω和零速区间长度L,并扩展零速区间;
(8)根据步骤(7)中加速度和角速度信息,解算行人的姿态ε和速度ν,理论上行人脚部触地时有ν=0,ν=δν,其中δν为速度误差;
(9)构造Kalman滤波器,以速度误差δν、位置误差δp和姿态误差δε为状态量,以速度误差为观测量,对零速区间内数据进行滤波;
(11)重复步骤(1)到步骤(10),直至惯性行人定位***导航结束;
所述的步骤(7)具体为:
读取步骤(6)中零速区间长度L,读取区间内的数据f、ω;记第i组数据为ui=[fi ωi]T,则零速区间中数据可写作U=[u1 u2 … uL];
2.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法,其特征在于,所述的步骤(1)具体为:
导航初始时刻,需初始化***:(1.1)初始化惯性定位***初值:行人三轴位置信息 三轴速度信息以及三个姿态角信息φ0、θ0、ψ0,初始转换矩阵初始四元数q0;(1.2)初始化常值参量:当地重力海拔h,当地纬度λ,零速检测阈值γ,加速度计白噪声误差σa,陀螺仪白噪声误差σω,采样时间T(1.3)Kalman滤波器参数初值:状态变量初值均方误差阵P0,***噪声方差阵Q,量测噪声方差阵R,量测阵H;
初始四元数q0计算如下:
当地重力加速度计算如下:
g=9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10-9sin2λ)×h+7.2×10-14×h2
其中,0为三行三列的零矩阵;
3.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法,其特征在于,所述的步骤(4)具体为:
四元数姿态矩阵更新:
设任意时刻载体系相对导航坐标系的转动四元数为:
q=[q0 q1 q2 q3]T (1)
其中,q为四元数;q0为q的实数部分,q1、q2、q3为q的虚数部分;
四元数q的及时修正:
根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k),求取k时刻转动四元数的变化率为:
在k+1时刻载体的转动四元数具体为:
其中,(5)式中的qi(i=1,2,3,4)为(4)式中qi(k+1)(i=1,2,3,4),(5)式中省略(k+1);
更新载体姿态信息,具体为:
利用下列微分方程求解载体运动速度:
根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k),求取k时刻载体速度变化率为:
在k+1时刻载体速度和位置分别为:
5.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法,其特征在于,所述的步骤(9)具体为:
对Data进行正反向滤波,滤波状态量为X=[δpn δνn δεn],观测量为z=δνn,滤波过程如下:
利用下列微分方程建立载体运动速度、位置和姿态的误差模型:
其中:δp为载***置误差,且δν为载体速度误差,且δε为载体姿态角误差并且有δε=[δφ δθ δψ];分别为δp,δν,δε的变化率,ωb为载体角速度,且[fn×]为由加速度信息构成的反对称阵,且03×1为三行一列的零向量;
根据k时刻的载***置误差δp(k),载体速度误差δν(k),载体姿态角误差δε(k),求取k时刻载***置误差变化率、速度误差变化率、姿态角误差变化率为:
在k+1时刻载***置误差δp(k+1)、速度误差δν(k+1)、姿态角误差δε(k+1)分别为:
由于理论上行人脚部触地时,应有ν=0,故此时***解算出的速度信息ν为误差量,即ν=δν,其中δν为速度误差;
根据上述误差模型,利用卡尔曼滤波器,对行人的位姿信息进行误差修正,具体算法如下:
Kk=PkHT(HPkHT+R)-1
Xk=-KkZk
更新Pk:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711220837.7A CN108007477B (zh) | 2017-11-29 | 2017-11-29 | 一种基于正反向滤波的惯性行人定位***误差抑制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711220837.7A CN108007477B (zh) | 2017-11-29 | 2017-11-29 | 一种基于正反向滤波的惯性行人定位***误差抑制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108007477A CN108007477A (zh) | 2018-05-08 |
CN108007477B true CN108007477B (zh) | 2020-09-25 |
Family
ID=62054570
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711220837.7A Active CN108007477B (zh) | 2017-11-29 | 2017-11-29 | 一种基于正反向滤波的惯性行人定位***误差抑制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108007477B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109855620B (zh) * | 2018-12-26 | 2020-10-09 | 北京壹氢科技有限公司 | 一种基于自回溯算法的室内行人导航方法 |
CN110274592B (zh) * | 2019-07-18 | 2021-07-27 | 北京航空航天大学 | 一种腰部脚部惯性测量单元信息融合的零速区间确定方法 |
CN111024070A (zh) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | 一种基于航向自观测的惯性足绑式行人定位方法 |
CN111189443B (zh) * | 2020-01-14 | 2022-11-11 | 电子科技大学 | 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法 |
CN111521179B (zh) * | 2020-04-28 | 2021-01-15 | 中国人民解放军国防科技大学 | 基于行进速度自检测的钻探用定位定向仪孔内定位方法 |
CN112762944B (zh) * | 2020-12-25 | 2024-03-22 | 上海商汤临港智能科技有限公司 | 零速区间检测及零速更新方法 |
CN112781622B (zh) * | 2020-12-31 | 2022-07-05 | 厦门华源嘉航科技有限公司 | 一种行人导航mimu安装误差在线标定方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2014147544A1 (en) * | 2013-03-21 | 2014-09-25 | Sony Corporation | Recalibrating an inertial navigation system |
CN106225786A (zh) * | 2016-08-15 | 2016-12-14 | 北京理工大学 | 一种自适应的行人导航***零速区间检测方法 |
-
2017
- 2017-11-29 CN CN201711220837.7A patent/CN108007477B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2014147544A1 (en) * | 2013-03-21 | 2014-09-25 | Sony Corporation | Recalibrating an inertial navigation system |
CN106225786A (zh) * | 2016-08-15 | 2016-12-14 | 北京理工大学 | 一种自适应的行人导航***零速区间检测方法 |
Non-Patent Citations (1)
Title |
---|
Research on Indoor Pedestrian Location Based on Miniature Inertial Measurement Unit;郭铮等;《2017 Forum on Cooperative Positioning and Service (CPGPS)》;20171019;第289-293页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108007477A (zh) | 2018-05-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108007477B (zh) | 一种基于正反向滤波的惯性行人定位***误差抑制方法 | |
CN108362282B (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
Tong et al. | A double-step unscented Kalman filter and HMM-based zero-velocity update for pedestrian dead reckoning using MEMS sensors | |
WO2020220729A1 (zh) | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 | |
CN105300379B (zh) | 一种基于加速度的卡尔曼滤波姿态估计方法及*** | |
CN107560613B (zh) | 基于九轴惯性传感器的机器人室内轨迹跟踪***及方法 | |
CN111024070A (zh) | 一种基于航向自观测的惯性足绑式行人定位方法 | |
CN103196445B (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
CN110207692B (zh) | 一种地图辅助的惯性预积分行人导航方法 | |
CN102445200A (zh) | 微小型个人组合导航***及其导航定位方法 | |
CN106662443A (zh) | 用于垂直轨迹确定的方法和*** | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及*** | |
CN112683269B (zh) | 一种附有运动加速度补偿的marg姿态计算方法 | |
CN106153069B (zh) | 自主导航***中的姿态修正装置和方法 | |
JP2007232443A (ja) | 慣性航法装置およびその誤差補正方法 | |
CN110672095A (zh) | 一种基于微惯导的行人室内自主定位算法 | |
CN113340298A (zh) | 一种惯导和双天线gnss外参标定方法 | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN111121820B (zh) | 基于卡尔曼滤波的mems惯性传感器阵列融合方法 | |
RU2382988C1 (ru) | Бесплатформенная инерциальная система ориентации на "грубых" чувствительных элементах | |
CN111141285B (zh) | 一种航空重力测量装置 | |
RU2487318C1 (ru) | Бесплатформенная инерциальная курсовертикаль на чувствительных элементах средней точности | |
CN103256932B (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 |