CN104897158A - 一种步行者室内双层定位方法及系统 - Google Patents

Abstract

本发明提供一种步行者室内双层定位方法，包括：下层滤波，包括：根据预定静止检测方式判断检测对象在当前时刻是否处于静止状态；若是，推算当前时刻的移动位置坐标信息，进行误差状态偏置估计，并获取当前时刻校准后的移动位置坐标信息；若否，继续判断；上层滤波，包括：查找当前时刻中零速度阶段的结束时刻，在结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，获取第一差；比较第一差与当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，检测当前步伐是逆时针偏移还是顺时针偏移，若步行者的当前步伐存在逆时针偏移或顺时针偏移，执行逆时针或顺时针校正。本发明使基于惯性传感器的室内定位精度更高。

Description

一种步行者室内双层定位方法及系统

技术领域

本发明属于室内定位技术领域，特别是涉及一种步行者室内双层定位方法及系统。

背景技术

随着微机电系统(MEMS)快速发展，使得利用微型惯性传感器和三轴磁力计的来跟踪步行者的运动特征，并对其运动进行推算得到实时的位置信息成为了可能。此类步行者定位系统最大的优势在于拥有自完备特性。正是由于这种特性，此类步行者室内定位技术越来越多得到了关注，其在某些特定的场景下有着非常重要的应用前景(应急救援，应急医疗，商场位置确定等的)。但是由于受到传感器自身误差在长时间计算后会产生不可忽略的误差累积问题，同时在位置推算的滤波器参数对不同步行者的运动习惯不具备自适应性，使得这类算法在适用性上存在进一步提升的空间。于此种种问题都使得这种技术在定位精度上一直无法满足人们的应用需求。

在惯性定位的基础上，现有的技术提出了一种双层滤波定位架构，其基于粒子滤波并结合室内地图信息辅助惯性定位的技术是另一种辅助定位的方法，该方法结合地图信息利用粒子滤波算法纠正无效位置点，以达到提高定位精度的目的。这种方法在有丰富地图信息的情况下，可以有效遏制了由于传感器误差而造成的位置误差的无休止的积累，能得到相对较高的定位精度。但是在实际应用中，利用地图辅助修正位置信息的方法仍然存在一定的应用难题和缺陷。当无法得到详细地图信息时，例如仅仅是建筑物轮廓图，或者步行者长期在某个建筑物内自由运动时，地图信息不能有效的对估算位置的累积误差进行修正。

因此，如何提供一种步行者室内双层定位方法及系统，以解决现有技术中的利用地图辅助步行者定位方法及系统对室内人员的定位精度不高，无法对估算的室内人员的位置信息的累积误差进行自适应地修正等种种缺陷，实已成为本领域从业者亟待解决的技术问题

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种步行者室内双层定位方法及系统，用于解决现有技术中利用地图辅助步行者定位方法及系统对室内人员的定位精度不高，耦合性低、无法对估算的室内人员的位置信息的累积误差进行自适应地修正的问题。

为实现上述目的及其他相关目的，本发明一方面提供一种步行者室内双层定位方法，包括：下层滤波步骤；所述下层滤波步骤包括：选取所述步行者的一只脚为检测对象，并采集所述检测对象的运动数据；根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若是，继续执行下一步骤；若否，继续判断；所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态；利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位置坐标信息；上层滤波步骤；所述上层滤波步骤包括：查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一差；将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行逆时针校正；若所述步行者的当前步伐存在顺时针偏移，执行顺时针校正。

可选地，所述下层滤波步骤中的所述预存位置坐标信息推算法为捷联惯导算法；所述步骤三采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未校准的移动位置坐标信息。

可选地，所述下层滤波步骤中采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度误差估计。

可选地，姿态误差估计为通过一姿态误差矩阵对方向余弦矩阵进行修正；角速度误差估计为采用角速度误差对下一时刻的角速度进行补偿；其中，下一时刻的角速度＝当前时刻的角速度+角速度误差；位置误差估计为采用位置误差对当前时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信息＝当前时刻未校准的移动位置坐标信息－位置误差；速度误差估计为采用速度误差对当前时刻的速度进行补偿；其中，所述当前时刻的速度＝未校准的所述步行者当前时刻的移动位置坐标信息中的速度－速度误差；加速度误差估计为采用加速度误差对下一时刻的加速度进行补偿；其中，下一时刻的加速度＝当前时刻的加速度+加速度误差。

可选地，在所述上层滤波步骤中执行的预制修正为执行粒子滤波算法以完成位置校准。

本发明另一方面还提供一种步行者室内双层定位系统，包括：下层滤波单元，所述下层滤波单元包括：选取模块，用于选取所述步行者的一只脚为检测对象；采集模块，与所述选取模块连接，用于采集所述检测对象的运动数据；判断模块，与所述采集模块连接，用于根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态，其中，所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态；若是，调用用于利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位置坐标信息的第一处理模块；若否，继续调用所述判断模块；与所述下层滤波单元连接的上层滤波单元，所述上层滤波单元包括：第二处理模块，与所述判断模块和第一处理模块连接，用于查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一差；；检测模块，与所述第一处理模块和第二处理模块连接，用于将将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，调用用于执行逆时针校正的校准模块；若所述步行者的当前步伐存在顺时针偏移，继续调用所述校准模块执行顺时针校正。

可选地，所述第一处理模块中的所述预存位置坐标信息推算法为捷联惯导算法；所述第一处理模块还用于采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未校准的移动位置坐标信息。

可选地，所述第一处理模块还用于采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度误差估计。

如上所述，本发明的步行者室内定位方法及系统，具有以下有益效果：

本发明增加了两个滤波层次之间的耦合度，提高了数据的融合效果。使基于惯性传感器的室内定位精度更高，实用性更强，并延长了算法有效计算的生命周期。

附图说明

图1显示为本发明的步行者室内定位方法流程示意图。

图2显示为本发明的基于三轴加速度与三轴陀螺仪采集的数据计算的零速度阶段与零速度阶段结束的结束时刻的计算效果示意图。

图3显示为本发明的步行者室内定位系统的原理结构示意图。

元件标号说明

1       步行者室内定位系统

11      下层滤波单元

12      上层滤波单元

111     选取模块

112     采集模块

113     判断模块

114     第一处理模块

121     第二处理模块

122     检测模块

123     校准模块

S1～S2  步骤

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

本发明的基本技术方案如下：

底层基于安装于脚步的微型传感器所采集的数据,利用零速度更新(ZUPT,Zero VelocityUpdate)和零角速度更新(ZARU,Zero Angular Rate Update)辅助策略的误差状态卡尔曼滤波器进行初步的步行者位置推算，然后基于检出的信号特征，判断出每一步的结束时刻，在此时刻将计算的这一步所运动的位移长度和运动方向信息传输到上一层滤波器，上一层滤波采用粒子滤波方法，基于地图信息，对本步所计算的步长和方向进行再次滤波处理，得出最优结果。然后将最优结果与底层滤波数据的结果进行对比，将此对比的差值反馈回底层误差状态卡尔曼滤波器，用以自适应修正卡尔曼滤波器的计算参数。

实施例一

本实施例提供一种步行者室内双层定位方法，请参阅图1，显示为步行者室内定位方法流程示意图。如图1所示，所述步行者室内双层定位方法包括以下步骤：

S1，下层滤波步骤，所述下层滤波步骤具体包括：

S11，选取所述步行者的一只脚为检测对象，并采集所述检测对象的运动数据。在本步骤中，首先需要选取固定一只脚作为检测对象，然后基于在该脚上安装惯性传感器采集运动数据。所述惯性传感器包括三轴加速度计、三轴陀螺仪、磁力计分别采集加速度和角速度等运动数据。

S12，根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若是，继续执行下一步骤，即S13；若否，返回步骤S12，继续判断所述检测对象在当前时刻是否处于静止状态；所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态。在本实施例中，所述预定精致检测方式可以采用传统的联合阈值条件判断法，也可以概率统计推算法，如贝叶斯估计，HMM等算法。例如，在本实施例中，采用贝叶斯估计判断静止状态。

所述步骤S12包括以下步骤：

第一步，基于所述传感数据计算所述步行者的跨步周期；所述跨步周期包括脚接触阶段、脚站立阶段，脚离地阶段、及脚摆布阶段。在本实施例中，计算所述步行者的跨步周期需要基于所述传感模块中三轴陀螺仪y轴数据，根据三轴陀螺仪y轴信号中跨步信号特征来计算跨步周期。在摆步周期的起始阶段，任何人的陀螺仪y轴数据均存在一个谷值，可以利用对过零点设置阈值的方法进行谷值探测，但是不同人信号特征可能会引入误判断谷值，因此针对跨步信号特征，维护一个标志位，当探测到零速度区间是，将此标志位置true，当在摆步阶段探测到第一个谷值的时候讲此标志位置false，在之后false阶段探测到谷值均忽略。同时基于人体运动学信息，摆步阶段要持续38％时间，在此时间段内检测到的其余谷值均忽略。实验证明，此两种机制可以保证100％的跨步周期内的局部谷值计算准确率。也就是说，在每个跨步周期中均可以找到一个明显的谷值(局部最小值)以这个谷值为脚尖离地时刻，作为判断一个完整跨步周期的分割点，实现对不同脚步运动阶段的划分估计。

第二步，按照预定规则对应查询所述步行者在第k时刻(当前时刻)应该处于跨步周期中的哪个阶段，并获取查询结果。在本实施例中，所述预定规则为人体运动学模型，此模型定义了一个跨步周期为单条腿从脚跟触地，到脚掌触地，到脚跟离地，脚尖离地，摆步，最后脚跟再次触地。此周期内包含站位阶段和摆步阶段，其中站位阶段包括接触阶段，站立阶段，离地阶段。人体运动学模型定义了在正常运动是各个阶段在真个跨步周期内所占据的时间比例。在本步骤中，所述步行者所处第k时刻对应到该人体运动学模型中对应查询所述步行者在第k时刻(当前时刻)应该处于跨步周期中的哪个阶段，便得到查询结果，所述查询结果包括所述步行者在第k时刻(当前时刻)处于站位阶段或所述步行者在第k时刻(当前时刻)处于摆布阶段，假如把一个跨步周期分为100份，A点表示脚跟触地(0％)，B点表示脚掌触地(16.8％)，C点表示脚跟离地(41.5％)，D点表示脚尖离地(62％)，E点表示脚跟触地(100％)，脚跟触地到脚掌触地为接触阶段，脚掌触地到脚跟离地为站立阶段，脚跟离地到脚尖离地为离地阶段，脚尖离地到脚跟触地为摆布阶段。因此，可以从图3中可以看出，站位阶段占62％，摆布阶段占38％。

第三步，根据预存的陀螺仪设置阈值判断所述检测对象是否处于静止状态，获取第一判断结果，即通过三轴陀螺仪获取到的所述步行者第k时刻(当前时刻)的角速度信息与预存的陀螺仪设置阈值进行比对，若所述步行者第k时刻(当前时刻)的角速度信息小于预存的陀螺仪设置阈值，则表示所述检测对象处于静止状态，若所述步行者第k时刻(当前时刻)的角速度信息大于预存的陀螺仪设置阈值，则表示所述检测对象处于运动状态，返回步骤S3，继续判断；若所述第一判断结果为所述检测对象处于静止状态，继续执行下一步骤。

第四步，判断所述第一判断结果是否与所述查询结果一致(检测对象处于静止状态对应所述检测对象处于站位阶段，即表示第一判断结果与所述查询结果一致，或检测对象处于运动状态即表示所述检测对象处于摆布阶段即表示第一判断结果与所述查询结果一致)，若是，则表示不存在潜在误差点；若否，则表示存在潜在误差点，执行第五步。

第五步，通过朴素贝叶斯估计所述潜在误差点是否处于零速度区间，若是，则表示所述检测对象在第k时刻(当前时刻)处于静止状态，继续执行步骤S13，若否，则表示所述检测对象在第k时刻处于运动状态，则执行步骤S12，继续判断。在本步骤中，采用以下朴素贝叶斯估计公式来估计，朴素贝叶斯估计公式为：

zv＝argmaxP(s|zv)P(a|zv)P(zv)         公式(1)其中，P(s|zv)表示脚步接触地面的结束时刻的条件概率，

$P\left(s\right|zv)=\left[\begin{array}{cc}\frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]& 1-\frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]\\ 1-\frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]& \frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]\end{array}\right]$     公式(2)

$P\left(s\right|zv)=\left[\begin{array}{cc}\frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]& 1-\frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]\\ 1-\frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]& \frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]\end{array}\right]$   公式(3)

其中，σ标识正态分布的方差，σ²＝C×T_gait，t₀为摆布的起始时刻，t为第k时刻(当前时刻)，C为一常数，T_gait表示整个跨步周期的时间长度。P(a|zv)表示在已知零速度概率条件下的加速度的条件概率。

$P\left(a\right|zv)=\left[\begin{array}{cc}\frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]& 1-\frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]\\ 1-\frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]& \frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]\end{array}\right]$  公式(4)

其中，a_n表示在地球坐标系下的加速度。P(zv)表示基于人体运动学的概率统计得到第k时刻(当前时刻)为零速度的概率，

$P\left(zv\right)=\left\{\begin{array}{cc}0.548,& isZV\\ 0.452,& notZV\end{array}\right.$        公式(5)

S13，利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取校准后的移动位置坐标信息。步骤S12是在检测到的零速度阶段，利用误差状态卡尔曼滤波器，对推算的运动数据误差状态进行估计，然后将估计的误差去修正捷联惯导算法所推算的移动位置坐标信息。具体包括以下步骤：

第六步，通过定位设备获取所述步行者的初始移动位置坐标信息。

第七步，利用捷联惯导算法，对所述三轴加速度计和三轴陀螺仪采集到的运动数据进行计算。本步骤通过公式(6)计算步行者当前时刻，即第k时刻的加速度。

$a_k^n=C_{b_{k/k-1}}^n\·a_k^b$         公式(6)

其中，为所述步行者第k时刻(当前时刻)在地球坐标系下X轴/Y轴/Z轴的加速度，k表示第k时刻(当前时刻)，即，n表示地球坐标系下的X轴/Y轴/Z轴；为第k时刻(当前时刻)的方向余弦矩阵，通过该方向余弦矩阵将所述传感数据从传感器坐标系下转换至地球坐标系下，k-1为第k-1时刻(前一时刻)；为所述步行者第k时刻在传感器坐标系下第一方向/第二方向/第三方向的加速度，k表示第k时刻(当前时刻)，b表示传感器坐标系下第一方向/第二方向/第三方向。同理，计算所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息，同样是通过为第k时刻(当前时刻)的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下转换至地球坐标系下。其中，第k时刻(当前时刻)的方向余弦矩阵的计算公式为：

$C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n.\frac{2I_{3\×3}+{\mathrm{\δ\Ω}}_K\·\mathrm{\Δ}t}{2I_{3\×3}-{\mathrm{\δ\Ω}}_k\·\mathrm{\Δ}t}$      公式(7)

其中，为第k-1时刻(前一时刻)的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下转换至地球坐标系下；δΩ_k表示为基于角速度的矩阵用以表示小的角度变换对方向变换的影响，δΩ_k的表达式为 ${\mathrm{\δ\Ω}}_k=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_k^b\left(3\right)& {\mathrm{\ω}}_k^b\left(2\right)\\ {\mathrm{\ω}}_k^b\left(3\right)& 0& -{\mathrm{\ω}}_k^b\left(1\right)\\ -{\mathrm{\ω}}_k^b\left(2\right)& {\mathrm{\ω}}_k^b\left(1\right)& 0\end{array}\right],$ 1，2，3表示传感器坐标系下第一方向/第二方向/第三方向。方向余弦矩阵当k＝1时， $C_{b_0}^n\left[\begin{array}{ccc}\cos \left(pitch\right)& \sin \left(pitch\right)& \cos \left(roll\right)\sin \left(pitch\right)\\ 0& \cos \left(roll\right)& -\sin \left(roll\right)\\ -\sin \left(pitch\right)& \sin \left(roll\right)\cos \left(pitch\right)& \cos \left(roll\right)\cos \left(pitch\right)\end{array}\right],$ 其中，roll表示翻滚角， $roll=arctan(a_y^{sensor}/a_z^{sensor});$ pitch是俯仰角， $pitch=-arcsin(a_x^{sensor}/g),$ yaw是航向角。指加速度传感器测得的x轴的数据，指加速度传感器测得的y轴的数据,指加速度传感器测得的z轴的数据；g为重力加速度。在有磁力计的情况下，yaw为磁力计在水平面上的度数，没有磁力计的情况下初始值设置为0。I_3*3为3维单位矩阵，即 $I_{3*3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right];$ 为在传感器坐标系下所述步行者第k时刻的角速度信息；Δt表示采样时间间隔，即第k时刻(当前时刻)与第k-1时刻(前一时刻)之间相差的时间。

第八步，对进行两次积分以获取所述步行者第k时刻(当前时刻)未校准的移动位置坐标信息。

第九步，在所述零速度阶段利用误差状态卡尔曼滤波对运动数据误差状态进行误差状态偏置估计。误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度误差估计。

所述步行者第k时刻的运动数据误差状态可以标记为δx_k，该位置信息计算误差δx_k采用如下误差向量表示：

其中，表示所述步行者第k时刻的姿态误差，表示所述步行者第k时刻的角速度误差，δr_k表示所述步行者第k时刻的位置误差，δv_k表示所述步行者第k时刻的速度误差，表示所述步行者第k时刻的加速度误差，这5个误差向量都是对应3个正交轴。

所述步行者第k时刻的运动数据误差状态δx_k的计算公式为

δx_k＝F_kδx_k-1+w_k-1             公式(8)

其中，F_k表示状态转移矩阵，δx_k-1表示所述步行者第k-1时刻的位置信息计算误差，w_k-1表示所述步行者第k时刻的位置信息计算误差的噪声。在本实施例中，由于需要采用卡尔曼滤波法对误差进行估计，则卡尔曼滤波的量测模型为：

Z_k＝Hδx_k|k+n_k             公式(9)

当脚步处于静止状态的时候，加速度和角速度应该为0，但是实际采集的数据存在噪声，这个噪声经过积分后得到速度误差，另外一个就是实际测量到的角速度误差，这两个值作为误差状态的测量值Z_k，即H为量测误差， $H=\left[\begin{array}{ccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\end{array}\right],$ I_3×3为单位矩阵。

校准后的所述步行者第k时刻的运动数据误差状态

δx_k＝δx_k-1+K_k·[Z_k-Hδx_k-1]        公式(10)

其中，K_k为卡尔曼增益，卡尔曼增益K_k＝P_k-1H^T(HP_KH^T+R_k)^-1中P_k为所述步行者第k时刻为误差状态协方差矩阵，P_k＝(I_15×15-K_kH)P_k-1(I_15×15-K_kH)^T+R_k，P_k-1为所述步行者第k-1时刻为误差状态协方差矩阵，R_k为测量噪声协方差矩阵。在本步骤中，采用公式(10)分别估计姿态误差，角速度误差、位置误差、速度误差、及加速度误差，具体如下：

${\mathrm{\δw}}_k^b={\mathrm{\δw}}_{k-1}^b+K_k\·\[Z_k-{\mathrm{H\δw}}_{k-1}^b\]$

δr_k＝δr_k-1+K_k·[Z_k-Hδr_k-1]

δv_k＝δv_k-1+K_k·[Z_k-Hδv_k-1]

${\mathrm{\δa}}_k^b={\mathrm{\δa}}_{k-1}^b+K_k\·\[Z_k-{\mathrm{H\δa}}_{k-1}^b\]$

第十步，修正第k时刻(当前时刻)运动数据误差状态并获取校准后的移动位置坐标信息。姿态误差估计为采用姿态误差，即所述步行者第k时刻的姿态误差通过一姿态误差矩阵δΩ_k对方向余弦矩阵 $C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n\×\frac{2I_{3\×3}+\mathrm{\δ}{2}_K\·\mathrm{\Δ}t}{2I_{3\×3}-{\mathrm{\δ\Ω}}_k\·\mathrm{\Δ}t}$ 进行修正，其中姿态误差矩阵δΩ_k为

角速度误差估计为采用角速度误差，即所述步行者第k时刻(当前时刻)的角速度误差对第k+1时刻(下一时刻)的角速度进行补偿；其中，下一时刻的角速度＝当前时刻的角速度+角速度误差；

位置误差估计为采用所述步行者第k时刻(当前时刻)的位置误差δr_k对当前时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信息＝当前时刻未校准的移动位置坐标信息－位置误差；

速度误差估计为采用速度误差δv_k，所述步行者第k时刻的速度误差δv_k对当前时刻的速度进行补偿；其中，所述当前时刻的速度＝未校准的所述步行者当前时刻的移动位置坐标信息中的速度－速度误差；

加速度误差估计为采用加速度误差即所述步行者第k时刻的加速度偏置对下一时刻的加速度进行补偿；其中，下一时刻的加速度＝当前时刻的加速度+加速度误差。

S2，上层滤波步骤。所述上层滤波步骤具体包括：

S21，查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差。请参阅图2，显示为基于三轴加速度与三轴陀螺仪采集的数据计算的零速度阶段与零速度阶段结束的结束时刻的计算效果示意图，图2依次展现了三轴加速度数据,三轴陀螺仪数据和基于这2个原始数据计算的零速度结果，其中第三张图中，1表示为零速度时刻，0表示非零速度时刻，(零速度即脚步静止时刻)，其中图中的点A即零速度阶段的结束时刻，在这个时刻触发利用粒子滤波算法，结合地图信息来校准计算的移动位置坐标信息。点A的获取方法如下：

若((零速度_k-1＝＝1)&&(零速度_k＝＝0))，则时刻k是零速度阶段的结束时刻。

对在所述结束时刻对当前时刻校准后的移动位置坐标信息，结合地图信息，利用粒子滤波进一步对位置进行校准具体如下：

基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新的有效粒子仍然失败，那么就将其标记为无效粒子，其中，s表示移动位置坐标信息。粒子的权值如下：

          公式(11)

粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设混入到一步步长的噪声v_l和一步方位变化的噪声v_ψ都服从零均值的高斯分布。以此列写出水平位置(x,y)和方位ψ的状态转移方程：

$\left\{\begin{array}{c}{x}_s^i=x_{s-1}^i+l_s^i{\mathrm{cos\ψ}}_{s-1}^i\\ y_s^i=y_{s-1}^i+l_s^i{\mathrm{sin\ψ}}_{s-1}^i\\ {\mathrm{\ψ}}_s^i={\mathrm{\ψ}}_{s-1}^i+{\mathrm{\δ\ψ}}_s^i\end{array}\right.$                公式(12)

在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子的基础上由重要性分布采样得到。这可以通过先采样得到粒子和然后代入状态转移方程分别得到换言之，凡是能够修正位置计算累计误差进行修正的方法均适用于本发明。

S22，将修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差与下层滤波后的当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行步骤S23，即逆时针校正；若所述步行者的当前步伐存在顺时针偏移，执行步骤S24，即顺时针校正。

在本步骤中，将修正修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差，即校准的本步角度ψ_PF，(PF,Particle Filter,粒子滤波)与当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，表现在本实施例中为当前步所跨角度ψ_EKF，(EKF，Extended Kalman Filter,扩展卡尔曼滤波)反馈回卡尔曼滤波器中，对所述测量噪声协方差矩阵R_k进行修正。

计算当前步所跨的角度ψ_EKF，然后与利用地图配准计算的本步角度ψ_PF进行对比得到：

δ_ψ＝ψ_EKF-ψ_PF               公式(13)

因此判断R值修正时刻可以采用如下策略：

执行顺时针校正：若δ_ψ＜0时

说明存在顺时针航向角估计偏移，这种情况说明在零速度阶段对顺时针方向的补偿过度，因此采用公式(14)，对R值进行减小得到修正后的以增加估计量的比例，减小测量量的对估计

$\hat{R}=R-\mathrm{\ϵ}$               公式(14)

其中ε，为固定的R值减小量。

执行逆时针校正：若δ_ψ＞0时

说明存在逆时针航向角估计偏移，这种情况说明在零速度阶段对逆时针方向的补偿过度。同样道理，针对公式(15)，对R值进行增大得到修正后的以减小估计量的比例，增大测量量的对估计结果的影响。

$\hat{R}=R+\mathrm{\ϵ}$                公式(15)

若发现连续多步存在地图修正后的航向角向某个方向偏移的问题，则对EKF(误差状态卡尔曼滤波器修正)的测量噪声协方差进行连续小幅度修正。经过试验结果分析，R矩阵内非零元素合理的取值范围应该在[0.001，0.01]区间内。

本实施例所述的步行者室内双层定位方法基于安装于脚部的加速度计，陀螺仪，磁力计等微型传感器，来跟踪脚部的运动特征。再底层利用误差状态卡尔曼滤波来估计误差参数来修正捷联惯导推算的位置信息，然后根据原始数据特征检查出每步的结束时刻，将底层计算的这一步步长和运动方向信息输入到上层粒子滤波器，结合地图信息，对底层滤波器估算的位置进行修正。最后将上层粒子滤波器与下层卡尔曼滤波器估算的位置进行对比，将差值反馈回下层滤波器，实现对卡尔曼滤波器的自适应参数修正，以提高整体框架算法的计算的位置精度。本发明增加了两个滤波层次之间的耦合度，提高了数据的融合效果。使基于惯性传感器的室内定位精度更高，实用性更强，并延长了算法有效计算的生命周期。

实施例二

本实施例提供一种步行者室内双层定位系统1，请参阅图3，显示为步行者室内双层定位系统的原理结构示意图。所述步行者室内双层定位系统1包括下层滤波单元11和与所述下层滤波单元11连接的上层滤波单元12，所述下层滤波模块11包括选取模块111、采集模块112、判断模块113、及第一处理模块114。所述上层滤波单元12包括第二处理模块121、检测模块122、及校准模块123。

其中，所述下层滤波模块11用于执行下层滤波。

所述选取模块111用于选取所述步行者的一只脚为检测对象。

与所述选取模块111连接的采集模块112用于采集所述检测对象的运动数据。在本实施例中，首先需要选取固定一只脚作为检测对象，然后基于在该脚上安装惯性传感器采集运动数据。所述惯性传感器包括三轴加速度计、三轴陀螺仪、磁力计分别采集加速度和角速度等运动数据。

与所述采集模块112连接的所述判断模块113用于根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若是，调用所述第一处理模块114；若否，继续调用所述判断模块113，继续判断所述检测对象在当前时刻是否处于静止状态；所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态。在本实施例中，所述预定精致检测方式可以采用传统的联合阈值条件判断法，也可以概率统计推算法，如贝叶斯估计，HMM等算法。例如，在本实施例中，采用贝叶斯估计判断静止状态。

所述判断模块具体执行功能包括：

基于所述传感数据计算所述步行者的跨步周期；所述跨步周期包括脚接触阶段、脚站立阶段，脚离地阶段、及脚摆布阶段。在本实施例中，计算所述步行者的跨步周期需要基于所述传感模块中三轴陀螺仪y轴数据，根据三轴陀螺仪y轴信号中跨步信号特征来计算跨步周期。在摆步周期的起始阶段，任何人的陀螺仪y轴数据均存在一个谷值，可以利用对过零点设置阈值的方法进行谷值探测，但是不同人信号特征可能会引入误判断谷值，因此针对跨步信号特征，维护一个标志位，当探测到零速度区间是，将此标志位置true，当在摆步阶段探测到第一个谷值的时候讲此标志位置false，在之后false阶段探测到谷值均忽略。同时基于人体运动学信息，摆步阶段要持续38％时间，在此时间段内检测到的其余谷值均忽略。实验证明，此两种机制可以保证100％的跨步周期内的局部谷值计算准确率。也就是说，在每个跨步周期中均可以找到一个明显的谷值(局部最小值)以这个谷值为脚尖离地时刻，作为判断一个完整跨步周期的分割点，实现对不同脚步运动阶段的划分估计。

按照预定规则对应查询所述步行者在第k时刻(当前时刻)应该处于跨步周期中的哪个阶段，并获取查询结果。在本实施例中，所述预定规则为人体运动学模型，此模型定义了一个跨步周期为单条腿从脚跟触地，到脚掌触地，到脚跟离地，脚尖离地，摆步，最后脚跟再次触地。此周期内包含站位阶段和摆步阶段，其中站位阶段包括接触阶段，站立阶段，离地阶段。人体运动学模型定义了在正常运动是各个阶段在真个跨步周期内所占据的时间比例。在本步骤中，所述步行者所处第k时刻对应到该人体运动学模型中对应查询所述步行者在第k时刻(当前时刻)应该处于跨步周期中的哪个阶段，便得到查询结果，所述查询结果包括所述步行者在第k时刻(当前时刻)处于站位阶段或所述步行者在第k时刻(当前时刻)处于摆布阶段，假如把一个跨步周期分为100份，A点表示脚跟触地(0％)，B点表示脚掌触地(16.8％)，C点表示脚跟离地(41.5％)，D点表示脚尖离地(62％)，E点表示脚跟触地(100％)，脚跟触地到脚掌触地为接触阶段，脚掌触地到脚跟离地为站立阶段，脚跟离地到脚尖离地为离地阶段，脚尖离地到脚跟触地为摆布阶段。因此，可以从图3中可以看出，站位阶段占62％，摆布阶段占38％。

根据预存的陀螺仪设置阈值判断所述检测对象是否处于静止状态，获取第一判断结果，即通过三轴陀螺仪获取到的所述步行者第k时刻(当前时刻)的角速度信息与预存的陀螺仪设置阈值进行比对，若所述步行者第k时刻(当前时刻)的角速度信息小于预存的陀螺仪设置阈值，则表示所述检测对象处于静止状态，若所述步行者第k时刻(当前时刻)的角速度信息大于预存的陀螺仪设置阈值，则表示所述检测对象处于运动状态，返回步骤S3，继续判断；若所述第一判断结果为所述检测对象处于静止状态，继续执行下一步骤。

判断所述第一判断结果是否与所述查询结果一致(检测对象处于静止状态对应所述检测对象处于站位阶段，即表示第一判断结果与所述查询结果一致，或检测对象处于运动状态即表示所述检测对象处于摆布阶段即表示第一判断结果与所述查询结果一致)，若是，则表示不存在潜在误差点；若否，则表示存在潜在误差点，执行第五步。

通过预定静止检测方式，即朴素贝叶斯估计所述潜在误差点是否处于零速度区间，若是，则表示所述检测对象在第k时刻(当前时刻)处于静止状态，调用所述第一处理模块113，若否，则表示所述检测对象在第k时刻处于运动状态，则继续调用所述判断模块112，继续判断。在本实施例中，采用以下朴素贝叶斯估计公式来估计，朴素贝叶斯估计公式为：

zv＝argmaxP(s|zv)P(a|zv)P(zv)

其中，P(s|zv)表示脚步接触地面的结束时刻的条件概率，

$P\left(s\right|zv)=\left[\begin{array}{cc}\frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]& 1-\frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]\\ 1-\frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]& \frac{1}{2}\[1+erf\left(\frac{t_0-t}{\mathrm{\σ}\sqrt{2}}\right)\]\end{array}\right]$

$P\left(s\right|zv)=\left[\begin{array}{cc}\frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]& 1-\frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]\\ 1-\frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]& \frac{1}{2}\[1+erf\left(\frac{t-(t_0+Tgait*0.452)}{\mathrm{\σ}\sqrt{2}}\right)\]\end{array}\right]$

其中，σ标识正态分布的方差，σ²＝C×T_gait，t₀为摆布的起始时刻，t为第k时刻(当前时刻)，C为一常数，T_gait表示整个跨步周期的时间长度。P(a|zv)表示在已知零速度概率条件下的加速度的条件概率。

$P\left(a\right|zv)=\left[\begin{array}{cc}\frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]& 1-\frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]\\ 1-\frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]& \frac{1}{2}\[1+erf\left(\frac{norm(a_n-\[0,0,g\])}{\mathrm{\σ}\sqrt{2}}\right)\]\end{array}\right]$

其中，a_n表示在地球坐标系下的加速度。P(zv)表示基于人体运动学的概率统计得到第k时刻(当前时刻)为零速度的概率，

$P\left(zv\right)=\left\{\begin{array}{cc}0.548,& isZV\\ 0.452,& notZV\end{array}\right.$

与所述判断模块113连接的第一处理模块114用于利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取校准后的移动位置坐标信息。步骤S12是在检测到的零速度阶段，利用误差状态卡尔曼滤波器，对推算的运动数据误差状态进行估计，然后将估计的误差去修正捷联惯导算法所推算的移动位置坐标信息。所述第一处理模块114具体包括以下功能：

通过定位设备获取所述步行者的初始移动位置坐标信息。

利用捷联惯导算法，对所述三轴加速度计和三轴陀螺仪采集到的运动数据进行计算。本步骤通过公式(6)计算步行者当前时刻，即第k时刻的加速度。

$a_k^n=C_{b_{k/k-1}}^n\·a_k^b$

其中，为所述步行者第k时刻(当前时刻)在地球坐标系下X轴/Y轴/Z轴的加速度，k表示第k时刻(当前时刻)，即，n表示地球坐标系下的X轴/Y轴/Z轴；为第k时刻(当前时刻)的方向余弦矩阵，通过该方向余弦矩阵将所述传感数据从传感器坐标系下转换至地球坐标系下，k-1为第k-1时刻(前一时刻)；为所述步行者第k时刻在传感器坐标系下第一方向/第二方向/第三方向的加速度，k表示第k时刻(当前时刻)，b表示传感器坐标系下第一方向/第二方向/第三方向。同理，计算所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息，同样是通过为第k时刻(当前时刻)的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下转换至地球坐标系下。其中，第k时刻(当前时刻)的方向余弦矩阵的计算公式为：

$C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n.\frac{2I_{3\×3}+{\mathrm{\δ\Ω}}_K\·\mathrm{\Δ}t}{2I_{3\×3}-{\mathrm{\δ\Ω}}_k\·\mathrm{\Δ}t}$

其中，为第k-1时刻(前一时刻)的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下转换至地球坐标系下；δΩ_k表示为基于角速度的矩阵用以表示小的角度变换对方向变换的影响，δΩ_k的表达式为 ${\mathrm{\δ\Ω}}_k=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_k^b\left(3\right)& {\mathrm{\ω}}_k^b\left(2\right)\\ {\mathrm{\ω}}_k^b\left(3\right)& 0& -{\mathrm{\ω}}_k^b\left(1\right)\\ -{\mathrm{\ω}}_k^b\left(2\right)& {\mathrm{\ω}}_k^b\left(1\right)& 0\end{array}\right],$ 1，2，3表示传感器坐标系下第一方向/第二方向/第三方向。方向余弦矩阵当k＝1时， $C_{b_0}^n\left[\begin{array}{ccc}\cos \left(pitch\right)& \sin \left(pitch\right)& \cos \left(roll\right)\sin \left(pitch\right)\\ 0& \cos \left(roll\right)& -\sin \left(roll\right)\\ -\sin \left(pitch\right)& \sin \left(roll\right)\cos \left(pitch\right)& \cos \left(roll\right)\cos \left(pitch\right)\end{array}\right],$ 其中，roll表示翻滚角， $roll=arctan(a_y^{sensor}/a_z^{sensor});$ pitch是俯仰角， $pitch=-arcsin(a_x^{sensor}/g),$ yaw是航向角。指加速度传感器测得的x轴的数据，指加速度传感器测得的y轴的数据,指加速度传感器测得的z轴的数据；g为重力加速度。在有磁力计的情况下，yaw为磁力计在水平面上的度数，没有磁力计的情况下初始值设置为0。I_3*3为3维单位矩阵，即 $I_{3*3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right];$ 为在传感器坐标系下所述步行者第k时刻的角速度信息；Δt表示采样时间间隔，即第k时刻(当前时刻)与第k-1时刻(前一时刻)之间相差的时间。

对进行两次积分以获取所述步行者第k时刻(当前时刻)未校准的移动位置坐标信息。

在所述零速度阶段利用误差状态卡尔曼滤波对运动数据误差状态进行误差状态偏置估计。误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度误差估计。

所述步行者第k时刻的运动数据误差状态可以标记为δx_k，该位置信息计算误差δx_k采用如下误差向量表示：

其中，表示所述步行者第k时刻的姿态误差，表示所述步行者第k时刻的角速度误差，δr_k表示所述步行者第k时刻的位置误差，δv_k表示所述步行者第k时刻的速度误差，表示所述步行者第k时刻的加速度误差，这5个误差向量都是对应3个正交轴。

所述步行者第k时刻的运动数据误差状态δx_k的计算公式为

δx_k＝F_kδx_k-1+w_k-1

其中，F_k表示状态转移矩阵，δx_k-1表示所述步行者第k-1时刻的位置信息计算误差，w_k-1表示所述步行者第k时刻的位置信息计算误差的噪声。在本实施例中，由于需要采用卡尔曼滤波法对误差进行估计，则卡尔曼滤波的量测模型为

Z_k＝HδX_k|k+n_k

当脚步处于静止状态的时候，加速度和角速度应该为0，但是实际采集的数据存在噪声，这个噪声经过积分后得到速度误差，另外一个就是实际测量到的角速度误差，这两个值作为误差状态的测量值Z_k，即H为量测误差， $H=\left[\begin{array}{ccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\end{array}\right],$ I_3×3为单位矩阵。

校准后的所述步行者第k时刻的运动数据误差状态

δx_k＝δx_k-1+K_k·[Z_k-Hδx_k-1]

其中，K_k为卡尔曼增益，卡尔曼增益K_k＝P_k-1H^T(HP_KH^T+R_k)^-1中P_k为所述步行者第k时刻为误差状态协方差矩阵，P_k＝(I_15×15-K_kH)P_k-1(I_15×15-K_kH)^T+R_k，P_k-1为所述步行者第k-1时刻为误差状态协方差矩阵，R_k为测量噪声协方差矩阵。在本步骤中，采用以下公式分别估计姿态误差，角速度误差、位置误差、速度误差、及加速度误差，具体如下：

${\mathrm{\δw}}_k^b={\mathrm{\δw}}_{k-1}^b+K_k\·\[Z_k-{\mathrm{H\δw}}_{k-1}^b\]$

δr_k＝δr_k-1+K_k·[Z_k-Hδr_k-1]

δv_k＝δv_k-1+K_k·[Z_k-Hδv_k-1]

${\mathrm{\δa}}_k^b={\mathrm{\δa}}_{k-1}^b+K_k\·\[Z_k-{\mathrm{H\δa}}_{k-1}^b\]$

修正第k时刻(当前时刻)运动数据误差状态并获取校准后的移动位置坐标信息。姿态误差估计为采用姿态误差，即所述步行者第k时刻的姿态误差通过一姿态误差矩阵δΩ_k对方向余弦矩阵 $C_{b_{k/k-1}}^n=f(C_{b_{k/k-1}}^n,{\mathrm{\ω}}_k^b)=C_{b_{k-1/k-1}}^n\×\frac{2I_{3\×3}+\mathrm{\δ}{2}_K\·\mathrm{\Δ}t}{2I_{3\×3}-{\mathrm{\δ\Ω}}_k\·\mathrm{\Δ}t}$ 进行修正，其中姿态误差矩阵

角速度误差估计为采用角速度误差，即所述步行者第k时刻(当前时刻)的角速度误差对第k+1时刻(下一时刻)的角速度进行补偿；其中，下一时刻的角速度＝当前时刻的角速度+角速度误差；

位置误差估计为采用所述步行者第k时刻(当前时刻)的位置误差δr_k对当前时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信息＝当前时刻未校准的移动位置坐标信息－位置误差；

速度误差估计为采用速度误差δv_k，所述步行者第k时刻的速度误差δv_k对当前时刻的速度进行补偿；其中，所述当前时刻的速度＝未校准的所述步行者当前时刻的移动位置坐标信息中的速度－速度误差；

加速度误差估计为采用加速度误差即所述步行者第k时刻的加速度偏置对下一时刻的加速度进行补偿；其中，下一时刻的加速度＝当前时刻的加速度+加速度误差。

所述上层滤波单元12用于执行上层滤波。所述上层滤波单元12包括：

与所述判断模块113和第一处理模块114连接的第二处理模块121用于查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差。请参阅图2，显示为基于三轴加速度与三轴陀螺仪采集的数据计算的零速度阶段与零速度阶段结束的结束时刻的计算效果示意图，图2依次展现了三轴加速度数据,三轴陀螺仪数据和基于这2个原始数据计算的零速度结果，其中第三张图中，1表示为零速度时刻，0表示非零速度时刻，(零速度即脚步静止时刻)，其中图中的点A即零速度阶段的结束时刻，在这个时刻触发利用粒子滤波算法，结合地图信息来校准计算的移动位置坐标信息。点A的获取方法如下：

若((零速度_k-1＝＝1)&&(零速度_k＝＝0))，则时刻k是零速度阶段的结束时刻。

对在所述结束时刻对当前时刻校准后的移动位置坐标信息，结合地图信息，进行粒子滤波方法进一步进行位置校准具体如下：

基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新的有效粒子仍然失败，那么就将其标记为无效粒子，其中，s表示移动位置坐标信息。粒子的权值如下：

粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设混入到一步步长的噪声v_l和一步方位变化的噪声v_ψ都服从零均值的高斯分布。以此列写出水平位置(x,y)和方位ψ的状态转移方程：

$\left\{\begin{array}{c}{x}_s^i=x_{s-1}^i+l_s^i{\mathrm{cos\ψ}}_{s-1}^i\\ y_s^i=y_{s-1}^i+l_s^i{\mathrm{sin\ψ}}_{s-1}^i\\ {\mathrm{\ψ}}_s^i={\mathrm{\ψ}}_{s-1}^i+{\mathrm{\δ\ψ}}_s^i\end{array}\right.$

在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子的基础上由重要性分布采样得到。这可以通过先采样得到粒子和然后代入状态转移方程分别得到换言之，凡是能够修正位置计算累计误差进行修正的方法均适用于本发明。

与所述第一处理模块114和第二处理模块121连接的所述检测模块122用于将修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差与当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，调用用于执行逆时针校正的校准模块123；若所述步行者的当前步伐存在顺时针偏移，调用所述校准模块123执行顺时针校正。

在本步骤中，将粒子滤波后的修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差，即校准的本步角度ψ_PF，与下层滤波后的当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，表现在本实施例中为当前步所跨角度ψ_EKF，反馈回卡尔曼滤波器中，对所述测量噪声协方差矩阵R_k进行修正。

计算当前步所跨的角度ψ_EKF，然后与利用地图配准计算的本步角度ψ_PF进行对比得到：

δ_ψ＝ψ_EKF-ψ_PF

所述校准模块123具体执行以下功能：

顺时针校正：若δ_ψ＜0时，说明存在顺时针航向角估计偏移，这种情况说明在零速度阶段对顺时针方向的补偿过度，因此采用公式(14)，对R值进行减小得到修正后的以增加估计量的比例，减小测量量的对估计

$\hat{R}=R-\mathrm{\ϵ}$

其中ε，为固定的R值减小量。

逆时针校正：若δ_ψ＞0时，说明存在逆时针航向角估计偏移，这种情况说明在零速度阶段对逆时针方向的补偿过度。同样道理，针对公式(15)，对R值进行增大得到修正后的以减小估计量的比例，增大测量量的对估计结果的影响。

$\hat{R}=R+\mathrm{\ϵ}$

若发现连续多步存在地图修正后的航向角向某个方向偏移的问题，则对EKF(误差状态卡尔曼滤波器修正)的测量噪声协方差进行连续小幅度修正。经过试验结果分析，R矩阵内非零元素合理的取值范围应该在[0.001，0.01]区间内。

综上所述，本发明所述的步行者室内双层定位方法及系统基于安装于脚部的加速度计，陀螺仪，磁力计等微型传感器，来跟踪脚部的运动特征。再底层利用误差状态卡尔曼滤波来估计误差参数来修正捷联惯导推算的位置信息，然后根据原始数据特征检查出每步的结束时刻，将底层计算的这一步步长和运动方向信息输入到上层粒子滤波器，结合地图信息，对底层滤波器估算的位置进行修正。最后将上层粒子滤波器与下层卡尔曼滤波器估算的位置进行对比，将差值反馈回下层滤波器，实现对卡尔曼滤波器的自适应参数修正，以提高整体框架算法的计算的位置精度。本发明增加了两个滤波层次之间的耦合度，提高了数据的融合效果。使基于惯性传感器的室内定位精度更高，实用性更强，并延长了算法有效计算的生命周期。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (8)

1.一种步行者室内双层定位方法，其特征在于，包括：

下层滤波步骤；所述下层滤波步骤包括：

选取所述步行者的一只脚为检测对象，并采集所述检测对象的运动数据；

根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若是，继续执行下一步骤；若否，继续判断；所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态；

利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位置坐标信息；

上层滤波步骤；所述上层滤波步骤包括：

查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一差；

将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行逆时针校正；若所述步行者的当前步伐存在顺时针偏移，执行顺时针校正。

2.根据权利要求1所述的步行者室内双层定位方法，其特征在于：所述下层滤波步骤中的所述预存位置坐标信息推算法为捷联惯导算法；所述步骤三采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未校准的移动位置坐标信息。

3.根据权利要求2所述的步行者室内双层定位方法，其特征在于：

所述下层滤波步骤中采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度误差估计。

4.根据权利要求3所述的步行者室内双层定位方法，其特征在于：

姿态误差估计为通过一姿态误差矩阵对方向余弦矩阵进行修正；

角速度误差估计为采用角速度误差对下一时刻的角速度进行补偿；其中，下一时刻的角速度＝当前时刻的角速度+角速度误差；

位置误差估计为采用位置误差对当前时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信息＝当前时刻未校准的移动位置坐标信息－位置误差；

速度误差估计为采用速度误差对当前时刻的速度进行补偿；其中，所述当前时刻的速度＝未校准的所述步行者当前时刻的移动位置坐标信息中的速度－速度误差；

加速度误差估计为采用加速度误差对下一时刻的加速度进行补偿；其中，下一时刻的加速度＝当前时刻的加速度+加速度误差。

5.根据权利要求1所述的步行者室内双层定位方法，其特征在于：在所述上层滤波步骤中执行的预制修正为执行粒子滤波算法以完成位置校准。

6.一种步行者室内双层定位系统，其特征在于，包括：

下层滤波单元，所述下层滤波单元包括：

选取模块，用于选取所述步行者的一只脚为检测对象；

采集模块，与所述选取模块连接，用于采集所述检测对象的运动数据；

判断模块，与所述采集模块连接，用于根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态，其中，所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态；若是，调用用于利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位置坐标信息的第一处理模块；若否，继续调用所述判断模块；

与所述下层滤波单元连接的上层滤波单元，所述上层滤波单元包括：

第二处理模块，与所述判断模块和第一处理模块连接，用于查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一差；

检测模块，与所述第一处理模块和第二处理模块连接，用于将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，调用用于执行逆时针校正的校准模块；若所述步行者的当前步伐存在顺时针偏移，继续调用所述校准模块执行顺时针校正。

7.根据权利要求6所述的步行者室内双层定位系统，其特征在于：所述第一处理模块中的所述预存位置坐标信息推算法为捷联惯导算法；所述第一处理模块还用于采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未校准的移动位置坐标信息。

8.根据权利要求7所述的步行者室内双层定位系统，其特征在于：所述第一处理模块还用于采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度误差估计。