CN113295158B - 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法 - Google Patents
一种融合惯性数据、地图信息以及行人运动状态的室内定位方法 Download PDFInfo
- Publication number
- CN113295158B CN113295158B CN202110526323.4A CN202110526323A CN113295158B CN 113295158 B CN113295158 B CN 113295158B CN 202110526323 A CN202110526323 A CN 202110526323A CN 113295158 B CN113295158 B CN 113295158B
- Authority
- CN
- China
- Prior art keywords
- pedestrian
- axis
- acceleration
- motion
- pedestrians
- 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
- 230000033001 locomotion Effects 0.000 title claims abstract description 107
- 238000000034 method Methods 0.000 title claims abstract description 63
- 230000001133 acceleration Effects 0.000 claims description 70
- 239000011159 matrix material Substances 0.000 claims description 39
- 230000008569 process Effects 0.000 claims description 36
- 230000005021 gait Effects 0.000 claims description 23
- 239000013598 vector Substances 0.000 claims description 22
- 238000006073 displacement reaction Methods 0.000 claims description 19
- 238000001914 filtration Methods 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 14
- 230000005484 gravity Effects 0.000 claims description 13
- 238000012706 support-vector machine Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 7
- 238000005259 measurement Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 230000006399 behavior Effects 0.000 claims description 5
- 238000013145 classification model Methods 0.000 claims description 5
- 230000001174 ascending effect Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 2
- 238000000605 extraction Methods 0.000 abstract description 2
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000012549 training Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 206010017577 Gait disturbance Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011897 real-time detection Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004826 seaming Methods 0.000 description 1
- 238000000926 separation method Methods 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- 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
技术领域
本发明属于惯性定位领域，具体涉及到融合惯性数据、地图信息和行人运动状态的室内定位方向。
背景技术
惯性定位又叫做自主定位，其定位原理是利用陀螺仪、加速度计等组合测量单元，实时获取人员的位置、速度和方向等信息，实现定位。惯性定位的优势在于受外界干扰小，自主性强。在室内无线信号传播条件差的情况下，例如火灾救援中，惯性定位具有极大优势。此外，极强的自主性使得惯性导航技术在极地导航方面也有着巨大发展前景。
随着对定位精度和长时间定位的要求越来越高，如何提高惯性定位的定位精度并延长定位时间，是一个巨大的挑战。为此，学者提出了多种方案。
传统的捷联式惯性导航***(SINS)利用加速度计和陀螺仪采集数据并进行不断积分来获取行人的方向与速度信息，从而确定行人的位置。然而，由于推算结果的不断迭代，其测量误差会随着时间累计，严重地制约了该类***的连续定位能力。因而现在普遍采用步长和步数乘积得到移动距离的方法来实现定位，也就是行人航位推算(PDR)。
行人航位推算避免了对传感器数据的直接积分运算，转而通过研究行人步行时的肢体运动特征，根据传感器所得加速度数据，判断行人运动姿态，直接估计行人的步伐数目与步伐长度。利用磁力传感器的数据计算获取或通过电子罗盘直接获取行人的航向角。
南京邮电大学的李一帆针对传统基于二次积分运算的捷联式惯性导航***算法中的累计误差问题，在传感节点平台上提出了一种以行人步伐为周期的分段式轨迹推算算法。Sang等人改进了MEMS传感器的误差补偿和算法，提高了定位精度。武汉大学陈锐志教授针对加速度计测量存在重力干扰和外部抖动干扰的问题，提出了基于EMG传感器的步态和步长估计的创新性算法。针对人员非步态扰动以及传感器自身测量误差产生漂移情况下，传感器信号采集的波动干扰大的问题，重庆理工大学涂巧玲教授提出一种改进的卡尔曼滤波。
但目前针对惯性定位的研究主要聚焦于减少惯性定位的累计误差，提升惯性定位的精度，仅能够延长惯性定位可靠工作的时间，仍然无法保证其长时间的工作，因此，本文针对惯性定位结果漂移的问题，提出一种融合算法以避免定位结果的发散。
发明内容
本发明提出融合惯性数据、地图信息以及行人运动状态的室内定位方法，使定位误差始终处于一定的较小范围内，保证了利用低精度惯性传感器定位的可靠度，满足行人需求。本发明利用布置在行人腰部的惯性传感器采集行人的惯性数据，使用卡尔曼滤波削弱***中噪声和干扰的影响，再根据X轴加速度峰值出现阶段划分步态周期。在提取关键地标处行人在每一步态周期内的运动特征后，将特征向量放入支持向量机(SVM)中训练模型，从而得到能够识别行人运动状态的模型，并通过识别行人的运动状态推断出行人所处的关键地标的类型。进一步地，当提取行人步态周期内的运动特征并放入分类器中判定出行人m时刻所处的关键地标类型后，根据预先存储的关键地标坐标以及根据惯性数据积分解算出的行人位置，选择距离行人最近的关键地标的坐标作为m时刻行人坐标的参考值，并融合m-n时刻行人的位移和修正前的行人位置计算出修正后的行人坐标。
本发明的技术方案为：一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，包括以下步骤：
步骤1，搜集地图信息：首先选取关键地标及行人特殊运动行为，然后搜集关键地标信息；步骤2，建立行人位于关键地标处的运动模型：包括采集行人的惯性数据，处理惯性数据，划分行人正常行走以及行人停止运动的步态周期并提取运动特征，最后建立运动数据库；步骤3，识别行人运动状态并推断行人所处位置，实现位置修正：实时采集行人运动的惯性数据，解算行人当前坐标，实时检测行人运动状态，判断行人所处的关键地标类型，修正定位坐标。
进一步，步骤1的具体过程为：
选取行人所处的室内环境中的关键地标，关键地标的定义原则为：行人到达该类关键地标时，会有特有的、明显的且能够被惯性传感器捕捉到的运动状态的位置；由于行人在拐角处的运动状态为转弯，此时Z轴角速度有明显变化；在楼梯处行人的运动状态为上楼梯或是下楼梯，此时Z轴方向位移有明显变化，且加速度曲线波动较大；在乘坐扶梯和电梯时，行人的运动状态分别为乘扶梯上行或是乘扶梯下行和乘电梯上行或是乘电梯下行，此时Z轴方向位移有明显变化，且加速度曲线更为稳定；根据以上标准，选取拐弯的起始点和结束点、楼梯与平地的交界点、扶梯与平地的交界点以及电梯入口点作为关键地标。
进一步，所述步骤2中，采集行人的惯性数据的具体过程为：将惯性传感器放置在行人的腰部，以近似的描述人体质心的运动，传感器的布置方向为：Z轴方向与垂直于地面方向一致，X轴方向与行人前进方向一致，Y轴垂直于XZ平面，且与行人前进方向的右侧方向一致。
进一步，所述步骤2中，处理惯性数据主要包括滤波和坐标变换两个部分，具体过程为：
步骤2.1，卡尔曼滤波：为了削减惯性数据中***中的噪声和干扰的影响，采用卡尔曼滤波处理原始数据；
采用公式(1)预测本次状态：其中X(k)为当前时刻状态，X(k-1)为***上一刻的状态，A为状态转移矩阵；详细的，有：
其中，ax为X轴上的加速度，ay为Y轴上的加速度，az为Z轴上的加速度,ωx为X轴的角速度，ωx为Y轴的角速度，ωz为Z轴的角速度；
采用公式(2)计算数据的输出量：Y(k)＝H·X(k)(2)
其中，Y(k)为传感器的实际测量值，H＝A；
采用公式(3)预测本次协方差矩阵
其中P(k-1)为上一次的协方差矩阵，P(0)取单位矩阵，为当前协方差矩阵的预测结果，C为单位矩阵，Q为6阶对角矩阵，每个对角元素的值为对应变量的过程误差；
接着采用公式(4)，计算滤波增益矩阵：
其中，K(k)为滤波增益矩阵，R为噪声矩阵；
再然后，计算本次状态的最优估计：
其中，X(k)为最优估计值；
最后，利用公式(6)更新协方差矩阵，为下一次滤波做准备：
步骤2.2，利用欧拉角进行坐标变换
接着，使用公式(7)计算姿态角变换矩阵将惯性数据从惯性传感器自身的载体系oxbybzb转移到当地导航坐标系oxnynzn：
简写为：
则载体系oxbybzb和导航系oxnynzn之间的转换关系，即姿态矩阵为：
其中，α为偏航角、β为俯仰角，δ为横滚角，oxbybzb为固连在汽车上的载体坐标系，oxnynzn为用于导航的导航坐标系；根据姿态矩阵，将惯性传感器输出的载体坐标系下加速度信息转换到导航坐标系下：
其中，abx、aby、abz为载体坐标系下的XYZ三轴加速度，a′nx，a′ny，a′nz为消除重力影响前的导航坐标系下三轴加速度；
再消除重力加速度的影响，得到导航坐标系下的纯线性运动加速度：
其中，anx、any、anz为消除重力影响后的导航坐标系下的三轴加速度。
进一步，所述步骤2中，提取运动特征的具体过程为：
针对惯性数据提取每一步态周期内相应的特征值组成特征向量；由于行人运动至扶梯、电梯、楼梯处时，Z轴加速度以及Z轴方向上的位移有明显特征，因此根据公式(12)-(14)提取单个周期内的XYZ方向的平均加速度
其中anxi、anyi、anzi分别为i时刻导航坐标系下汽车的XYZ三轴加速度，N为周期内的采样点数量；根据公式(15)、(16)计算行人i时刻沿着Z轴的速度vzni以及沿X方向的速度vxni：
其中，vnz0为上一时刻Z轴速度，vnx0为上一时刻X轴速度anxi、anyi、anzi分别为i时刻导航坐标系下汽车的XYZ三轴加速度；
根据公式(17)、(18)提取单个周期行人沿着Z轴的位移Xz以及沿X方向的位移Xx：
由于行人在拐弯时Z轴角速度有明显变化，且相邻两个周期的平均角速度方向大多相同，提取单个周期内的Z轴的平均角速度；由于Y轴角速度曲线在上楼处较在其他关键位置处更加平滑,因此首先使用最小二乘法拟合出单个周期内ωy(k)的函数ωY(k)，再根据公式(19)计算单个周期内Y轴的平均曲率
其中，N为周期内的采样点数量，ωY(i)为拟合后的Y轴角速度函数；
根据公式(20)-(21)提取X、Y、Z方向上的标准差：
其中μx、μy、μz分别为X、Y、Z方向上加速度的算术平均值；根据公式(23)-(25)提取单个周期内的XYZ加速度的相关系数：
其中，为单个周期内的XYZ方向的平均加速度；
进一步，所述步骤3中，实时采集行人运动的惯性数据，解算行人当前坐标的具体过程为：
(1)估计步长
将获得的纯线性运动加速度进行一次积分便得到速度：
vn＝∫andt (26)
其中，an为导航坐标系下的行人加速度；由于传感器的数据是离散的，将公式(26)转化为差分方程形式：
将获得的速度信息进行积分便得到位置信息和步长，位置信息公式为：
Sn＝∫vndt (28)
转化为差分方程形式，有：
人体在行走过程中，两条腿会发生周期性的交替动作，人行走一步可以分解成抬脚和收脚两个过程，在抬脚过程中人的身体重心会上升,这样会产生Z方向上的加速度，而收脚过程身体重心下降产生-Z方向上的加速度，因此当行人连续行走时，在Z方向上的加速度会呈周期性的变化，其变化的周期个数便是行人行走的步数，记为N；
每一步的步长用如下公式：
(3)行人轨迹推算
行人的位置利用以下公式进行行人轨迹推算：
式中，n＝1,2,…,N，En为行人东向坐标，Nn为行人北向坐标，当n＝1时，E0和N0为初始位置的东向坐标和北向坐标；SLn、αn分别为第n步计算得到的步长和航向，N为最终步数。
进一步，所述步骤3中，实时检测行人运动状态的具体过程为：
实时提取每一步态周期的运动特征向量并放入分类模型中，根据公式(32)检测行人的运动状态：
y(k)＝ωF[x(k),xi]+b (32)
其中xi为支持向量，x(k)为第k个步态周期内行人运动的特征向量，ω为向量的权重因子,F[x(k),xi]为支持向量机的核函数，y(k)为第k个步态周期时行人的运动状态，b为支持向量机的阈值；
当y(k)的值为1、2、3、4、5、6、7、8时，分别对应在第k个周期时行人的运动状态为平地直行、拐弯、上楼梯、下楼梯、乘扶梯上行、乘扶梯下行、乘电梯上行以及乘电梯下行。
进一步，所述步骤3中，判断行人所处的关键地标类型的具体过程为：
将采集到的支持向量xi带入公式(32)，便可以得到行人的运动状态y(n)；
(5)判断行人位于拐弯的起始点与结束点
当在n时刻时，y(n-1)＝1，y(n)＝2，判定n时刻行人位于拐弯处的起始点，当在距离n时刻不久的m时刻时，y(m-1)＝2，y(m)＝1，则m时刻行人位于拐弯的结束点；
(6)判断行人位于楼梯与平地交界点
当在n时刻时，y(n-1)＝1，y(n)＝3或y(n)＝4，当在距离n时刻不久的m时刻时，y(m-1)＝3或y(m-1)＝4，y(m)＝1，则确定行人在n与m时刻位于楼梯与平地交界点；
(7)判断行人位于扶梯与平地交界点
当在n时刻时，y(n-1)＝1，y(n)＝5或y(n)＝6，当在距离n时刻不久的m时刻时，y(m-1)＝5或y(m-1)＝6，y(m)＝1，则确定行人在n与m时刻位于楼梯与平地交界点；
(8)判断行人位于电梯入口点
当在n时刻时，y(n-1)＝1，y(n)＝7或y(n)＝8，当在距离n时刻不久的m时刻时，y(m-1)＝7或y(m-1)＝8，y(m)＝1，则确定行人在n与m时刻位于电梯口。
进一步，所述步骤3中，修正定位坐标的具体过程为：
当判定行人在m时刻位于某一关键地标后，结合地图信息以及根据积分解算的行人位置，选择距离行人最近的该类关键地标的坐标作为修正行人坐标的参考坐标，再根据公式(33)修正行人的当前时刻，即n时刻的坐标；
X′(n)＝α[X(m)+ΔXn-m]+(1-α)X(n) (33)
其中，X′(n)为修正后行人的当前坐标,X(n)为修正前行人的当前坐标，X(m)为m时刻所经过的关键地标的坐标，ΔXn-m为从m时刻到n时刻行人的位移，α为可信度系数，取决于传感器的精度，取值范围为(0,1)。
本发明的有益效果：
(1)避免了惯性定位***长时间工作后定位结果发生漂移，误差过大的问题，保证了惯性定位***长时间、稳定且有效的工作。
(2)本文对运动行为识别采用的分类器为支持向量机SVM。该算法对于高维数据的分类速度快且准确性高，因而能够有效分辨不同关键地标处行人的运动行为。
附图说明
图1为该方法工作的总流程图
图2为卡尔曼滤波后的效果图
图3为载体坐标系和导航坐标系变换示意图
图4为步态周期划分的效果图
图5为惯性导航***计算流程图
具体实施方式
本方法主要包括搜集地图信息，建立关键位置处行人运动模型以及识别行人的运动状态并推断行人所处位置从而修正定位结果三个部分。
一、搜集地图信息
STEP1:选取关键地标及行人特殊运动行为
选取行人所处的室内环境中的关键地标，关键地标的定义原则为：行人到达该类关键地标时，会有特有的、明显的且能够被惯性传感器捕捉到的运动状态的位置。由于行人在拐角处的运动状态为转弯，此时Z轴角速度有明显变化；在楼梯处行人的运动状态为上楼梯或是下楼梯，此时Z轴方向位移有明显变化，且加速度曲线波动较大；在乘坐扶梯和电梯时，行人的运动状态分别为乘扶梯上行或是乘扶梯下行和乘电梯上行或是乘电梯下行，此时Z轴方向位移有明显变化，且加速度曲线更为稳定，因此便于在这些位置上提取行人的运动特征。
根据以上标准，选取拐弯的起始点和结束点、楼梯与平地的交界点、扶梯与平地的交界点以及电梯入口点作为关键地标。
STEP2:搜集关键地标信息
在选取完关键地标后，将行人即将经过的室内环境的所有关键地标信息，包括关键地标的类型以及关键地标的坐标搜集存入***中。
二、建立行人位于关键地标处的运动模型
建立行人位于关键地标处的运动模型，从而能够根据该模型，通过行人运动产生的惯性数据，判断出行人的运动状态，以推断行人所处的关键地标类型。建模过程主要包括：采集数据、处理数据、提取行人的运动特征以及使用分类器构建分类模型这几个步骤。具体包括：
STEP1:采集行人的惯性数据
在本发明中，将惯性传感器放置在行人的腰部，以近似的描述人体质心的运动。传感器的布置方向为：Z轴方向与垂直于地面方向一致，X轴方向与行人前进方向一致，Y轴垂直于XZ平面，且与行人前进方向的右侧方向一致。
STEP2:处理惯性数据
处理惯性数据主要包括滤波和坐标变换两个部分。
(1)卡尔曼滤波
为了削减惯性数据中***中的噪声和干扰的影响，采用卡尔曼滤波处理原始数据。
采用公式(1)预测本次状态：
其中X(k)为当前时刻状态，X(k-1)为***上一刻的状态，A为状态转移矩阵。详细的，有：
其中，ax为X轴上的加速度，ay为Y轴上的加速度，az为Z轴上的加速度,ωx为X轴的角速度，ωx为Y轴的角速度，ωz为Z轴的角速度。
采用公式(2)计算数据的输出量：
Y(k)＝H·X(k) (2)
其中，Y(k)为传感器的实际测量值，H＝A。
采用公式(3)预测本次协方差矩阵
其中P(k-1)为上一次的协方差矩阵，P(0)取单位矩阵，为当前协方差矩阵的预测结果，C为单位矩阵，Q为6阶对角矩阵，每个对角元素的值为对应变量的过程误差。
接着采用公式(4)，计算滤波增益矩阵
其中，K(k)为滤波增益矩阵，R为噪声矩阵。
再然后，计算本次状态的最优估计
其中，X(k)为最优估计值。
最后，利用公式(6)更新协方差矩阵，为下一次滤波做准备，滤波的结果如图2所示。
(2)利用欧拉角进行坐标变换
接着，使用公式(7)计算姿态角变换矩阵将惯性数据从惯性传感器自身的载体系oxbybzb转移到当地导航坐标系oxnynzn，坐标变换的过程如图2所示：
简写为：
则载体系oxbybzb和导航系oxnynzn之间的转换关系，即姿态矩阵为：
其中，α为偏航角、β为俯仰角，δ为横滚角，oxbybzb为固连在汽车上的载体坐标系，oxnynzn为用于导航的导航坐标系。
根据姿态矩阵，将惯性传感器输出的载体坐标系下加速度信息转换到导航坐标系下：
并且消除重力加速度的影响，得到导航坐标系下的纯线性运动加速度：
STEP3:划分步态周期
划分步态周期有两种情况：行人正常行走以及行人停止运动。
(1)正常行走
当行人正常行走时，X轴角速度按着一定的规律波动，根据X轴加速度数据由于行人正常行走时迈步阶段出现的极大值划分步态周期，其中，为了避免行人运动时抖动产生毛刺干扰周期划分的情况，使用公式(11)过滤掉曲线中毛刺的影响,筛选出真正可以划分周期的峰值点，划分的结果如图4所示。
Xcycle＝Xpeak (11)
其中Xcycle为周期分隔点，Xpeak为X轴加速度的峰值点，且满足:
Xpeak＝max(Xarrange)
Xarrange为以峰值点出现时刻为中心，时间跨度为σ的X轴角速度的值的集合，σ取正常成人迈步周期的一半。
(2)停止运动
在停止运动的情况下，行人的加速度与角速度都无明显的变化，此时取1秒钟作为步态周期。
STEP4:提取运动特征
进一步地，针对惯性数据提取每一步态周期内相应的特征值组成特征向量。
由于行人运动至扶梯、电梯、楼梯处时，Z轴加速度以及Z轴方向上的位移有明显特征，因此根据公式(12)-(14)提取单个周期内的XYZ方向的平均加速度
其中anxi、anyi、anzi分别为i时刻导航坐标系下汽车的XYZ三轴加速度，N为周期内的采样点数量。
根据公式(15)、(16)计算行人i时刻沿着Z轴的速度vzni以及沿X方向的速度vxni：
其中，vnz0为上一时刻Z轴速度，vnx0为上一时刻X轴速度anxi、anyi、anzi分别为i时刻导航坐标系下汽车的XYZ三轴加速度。
根据公式(17)、(18)提取单个周期行人沿着Z轴的位移Xz以及沿X方向的位移Xx
由于行人在拐弯时Z轴角速度有明显变化，且相邻两个周期的平均角速度方向大多相同，提取单个周期内的Z轴的平均角速度，根据算法(1)判断当前角速度方向是否与上一时刻一致。
当Flag_same＝0时，当前周期角速度方向与上一时刻不一致，当Flag_same＝1时，两周期的角速度方向一致。
由于Y轴角速度曲线在上楼处较在其他关键位置处更加平滑,因此首先使用最小二乘法拟合出单个周期内ωy(k)的函数ωY(k)。再根据公式(19)计算单个周期内Y轴的平均曲率
其中，N为周期内的采样点数量，ωY(k)为拟合后的Y轴角速度函数。
根据公式(20)-(21)提取X、Y、Z方向上的标准差
其中μx、μy、μz分别为X、Y、Z方向上加速度的算术平均值。
根据公式(23)-(25)提取单个周期内的XYZ加速度的相关系数
STEP5:建立运动数据库(即关键位置处行人运动模型)
定义平地直行、拐弯、上楼梯、下楼梯、乘扶梯上行、乘扶梯下行、乘电梯上行以及乘电梯下行对应的标签分别为1、2、3、4、5、6、7、8。在得到归一化后的特征后，将关键地标的类型作为训练集标签，将对应的特征向量作为训练集数据放入支持向量机(SVM)中训练，得到分类模型。
三、识别行人运动状态并推断行人所处位置，实现位置修正
STEP1:实时采集行人运动的惯性数据，解算行人当前坐标。
解算行人坐标主要包括估计步长以及行人轨迹推算,解算的流程图如图5所示。
(1)估计步长
将获得的纯线性运动加速度进行一次积分便得到速度：
vn＝∫andt (26)
其中，an为导航坐标系下的行人加速度。
由于传感器的数据是离散的，将公式(26)转化为差分方程形式：
将获得的速度信息进行积分便得到位置信息和步长：
位置信息公式：
Sn＝∫vndt (28)
转化为差分方程形式，有：
人体在行走过程中，两条腿会发生周期性的交替动作，人行走一步可以分解成抬脚和收脚两个过程，在抬脚过程中人的身体重心会上升,这样会产生Z方向上的加速度，而收脚过程身体重心下降产生-Z方向上的加速度。因此当行人连续行走时，在Z方向上的加速度会呈周期性的变化，其变化的周期个数便是行人行走的步数，记为N。
每一步的步长用如下公式：
(4)行人轨迹推算
行人的位置利用以下公式进行行人轨迹推算：
式中，n＝1,2,…,N，En为行人东向坐标，Nn为行人北向坐标，当n＝1时，E0和N0为初始位置的东向坐标和北向坐标；SLn、αn分别为第n步计算得到的步长和航向，N为最终步数。
STEP2:实时检测行人运动状态
实时提取每一步态周期的运动特征向量并放入分类模型中，根据公式(32)检测行人的运动状态。
y(k)＝ωF[x(k),xi]+b (32)
其中xi为支持向量，x(k)为第k个步态周期内行人运动的特征向量，ω为向量的权重因子,F[x(k),xi]为支持向量机的核函数，y(k)为第k个步态周期时行人的运动状态，b为支持向量机的阈值。
当y(k)的值为1、2、3、4、5、6、7、8时，分别对应在第k个周期时行人的运动状态为平地直行、拐弯、上楼梯、下楼梯、乘扶梯上行、乘扶梯下行、乘电梯上行以及乘电梯下行。
STEP3:判断行人所处的关键地标类型
将采集到的支持向量xi带入公式(32)，便可以得到行人的运动状态y(n)。
(9)判断行人位于拐弯的起始点与结束点
当在n时刻时，y(n-1)＝1，y(n)＝2，判定n时刻行人位于拐弯处的起始点。当在距离n时刻不久的m时刻时，y(m-1)＝2，y(m)＝1，则m时刻行人位于拐弯的结束点。
(10)判断行人位于楼梯与平地交界点
当在n时刻时，y(n-1)＝1，y(n)＝3或y(n)＝4，当在距离n时刻不久的m时刻时，y(m-1)＝3或y(m-1)＝4，y(m)＝1，则确定行人在n与m时刻位于楼梯与平地交界点。
(11)判断行人位于扶梯与平地交界点
当在n时刻时，y(n-1)＝1，y(n)＝5或y(n)＝6，当在距离n时刻不久的m时刻时，y(m-1)＝5或y(m-1)＝6，y(m)＝1，则确定行人在n与m时刻位于楼梯与平地交界点。
(12)判断行人位于电梯入口点
当在n时刻时，y(n-1)＝1，y(n)＝7或y(n)＝8，当在距离n时刻不久的m时刻时，y(m-1)＝7或y(m-1)＝8，y(m)＝1，则确定行人在n与m时刻位于电梯口。
STEP4:修正定位坐标
当判定行人在m时刻位于某一关键地标后，结合地图信息以及根据积分解算的行人位置，选择距离行人最近的该类关键地标的坐标作为修正行人坐标的参考坐标，再根据公式(33)修正行人的当前时刻，即n时刻的坐标。
X′(n)＝α[X(m)+ΔXn-m]+(1-α)X(n) (33)
其中，X′(n)为修正后行人的当前坐标,X(n)为修正前行人的当前坐标，X(m)为m时刻所经过的关键地标的坐标，ΔXn-m为从m时刻到n时刻行人的位移，α为可信度系数，取决于传感器的精度，取值范围为(0,1)。
Claims (7)
1.一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，其特征在于，包括以下步骤：
步骤1，搜集地图信息：首先选取关键地标及行人特殊运动行为，然后搜集关键地标信息；
步骤2，建立行人位于关键地标处的运动模型：包括采集行人的惯性数据，处理惯性数据，划分行人正常行走以及行人停止运动的步态周期并提取运动特征，最后建立运动数据库；
步骤3，识别行人运动状态并推断行人所处位置，实现位置修正：实时采集行人运动的惯性数据，解算行人当前坐标，实时检测行人运动状态，判断行人所处的关键地标类型，修正定位坐标；
所述步骤3中，实时检测行人运动状态的具体过程为：
实时提取每一步态周期的运动特征向量并放入分类模型中，根据公式(32)检测行人的运动状态：
y(k)＝ωF[x(k)，xi]+b (32)
其中xi为支持向量，x(k)为第k个步态周期内行人运动的特征向量，ω为向量的权重因子，F[x(k)，xi]为支持向量机的核函数，y(k)为第k个步态周期时行人的运动状态，b为支持向量机的阈值；
当y(k)的值为1、2、3、4、5、6、7、8时，分别对应在第k个周期时行人的运动状态为平地直行、拐弯、上楼梯、下楼梯、乘扶梯上行、乘扶梯下行、乘电梯上行以及乘电梯下行；
所述步骤3中，判断行人所处的关键地标类型的具体过程为：
将采集到的支持向量xi带入公式(32)，便得到行人的运动状态y(n)；
(1)判断行人位于拐弯的起始点与结束点
当在n时刻时，y(n-1)＝1，y(n)＝2，判定n时刻行人位于拐弯处的起始点，当在距离n时刻不久的m时刻时，y(m-1)＝2，y(m)＝1，则m时刻行人位于拐弯的结束点；
(2)判断行人位于楼梯与平地交界点
当在n时刻时，y(n-1)＝1，y(n)＝3或y(n)＝4，当在距离n时刻不久的m时刻时，y(m-1)＝3或y(m-1)＝4，y(m)＝1，则确定行人在n与m时刻位于楼梯与平地交界点；
(3)判断行人位于扶梯与平地交界点
当在n时刻时，y(n-1)＝1，y(n)＝5或y(n)＝6，当在距离n时刻不久的m时刻时，y(m-1)＝5或y(m-1)＝6，y(m)＝1，则确定行人在n与m时刻位于楼梯与平地交界点；
(4)判断行人位于电梯入口点
当在n时刻时，y(n-1)＝1，y(n)＝7或y(n)＝8，当在距离n时刻不久的m时刻时，y(m-1)＝7或y(m-1)＝8，y(m)＝1，则确定行人在n与m时刻位于电梯口。
2.根据权利要求1所述的一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，其特征在于，步骤1的具体过程为：
选取行人所处的室内环境中的关键地标，关键地标的定义原则为：
行人到达该类关键地标时，会有特有的、明显的且能够被惯性传感器捕捉到的运动状态的位置；
由于行人在拐角处的运动状态为转弯，此时Z轴角速度有明显变化；
在楼梯处行人的运动状态为上楼梯或是下楼梯，此时Z轴方向位移有明显变化，且加速度曲线波动较大；
在乘坐扶梯和电梯时，行人的运动状态分别为乘扶梯上行或是乘扶梯下行和乘电梯上行或是乘电梯下行，此时Z轴方向位移有明显变化，且加速度曲线更为稳定；
根据以上标准，选取拐弯的起始点和结束点、楼梯与平地的交界点、扶梯与平地的交界点以及电梯入口点作为关键地标。
3.根据权利要求1所述的一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，其特征在于，所述步骤2中，采集行人的惯性数据的具体过程为：将惯性传感器放置在行人的腰部，以近似的描述人体质心的运动，传感器的布置方向为：Z轴方向与垂直于地面方向一致，X轴方向与行人前进方向一致，Y轴垂直于XZ平面，且与行人前进方向的右侧方向一致。
4.根据权利要求1所述的一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，其特征在于，所述步骤2中，处理惯性数据主要包括滤波和坐标变换两个部分，具体过程为：
步骤2.1，卡尔曼滤波：为了削减惯性数据中***中的噪声和干扰的影响，采用卡尔曼滤波处理原始数据；
采用公式(1)预测本次状态：
其中X(k)为当前时刻状态，X(k-1)为***上一刻的状态，A为状态转移矩阵；详细的，有：
其中，ax为X轴上的加速度，ay为Y轴上的加速度，az为Z轴上的加速度，ωx为X轴的角速度，ωx为Y轴的角速度，ωz为Z轴的角速度；
采用公式(2)计算数据的输出量：Y(k)＝H·X(k) (2)
其中，Y(k)为传感器的实际测量值，H＝A；
采用公式(3)预测本次协方差矩阵
其中P(k-1)为上一次的协方差矩阵，P(0)取单位矩阵，为当前协方差矩阵的预测结果，C为单位矩阵，Q为6阶对角矩阵，每个对角元素的值为对应变量的过程误差；
接着采用公式(4)，计算滤波增益矩阵：
其中，K(k)为滤波增益矩阵，R为噪声矩阵；
再然后，计算本次状态的最优估计：
其中，X(k)为最优估计值；
最后，利用公式(6)更新协方差矩阵，为下一次滤波做准备：
步骤2.2，利用欧拉角进行坐标变换
接着，使用公式(7)计算姿态角变换矩阵将惯性数据从惯性传感器自身的载体系oxbybzb转移到当地导航坐标系oxnynzn：
简写为：
则载体系oxbybzb和导航系oxnynzn之间的转换关系，即姿态矩阵为：
其中，α为偏航角、β为俯仰角，δ为横滚角，oxbybzb为固连在汽车上的载体坐标系，oxnynzn为用于导航的导航坐标系；根据姿态矩阵，将惯性传感器输出的载体坐标系下加速度信息转换到导航坐标系下：
其中，abx、aby、abz为载体坐标系下的XYZ三轴加速度，a′nx，a′ny，a′nz为消除重力影响前的导航坐标系下三轴加速度；
再消除重力加速度的影响，得到导航坐标系下的纯线性运动加速度：
其中，anx、any、anz为消除重力影响后的导航坐标系下的三轴加速度。
5.根据权利要求1所述的一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，其特征在于，所述步骤2中，提取运动特征的具体过程为：
针对惯性数据提取每一步态周期内相应的特征值组成特征向量；
由于行人运动至扶梯、电梯、楼梯处时，Z轴加速度以及Z轴方向上的位移有明显特征，因此根据公式(12)-(14)提取单个周期内的XYZ方向的平均加速度
其中anxi、anyi、anzi分别为i时刻导航坐标系下汽车的XYZ三轴加速度，N为周期内的采样点数量；根据公式(15)、(16)计算行人i时刻沿着Z轴的速度vnzi以及沿X方向的速度vnxi：
其中，vnz0为上一时刻Z轴速度，vnx0为上一时刻X轴速度anxi、anyi、anzi分别为i时刻导航坐标系下汽车的XYZ三轴加速度；
根据公式(17)、(18)提取单个周期行人沿着Z轴的位移Xz以及沿X方向的位移Xx：
由于行人在拐弯时Z轴角速度有明显变化，且相邻两个周期的平均角速度方向大多相同，提取单个周期内的Z轴的平均角速度；由于Y轴角速度曲线在上楼处较在其他关键位置处更加平滑，因此首先使用最小二乘法拟合出单个周期内ωy(k)的函数ωY(k)，再根据公式(19)计算单个周期内Y轴的平均曲率
其中，N为周期内的采样点数量，ωY(i)为拟合后的Y轴角速度函数；
根据公式(20)-(21)提取X、Y、Z方向上的标准差：
其中μx、μy、μz分别为X、Y、Z方向上加速度的算术平均值；根据公式(23)-(25)提取单个周期内的XYZ加速度的相关系数：
其中，为单个周期内的XYZ方向的平均加速度。
6.根据权利要求1所述的一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，其特征在于，所述步骤3中，实时采集行人运动的惯性数据，解算行人当前坐标的具体过程为：
(1)估计步长
将获得的纯线性运动加速度进行一次积分便得到速度：
vn＝∫andt (26)
其中，an为导航坐标系下的行人加速度；由于传感器的数据是离散的，将公式(26)转化为差分方程形式：
将获得的速度信息进行积分便得到位置信息和步长，位置信息公式为：
Sn＝∫vndt (28)
转化为差分方程形式，有：
人体在行走过程中，两条腿会发生周期性的交替动作，人行走一步分解成抬脚和收脚两个过程，在抬脚过程中人的身体重心会上升，这样会产生Z方向上的加速度，而收脚过程身体重心下降产生-Z方向上的加速度，因此当行人连续行走时，在Z方向上的加速度会呈周期性的变化，其变化的周期个数便是行人行走的步数，记为N；
每一步的步长用如下公式：
(2)行人轨迹推算
行人的位置利用以下公式进行行人轨迹推算：
式中，n＝1，2，…，N，En为行人东向坐标，Nn为行人北向坐标，当n＝1时，E0和N0为初始位置的东向坐标和北向坐标；SLn、αn分别为第n步计算得到的步长和航向，N为最终步数。
7.根据权利要求1所述的一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，其特征在于，所述步骤3中，修正定位坐标的具体过程为：
当判定行人在m时刻位于某一关键地标后，结合地图信息以及根据积分解算的行人位置，选择距离行人最近的该类关键地标的坐标作为修正行人坐标的参考坐标，再根据公式(33)修正行人的当前时刻，即n时刻的坐标；
X′(n)＝α[X(m)+ΔXn-m]+(1-α)X(n) (33)
其中，X′(n)为修正后行人的当前坐标，X(n)为修正前行人的当前坐标，X(m)为m时刻所经过的关键地标的坐标，ΔXn-m为从m时刻到n时刻行人的位移，α为可信度系数，取决于传感器的精度，取值范围为(0，1)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110526323.4A CN113295158B (zh) | 2021-05-14 | 2021-05-14 | 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110526323.4A CN113295158B (zh) | 2021-05-14 | 2021-05-14 | 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113295158A CN113295158A (zh) | 2021-08-24 |
| CN113295158B true CN113295158B (zh) | 2024-05-14 |
Family
ID=77322018
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110526323.4A Active CN113295158B (zh) | 2021-05-14 | 2021-05-14 | 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113295158B (zh) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114061616A (zh) * | 2021-10-22 | 2022-02-18 | 北京自动化控制设备研究所 | 一种自适应波峰检测计步方法 |
| CN114383605B (zh) * | 2021-12-03 | 2024-04-02 | 理大产学研基地(深圳)有限公司 | 基于mems传感器和稀疏地标点的室内定位及优化方法 |
| CN114608576B (zh) * | 2022-02-18 | 2023-07-04 | 北京建筑大学 | 室内定位方法及装置 |
| CN114608571A (zh) * | 2022-02-25 | 2022-06-10 | 南京航空航天大学 | 一种适用于运动平台场景的行人惯性导航方法 |
| CN114993304A (zh) * | 2022-06-29 | 2022-09-02 | 北京石头创新科技有限公司 | 一种机器人位置的确定方法、装置、介质和电子设备 |
| CN115406435A (zh) * | 2022-08-24 | 2022-11-29 | 同济大学 | 基于wlan和mems的室内电子地图构建方法、装置及存储介质 |
| WO2024077237A1 (en) * | 2022-10-07 | 2024-04-11 | Ceva Technologies, Inc. | Methods and systems for robust heading estimation in pedestrian dead reckoning navigation |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106248081A (zh) * | 2016-09-09 | 2016-12-21 | 常州大学 | 一种基于惯性导航结合Wi‑Fi辅助定位的盲人室内导航方法 |
| CN106705968A (zh) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | 基于姿态识别和步长模型的室内惯性导航算法 |
| CN106908060A (zh) * | 2017-02-15 | 2017-06-30 | 东南大学 | 一种基于mems惯性传感器的高精度室内定位方法 |
| CN107339992A (zh) * | 2017-08-24 | 2017-11-10 | 武汉大学 | 一种基于行为的室内定位和地标语义标识的方法 |
| CN110207704A (zh) * | 2019-05-21 | 2019-09-06 | 南京航空航天大学 | 一种基于建筑楼梯场景智能识别的行人导航方法 |
| WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220042801A1 (en) * | 2020-08-07 | 2022-02-10 | The Regents Of The University Of California | Methods and systems for adaptive pedestrian inertial navigation |
-
2021
- 2021-05-14 CN CN202110526323.4A patent/CN113295158B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106248081A (zh) * | 2016-09-09 | 2016-12-21 | 常州大学 | 一种基于惯性导航结合Wi‑Fi辅助定位的盲人室内导航方法 |
| CN106705968A (zh) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | 基于姿态识别和步长模型的室内惯性导航算法 |
| CN106908060A (zh) * | 2017-02-15 | 2017-06-30 | 东南大学 | 一种基于mems惯性传感器的高精度室内定位方法 |
| CN107339992A (zh) * | 2017-08-24 | 2017-11-10 | 武汉大学 | 一种基于行为的室内定位和地标语义标识的方法 |
| WO2020220729A1 (zh) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 |
| CN110207704A (zh) * | 2019-05-21 | 2019-09-06 | 南京航空航天大学 | 一种基于建筑楼梯场景智能识别的行人导航方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于运动姿态识别的行人惯性导航算法;张会清;许潇民;;传感器与微***(第05期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113295158A (zh) | 2021-08-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113295158B (zh) | 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法 | |
| CN109827577B (zh) | 基于运动状态检测的高精度惯性导航定位算法 | |
| EP2850392B1 (en) | Method for step detection and gait direction estimation | |
| CN108362282B (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
| JP4243684B2 (ja) | 歩行動作検出処理装置および歩行動作検出処理方法 | |
| US10352959B2 (en) | Method and system for estimating a path of a mobile element or body | |
| CN109297485B (zh) | 一种基于高度自观测算法的室内个人惯性导航高程精度提升方法 | |
| CN106705968A (zh) | 基于姿态识别和步长模型的室内惯性导航算法 | |
| CN107990895A (zh) | 一种基于穿戴式imu的建筑物楼层间行人轨迹跟踪方法及*** | |
| CN111024126B (zh) | 一种行人导航定位中的自适应零速修正方法 | |
| CN108180923B (zh) | 一种基于人体里程计的惯性导航定位方法 | |
| CN104613965B (zh) | 一种基于双向滤波平滑技术的步进式行人导航方法 | |
| Deng et al. | Foot-mounted pedestrian navigation method based on gait classification for three-dimensional positioning | |
| CN112362057B (zh) | 基于零速修正与姿态自观测的惯性行人导航算法 | |
| CN112066980B (zh) | 一种基于人体四节点运动约束的行人导航定位方法 | |
| CN106153069A (zh) | 自主导航***中的姿态修正装置和方法 | |
| CN104897158A (zh) | 一种步行者室内双层定位方法及*** | |
| Zhou et al. | An improved dead reckoning algorithm for indoor positioning based on inertial sensors | |
| Li et al. | Lower limb model based inertial indoor pedestrian navigation system for walking and running | |
| WO2013057942A1 (ja) | 参照値生成方法及び参照値生成装置 | |
| CN108106630B (zh) | 一种行人导航的二维人体里程计及里程计算方法 | |
| Liu et al. | Knee and waist attached gyroscopes for personal navigation: Comparison of knee, waist and foot attached inertial sensors | |
| Li et al. | A pedestrian 3-D position estimation method based on equality constraint and moving platforms correction model | |
| CN116448103A (zh) | 基于uwb测距辅助的行人足绑式惯性导航***误差修正方法 | |
| CN116222559A (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 |