CN112539747A - 一种基于惯性传感器和雷达的行人航位推算方法和*** - Google Patents

一种基于惯性传感器和雷达的行人航位推算方法和*** Download PDF

Info

Publication number
CN112539747A
CN112539747A CN202011316318.2A CN202011316318A CN112539747A CN 112539747 A CN112539747 A CN 112539747A CN 202011316318 A CN202011316318 A CN 202011316318A CN 112539747 A CN112539747 A CN 112539747A
Authority
CN
China
Prior art keywords
pedestrian
wall
current
distance
course
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
Application number
CN202011316318.2A
Other languages
English (en)
Other versions
CN112539747B (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.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
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 Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202011316318.2A priority Critical patent/CN112539747B/zh
Publication of CN112539747A publication Critical patent/CN112539747A/zh
Application granted granted Critical
Publication of CN112539747B publication Critical patent/CN112539747B/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
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/50—Systems of measurement based on relative movement of target
    • G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S13/62—Sense-of-movement determination

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种基于惯性传感器和雷达的行人航位推算方法和***，属于通信与无线网络技术领域。本发明使用扩展卡尔曼滤波器更新墙壁参数，再利用粒子滤波器融合墙壁参数，雷达测距结果以及惯性传感器测量估计出最终的位置。雷达采集的距离数据是独立于惯性传感器数据之外的新维度的数据，且同样与行人的姿态(位置+航向)有关，因此将这两者融合能够减小每一步的位置估计误差，从而减小全程的累积误差。装备在行人侧的雷达本身属于自收自发设备，因此无需额外的基础设施部署。本发明提出的步操作分配和距离异常值修正方法，能够分场景地对不同类型的轨迹段采取相应的位置估计方法，减少环境中障碍物对距离观测产生的影响，扩展了方案的应用场景。

Description

一种基于惯性传感器和雷达的行人航位推算方法和***
技术领域
本发明属于通信与无线网络技术领域，更具体地，涉及一种基于惯性传感器和雷达的行人航位推算方法和***。
背景技术
基于惯性传感器的行人航位推算算法是最经典的行人航位推算方法，其处理流程是：行人装备惯性传感器(加速度计和陀螺仪)，通过加速度进行步态划分并估计每一步的步长；通过陀螺仪估计每一步的航向增量，由此可以得到每一步的相对位置变化。从给定的起始位置开始，逐步地叠加每一步的相对位置变化，最终可以递推出整个行人运动轨迹。该方式存在以下问题：每一步的位置估计误差会因为这种递推计算方式而累积，最终导致行人定位误差随着运动距离线性增长。
为了解决上述问题，人们在惯性传感器的基础上又融合了额外的技术和/或信息来实现行人航位推算。常见的技术包括Wi-Fi、蓝牙、超宽带等，常见的信息是室内地图信息。专利CN110602647A公开一种基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，通过引入Wi-Fi技术和室内地图信息来辅助行人航位推算。然而，该方法需要环境中额外部署Wi-Fi接入点，并且需要已知室内地图信息。
发明内容
针对现有技术的缺陷和改进需求，本发明提供了一种基于惯性传感器和雷达的行人航位推算方法和***，其目的在于通过引入额外技术(雷达)，采用多源信息融合，在无需额外基础设施部署和详细室内地图信息的情形下，同时减小行人航位推算存在的累积误差。
为实现上述目的，按照本发明的第一方面，提供了一种基于惯性传感器和雷达的行人航位推算方法，该方法包括以下步骤：
S1.惯性传感器按照设定的采集频率持续地收集行人行走过程中的加速度和角速度，所述惯性传感器装备在行人身上，雷达按照设定的采集频率持续地收集行人行走过程中行人到墙壁距离，所述雷达装备在行人身上，测距方向与行人前进方向的夹角处于区间[45°,135°]，在行走过程中保持不变；
S2.根据采集到的数据进行步态分割，得到各步伐对应的时段，并按照划分结果得到各步伐对应的加速度序列、角速度序列和墙壁距离序列；
S3.对于每个步伐，进行以下处理：
A.基于当前步伐对应的加速度序列进行步长估计，得到当前步伐的步长，基于当前步伐对应的角速度序列进行航向估计，得到当前步伐的航向，其与上一步伐的航向的差值作为当前步伐的航向增量；
B.对当前步伐的步长、航向增量和墙壁距离以及上一步伐的行人姿态进行多源信息融合，估计出当前步伐的行人姿态；
所述多源信息融合包括：基于当前步伐的墙壁距离和上一步伐的行人姿态对墙壁参数进行更新，将当前步伐的步长、航向增量、墙壁距离、更新后的墙壁参数和上一步伐的行人姿态输入至粒子滤波器，估计出当前步伐的行人姿态，其中，上一步伐的行人姿态作为状态，当前步伐的步长和航向增量作为控制量，当前步伐的墙壁距离作为观测，更新后的墙壁参数作为观测模型参数。
优选地，在多源信息融合之前，采用以下方式进行墙壁距离异常值修正，得到修正后的墙壁距离：
(1)对墙壁距离序列进行卡尔曼滤波，得到滤波后墙壁距离序列；
(2)对滤波后墙壁距离序列的每个墙壁距离，当距离变化率估计值
Figure BDA0002791526760000031
大于某一预先设定的阈值Tv时，判定当前墙壁距离为异常观测值，进入步骤(3)，否则，判定为正常观测值，进入步骤(4)；
(3)采用以下修正式进行修正：
Figure BDA0002791526760000032
Figure BDA0002791526760000033
其中，
Figure BDA0002791526760000034
表示时刻n对应的距离修正值，
Figure BDA0002791526760000035
表示时刻n对应的距离变化趋势预测，γ是平滑参数，0<γ<1；
(4)直接使用标准卡尔曼滤波结果作为距离修正结果。
优选地，基于当前步伐的墙壁距离和上一步伐的行人姿态，采用扩展卡尔曼滤波算法对墙壁参数进行更新，其中，上一步伐的墙壁参数作为状态，当前步伐的墙壁距离作为观测，上一步伐的行人姿态作为观测模型参数。
优选地，步骤A之后，通过以下方式估计出当前步伐的行人姿态：
(1)当前步伐的航向增量大于设定阈值时，标记为转角步伐，使用转角步伐划分行人轨迹，将划分后的每段行动轨迹的前Ni个步伐标记为初始化步伐，将每段行动轨迹的剩余步伐标记为融合步伐；
(2)对于转角步伐，利用转角步伐对应的步长和航向增量，通过递推式得到当前行人姿态；对于初始化步伐，利用当前步伐对应的步长和航向增量，通过递推公式得到当前行人姿态，此外，基于当前行人姿态、前一行人姿态及当前步伐的墙壁距离序列进行墙壁参数初始值估计；对于融合步伐，采用步骤B中所述方式进行行人姿态估计和墙壁参数更新；
所述递推公式如下：
Figure BDA0002791526760000036
其中，sn表示当前步伐的行人姿态，它包括航向hn和二维平面坐标(xn,yn)，ln,θn分别表示当前步伐区间的步长和航向增量。
优选地，所述墙壁参数初始值估计具体如下：
对同一步伐区间，利用前后行人姿态及本区间内的距离观测计算观测对应的墙壁点，墙壁点计算式表示如下：
Figure BDA0002791526760000041
Figure BDA0002791526760000042
其中，
Figure BDA0002791526760000043
表示第n个步伐区间里的第m个墙壁点，(xn,m,yn,m)为对应的观测点，即距离观测dn,m产生时行人所在的位置；
再利用本区间内的所有墙壁点，通过最小二乘法对墙壁参数进行拟合，得到墙壁参数的初始值。
为实现上述目的，按照本发明的第二方面，提供了一种基于惯性传感器和雷达的行人航位推算***，该***包括：
惯性传感器，用于按照设定的采集频率持续地收集行人行走过程中的加速度和角速度，所述惯性传感器装备在行人身上；
雷达，用于按照设定的采集频率持续地收集行人行走过程中行人到墙壁距离，所述雷达装备在行人身上，测距方向与行人前进方向的夹角处于区间[45°,135°]，在行走过程中保持不变；
步态分割单元，用于根据采集到的数据进行步态分割，得到各步伐对应的时段，并按照划分结果得到各步伐对应的加速度序列、角速度序列和墙壁距离序列；
行人航位推算模块，用于对于每个步伐进行行人姿态估计，包括以下子模块：
步长估计模块，用于基于当前步伐对应的加速度序列进行步长估计，得到当前步伐的步长；航向增量计算模块，用于基于当前步伐对应的角速度序列进行航向估计，得到当前步伐的航向，其与上一步伐的航向的差值作为当前步伐的航向增量；多源信息融合模块，用于对当前步伐的步长、航向增量和墙壁距离以及上一步伐的行人姿态进行多源信息融合，估计出当前步伐的行人姿态；所述多源信息融合包括：基于当前步伐的墙壁距离和上一步伐的行人姿态对墙壁参数进行更新，将当前步伐的步长、航向增量、墙壁距离、更新后的墙壁参数和上一步伐的行人姿态输入至粒子滤波器，估计出当前步伐的行人姿态，其中，上一步伐的行人姿态作为状态，当前步伐的步长和航向增量作为控制量，当前步伐的墙壁距离作为观测，更新后的墙壁参数作为观测模型参数。
优选地，惯性传感器采用移动终端内置惯性传感器。
总体而言，通过本发明所构思的以上技术方案，能够取得以下有益效果：
(1)本发明使用扩展卡尔曼滤波器更新墙壁参数，再利用粒子滤波器融合墙壁参数，雷达测距结果以及惯性传感器测量估计出最终的位置。雷达采集的距离数据是独立于惯性传感器数据之外的新维度的数据，且同样与行人的状态(位置+航向)有关，因此将这两者融合能够减小每一步的位置估计误差，从而减小全程的累积误差。装备在行人侧的雷达本身属于自收自发设备，因此无需额外的基础设施部署。
(2)本发明提出的步操作分配和距离异常值修正方法，能够分场景地对不同类型的轨迹段采取相应的位置估计方法，减少环境中障碍物对距离观测产生的影响，扩展了方案的应用场景。
附图说明
图1为本发明提供的一种基于惯性传感器和雷达的行人航位推算方法流程图；
图2为本发明提供的目标区域示意图；
图3为本发明提供的L形路线上各对比方法的行人轨迹估计图；
图4为本发明提供的Z形路线上各对比方法的行人轨迹估计图；
图5为图3对应路线各对比方法的定位误差的累积分布函数图；
图6为图4对应路线各对比方法的定位误差的累积分布函数图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
为了解决现有行人航位推算方法中由于传感器误差累积而导致的位置轨迹误差随行人行走距离快速增长的技术问题，在本发明的一个实施例中，提供了一种基于惯性传感器和雷达的行人航位推算方法，主要流程如图1所示，包括：
(1)惯性传感器和雷达按照设定的采集频率持续地收集行人行走过程中的加速度、角速度和墙壁距离数据，对采集到的数据进行步态分割，并在各观测序列上标记分割结果；
(2)对每个步伐，通过惯性传感器数据计算出行人航位信息，包括步长和航向增量；
(3)对每个步伐，对原始距离测量值进行处理，包括步操作指示分配和异常距离修正；
(4)通过基于粒子滤波器和扩展卡尔曼滤波器的多源信息融合方法对步长，航向增量和墙壁距离进行融合，估计出行人姿态。
在本发明实施例中，步骤(1)包括：
(1.1)采用华为荣耀8手机内部的惯性传感器组以20Hz的频率进行数据采集，测量运动过程中的各向加速度和角速度；雷达通过支架与手机固定，测距方向与行人前进方向保持固定角度φ＝90°，数据采集频率为10Hz。本实施例采用小型近程雷达，其采用调频连续波。
(1.2)对加速度进行平滑处理和峰值检测，划分每个连续的波峰-波谷周期作为一个步伐区间，同时对角速度和距离数据对应时间索引进行标记。
在本发明实施例中，步骤(2)包括：
(2.1)提取竖直方向的加速度分量并进行峰值检测，检测得到波峰序列
Figure BDA0002791526760000071
和波谷序列
Figure BDA0002791526760000072
通过一个步伐区间内波峰波谷的数值计算出当前区间的步长ln，计算公式为
Figure BDA0002791526760000073
其中，k是一个由预备实验测试得到的步长比例参数，在本实施例中设置为k＝0.68。
(2.2)提取大地平面内的角速度分量，通过滤波处理后对角速度进行积分得到航向的观测值，对一个步伐区间内的航向的观测值进行求差，得到本区间内的航向增量θn。
在本发明实施例中，步骤(3)包括：
(3.1)基于惯性传感器数据进行步操作指示的分配，标识不同场景下的步伐区间应当采取的位置估计策略。
(3.2)对每一个步伐区间，基于惯性传感器数据进行转角检测，当检测到转角存在时分配操作指示I2；同时，在接下来的Ni个步伐区间分配操作指示I0，对整条轨迹的前Ni个步伐区间分配操作指示I0；此外，其余各区间全部分配操作指示I1。
其中，I0表示墙壁参数初始化区间，I1表示融合定位区间，I2表示惯性定位区间。在本实施例中，初始化区间数设定为Ni＝4。当前步伐的航向增量大于设定阈值(实施例中为25°)时，判定为转角步伐。
(3.3)对原始观测值进行标准的卡尔曼滤波，得到距离估计值
Figure BDA0002791526760000074
和距离变化率估计值
Figure BDA0002791526760000075
状态空间模型表示如下：
dn＝dn-1+vn
vn＝vn-1+qn
distn＝dn+rn
其中，dn,vn分别表示第n个距离状态和对应的距离变化率；qn,rn分别表示零均值，方差分别为Qn＝0.012和Rn＝0.22的高斯过程噪声和观测噪声；distn表示第n个原始距离观测值。
当距离变化率估计值
Figure BDA0002791526760000081
大于某一预先设定的阈值Tv时，则判定当前的第n个观测值为异常观测值；否则，判定为正常观测值。在本实施例中，阈值设定为Tv＝0.01。
(3.4)对正常观测值，直接使用标准卡尔曼滤波结果
Figure BDA0002791526760000082
作为距离修正结果，即
Figure BDA0002791526760000083
对异常观测值，采用以下修正式进行修正：
Figure BDA0002791526760000084
Figure BDA0002791526760000085
其中，
Figure BDA0002791526760000086
表示距离修正值，
Figure BDA0002791526760000087
表示距离变化趋势预测，γ是平滑参数，0<γ<1。在本实施例中，平滑参数设定为γ＝0.9。
在本发明实施例中，步骤(4)包括：
(4.1)当操作指示为I0时，依照以下递推式进行航位推算：
Figure BDA0002791526760000088
其中，sn表示当前的行人姿态，它包括航向hn和二维平面坐标(xn,yn)。ln,θn分别表示当前步伐区间的步长和航向增量。递推结果sn确定为本区间最终的位置估计结果。
对同一步伐区间，利用前后行人姿态及本区间内的距离观测计算观测对应的墙壁点，墙壁点计算式表示如下：
Figure BDA0002791526760000089
Figure BDA00027915267600000810
其中，
Figure BDA00027915267600000811
表示第n个步伐区间里的第m个墙壁点，(xn,m,yn,m)为对应的观测点，即距离观测dn,m产生时行人所在的位置。在本实施例中，假设M个观测点在行人的相邻两个位置间均匀分布。再利用本区间内的所有墙壁点通过最小二乘法(y＝kx+b)对墙壁参数进行拟合，得到墙壁参数的初始化值(ki,bi)。
(4.2)当操作指示为I1时，对本区间的步长和航向增量的误差按预设范围进行高斯分布采样。利用采样结果对步长和航向增量进行修正，并利用(4.1)中的递推式得到K个行人姿态粒子
Figure BDA0002791526760000091
对每个姿态粒子，计算距离观测预测值
Figure BDA0002791526760000092
距离观测预测式表示为
Figure BDA0002791526760000093
依据
Figure BDA0002791526760000094
与距离观测dn的差值确定粒子权重，粒子的权重计算式表示如下：
Figure BDA0002791526760000095
其中，
Figure BDA0002791526760000096
表示第n个步伐区间的第k个粒子的权重，Sn表示距离观测残差的方差。值得注意的是，这里的权重表达式相当于距离观测残差的一个零均值，方差为Sn的高斯分布。当某一粒子的残差的绝对值越大时，权重越小，表明该粒子很有可能是一个偏离真实值的猜测。当区间内的观测数目大于1时，会出现多个权重值，本实施例取它们的平均作为本区间最终的粒子权重。
依据粒子权重大小，对K个候选粒子进行L次随机采样，再对L个采样得到的粒子状态进行平均，确定为本区间最终的位置估计结果，在本实施例中K＝L＝128。
对同一步伐区间，利用前后行人姿态及本区间内的距离观测通过扩展卡尔曼滤波器(EKF)进行墙壁参数的实时更新，滤波器的状态空间表示如下：
on＝Fnon-1+wn
dn＝g(sn；on)+zn
其中，on＝(kn,bn)为本区间的墙壁参数状态，dn表示当前的距离观测，sn表示当前的行人姿态或观测点，wn和zn分别为零均值，方差为Wn＝[[0.01,0],[0,0.1]]和Zn＝2.4的高斯过程噪声和观测噪声。Fn为2维的单位矩阵，表示墙壁参数在整条轨迹上的一致性。g(·)为观测方程，描述了观测点、墙壁参数以及墙壁距离的关系，表示如下：
Figure BDA0002791526760000101
(4.3)当操作指示为I2时，利用本区间的步长和航向增量通过(4.1)中的递推式得到当前行人姿态，并确定为本区间最终的位置估计结果。
以下结果一个具体场景下的应用实例，对上述方法实施例所提供基于惯性传感器和调频连续波雷达的行人航位推算方法做进一步的解释说明。
在本实施例中，目标区域如图2所示。实验场景在一座教学楼的5层，共规划2条路线。其中，L形路线的长度为192m，Z形路线的长度为136.8m，轨迹形状为直线段与直角的组合，主要的墙壁距离误差干扰因素有门洞，通道和转角。
本发明使用荣耀8手机内置的惯性传感器(加速度计、陀螺仪)来收集行人运动过程中产生的惯性运动数据；使用英飞凌(Infineon)公司生产的基于24GHz的无线收发控制器BGT24MTR11和微控制单元XMC4200的小型FMCW雷达收集距离数据。雷达测距误差约为0.1m，工作在24GHz频段。距离数据更新速度为10Hz。雷达通过一个支架固定在手机上，使用USB接口与手机进行通信进行数据传输。
为了验证本发明的室内定位性能及各个组成部分的功能，提出了以下对比方法：
PurePDR：传统的行人航位推算方法，仅使用(4.1)的递推式计算行人运动轨迹；
RadarPDR：本发明提出的基于惯性传感器和调频连续波雷达的行人航位推算方法；
RadarPDR w/o DC：与本发明提出的RadarPDR方法相比，仅去除距离异常值修正部分；
RadarPDR w/o PU：与本发明提出的RadarPDR方法相比，仅去除基于EKF的墙壁参数更新部分。
图3是L形路线各方案的轨迹估计效果图，如图所示，相比其它方案RadarPDR对真实轨迹(如GroundTruth轨迹所示)偏移最小；图4是Z形路线各方案的轨迹估计效果图，与图3有类似的结论，RadarPDR最接近真实轨迹。图5、图6分别是L形轨迹和Z形轨迹定位误差的累计分布函数图形，从图中可以看出无论是L形轨迹还是Z形轨迹上，RadarPDR方案都具有最小的定位误差。
实验结果表明，RadarPDR方法具有最好的轨迹估计效果，在L-shape和Z-shape上相对传统PDR方法分别有48.1％和41.1％的提升；与RadarPDR w/o PU和RadarPDR w/o DC方法对比，分别论证了墙壁参数部分和墙壁距离修正部分的有效性。这说明本发明的完整方法能够有效地减小传统行人航位推算方法中随着行走距离快速增长的累积定位误差。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于惯性传感器和雷达的行人航位推算方法，其特征在于，该方法包括以下步骤：
S1.惯性传感器按照设定的采集频率持续地收集行人行走过程中的加速度和角速度，所述惯性传感器装备在行人身上，雷达按照设定的采集频率持续地收集行人行走过程中行人到墙壁距离，所述雷达装备在行人身上，测距方向与行人前进方向的夹角处于区间[45°,135°]，在行走过程中保持不变；
S2.根据采集到的数据进行步态分割，得到各步伐对应的时段，并按照划分结果得到各步伐对应的加速度序列、角速度序列和墙壁距离序列；
S3.对于每个步伐，进行以下处理：
A.基于当前步伐对应的加速度序列进行步长估计，得到当前步伐的步长，基于当前步伐对应的角速度序列进行航向估计，得到当前步伐的航向，其与上一步伐的航向的差值作为当前步伐的航向增量；
B.对当前步伐的步长、航向增量和墙壁距离以及上一步伐的行人姿态进行多源信息融合，估计出当前步伐的行人姿态；
所述多源信息融合包括：基于当前步伐的墙壁距离和上一步伐的行人姿态对墙壁参数进行更新，将当前步伐的步长、航向增量、墙壁距离、更新后的墙壁参数和上一步伐的行人姿态输入至粒子滤波器，估计出当前步伐的行人姿态，其中，上一步伐的行人姿态作为状态，当前步伐的步长和航向增量作为控制量，当前步伐的墙壁距离作为观测，更新后的墙壁参数作为观测模型参数。
2.如权利要求1所述的方法，其特征在于，在多源信息融合之前，采用以下方式进行墙壁距离异常值修正，得到修正后的墙壁距离：
(1)对墙壁距离序列进行卡尔曼滤波，得到滤波后墙壁距离序列；
(2)对滤波后墙壁距离序列的每个墙壁距离，当距离变化率估计值
Figure FDA0002791526750000025
大于某一预先设定的阈值Tv时，判定当前墙壁距离为异常观测值，进入步骤(3)，否则，判定为正常观测值，进入步骤(4)；
(3)采用以下修正式进行修正：
Figure FDA0002791526750000021
Figure FDA0002791526750000022
其中，
Figure FDA0002791526750000023
表示时刻n对应的距离修正值，
Figure FDA0002791526750000024
表示时刻n对应的距离变化趋势预测，γ是平滑参数，0<γ<1；
(4)直接使用标准卡尔曼滤波结果作为距离修正结果。
3.如权利要求1或2所述的方法，其特征在于，基于当前步伐的墙壁距离和上一步伐的行人姿态，采用扩展卡尔曼滤波算法对墙壁参数进行更新，其中，上一步伐的墙壁参数作为状态，当前步伐的墙壁距离作为观测，上一步伐的行人姿态作为观测模型参数。
4.如权利要求1至3任一项所述的方法，其特征在于，步骤A之后，通过以下方式估计出当前步伐的行人姿态：
(1)当前步伐的航向增量大于设定阈值时，标记为转角步伐，使用转角步伐划分行人轨迹，将划分后的每段行动轨迹的前Ni个步伐标记为初始化步伐，将每段行动轨迹的剩余步伐标记为融合步伐；
(2)对于转角步伐，利用转角步伐对应的步长和航向增量，通过递推式得到当前行人姿态；对于初始化步伐，利用当前步伐对应的步长和航向增量，通过递推公式得到当前行人姿态，此外，基于当前行人姿态、前一行人姿态及当前步伐的墙壁距离序列进行墙壁参数初始值估计；对于融合步伐，采用步骤B中所述方式进行行人姿态估计和墙壁参数更新；
所述递推公式如下：
Figure FDA0002791526750000031
其中，sn表示当前步伐的行人姿态，它包括航向hn和二维平面坐标(xn,yn)，ln,θn分别表示当前步伐区间的步长和航向增量。
5.如权利要求4所述的方法，其特征在于，所述墙壁参数初始值估计具体如下：
对同一步伐区间，利用前后行人姿态及本区间内的距离观测计算观测对应的墙壁点，墙壁点计算式表示如下：
Figure FDA0002791526750000032
Figure FDA0002791526750000033
其中，
Figure FDA0002791526750000034
表示第n个步伐区间里的第m个墙壁点，(xn,m,yn,m)为对应的观测点，即距离观测dn,m产生时行人所在的位置；
再利用本区间内的所有墙壁点，通过最小二乘法对墙壁参数进行拟合，得到墙壁参数的初始值。
6.一种基于惯性传感器和雷达的行人航位推算***，其特征在于，该***包括：
惯性传感器，用于按照设定的采集频率持续地收集行人行走过程中的加速度和角速度，所述惯性传感器装备在行人身上；
雷达，用于按照设定的采集频率持续地收集行人行走过程中行人到墙壁距离，所述雷达装备在行人身上，测距方向与行人前进方向的夹角处于区间[45°,135°]，在行走过程中保持不变；
步态分割单元，用于根据采集到的数据进行步态分割，得到各步伐对应的时段，并按照划分结果得到各步伐对应的加速度序列、角速度序列和墙壁距离序列；
行人航位推算模块，用于对于每个步伐进行行人姿态估计，包括以下子模块：
步长估计模块，用于基于当前步伐对应的加速度序列进行步长估计，得到当前步伐的步长；航向增量计算模块，用于基于当前步伐对应的角速度序列进行航向估计，得到当前步伐的航向，其与上一步伐的航向的差值作为当前步伐的航向增量；多源信息融合模块，用于对当前步伐的步长、航向增量和墙壁距离以及上一步伐的行人姿态进行多源信息融合，估计出当前步伐的行人姿态；所述多源信息融合包括：基于当前步伐的墙壁距离和上一步伐的行人姿态对墙壁参数进行更新，将当前步伐的步长、航向增量、墙壁距离、更新后的墙壁参数和上一步伐的行人姿态输入至粒子滤波器，估计出当前步伐的行人姿态，其中，上一步伐的行人姿态作为状态，当前步伐的步长和航向增量作为控制量，当前步伐的墙壁距离作为观测，更新后的墙壁参数作为观测模型参数。
7.如权利要求6所述的***，其特征在于，惯性传感器采用移动终端内置惯性传感器。
CN202011316318.2A 2020-11-23 2020-11-23 一种基于惯性传感器和雷达的行人航位推算方法和*** Active CN112539747B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011316318.2A CN112539747B (zh) 2020-11-23 2020-11-23 一种基于惯性传感器和雷达的行人航位推算方法和***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011316318.2A CN112539747B (zh) 2020-11-23 2020-11-23 一种基于惯性传感器和雷达的行人航位推算方法和***

Publications (2)

Publication Number Publication Date
CN112539747A true CN112539747A (zh) 2021-03-23
CN112539747B CN112539747B (zh) 2023-04-28

Family

ID=75014640

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011316318.2A Active CN112539747B (zh) 2020-11-23 2020-11-23 一种基于惯性传感器和雷达的行人航位推算方法和***

Country Status (1)

Country Link
CN (1) CN112539747B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113916221A (zh) * 2021-09-09 2022-01-11 北京理工大学 一种融合视觉里程计和bp网络的自适应行人航迹推算方法
CN114485647A (zh) * 2022-01-29 2022-05-13 宁波诺丁汉大学 一种运动轨迹确定方法、装置及***

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103267524A (zh) * 2013-04-24 2013-08-28 华中科技大学 一种穿戴式的人员步态检测室内定位***及方法
CN105547288A (zh) * 2015-12-08 2016-05-04 华中科技大学 一种煤矿井下移动设备自主定位的方法及***
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和***
CN106643739A (zh) * 2017-02-23 2017-05-10 烟台中飞海装科技有限公司 一种室内环境人员定位方法及***
US20190170871A1 (en) * 2017-11-22 2019-06-06 Foster-Miller, Inc. Airborne Wind Profiling Portable Radar System and Method
CN110602647A (zh) * 2019-09-11 2019-12-20 江南大学 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103267524A (zh) * 2013-04-24 2013-08-28 华中科技大学 一种穿戴式的人员步态检测室内定位***及方法
CN105547288A (zh) * 2015-12-08 2016-05-04 华中科技大学 一种煤矿井下移动设备自主定位的方法及***
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和***
CN106643739A (zh) * 2017-02-23 2017-05-10 烟台中飞海装科技有限公司 一种室内环境人员定位方法及***
US20190170871A1 (en) * 2017-11-22 2019-06-06 Foster-Miller, Inc. Airborne Wind Profiling Portable Radar System and Method
CN110602647A (zh) * 2019-09-11 2019-12-20 江南大学 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
THI THOAMAC 等: "Detection and Estimation of Moving obstacles for a UAV", 《IFAC-PAPERSONLINE》 *
彭锐等: "基于AHRS与PDR融合的个人室内自定位方法研究", 《高技术通讯》 *
王锐 等: "单线激光雷达与GNSS_INS的空间重构", 《光学精密工程》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113916221A (zh) * 2021-09-09 2022-01-11 北京理工大学 一种融合视觉里程计和bp网络的自适应行人航迹推算方法
CN113916221B (zh) * 2021-09-09 2024-01-09 北京理工大学 一种融合视觉里程计和bp网络的自适应行人航迹推算方法
CN114485647A (zh) * 2022-01-29 2022-05-13 宁波诺丁汉大学 一种运动轨迹确定方法、装置及***
WO2023142633A1 (zh) * 2022-01-29 2023-08-03 宁波诺丁汉大学 一种运动轨迹确定方法、装置及***

Also Published As

Publication number Publication date
CN112539747B (zh) 2023-04-28

Similar Documents

Publication Publication Date Title
CN103926925B (zh) 一种基于改进的vfh算法的定位与避障方法及机器人
CN110262495B (zh) 可实现移动机器人自主导航与精确定位的控制***及方法
CN111307162B (zh) 用于自动驾驶场景的多传感器融合定位方法
CN106384540B (zh) 车辆实时轨迹预测方法及预测***
CN104180805B (zh) 基于智能手机的室内行人定位与跟踪方法
CN108871336B (zh) 一种车辆位置估算***及方法
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN104061934B (zh) 基于惯性传感器的行人室内位置跟踪方法
JP5162849B2 (ja) 不動点位置記録装置
JP2022113746A (ja) 判定装置
CN110471441A (zh) 一种基于激光探测的四旋翼无人机避障方法
CN110208842A (zh) 一种车联网环境下车辆高精度定位方法
WO2018072350A1 (zh) 车辆轨迹预测方法及装置
CN104677361B (zh) 一种综合定位的方法
CN112539747A (zh) 一种基于惯性传感器和雷达的行人航位推算方法和***
US10921137B2 (en) Data generation method for generating and updating a topological map for at least one room of at least one building
CN108106613B (zh) 基于视觉辅助的定位方法与***
CN110361008A (zh) 地下车库自动泊车的定位方法及装置
EP2843364B1 (en) Method and system for estimating aircraft course
CN113029148A (zh) 一种基于航向角准确修正的惯性导航室内定位方法
CN108387233B (zh) 一种基于模糊逻辑的行人运动模式判断方法
CN109945864B (zh) 室内行车定位融合方法、装置、存储介质及终端设备
Deusch et al. Improving localization in digital maps with grid maps
Goli et al. Cooperative multi-sensor multi-vehicle localization in vehicular adhoc networks
CN114035154B (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