其标记为无效粒子，其中，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日