CN108180923B - 一种基于人体里程计的惯性导航定位方法 - Google Patents
一种基于人体里程计的惯性导航定位方法 Download PDFInfo
- Publication number
- CN108180923B CN108180923B CN201711291646.XA CN201711291646A CN108180923B CN 108180923 B CN108180923 B CN 108180923B CN 201711291646 A CN201711291646 A CN 201711291646A CN 108180923 B CN108180923 B CN 108180923B
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- human body
- navigation system
- odometer
- human
- 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 33
- 238000012937 correction Methods 0.000 claims abstract description 17
- 238000006073 displacement reaction Methods 0.000 claims description 24
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 7
- 230000005021 gait Effects 0.000 claims description 7
- YBJHBAHKTGYVGT-ZKWXMUAHSA-N (+)-Biotin Chemical compound N1C(=O)N[C@@H]2[C@H](CCCCC(=O)O)SC[C@@H]21 YBJHBAHKTGYVGT-ZKWXMUAHSA-N 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 210000002303 tibia Anatomy 0.000 claims description 3
- FEPMHVLSLDOMQC-UHFFFAOYSA-N virginiamycin-S1 Natural products CC1OC(=O)C(C=2C=CC=CC=2)NC(=O)C2CC(=O)CCN2C(=O)C(CC=2C=CC=CC=2)N(C)C(=O)C2CCCN2C(=O)C(CC)NC(=O)C1NC(=O)C1=NC=CC=C1O FEPMHVLSLDOMQC-UHFFFAOYSA-N 0.000 claims description 3
- 238000005070 sampling Methods 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 3
- 230000008859 change Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000008451 emotion Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000013139 quantization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
- G01C22/006—Pedometers
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明提供了一种基于人体里程计的惯性导航定位方法，能够实现行人全运动状态下的精确导航。本发明选用人体里程计来辅助惯性导航***，其中，人体里程计参照陆用车载里程计将每个脉冲所代表的行程作为刻度因子的方法，将单步步长作为刻度因子，刻度因子包含修正因子，得到的刻度因子更为准确，从而得到精确的人体的行进轨迹。同时将修正因子增加到惯性导航***的状态向量，能够针对人体运动的任意性和不确定性，充分利用惯性导航***短时定位精度高的特点，辅助惯性导航***完成室内行人全运动状态下的精确导航。
Description
技术领域
本发明属于行人导航技术领域，具体涉及一种基于人体里程计的惯性导航定位方法。
背景技术
“智慧城市”的提出和发展对室内人员的定位导航提出了更高的要求，基于惯性传感器的行人航位推算***(PDR)具有充分的自主性和灵活性，越来越受到人们重视。目前针对PDR***的研究主要基于零速修正(ZUPT)原理，通过查找人行走过程中的零速点来修正惯性导航***，还有研究将建筑物特征信息、人行走经验步长信息等与惯性导航***进行融合的方式，辅助惯性导航***完成室内行人全运动状态下的导航。
但ZUPT方法需要对零速点有精准的判断识别，因此仅适用于平地走、上下楼梯等简单步态；而建筑物特征复杂、各有差异，人行进步长又受周边环境、情绪等影响，所以使用这些信息与惯性导航***进行融合的惯性导航定位精度都不够理想，定位精度不高。
发明内容
有鉴于此，本发明提供了一种基于人体里程计的惯性导航定位方法，能够实现行人全运动状态下的精确导航。
本发明是通过以下技术方案来实现的：
包括如下步骤：
步骤1，采集人体运动的步频以及加速度和角速度信息，并对采集到的人体运动的加速度和角速度信息进行惯性导航解算，得到惯性导航解算位移增量；
步骤2，将刻度因子与步骤1采集到的人体运动的步频相乘，得到人体里程计输出位移增量；
其中所述刻度因子S为：
S＝(1+δK)[h·(a·fstep+b)+c]
其中，[h·(a·fstep+b)+c]为参考步长，fstep为步频，h为人的身高，a、b和c为与步态相关的参考步长系数，为已知量；δK为修正因子，用于修正参考步长误差；
步骤3，将修正因子增加到惯性导航***的状态向量中，利用惯性导航原理建立惯性导航***的状态方程；
将步骤1得到的惯性导航解算位移增量与步骤2得到的人体里程计输出位移增量之差作为观测量，建立惯性导航***的观测方程；
步骤4，基于步骤3建立的状态方程和观测方程，利用卡尔曼滤波方法获得惯性导航***的状态向量估计值；
步骤5，利用步骤4滤波所得状态向量估计值对人体里程计刻度因子以及惯性导航***的零偏进行修正，完成行人全运动状态下的定位。
其中，运用位移积分匹配方法建立惯性导航***的观测方程。
其中，利用陀螺仪和加速度计采集人体运动的步频以及加速度和角速度信息；惯性导航***的状态向量为：
其中，ψN为姿态误差；δVN为速度误差；ζN为经纬度误差；δh为高程误差；ε为三个轴向的陀螺常值零偏；
为三个轴向的加速度计常值零偏。
其中，所述惯性导航***的观测方程为：δZ(k)＝δΔRINS(tk)-δΔSN(tk)，其中δZ(k)为观测量微分，δΔRINS(tk)为惯性导航***解算位移增量微分，δΔSN(tk)为人体里程计输出位移增量微分。
其中，利用微惯性传感器采集人体在不同运动步态下加速度和角速度信息。
其中，所述微惯性传感器配置于人体足部、腰部或胫骨。
有益效果：
本发明选用人体里程计来辅助惯性导航***，其中，人体里程计参照陆用车载里程计将每个脉冲所代表的行程作为刻度因子的方法，将单步步长作为刻度因子，刻度因子包含修正因子，得到的刻度因子更为准确，从而得到精确的人体的行进轨迹。同时将修正因子增加到惯性导航***的状态向量，能够针对人体运动的任意性和不确定性，充分利用惯性导航***短时定位精度高的特点，辅助惯性导航***完成室内行人全运动状态下的精确导航。
附图说明
图1为基于人体里程计的惯性导航定位方法流程图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
为了实现对室内行人全运动状态下的精确导航，需要针对人体运动的任意性和不确定性，找到新的外部辅助源信息，充分利用惯性导航***短时定位精度高的特点，辅助惯性导航***完成室内行人全运动状态下的精确导航。
本发明选用人体里程计来辅助惯性导航***，其中，人体里程计参照陆用车载里程计将每个脉冲所代表的行程作为刻度因子的方法，将单步步长作为刻度因子，刻度因子包含修正因子，得到的刻度因子更为准确，从而得到精确的人体的行进轨迹。同时将修正因子增加到惯性导航***的状态向量，能够针对人体运动的任意性和不确定性，充分利用惯性导航***短时定位精度高的特点，利用卡尔曼滤波方法，将两个或多个子***利用有效的信息融合方法进行融合，进而实现行人全运动状态下的精确导航。
本发明提供了一种基于人体里程计的惯性导航定位方法，可以完成室内行人全运动状态下的高精度定位。
人体里程计的惯性导航定位方法流程图如图1所示，所述方法包括如下步骤：
步骤1，采集人体运动的步频以及加速度和角速度信息，并对采集到的人体运动的加速度和角速度信息进行惯性导航解算，得到惯性导航解算位移增量；
其中，利用微惯性传感器采集人体在不同运动步态下加速度和角速度信息，目前微惯性传感器常用陀螺仪和加速度计。微惯性传感器配置于人体足部、腰部或胫骨。
其中，在第k个采样周期ΔT＝tk-tk-1内，惯性导航***解算位移增量为：
其中，VN为惯性导航***解算得到的速度，k＝1,2,3....M，M为采样周期总数，t0为采样初始时刻，t1为第一个采样周期末的时刻，t2为第二个采样周期末的时刻，以此类推，tk为第k个采样周期末的时刻。
步骤2，将刻度因子与采集到的人体运动的步频相乘得到人体里程计输出位移增量；
其中所述刻度因子S为：
S＝(1+δK)[h·(a·fstep+b)+c]
其中，fstep为步频；h为人的身高；δK为修正因子；a、b和c为不同步态下的参考步长系数，[h·(a·fstep+b)+c]为参考步长。其中，参考步长通过前期步态划分结果得到，不同步态的参考步长不同；修正因子用于修正参考步长误差，表示由人体运动的任意性和随机性导致的步长变化，通过在线辨识和自适应估计获得。
刻度因子即为人体里程计的单步步长，将采集到的人体运动的步频作为人体里程计输出脉冲数，输出脉冲数与刻度因子相乘便可计算出的人体里程计输出位移增量。
人体里程计输出位移增量为：
其中，
为惯性导航***解算得到的人体里程计坐标系到导航坐标系的变换矩阵；ΔSVMS即为通过人体里程计得到的单步步长S。
另外，人体运动不一定是在某一特定维度，若将人体运动分为前后方向和左右方向两个维度，每个维度上视为有一个单独的人体里程计，此时人体里程计为二维人体里程计，如图1所示；
步骤3，建立惯性导航***的状态方程和观测方程：
将步骤2得到的修正因子增加到惯性导航的状态向量中，利用惯性导航原理建立惯性导航***的状态方程；
其中，惯性导航***模型为现有公知模型，而对于人体里程计的修正因子，有：
本实施例中，在建立了惯性导航***和人体里程计模型的基础上，将修正因子增加到状态向量中，得到状态向量为：
其中，ψN为姿态误差；δVN为速度误差；ζN为经纬度误差；δh为高程误差；ε为三个轴向的陀螺常值零偏；
为三个轴向的加速度计常值零偏。
得到惯性导航***的状态方程为：
其中，F(tk)表示***转移矩阵，由惯性导航***和人体里程计的动态误差模型得到；W(tk)为***噪声。
将步骤1得到的惯性导航解算位移增量与步骤2得到的人体里程计输出位移增量之差作为观测量，运用位移积分匹配方法建立惯性导航***的观测方程；
其中观测量为Z(k)：
Z(k)＝ΔRINS(tk)-ΔSN(tk)
对Z(k)等式两边进行微分，得到惯性导航***的观测方程为：
δZ(k)＝δΔRINS(tk)-δΔSN(tk)
本实施例选用位移积分匹配方法来建立观测方程，利用位移积分匹配方法建立惯性导航***的观测方程，相对于传统的以速度为观测量的里程计辅助导航方法，避免了因计算人体里程计速度而引入的量化误差，有效降低了量测噪声水平，提高了导航***性能。
对观测方程进一步整理可得：
由此可得观测矩阵H(k)为：
步骤4，基于步骤3建立的惯性导航***的状态方程和观测方程，利用卡尔曼滤波的基本方程对人体里程计和惯性导航***的信息进行融合，实现人体里程计和惯性导航***之间的相互校正和最优融合，获得状态向量估计值；
步骤5，利用滤波所得状态向量估计值对人体里程计刻度因子以及惯性导航***的零偏进行修正，完成行人全运动状态下的定位。
当步骤1利用陀螺仪和加速度计采集人体运动的步频以及加速度和角速度信息时，步骤5中惯性导航***的零偏是指陀螺仪和加速度计的零偏。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (6)
1.一种基于人体里程计的惯性导航定位方法，其特征在于，包括如下步骤：
步骤1，采集人体运动的步频以及加速度和角速度信息，并对采集到的人体运动的加速度和角速度信息进行惯性导航解算，得到惯性导航解算位移增量；
步骤2，将刻度因子与步骤1采集到的人体运动的步频相乘，得到人体里程计输出位移增量；
其中所述刻度因子S为：
S＝(1+δK)[h·(a·fstep+b)+c]
其中，[h·(a·fstep+b)+c]为参考步长，fstep为步频，h为人的身高，a、b和c为不同步态下的参考步长系数，参考步长通过前期步态划分结果得到，不同步态的参考步长不同；δK为修正因子，用于修正参考步长误差；
步骤3，将修正因子增加到惯性导航***的状态向量中，利用惯性导航原理建立惯性导航***的状态方程；
将步骤1得到的惯性导航解算位移增量与步骤2得到的人体里程计输出位移增量之差作为观测量，建立惯性导航***的观测方程；
步骤4，基于步骤3建立的状态方程和观测方程，利用卡尔曼滤波方法获得惯性导航***的状态向量估计值；
步骤5，利用步骤4滤波所得状态向量估计值对人体里程计刻度因子以及惯性导航***的零偏进行修正，完成行人全运动状态下的定位。
2.如权利要求1所述的一种基于人体里程计的惯性导航定位方法，其特征在于，运用位移积分匹配方法建立惯性导航***的观测方程。
3.如权利要求1所述的一种基于人体里程计的惯性导航定位方法，其特征在于，利用陀螺仪和加速度计采集人体运动的步频以及加速度和角速度信息；惯性导航***的状态向量为：
其中，ψN为姿态误差；δVN为速度误差；ζN为经纬度误差；δh为高程误差；ε为三个轴向的陀螺常值零偏；
为三个轴向的加速度计常值零偏。
4.如权利要求1所述的一种基于人体里程计的惯性导航定位方法，其特征在于，所述惯性导航***的观测方程为：δZ(k)＝δΔRINS(tk)-δΔSN(tk)，其中δZ(k)为观测量微分，δΔRINS(tk)为惯性导航***解算位移增量微分，δΔSN(tk)为人体里程计输出位移增量微分。
5.如权利要求1所述的一种基于人体里程计的惯性导航定位方法，其特征在于，利用微惯性传感器采集人体在不同运动步态下加速度和角速度信息。
6.如权利要求5所述的一种基于人体里程计的惯性导航定位方法，其特征在于，所述微惯性传感器配置于人体足部、腰部或胫骨。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711291646.XA CN108180923B (zh) | 2017-12-08 | 2017-12-08 | 一种基于人体里程计的惯性导航定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711291646.XA CN108180923B (zh) | 2017-12-08 | 2017-12-08 | 一种基于人体里程计的惯性导航定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108180923A CN108180923A (zh) | 2018-06-19 |
| CN108180923B true CN108180923B (zh) | 2020-10-20 |
Family
ID=62545704
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711291646.XA Active CN108180923B (zh) | 2017-12-08 | 2017-12-08 | 一种基于人体里程计的惯性导航定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108180923B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109297486B (zh) * | 2018-09-30 | 2020-11-13 | 重庆自行者科技有限公司 | 惯性与多里程计信息辅助的车体运动状态判定方法及*** |
| CN109769206B (zh) * | 2019-02-25 | 2021-02-02 | 广州市香港科大***研究院 | 一种室内定位融合方法、装置、存储介质及终端设备 |
| CN111157984B (zh) * | 2020-01-08 | 2021-10-22 | 电子科技大学 | 基于毫米波雷达及惯性测量单元的一种行人自主导航方法 |
| CN111380516B (zh) * | 2020-02-27 | 2022-04-08 | 上海交通大学 | 基于里程计测量信息的惯导/里程计车辆组合导航方法及*** |
| CN111307148B (zh) * | 2020-04-03 | 2021-09-03 | 北京航空航天大学 | 一种基于惯性网络的行人定位方法 |
| CN111811500A (zh) * | 2020-05-06 | 2020-10-23 | 北京嘀嘀无限科技发展有限公司 | 目标对象的位姿估计方法、装置、存储介质和电子设备 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103983273A (zh) * | 2014-04-29 | 2014-08-13 | 华南理工大学 | 一种基于加速度传感器的实时步长估计方法 |
| CN104819716A (zh) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | 一种基于mems的ins/gps组合的室内外个人导航算法 |
| CN105628027A (zh) * | 2016-02-19 | 2016-06-01 | 中国矿业大学 | 一种基于mems惯性器件的室内环境精确实时定位方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10837794B2 (en) * | 2014-12-12 | 2020-11-17 | Invensense, Inc. | Method and system for characterization of on foot motion with multiple sensor assemblies |
-
2017
- 2017-12-08 CN CN201711291646.XA patent/CN108180923B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103983273A (zh) * | 2014-04-29 | 2014-08-13 | 华南理工大学 | 一种基于加速度传感器的实时步长估计方法 |
| CN104819716A (zh) * | 2015-04-21 | 2015-08-05 | 北京工业大学 | 一种基于mems的ins/gps组合的室内外个人导航算法 |
| CN105628027A (zh) * | 2016-02-19 | 2016-06-01 | 中国矿业大学 | 一种基于mems惯性器件的室内环境精确实时定位方法 |
Non-Patent Citations (2)
| Title |
|---|
| Comparison of step length and heading estimation methods for indoor environments;Juan Bravo;《IEEE》;20171023;第1-4页 * |
| MEMS传感器的计步算法研究;楼喜中等;《中国计量大学学报》;20170315;第28卷(第01期);第81-86页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108180923A (zh) | 2018-06-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108180923B (zh) | 一种基于人体里程计的惯性导航定位方法 | |
| CN104406586B (zh) | 基于惯性传感器的行人导航装置和方法 | |
| US10352959B2 (en) | Method and system for estimating a path of a mobile element or body | |
| CN102538781B (zh) | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 | |
| CN107588769B (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
| CN103033189B (zh) | 一种深空探测巡视器惯性/视觉组合导航方法 | |
| CN103759730B (zh) | 一种基于导航信息双向融合的行人与智能移动载体的协同导航***及其导航方法 | |
| CN102445200B (zh) | 微小型个人组合导航***及其导航定位方法 | |
| CN103776446B (zh) | 一种基于双mems-imu的行人自主导航解算算法 | |
| CN105318876A (zh) | 一种惯性里程计组合高精度姿态测量方法 | |
| CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
| CN104713554A (zh) | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 | |
| CN106168485B (zh) | 步行航迹数据推算方法及装置 | |
| CN113295158B (zh) | 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法 | |
| CN109959374B (zh) | 一种行人惯性导航全时全程逆向平滑滤波方法 | |
| CN104181573A (zh) | 北斗惯导深组合导航微*** | |
| CN109459028A (zh) | 一种基于梯度下降的自适应步长估计方法 | |
| CN104316058B (zh) | 一种采用交互多模型的移动机器人wsn/ins组合导航方法 | |
| CN109741372A (zh) | 一种基于双目视觉的里程计运动估计方法 | |
| Rhudy et al. | Wide-field optical flow aided inertial navigation for unmanned aerial vehicles | |
| CN109708647A (zh) | 一种基于融合特征元素的室内拓扑地图行人定位方法 | |
| CN108106630B (zh) | 一种行人导航的二维人体里程计及里程计算方法 | |
| Min et al. | Design of complementary filter using least square method | |
| CN103017773B (zh) | 一种基于天体表面特征和天然卫星路标的环绕段导航方法 | |
| Meng et al. | Pedestrian Navigation Method based on PDR/INS KF fusion and Height Update for Three-Dimensional Positioning |
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 |