一种融合惯性数据、地图信息以及行人运动状态的室内定位方法

摘要

本发明公开一种融合惯性数据、地图信息以及行人运动状态的室内定位方法，该方法实时采集行人运动的惯性数据，解算行人当前坐标、实时检测行人运动状态、修正定位坐标等步骤组成。本发明根据室内关键地标的路型特征以及关键地标处行人的运动状态特征，通过放置在行人腰部的惯性传感器采集行人的运动数据，对数据进行特征提取，利用分类器建立了行人运动状态模型，并借助该模型，通过识别行人所处的拐弯、上楼、下楼、坐电梯、乘扶梯等运动状态推断处行人所处的关键地标类型。该方法能使定位误差始终处于一定的较小范围内，保证了利用低精度惯性传感器定位的可靠度。

说明书

技术领域

本发明属于惯性定位领域，具体涉及到融合惯性数据、地图信息和行人运动状态的室内定位方向。

背景技术

惯性定位又叫做自主定位，其定位原理是利用陀螺仪、加速度计等组合测量单元，实时获取人员的位置、速度和方向等信息，实现定位。惯性定位的优势在于受外界干扰小，自主性强。在室内无线信号传播条件差的情况下，例如火灾救援中，惯性定位具有极大优势。此外，极强的自主性使得惯性导航技术在极地导航方面也有着巨大发展前景。

随着对定位精度和长时间定位的要求越来越高，如何提高惯性定位的定位精度并延长定位时间，是一个巨大的挑战。为此，学者提出了多种方案。

传统的捷联式惯性导航系统(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)预测本次状态：

其中，a

采用公式(2)计算数据的输出量：Y(k)＝H·X(k)(2)

其中，Y(k)为传感器的实际测量值，H＝A；

采用公式(3)预测本次协方差矩阵

其中P(k-1)为上一次的协方差矩阵，P(0)取单位矩阵，

接着采用公式(4)，计算滤波增益矩阵：

其中，K(k)为滤波增益矩阵，R为噪声矩阵；

再然后，计算本次状态的最优估计：

其中，X(k)为最优估计值；

最后，利用公式(6)更新协方差矩阵，为下一次滤波做准备：

步骤2.2，利用欧拉角进行坐标变换

接着，使用公式(7)计算姿态角变换矩阵

简写为：

则载体系ox

其中，α为偏航角、β为俯仰角，δ为横滚角，ox

其中，a

再消除重力加速度的影响，得到导航坐标系下的纯线性运动加速度：

其中，a

进一步，所述步骤2中，提取运动特征的具体过程为：

针对惯性数据提取每一步态周期内相应的特征值组成特征向量；由于行人运动至扶梯、电梯、楼梯处时，Z轴加速度以及Z轴方向上的位移有明显特征，因此根据公式(12)-(14)提取单个周期内的XYZ方向的平均加速度

其中a

其中，v

根据公式(17)、(18)提取单个周期行人沿着Z轴的位移X

由于行人在拐弯时Z轴角速度有明显变化，且相邻两个周期的平均角速度方向大多相同，提取单个周期内的Z轴的平均角速度；由于Y轴角速度曲线在上楼处较在其他关键位置处更加平滑,因此首先使用最小二乘法拟合出单个周期内ω

其中，N为周期内的采样点数量，ω

根据公式(20)-(21)提取X、Y、Z方向上的标准差：

其中μ

其中，

进一步，所述步骤3中，实时采集行人运动的惯性数据，解算行人当前坐标的具体过程为：

(1)估计步长

将获得的纯线性运动加速度进行一次积分便得到速度：

v

其中，a

将获得的速度信息进行积分便得到位置信息和步长，位置信息公式为：

S

转化为差分方程形式，有：

人体在行走过程中，两条腿会发生周期性的交替动作，人行走一步可以分解成抬脚和收脚两个过程，在抬脚过程中人的身体重心会上升,这样会产生Z方向上的加速度，而收脚过程身体重心下降产生-Z方向上的加速度，因此当行人连续行走时，在Z方向上的加速度会呈周期性的变化，其变化的周期个数便是行人行走的步数，记为N；

每一步的步长用如下公式：

(3)行人轨迹推算

行人的位置利用以下公式进行行人轨迹推算：

式中，n＝1,2,…,N，E

进一步，所述步骤3中，实时检测行人运动状态的具体过程为：

实时提取每一步态周期的运动特征向量并放入分类模型中，根据公式(32)检测行人的运动状态：

y(k)＝ωF[x(k),x

其中x

当y(k)的值为1、2、3、4、5、6、7、8时，分别对应在第k个周期时行人的运动状态为平地直行、拐弯、上楼梯、下楼梯、乘扶梯上行、乘扶梯下行、乘电梯上行以及乘电梯下行。

进一步，所述步骤3中，判断行人所处的关键地标类型的具体过程为：

将采集到的支持向量x

(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)+ΔX

其中，X′(n)为修正后行人的当前坐标,X(n)为修正前行人的当前坐标，X(m)为m时刻所经过的关键地标的坐标，ΔX

本发明的有益效果：

(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为状态转移矩阵。详细的，有：

其中，a

采用公式(2)计算数据的输出量：

Y(k)＝H·X(k) (2)

其中，Y(k)为传感器的实际测量值，H＝A。

采用公式(3)预测本次协方差矩阵

其中P(k-1)为上一次的协方差矩阵，P(0)取单位矩阵，

接着采用公式(4)，计算滤波增益矩阵

其中，K(k)为滤波增益矩阵，R为噪声矩阵。

再然后，计算本次状态的最优估计

其中，X(k)为最优估计值。

最后，利用公式(6)更新协方差矩阵，为下一次滤波做准备，滤波的结果如图2所示。

(2)利用欧拉角进行坐标变换

接着，使用公式(7)计算姿态角变换矩阵

简写为：

则载体系ox

其中，α为偏航角、β为俯仰角，δ为横滚角，ox

根据姿态矩阵，将惯性传感器输出的载体坐标系下加速度信息转换到导航坐标系下：

并且消除重力加速度的影响，得到导航坐标系下的纯线性运动加速度：

STEP3:划分步态周期

划分步态周期有两种情况：行人正常行走以及行人停止运动。

(1)正常行走

当行人正常行走时，X轴角速度按着一定的规律波动，根据X轴加速度数据由于行人正常行走时迈步阶段出现的极大值划分步态周期，其中，为了避免行人运动时抖动产生毛刺干扰周期划分的情况，使用公式(11)过滤掉曲线中毛刺的影响,筛选出真正可以划分周期的峰值点，划分的结果如图4所示。

X

其中X

X

X

(2)停止运动

在停止运动的情况下，行人的加速度与角速度都无明显的变化，此时取1秒钟作为步态周期。

STEP4:提取运动特征

进一步地，针对惯性数据提取每一步态周期内相应的特征值组成特征向量。

由于行人运动至扶梯、电梯、楼梯处时，Z轴加速度以及Z轴方向上的位移有明显特征，因此根据公式(12)-(14)提取单个周期内的XYZ方向的平均加速度

其中a

根据公式(15)、(16)计算行人i时刻沿着Z轴的速度v

其中，v

根据公式(17)、(18)提取单个周期行人沿着Z轴的位移X

由于行人在拐弯时Z轴角速度有明显变化，且相邻两个周期的平均角速度方向大多相同，提取单个周期内的Z轴的平均角速度，根据算法(1)判断当前角速度方向是否与上一时刻一致。

当Flag_same＝0时，当前周期角速度方向与上一时刻不一致，当Flag_same＝1时，两周期的角速度方向一致。

由于Y轴角速度曲线在上楼处较在其他关键位置处更加平滑,因此首先使用最小二乘法拟合出单个周期内ω

其中，N为周期内的采样点数量，ω

根据公式(20)-(21)提取X、Y、Z方向上的标准差

其中μ

根据公式(23)-(25)提取单个周期内的XYZ加速度的相关系数

STEP5:建立运动数据库(即关键位置处行人运动模型)

定义平地直行、拐弯、上楼梯、下楼梯、乘扶梯上行、乘扶梯下行、乘电梯上行以及乘电梯下行对应的标签分别为1、2、3、4、5、6、7、8。在得到归一化后的特征后，将关键地标的类型作为训练集标签，将对应的特征向量作为训练集数据放入支持向量机(SVM)中训练，得到分类模型。

三、识别行人运动状态并推断行人所处位置，实现位置修正

STEP1:实时采集行人运动的惯性数据，解算行人当前坐标。

解算行人坐标主要包括估计步长以及行人轨迹推算,解算的流程图如图5所示。

(1)估计步长

将获得的纯线性运动加速度进行一次积分便得到速度：

v

其中，a

由于传感器的数据是离散的，将公式(26)转化为差分方程形式：

将获得的速度信息进行积分便得到位置信息和步长：

位置信息公式：

S

转化为差分方程形式，有：

人体在行走过程中，两条腿会发生周期性的交替动作，人行走一步可以分解成抬脚和收脚两个过程，在抬脚过程中人的身体重心会上升,这样会产生Z方向上的加速度，而收脚过程身体重心下降产生-Z方向上的加速度。因此当行人连续行走时，在Z方向上的加速度会呈周期性的变化，其变化的周期个数便是行人行走的步数，记为N。

每一步的步长用如下公式：

(4)行人轨迹推算

行人的位置利用以下公式进行行人轨迹推算：

式中，n＝1,2,…,N，E

STEP2:实时检测行人运动状态

实时提取每一步态周期的运动特征向量并放入分类模型中，根据公式(32)检测行人的运动状态。

y(k)＝ωF[x(k),x

其中x

当y(k)的值为1、2、3、4、5、6、7、8时，分别对应在第k个周期时行人的运动状态为平地直行、拐弯、上楼梯、下楼梯、乘扶梯上行、乘扶梯下行、乘电梯上行以及乘电梯下行。

STEP3:判断行人所处的关键地标类型

将采集到的支持向量x

(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)+ΔX

其中，X′(n)为修正后行人的当前坐标,X(n)为修正前行人的当前坐标，X(m)为m时刻所经过的关键地标的坐标，ΔX