CN117606473A - 一种抑制高度和航向角误差累积的行人自主导航方法 - Google Patents
一种抑制高度和航向角误差累积的行人自主导航方法 Download PDFInfo
- Publication number
- CN117606473A CN117606473A CN202410095580.0A CN202410095580A CN117606473A CN 117606473 A CN117606473 A CN 117606473A CN 202410095580 A CN202410095580 A CN 202410095580A CN 117606473 A CN117606473 A CN 117606473A
- Authority
- CN
- China
- Prior art keywords
- foot
- representing
- pedestrian
- matrix
- measurement
- 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 33
- 238000009825 accumulation Methods 0.000 title claims abstract description 18
- 230000002401 inhibitory effect Effects 0.000 title abstract description 10
- 238000001514 detection method Methods 0.000 claims abstract description 32
- 238000001914 filtration Methods 0.000 claims abstract description 28
- 239000011159 matrix material Substances 0.000 claims description 64
- 238000005259 measurement Methods 0.000 claims description 62
- 238000012937 correction Methods 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 13
- 238000012512 characterization method Methods 0.000 claims description 12
- 230000005021 gait Effects 0.000 claims description 11
- 230000003068 static effect Effects 0.000 claims description 9
- 230000006870 function Effects 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000012986 modification Methods 0.000 claims description 3
- 230000004048 modification Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 210000004556 brain Anatomy 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000284 resting effect 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/165—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 combined with non-inertial navigation instruments
- G01C21/1652—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 combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- 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/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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
本发明属于行人导航技术领域，具体提供一种抑制高度和航向角误差累积的行人自主导航方法，用以抑制行人导航系统的高度及航向漂移、缓解导航系统的误差累积、提升定位精度。本发明提出一种自适应零速检测方法，在行人新进过程中自适应更新零速检测阈值，实现行人状态的准确检测；当行人状态处于零速区间时，分别建立左脚与右脚的单足系统，并依次利用椭球约束与ZUPT对系统误差状态量进行两次修正，有效抑制惯性误差的漂移；再基于UWB测量的双足间距建立双足卡尔曼滤波系统，并利用双足间距的UWB测距约束对系统误差状态量进行再次修正，实现对航向角的修正，最终提升定位精度。
Description
技术领域
本发明属于行人导航技术领域，涉及一种足绑式行人导航方法，具体提供一种抑制高度和航向角误差累积的行人自主导航方法。
背景技术
随着城市化进程的加快，人们的活动范围随着生活及工作需求的扩大而逐渐增大，仅依赖大脑记忆或传统纸质地图的导航方式已经无法满足现代社会日益复杂的导航需求，导航定位服务已经逐渐成为人们日常生活中不可或缺的工具。尤其在管道作业、消防救援、电气线路等专业应用领域，工作人员的位置变化需要导航系统进行实时反应，并进行相应的指挥和调度；由此可见，用以在室内外环境中提供连续、精准导航服务的行人导航系统至关重要。
目前，基于惯性传感器的自主式行人导航技术得到广泛研究，主流方法包括行人航迹推算与基于INS解算的行人导航算法；前者原理简单，后者鲁棒性好，通常结合零速修正技术（ZUPT）可以有效抑制惯导漂移；但是，使用ZUPT需要有精确的零速状态检测技术，能否正确检测出足部的静止状态至关重要；其次，在ZUPT算法中位置和航向是不可观测的，而在长时间的导航定位过程中，航向误差是造成定位误差的主要原因；因此，如何消除ZUPT算法中的航向误差成为ZUPT辅助的行人导航方法的研究重点。
发明内容
本发明的目的在于提供一种抑制高度和航向角误差累积的行人自主导航方法，用以抑制行人导航系统的高度及航向漂移、缓解导航系统的误差累积、提升定位精度。本发明提出一种自适应零速检测方法，能够准确检测出行人在多种运动模式下（行走、跑步、上下楼）左右脚的零速状态，进而实现对行人静止状态的有效检测；当行人状态处于零速区间时，分别建立左脚与右脚的单足系统，并依次利用椭球约束与ZUPT对系统误差状态量进行两次修正，有效抑制惯性误差的漂移；再基于UWB测量的双足间距建立双足卡尔曼滤波系统，并利用双足间距的UWB测距约束对系统误差状态量进行再次修正，实现对航向角的修正，最终提升定位精度。
为实现上述目的，本发明采用的技术方案为：
一种抑制高度和航向角误差累积的行人自主导航方法，包括以下步骤：
步骤1、将微惯性测量单元分别安装在行人的左脚与右脚上，测量得到行人惯性数据；
步骤2、根据行人惯性数据，采用捷联惯性导航算法计算行人的姿态、速度、位置信息；
步骤3、根据行人惯性数据，采用自适应零速检测算法对行人状态进行判定；
步骤4、当行人状态为静止状态时，分别建立左脚与右脚的单足卡尔曼滤波系统，并依次利用椭球约束与ZUPT对系统误差状态量进行两次修正；
步骤5、建立双足卡尔曼滤波系统，并利用双足间距的UWB测距约束对系统误差状态量进行再次修正；
步骤6、采用修正后误差状态量对行人的姿态、速度、位置信息进行误差修正。
进一步的，步骤3具体为：
步骤3.1、根据微惯性测量单元测量得到的Z轴加速度数据计算单足的步态变化表征值：
，
其中，k表示时刻，G(k)表示单足在k时刻的步态变化表征值，az(k)表示单足在k时刻的Z轴加速度；
设定零速检测阈值自适应时间窗为kc，搜索(k-kc)~k内步态变化表征值的最大值Gmax与最小值Gmin，并将最大值Gmax与最小值Gmin按固定数值步长进行区间划分，依次表示为(xi,xi+1)，x1=Gmin，xi+1=xi+Gd，i=1,2,...n，n为区间数，Gd为数值步长；统计每个区间内步态变化表征值的频数，以xi为横坐标、频数为纵坐标绘制直方图，并通过曲线拟合得到函数Fk(xi)；
步骤3.2、根据函数Fk(xi)自适应更新零速检测阈值εk：
，
，
，
其中，、/>与/>分别表示函数Fk(xi)的一阶导数、二阶导数与三阶导数，/>表示符号前后两个条件同时满足；
步骤3.3、根据零速检测阈值εk对单足状态进行判定，若单足的Z轴加速度小于等于零速检测阈值εk，则判定单足处于运动状态，否则，单足处于零速区间；
步骤3.4、针对行人的左脚与右脚分别进行自适应零速检测，当左脚与右脚均处于零速区间时，判定行人处于静止状态；否则，判定行人处于运动状态。
进一步的，步骤4具体为：
步骤4.1、建立单足卡尔曼滤波系统，其状态方程为：
，
，
其中，k表示当前时刻，k-1表示上一时刻；表示单足系统的转移矩阵，/>表示单足系统的系统驱动矩阵，/>表示单足系统的系统噪声向量；/>表示右脚在k时刻的误差状态量，/>表示左脚在k时刻的误差状态量；
步骤4.2、建立单足卡尔曼滤波系统的量测方程：
，
，
其中，、/>分别表示右脚、左脚在k时刻的量测值，/>表示量测噪声向量，/>、分别表示右脚、左脚在k时刻的观测矩阵；
步骤4.3、建立椭球约束的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第一次修正；
椭球约束的量测方程为：
，
，
其中，、/>分别表示左脚、右脚的高度，/>、/>分别表示左脚、右脚在椭圆约束下的量测值；
椭球约束的观测矩阵为：
，
，
其中，表示/>、/>表示/>、/>表示/>、表示/>、/>表示/>、/>表示/>；/>为地球偏心率，/>为横向曲率半径，/>表示行人所在地点的海拔高度，/>表示维度为1×6的零矩阵；表示地球系下右脚的坐标，/>表示地球系下左脚的坐标；、/>分别表示右脚、左脚在椭球约束下的观测矩阵；
将椭圆约束的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，计算得到第一次修正后的左脚与右脚的误差状态量；
步骤4.4、建立ZUPT的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第二次修改；
ZUPT的量测方程为：
，
，
其中，、/>分别表示左脚、右脚的惯性解算速度值，/>、/>分别表示左脚、右脚在ZUPT下的量测值；
ZUPT的观测矩阵为：
，
，
其中，表示3阶单位矩阵，/>、/>分别表示左脚、右脚在ZUPT下的量测矩阵；
在第一次修正的基础上，将ZUPT的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，计算得到第二次修正后的左脚与右脚的误差状态量。
进一步的，步骤5具体为：
步骤5.1、建立双足卡尔曼滤波系统，其状态方程与量测方程：
，
，
其中，k表示当前时刻，k-1表示上一时刻；表示双足系统的转移矩阵、/>表示双足系统的系统驱动矩阵、/>表示双足系统的系统噪声向量，/>表示双足系统的量测噪声向量；/>表示双足系统在k时刻的量测值，/>表示双足系统在k时刻的观测矩阵；/>表示双足系统在k时刻的误差状态量，/>，/>表示右脚在k时刻的误差状态量，/>表示左脚在k时刻的误差状态量；
步骤5.2、建立UWB测距约束的量测方程与观测矩阵，并对双足系统的误差状态量进行修正；
UWB测距约束的量测方程为：
，
其中，表示双足系统在UWB测距约束下的量测值，/>为UWB的测量值，/>为双足间距；
UWB测距约束的观测矩阵为：
，
其中，表示/>，/>表示/>，表示/>，/>表示/>，/>表示/>，/>表示/>，/>表示双足系统在UWB测距约束下的观测矩阵，/>、/>分别表示左脚、右脚对应的载体系到地球系的旋转矩阵，/>、/>分别表示左脚、右脚上的UWB与微惯性测量单元之间的杆臂，/>表示杆臂误差，/>、/>分别表示维度为1×3、1×6的零矩阵，/>表示取模运算，/>表示矩阵的反对称矩阵；
将UWB测距约束的量测值与量测矩阵带入双足卡尔曼滤波系统的状态方程中，计算得到修正后双足系统的误差状态量。
更进一步的，步骤5.2中，双足间距具体为：
，
，
其中，、/>分别表示左脚、右脚在地球系下的位置坐标，/>表示距离量测噪声。
基于上述技术方案，本发明的有益效果在于：
本发明提供一种抑制高度和航向角误差累积的行人自主导航方法，首先，设计自适应零速检测方法，准确判断行人在行进过程中左右脚的零速状态，当检测到零速状态后，建立左右脚单足行人导航系统，分别利用椭球约束与ZUPT进行误差修正；然后，利用UWB测距约束将左右脚单足系统融合构成双足系统，并进行再次误差修正；最后，根据修正后姿态、速度、位置信息实现行人自主导航。
综上，本发明提出自适应零速检测方法，能够准确检测出行人在多种运动模式下（行走、跑步、上下楼）左右脚的零速状态，进而实现对行人静止状态的有效检测；在此基础上，提出左右脚单足行人导航系统融合构成的双足系统，并利用椭球约束、ZUPT以及UWB测距约束有效抑制高度与航向的误差累积，有效提升定位精度，提高导航系统的工作效率。
附图说明
图1为本发明中抑制高度和航向角误差累积的行人自主导航方法的流程示意图。
图2为本发明中系统误差状态量的修正流程示意图。
具体实施方式
为使本发明的目的、技术方案与有益效果更加清楚明白，下面结合附图和实施例对本发明做进一步详细说明。
本实施例提供一种抑制高度和航向角误差累积的行人自主导航方法，其流程如图1所示，具体包括以下步骤：
步骤1、将微惯性测量单元（MEMS IMU）分别安装在行人的左脚与右脚上，测量得到行人惯性数据，包括加速度信息与角速度信息；
步骤2、通过捷联惯性导航算法计算行人的姿态、速度、位置信息，包括惯性解算与状态更新；
步骤3、根据行人惯性数据，采用自适应零速检测算法对行人状态进行判定，若行人状态为静止状态，进行误差修正，否则，进行下一时刻惯性解算与状态更新；
步骤4、分别建立左脚与右脚的单足卡尔曼滤波系统，并依次利用椭球约束与ZUPT对系统误差状态量进行两次修正；
步骤5、建立双足卡尔曼滤波系统，并利用双足间距的UWB测距约束对系统误差状态量进行再次修正；
步骤6、采用修正后误差状态量对行人的姿态、速度、位置信息进行误差修正。
进一步的，步骤3中，对于自适应零速检测算法，首先统计零速检测阈值自适应时间窗内行人行走时重力加速度幅值变化的频数，通过曲线拟合近似刻画频数与幅值的关系，利用波峰间的拐点判断零速检测阈值，并在行走过程中不断更新重力加速度幅值，自适应判断行人行走时的零速检测阈值；具体为：
步骤3.1、根据微惯性测量单元测量得到的Z轴加速度数据计算单足的步态变化表征值：
，
其中，k表示时刻，G(k)表示单足在k时刻的步态变化表征值，az(k)表示单足在k时刻的Z轴加速度；
设定零速检测阈值自适应时间窗为kc，搜索(k-kc)~k内步态变化表征值的最大值Gmax与最小值Gmin，并将最大值Gmax与最小值Gmin按固定数值步长进行区间划分，依次表示为(xi,xi+1)，x1=Gmin，xi+1=xi+Gd，i=1,2,...n，n为区间数，Gd为数值步长；统计每个区间内步态变化表征值的频数，以xi为横坐标、频数为纵坐标绘制直方图，并通过曲线拟合得到直方图中频数空间离散点之间的函数关系Fk(xi)；
步骤3.2、计算函数Fk(xi)中第一个波峰与第二个波峰之间拐点的横坐标值xk,min，并根据xk,min自适应更新零速检测阈值εk；
拐点的横坐标值xk,min为：
，
，
其中，、/>与/>分别表示函数Fk(xi)的一阶导数、二阶导数与三阶导数；
零速检测阈值εk为：；
步骤3.3、根据零速检测阈值εk对单足状态进行判定，若单足的Z轴加速度小于等于零速检测阈值εk，则判定单足处于运动状态，否则，单足处于零速区间（即：静止状态）；
步骤3.4、针对行人的左脚与右脚分别进行自适应零速检测，当左脚与右脚均处于零速区间时，判定行人处于静止状态；否则，判定行人处于运动状态。
进一步的，步骤4中，首先建立左脚与右脚的单足卡尔曼滤波系统，再依次利用椭球约束与ZUPT对系统误差进行修正；具体为：
步骤4.1、建立单足卡尔曼滤波系统的状态方程：
，
，
其中，k表示当前时刻，k-1表示上一时刻；表示单足系统的转移矩阵，/>表示单足系统的系统驱动矩阵，/>表示单足系统的系统噪声向量；
表示右脚在k时刻的误差状态量，/>，依次表示右脚的姿态误差、速度误差、位置误差、陀螺仪零偏、加速度计零偏；
表示左脚在k时刻的误差状态量，/>，依次表示左脚的姿态误差、速度误差、位置误差、陀螺仪零偏、加速度计零偏；
步骤4.2、建立单足卡尔曼滤波系统的量测方程：
，
，
其中，、/>分别表示右脚、左脚在k时刻的量测值，/>表示量测噪声向量，/>、分别表示右脚、左脚在k时刻的观测矩阵；
步骤4.3、建立椭球约束的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第一次修正；
椭球约束的量测方程为：
，
，
其中，、/>分别表示左脚、右脚的高度，/>、/>分别表示左脚、右脚在椭圆约束下的量测值，即为：单足在k时刻与k-1时刻的高度变化值；
椭球约束的观测矩阵为：
，
，
其中，表示/>、/>表示/>、/>表示/>、表示/>、/>表示/>、/>表示/>；/>为地球偏心率，/>为横向曲率半径，/>表示行人所在地点的海拔高度，/>表示维度为1×6的零矩阵；表示地球系下右脚的坐标，/>表示地球系下左脚的坐标；、/>分别表示右脚、左脚在椭球约束下的观测矩阵；
将椭圆约束的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，实现左脚、右脚的误差状态量的第一次修正；
步骤4.4、建立ZUPT的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第二次修改；
ZUPT的量测方程为：
，
，
其中，、/>分别表示左脚、右脚的惯性解算速度值，/>、/>分别表示左脚、右脚在ZUPT下的量测值；
ZUPT的观测矩阵为：
，
，
其中，表示3阶单位矩阵，/>、/>分别表示左脚、右脚在ZUPT下的量测矩阵；
在第一次修正的基础上，将ZUPT的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，实现左脚、右脚的误差状态量的第二次修正。
进一步的，步骤5中，利用双足间距的UWB测距约束在单足卡尔曼滤波系统基础上构建双足卡尔曼滤波系统，并利用UWB测距约束对系统误差进行再次修正；具体为：
步骤5.1、建立双足卡尔曼滤波系统的状态方程与量测方程：
，
，
其中，k表示当前时刻，k-1表示上一时刻；表示双足系统的转移矩阵、/>表示双足系统的系统驱动矩阵、/>表示双足系统的系统噪声向量，/>表示双足系统的量测噪声向量；/>表示双足系统在k时刻的量测值，/>表示双足系统在k时刻的观测矩阵；/>表示双足系统在k时刻的误差状态量，/>；
步骤5.2、建立UWB测距约束的量测方程与观测矩阵，并对双足系统的误差状态量进行修正；
UWB测距约束的量测方程为：
，
其中，表示双足系统UWB测距约束下的量测值，/>为UWB的测量值；
为双足间距，计算式为：/>，/>、分别表示左脚、右脚在地球系下的位置坐标，/>、/>分别表示左脚、右脚对应的载体系到地球系的旋转矩阵，/>、/>分别表示左脚、右脚上的UWB与微惯性测量单元之间的杆臂，表示杆臂误差，/>表示协方差为/>的距离量测噪声，/>表示取模运算；
UWB测距约束的观测矩阵为：
，
,
,
其中，表示/>，/>表示/>，/>表示/>，/>表示/>，/>表示双足系统在UWB测距约束下的观测矩阵，/>、/>分别表示左脚、右脚对应的载体系到地球系的旋转矩阵，/>、/>分别表示左脚、右脚上的UWB与微惯性测量单元之间的杆臂，/>表示杆臂误差，/>、/>分别表示维度为1×3、1×6的零矩阵，/>表示矩阵的反对称矩阵；
在第二次修正的基础上，将UWB测距约束的量测值与量测矩阵带入双足卡尔曼滤波系统的状态方程中，实现双足系统的误差状态量的修正；
综上，本发明首先提出一种自适应零速检测方法，在行人新进过程中自适应更新零速检测阈值，实现行人状态的准确检测；然后，当行人状态处于零速区间时，分别建立左脚与右脚的单足系统，并依次利用椭球约束与ZUPT对系统误差状态量进行两次修正，有效抑制惯性误差的漂移；再利用UWB传感器测量的双足间距，将基于ZUPT的左右脚单足系统融合构成双足系统，在观测量为ZUPT的基础上修正导航系统航向误差，如图2所示；本发明不仅能够有效检测行人行进过程中的零速状态，还能有效抑制行人导航系统的高度航向漂移，缓解导航系统的误差累积，提升定位精度。
以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合。
Claims (5)
1.一种抑制高度和航向角误差累积的行人自主导航方法，其特征在于，包括以下步骤：
步骤1、将微惯性测量单元分别安装在行人的左脚与右脚上，测量得到行人惯性数据；
步骤2、根据行人惯性数据，采用捷联惯性导航算法计算行人的姿态、速度、位置信息；
步骤3、根据行人惯性数据，采用自适应零速检测算法对行人状态进行判定；
步骤4、当行人状态为静止状态时，分别建立左脚与右脚的单足卡尔曼滤波系统，并依次利用椭球约束与ZUPT对系统误差状态量进行两次修正；
步骤5、建立双足卡尔曼滤波系统，并利用双足间距的UWB测距约束对系统误差状态量进行再次修正；
步骤6、采用修正后误差状态量对行人的姿态、速度、位置信息进行误差修正。
2.根据权利要求1所述抑制高度和航向角误差累积的行人自主导航方法，其特征在于，步骤3具体为：
步骤3.1、根据微惯性测量单元测量得到的Z轴加速度数据计算单足的步态变化表征值：
，
其中，k表示时刻，G(k)表示单足在k时刻的步态变化表征值，az(k)表示单足在k时刻的Z轴加速度；
设定零速检测阈值自适应时间窗为kc，搜索(k-kc)~k内步态变化表征值的最大值Gmax与最小值Gmin，并将最大值Gmax与最小值Gmin按固定数值步长进行区间划分，依次表示为(xi,xi+1)，x1=Gmin，xi+1=xi+Gd，i=1,2,...n，n为区间数，Gd为数值步长；统计每个区间内步态变化表征值的频数，以xi为横坐标、频数为纵坐标绘制直方图，并通过曲线拟合得到函数Fk(xi)；
步骤3.2、根据函数Fk(xi)自适应更新零速检测阈值εk：
，
，
，
其中，、/>与/>分别表示函数Fk(xi)的一阶导数、二阶导数与三阶导数，/>表示符号前后两个条件同时满足；
步骤3.3、根据零速检测阈值εk对单足状态进行判定，若单足的Z轴加速度小于等于零速检测阈值εk，则判定单足处于运动状态，否则，单足处于零速区间；
步骤3.4、针对行人的左脚与右脚分别进行自适应零速检测，当左脚与右脚均处于零速区间时，判定行人处于静止状态；否则，判定行人处于运动状态。
3.根据权利要求1所述抑制高度和航向角误差累积的行人自主导航方法，其特征在于，步骤4具体为：
步骤4.1、建立单足卡尔曼滤波系统，其状态方程为：
，
，
其中，k表示当前时刻，k-1表示上一时刻；表示单足系统的转移矩阵，/>表示单足系统的系统驱动矩阵，/>表示单足系统的系统噪声向量；/>表示右脚在k时刻的误差状态量，/>表示左脚在k时刻的误差状态量；
步骤4.2、建立单足卡尔曼滤波系统的量测方程：
，
，
其中，、/>分别表示右脚、左脚在k时刻的量测值，/>表示量测噪声向量，/>、分别表示右脚、左脚在k时刻的观测矩阵；
步骤4.3、建立椭球约束的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第一次修正；
椭球约束的量测方程为：
，
，
其中，、/>分别表示左脚、右脚的高度，/>、/>分别表示左脚、右脚在椭圆约束下的量测值；
椭球约束的观测矩阵为：
，
，
其中，表示/>、/>表示/>、/>表示/>、表示/>、/>表示/>、/>表示/>；/>为地球偏心率，/>为横向曲率半径，/>表示行人所在地点的海拔高度，/>表示维度为1×6的零矩阵；表示地球系下右脚的坐标，/>表示地球系下左脚的坐标；、/>分别表示右脚、左脚在椭球约束下的观测矩阵；
将椭圆约束的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，计算得到第一次修正后的左脚与右脚的误差状态量；
步骤4.4、建立ZUPT的量测方程与观测矩阵，并分别对左脚、右脚的误差状态量进行第二次修改；
ZUPT的量测方程为：
，
，
其中，、/>分别表示左脚、右脚的惯性解算速度值，/>、/>分别表示左脚、右脚在ZUPT下的量测值；
ZUPT的观测矩阵为：
，
，
其中，表示3阶单位矩阵，/>、/>分别表示左脚、右脚在ZUPT下的量测矩阵；
在第一次修正的基础上，将ZUPT的量测值与量测矩阵带入单足卡尔曼滤波系统的状态方程中，计算得到第二次修正后的左脚与右脚的误差状态量。
4.根据权利要求1所述抑制高度和航向角误差累积的行人自主导航方法，其特征在于，步骤5具体为：
步骤5.1、建立双足卡尔曼滤波系统，其状态方程与量测方程：
，
，
其中，k表示当前时刻，k-1表示上一时刻；表示双足系统的转移矩阵、/>表示双足系统的系统驱动矩阵、/>表示双足系统的系统噪声向量，/>表示双足系统的量测噪声向量；/>表示双足系统在k时刻的量测值，/>表示双足系统在k时刻的观测矩阵；/>表示双足系统在k时刻的误差状态量，/>，/>表示右脚在k时刻的误差状态量，表示左脚在k时刻的误差状态量；
步骤5.2、建立UWB测距约束的量测方程与观测矩阵，并对双足系统的误差状态量进行修正；
UWB测距约束的量测方程为：
，
其中，表示双足系统在UWB测距约束下的量测值，/>为UWB的测量值，/>为双足间距；
UWB测距约束的观测矩阵为：
，
其中，表示/>，/>表示，/>表示/>，/>表示/>，/>表示，/>表示/>，/>表示双足系统在UWB测距约束下的观测矩阵，/>、/>分别表示左脚、右脚对应的载体系到地球系的旋转矩阵，/>、/>分别表示左脚、右脚上的UWB与微惯性测量单元之间的杆臂，/>表示杆臂误差，/>、/>分别表示维度为1×3、1×6的零矩阵，/>表示取模运算，/>表示矩阵的反对称矩阵；
将UWB测距约束的量测值与量测矩阵带入双足卡尔曼滤波系统的状态方程中，计算得到修正后双足系统的误差状态量。
5.根据权利要求4所述抑制高度和航向角误差累积的行人自主导航方法，其特征在于，步骤5.2中，双足间距具体为：
，
，
其中，、/>分别表示左脚、右脚在地球系下的位置坐标，/>表示距离量测噪声。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410095580.0A CN117606473B (zh) | 2024-01-24 | 2024-01-24 | 一种抑制高度和航向角误差累积的行人自主导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410095580.0A CN117606473B (zh) | 2024-01-24 | 2024-01-24 | 一种抑制高度和航向角误差累积的行人自主导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117606473A true CN117606473A (zh) | 2024-02-27 |
| CN117606473B CN117606473B (zh) | 2024-05-10 |
Family
ID=89952071
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410095580.0A Active CN117606473B (zh) | 2024-01-24 | 2024-01-24 | 一种抑制高度和航向角误差累积的行人自主导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117606473B (zh) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102445200A (zh) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | 微小型个人组合导航系统及其导航定位方法 |
| KR101642286B1 (ko) * | 2015-02-12 | 2016-07-25 | 한국항공우주연구원 | 보행특성을 이용한 실내에서의 보행자 이동방향 추정 방법 |
| CN106482733A (zh) * | 2016-09-23 | 2017-03-08 | 南昌大学 | 行人导航中基于足底压力检测的零速修正方法 |
| CN106767790A (zh) * | 2017-01-12 | 2017-05-31 | 厦门大学 | 人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法 |
| CN107421566A (zh) * | 2017-08-25 | 2017-12-01 | 北京理工大学 | 一种无人车多源传感器信息仿真平台 |
| CN107843256A (zh) * | 2017-10-11 | 2018-03-27 | 南京航空航天大学 | 基于mems传感器的自适应零速修正行人导航方法 |
| CN109099913A (zh) * | 2018-10-10 | 2018-12-28 | 格物感知（深圳）科技有限公司 | 一种基于mems惯性器件的穿戴式导航装置和方法 |
| CN114623826A (zh) * | 2022-02-25 | 2022-06-14 | 南京航空航天大学 | 一种基于人体下肢dh模型的行人惯性导航定位方法 |
| CN116448103A (zh) * | 2023-03-16 | 2023-07-18 | 重庆邮电大学 | 基于uwb测距辅助的行人足绑式惯性导航系统误差修正方法 |
-
2024
- 2024-01-24 CN CN202410095580.0A patent/CN117606473B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102445200A (zh) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | 微小型个人组合导航系统及其导航定位方法 |
| KR101642286B1 (ko) * | 2015-02-12 | 2016-07-25 | 한국항공우주연구원 | 보행특성을 이용한 실내에서의 보행자 이동방향 추정 방법 |
| CN106482733A (zh) * | 2016-09-23 | 2017-03-08 | 南昌大学 | 行人导航中基于足底压力检测的零速修正方法 |
| CN106767790A (zh) * | 2017-01-12 | 2017-05-31 | 厦门大学 | 人体下肢运动模型与卡尔曼滤波融合估计行人移动跟踪的方法 |
| CN107421566A (zh) * | 2017-08-25 | 2017-12-01 | 北京理工大学 | 一种无人车多源传感器信息仿真平台 |
| CN107843256A (zh) * | 2017-10-11 | 2018-03-27 | 南京航空航天大学 | 基于mems传感器的自适应零速修正行人导航方法 |
| CN109099913A (zh) * | 2018-10-10 | 2018-12-28 | 格物感知（深圳）科技有限公司 | 一种基于mems惯性器件的穿戴式导航装置和方法 |
| CN114623826A (zh) * | 2022-02-25 | 2022-06-14 | 南京航空航天大学 | 一种基于人体下肢dh模型的行人惯性导航定位方法 |
| CN116448103A (zh) * | 2023-03-16 | 2023-07-18 | 重庆邮电大学 | 基于uwb测距辅助的行人足绑式惯性导航系统误差修正方法 |
Non-Patent Citations (5)
| Title |
|---|
| MAORAN ZHU: "f²IMU-R: Pedestrian Navigation by Low-Cost Foot-Mounted Dual IMUs and Interfoot Ranging", IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, vol. 30, no. 1, 31 July 2022 (2022-07-31), pages 247 - 260, XP011893892, DOI: 10.1109/TCST.2021.3063533 * |
| 岑世欣等: "基于自适应阈值的行人惯性导航零速检测算法", 《压电与声光》, vol. 41, no. 4, 31 August 2019 (2019-08-31), pages 601 - 606 * |
| 王云瑞等: "基于MEMS传感器的双轨迹融合导航系统", 《微电子学与计算机》, vol. 35, no. 5, 31 May 2018 (2018-05-31), pages 116 - 123 * |
| 贾浩男等: "基于惯性传感器的行人室内定位算法研究", 《无线电工程》, vol. 48, no. 8, 1 August 2018 (2018-08-01), pages 619 - 623 * |
| 鲁琪: "结合等式约束的双足IMU行人导航技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 01, 15 January 2023 (2023-01-15), pages 136 - 1849 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117606473B (zh) | 2024-05-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105043385B (zh) | 一种行人自主导航定位的自适应卡尔曼滤波方法 | |
| CN109827577B (zh) | 基于运动状态检测的高精度惯性导航定位算法 | |
| CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
| CN104713554A (zh) | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 | |
| CN105628027A (zh) | 一种基于mems惯性器件的室内环境精确实时定位方法 | |
| CN109959374B (zh) | 一种行人惯性导航全时全程逆向平滑滤波方法 | |
| CN108827339B (zh) | 一种基于惯性辅助的高效视觉里程计 | |
| CN110986941B (zh) | 一种手机安装角的估计方法 | |
| CN112362057A (zh) | 基于零速修正与姿态自观测的惯性行人导航算法 | |
| CN115767412A (zh) | 融合超宽带和惯性测量单元的室内定位方法 | |
| CN108592907A (zh) | 一种基于双向滤波平滑技术的准实时步进式行人导航方法 | |
| CN111522034A (zh) | 基于惯性导航的定位方法、设备及装置 | |
| CN104897155B (zh) | 一种个人携行式多源定位信息辅助修正方法 | |
| CN115451949A (zh) | 一种基于车轮安装惯性测量单元的车辆定位方法 | |
| CN110260860A (zh) | 基于足部惯性传感器的室内移动测量定位定姿方法及系统 | |
| CN112904884B (zh) | 足式机器人轨迹跟踪方法、设备及可读存储介质 | |
| CN117606473B (zh) | 一种抑制高度和航向角误差累积的行人自主导航方法 | |
| CN117119586B (zh) | 一种基于uwb和imu室内定位融合方法 | |
| CN116857569A (zh) | 一种自流式输水管道泄漏检测装置定位修正方法 | |
| CN117213476A (zh) | 一种隧道施工人员定位方法及装置 | |
| CN115451946A (zh) | 一种MEMS-IMU和Wi-Fi组合的室内行人定位方法 | |
| CN115523919A (zh) | 一种基于陀螺漂移优化的九轴姿态解算方法 | |
| CN111307148B (zh) | 一种基于惯性网络的行人定位方法 | |
| CN112985390B (zh) | 一种基于磁强计辅助的步态检测方法 | |
| CN110146079A (zh) | 一种基于主副imu和气压计的三维行人导航方法 |
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 |