一种步行者室内双层定位方法及系统的制作方法
一种步行者室内双层定位方法及系统的制作方法
【技术领域】
[0001] 本发明属于室内定位技术领域，特别是涉及一种步行者室内双层定位方法及系 统。
【背景技术】
[0002] 随着微机电系统（MEMS)快速发展，使得利用微型惯性传感器和三轴磁力计的来 跟踪步行者的运动特征，并对其运动进行推算得到实时的位置信息成为了可能。此类步行 者定位系统最大的优势在于拥有自完备特性。正是由于这种特性，此类步行者室内定位技 术越来越多得到了关注，其在某些特定的场景下有着非常重要的应用前景（应急救援，应 急医疗，商场位置确定等的）。但是由于受到传感器自身误差在长时间计算后会产生不可忽 略的误差累积问题，同时在位置推算的滤波器参数对不同步行者的运动习惯不具备自适应 性，使得这类算法在适用性上存在进一步提升的空间。于此种种问题都使得这种技术在定 位精度上一直无法满足人们的应用需求。
[0003] 在惯性定位的基础上，现有的技术提出了一种双层滤波定位架构，其基于粒子滤 波并结合室内地图信息辅助惯性定位的技术是另一种辅助定位的方法，该方法结合地图信 息利用粒子滤波算法纠正无效位置点，以达到提高定位精度的目的。这种方法在有丰富地 图信息的情况下，可以有效遏制了由于传感器误差而造成的位置误差的无休止的积累，能 得到相对较高的定位精度。但是在实际应用中，利用地图辅助修正位置信息的方法仍然存 在一定的应用难题和缺陷。当无法得到详细地图信息时，例如仅仅是建筑物轮廓图，或者步 行者长期在某个建筑物内自由运动时，地图信息不能有效的对估算位置的累积误差进行修 正。
[0004] 因此，如何提供一种步行者室内双层定位方法及系统，以解决现有技术中的利用 地图辅助步行者定位方法及系统对室内人员的定位精度不高，无法对估算的室内人员的位 置信息的累积误差进行自适应地修正等种种缺陷，实已成为本领域从业者亟待解决的技术 问题
【发明内容】
[0005] 鉴于以上所述现有技术的缺点，本发明的目的在于提供一种步行者室内双层定位 方法及系统，用于解决现有技术中利用地图辅助步行者定位方法及系统对室内人员的定位 精度不高，耦合性低、无法对估算的室内人员的位置信息的累积误差进行自适应地修正的 问题。
[0006] 为实现上述目的及其他相关目的，本发明一方面提供一种步行者室内双层定位方 法，包括：下层滤波步骤；所述下层滤波步骤包括：选取所述步行者的一只脚为检测对象， 并采集所述检测对象的运动数据；根据预定静止检测方式判断所述检测对象在当前时刻 是否处于静止状态；若是，继续执行下一步骤；若否，继续判断；所述静止状态分为非零速 度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态；利用预存位置 坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状 态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位 置坐标信息；上层滤波步骤；所述上层滤波步骤包括：查找当前时刻中所述零速度阶段的 结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预 制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一 差；将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后的 移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还 是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行逆时针校正；若所述步 行者的当前步伐存在顺时针偏移，执行顺时针校正。
[0007] 可选地，所述下层滤波步骤中的所述预存位置坐标信息推算法为捷联惯导算法； 所述步骤三采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算 所述步行者当前时刻未校准的移动位置坐标信息。
[0008] 可选地，所述下层滤波步骤中采用误差状态卡尔曼滤波对所述运动数据误差状态 进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差 估计、速度误差估计、及加速度误差估计。
[0009] 可选地，姿态误差估计为通过一姿态误差矩阵对方向余弦矩阵进行修正；角速度 误差估计为采用角速度误差对下一时刻的角速度进行补偿；其中，下一时刻的角速度=当 前时刻的角速度+角速度误差；位置误差估计为采用位置误差对当前时刻未校准的移动位 置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信息=当前时刻未校准 的移动位置坐标信息一位置误差；速度误差估计为采用速度误差对当前时刻的速度进行 补偿；其中，所述当前时刻的速度=未校准的所述步行者当前时刻的移动位置坐标信息中 的速度一速度误差；加速度误差估计为采用加速度误差对下一时刻的加速度进行补偿；其 中，下一时刻的加速度=当前时刻的加速度+加速度误差。
[0010] 可选地，在所述上层滤波步骤中执行的预制修正为执行粒子滤波算法以完成位置 校准。
[0011] 本发明另一方面还提供一种步行者室内双层定位系统，包括：下层滤波单元，所述 下层滤波单元包括：选取模块，用于选取所述步行者的一只脚为检测对象；采集模块，与所 述选取模块连接，用于采集所述检测对象的运动数据；判断模块，与所述采集模块连接，用 于根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态，其中，所述静 止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状 态；若是，调用用于利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置 坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态 并获取当前时刻校准后的移动位置坐标信息的第一处理模块；若否，继续调用所述判断模 块；与所述下层滤波单元连接的上层滤波单元，所述上层滤波单元包括：第二处理模块，与 所述判断模块和第一处理模块连接，用于查找当前时刻中所述零速度阶段的结束时刻，利 用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获 取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一差；；检测模 块，与所述第一处理模块和第二处理模块连接，用于将将所述第一差与下层滤波后当前时 刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较， 以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当 前步伐存在逆时针偏移，调用用于执行逆时针校正的校准模块；若所述步行者的当前步伐 存在顺时针偏移，继续调用所述校准模块执行顺时针校正。
[0012] 可选地，所述第一处理模块中的所述预存位置坐标信息推算法为捷联惯导算法； 所述第一处理模块还用于采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和 角速度以推算所述步行者当前时刻未校准的移动位置坐标信息。
[0013] 可选地，所述第一处理模块还用于采用误差状态卡尔曼滤波对所述运动数据误差 状态进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置 误差估计、速度误差估计、及加速度误差估计。
[0014] 如上所述，本发明的步行者室内定位方法及系统，具有以下有益效果：
[0015] 本发明增加了两个滤波层次之间的耦合度，提高了数据的融合效果。使基于惯性 传感器的室内定位精度更高，实用性更强，并延长了算法有效计算的生命周期。
【附图说明】
[0016] 图1显示为本发明的步行者室内定位方法流程示意图。
[0017] 图2显示为本发明的基于三轴加速度与三轴陀螺仪采集的数据计算的零速度阶 段与零速度阶段结束的结束时刻的计算效果示意图。
[0018] 图3显示为本发明的步行者室内定位系统的原理结构示意图。
[0019] 元件标号说明
[0020] 1步行者室内定位系统
[0021] 11下层滤波单元
[0022] 12上层滤波单元
[0023] 111选取模块
[0024] 112采集模块
[0025] 113判断模块
[0026] 114第一处理模块
[0027] 121第二处理模块
[0028] 122检测模块
[0029] 123校准模块
[0030] S1 ~S2 步骤
【具体实施方式】
[0031] 以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书 所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实 施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离 本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施 例中的特征可以相互组合。
[0032] 需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构 想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸 绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也 可能更为复杂。
[0033] 本发明的基本技术方案如下：
[0034] 底层基于安装于脚步的微型传感器所采集的数据，利用零速度更新（ZUPT，Zero VelocityUpdate)和零角速度更新（ZARU,ZeroAngularRateUpdate)辅助策略的误差状 态卡尔曼滤波器进行初步的步行者位置推算，然后基于检出的信号特征，判断出每一步的 结束时刻，在此时刻将计算的这一步所运动的位移长度和运动方向信息传输到上一层滤波 器，上一层滤波采用粒子滤波方法，基于地图信息，对本步所计算的步长和方向进行再次滤 波处理，得出最优结果。然后将最优结果与底层滤波数据的结果进行对比，将此对比的差值 反馈回底层误差状态卡尔曼滤波器，用以自适应修正卡尔曼滤波器的计算参数。
[0035] 实施例一
[0036] 本实施例提供一种步行者室内双层定位方法，请参阅图1，显示为步行者室内定位 方法流程示意图。如图1所示，所述步行者室内双层定位方法包括以下步骤：
[0037] S1，下层滤波步骤，所述下层滤波步骤具体包括： [0038] S11，选取所述步行者的一只脚为检测对象，并采集所述检测对象的运动数据。在 本步骤中，首先需要选取固定一只脚作为检测对象，然后基于在该脚上安装惯性传感器采 集运动数据。所述惯性传感器包括三轴加速度计、三轴陀螺仪、磁力计分别采集加速度和角 速度等运动数据。
[0039] S12,根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若 是，继续执行下一步骤，即S13 ;若否，返回步骤S12,继续判断所述检测对象在当前时刻是 否处于静止状态；所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动 数据存在运动数据误差状态。在本实施例中，所述预定精致检测方式可以采用传统的联合 阈值条件判断法，也可以概率统计推算法，如贝叶斯估计，HMM等算法。例如，在本实施例中， 采用贝叶斯估计判断静止状态。
[0040] 所述步骤S12包括以下步骤：
[0041] 第一步，基于所述传感数据计算所述步行者的跨步周期；所述跨步周期包括脚接 触阶段、脚站立阶段，脚离地阶段、及脚摆布阶段。在本实施例中，计算所述步行者的跨步周 期需要基于所述传感模块中三轴陀螺仪y轴数据，根据三轴陀螺仪y轴信号中跨步信号特 征来计算跨步周期。在摆步周期的起始阶段，任何人的陀螺仪y轴数据均存在一个谷值，可 以利用对过零点设置阈值的方法进行谷值探测，但是不同人信号特征可能会引入误判断谷 值，因此针对跨步信号特征，维护一个标志位，当探测到零速度区间是，将此标志位置true， 当在摆步阶段探测到第一个谷值的时候讲此标志位置false，在之后false阶段探测到谷 值均忽略。同时基于人体运动学信息，摆步阶段要持续38%时间，在此时间段内检测到的其 余谷值均忽略。实验证明，此两种机制可以保证100%的跨步周期内的局部谷值计算准确 率。也就是说，在每个跨步周期中均可以找到一个明显的谷值（局部最小值）以这个谷值 为脚尖离地时刻，作为判断一个完整跨步周期的分割点，实现对不同脚步运动阶段的划分 估计。
[0042] 第二步，按照预定规则对应查询所述步行者在第k时刻（当前时刻）应该处于跨 步周期中的哪个阶段，并获取查询结果。在本实施例中，所述预定规则为人体运动学模型， 此模型定义了一个跨步周期为单条腿从脚跟触地，到脚掌触地，到脚跟离地，脚尖离地，摆 步，最后脚跟再次触地。此周期内包含站位阶段和摆步阶段，其中站位阶段包括接触阶段， 站立阶段，离地阶段。人体运动学模型定义了在正常运动是各个阶段在真个跨步周期内所 占据的时间比例。在本步骤中，所述步行者所处第k时刻对应到该人体运动学模型中对应 查询所述步行者在第k时刻（当前时刻）应该处于跨步周期中的哪个阶段，便得到查询结 果，所述查询结果包括所述步行者在第k时刻（当前时刻）处于站位阶段或所述步行者在 第k时刻（当前时刻）处于摆布阶段，假如把一个跨步周期分为100份，A点表示脚跟触 地（0% )，B点表示脚掌触地（16. 8% )，C点表示脚跟离地（41. 5% )，D点表示脚尖离地 (62% )，E点表示脚跟触地（100% )，脚跟触地到脚掌触地为接触阶段，脚掌触地到脚跟离 地为站立阶段，脚跟离地到脚尖离地为离地阶段，脚尖离地到脚跟触地为摆布阶段。因此， 可以从图3中可以看出，站位阶段占62%，摆布阶段占38%。
[0043] 第三步，根据预存的陀螺仪设置阈值判断所述检测对象是否处于静止状态，获取 第一判断结果，即通过三轴陀螺仪获取到的所述步行者第k时刻（当前时刻）的角速度信 息与预存的陀螺仪设置阈值进行比对，若所述步行者第k时刻（当前时刻）的角速度信息 小于预存的陀螺仪设置阈值，则表示所述检测对象处于静止状态，若所述步行者第k时刻 (当前时刻）的角速度信息大于预存的陀螺仪设置阈值，则表示所述检测对象处于运动状 态，返回步骤S3,继续判断；若所述第一判断结果为所述检测对象处于静止状态，继续执行 下一步骤。
[0044] 第四步，判断所述第一判断结果是否与所述查询结果一致（检测对象处于静止状 态对应所述检测对象处于站位阶段，即表示第一判断结果与所述查询结果一致，或检测对 象处于运动状态即表示所述检测对象处于摆布阶段即表示第一判断结果与所述查询结果 一致），若是，则表示不存在潜在误差点；若否，则表示存在潜在误差点，执行第五步。
[0045] 第五步，通过朴素贝叶斯估计所述潜在误差点是否处于零速度区间，若是，则表示 所述检测对象在第k时刻（当前时刻）处于静止状态，继续执行步骤S13,若否，则表示所述 检测对象在第k时刻处于运动状态，则执行步骤S12,继续判断。在本步骤中，采用以下朴素 贝叶斯估计公式来估计，朴素贝叶斯估计公式为：
[0046]zv=argmaxP(s|zv)P(a|zv)P(zv) 公式（1)其中，P(s|zv)表示脚步接 触地面的结束时刻的条件概率，
[0049]其中，〇标识正态分布的方差，〇 2=CXTgait，tQ为摆布的起始时刻，t为第k时 刻（当前时刻），C为一常数，Tgait表示整个跨步周期的时间长度。P(a|zv)表示在已知零 速度概率条件下的加速度的条件概率。
[0051] 其中，an表示在地球坐标系下的加速度。P(zv)表示基于人体运动学的概率统计 得到第k时刻（当前时刻）为零速度的概率，
[0053]S13,利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标 信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获 取校准后的移动位置坐标信息。步骤S12是在检测到的零速度阶段，利用误差状态卡尔曼 滤波器，对推算的运动数据误差状态进行估计，然后将估计的误差去修正捷联惯导算法所 推算的移动位置坐标信息。具体包括以下步骤：
[0054] 第六步，通过定位设备获取所述步行者的初始移动位置坐标信息。
[0055] 第七步，利用捷联惯导算法，对所述三轴加速度计和三轴陀螺仪采集到的运动数 据进行计算。本步骤通过公式（6)计算步行者当前时刻，即第k时刻的加速度。
[0057] 其中，< 为所述步行者第k时刻（当前时刻）在地球坐标系下X轴/Y轴/Z轴的 加速度，k表示第k时刻（当前时刻），即，n表示地球坐标系下的X轴/Y轴/Z轴；为 第k时刻（当前时刻）的方向余弦矩阵，通过该方向余弦矩阵将所述传感数据从传感器坐 标系下转换至地球坐标系下，k-1为第k-1时刻（前一时刻）；4为所述步行者第k时刻在 传感器坐标系下第一方向/第二方向/第三方向的加速度，k表示第k时刻（当前时刻），b表示传感器坐标系下第一方向/第二方向/第三方向。同理，计算所述步行者第k时刻的 角速度信息、位置信息、速度信息、及角速度变化信息，同样是通过m为第k时刻（当前 时刻）的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速 度变化信息从传感器坐标系下转换至地球坐标系下。其中，第k时刻（当前时刻）的方向 余弦矩阵^^^,的计算公式为：
[0059] 其中，，为第k-1时刻（前一时刻）的方向余弦矩阵将所述步行者 第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下 转换至地球坐标系下；SQk表示为基于角速度的矩阵用以表示小的角度变换对方 向变换的影响，SQk的表达式为
，1，2,3表示传 感器坐标系下第一方向/第二方向/第三方向。方向余弦矩阵<^^,当k= 1时，
其中，roll表示翻滚角，
;pitch是俯仰角，
，yaw是航向角。 a：-mmmmmrimxmamm,<mormmmimmrimj,cr〇r 指加速度传感器测得的Z轴的数据；g为重力加速度。在有磁力计的情况下，yaw为磁 力计在水平面上的度数，没有磁力计的情况下初始值设置为〇。13"为3维单位矩阵，即
;4为在传感器坐标系下所述步行者第k时刻的角速度信息；At表示采 样时间间隔，即第k时刻（当前时刻）与第k-1时刻（前一时刻）之间相差的时间。
[0060] 第八步，对<进行两次积分以获取所述步行者第k时刻（当前时刻）未校准的移 动位置坐标信息。
[0061] 第九步，在所述零速度阶段利用误差状态卡尔曼滤波对运动数据误差状态进行误 差状态偏置估计。误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、 速度误差估计、及加速度误差估计。
[0062]所述步行者第k时刻的运动数据误差状态可以标记为SXk，该位置信息计算误差 sxk采用如下误差向量表示：
[0064]其中，表示所述步行者第k时刻的姿态误差，表示所述步行者第k时刻的 角速度误差，Srk表示所述步行者第k时刻的位置误差，SVk表示所述步行者第k时刻的 速度误差，表示所述步行者第k时刻的加速度误差，这5个误差向量都是对应3个正交 轴。
[0065] 所述步行者第k时刻的运动数据误差状态SXk的计算公式为
[0066] 5xk=Fk 8Xh+Wh公式（8)
[0067]其中，Fk表示状态转移矩阵，Sxh表示所述步行者第k_l时刻的位置信息计算误 差，Wh表示所述步行者第k时刻的位置信息计算误差的噪声。在本实施例中，由于需要采 用卡尔曼滤波法对误差进行估计，则卡尔曼滤波的量测模型为：
[0068]Zk=H8xk|k+nk 公式（9)
[0069]当脚步处于静止状态的时候，加速度和角速度应该为0,但是实际采集的数据 存在噪声，这个噪声经过积分后得到速度误差，另外一个就是实际测量到的角速度误 差，这两个值作为误差状态的测量值zk，g卩
，H为量测误差，
，为单位矩阵。
[0070] 校准后的所述步行者第k时刻的运动数据误差状态
[0071] 8 xk= 8 x H+Kk ? [Zk_H 8 xk_J 公式（10)
[0072] 其中，Kk为卡尔曼增益，卡尔曼增益Kk=PuHTOlPKHT+Rkr1中Pk为所述步行者第k 时刻为误差状态协方差矩阵，Pk=(I 所述步行者第k-1 时刻为误差状态协方差矩阵，Rk为测量噪声协方差矩阵。在本步骤中，采用公式（10)分别 估计姿态误差，角速度误差、位置误差、速度误差、及加速度误差，具体如 下：
[0078] 第十步，修正第k时刻（当前时刻）运动数据误差状态并获取校准后的移动位置 坐标信息。姿态误差估计为采用姿态误差，即所述步行者第k时刻的姿态误差^>>/,，通过一 姿态误差矩阵S Qk对方向余弦矩阵
进行修 正，其中姿态误差矩阵S Qk为
[0079] 角速度误差估计为采用角速度误差，即所述步行者第k时刻（当前时刻）的角速 度误差对第k+1时刻（下一时刻）的角速度进行补偿；其中，下一时刻的角速度=当前 时刻的角速度+角速度误差；
[0080] 位置误差估计为采用所述步行者第k时刻（当前时刻）的位置误差S rk对当前 时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信 息=当前时刻未校准的移动位置坐标信息一位置误差；
[0081] 速度误差估计为采用速度误差SVk，所述步行者第k时刻的速度误差sVk对当前 时刻的速度进行补偿；其中，所述当前时刻的速度=未校准的所述步行者当前时刻的移动 位置坐标信息中的速度一速度误差；
[0082] 加速度误差估计为采用加速度误差&[，即所述步行者第k时刻的加速度偏置 对下一时刻的加速度进行补偿；其中，下一时刻的加速度=当前时刻的加速度+加速度误 差。
[0083] S2,上层滤波步骤。所述上层滤波步骤具体包括：
[0084] S21，查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时 刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动坐标位置信 息和上一时刻校准后的移动位置坐标信息的第一差。请参阅图2,显示为基于三轴加速度与 三轴陀螺仪采集的数据计算的零速度阶段与零速度阶段结束的结束时刻的计算效果示意 图，图2依次展现了三轴加速度数据，三轴陀螺仪数据和基于这2个原始数据计算的零速 度结果，其中第三张图中，1表示为零速度时刻，〇表示非零速度时刻，（零速度即脚步静止 时刻），其中图中的点A即零速度阶段的结束时刻，在这个时刻触发利用粒子滤波算法，结 合地图信息来校准计算的移动位置坐标信息。点A的获取方法如下：
[0085]若（（零速度h= =1)&&(零速度k== 〇))，则时刻k是零速度阶段的结束时刻。 [0086] 对在所述结束时刻对当前时刻校准后的移动位置坐标信息，结合地图信息，利用 粒子滤波进一步对位置进行校准具体如下：
[0087] 基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对 于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信 息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新 的有效粒子仍然失败，那么就将其标记为无效粒子，其中，s表示移动位置坐标信息。粒子 的权值如下：
[0089] 粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设 混入到一步步长〖的噪声^和一步方位变化M的噪声^都服从零均值的高斯分布。以此列 写出水平位置（x，y)和方位步的状态转移方程：
[0091] 在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子酌基础上由 重要性分布Philx〗])采样得到。这可以通过先采样得到粒子4和而/;〗，然后代入状态转移 方程分别得到冲〗。换言之，凡是能够修正位置计算累计误差进行修正的方法均适用于 本发明。
[0092]S22,将修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第 一差与下层滤波后的当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置 坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺 时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行步骤S23,即逆时针校正；若所 述步行者的当前步伐存在顺时针偏移，执行步骤S24,即顺时针校正。
[0093] 在本步骤中，将修正修正后的移动坐标位置信息和上一时刻校准后的移动位置坐 标信息的第一差，即校准的本步角度!tPF，（PF,ParticleFilter,粒子滤波）与当前时刻校 准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，表现在本实施 例中为当前步所跨角度！tEKF，（EKF，ExtendedKalmanFilter,扩展卡尔曼滤波）反馈回卡 尔曼滤波器中，对所述测量噪声协方差矩阵Rk进行修正。
[0094] 计算当前步所跨的角度步EKF，然后与利用地图配准计算的本步角度步^进行对比 得到：
[0095] 8^=i]) EKF-itPF 公式（13)
[0096] 因此判断R值修正时刻可以采用如下策略：
[0097] 执行顺时针校正：若S$ < 〇时
[0098] 说明存在顺时针航向角估计偏移，这种情况说明在零速度阶段对顺时针方向的补 偿过度，因此采用公式（14)，对R值进行减小得到修正后的々'，以增加估计量的比例，减小测 量量的对估计
[0100] 其中e，为固定的R值减小量。
[0101] 执行逆时针校正：若s〇时
[0102] 说明存在逆时针航向角估计偏移，这种情况说明在零速度阶段对逆时针方向的补 偿过度。同样道理，针对公式（15)，对R值进行增大得到修正后的哀，以减小估计量的比例， 增大测量量的对估计结果的影响。
[0104] 若发现连续多步存在地图修正后的航向角向某个方向偏移的问题，则对EKF(误 差状态卡尔曼滤波器修正）的测量噪声协方差进行连续小幅度修正。经过试验结果分析， R矩阵内非零元素合理的取值范围应该在[0.001，0.01]区间内。
[0105] 本实施例所述的步行者室内双层定位方法基于安装于脚部的加速度计，陀螺仪， 磁力计等微型传感器，来跟踪脚部的运动特征。再底层利用误差状态卡尔曼滤波来估计误 差参数来修正捷联惯导推算的位置信息，然后根据原始数据特征检查出每步的结束时刻， 将底层计算的这一步步长和运动方向信息输入到上层粒子滤波器，结合地图信息，对底层 滤波器估算的位置进行修正。最后将上层粒子滤波器与下层卡尔曼滤波器估算的位置进行 对比，将差值反馈回下层滤波器，实现对卡尔曼滤波器的自适应参数修正，以提高整体框架 算法的计算的位置精度。本发明增加了两个滤波层次之间的耦合度，提高了数据的融合效 果。使基于惯性传感器的室内定位精度更高，实用性更强，并延长了算法有效计算的生命周 期。
[0106] 实施例二
[0107] 本实施例提供一种步彳丁者室内双层定位系统1，请参阅图3,显不为步彳丁者室内双 层定位系统的原理结构示意图。所述步行者室内双层定位系统1包括下层滤波单元11和 与所述下层滤波单元11连接的上层滤波单元12,所述下层滤波模块11包括选取模块111、 采集模块112、判断模块113、及第一处理模块114。所述上层滤波单元12包括第二处理模 块121、检测模块122、及校准模块123。
[0108] 其中，所述下层滤波模块11用于执行下层滤波。
[0109] 所述选取模块111用于选取所述步行者的一只脚为检测对象。
[0110] 与所述选取模块111连接的采集模块112用于采集所述检测对象的运动数据。在 本实施例中，首先需要选取固定一只脚作为检测对象，然后基于在该脚上安装惯性传感器 采集运动数据。所述惯性传感器包括三轴加速度计、三轴陀螺仪、磁力计分别采集加速度和 角速度等运动数据。
[0111]与所述采集模块112连接的所述判断模块113用于根据预定静止检测方式判断 所述检测对象在当前时刻是否处于静止状态；若是，调用所述第一处理模块114 ;若否，继 续调用所述判断模块113,继续判断所述检测对象在当前时刻是否处于静止状态；所述静 止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状 态。在本实施例中，所述预定精致检测方式可以采用传统的联合阈值条件判断法，也可以概 率统计推算法，如贝叶斯估计，HMM等算法。例如，在本实施例中，采用贝叶斯估计判断静止 状态。
[0112] 所述判断模块具体执行功能包括：
[0113] 基于所述传感数据计算所述步行者的跨步周期；所述跨步周期包括脚接触阶段、 脚站立阶段，脚离地阶段、及脚摆布阶段。在本实施例中，计算所述步行者的跨步周期需要 基于所述传感模块中三轴陀螺仪y轴数据，根据三轴陀螺仪y轴信号中跨步信号特征来计 算跨步周期。在摆步周期的起始阶段，任何人的陀螺仪y轴数据均存在一个谷值，可以利用 对过零点设置阈值的方法进行谷值探测，但是不同人信号特征可能会引入误判断谷值，因 此针对跨步信号特征，维护一个标志位，当探测到零速度区间是，将此标志位置true，当在 摆步阶段探测到第一个谷值的时候讲此标志位置false，在之后false阶段探测到谷值均 忽略。同时基于人体运动学信息，摆步阶段要持续38%时间，在此时间段内检测到的其余 谷值均忽略。实验证明，此两种机制可以保证100%的跨步周期内的局部谷值计算准确率。 也就是说，在每个跨步周期中均可以找到一个明显的谷值（局部最小值）以这个谷值为脚 尖离地时刻，作为判断一个完整跨步周期的分割点，实现对不同脚步运动阶段的划分估计。
[0114] 按照预定规则对应查询所述步行者在第k时刻（当前时刻）应该处于跨步周期中 的哪个阶段，并获取查询结果。在本实施例中，所述预定规则为人体运动学模型，此模型定 义了一个跨步周期为单条腿从脚跟触地，到脚掌触地，到脚跟离地，脚尖离地，摆步，最后脚 跟再次触地。此周期内包含站位阶段和摆步阶段，其中站位阶段包括接触阶段，站立阶段， 离地阶段。人体运动学模型定义了在正常运动是各个阶段在真个跨步周期内所占据的时间 比例。在本步骤中，所述步行者所处第k时刻对应到该人体运动学模型中对应查询所述步 行者在第k时刻（当前时刻）应该处于跨步周期中的哪个阶段，便得到查询结果，所述查询 结果包括所述步行者在第k时刻（当前时刻）处于站位阶段或所述步行者在第k时刻（当 前时刻）处于摆布阶段，假如把一个跨步周期分为100份，A点表示脚跟触地（0% )，B点表 示脚掌触地（16. 8% )，C点表示脚跟离地（41. 5% )，D点 表示脚尖离地（62% )，E点表示 脚跟触地（100% )，脚跟触地到脚掌触地为接触阶段，脚掌触地到脚跟离地为站立阶段，脚 跟离地到脚尖离地为离地阶段，脚尖离地到脚跟触地为摆布阶段。因此，可以从图3中可以 看出，站位阶段占62%，摆布阶段占38%。
[0115] 根据预存的陀螺仪设置阈值判断所述检测对象是否处于静止状态，获取第一判断 结果，即通过三轴陀螺仪获取到的所述步行者第k时刻（当前时刻）的角速度信息与预存 的陀螺仪设置阈值进行比对，若所述步行者第k时刻（当前时刻）的角速度信息小于预存 的陀螺仪设置阈值，则表示所述检测对象处于静止状态，若所述步行者第k时刻（当前时 亥IJ)的角速度信息大于预存的陀螺仪设置阈值，则表示所述检测对象处于运动状态，返回 步骤S3,继续判断；若所述第一判断结果为所述检测对象处于静止状态，继续执行下一步 骤。
[0116] 判断所述第一判断结果是否与所述查询结果一致（检测对象处于静止状态对应 所述检测对象处于站位阶段，即表示第一判断结果与所述查询结果一致，或检测对象处于 运动状态即表示所述检测对象处于摆布阶段即表示第一判断结果与所述查询结果一致）， 若是，则表示不存在潜在误差点；若否，则表示存在潜在误差点，执行第五步。
[0117] 通过预定静止检测方式，即朴素贝叶斯估计所述潜在误差点是否处于零速度区 间，若是，则表示所述检测对象在第k时刻（当前时刻）处于静止状态，调用所述第一处理 模块113,若否，则表示所述检测对象在第k时刻处于运动状态，则继续调用所述判断模块 112,继续判断。在本实施例中，采用以下朴素贝叶斯估计公式来估计，朴素贝叶斯估计公式 为：
[0118] zv = argmaxP (s | zv) P (a | zv) P (zv)
[0119] 其中，P(s | zv)表示脚步接触地面的结束时刻的条件概率，
[0122] 其中，〇标识正态分布的方差，〇 2= CXTgait，h为摆布的起始时刻，t为第k时 刻（当前时刻），C为一常数，Tgait表示整个跨步周期的时间长度。P(a|zv)表示在已知零 速度概率条件下的加速度的条件概率。
[0124] 其中，an表示在地球坐标系下的加速度。P(zv)表示基于人体运动学的概率统计 得到第k时刻（当前时刻）为零速度的概率，
[0126] 与所述判断模块113连接的第一处理模块114用于利用预存位置坐标信息推算法 推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态 偏置估计以修正所述运动数据误差状态并获取校准后的移动位置坐标信息。步骤S12是在 检测到的零速度阶段，利用误差状态卡尔曼滤波器，对推算的运动数据误差状态进行估计， 然后将估计的误差去修正捷联惯导算法所推算的移动位置坐标信息。所述第一处理模块 114具体包括以下功能：
[0127] 通过定位设备获取所述步行者的初始移动位置坐标信息。
[0128] 利用捷联惯导算法，对所述三轴加速度计和三轴陀螺仪采集到的运动数据进行计 算。本步骤通过公式（6)计算步行者当前时刻，即第k时刻的加速度。
[0130] 其中，<为所述步行者第k时刻（当前时刻）在地球坐标系下X轴/Y轴/Z轴的 加速度，k表示第k时刻（当前时刻），即，n表示地球坐标系下的X轴/Y轴/Z轴；,为 第k时刻（当前时刻）的方向余弦矩阵，通过该方向余弦矩阵将所述传感数据从传感器坐 标系下转换至地球坐标系下，k-1为第k-1时刻（前一时刻）； < 为所述步行者第k时刻在 传感器坐标系下第一方向/第二方向/第三方向的加速度，k表示第k时刻（当前时刻），b表示传感器坐标系下第一方向/第二方向/第三方向。同理，计算所述步行者第k时刻的 角速度信息、位置信息、速度信息、及角速度变化信息，同样是通过，为第k时刻（当前 时刻）的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速 度变化信息从传感器坐标系下转换至地球坐标系下。其中，第k时刻（当前时刻）的方向 余弦矩阵^^^,的计算公式为：
[0132] 其中，i为第k-1时刻（前一时刻）的方向余弦矩阵将所述步行者 第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下 转换至地球坐标系下；SQk表示为基于角速度的矩阵用以表示小的角度变换对方 向变换的影响，SQk的表达式为
1，2,3表示传 感器坐标系下第一方向/第二方向/第三方向。方向余弦矩阵6^ ，当k= 1时，
其中，roll表示翻滚角，
;pitch是俯仰角，
，yaw是航向角。 '指加速度传感器测得的x轴的数据，"r"指加速度传感器测得的y轴的数据，<_' 指加速度传感器测得的z轴的数据；g为重力加速度。在有磁力计的情况下，yaw为磁 力计在水平面上的度数，没有磁力计的情况下初始值设置为〇。13"为3维单位矩阵，即
;%6为在传感器坐标系下所述步行者第k时刻的角速度信息；At表示采 样时间间隔，即第k时刻（当前时刻）与第k-1时刻（前一时刻）之间相差的时间。
[0133] 对 <进行两次积分以获取所述步行者第k时刻（当前时刻）未校准的移动位置坐 标f目息。
[0134] 在所述零速度阶段利用误差状态卡尔曼滤波对运动数据误差状态进行误差状态 偏置估计。误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误 差估计、及加速度误差估计。
[0135] 所述步行者第k时刻的运动数据误差状态可以标记为SXk，该位置信息计算误差 Sxk采用如下误差向量表示：
[0137] 其中，表示所述步行者第k时刻的姿态误差，<5^表示所述步行者第k时刻的 角速度误差，Srk表示所述步行者第k时刻的位置误差，SVk表示所述步行者第k时刻的 速度误差，&〖表示所述步行者第k时刻的加速度误差，这5个误差向量都是对应3个正交 轴。
[0138] 所述步行者第k时刻的运动数据误差状态SXk的计算公式为
[0139] 8 xk= F k 8 Xh+Wh
[0140] 其中，Fk表示状态转移矩阵，Sxh表示所述步行者第k_l时刻的位置信息计算误 差，Wh表示所述步行者第k时刻的位置信息计算误差的噪声。在本实施例中，由于需要采 用卡尔曼滤波法对误差进行估计，则卡尔曼滤波的量测模型为
[0141] Zk=H8Xk|k+nk
[0142] 当脚步处于静止状态的时候，加速度和角速度应该为0,但是实际采集的数据 存在噪声，这个噪声经过积分后得到速度误差，另外一个就是实际测量到的角速度误 差，这两个值作为误差状态的测量值Zk，S卩
，H为量测误差，
1I3X3为单位矩阵。
[0143] 校准后的所述步行者第k时刻的运动数据误差状态
[0144] 8 xk= 8 xH+Kk? [Zk_H 8 xk_J
[0145] 其中，Kk为卡尔曼增益，卡尔曼增益Kk=PuHTOlPKHT+Rkr1中Pk为所述步行者第k 时刻为误差状态协方差矩阵，Pk= (I 所述步行者第k-1 时刻为误差状态协方差矩阵，Rk为测量噪声协方差矩阵。在本步骤中，采用以下公式分别估 计姿态误差，角速度误差、位置误差、速度误差、及加速度误差，具体如下：
[0151] 修正第k时刻（当前时刻）运动数据误差状态并获取校准后的移动位置坐标信 息。姿态误差估计为采用姿态误差，即所述步行者第k时刻的姿态误差5奶，通过一姿态误 差矩阵SQk对方向余弦矩阵
?进行修正，其 中姿态误差矩阵况\为
[0152] 角速度误差估计为采用角速度误差，即所述步行者第k时刻（当前时刻）的角速 度误差对第k+1时刻（下一时刻）的角速度进行补偿；其中，下一时刻的角速度=当前 时刻的角速度+角速度误差；
[0153] 位置误差估计为采用所述步行者第k时刻（当前时刻）的位置误差S rk对当前 时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信 息=当前时刻未校准的移动位置坐标信息一位置误差；
[0154] 速度误差估计为采用速度误差S Vk，所述步行者第k时刻的速度误差S Vk对当前 时刻的速度进行补偿；其中，所述当前时刻的速度=未校准的所述步行者当前时刻的移动 位置坐标信息中的速度一速度误差；
[0155] 加速度误差估计为采用加速度误差&(，即所述步行者第k时刻的加速度偏置 对下一时刻的加速度进行补偿；其中，下一时刻的加速度=当前时刻的加速度+加速度误 差。
[0156] 所述上层滤波单元12用于执行上层滤波。所述上层滤波单元12包括：
[0157] 与所述判断模块113和第一处理模块114连接的第二处理模块121用于查找当前 时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的 移动位置坐标信息执行预制修正，并获取修正后的移动坐标位置信息和上一时刻校准后的 移动位置坐标信息的第一差。请参阅图2,显示为基于三轴加速度与三轴陀螺仪采集的数 据计算的零速度阶段与零速度阶段结束的结束时刻的计算效果示意图，图2依次展现了三 轴加速度数据，三轴陀螺仪数据和基于这2个原始数据计算的零速度结果，其中第三张图 中，1表示为零速度时刻，0表示非零速度时刻，（零速度即脚步静止时刻），其中图中的点 A即零速度阶段的结束时刻，在这个时刻触发利用粒子滤波算法，结合地图信息来校准计算 的移动位置坐标信息。点A的获取方法如下：
[0158] 若（（零速度1)&&(零速度k== 0))，则时刻k是零速度阶段的结束时刻。
[0159] 对在所述结束时刻对当前时刻校准后的移动位置坐标信息，结合地图信息，进行 粒子滤波方法进一步进行位置校准具体如下：
[0160] 基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对 于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信 息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新 的有效粒子仍然失败，那么就将 其标记为无效粒子，其中，s表示移动位置坐标信息。粒子 的权值如下：
[0161]
[0162] 粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设 混入到一步步长〖啲噪声^和一步方位变化ST的噪声^都服从零均值的高斯分布。以此列 写出水平位置（x，y)和方位步的状态转移方程：
[0164] 在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子的基础上由 重要性分布
采样得到。这可以通过先采样得到粒子以和5也，然后代入状态转移 方程分别得到
> 换言之，凡是能够修正位置计算累计误差进行修正的方法均适用于 本发明。
[0165] 与所述第一处理模块114和第二处理模块121连接的所述检测模块122用于将修 正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差与当前时刻校 准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检 测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步 伐存在逆时针偏移，调用用于执行逆时针校正的校准模块123 ;若所述步行者的当前步伐 存在顺时针偏移，调用所述校准模块123执行顺时针校正。
[0166] 在本步骤中，将粒子滤波后的修正后的移动坐标位置信息和上一时刻校准后的移 动位置坐标信息的第一差，即校准的本步角度itPF，与下层滤波后的当前时刻校准后的移动 位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，表现在本实施例中为当前 步所跨角度步EKF，反馈回卡尔曼滤波器中，对所述测量噪声协方差矩阵Rk进行修正。
[0167] 计算当前步所跨的角度！tEKF，然后与利用地图配准计算的本步角度！^^进行对比 得到：
[01 68] 5 * = ^ EKF- ^ PF
[0169] 所述校准模块123具体执行以下功能：
[0170] 顺时针校正：若〇时，说明存在顺时针航向角估计偏移，这种情况说明在零 速度阶段对顺时针方向的补偿过度，因此采用公式（14)，对R值进行减小得到修正后的及， 以增加估计量的比例，减小测量量的对估计
[0172] 其中e，为固定的R值减小量。
[0173] 逆时针校正：若S 〇时，说明存在逆时针航向角估计偏移，这种情况说明在零 速度阶段对逆时针方向的补偿过度。同样道理，针对公式（15)，对R值进行增大得到修正后 的哀，以减小估计量的比例，增大测量量的对估计结果的影响。
[0175] 若发现连续多步存在地图修正后的航向角向某个方向偏移的问题，则对EKF(误 差状态卡尔曼滤波器修正）的测量噪声协方差进行连续小幅度修正。经过试验结果分析，R矩阵内非零元素合理的取值范围应该在[0.001，0.01]区间内。
[0176] 综上所述，本发明所述的步行者室内双层定位方法及系统基于安装于脚部的加速 度计，陀螺仪，磁力计等微型传感器，来跟踪脚部的运动特征。再底层利用误差状态卡尔曼 滤波来估计误差参数来修正捷联惯导推算的位置信息，然后根据原始数据特征检查出每步 的结束时刻，将底层计算的这一步步长和运动方向信息输入到上层粒子滤波器，结合地图 信息，对底层滤波器估算的位置进行修正。最后将上层粒子滤波器与下层卡尔曼滤波器估 算的位置进行对比，将差值反馈回下层滤波器，实现对卡尔曼滤波器的自适应参数修正，以 提高整体框架算法的计算的位置精度。本发明增加了两个滤波层次之间的耦合度，提高了 数据的融合效果。使基于惯性传感器的室内定位精度更高，实用性更强，并延长了算法有效 计算的生命周期。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。
[0177] 上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟 悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因 此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完 成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。
【主权项】
1. 一种步行者室内双层定位方法，其特征在于，包括： 下层滤波步骤；所述下层滤波步骤包括： 选取所述步行者的一只脚为检测对象，并采集所述检测对象的运动数据； 根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若是，继续 执行下一步骤；若否，继续判断；所述静止状态分为非零速度阶段和零速度阶段；所述零速 度阶段的运动数据存在运动数据误差状态； 利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所 述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时 刻校准后的移动位置坐标信息； 上层滤波步骤；所述上层滤波步骤包括： 查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前 时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动位置坐标信息和上一 时刻校准后的移动位置坐标信息的第一差； 将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后 的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移 还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行逆时针校正；若所述 步行者的当前步伐存在顺时针偏移，执行顺时针校正。2. 根据权利要求1所述的步行者室内双层定位方法，其特征在于：所述下层滤波步骤 中的所述预存位置坐标信息推算法为捷联惯导算法；所述步骤三采用所述捷联惯导算法计 算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未校准的移动位 置坐标信息。3. 根据权利要求2所述的步行者室内双层定位方法，其特征在于： 所述下层滤波步骤中采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状 态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度 误差估计、及加速度误差估计。4. 根据权利要求3所述的步行者室内双层定位方法，其特征在于： 姿态误差估计为通过一姿态误差矩阵对方向余弦矩阵进行修正； 角速度误差估计为采用角速度误差对下一时刻的角速度进行补偿；其中，下一时刻的 角速度=当前时刻的角速度+角速度误差； 位置误差估计为采用位置误差对当前时刻未校准的移动位置坐标信息进行补偿；其 中，所述当前时刻校准后的移动位置坐标信息=当前时刻未校准的移动位置坐标信息一位 a'?口 祆差； 速度误差估计为采用速度误差对当前时刻的速度进行补偿；其中，所述当前时刻的速 度=未校准的所述步行者当前时刻的移动位置坐标信息中的速度一速度误差； 加速度误差估计为采用加速度误差对下一时刻的加速度进行补偿；其中，下一时刻的 加速度=当前时刻的加速度+加速度误差。5.根据权利要求1所述的步行者室内双层定位方法，其特征在于：在所述上层滤波步 骤中执行的预制修正为执行粒子滤波算法以完成位置校准。6. -种步行者室内双层定位系统，其特征在于，包括： 下层滤波单元，所述下层滤波单元包括： 选取模块，用于选取所述步行者的一只脚为检测对象； 采集模块，与所述选取模块连接，用于采集所述检测对象的运动数据； 判断模块，与所述采集模块连接，用于根据预定静止检测方式判断所述检测对象在当 前时刻是否处于静止状态，其中，所述静止状态分为非零速度阶段和零速度阶段；所述零速 度阶段的运动数据存在运动数据误差状态；若是，调用用于利用预存位置坐标信息推算法 推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态 偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位置坐标信息的第 一处理模块；若否，继续调用所述判断模块； 与所述下层滤波单元连接的上层滤波单元，所述上层滤波单元包括： 第二处理模块，与所述判断模块和第一处理模块连接，用于查找当前时刻中所述零速 度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信 息执行预制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信 息的第一差； 检测模块，与所述第一处理模块和第二处理模块连接，用于将所述第一差与下层滤波 后当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差 进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述 步行者的当前步伐存在逆时针偏移，调用用于执行逆时针校正的校准模块；若所述步行者 的当前步伐存在顺时针偏移，继续调用所述校准模块执行顺时针校正。7. 根据权利要求6所述的步行者室内双层定位系统，其特征在于：所述第一处理模块 中的所述预存位置坐标信息推算法为捷联惯导算法；所述第一处理模块还用于采用所述捷 联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未 校准的移动位置坐标信息。8. 根据权利要求7所述的步行者室内双层定位系统，其特征在于：所述第一处理模块 还用于采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状态偏置估计；误差状 态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度 误差估计。
【专利摘要】本发明提供一种步行者室内双层定位方法，包括：下层滤波，包括：根据预定静止检测方式判断检测对象在当前时刻是否处于静止状态；若是，推算当前时刻的移动位置坐标信息，进行误差状态偏置估计，并获取当前时刻校准后的移动位置坐标信息；若否，继续判断；上层滤波，包括：查找当前时刻中零速度阶段的结束时刻，在结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，获取第一差；比较第一差与当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，检测当前步伐是逆时针偏移还是顺时针偏移，若步行者的当前步伐存在逆时针偏移或顺时针偏移，执行逆时针或顺时针校正。本发明使基于惯性传感器的室内定位精度更高。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN104897158
【申请号】CN201510359813
【发明人】徐正蓺, 杨卫军, 黄超, 魏建明
【申请人】中国科学院上海高等研究院
【公开日】2015年9月9日
【申请日】2015年6月26日
【技术领域】
[0001] 本发明属于室内定位技术领域，特别是涉及一种步行者室内双层定位方法及系 统。
【背景技术】
[0002] 随着微机电系统（MEMS)快速发展，使得利用微型惯性传感器和三轴磁力计的来 跟踪步行者的运动特征，并对其运动进行推算得到实时的位置信息成为了可能。此类步行 者定位系统最大的优势在于拥有自完备特性。正是由于这种特性，此类步行者室内定位技 术越来越多得到了关注，其在某些特定的场景下有着非常重要的应用前景（应急救援，应 急医疗，商场位置确定等的）。但是由于受到传感器自身误差在长时间计算后会产生不可忽 略的误差累积问题，同时在位置推算的滤波器参数对不同步行者的运动习惯不具备自适应 性，使得这类算法在适用性上存在进一步提升的空间。于此种种问题都使得这种技术在定 位精度上一直无法满足人们的应用需求。
[0003] 在惯性定位的基础上，现有的技术提出了一种双层滤波定位架构，其基于粒子滤 波并结合室内地图信息辅助惯性定位的技术是另一种辅助定位的方法，该方法结合地图信 息利用粒子滤波算法纠正无效位置点，以达到提高定位精度的目的。这种方法在有丰富地 图信息的情况下，可以有效遏制了由于传感器误差而造成的位置误差的无休止的积累，能 得到相对较高的定位精度。但是在实际应用中，利用地图辅助修正位置信息的方法仍然存 在一定的应用难题和缺陷。当无法得到详细地图信息时，例如仅仅是建筑物轮廓图，或者步 行者长期在某个建筑物内自由运动时，地图信息不能有效的对估算位置的累积误差进行修 正。
[0004] 因此，如何提供一种步行者室内双层定位方法及系统，以解决现有技术中的利用 地图辅助步行者定位方法及系统对室内人员的定位精度不高，无法对估算的室内人员的位 置信息的累积误差进行自适应地修正等种种缺陷，实已成为本领域从业者亟待解决的技术 问题
【发明内容】
[0005] 鉴于以上所述现有技术的缺点，本发明的目的在于提供一种步行者室内双层定位 方法及系统，用于解决现有技术中利用地图辅助步行者定位方法及系统对室内人员的定位 精度不高，耦合性低、无法对估算的室内人员的位置信息的累积误差进行自适应地修正的 问题。
[0006] 为实现上述目的及其他相关目的，本发明一方面提供一种步行者室内双层定位方 法，包括：下层滤波步骤；所述下层滤波步骤包括：选取所述步行者的一只脚为检测对象， 并采集所述检测对象的运动数据；根据预定静止检测方式判断所述检测对象在当前时刻 是否处于静止状态；若是，继续执行下一步骤；若否，继续判断；所述静止状态分为非零速 度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状态；利用预存位置 坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状 态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位 置坐标信息；上层滤波步骤；所述上层滤波步骤包括：查找当前时刻中所述零速度阶段的 结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预 制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一 差；将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后的 移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还 是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行逆时针校正；若所述步 行者的当前步伐存在顺时针偏移，执行顺时针校正。
[0007] 可选地，所述下层滤波步骤中的所述预存位置坐标信息推算法为捷联惯导算法； 所述步骤三采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算 所述步行者当前时刻未校准的移动位置坐标信息。
[0008] 可选地，所述下层滤波步骤中采用误差状态卡尔曼滤波对所述运动数据误差状态 进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差 估计、速度误差估计、及加速度误差估计。
[0009] 可选地，姿态误差估计为通过一姿态误差矩阵对方向余弦矩阵进行修正；角速度 误差估计为采用角速度误差对下一时刻的角速度进行补偿；其中，下一时刻的角速度=当 前时刻的角速度+角速度误差；位置误差估计为采用位置误差对当前时刻未校准的移动位 置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信息=当前时刻未校准 的移动位置坐标信息一位置误差；速度误差估计为采用速度误差对当前时刻的速度进行 补偿；其中，所述当前时刻的速度=未校准的所述步行者当前时刻的移动位置坐标信息中 的速度一速度误差；加速度误差估计为采用加速度误差对下一时刻的加速度进行补偿；其 中，下一时刻的加速度=当前时刻的加速度+加速度误差。
[0010] 可选地，在所述上层滤波步骤中执行的预制修正为执行粒子滤波算法以完成位置 校准。
[0011] 本发明另一方面还提供一种步行者室内双层定位系统，包括：下层滤波单元，所述 下层滤波单元包括：选取模块，用于选取所述步行者的一只脚为检测对象；采集模块，与所 述选取模块连接，用于采集所述检测对象的运动数据；判断模块，与所述采集模块连接，用 于根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态，其中，所述静 止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状 态；若是，调用用于利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置 坐标信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态 并获取当前时刻校准后的移动位置坐标信息的第一处理模块；若否，继续调用所述判断模 块；与所述下层滤波单元连接的上层滤波单元，所述上层滤波单元包括：第二处理模块，与 所述判断模块和第一处理模块连接，用于查找当前时刻中所述零速度阶段的结束时刻，利 用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获 取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第一差；；检测模 块，与所述第一处理模块和第二处理模块连接，用于将将所述第一差与下层滤波后当前时 刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较， 以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当 前步伐存在逆时针偏移，调用用于执行逆时针校正的校准模块；若所述步行者的当前步伐 存在顺时针偏移，继续调用所述校准模块执行顺时针校正。
[0012] 可选地，所述第一处理模块中的所述预存位置坐标信息推算法为捷联惯导算法； 所述第一处理模块还用于采用所述捷联惯导算法计算所述步行者在当前时刻的加速度和 角速度以推算所述步行者当前时刻未校准的移动位置坐标信息。
[0013] 可选地，所述第一处理模块还用于采用误差状态卡尔曼滤波对所述运动数据误差 状态进行误差状态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置 误差估计、速度误差估计、及加速度误差估计。
[0014] 如上所述，本发明的步行者室内定位方法及系统，具有以下有益效果：
[0015] 本发明增加了两个滤波层次之间的耦合度，提高了数据的融合效果。使基于惯性 传感器的室内定位精度更高，实用性更强，并延长了算法有效计算的生命周期。
【附图说明】
[0016] 图1显示为本发明的步行者室内定位方法流程示意图。
[0017] 图2显示为本发明的基于三轴加速度与三轴陀螺仪采集的数据计算的零速度阶 段与零速度阶段结束的结束时刻的计算效果示意图。
[0018] 图3显示为本发明的步行者室内定位系统的原理结构示意图。
[0019] 元件标号说明
[0020] 1步行者室内定位系统
[0021] 11下层滤波单元
[0022] 12上层滤波单元
[0023] 111选取模块
[0024] 112采集模块
[0025] 113判断模块
[0026] 114第一处理模块
[0027] 121第二处理模块
[0028] 122检测模块
[0029] 123校准模块
[0030] S1 ~S2 步骤
【具体实施方式】
[0031] 以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书 所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实 施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离 本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施 例中的特征可以相互组合。
[0032] 需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构 想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸 绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也 可能更为复杂。
[0033] 本发明的基本技术方案如下：
[0034] 底层基于安装于脚步的微型传感器所采集的数据，利用零速度更新（ZUPT，Zero VelocityUpdate)和零角速度更新（ZARU,ZeroAngularRateUpdate)辅助策略的误差状 态卡尔曼滤波器进行初步的步行者位置推算，然后基于检出的信号特征，判断出每一步的 结束时刻，在此时刻将计算的这一步所运动的位移长度和运动方向信息传输到上一层滤波 器，上一层滤波采用粒子滤波方法，基于地图信息，对本步所计算的步长和方向进行再次滤 波处理，得出最优结果。然后将最优结果与底层滤波数据的结果进行对比，将此对比的差值 反馈回底层误差状态卡尔曼滤波器，用以自适应修正卡尔曼滤波器的计算参数。
[0035] 实施例一
[0036] 本实施例提供一种步行者室内双层定位方法，请参阅图1，显示为步行者室内定位 方法流程示意图。如图1所示，所述步行者室内双层定位方法包括以下步骤：
[0037] S1，下层滤波步骤，所述下层滤波步骤具体包括： [0038] S11，选取所述步行者的一只脚为检测对象，并采集所述检测对象的运动数据。在 本步骤中，首先需要选取固定一只脚作为检测对象，然后基于在该脚上安装惯性传感器采 集运动数据。所述惯性传感器包括三轴加速度计、三轴陀螺仪、磁力计分别采集加速度和角 速度等运动数据。
[0039] S12,根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若 是，继续执行下一步骤，即S13 ;若否，返回步骤S12,继续判断所述检测对象在当前时刻是 否处于静止状态；所述静止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动 数据存在运动数据误差状态。在本实施例中，所述预定精致检测方式可以采用传统的联合 阈值条件判断法，也可以概率统计推算法，如贝叶斯估计，HMM等算法。例如，在本实施例中， 采用贝叶斯估计判断静止状态。
[0040] 所述步骤S12包括以下步骤：
[0041] 第一步，基于所述传感数据计算所述步行者的跨步周期；所述跨步周期包括脚接 触阶段、脚站立阶段，脚离地阶段、及脚摆布阶段。在本实施例中，计算所述步行者的跨步周 期需要基于所述传感模块中三轴陀螺仪y轴数据，根据三轴陀螺仪y轴信号中跨步信号特 征来计算跨步周期。在摆步周期的起始阶段，任何人的陀螺仪y轴数据均存在一个谷值，可 以利用对过零点设置阈值的方法进行谷值探测，但是不同人信号特征可能会引入误判断谷 值，因此针对跨步信号特征，维护一个标志位，当探测到零速度区间是，将此标志位置true， 当在摆步阶段探测到第一个谷值的时候讲此标志位置false，在之后false阶段探测到谷 值均忽略。同时基于人体运动学信息，摆步阶段要持续38%时间，在此时间段内检测到的其 余谷值均忽略。实验证明，此两种机制可以保证100%的跨步周期内的局部谷值计算准确 率。也就是说，在每个跨步周期中均可以找到一个明显的谷值（局部最小值）以这个谷值 为脚尖离地时刻，作为判断一个完整跨步周期的分割点，实现对不同脚步运动阶段的划分 估计。
[0042] 第二步，按照预定规则对应查询所述步行者在第k时刻（当前时刻）应该处于跨 步周期中的哪个阶段，并获取查询结果。在本实施例中，所述预定规则为人体运动学模型， 此模型定义了一个跨步周期为单条腿从脚跟触地，到脚掌触地，到脚跟离地，脚尖离地，摆 步，最后脚跟再次触地。此周期内包含站位阶段和摆步阶段，其中站位阶段包括接触阶段， 站立阶段，离地阶段。人体运动学模型定义了在正常运动是各个阶段在真个跨步周期内所 占据的时间比例。在本步骤中，所述步行者所处第k时刻对应到该人体运动学模型中对应 查询所述步行者在第k时刻（当前时刻）应该处于跨步周期中的哪个阶段，便得到查询结 果，所述查询结果包括所述步行者在第k时刻（当前时刻）处于站位阶段或所述步行者在 第k时刻（当前时刻）处于摆布阶段，假如把一个跨步周期分为100份，A点表示脚跟触 地（0% )，B点表示脚掌触地（16. 8% )，C点表示脚跟离地（41. 5% )，D点表示脚尖离地 (62% )，E点表示脚跟触地（100% )，脚跟触地到脚掌触地为接触阶段，脚掌触地到脚跟离 地为站立阶段，脚跟离地到脚尖离地为离地阶段，脚尖离地到脚跟触地为摆布阶段。因此， 可以从图3中可以看出，站位阶段占62%，摆布阶段占38%。
[0043] 第三步，根据预存的陀螺仪设置阈值判断所述检测对象是否处于静止状态，获取 第一判断结果，即通过三轴陀螺仪获取到的所述步行者第k时刻（当前时刻）的角速度信 息与预存的陀螺仪设置阈值进行比对，若所述步行者第k时刻（当前时刻）的角速度信息 小于预存的陀螺仪设置阈值，则表示所述检测对象处于静止状态，若所述步行者第k时刻 (当前时刻）的角速度信息大于预存的陀螺仪设置阈值，则表示所述检测对象处于运动状 态，返回步骤S3,继续判断；若所述第一判断结果为所述检测对象处于静止状态，继续执行 下一步骤。
[0044] 第四步，判断所述第一判断结果是否与所述查询结果一致（检测对象处于静止状 态对应所述检测对象处于站位阶段，即表示第一判断结果与所述查询结果一致，或检测对 象处于运动状态即表示所述检测对象处于摆布阶段即表示第一判断结果与所述查询结果 一致），若是，则表示不存在潜在误差点；若否，则表示存在潜在误差点，执行第五步。
[0045] 第五步，通过朴素贝叶斯估计所述潜在误差点是否处于零速度区间，若是，则表示 所述检测对象在第k时刻（当前时刻）处于静止状态，继续执行步骤S13,若否，则表示所述 检测对象在第k时刻处于运动状态，则执行步骤S12,继续判断。在本步骤中，采用以下朴素 贝叶斯估计公式来估计，朴素贝叶斯估计公式为：
[0046]zv=argmaxP(s|zv)P(a|zv)P(zv) 公式（1)其中，P(s|zv)表示脚步接 触地面的结束时刻的条件概率，
[0049]其中，〇标识正态分布的方差，〇 2=CXTgait，tQ为摆布的起始时刻，t为第k时 刻（当前时刻），C为一常数，Tgait表示整个跨步周期的时间长度。P(a|zv)表示在已知零 速度概率条件下的加速度的条件概率。
[0051] 其中，an表示在地球坐标系下的加速度。P(zv)表示基于人体运动学的概率统计 得到第k时刻（当前时刻）为零速度的概率，
[0053]S13,利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标 信息，对所述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获 取校准后的移动位置坐标信息。步骤S12是在检测到的零速度阶段，利用误差状态卡尔曼 滤波器，对推算的运动数据误差状态进行估计，然后将估计的误差去修正捷联惯导算法所 推算的移动位置坐标信息。具体包括以下步骤：
[0054] 第六步，通过定位设备获取所述步行者的初始移动位置坐标信息。
[0055] 第七步，利用捷联惯导算法，对所述三轴加速度计和三轴陀螺仪采集到的运动数 据进行计算。本步骤通过公式（6)计算步行者当前时刻，即第k时刻的加速度。
[0057] 其中，< 为所述步行者第k时刻（当前时刻）在地球坐标系下X轴/Y轴/Z轴的 加速度，k表示第k时刻（当前时刻），即，n表示地球坐标系下的X轴/Y轴/Z轴；为 第k时刻（当前时刻）的方向余弦矩阵，通过该方向余弦矩阵将所述传感数据从传感器坐 标系下转换至地球坐标系下，k-1为第k-1时刻（前一时刻）；4为所述步行者第k时刻在 传感器坐标系下第一方向/第二方向/第三方向的加速度，k表示第k时刻（当前时刻），b表示传感器坐标系下第一方向/第二方向/第三方向。同理，计算所述步行者第k时刻的 角速度信息、位置信息、速度信息、及角速度变化信息，同样是通过m为第k时刻（当前 时刻）的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速 度变化信息从传感器坐标系下转换至地球坐标系下。其中，第k时刻（当前时刻）的方向 余弦矩阵^^^,的计算公式为：
[0059] 其中，，为第k-1时刻（前一时刻）的方向余弦矩阵将所述步行者 第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下 转换至地球坐标系下；SQk表示为基于角速度的矩阵用以表示小的角度变换对方 向变换的影响，SQk的表达式为
，1，2,3表示传 感器坐标系下第一方向/第二方向/第三方向。方向余弦矩阵<^^,当k= 1时，
其中，roll表示翻滚角，
;pitch是俯仰角，
，yaw是航向角。 a：-mmmmmrimxmamm,<mormmmimmrimj,cr〇r 指加速度传感器测得的Z轴的数据；g为重力加速度。在有磁力计的情况下，yaw为磁 力计在水平面上的度数，没有磁力计的情况下初始值设置为〇。13"为3维单位矩阵，即
;4为在传感器坐标系下所述步行者第k时刻的角速度信息；At表示采 样时间间隔，即第k时刻（当前时刻）与第k-1时刻（前一时刻）之间相差的时间。
[0060] 第八步，对<进行两次积分以获取所述步行者第k时刻（当前时刻）未校准的移 动位置坐标信息。
[0061] 第九步，在所述零速度阶段利用误差状态卡尔曼滤波对运动数据误差状态进行误 差状态偏置估计。误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、 速度误差估计、及加速度误差估计。
[0062]所述步行者第k时刻的运动数据误差状态可以标记为SXk，该位置信息计算误差 sxk采用如下误差向量表示：
[0064]其中，表示所述步行者第k时刻的姿态误差，表示所述步行者第k时刻的 角速度误差，Srk表示所述步行者第k时刻的位置误差，SVk表示所述步行者第k时刻的 速度误差，表示所述步行者第k时刻的加速度误差，这5个误差向量都是对应3个正交 轴。
[0065] 所述步行者第k时刻的运动数据误差状态SXk的计算公式为
[0066] 5xk=Fk 8Xh+Wh公式（8)
[0067]其中，Fk表示状态转移矩阵，Sxh表示所述步行者第k_l时刻的位置信息计算误 差，Wh表示所述步行者第k时刻的位置信息计算误差的噪声。在本实施例中，由于需要采 用卡尔曼滤波法对误差进行估计，则卡尔曼滤波的量测模型为：
[0068]Zk=H8xk|k+nk 公式（9)
[0069]当脚步处于静止状态的时候，加速度和角速度应该为0,但是实际采集的数据 存在噪声，这个噪声经过积分后得到速度误差，另外一个就是实际测量到的角速度误 差，这两个值作为误差状态的测量值zk，g卩
，H为量测误差，
，为单位矩阵。
[0070] 校准后的所述步行者第k时刻的运动数据误差状态
[0071] 8 xk= 8 x H+Kk ? [Zk_H 8 xk_J 公式（10)
[0072] 其中，Kk为卡尔曼增益，卡尔曼增益Kk=PuHTOlPKHT+Rkr1中Pk为所述步行者第k 时刻为误差状态协方差矩阵，Pk=(I 所述步行者第k-1 时刻为误差状态协方差矩阵，Rk为测量噪声协方差矩阵。在本步骤中，采用公式（10)分别 估计姿态误差，角速度误差、位置误差、速度误差、及加速度误差，具体如 下：
[0078] 第十步，修正第k时刻（当前时刻）运动数据误差状态并获取校准后的移动位置 坐标信息。姿态误差估计为采用姿态误差，即所述步行者第k时刻的姿态误差^>>/,，通过一 姿态误差矩阵S Qk对方向余弦矩阵
进行修 正，其中姿态误差矩阵S Qk为
[0079] 角速度误差估计为采用角速度误差，即所述步行者第k时刻（当前时刻）的角速 度误差对第k+1时刻（下一时刻）的角速度进行补偿；其中，下一时刻的角速度=当前 时刻的角速度+角速度误差；
[0080] 位置误差估计为采用所述步行者第k时刻（当前时刻）的位置误差S rk对当前 时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信 息=当前时刻未校准的移动位置坐标信息一位置误差；
[0081] 速度误差估计为采用速度误差SVk，所述步行者第k时刻的速度误差sVk对当前 时刻的速度进行补偿；其中，所述当前时刻的速度=未校准的所述步行者当前时刻的移动 位置坐标信息中的速度一速度误差；
[0082] 加速度误差估计为采用加速度误差&[，即所述步行者第k时刻的加速度偏置 对下一时刻的加速度进行补偿；其中，下一时刻的加速度=当前时刻的加速度+加速度误 差。
[0083] S2,上层滤波步骤。所述上层滤波步骤具体包括：
[0084] S21，查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时 刻对当前时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动坐标位置信 息和上一时刻校准后的移动位置坐标信息的第一差。请参阅图2,显示为基于三轴加速度与 三轴陀螺仪采集的数据计算的零速度阶段与零速度阶段结束的结束时刻的计算效果示意 图，图2依次展现了三轴加速度数据，三轴陀螺仪数据和基于这2个原始数据计算的零速 度结果，其中第三张图中，1表示为零速度时刻，〇表示非零速度时刻，（零速度即脚步静止 时刻），其中图中的点A即零速度阶段的结束时刻，在这个时刻触发利用粒子滤波算法，结 合地图信息来校准计算的移动位置坐标信息。点A的获取方法如下：
[0085]若（（零速度h= =1)&&(零速度k== 〇))，则时刻k是零速度阶段的结束时刻。 [0086] 对在所述结束时刻对当前时刻校准后的移动位置坐标信息，结合地图信息，利用 粒子滤波进一步对位置进行校准具体如下：
[0087] 基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对 于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信 息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新 的有效粒子仍然失败，那么就将其标记为无效粒子，其中，s表示移动位置坐标信息。粒子 的权值如下：
[0089] 粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设 混入到一步步长〖的噪声^和一步方位变化M的噪声^都服从零均值的高斯分布。以此列 写出水平位置（x，y)和方位步的状态转移方程：
[0091] 在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子酌基础上由 重要性分布Philx〗])采样得到。这可以通过先采样得到粒子4和而/;〗，然后代入状态转移 方程分别得到冲〗。换言之，凡是能够修正位置计算累计误差进行修正的方法均适用于 本发明。
[0092]S22,将修正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第 一差与下层滤波后的当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置 坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺 时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行步骤S23,即逆时针校正；若所 述步行者的当前步伐存在顺时针偏移，执行步骤S24,即顺时针校正。
[0093] 在本步骤中，将修正修正后的移动坐标位置信息和上一时刻校准后的移动位置坐 标信息的第一差，即校准的本步角度!tPF，（PF,ParticleFilter,粒子滤波）与当前时刻校 准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，表现在本实施 例中为当前步所跨角度！tEKF，（EKF，ExtendedKalmanFilter,扩展卡尔曼滤波）反馈回卡 尔曼滤波器中，对所述测量噪声协方差矩阵Rk进行修正。
[0094] 计算当前步所跨的角度步EKF，然后与利用地图配准计算的本步角度步^进行对比 得到：
[0095] 8^=i]) EKF-itPF 公式（13)
[0096] 因此判断R值修正时刻可以采用如下策略：
[0097] 执行顺时针校正：若S$ < 〇时
[0098] 说明存在顺时针航向角估计偏移，这种情况说明在零速度阶段对顺时针方向的补 偿过度，因此采用公式（14)，对R值进行减小得到修正后的々'，以增加估计量的比例，减小测 量量的对估计
[0100] 其中e，为固定的R值减小量。
[0101] 执行逆时针校正：若s〇时
[0102] 说明存在逆时针航向角估计偏移，这种情况说明在零速度阶段对逆时针方向的补 偿过度。同样道理，针对公式（15)，对R值进行增大得到修正后的哀，以减小估计量的比例， 增大测量量的对估计结果的影响。
[0104] 若发现连续多步存在地图修正后的航向角向某个方向偏移的问题，则对EKF(误 差状态卡尔曼滤波器修正）的测量噪声协方差进行连续小幅度修正。经过试验结果分析， R矩阵内非零元素合理的取值范围应该在[0.001，0.01]区间内。
[0105] 本实施例所述的步行者室内双层定位方法基于安装于脚部的加速度计，陀螺仪， 磁力计等微型传感器，来跟踪脚部的运动特征。再底层利用误差状态卡尔曼滤波来估计误 差参数来修正捷联惯导推算的位置信息，然后根据原始数据特征检查出每步的结束时刻， 将底层计算的这一步步长和运动方向信息输入到上层粒子滤波器，结合地图信息，对底层 滤波器估算的位置进行修正。最后将上层粒子滤波器与下层卡尔曼滤波器估算的位置进行 对比，将差值反馈回下层滤波器，实现对卡尔曼滤波器的自适应参数修正，以提高整体框架 算法的计算的位置精度。本发明增加了两个滤波层次之间的耦合度，提高了数据的融合效 果。使基于惯性传感器的室内定位精度更高，实用性更强，并延长了算法有效计算的生命周 期。
[0106] 实施例二
[0107] 本实施例提供一种步彳丁者室内双层定位系统1，请参阅图3,显不为步彳丁者室内双 层定位系统的原理结构示意图。所述步行者室内双层定位系统1包括下层滤波单元11和 与所述下层滤波单元11连接的上层滤波单元12,所述下层滤波模块11包括选取模块111、 采集模块112、判断模块113、及第一处理模块114。所述上层滤波单元12包括第二处理模 块121、检测模块122、及校准模块123。
[0108] 其中，所述下层滤波模块11用于执行下层滤波。
[0109] 所述选取模块111用于选取所述步行者的一只脚为检测对象。
[0110] 与所述选取模块111连接的采集模块112用于采集所述检测对象的运动数据。在 本实施例中，首先需要选取固定一只脚作为检测对象，然后基于在该脚上安装惯性传感器 采集运动数据。所述惯性传感器包括三轴加速度计、三轴陀螺仪、磁力计分别采集加速度和 角速度等运动数据。
[0111]与所述采集模块112连接的所述判断模块113用于根据预定静止检测方式判断 所述检测对象在当前时刻是否处于静止状态；若是，调用所述第一处理模块114 ;若否，继 续调用所述判断模块113,继续判断所述检测对象在当前时刻是否处于静止状态；所述静 止状态分为非零速度阶段和零速度阶段；所述零速度阶段的运动数据存在运动数据误差状 态。在本实施例中，所述预定精致检测方式可以采用传统的联合阈值条件判断法，也可以概 率统计推算法，如贝叶斯估计，HMM等算法。例如，在本实施例中，采用贝叶斯估计判断静止 状态。
[0112] 所述判断模块具体执行功能包括：
[0113] 基于所述传感数据计算所述步行者的跨步周期；所述跨步周期包括脚接触阶段、 脚站立阶段，脚离地阶段、及脚摆布阶段。在本实施例中，计算所述步行者的跨步周期需要 基于所述传感模块中三轴陀螺仪y轴数据，根据三轴陀螺仪y轴信号中跨步信号特征来计 算跨步周期。在摆步周期的起始阶段，任何人的陀螺仪y轴数据均存在一个谷值，可以利用 对过零点设置阈值的方法进行谷值探测，但是不同人信号特征可能会引入误判断谷值，因 此针对跨步信号特征，维护一个标志位，当探测到零速度区间是，将此标志位置true，当在 摆步阶段探测到第一个谷值的时候讲此标志位置false，在之后false阶段探测到谷值均 忽略。同时基于人体运动学信息，摆步阶段要持续38%时间，在此时间段内检测到的其余 谷值均忽略。实验证明，此两种机制可以保证100%的跨步周期内的局部谷值计算准确率。 也就是说，在每个跨步周期中均可以找到一个明显的谷值（局部最小值）以这个谷值为脚 尖离地时刻，作为判断一个完整跨步周期的分割点，实现对不同脚步运动阶段的划分估计。
[0114] 按照预定规则对应查询所述步行者在第k时刻（当前时刻）应该处于跨步周期中 的哪个阶段，并获取查询结果。在本实施例中，所述预定规则为人体运动学模型，此模型定 义了一个跨步周期为单条腿从脚跟触地，到脚掌触地，到脚跟离地，脚尖离地，摆步，最后脚 跟再次触地。此周期内包含站位阶段和摆步阶段，其中站位阶段包括接触阶段，站立阶段， 离地阶段。人体运动学模型定义了在正常运动是各个阶段在真个跨步周期内所占据的时间 比例。在本步骤中，所述步行者所处第k时刻对应到该人体运动学模型中对应查询所述步 行者在第k时刻（当前时刻）应该处于跨步周期中的哪个阶段，便得到查询结果，所述查询 结果包括所述步行者在第k时刻（当前时刻）处于站位阶段或所述步行者在第k时刻（当 前时刻）处于摆布阶段，假如把一个跨步周期分为100份，A点表示脚跟触地（0% )，B点表 示脚掌触地（16. 8% )，C点表示脚跟离地（41. 5% )，D点 表示脚尖离地（62% )，E点表示 脚跟触地（100% )，脚跟触地到脚掌触地为接触阶段，脚掌触地到脚跟离地为站立阶段，脚 跟离地到脚尖离地为离地阶段，脚尖离地到脚跟触地为摆布阶段。因此，可以从图3中可以 看出，站位阶段占62%，摆布阶段占38%。
[0115] 根据预存的陀螺仪设置阈值判断所述检测对象是否处于静止状态，获取第一判断 结果，即通过三轴陀螺仪获取到的所述步行者第k时刻（当前时刻）的角速度信息与预存 的陀螺仪设置阈值进行比对，若所述步行者第k时刻（当前时刻）的角速度信息小于预存 的陀螺仪设置阈值，则表示所述检测对象处于静止状态，若所述步行者第k时刻（当前时 亥IJ)的角速度信息大于预存的陀螺仪设置阈值，则表示所述检测对象处于运动状态，返回 步骤S3,继续判断；若所述第一判断结果为所述检测对象处于静止状态，继续执行下一步 骤。
[0116] 判断所述第一判断结果是否与所述查询结果一致（检测对象处于静止状态对应 所述检测对象处于站位阶段，即表示第一判断结果与所述查询结果一致，或检测对象处于 运动状态即表示所述检测对象处于摆布阶段即表示第一判断结果与所述查询结果一致）， 若是，则表示不存在潜在误差点；若否，则表示存在潜在误差点，执行第五步。
[0117] 通过预定静止检测方式，即朴素贝叶斯估计所述潜在误差点是否处于零速度区 间，若是，则表示所述检测对象在第k时刻（当前时刻）处于静止状态，调用所述第一处理 模块113,若否，则表示所述检测对象在第k时刻处于运动状态，则继续调用所述判断模块 112,继续判断。在本实施例中，采用以下朴素贝叶斯估计公式来估计，朴素贝叶斯估计公式 为：
[0118] zv = argmaxP (s | zv) P (a | zv) P (zv)
[0119] 其中，P(s | zv)表示脚步接触地面的结束时刻的条件概率，
[0122] 其中，〇标识正态分布的方差，〇 2= CXTgait，h为摆布的起始时刻，t为第k时 刻（当前时刻），C为一常数，Tgait表示整个跨步周期的时间长度。P(a|zv)表示在已知零 速度概率条件下的加速度的条件概率。
[0124] 其中，an表示在地球坐标系下的加速度。P(zv)表示基于人体运动学的概率统计 得到第k时刻（当前时刻）为零速度的概率，
[0126] 与所述判断模块113连接的第一处理模块114用于利用预存位置坐标信息推算法 推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态 偏置估计以修正所述运动数据误差状态并获取校准后的移动位置坐标信息。步骤S12是在 检测到的零速度阶段，利用误差状态卡尔曼滤波器，对推算的运动数据误差状态进行估计， 然后将估计的误差去修正捷联惯导算法所推算的移动位置坐标信息。所述第一处理模块 114具体包括以下功能：
[0127] 通过定位设备获取所述步行者的初始移动位置坐标信息。
[0128] 利用捷联惯导算法，对所述三轴加速度计和三轴陀螺仪采集到的运动数据进行计 算。本步骤通过公式（6)计算步行者当前时刻，即第k时刻的加速度。
[0130] 其中，<为所述步行者第k时刻（当前时刻）在地球坐标系下X轴/Y轴/Z轴的 加速度，k表示第k时刻（当前时刻），即，n表示地球坐标系下的X轴/Y轴/Z轴；,为 第k时刻（当前时刻）的方向余弦矩阵，通过该方向余弦矩阵将所述传感数据从传感器坐 标系下转换至地球坐标系下，k-1为第k-1时刻（前一时刻）； < 为所述步行者第k时刻在 传感器坐标系下第一方向/第二方向/第三方向的加速度，k表示第k时刻（当前时刻），b表示传感器坐标系下第一方向/第二方向/第三方向。同理，计算所述步行者第k时刻的 角速度信息、位置信息、速度信息、及角速度变化信息，同样是通过，为第k时刻（当前 时刻）的方向余弦矩阵将所述步行者第k时刻的角速度信息、位置信息、速度信息、及角速 度变化信息从传感器坐标系下转换至地球坐标系下。其中，第k时刻（当前时刻）的方向 余弦矩阵^^^,的计算公式为：
[0132] 其中，i为第k-1时刻（前一时刻）的方向余弦矩阵将所述步行者 第k时刻的角速度信息、位置信息、速度信息、及角速度变化信息从传感器坐标系下 转换至地球坐标系下；SQk表示为基于角速度的矩阵用以表示小的角度变换对方 向变换的影响，SQk的表达式为
1，2,3表示传 感器坐标系下第一方向/第二方向/第三方向。方向余弦矩阵6^ ，当k= 1时，
其中，roll表示翻滚角，
;pitch是俯仰角，
，yaw是航向角。 '指加速度传感器测得的x轴的数据，"r"指加速度传感器测得的y轴的数据，<_' 指加速度传感器测得的z轴的数据；g为重力加速度。在有磁力计的情况下，yaw为磁 力计在水平面上的度数，没有磁力计的情况下初始值设置为〇。13"为3维单位矩阵，即
;%6为在传感器坐标系下所述步行者第k时刻的角速度信息；At表示采 样时间间隔，即第k时刻（当前时刻）与第k-1时刻（前一时刻）之间相差的时间。
[0133] 对 <进行两次积分以获取所述步行者第k时刻（当前时刻）未校准的移动位置坐 标f目息。
[0134] 在所述零速度阶段利用误差状态卡尔曼滤波对运动数据误差状态进行误差状态 偏置估计。误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误 差估计、及加速度误差估计。
[0135] 所述步行者第k时刻的运动数据误差状态可以标记为SXk，该位置信息计算误差 Sxk采用如下误差向量表示：
[0137] 其中，表示所述步行者第k时刻的姿态误差，<5^表示所述步行者第k时刻的 角速度误差，Srk表示所述步行者第k时刻的位置误差，SVk表示所述步行者第k时刻的 速度误差，&〖表示所述步行者第k时刻的加速度误差，这5个误差向量都是对应3个正交 轴。
[0138] 所述步行者第k时刻的运动数据误差状态SXk的计算公式为
[0139] 8 xk= F k 8 Xh+Wh
[0140] 其中，Fk表示状态转移矩阵，Sxh表示所述步行者第k_l时刻的位置信息计算误 差，Wh表示所述步行者第k时刻的位置信息计算误差的噪声。在本实施例中，由于需要采 用卡尔曼滤波法对误差进行估计，则卡尔曼滤波的量测模型为
[0141] Zk=H8Xk|k+nk
[0142] 当脚步处于静止状态的时候，加速度和角速度应该为0,但是实际采集的数据 存在噪声，这个噪声经过积分后得到速度误差，另外一个就是实际测量到的角速度误 差，这两个值作为误差状态的测量值Zk，S卩
，H为量测误差，
1I3X3为单位矩阵。
[0143] 校准后的所述步行者第k时刻的运动数据误差状态
[0144] 8 xk= 8 xH+Kk? [Zk_H 8 xk_J
[0145] 其中，Kk为卡尔曼增益，卡尔曼增益Kk=PuHTOlPKHT+Rkr1中Pk为所述步行者第k 时刻为误差状态协方差矩阵，Pk= (I 所述步行者第k-1 时刻为误差状态协方差矩阵，Rk为测量噪声协方差矩阵。在本步骤中，采用以下公式分别估 计姿态误差，角速度误差、位置误差、速度误差、及加速度误差，具体如下：
[0151] 修正第k时刻（当前时刻）运动数据误差状态并获取校准后的移动位置坐标信 息。姿态误差估计为采用姿态误差，即所述步行者第k时刻的姿态误差5奶，通过一姿态误 差矩阵SQk对方向余弦矩阵
?进行修正，其 中姿态误差矩阵况\为
[0152] 角速度误差估计为采用角速度误差，即所述步行者第k时刻（当前时刻）的角速 度误差对第k+1时刻（下一时刻）的角速度进行补偿；其中，下一时刻的角速度=当前 时刻的角速度+角速度误差；
[0153] 位置误差估计为采用所述步行者第k时刻（当前时刻）的位置误差S rk对当前 时刻未校准的移动位置坐标信息进行补偿；其中，所述当前时刻校准后的移动位置坐标信 息=当前时刻未校准的移动位置坐标信息一位置误差；
[0154] 速度误差估计为采用速度误差S Vk，所述步行者第k时刻的速度误差S Vk对当前 时刻的速度进行补偿；其中，所述当前时刻的速度=未校准的所述步行者当前时刻的移动 位置坐标信息中的速度一速度误差；
[0155] 加速度误差估计为采用加速度误差&(，即所述步行者第k时刻的加速度偏置 对下一时刻的加速度进行补偿；其中，下一时刻的加速度=当前时刻的加速度+加速度误 差。
[0156] 所述上层滤波单元12用于执行上层滤波。所述上层滤波单元12包括：
[0157] 与所述判断模块113和第一处理模块114连接的第二处理模块121用于查找当前 时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的 移动位置坐标信息执行预制修正，并获取修正后的移动坐标位置信息和上一时刻校准后的 移动位置坐标信息的第一差。请参阅图2,显示为基于三轴加速度与三轴陀螺仪采集的数 据计算的零速度阶段与零速度阶段结束的结束时刻的计算效果示意图，图2依次展现了三 轴加速度数据，三轴陀螺仪数据和基于这2个原始数据计算的零速度结果，其中第三张图 中，1表示为零速度时刻，0表示非零速度时刻，（零速度即脚步静止时刻），其中图中的点 A即零速度阶段的结束时刻，在这个时刻触发利用粒子滤波算法，结合地图信息来校准计算 的移动位置坐标信息。点A的获取方法如下：
[0158] 若（（零速度1)&&(零速度k== 0))，则时刻k是零速度阶段的结束时刻。
[0159] 对在所述结束时刻对当前时刻校准后的移动位置坐标信息，结合地图信息，进行 粒子滤波方法进一步进行位置校准具体如下：
[0160] 基于所述目标建筑物的轮廓利用粒子滤波算法对位置计算累计误差进行修正。对 于室内定位场景，建筑平面信息的利用能够限定行走轨迹，减少其不确定性。在给出地图信 息后，相邻两步对应粒子之间的连线不能穿越墙壁或者其他障碍物。如果几次尝试生成新 的有效粒子仍然失败，那么就将 其标记为无效粒子，其中，s表示移动位置坐标信息。粒子 的权值如下：
[0161]
[0162] 粒子滤波的量测是卡尔曼滤波的估计结果，粒子滤波采用一种简化模型，即假设 混入到一步步长〖啲噪声^和一步方位变化ST的噪声^都服从零均值的高斯分布。以此列 写出水平位置（x，y)和方位步的状态转移方程：
[0164] 在粒子滤波的算法中，粒子的传递更新需要在上一时刻状态粒子的基础上由 重要性分布
采样得到。这可以通过先采样得到粒子以和5也，然后代入状态转移 方程分别得到
> 换言之，凡是能够修正位置计算累计误差进行修正的方法均适用于 本发明。
[0165] 与所述第一处理模块114和第二处理模块121连接的所述检测模块122用于将修 正后的移动坐标位置信息和上一时刻校准后的移动位置坐标信息的第一差与当前时刻校 准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差进行比较，以检 测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述步行者的当前步 伐存在逆时针偏移，调用用于执行逆时针校正的校准模块123 ;若所述步行者的当前步伐 存在顺时针偏移，调用所述校准模块123执行顺时针校正。
[0166] 在本步骤中，将粒子滤波后的修正后的移动坐标位置信息和上一时刻校准后的移 动位置坐标信息的第一差，即校准的本步角度itPF，与下层滤波后的当前时刻校准后的移动 位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，表现在本实施例中为当前 步所跨角度步EKF，反馈回卡尔曼滤波器中，对所述测量噪声协方差矩阵Rk进行修正。
[0167] 计算当前步所跨的角度！tEKF，然后与利用地图配准计算的本步角度！^^进行对比 得到：
[01 68] 5 * = ^ EKF- ^ PF
[0169] 所述校准模块123具体执行以下功能：
[0170] 顺时针校正：若〇时，说明存在顺时针航向角估计偏移，这种情况说明在零 速度阶段对顺时针方向的补偿过度，因此采用公式（14)，对R值进行减小得到修正后的及， 以增加估计量的比例，减小测量量的对估计
[0172] 其中e，为固定的R值减小量。
[0173] 逆时针校正：若S 〇时，说明存在逆时针航向角估计偏移，这种情况说明在零 速度阶段对逆时针方向的补偿过度。同样道理，针对公式（15)，对R值进行增大得到修正后 的哀，以减小估计量的比例，增大测量量的对估计结果的影响。
[0175] 若发现连续多步存在地图修正后的航向角向某个方向偏移的问题，则对EKF(误 差状态卡尔曼滤波器修正）的测量噪声协方差进行连续小幅度修正。经过试验结果分析，R矩阵内非零元素合理的取值范围应该在[0.001，0.01]区间内。
[0176] 综上所述，本发明所述的步行者室内双层定位方法及系统基于安装于脚部的加速 度计，陀螺仪，磁力计等微型传感器，来跟踪脚部的运动特征。再底层利用误差状态卡尔曼 滤波来估计误差参数来修正捷联惯导推算的位置信息，然后根据原始数据特征检查出每步 的结束时刻，将底层计算的这一步步长和运动方向信息输入到上层粒子滤波器，结合地图 信息，对底层滤波器估算的位置进行修正。最后将上层粒子滤波器与下层卡尔曼滤波器估 算的位置进行对比，将差值反馈回下层滤波器，实现对卡尔曼滤波器的自适应参数修正，以 提高整体框架算法的计算的位置精度。本发明增加了两个滤波层次之间的耦合度，提高了 数据的融合效果。使基于惯性传感器的室内定位精度更高，实用性更强，并延长了算法有效 计算的生命周期。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。
[0177] 上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟 悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因 此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完 成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。
【主权项】
1. 一种步行者室内双层定位方法，其特征在于，包括： 下层滤波步骤；所述下层滤波步骤包括： 选取所述步行者的一只脚为检测对象，并采集所述检测对象的运动数据； 根据预定静止检测方式判断所述检测对象在当前时刻是否处于静止状态；若是，继续 执行下一步骤；若否，继续判断；所述静止状态分为非零速度阶段和零速度阶段；所述零速 度阶段的运动数据存在运动数据误差状态； 利用预存位置坐标信息推算法推算所述步行者在当前时刻的移动位置坐标信息，对所 述运动数据误差状态进行误差状态偏置估计以修正所述运动数据误差状态并获取当前时 刻校准后的移动位置坐标信息； 上层滤波步骤；所述上层滤波步骤包括： 查找当前时刻中所述零速度阶段的结束时刻，利用地图信息，在所述结束时刻对当前 时刻校准后的移动位置坐标信息执行预制修正，并获取修正后的移动位置坐标信息和上一 时刻校准后的移动位置坐标信息的第一差； 将所述第一差与下层滤波后当前时刻校准后的移动位置坐标信息和上一时刻校准后 的移动位置坐标信息的第二差进行比较，以检测所述步行者的当前步伐是存在逆时针偏移 还是存在顺时针偏移，若所述步行者的当前步伐存在逆时针偏移，执行逆时针校正；若所述 步行者的当前步伐存在顺时针偏移，执行顺时针校正。2. 根据权利要求1所述的步行者室内双层定位方法，其特征在于：所述下层滤波步骤 中的所述预存位置坐标信息推算法为捷联惯导算法；所述步骤三采用所述捷联惯导算法计 算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未校准的移动位 置坐标信息。3. 根据权利要求2所述的步行者室内双层定位方法，其特征在于： 所述下层滤波步骤中采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状 态偏置估计；误差状态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度 误差估计、及加速度误差估计。4. 根据权利要求3所述的步行者室内双层定位方法，其特征在于： 姿态误差估计为通过一姿态误差矩阵对方向余弦矩阵进行修正； 角速度误差估计为采用角速度误差对下一时刻的角速度进行补偿；其中，下一时刻的 角速度=当前时刻的角速度+角速度误差； 位置误差估计为采用位置误差对当前时刻未校准的移动位置坐标信息进行补偿；其 中，所述当前时刻校准后的移动位置坐标信息=当前时刻未校准的移动位置坐标信息一位 a'?口 祆差； 速度误差估计为采用速度误差对当前时刻的速度进行补偿；其中，所述当前时刻的速 度=未校准的所述步行者当前时刻的移动位置坐标信息中的速度一速度误差； 加速度误差估计为采用加速度误差对下一时刻的加速度进行补偿；其中，下一时刻的 加速度=当前时刻的加速度+加速度误差。5.根据权利要求1所述的步行者室内双层定位方法，其特征在于：在所述上层滤波步 骤中执行的预制修正为执行粒子滤波算法以完成位置校准。6. -种步行者室内双层定位系统，其特征在于，包括： 下层滤波单元，所述下层滤波单元包括： 选取模块，用于选取所述步行者的一只脚为检测对象； 采集模块，与所述选取模块连接，用于采集所述检测对象的运动数据； 判断模块，与所述采集模块连接，用于根据预定静止检测方式判断所述检测对象在当 前时刻是否处于静止状态，其中，所述静止状态分为非零速度阶段和零速度阶段；所述零速 度阶段的运动数据存在运动数据误差状态；若是，调用用于利用预存位置坐标信息推算法 推算所述步行者在当前时刻的移动位置坐标信息，对所述运动数据误差状态进行误差状态 偏置估计以修正所述运动数据误差状态并获取当前时刻校准后的移动位置坐标信息的第 一处理模块；若否，继续调用所述判断模块； 与所述下层滤波单元连接的上层滤波单元，所述上层滤波单元包括： 第二处理模块，与所述判断模块和第一处理模块连接，用于查找当前时刻中所述零速 度阶段的结束时刻，利用地图信息，在所述结束时刻对当前时刻校准后的移动位置坐标信 息执行预制修正，并获取修正后的移动位置坐标信息和上一时刻校准后的移动位置坐标信 息的第一差； 检测模块，与所述第一处理模块和第二处理模块连接，用于将所述第一差与下层滤波 后当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差 进行比较，以检测所述步行者的当前步伐是存在逆时针偏移还是存在顺时针偏移，若所述 步行者的当前步伐存在逆时针偏移，调用用于执行逆时针校正的校准模块；若所述步行者 的当前步伐存在顺时针偏移，继续调用所述校准模块执行顺时针校正。7. 根据权利要求6所述的步行者室内双层定位系统，其特征在于：所述第一处理模块 中的所述预存位置坐标信息推算法为捷联惯导算法；所述第一处理模块还用于采用所述捷 联惯导算法计算所述步行者在当前时刻的加速度和角速度以推算所述步行者当前时刻未 校准的移动位置坐标信息。8. 根据权利要求7所述的步行者室内双层定位系统，其特征在于：所述第一处理模块 还用于采用误差状态卡尔曼滤波对所述运动数据误差状态进行误差状态偏置估计；误差状 态偏置估计包括姿态误差估计、角速度误差估计、位置误差估计、速度误差估计、及加速度 误差估计。
【专利摘要】本发明提供一种步行者室内双层定位方法，包括：下层滤波，包括：根据预定静止检测方式判断检测对象在当前时刻是否处于静止状态；若是，推算当前时刻的移动位置坐标信息，进行误差状态偏置估计，并获取当前时刻校准后的移动位置坐标信息；若否，继续判断；上层滤波，包括：查找当前时刻中零速度阶段的结束时刻，在结束时刻对当前时刻校准后的移动位置坐标信息执行预制修正，获取第一差；比较第一差与当前时刻校准后的移动位置坐标信息和上一时刻校准后的移动位置坐标信息的第二差，检测当前步伐是逆时针偏移还是顺时针偏移，若步行者的当前步伐存在逆时针偏移或顺时针偏移，执行逆时针或顺时针校正。本发明使基于惯性传感器的室内定位精度更高。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN104897158
【申请号】CN201510359813
【发明人】徐正蓺, 杨卫军, 黄超, 魏建明
【申请人】中国科学院上海高等研究院
【公开日】2015年9月9日
【申请日】2015年6月26日
最新回复(0)