CN108007477B - 一种基于正反向滤波的惯性行人定位***误差抑制方法 - Google Patents

一种基于正反向滤波的惯性行人定位***误差抑制方法 Download PDF

Info

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
Application number
CN201711220837.7A
Other languages
English (en)
Other versions
CN108007477A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201711220837.7A priority Critical patent/CN108007477B/zh
Publication of CN108007477A publication Critical patent/CN108007477A/zh
Application granted granted Critical
Publication of CN108007477B publication Critical patent/CN108007477B/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
    • 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(陀螺仪和加速度计)输出信息进行解算,由陀螺仪输出的角速度计算行人姿态信息，得到
Figure BDA0001486361140000031
其中b表示载体坐标系，n表示导航坐标系，
Figure BDA0001486361140000032
表示b系到n系的转换矩阵；利用
Figure BDA0001486361140000033
将加速度计输出的加速度信息投影到导航坐标系后，有
Figure BDA0001486361140000034
其中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和姿态误差δε为状态量，以速度误差为观测量，对零速区间内数据进行滤波；
(10)利用步骤(9)中解算出的δν，δp，δε补偿
Figure BDA0001486361140000035
并将补偿后的结果作为最终导航输出，其中，
Figure BDA0001486361140000036
分别为***解算的ν，p，ε的值；
(11)重复步骤(1)到步骤(10)，直至惯性行人定位***导航结束。
本发明的有益效果在于：
本根据惯性行人定位***解算定位信息中位姿误差形式，在得出行人运动过程中的运动状态后，结合零速校正算法设计了正反向滤波算法，消除运动速度对误差补偿的影响，滤波后的定位信息作为最终导航解算输出信息。
增强了不同运动速度下惯性行人定位***的适用性，减小了因行人运动速度加快而导致的误差补偿不充分的问题；不需任何外界辅助信息就可提高定位精度；计算量小，简单易操作。
附图说明
图1是本发明的方法流程图；
图2为本发明中利用MATLAB仿真得到有无正反向滤波的定位轨迹比较曲线；
图3为行人预设路线。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明：
一种基于正反向滤波的惯性行人定位***误差抑制方法，方法流程如图1所示，包括以下步骤：
步骤一：将MIMU器件水平固定在行人脚部，其x、y、z轴分别指向行人的右、前、上方向。上电，运行。
导航初始时刻，需初始化***：(1)初始化惯性定位***初值：行人三轴位置信息
Figure BDA00014863611400000416
Figure BDA0001486361140000041
(单位均为m)，三轴速度信息
Figure BDA0001486361140000042
单位均为m/s)，以及三个姿态角信息φ0、θ0、ψ0((单位均为rad))，初始转换矩阵
Figure BDA0001486361140000043
初始四元数q0；(2)初始化常值参量：当地重力海拔h(单位为m)，当地纬度λ(单位为rad)，零速检测阈值γ，加速度计白噪声误差σa，陀螺仪白噪声误差σω，采样时间T(单位为s)；(3)Kalman滤波器参数初值：状态变量初值
Figure BDA0001486361140000044
均方误差阵P0，***噪声方差阵Q，量测噪声方差阵R，量测阵H；将以上初始化信息装订至导航计算机中。
通常情况下，
Figure BDA0001486361140000045
姿态角φ0、θ0计算如下：
Figure BDA0001486361140000046
其中，φ0表示横滚角初始值，θ0表示俯仰角初始值，
Figure BDA0001486361140000047
分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量。
初始转换矩阵
Figure BDA0001486361140000048
计算如下：
Figure BDA0001486361140000049
其中，b表示载体坐标系，n表示导航坐标系，
Figure BDA00014863611400000410
表示b系到n系的转换矩阵。
初始四元数q0计算如下：
令
Figure BDA00014863611400000411
则
Figure BDA00014863611400000412
其中，cij(i＝1,2,3，j＝1,2,3)表示
Figure BDA00014863611400000413
中第i行第j列矩阵元素，
Figure BDA00014863611400000414
表示矩阵的转置；
当地重力加速度计算如下：
g＝9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10- 9sin2λ)×h+7.2×10-14×h2
Figure BDA00014863611400000415
其中，0为三行三列的零矩阵；
Figure BDA0001486361140000051
其余初值需根据实际情况设定；
定位过程中，利用该初始信息进行更新，得到任意时刻行人的位置、速度和姿态信息；
步骤二：***实时采集MIMU的输出数据，包括加速度计输出的三轴加速度
Figure BDA0001486361140000052
Figure BDA0001486361140000053
分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为m/s2)；陀螺仪输出的三轴角速度
Figure BDA0001486361140000054
Figure BDA0001486361140000055
分别表示陀螺仪测量的角速度在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为rads)；
步骤三：***利用fb和ωb实时检测并记录行人脚部的运动状态，并用ZUPT表示运动状态。
在m时刻，若
Figure BDA0001486361140000056
则认为脚部处于离地状态，记作ZUPT(m)＝0；反之，则认为脚部处于触地状态，记作ZUPT(m)＝1，理论上认为此时脚部与地面相对静止，即脚部运动速度为0，故将这一段相对静止的时间称为“零速区间”。其中，
Figure BDA0001486361140000057
表示向量的2范数，γ为零速检测阈值，σa为加速度计白噪声误差，σω为陀螺仪白噪声误差；
步骤四：***根据步骤二中采集到的加速度fb和角速度ωb进行导航解算，由ωb计算行人姿态信息，得到
Figure BDA0001486361140000058
利用
Figure BDA0001486361140000059
将加速度计输出的加速度信息投影到导航坐标系后，有
Figure BDA00014863611400000510
加速度信息经过一次积分求解出速度信息：νn＝∫fndt，再次积分求解出位置信息：pn＝∫vndt；其中ν表示速度信息，p表示位置信息，pn表示导航坐标系下的位置信息，t表示时间；
在行人运动过程中，通过ωb更新转换矩阵
Figure BDA00014863611400000513
具体为：
四元数姿态矩阵更新：
设任意时刻载体系相对导航坐标系的转动四元数为：
q＝[q0 q1 q2 q3]T (1)
其中，q为四元数；q0为q的实数部分，q1、q2、q3为q的虚数部分。
四元数q的及时修正：
Figure BDA00014863611400000511
其中，
Figure BDA00014863611400000512
分别表示q0、q1、q2、q3的变化率；
根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k)，求取k时刻转动四元数的变化率为：
Figure BDA0001486361140000061
在k+1时刻载体的转动四元数具体为：
Figure BDA0001486361140000062
其中，I为单位阵，T为采样时间，
Figure BDA0001486361140000063
(4)式中ωx、ωy、ωz均省略了上角标b，当k＝1时，q(k)为步骤一中初始化***时获得的载体初始四元数。
利用得到的q(k+1)中元素q0(k+1)、q1(k+1)、q2(k+1)、q3(k+1)，更新捷联矩阵
Figure BDA0001486361140000064
Figure BDA0001486361140000065
其中，(5)式中的qi(i＝1,2,3,4)为(4)式中qi(k+1)(i＝1,2,3,4)，(5)式中省略了(k+1)。
更新载体姿态信息，具体为：
Figure BDA0001486361140000066
将加速度计沿载体坐标系测量的比力信息，通过捷联矩阵
Figure BDA0001486361140000067
进行投影转换：
Figure BDA0001486361140000068
利用下列微分方程求解载体运动速度：
Figure BDA0001486361140000069
其中，
Figure BDA00014863611400000610
分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量；
Figure BDA00014863611400000611
Figure BDA00014863611400000612
表示vx、vy、vz的变化率，即载体沿导航系oxn轴、oyn轴、ozn轴的运动加速度；g为当地重力加速度。
根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k)，求取k时刻载体速度变化率为：
Figure BDA0001486361140000071
在k+1时刻载体速度和位置分别为：
Figure BDA0001486361140000072
Figure BDA0001486361140000073
其中，
Figure BDA0001486361140000074
分别表示载***置在导航系oxn轴、oyn轴、ozn轴上的分量，当k＝1时，vx(1)、vy(1)、vz(1)为步骤一中初始化***时获得的载体初始速度，
Figure BDA0001486361140000075
为步骤一中初始化***时获得的载体初始位置。
至此，根据(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]；
记D＝[-uL -uL-1 … -u1]，扩展零速区间至sL，得到
Figure BDA0001486361140000076
一般取s＝5，则有Data＝[U D U D U]；
步骤八：根据步骤七中Data，解算行人的速度
Figure BDA0001486361140000077
位置
Figure BDA0001486361140000078
和姿态
Figure BDA0001486361140000079
Figure BDA00014863611400000710
和转换矩阵
Figure BDA00014863611400000711
的计算与步骤四相同；
理论上行人脚部触地时，位置保持不变，k+1时刻载体速度和位置分别为：
Figure BDA00014863611400000712
Figure BDA00014863611400000713
其中，
Figure BDA00014863611400000714
分别为ν，p，ε，
Figure BDA00014863611400000715
的计算值。
步骤九：对Data进行正反向滤波，滤波状态量为X＝[δpn δνn δεn]，观测量为z＝δνn，滤波过程如下：
利用下列微分方程建立载体运动速度、位置和姿态的误差模型：
Figure BDA0001486361140000081
Figure BDA0001486361140000082
Figure BDA0001486361140000083
其中：δp为载***置误差，且
Figure BDA0001486361140000084
δν为载体速度误差，且
Figure BDA0001486361140000085
δε为载体姿态角误差并且有δε＝[δφ δθ δψ]。
Figure BDA0001486361140000086
分别为δp，δν，δε的变化率，ωb为载体角速度，且
Figure BDA0001486361140000087
[fn×]为由加速度信息构成的反对称阵，且
Figure BDA0001486361140000088
03×1为三行一列的零向量。
根据k时刻的载***置误差δp(k)，载体速度误差δν(k)，载体姿态角误差δε(k)，求取k时刻载***置误差变化率、速度误差变化率、姿态角误差变化率为：
Figure BDA0001486361140000089
在k+1时刻载***置误差δp(k+1)、速度误差δν(k+1)、姿态角误差δε(k+1)分别为：
Figure BDA00014863611400000810
其中，式(17)和式(18)中省去了上角标n。
由于理论上行人脚部触地时，应有ν＝0，故此时***解算出的速度信息ν为误差量，即ν＝δν，其中δν为速度误差。
根据上述误差模型，利用卡尔曼滤波器，对行人的位姿信息进行误差修正，具体算法如下：
Figure BDA00014863611400000811
Kk＝PkHT(HPkHT+R)-1 (18)
其中，Pk为k时刻的估计均方误差阵，Fk,k-1为k-1时刻到k时刻的为状态转移矩阵，且
Figure BDA0001486361140000091
I为单位阵，0为零矩阵，
Figure BDA0001486361140000092
为k-1时刻的噪声驱动阵，Kk为k时刻的滤波增益阵。
Xk＝-KkZk (19)
更新Pk：
Figure BDA0001486361140000093
其中，Xk为k时刻的状态变量，当k＝1时，状态量X＝[δp(1) δν(1) δε(1)]为步骤一中初始化***时获得的载体初始状态量X0，
Figure BDA0001486361140000094
为更新后的Pk。
保存Pk与Xk，并将其作为下一次滤波的输入。
式(17)应当在整个定位过程中实时结算，而非只针对零速区间。
步骤十：利用步骤九中解算出的δν，δp，δε补偿步骤八中的
Figure BDA0001486361140000095
更新四元数qk，并将补偿后的结果作为最终导航输出：
位置、速度信息补偿：
Figure BDA0001486361140000096
姿态角补偿：
令
Figure BDA0001486361140000097
利用公式
Figure BDA0001486361140000098
更新转换矩阵
Figure BDA0001486361140000099
利用式(6)求出更新后的φ，θ，ψ；
其中，
Figure BDA00014863611400000910
为矩阵的逆运算。
更新四元数：
令
Figure BDA00014863611400000911
则
Figure BDA00014863611400000912
步骤十一：重复步骤一到步骤十，直至惯性行人定位***导航结束。
实施例：
对本发明的有益效果如下方式得以验证：
采用MTi-710系列MIMU构建惯性行人定位***，进行惯性行人定位***的快速行走试验。根据图3，其中，黑色方框为行人起始位置。
***初始化参数如下：
载体三轴位置信息：
Figure BDA0001486361140000101
载体三轴速度信息：
Figure BDA0001486361140000102
载体航向角信息：ψ0＝0rad；
当地重力海拔：h＝100m；
当地纬度：λ＝0.7988rad；
采样时间：T＝0.01s；
加速度计白噪声误差：σa＝0.008；
陀螺仪白噪声误差：σω＝0.00506；
零速检测阈值：γ＝30000；
最短零速区间长度：N＝2；
零速区间扩展倍数：s＝5；
Kalman滤波器初始参数：
初始状态量：
Figure BDA0001486361140000103
初始均方误差阵：
Figure BDA0001486361140000104
***噪声方差阵：
Figure BDA0001486361140000105
量测噪声方差阵：
Figure BDA0001486361140000111
利用发明所述方法，得到行人快速行走时有、无正反向滤波的定位轨迹比较曲线，其中(a)、(b)图分别表示有正反向滤波的定位轨迹曲线，无正反向滤波的定位轨迹曲线。结果表明本发明抑制行人快速运动时定位误差能力较好，可以满足实际需求。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于正反向滤波的惯性行人定位***误差抑制方法，其特征在于，包括以下步骤：
(1)将惯性器件固定在行人脚部，打开电源并初始化***；
(2)***实时采集惯性器件的输出数据；
(3)***实时检测并记录行人脚部的运动状态，用ZUPT(m)表示第m时刻脚步运动状态，当第m时刻脚部处于离地状态时，记作ZUPT(m)＝0；当第m时刻行人脚部触地时，记作ZUPT(m)＝1，理论上认为此时脚部与地面相对静止，即脚部运动速度为0，故将这一段相对静止的时间称为零速区间；
(4)行人定位***对惯性器件输出信息进行解算,由陀螺仪输出的角速度计算行人姿态信息，得到
Figure FDA0002535103580000011
其中b表示载体坐标系，n表示导航坐标系，
Figure FDA0002535103580000012
表示b系到n系的转换矩阵；利用
Figure FDA0002535103580000013
将加速度计输出的加速度信息投影到导航坐标系后，有
Figure FDA0002535103580000014
其中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和姿态误差δε为状态量，以速度误差为观测量，对零速区间内数据进行滤波；
(10)利用步骤(9)中解算出的δν，δp，δε补偿
Figure FDA0002535103580000015
并将补偿后的结果作为最终导航输出，其中，
Figure FDA0002535103580000016
分别为***解算的ν，p，ε的值；
(11)重复步骤(1)到步骤(10)，直至惯性行人定位***导航结束；
所述的步骤(7)具体为：
读取步骤(6)中零速区间长度L，读取区间内的数据f、ω；记第i组数据为ui＝[fi ωi]T，则零速区间中数据可写作U＝[u1 u2 … uL]；
记D＝[-uL -uL-1 … -u1]，扩展零速区间至sL，得到
Figure FDA0002535103580000021
一般取s＝5，则有Data＝[U D U D U]。
2.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法，其特征在于，所述的步骤(1)具体为：
导航初始时刻，需初始化***：(1.1)初始化惯性定位***初值：行人三轴位置信息
Figure FDA0002535103580000022
Figure FDA0002535103580000023
三轴速度信息
Figure FDA0002535103580000024
以及三个姿态角信息φ0、θ0、ψ0，初始转换矩阵
Figure FDA00025351035800000216
初始四元数q0；(1.2)初始化常值参量：当地重力海拔h，当地纬度λ，零速检测阈值γ，加速度计白噪声误差σa，陀螺仪白噪声误差σω，采样时间T(1.3)Kalman滤波器参数初值：状态变量初值
Figure FDA0002535103580000025
均方误差阵P0，***噪声方差阵Q，量测噪声方差阵R，量测阵H；
通常情况下，
Figure FDA0002535103580000026
姿态角φ0、θ0计算如下：
Figure FDA0002535103580000027
其中，φ0表示横滚角初始值，θ0表示俯仰角初始值，
Figure FDA0002535103580000028
分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量；
初始转换矩阵
Figure FDA0002535103580000029
计算如下：
Figure FDA00025351035800000210
其中，b表示载体坐标系，n表示导航坐标系，
Figure FDA00025351035800000211
表示b系到n系的转换矩阵；
初始四元数q0计算如下：
令
Figure FDA00025351035800000212
则
Figure FDA00025351035800000213
其中，cij表示
Figure FDA00025351035800000214
中第i行第j列矩阵元素，[·]T表示矩阵的转置，i＝1,2,3，j＝1,2,3；
当地重力加速度计算如下：
g＝9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10-9sin2λ)×h+7.2×10-14×h2
Figure FDA00025351035800000215
其中，0为三行三列的零矩阵；
Figure FDA0002535103580000031
其余初值需根据实际情况设定。
3.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法，其特征在于，所述的步骤(4)具体为：
在行人运动过程中，通过ωb更新转换矩阵
Figure FDA0002535103580000032
具体为：
四元数姿态矩阵更新：
设任意时刻载体系相对导航坐标系的转动四元数为：
q＝[q0 q1 q2 q3]T (1)
其中，q为四元数；q0为q的实数部分，q1、q2、q3为q的虚数部分；
四元数q的及时修正：
Figure FDA0002535103580000033
其中，
Figure FDA0002535103580000034
分别表示q0、q1、q2、q3的变化率；
根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k)，求取k时刻转动四元数的变化率为：
Figure FDA0002535103580000035
在k+1时刻载体的转动四元数具体为：
Figure FDA0002535103580000036
其中，I为单位阵，T为采样时间，
Figure FDA0002535103580000037
(4)式中ωx、ωy、ωz均省略了上角标b，当k＝1时，q(k)为步骤(1)中初始化***时获得的载体初始四元数；
利用得到的q(k+1)中元素q0(k+1)、q1(k+1)、q2(k+1)、q3(k+1)，更新捷联矩阵
Figure FDA0002535103580000038
Figure FDA0002535103580000041
其中，(5)式中的qi(i＝1,2,3,4)为(4)式中qi(k+1)(i＝1,2,3,4)，(5)式中省略(k+1)；
更新载体姿态信息，具体为：
Figure FDA0002535103580000042
将加速度计沿载体坐标系测量的比力信息，通过捷联矩阵
Figure FDA0002535103580000043
进行投影转换：
Figure FDA0002535103580000044
利用下列微分方程求解载体运动速度：
Figure FDA0002535103580000045
其中，
Figure FDA0002535103580000046
分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量；
Figure FDA0002535103580000047
Figure FDA0002535103580000048
表示vx、vy、vz的变化率，即载体沿导航系oxn轴、oyn轴、ozn轴的运动加速度；g为当地重力加速度；
根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k)，求取k时刻载体速度变化率为：
Figure FDA0002535103580000049
在k+1时刻载体速度和位置分别为：
Figure FDA00025351035800000410
Figure FDA00025351035800000411
其中，
Figure FDA00025351035800000412
分别表示载***置在导航系oxn轴、oyn轴、ozn轴上的分量，当k＝1时，vx(1)、vy(1)、vz(1)为步骤(1)中初始化***时获得的载体初始速度，
Figure FDA00025351035800000413
为步骤(1)中初始化***时获得的载体初始位置。
4.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法，其特征在于，所述的步骤(8)具体为：
根据步骤(7)中Data，解算行人的速度
Figure FDA00025351035800000414
位置
Figure FDA00025351035800000415
和姿态
Figure FDA00025351035800000416
Figure FDA0002535103580000051
和转换矩阵
Figure FDA0002535103580000052
的计算与步骤(4)相同；
理论上行人脚部触地时，位置保持不变，k+1时刻载体速度和位置分别为：
Figure FDA0002535103580000053
Figure FDA0002535103580000054
其中，
Figure FDA0002535103580000055
分别为ν，p，ε，
Figure FDA0002535103580000056
的计算值。
5.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法，其特征在于，所述的步骤(9)具体为：
对Data进行正反向滤波，滤波状态量为X＝[δpn δνn δεn]，观测量为z＝δνn，滤波过程如下：
利用下列微分方程建立载体运动速度、位置和姿态的误差模型：
Figure FDA0002535103580000057
Figure FDA0002535103580000058
Figure FDA0002535103580000059
其中：δp为载***置误差，且
Figure FDA00025351035800000510
δν为载体速度误差，且
Figure FDA00025351035800000511
δε为载体姿态角误差并且有δε＝[δφ δθ δψ]；
Figure FDA00025351035800000512
分别为δp，δν，δε的变化率，ωb为载体角速度，且
Figure FDA00025351035800000513
[fn×]为由加速度信息构成的反对称阵，且
Figure FDA00025351035800000514
03×1为三行一列的零向量；
根据k时刻的载***置误差δp(k)，载体速度误差δν(k)，载体姿态角误差δε(k)，求取k时刻载***置误差变化率、速度误差变化率、姿态角误差变化率为：
Figure FDA00025351035800000515
在k+1时刻载***置误差δp(k+1)、速度误差δν(k+1)、姿态角误差δε(k+1)分别为：
Figure FDA00025351035800000516
由于理论上行人脚部触地时，应有ν＝0，故此时***解算出的速度信息ν为误差量，即ν＝δν，其中δν为速度误差；
根据上述误差模型，利用卡尔曼滤波器，对行人的位姿信息进行误差修正，具体算法如下：
Figure FDA0002535103580000061
Kk＝PkHT(HPkHT+R)-1
其中，Pk为k时刻的估计均方误差阵，Fk,k-1为k-1时刻到k时刻的为状态转移矩阵，且
Figure FDA0002535103580000062
I为单位阵，0为零矩阵，
Figure FDA0002535103580000063
为k-1时刻的噪声驱动阵，Kk为k时刻的滤波增益阵；
Xk＝-KkZk
更新Pk：
Figure FDA0002535103580000064
其中，Xk为k时刻的状态变量，当k＝1时，状态量X＝[δp(1) δν(1) δε(1)]为步骤(1)中初始化***时获得的载体初始状态量X0，
Figure FDA0002535103580000065
为更新后的Pk；保存Pk与Xk，并将其作为下一次滤波的输入。
6.根据权利要求1所述的一种基于正反向滤波的惯性行人定位***误差抑制方法，其特征在于，所述的步骤(10)具体为：
利用步骤(9)中解算出的δν，δp，δε补偿步骤(8)中的
Figure FDA0002535103580000066
更新四元数qk，并将补偿后的结果作为最终导航输出：
位置、速度信息补偿：
Figure FDA0002535103580000067
姿态角补偿：
令
Figure FDA0002535103580000068
利用公式
Figure FDA0002535103580000069
更新转换矩阵
Figure FDA00025351035800000610
利用式(6)求出更新后的φ，θ，ψ；
其中，[·]-1为矩阵的逆运算；
更新四元数：
令
Figure FDA00025351035800000611
则
Figure FDA00025351035800000612
CN201711220837.7A 2017-11-29 2017-11-29 一种基于正反向滤波的惯性行人定位***误差抑制方法 Active CN108007477B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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 北京理工大学 一种自适应的行人导航***零速区间检测方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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