全姿态环境下自主室内定位算法
【专利摘要】本专利公开了一种实现全姿态环境下自主室内定位算法，首先，利用便携式手持终端里集成的低成本运动姿态测量模块中的MEMS惯性传感器实现手持姿态下自主室内定位功能；然后，在手持姿态的基础上，利用姿态切换检测算法完成对不同姿态之间的切换的精确跟踪，最后，在手持姿态定位算法和姿态切换检测的基础上，并结合不同姿态切换下的旋转矩阵实现了基于运动姿态测量模块中的MEMS惯性传感器在不同姿态下的自主室内定位功能。该算法的主要有以下四部分组成：对姿态切换过程精确的跟踪、姿态切换后对姿态精确分类、姿态切换过程中姿态角精度优化及各种姿态下自主定位算法实现与更新准则。便携式手持终端里集成的低成本运动姿态模块作为本算法实现的硬件平台。
【专利说明】全姿态环境下自主室内定位算法
【技术领域】
[0001] 本发明涉及MEMS惯性传感器导航领域，特别涉及姿态自由变换在保持自主惯性 导航精度领域中的应用。
【背景技术】
[0002] 行人在利用便携式手持终端进行室内定位时，长时间保持在手持姿态的可能性不 大。在定位阶段，可能会将设备放在耳边，也可能将手机放在腰间的口袋里；将上述便携式 手持终端用于战场士兵在不固定室内环境下的定位，如果定位方式只限制为手持，那么对 于战场上的士兵来说，不可能长时间保持一种姿态不变。考虑上述二种情况，单一的姿态定 位技术越来越不能满足定位的需求，在这种情况下，基于全姿态环境下的自主室内定位技 术就显得越来越重要了。
[0003] 目前对于不同姿态下能实现室内定位的技术且应用较为广泛的有蓝牙 (Bluetooth)、WLAN、红外线（Location Based Service, LBS)、超宽带（Ultra-Wideband, UWB)、射频识别（Radio Frequency Identification, RFID)等。这些定位技术由于需要事 先在定位区域进行布点、采集参考点信号强度并建立离线信号强度库来进行对目标进行定 位。首先，由于无线信号在室内环境下容易受墙壁、地板、空气中固态悬浮物等各种干扰，从 而严重影响最终的定位精度；其次，这些定位方式不能实现自主条件下的室内定位。
[0004] 基于MEMS惯性传感器的全姿态环境下进行自主室内定位是目前研究的热点。首 先，这种室内定位方式不需要在定位区域进行布点，极大减小了成本；其实，它能完全自主 的进行定位，不依靠任何外界条件。这些优势确定了基于MEMS惯性传感器的全姿态下自主 室内定位将会在未来定位领域占有很大的市场。
[0005] 利用MEMS惯性传感器则能实现在室内环境下的自主定位。目前主要是利用数据 融合算法对MEMS陀螺仪、加速度计和磁强计三个惯性传感器的数据进行融合，从而实现自 主室内定位。其主要的数据融合算法有卡尔曼滤波（KF)、扩展卡尔曼滤波（EKF)、无迹卡尔 曼滤波（UKF)以及粒子滤波。但是上述描述的定位技术只在MEMS惯性传感器处于相对稳 定的姿态下才能对目标进行定位，这极大的限制了 MEMS惯性导航的应用领域。
【发明内容】
[0006] 有鉴于此，本发明所要解决的技术问题是提供一种全姿态环境下自主室内定位算 法；克服传统MEMS惯性传感器自主室内定位方式由于姿态变换导致定位算法瘫痪等缺陷， 拓展了 MEMS惯性传感器自主室内定位应用领域。
[0007] 本发明的目的之一是提出一种各种不同姿态下自主室内定位算法；本发明的目的 之二是提出一种各种不同姿态切换时，自主室内定位算法的实时更新。
[0008] 本发明的目的之一是通过以下技术方案来实现的： 本发明提供的各种不同姿态下自主室内定位算法的硬件平台是便携式手持终端里集 成的MEMS惯性测量单元。姿态切换完成后，利用加速度计和磁强计来重新解算切换后姿态 的初始姿态角，并将其转化成四元数，为后期滤波提供基础；同时由于姿态切换前后，姿态 角存在一个差值，在姿态切换稳定后，将差值进行消除。
[0009] 本发明的目的之二是通过以下技术方案来实现的： 本发明提供的各种不同姿态切换时，自主室内定位算法的实时更新的条件，包括三轴 加速度计数据模值、三轴加速度计单轴数据正负以及横滚角正负变化情况； 所述三轴加速度计数据模值以及三轴加速度计单轴数据正负，用于判断姿态切换类 型； 所述姿态切换类型包括 所述横滚角正负用于判断姿态切换的方向； 所述姿态切换方向包括姿态向左切换和姿态向右切换； 本发明的优点在于：用户在定位阶段不受姿态的严格限制，在姿态种类的容许范围内， 用户可以根据需求更换定位的姿态，对基于MEMS惯性传感器实现的自主室内定位起到极 大的推动作用。
【专利附图】
【附图说明】
[0010] 为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进 一步的详细描述，其中： 图1为本发明硬件框图； 图2为手持姿态下定位算法框图； 图3为姿态切换跟踪算法框图； 图4为姿态切换后对姿态分类的算法框图； 图5为姿态切换过程中姿态角处理算法框图； 图6为姿态切换前后固定姿态角差算法框图； 图7为全姿态算法流程框图。
【具体实施方式】
[0011] 以下将结合附图，对本发明的优选实施例进行详细的描述；应当理解，优选实施例 仅为了说明本发明，而不是为了限制本发明的保护范围。
[0012] 实施例1 图1为本发明硬件框图；图2为手持姿态下定位算法框图；图3为姿态切换跟踪算法 框图；图4为姿态切换后对姿态分类的算法框图，图5为姿态切换过程中姿态角处理算法框 图，图6为姿态切换前后固定姿态角差算法框图，图7为全姿态算法流程框图； 所述硬件框图中主要含有三轴MEMS陀螺仪、三轴MEMS加速度计和三轴MEMS磁强计； 所述手持姿态下定位算法下的自主室内定位，即在定位的全过程中定位姿态维持在手 持姿态下； 所述姿态切换跟踪为利用加速度计数据和横滚角信息实时检测姿态是否在进行切 换； 所述姿态切换后对姿态分类为利用加速度计三轴数据和横滚角正负变化情况来对姿 态切换后的姿态进行精确的分类，其切换的姿态可以分为向左切换姿态和向右切换姿态二 大类； 所述加速度三轴数据正负组合变化，包括X轴、Y轴和Z轴正负组合方式； 所述向左切换姿态为：手持右一耳边左、手持右一腰间左、腰间右一耳边左、腰间右一 手持左、耳边右--手持左以及耳边右一腰间左； 所述向右切换姿态为：手持左一耳边右、手持左一腰间右、腰间左一耳边右、腰间左一 手持右、耳边左一手持右及耳边左一腰间右； 所述姿态切换过程中姿态角处理为利用姿态切换前可靠的姿态角信息代替姿态切换 过程中的姿态角，其技术解决方案如下： 在无磁干扰的环境下，利用姿态切换前0. 2s的传感器数据解算出的所有姿态角的平 均值来代替姿态切换过程中姿态角；如果这〇. 2s内有磁干扰，那么就在这0. 2s的基础上向 前取0. 2s传感器数据，依次类推，一直找到没有干扰的0. 2s的传感器数据。
[0013] 所述附图2中的初始姿态由磁强计和加速度计解算得来，再将初始姿态转成初始 四元数，后期采用利用前一时刻四元数、陀螺仪、加速度计和磁强计经EKF (扩展卡尔曼滤 波)滤波得出当前时刻四元数，然后利用当前时刻的四元数更新姿态角信息。但是姿态在切 换过程中由于陀螺仪和加速度计的数据都是不可信的，故在姿态切换过程中EKF滤波得出 的当前四元数是不可靠的，从而导致姿态角也是不可靠的。如果在姿态切换后，姿态角的更 新依旧利用前一刻四元数来更新当前时刻四元数，必然会导致姿态切换后，姿态角的紊乱。 因为在姿态切换后，当前时刻四元数依旧利用前一时刻四元数来更新，最终导致轨迹的错 误。
[0014] 所述上述姿态切换后，四元数的更新技术方案如下： 姿态切换稳定后，首先进行坐标变换，然后利用姿态切换稳定后的第一点加速度计和 磁强计数据来重新计算姿态角，再将姿态角转换成四元数，后期姿态角更新同上所述。
[0015] 所述姿态切换前后固定姿态角差为不同姿态情况下，便携式手持终端所处的位置 不一样，这样就会导致姿态切换后的轨迹与姿态切换的轨迹存在一个固定姿态角差； 所述固定姿态角差解决技术方案如下： 用姿态切换后稳定第一点的姿态角角减去姿态切换前稳定最后一点姿态角，得到固定 姿态角差。姿态切换后，更新的姿态角都要减去这个固定姿态角差。
[0016] 在姿态切换过程中根据前一时刻四元数和陀螺仪、加速度计以及磁强计解算出的 姿态角信息是完全紊乱的。
【权利要求】
1. 全姿态环境下自主室内定位算法，其特征在于：包括行人在定位过程中便携式手持 终端可能出现的姿态判别算法、姿态切换过程跟踪算法、姿态切换后的姿态分类算法、姿态 切换过程中姿态角精度优化算法和姿态切换后的定位算法更新准则等； 所述全姿态指，便携式设备在用户手中可能出现的各种姿态以及各种姿态之间的切 换； 所述行人在定位过程中便携式手持终端可能出现的姿态判别算法包括手持姿态算法、 耳边姿态算法、腰间姿态算法； 所述手持姿态指，将便携式手持终端水平放在右手或左手上； 所述打电话姿态指，将便携式手持终端放在右耳或左耳旁； 所述腰间姿态指，将便携式手持终端放在右腰间兜或左腰间兜里； 所述姿态切换过程跟踪算法包括对以下姿态切换方式的跟踪： 手持(左）一耳边(左)、手持(左）一耳边(右)、手持(左）一腰间（左)、手持(左）一腰间 (右)、手持(右）一耳边(左)、手持(右）一耳边(右)、手持(右）一腰间(左)、手持(右）一腰间 (右)、耳边(左）一手持(左)、耳边(左）一手持(右)、耳边(左）一腰间（左)、耳边(左一腰间 (右)、耳边(右）一手持(左)、耳边(右）一手持(右)、耳边(右）一腰间（左)、耳边(右）一腰间 (右)、腰间(左）一手持(左)、腰间(左）一手持(右)、腰间(左）一耳边(左)、腰间(左）一耳边 (右)、腰间（右）一手持(左)、腰间（右）一手持(右)、腰间（右）一耳边(左)、腰间（右）一耳边 (右）； 所述姿态切换后的姿态分类为手持(左)、手持(右)、耳边(左)、耳边(右)、腰间(左)和腰 间(右）6种姿态切换方式； 所述姿态切换后的定位算法更新准则指确定了姿态切换方式后，更新到对应姿态切换 方式下定位算法。
2. 根据权利要求1所述的全姿态环境下自主室内定位算法，其特征在于：所述各种姿 态下定位算法的思想指不同姿态切换下坐标变换的确定，即旋转矩阵的确定。
3. 所述坐标变换指，不同姿态下MEMS惯性传感器数据在载体坐标系中是不同的，这时 需要重新将传感器的数据变换到姿态切换后的载体坐标系下。
4. 所述旋转矩阵指，不同姿态对应的载体坐标系之间转化的度量。
5. 根据权利要求1所述的全姿态环境下自主室内定位算法，其特征在于：所述姿态切 换过程跟踪精确度取决于切换条件的选取，所选切换条件要能将姿态切换与同一姿态下正 常转弯进行区别。
6. 根据权利要求1所述的全姿态环境下自主室内定位算法，其特征在于：所述姿态切 换后对姿态分类精确度取决于切换条件的选取，所选切换条件要能将姿态切换与同一姿态 下正常转弯进行区别。
7. 根据权利要求1所述的全姿态环境下自主室内定位算法，其特征在于：所述姿态切 换后定位算法更新是指首先确定姿态切换的方式，再将对应切换方式下定位算法切换为当 前定位算法。
8. 根据权利要求1所述的全姿态环境下自主室内定位算法，其特征在于：所述姿态切 换过程中姿态角精度优化是指，在姿态切换过程中由于陀螺仪和、速度计和磁强计的数据 完全不可信，本发明采用姿态切换前姿态角信息对姿态切换过程中的姿态角进行优化处
【文档编号】G01C21/16GK104154913SQ201410297278
【公开日】2014年11月19日 申请日期:2014年6月28日 优先权日:2014年6月28日
【发明者】黎蕾蕾, 王仕亮, 刘宇, 曹阳, 黄河明, 陈燕苹, 史莉莉, 江宏毅, 王伊冰, 王瑞杰 申请人:南京诺导电子科技有限公司