CN110207704B - 一种基于建筑楼梯场景智能识别的行人导航方法 - Google Patents

一种基于建筑楼梯场景智能识别的行人导航方法 Download PDF

Info

Publication number
CN110207704B
CN110207704B CN201910422538.4A CN201910422538A CN110207704B CN 110207704 B CN110207704 B CN 110207704B CN 201910422538 A CN201910422538 A CN 201910422538A CN 110207704 B CN110207704 B CN 110207704B
Authority
CN
China
Prior art keywords
time
pedestrian
scene
moment
matrix
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
CN201910422538.4A
Other languages
English (en)
Other versions
CN110207704A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201910422538.4A priority Critical patent/CN110207704B/zh
Publication of CN110207704A publication Critical patent/CN110207704A/zh
Application granted granted Critical
Publication of CN110207704B publication Critical patent/CN110207704B/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
    • 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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

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

本发明公开了一种基于建筑楼梯场景智能识别的行人导航方法。步骤为：将惯性传感器设置于行人足部，令行人在建筑内场景下进行多次行走，包括平地步行、上楼梯和下楼梯场景，建立基于步态特征的场景识别模型；行人在建筑物内行走时，采集惯性传感数据，包括加速度信息和陀螺仪信息；对行人行走的姿态、速度和位置进行预测；判断足部是否处于零速状态，若处于零速状态，则通过卡尔曼滤波器对预测的速度和位置信息进行校正；判断行人在建筑物内的行走场景，若行人处于上下楼梯状态，则基于建筑物地图中的楼梯位置，通过卡尔曼滤波对行人位置进行校正。本发明提高了行人在有楼梯的建筑物内行走时的定位精度。

Description

一种基于建筑楼梯场景智能识别的行人导航方法
技术领域
本发明属于行人导航技术领域，特别涉及了一种基于建筑楼梯场景的行人导航方法。
背景技术
行人导航系统是导航定位领域的一个重要分支，近年来得到了越来越多研究人员的关注，被广泛运用于救援抢险、军事作战等领域。传统行人导航主要采用GPS定位技术，但GPS信号在室内以及城市环境下存在信号丢失现象，且民用精度较差，无法满足人们室内的导航需求。随着微机电系统(micro-electro mechanical system,MEMS)技术的发展，MEMS惯性测量单元的体积小，功耗低，质量轻，便于携带等优点逐渐凸显，对于以MEMS-IMU为基础的室内行人导航系统的研究也已经成为热点。
惯性传感器存在着时间累计的漂移误差，是行人导航位置航向发散的主要误差来源。通过零速修正算法，可以抑制速度误差发散并提高导航精度。由于位置及航向误差的可观测性差，零速修正算法无法对位置及航向误差进行修正，随着时间的累积最终会使轨迹发散。
发明内容
为了解决上述背景技术提到的技术问题，本发明提出了一种基于建筑楼梯场景智能识别的行人导航方法。
为了实现上述技术目的，本发明的技术方案为：
一种基于建筑楼梯场景智能识别的行人导航方法，包括以下步骤：
(1)将惯性传感器设置于行人足部，令行人在建筑内场景下进行多次行走，包括平地步行、上楼梯和下楼梯场景，建立基于步态特征的场景识别模型；
(2)行人在建筑物内行走时，采集惯性传感数据，包括加速度信息和陀螺仪信息；
(3)基于惯性导航算法，对行人行走的姿态、速度和位置进行预测；
(4)基于惯性传感数据，判断足部是否处于零速状态，若处于零速状态，则通过卡尔曼滤波器对预测的速度和位置信息进行校正；否则，直接进入步骤(5)；
(5)基于惯性传感数据和步骤(1)建立的场景识别模型，判断行人在建筑物内的行走场景，若行人处于上下楼梯状态，则基于建筑物地图中的楼梯位置，通过卡尔曼滤波对行人位置进行校正；否则，跳转至步骤(2)。
进一步地，在步骤(1)中，建立基于步态特征的场景识别模型的具体过程如下：
(101)记行人的第j个步态为Tj，利用行人的步态特征将该单个步态Tj分为活动数据段Tj1和非活动数据段Tj2，取Tj1作为该步态的特征取值范围；
(102)记
Figure BDA0002066475480000021
为第j个步态采集的惯性样本数据，N为样本个数，则采集特征向量T＝(accxmin,accysk,acczmax,accmax,gyroxmean,gyroyvar,gyrozmax,gyrozmean,gyrozsk,gyrozvar,hdiff,Vmax)，上式中：
Figure BDA0002066475480000022
Figure BDA0002066475480000023
Figure BDA0002066475480000031
Figure BDA0002066475480000032
Figure BDA0002066475480000033
其中，
Figure BDA0002066475480000034
为第j个步态内的第n个时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量；
Figure BDA0002066475480000035
为第j个步态内的第n个时刻时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；Vn(jn)为第j个步态内的第n个时刻在导航系下的速度；n＝1,2,…,N；h(j)为第j个步态在导航系下的初始高度；E表示数学期望；
(103)采用随机森林算法训练场景识别模型。
进一步地，步骤(103)的具体过程如下：
(a)利用bootstrap采样为每棵决策树生成训练集：
记训练集D＝(X,Y)，X为获取的步态特征，Y为每个特征对应的标记，从数据集D中有放回地随机抽取与D同样大小的训练样本(D1,D2,...,Dn)，利用每个训练样本Di来训练构造一个决策树；
(b)构建决策树：
对于训练样本Di，从总属性S中随机不放回地抽取数量为L的部分属性，构成该决策树的属性集合Ai，利用其进行决策树训练；
(c)生成随机森林：
将平地步行场景标记为0，上楼梯场景标记为1，下楼梯场景标记为2，重复上述步骤(b)，训练获得多棵决策树得到场景识别模型。
进一步地，在步骤(3)中，采用下式预测姿态：
Figure BDA0002066475480000041
Figure BDA0002066475480000042
Figure BDA0002066475480000043
Figure BDA0002066475480000044
上式中：
Figure BDA0002066475480000045
Figure BDA0002066475480000046
Figure BDA0002066475480000047
其中，q0(k)、q1(k)、q2(k)、q3(k)为k时刻的姿态四元数，q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为k-1时刻的姿态四元数，
Figure BDA0002066475480000048
为k时刻机体系相对于惯性系的角速度，
Figure BDA0002066475480000049
为k时刻机体系相对于导航系的角速度
Figure BDA00020664754800000410
在机体系X、Y、Z轴上的分量，ΔT为采样周期，
Figure BDA00020664754800000411
为k-1时刻的姿态转移矩阵，
Figure BDA00020664754800000412
为k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量，
Figure BDA00020664754800000413
为k-1时刻载体速度在导航系X、Y、Z轴上的分量，L(k-1)、h(k-1)为k-1时刻载体的纬度和高度，RM、RN为地球的子午圈与卯酉圈半径，ωie为地球自转角速度；
采用下式预测速度：
Figure BDA0002066475480000051
Figure BDA0002066475480000052
Figure BDA0002066475480000053
上式中，，
Figure BDA0002066475480000054
分别为机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量的估计值，
Figure BDA0002066475480000055
为k时刻载体速度在导航系X、Y、Z轴上的分量，g为重力加速度；
采用下式预测位置：
Figure BDA0002066475480000056
Figure BDA0002066475480000057
Figure BDA0002066475480000058
上式中，λ(k)、L(k)、h(k)为k时刻经度、纬度与高度，λ(k-1)、L(k-1)、h(k-1)为k-1时刻经度、纬度与高度。
进一步地，在步骤(4)中，判断足部是否处于零速状态的方法如下：
利用三条件判别法判别零速时刻：
Figure BDA0002066475480000059
Figure BDA00020664754800000510
Figure BDA0002066475480000061
其中，
Figure BDA0002066475480000062
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量，
Figure BDA0002066475480000063
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量；
Figure BDA0002066475480000064
为判别阈值，w为滑动窗口，上方横线表示取平均运算；
最终的零速检测结果为ZUPT(k)＝C1&C2&C3，&表示与运算，ZUPT(k)＝1表示零速状态，ZUPT(k)＝0表示非零速状态。
进一步地，在步骤(4)中，通过卡尔曼滤波器对预测的速度和位置信息进行校正的过程如下：
(A)计算一步预测均方误差：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T上式中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；
Figure BDA0002066475480000065
Figure BDA0002066475480000066
I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，ΔT为采样周期，
Figure BDA0002066475480000067
Figure BDA0002066475480000068
为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵，
Figure BDA0002066475480000069
Figure BDA00020664754800000610
为姿态转移矩阵；W(k-1)＝[εrx εry εrz εax εay εaz]T为k-1时刻状态噪声，εrx、εry和εrz分别为
Figure BDA0002066475480000071
和
Figure BDA0002066475480000072
的模型噪声，εax、εay和εaz分别为
Figure BDA0002066475480000073
和
Figure BDA0002066475480000074
的模型噪声，
Figure BDA0002066475480000075
和
Figure BDA0002066475480000076
为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的量，
Figure BDA0002066475480000077
和
Figure BDA0002066475480000078
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；上标T表示矩阵转置；
(B)计算k时刻卡尔曼滤波器滤波增益：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
上式中，K(k)为k时刻的滤波增益；
Figure BDA0002066475480000079
为k时刻的量测噪声，diag表示矩阵对角化，
Figure BDA00020664754800000710
分别为
Figure BDA00020664754800000711
的噪声，
Figure BDA00020664754800000712
Figure BDA00020664754800000713
为载体速度在导航系X、Y、Z轴上的分量；H(k)为k时刻量测矩阵，H(k)＝[03×3 I3×3 03×3]；
(C)计算k时卡尔曼滤波器状态估计值：
Figure BDA00020664754800000714
上式中，
Figure BDA00020664754800000715
为k时刻状态量的估计值，
Figure BDA00020664754800000716
为k-1到k时刻的状态变量一步预测值，L、λ、h为经度、纬度和高度，θ、γ、ψ为俯仰角、横滚角和偏航角；Y(k)＝[0 0 0]T为k时刻的量测值；
(D)计算k时刻卡尔曼滤波器估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
上式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
进一步地，步骤(5)的具体过程如下：
(501)采用下式进行场景关联：
Figure BDA00020664754800000717
上式中，SR(k)为第k个步态的场景识别结果，0表示行人步态为平地步行，1表示行人步态为上楼梯，2表示步态为下楼梯，“||”表示“或”；
获取数据库中与行人当前位置最近的位置坐标，选取该点作为关联的楼梯位置点；
(502)采用卡尔曼滤波进行位置校正：
(502a)计算一步预测均方误差：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T上式中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；
Figure BDA0002066475480000081
Figure BDA0002066475480000082
I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，ΔT为采样周期，
Figure BDA0002066475480000083
Figure BDA0002066475480000084
为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵.
Figure BDA0002066475480000085
Figure BDA0002066475480000086
为姿态转移矩阵；W(k-1)＝[εrx εry εrz εax εay εaz]T为k-1时刻状态噪声，εrx、εry和εrz分别为
Figure BDA0002066475480000087
和
Figure BDA0002066475480000088
的模型噪声，εax、εay和εaz分别为
Figure BDA0002066475480000089
和
Figure BDA00020664754800000810
的模型噪声，
Figure BDA00020664754800000811
和
Figure BDA00020664754800000812
为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的量，
Figure BDA00020664754800000813
和
Figure BDA00020664754800000814
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；
(502b)计算k时刻卡尔曼滤波器滤波增益：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
上式中，K(k)为k时刻的滤波增益；R(k)＝diag([εLελ]2)为k时刻的量测噪声，diag表示矩阵对角化，εL、ελ分别为经度和纬度的噪声；H(k)为k时刻量测矩阵，H(k)＝[I2×2 02×302×2 02×2]，I2×2为2×2的单位矩阵，02×3为2×3的零矩阵，02×2为2×2的零矩阵；
(502c)计算k时刻扩展卡尔曼滤波器估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
上式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
采用上述技术方案带来的有益效果：
本发明基于建立的场景识别模型对行人行走的场景进行实时辨识，若处于上下楼梯阶段，则利用已知的楼梯位置对行人的位置信息进行校正。相对于传统的行人导航算法，本发明可以有效地提高行人在有楼梯的建筑物内行走时的定位精度。
附图说明
图1为本发明的方法流程图；
图2为采用传统方法得到的解算结果图，包括(a)、(b)两幅子图，分别为二维图和三维图；
图3为采用本发明方法得到的解算结果图，包括(a)、(b)两幅子图，分别为二维图和三维图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
本发明设计了一种基于建筑楼梯场景智能识别的行人导航方法，如图1所示，步骤如下。
步骤1：将惯性传感器设置于行人足部，令行人在建筑内场景下进行多次行走，建立步态特征库，包括平地步行、上楼梯和下楼梯场景，建立基于步态特征的场景识别模型；
步骤2：行人在建筑物内行走时，采集惯性传感数据，包括加速度信息和陀螺仪信息；
步骤3：基于惯性导航算法，对行人行走的姿态、速度和位置进行预测；
步骤4：基于惯性传感数据，判断足部是否处于零速状态，若处于零速状态，则通过卡尔曼滤波器对预测的速度和位置信息进行校正；否则，直接进入步骤5；
步骤5：基于惯性传感数据和步骤1建立的场景识别模型，判断行人在建筑物内的行走场景，若行人处于上下楼梯状态，则基于建筑物地图中的楼梯位置，通过卡尔曼滤波对行人位置进行校正；否则，跳转至步骤2。
在本实施例中，步骤1采用如下优选方案实现：
建立基于步态特征的场景识别模型的具体过程如下：
101、数据分割：记行人的第j个步态为Tj，利用行人的步态特征将该单个步态Tj分为活动数据段Tj1和非活动数据段Tj2，取Tj1作为该步态的特征取值范围；
102、特征提取：记
Figure BDA0002066475480000101
为第j个步态采集的惯性样本数据，N为样本个数，则采集特征向量T＝(accxmin,accysk,acczmax,accmax,gyroxmean,gyroyvar,gyrozmax,gyrozmean,gyrozsk,gyrozvar,hdiff,Vmax)，上式中：
Figure BDA0002066475480000102
Figure BDA0002066475480000111
Figure BDA0002066475480000112
Figure BDA0002066475480000113
Figure BDA0002066475480000114
其中，
Figure BDA0002066475480000115
为第j个步态内的第n个时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量；
Figure BDA0002066475480000116
为第j个步态内的第n个时刻时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；Vn(jn)为第j个步态内的第n个时刻在导航系下的速度；n＝1,2,…,N；h(j)为第j个步态在导航系下的初始高度；E表示数学期望；
(103)采用随机森林算法训练场景识别模型，具体过程如下：
(a)利用bootstrap采样为每棵决策树生成训练集：
记训练集D＝(X,Y)，X为获取的步态特征，Y为每个特征对应的标记，从数据集D中有放回地随机抽取与D同样大小的训练样本(D1,D2,...,Dn)，利用每个训练样本Di来训练构造一个决策树；
(b)构建决策树：
对于训练样本Di，从总属性S中随机不放回地抽取数量为L(通常
Figure BDA0002066475480000117
)的部分属性，构成该决策树的属性集合Ai，利用其进行决策树训练；在进行节点分裂时，采用的分裂规则是Gini值最小原则，其计算公式为；
Figure BDA0002066475480000118
上式中，pl为样本点属于第l类的概率；
(c)生成随机森林：
将平地步行场景标记为0，上楼梯场景标记为1，下楼梯场景标记为2，重复上述步骤(b)，训练获得多棵决策树得到场景识别模型。
在本实施例中，步骤3采用如下优选方案实现：
采用下式预测姿态：
Figure BDA0002066475480000121
Figure BDA0002066475480000122
Figure BDA0002066475480000123
Figure BDA0002066475480000124
上式中：
Figure BDA0002066475480000125
Figure BDA0002066475480000126
Figure BDA0002066475480000127
其中，q0(k)、q1(k)、q2(k)、q3(k)为k时刻的姿态四元数，q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为k-1时刻的姿态四元数，
Figure BDA0002066475480000128
为k时刻机体系相对于惯性系的角速度，
Figure BDA0002066475480000129
为k时刻机体系相对于导航系的角速度
Figure BDA00020664754800001210
在机体系X、Y、Z轴上的分量，ΔT为采样周期，
Figure BDA00020664754800001211
为k-1时刻的姿态转移矩阵，
Figure BDA00020664754800001212
为k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量，
Figure BDA00020664754800001213
为k-1时刻载体速度在导航系X、Y、Z轴上的分量，L(k-1)、h(k-1)为k-1时刻载体的纬度和高度，RM、RN为地球的子午圈与卯酉圈半径，ωie为地球自转角速度；
采用下式预测速度：
Figure BDA0002066475480000131
Figure BDA0002066475480000132
Figure BDA0002066475480000133
上式中，
Figure BDA0002066475480000134
分别为机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量的估计值，
Figure BDA0002066475480000135
为k时刻载体速度在导航系X、Y、Z轴上的分量，g为重力加速度；
采用下式预测位置：
Figure BDA0002066475480000136
Figure BDA0002066475480000137
Figure BDA0002066475480000138
上式中，λ(k)、L(k)、h(k)为k时刻经度、纬度与高度，λ(k-1)、L(k-1)、h(k-1)为k-1时刻经度、纬度与高度。
在本实施例中，步骤4采用如下优选方案实现：
利用三条件判别法判别零速时刻：
Figure BDA0002066475480000139
Figure BDA00020664754800001310
Figure BDA0002066475480000141
其中，
Figure BDA0002066475480000142
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量，
Figure BDA0002066475480000143
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量；
Figure BDA0002066475480000144
为判别阈值，w为滑动窗口，上方横线表示取平均运算；
最终的零速检测结果为ZUPT(k)＝C1&C2&C3，&表示与运算，ZUPT(k)＝1表示零速状态，ZUPT(k)＝0表示非零速状态。
通过卡尔曼滤波器对预测的速度和位置信息进行校正的过程如下：
(A)计算一步预测均方误差：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T上式中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；
Figure BDA0002066475480000145
Figure BDA0002066475480000146
I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，ΔT为采样周期，
Figure BDA0002066475480000147
Figure BDA0002066475480000148
为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵，
Figure BDA0002066475480000149
Figure BDA00020664754800001410
为姿态转移矩阵；W(k-1)＝[εrx εry εrz εax εay εaz]T为k-1时刻状态噪声，εrx、εry和εrz分别为
Figure BDA00020664754800001411
和
Figure BDA00020664754800001412
的模型噪声，εax、εay和εaz分别为
Figure BDA00020664754800001413
和
Figure BDA00020664754800001414
的模型噪声，
Figure BDA0002066475480000151
和
Figure BDA0002066475480000152
为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的量，
Figure BDA0002066475480000153
和
Figure BDA0002066475480000154
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；上标T表示矩阵转置；
(B)计算k时刻卡尔曼滤波器滤波增益：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
上式中，K(k)为k时刻的滤波增益；
Figure BDA0002066475480000155
为k时刻的量测噪声，diag表示矩阵对角化，
Figure BDA0002066475480000156
分别为
Figure BDA0002066475480000157
的噪声，
Figure BDA0002066475480000158
Figure BDA0002066475480000159
为载体速度在导航系X、Y、Z轴上的分量；H(k)为k时刻量测矩阵，H(k)＝[03×3 I3×3 03×3]；
(C)计算k时卡尔曼滤波器状态估计值：
Figure BDA00020664754800001510
上式中，
Figure BDA00020664754800001511
为k时刻状态量的估计值，
Figure BDA00020664754800001512
为k-1到k时刻的状态变量一步预测值，L、λ、h为经度、纬度和高度，θ、γ、ψ为俯仰角、横滚角和偏航角；Y(k)＝[0 0 0]T为k时刻的量测值；
(D)计算k时刻卡尔曼滤波器估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
上式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
在本实施例中，步骤5采用如下优选方案实现：
501、采用下式进行场景关联：
Figure BDA00020664754800001513
上式中，SR(k)为第k个步态的场景识别结果，0表示行人步态为平地步行，1表示行人步态为上楼梯，2表示步态为下楼梯，“||”表示“或”；
获取数据库中与行人当前位置最近的位置坐标，选取该点作为关联的楼梯位置点；
502、采用卡尔曼滤波进行位置校正：
(502a)计算一步预测均方误差：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T上式中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；
Figure BDA0002066475480000161
Figure BDA0002066475480000162
I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，ΔT为采样周期，
Figure BDA0002066475480000163
Figure BDA0002066475480000164
为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵.
Figure BDA0002066475480000165
Figure BDA0002066475480000166
为姿态转移矩阵；W(k-1)＝[εrx εry εrz εax εay εaz]T为k-1时刻状态噪声，εrx、εry和εrz分别为
Figure BDA0002066475480000167
和
Figure BDA0002066475480000168
的模型噪声，εax、εay和εaz分别为
Figure BDA0002066475480000169
和
Figure BDA00020664754800001610
的模型噪声，
Figure BDA00020664754800001611
和
Figure BDA00020664754800001612
为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的量，
Figure BDA00020664754800001613
和
Figure BDA00020664754800001614
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；
(502b)计算k时刻卡尔曼滤波器滤波增益：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
上式中，K(k)为k时刻的滤波增益；R(k)＝diag([εLελ]2)为k时刻的量测噪声，diag表示矩阵对角化，εL、ελ分别为经度和纬度的噪声；H(k)为k时刻量测矩阵，H(k)＝[I2× 202×302×2 02×2]，I2×2为2×2的单位矩阵，02×3为2×3的零矩阵，02×2为2×2的零矩阵；
(502c)计算k时刻扩展卡尔曼滤波器估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
上式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
在实际实验中，采用的惯性器件型号为MTW awinda。在实验场地内绕着走廊行走，并伴随有平地步行，上楼梯，下楼梯。总行走里程为1100m。在相同的环境下，分别采用传统导航方法和本发明导航方法解算，得到的结果分别如图2和图3所示，实验精度对比如表1所示。
表1
实验结果 传统方法 本发明方法
最大位置误差 12m 1.9m
航向漂移 13° 1°
定位精度 1.1％ 0.2％
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (6)

1.一种基于建筑楼梯场景智能识别的行人导航方法，其特征在于，包括以下步骤：
(1)将惯性传感器设置于行人足部，令行人在建筑内场景下进行多次行走，包括平地步行、上楼梯和下楼梯场景，建立基于步态特征的场景识别模型；
建立基于步态特征的场景识别模型的具体过程如下：
(101)记行人的第j个步态为Tj，利用行人的步态特征将单个步态Tj分为活动数据段Tj1和非活动数据段Tj2，取Tj1作为该步态的特征取值范围；
(102)记
Figure FDA0003005689670000011
为第j个步态采集的惯性样本数据，N为样本个数，则采集特征向量T＝(accxmin,accysk,acczmax,accmax,gyroxmean,gyroyvar,gyrozmax,gyrozmean,gyrozsk,gyrozvar,hdiff,Vmax)，上式中：
Figure FDA0003005689670000012
Figure FDA0003005689670000013
Figure FDA0003005689670000014
Figure FDA0003005689670000015
Figure FDA0003005689670000016
hdiff＝h(j+1)-h(j)，
Figure FDA0003005689670000017
其中，
Figure FDA0003005689670000018
为第j个步态内的第n个时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量；
Figure FDA0003005689670000019
为第j个步态内的第n个时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的量；Vn(jn)为第j个步态内的第n个时刻在导航系下的速度；n＝1,2,…,N；h(j)为第j个步态在导航系下的初始高度；E表示数学期望；
(103)采用随机森林算法训练场景识别模型；
(2)行人在建筑物内行走时，采集惯性传感数据，包括加速度信息和陀螺仪信息；
(3)基于惯性导航算法，对行人行走的姿态、速度和位置进行预测；
(4)基于惯性传感数据，判断足部是否处于零速状态，若处于零速状态，则通过卡尔曼滤波器对预测的速度和位置信息进行校正；否则，直接进入步骤(5)；
(5)基于惯性传感数据和步骤(1)建立的场景识别模型，判断行人在建筑物内的行走场景，若行人处于上下楼梯状态，则基于建筑物地图中的楼梯位置，通过卡尔曼滤波对行人位置进行校正；否则，跳转至步骤(2)。
2.根据权利要求1所述基于建筑楼梯场景智能识别的行人导航方法，其特征在于，步骤(103)的具体过程如下：
(a)利用bootstrap采样为每棵决策树生成训练集：
记训练集D＝(X1,Y1)，X1为获取的步态特征，Y1为每个特征对应的标记，从数据集D中有放回地随机抽取与D同样大小的训练样本(D1,D2,...,Dn)，利用每个训练样本Di来训练构造一个决策树；
(b)构建决策树：
对于训练样本Di，从总属性S中随机不放回地抽取数量为L的部分属性，构成该决策树的属性集合Ai，利用其进行决策树训练；
(c)生成随机森林：
将平地步行场景标记为0，上楼梯场景标记为1，下楼梯场景标记为2，重复上述步骤(b)，训练获得多棵决策树得到场景识别模型。
3.根据权利要求1所述基于建筑楼梯场景智能识别的行人导航方法，其特征在于，在步骤(3)中，采用下式预测姿态：
Figure FDA0003005689670000031
Figure FDA0003005689670000032
Figure FDA0003005689670000033
Figure FDA0003005689670000034
上式中：
Figure FDA0003005689670000035
Figure FDA0003005689670000036
Figure FDA0003005689670000037
其中，q0(k)、q1(k)、q2(k)、q3(k)为k时刻的姿态四元数，q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)为k-1时刻的姿态四元数，
Figure FDA0003005689670000038
为k时刻机体系相对于惯性系的角速度，
Figure FDA0003005689670000039
为k时刻机体系相对于导航系的角速度
Figure FDA00030056896700000310
在机体系X、Y、Z轴上的分量，ΔT为采样周期，
Figure FDA00030056896700000311
为k-1时刻的姿态转移矩阵，
Figure FDA00030056896700000312
为k-1时刻导航系相对于惯性系的角速度在导航系X、Y、Z轴上的分量，
Figure FDA00030056896700000313
为k-1时刻载体速度在导航系X、Y、Z轴上的分量，L(k-1)、h(k-1)为k-1时刻载体的纬度和高度，RM、RN为地球的子午圈与卯酉圈半径，ωie为地球自转角速度；
采用下式预测速度：
Figure FDA0003005689670000041
Figure FDA0003005689670000042
Figure FDA0003005689670000043
上式中，
Figure FDA0003005689670000044
分别为机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量的估计值，
Figure FDA0003005689670000045
为k时刻载体速度在导航系X、Y、Z轴上的分量，g为重力加速度；
采用下式预测位置：
Figure FDA0003005689670000046
Figure FDA0003005689670000047
Figure FDA0003005689670000048
上式中，λ(k)、L(k)、h(k)为k时刻经度、纬度与高度，λ(k-1)、L(k-1)、h(k-1)为k-1时刻经度、纬度与高度。
4.根据权利要求1所述基于建筑楼梯场景智能识别的行人导航方法，其特征在于，在步骤(4)中，判断足部是否处于零速状态的方法如下：
利用三条件判别法判别零速时刻：
Figure FDA0003005689670000049
Figure FDA00030056896700000410
Figure FDA0003005689670000051
其中，
Figure FDA0003005689670000052
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的分量，
Figure FDA0003005689670000053
为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的分量；
Figure FDA0003005689670000054
为判别阈值，w为滑动窗口，上方横线表示取平均运算；
最终的零速检测结果为ZUPT(k)＝C1&C2&C3，&表示与运算，ZUPT(k)＝1表示零速状态，ZUPT(k)＝0表示非零速状态。
5.根据权利要求1所述基于建筑楼梯场景智能识别的行人导航方法，其特征在于，在步骤(4)中，通过卡尔曼滤波器对预测的速度和位置信息进行校正的过程如下：
(A)计算一步预测均方误差：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
上式中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；
Figure FDA0003005689670000055
Figure FDA0003005689670000056
I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，ΔT为采样周期，
Figure FDA0003005689670000057
Figure FDA0003005689670000058
为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵，
Figure FDA0003005689670000059
Figure FDA00030056896700000510
为姿态转移矩阵；W(k-1)＝[εrx εry εrz εax εay εaz]T为k-1时刻状态噪声，εrx、εry和εrz分别为
Figure FDA0003005689670000061
和
Figure FDA0003005689670000062
的模型噪声，εax、εay和εaz分别为
Figure FDA0003005689670000063
和
Figure FDA0003005689670000064
的模型噪声，
Figure FDA0003005689670000065
和
Figure FDA0003005689670000066
为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的量，
Figure FDA0003005689670000067
和
Figure FDA0003005689670000068
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；上标T表示矩阵转置；
(B)计算k时刻卡尔曼滤波器滤波增益：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
上式中，K(k)为k时刻的滤波增益；
Figure FDA0003005689670000069
为k时刻的量测噪声，diag表示矩阵对角化，
Figure FDA00030056896700000610
分别为
Figure FDA00030056896700000611
的噪声，
Figure FDA00030056896700000612
Figure FDA00030056896700000613
为载体速度在导航系X、Y、Z轴上的分量；H(k)为k时刻量测矩阵，H(k)＝[03×3 I3×3 03×3]；
(C)计算k时卡尔曼滤波器状态估计值：
Figure FDA00030056896700000614
上式中，
Figure FDA00030056896700000615
为k时刻状态量的估计值，
Figure FDA00030056896700000616
为k-1到k时刻的状态变量一步预测值，L、λ、h为经度、纬度和高度，θ、γ、ψ为俯仰角、横滚角和偏航角；Y(k)＝[0 0 0]T为k时刻的量测值；
(D)计算k时刻卡尔曼滤波器估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
上式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
6.根据权利要求1所述基于建筑楼梯场景智能识别的行人导航方法，其特征在于，步骤(5)的具体过程如下：
(501)采用下式进行场景关联：
Figure FDA0003005689670000071
上式中，SR(k)为第k个步态的场景识别结果，0表示行人步态为平地步行，1表示行人步态为上楼梯，2表示步态为下楼梯，“||”表示“或”；
获取数据库中与行人当前位置最近的位置坐标，选取该位置坐标作为关联的楼梯位置点；
(502)采用卡尔曼滤波进行位置校正：
(502a)计算一步预测均方误差：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
上式中，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；
Figure FDA0003005689670000072
Figure FDA0003005689670000073
I3×3为3×3的单位矩阵，03×3为3×3的零矩阵，ΔT为采样周期，
Figure FDA0003005689670000074
Figure FDA0003005689670000075
为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
G(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵，
Figure FDA0003005689670000076
Figure FDA0003005689670000077
为姿态转移矩阵；W(k-1)＝[εrx εry εrz εax εay εaz]T为k-1时刻状态噪声，εrx、εry和εrz分别为
Figure FDA0003005689670000078
和
Figure FDA0003005689670000079
的模型噪声，εax、εay和εaz分别为
Figure FDA00030056896700000710
和
Figure FDA00030056896700000711
的模型噪声，
Figure FDA00030056896700000712
和
Figure FDA00030056896700000713
为k时刻机体系相对于惯性系的角速度在机体系X、Y、Z轴上的量，
Figure FDA00030056896700000714
和
Figure FDA00030056896700000715
为k时刻机体系相对于惯性系的加速度在机体系X、Y、Z轴上的量；
(502b)计算k时刻卡尔曼滤波器滤波增益：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
上式中，K(k)为k时刻的滤波增益；R(k)＝diag([εL ελ]2)为k时刻的量测噪声，diag表示矩阵对角化，εL、ελ分别为经度和纬度的噪声；H(k)为k时刻量测矩阵，H(k)＝[I2×2 02×302×2 02×2]，I2×2为2×2的单位矩阵，02×3为2×3的零矩阵，02×2为2×2的零矩阵；
(502c)计算k时刻扩展卡尔曼滤波器估计均方误差：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
上式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
CN201910422538.4A 2019-05-21 2019-05-21 一种基于建筑楼梯场景智能识别的行人导航方法 Active CN110207704B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910422538.4A CN110207704B (zh) 2019-05-21 2019-05-21 一种基于建筑楼梯场景智能识别的行人导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910422538.4A CN110207704B (zh) 2019-05-21 2019-05-21 一种基于建筑楼梯场景智能识别的行人导航方法

Publications (2)

Publication Number Publication Date
CN110207704A CN110207704A (zh) 2019-09-06
CN110207704B true CN110207704B (zh) 2021-07-13

Family

ID=67787920

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910422538.4A Active CN110207704B (zh) 2019-05-21 2019-05-21 一种基于建筑楼梯场景智能识别的行人导航方法

Country Status (1)

Country Link
CN (1) CN110207704B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111076718B (zh) * 2019-12-18 2021-01-15 中铁电气化局集团有限公司 一种地铁列车的自主导航定位方法
CN111368749B (zh) * 2020-03-06 2023-06-13 创新奇智(广州)科技有限公司 一种楼梯区域的自动识别方法及系统
CN111649742B (zh) * 2020-05-08 2022-02-08 北京航空航天大学 一种基于anfis辅助的高程估计方法
CN111596325A (zh) * 2020-05-09 2020-08-28 北京东方海龙消防科技有限公司 一种定位搜救系统
CN112418649B (zh) * 2020-11-19 2022-11-11 东南大学 基于多维mems惯性传感器的建筑楼梯人流量估计系统
CN113295158B (zh) * 2021-05-14 2024-05-14 江苏大学 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法
CN113720332B (zh) * 2021-06-30 2022-06-07 北京航空航天大学 一种基于层高模型的楼层自主识别方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105628027A (zh) * 2016-02-19 2016-06-01 中国矿业大学 一种基于mems惯性器件的室内环境精确实时定位方法
CN106017461A (zh) * 2016-05-19 2016-10-12 北京理工大学 基于人体/环境约束的行人导航系统三维空间定位方法
CN106482733A (zh) * 2016-09-23 2017-03-08 南昌大学 行人导航中基于足底压力检测的零速修正方法
CN106908060A (zh) * 2017-02-15 2017-06-30 东南大学 一种基于mems惯性传感器的高精度室内定位方法
CN108759814A (zh) * 2018-04-13 2018-11-06 南京航空航天大学 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102060723B1 (ko) * 2013-01-18 2019-12-31 삼성전자주식회사 사용자 행동 인식 장치, ｏｎｍｆ 기반 기저 행렬 생성 방법 및 ｏｓｓｎｍｆ 기반 기저 행렬 생성 방법
CN109298389B (zh) * 2018-08-29 2022-09-23 东南大学 基于多粒子群优化的室内行人组合位姿估计方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105628027A (zh) * 2016-02-19 2016-06-01 中国矿业大学 一种基于mems惯性器件的室内环境精确实时定位方法
CN106017461A (zh) * 2016-05-19 2016-10-12 北京理工大学 基于人体/环境约束的行人导航系统三维空间定位方法
CN106482733A (zh) * 2016-09-23 2017-03-08 南昌大学 行人导航中基于足底压力检测的零速修正方法
CN106908060A (zh) * 2017-02-15 2017-06-30 东南大学 一种基于mems惯性传感器的高精度室内定位方法
CN108759814A (zh) * 2018-04-13 2018-11-06 南京航空航天大学 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
李辰祥.基于MEMS行人惯性导航的零速度修正技术研究 李辰祥 I136-605.《中国优秀硕士学位论文全文数据库 信息科技辑》.2014,第I136-605页. *
行人自主导航算法研究;左德胜;《中国优秀硕士学位论文全文数据库 信息科技辑》;20190115;第I136-1955页 *

Also Published As

Publication number Publication date
CN110207704A (zh) 2019-09-06

Similar Documents

Publication Publication Date Title
CN110207704B (zh) 一种基于建筑楼梯场景智能识别的行人导航方法
CN110118549B (zh) 一种多源信息融合定位方法和装置
US10267646B2 (en) Method and system for varying step length estimation using nonlinear system identification
CN110553643B (zh) 一种基于神经网络的行人自适应零速更新点选取方法
CN109297485B (zh) 一种基于高度自观测算法的室内个人惯性导航高程精度提升方法
EP3680618A1 (en) Method and system for tracking a mobile device
CN107339992B (zh) 一种基于行为的室内定位和地标语义标识的方法
CN107796391A (zh) 一种捷联惯性导航系统/视觉里程计组合导航方法
CN108426582B (zh) 行人室内三维地图匹配方法
CN108537101B (zh) 一种基于状态识别的行人定位方法
CN104061934A (zh) 基于惯性传感器的行人室内位置跟踪方法
Deng et al. Foot-mounted pedestrian navigation method based on gait classification for three-dimensional positioning
WO2016013095A1 (ja) 自律移動装置
CN104864873B (zh) 一种利用人体运动特征辅助地图定位的方法
CN109211229A (zh) 一种基于手机传感器及WiFi特征的人员室内定位方法
CN107702712A (zh) 基于惯性测量双层wlan指纹库的室内行人组合定位方法
Xiao et al. Residual attention network-based confidence estimation algorithm for non-holonomic constraint in GNSS/INS integrated navigation system
CN113188557A (zh) 一种融合语义特征的视觉惯性组合导航方法
Xia et al. Autonomous pedestrian altitude estimation inside a multi-story building assisted by motion recognition
CN116448103A (zh) 基于uwb测距辅助的行人足绑式惯性导航系统误差修正方法
CN113848878B (zh) 基于众源数据的室内外三维行人路网构建方法
Lin et al. LocMe: Human locomotion and map exploitation based indoor localization
CN114674317A (zh) 基于活动识别和融合滤波的自校正航位推算系统及方法
CN110332936B (zh) 基于多传感器的室内运动轨迹导航方法
CN117178292A (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