CN114383605B - 基于mems传感器和稀疏地标点的室内定位及优化方法 - Google Patents

基于mems传感器和稀疏地标点的室内定位及优化方法 Download PDF

Info

Publication number
CN114383605B
CN114383605B CN202111470294.0A CN202111470294A CN114383605B CN 114383605 B CN114383605 B CN 114383605B CN 202111470294 A CN202111470294 A CN 202111470294A CN 114383605 B CN114383605 B CN 114383605B
Authority
CN
China
Prior art keywords
target object
fusion
speed
information
observed quantity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111470294.0A
Other languages
English (en)
Other versions
CN114383605A (zh
Inventor
史文中
余跃
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Polyu Base Shenzhen Ltd
Original Assignee
Polyu Base Shenzhen Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Polyu Base Shenzhen Ltd filed Critical Polyu Base Shenzhen Ltd
Priority to CN202111470294.0A priority Critical patent/CN114383605B/zh
Publication of CN114383605A publication Critical patent/CN114383605A/zh
Application granted granted Critical
Publication of CN114383605B publication Critical patent/CN114383605B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Abstract

本发明公开了基于MEMS传感器和稀疏地标点的室内定位及优化方法，所述方法包括：基于惯性导航机械编排算法，计算目标物体的第一位置、第一速度和姿态信息；获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；基于稀疏地标点的预设位置，计算第二位置以及预设位置与第二位置之间的第一距离；基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。

Description

基于MEMS传感器和稀疏地标点的室内定位及优化方法
技术领域
本发明涉及定位技术领域，尤其涉及的是基于MEMS传感器和稀疏地标点的室内定位及优化方法。
背景技术
近年来，基于室内位置的服务因其在广泛的个性化服务中的潜在应用而变得越来越重要。越来越多的室内定位系统相继涌现，为解决室内定位难题提供了多样化的方案。而由于大众使用的移动终端硬件的局限性，一些高精度的室内定位系统无法被支持，实现低成本，高精度的室内定位困难。
因此，现有技术还有待改进和发展。
发明内容
本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种基于MEMS传感器和稀疏地标点的室内定位及优化方法，旨在解决现有技术中一些高精度的室内定位系统无法被支持，实现低成本，高精度的室内定位困难的问题。
本发明解决问题所采用的技术方案如下：
第一方面，本发明实施例提供一种基于MEMS传感器和稀疏地标点的室内定位及优化方法，其中，所述方法包括：
基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；
获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；
基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；
基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。
在一种实现方式中，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息包括：
获取MEMS传感器检测的运动矩阵和姿态矩阵，并将所述运动矩阵和所述姿态矩阵进行相乘，得到初始位置；
将所述初始位置进行微分运算，得到目标物体的第一位置；
获取MEMS传感器检测的第二速度、姿态矩阵、第一旋转速度、第二旋转速度、加速度向量和重力向量，并将所述第二速度、姿态矩阵、第一旋转速度、第二旋转速度、加速度向量和重力向量进行微分运算得到目标物体的第一速度；
获取MEMS传感器检测的第一旋转角速度和第二旋转角速度，并将所述姿态矩阵、所述第一旋转角速度和所述第二旋转角速度进行微分运算得到目标物体的姿态信息。
在一种实现方式中，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、第一速度和姿态信息之后包括：
基于所述第一位置、所述第一速度和所述姿态信息获取惯导误差向量。
在一种实现方式中，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、第一速度和姿态信息之后还包括：
获取第一参数误差、第二参数误差、第三参数误差和第四参数误差，并将所述第一参数误差、所述第二参数误差、所述第三参数误差和所述第四参数误差修正传感器的累积误差。
在一种实现方式中，若干所述约束信息具体为速度约束值、位置约束值、三维高度约束值和准静态磁场约束值；所述根据若干所述约束信息得到目标物体的观测量和状态量包括：
将所述速度约束值减去所述速度，得到速度观测量；
将所述位置约束值减去所述第一位置，得到二维位置观测量；
将所述二维位置观测量和所述三维高度约束值融合，得到三维位置观测量；
将所述准静态磁场约束值减去预设的磁场参考值，得到磁场观测量；
将所述速度观测量、所述三维位置观测量和所述磁场观测量融合得到目标物体的观测量；
基于所述观测量获取状态量。
在一种实现方式中，所述基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离包括：
获取基站接收的第二距离；
获取基站接收的信号强度值，并将所述信号强度值映射为第三距离；
将所述第二距离和所述第三距离进行融合，得到第一距离；
获取预设的地标点位置，将所述地标点位置和所述第一距离进行动态时间规整匹配，得到目标物体的第二位置。
在一种实现方式中，所述基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置包括：
基于双向平滑滤波算法，对所述融合状态量进行反向滤波平滑，得到修正后的融合状态量；
基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。
在一种实现方式中，所述基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置还包括：
构建步长航向模型，基于所述步长航向模型构建所述融合状态量和所述融合观测量的代价函数；
基于梯度下降算法中的所述代价函数，通过所述融合观测量获取修正后的融合观测量获取修正后的融合状态量；
基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。
第二方面，本发明实施例还提供一种基于MEMS传感器和稀疏地标点的室内定位及优化装置，其中，所述装置包括：
目标物体的移动信息获取模块，用于基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；
目标物体的观测量和状态量获取模块，用于获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；
目标物体的第二位置和第一距离的获取模块，用于基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；
目标位置获取模块，用于基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。
第三方面，本发明实施例还提供一种智能终端，包括有存储器，以及一个或者一个以上的程序，其中一个或者一个以上程序存储于存储器中，且经配置以由一个或者一个以上处理器执行所述一个或者一个以上程序包含用于执行如上述任意一项所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法。
第四方面，本发明实施例还提供一种非临时性计算机可读存储介质，当所述存储介质中的指令由电子设备的处理器执行时，使得电子设备能够执行如上述中任意一项所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法。
本发明的有益效果：本发明实施例首先基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；然后获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；接着基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；最后基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。可见，本发明实施例中基于惯性导航机械编排算法，通过将MEMS传感器得到第一位置和基于稀疏地标点得到第二位置进行无迹卡尔曼滤波融合，然后对融合的位置进行进一步的定位优化，消除融合计算导致的累积误差和发散误差，同时由于无迹卡尔曼滤波会将状态量和观测量反馈至惯性导航机械编排算法进一步消除MEMS传感器计算误差，从而得到精准的定位位置。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的基于MEMS传感器和稀疏地标点的室内定位及优化方法流程示意图。
图2为本发明实施例提供的基础定位模型和多约束定位模型框架示意图。
图3为本发明实施例提供的实施例提供的四种手持模式示意图。
图4为本发明实施例提供的航向对比图。
图5为本发明实施例提供的MEMS传感器定位和步长定位对比图。
图6为本发明实施例提供的不同手持模式定位轨迹图对比示意图。
图7为本发明实施例提供的不同手持模式定位精度对比图。
图8为本发明实施例提供的地标检测参考分布与实时分布对比图。
图9为本发明实施例提供的地标检测精度对比图。
图10为本发明实施例提供的场景一中二维定位轨迹对比图。
图11为本发明实施例提供的场景一中三维定位轨迹对比图。
图12为本发明实施例提供的场景二中二维定位轨迹对比图。
图13为本发明实施例提供的场景二中三维定位轨迹对比图。
图14为本发明实施例提供的与同类算法定位精度对比图。
图15为本发明实施例提供的基于MEMS传感器和稀疏地标点的室内定位及优化装置的原理框图。
图16为本发明实施例提供的智能终端的内部结构原理框图。
具体实施方式
本发明公开了基于MEMS传感器和稀疏地标点的室内定位及优化方法，为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。
本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样被特定定义，否则不会用理想化或过于正式的含义来解释。
由于现有技术中，基于智能手机的高精度三维室内定位的准确性受到低成本传感器性能和定位源覆盖范围有限的影响，具体体现在以下三个方面：
(1)手机端使用的MEMS传感器件精度较低，存在较大的量测误差，且受制于行人在室内复杂多变的运动和手持模式的影响。使用传统的传感器定位方式，还会受到室内人工磁场的影响，导致定位误差的进一步累积；
(2)室内天然存在的无线定位源覆盖范围有限，在一些特殊的场景下手机无法搜索到有用的定位源；而大规模布设高精度定位源的方法费时费力，且布设和维护的成本较高；
(3)传统的误差平滑算法可有效地消除前向组合定位带来的位置发散误差，但是计算量较大且依赖于定位模型，使用优化的方法需要例外建立误差模型，且对误差模型的准确度要求较高。
为了解决现有技术的问题，本实施例提供了基于MEMS传感器和稀疏地标点的室内定位及优化方法，基于惯性导航机械编排算法，通过将MEMS传感器得到第一位置和基于稀疏地标点得到第二位置进行无迹卡尔曼滤波融合，然后对融合的位置进行进一步的定位优化，消除融合计算导致的累积误差和发散误差，同时由于无迹卡尔曼滤波会将状态量和观测量反馈至惯性导航机械编排算法进一步消除MEMS传感器计算误差，从而得到精准的定位位置。具体实施时，首先基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；然后获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；接着基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；最后基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。
示例性方法
本实施例提供一种基于MEMS传感器和稀疏地标点的室内定位及优化方法，该方法可以应用于定位的智能终端。具体如图1所示，所述方法包括：
步骤S100、基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；相应的，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息包括如下步骤：获取MEMS传感器检测的运动矩阵和姿态矩阵，并将所述运动矩阵和所述姿态矩阵进行相乘，得到初始位置；将所述初始位置进行微分运算，得到目标物体的第一位置；获取MEMS传感器检测的第二速度、姿态矩阵、第一旋转矩阵、第二旋转矩阵、加速度向量和重力向量，并将所述第二速度、姿态矩阵、第一旋转矩阵、第二旋转矩阵、加速度向量和重力向量进行微分运算得到目标物体的第一速度；获取MEMS传感器检测的第一旋转角速度和第二旋转角速度，并将所述姿态矩阵、所述第一旋转角速度和所述第二旋转角速度进行微分运算得到目标物体的姿态信息。
在本实施例中，惯性导航机械编排算法的模型为：
式中，为位置，速度和姿态信息，/>为三轴陀螺仪和三轴加速度计获取的速度和姿态矩阵，gn为三轴加速度计获取的重力向量，fb为三轴加速度计获取的加速度分量，/>为三轴陀螺仪获取的地心固连坐标系相对惯性系的旋转速度，/>为三轴陀螺仪获取的导航坐标系相对地心固连坐标系的旋转速度，/>表示三轴加速度计获取的与载体运动相关联的运动矩阵，/>表示三轴陀螺仪获取的载体坐标系相对于惯性坐标系的旋转角速度，/>表示三轴陀螺仪获取的导航坐标系相对于惯性坐标系的旋转角速度。初始位置由/>和vn的积得到，然后将所述初始位置进行微分运算，得到目标物体的第一位置其中，第一位置/>为离散时间形式。然后获取MEMS传感器检测的第二速度vn、姿态矩阵第一旋转速度/>第二旋转速度/>加速度向量fb和重力向量gn，将的计算结果再进行微分运算，得到目标物体的第一速度/>其中，第一速度为离散时间形式。最后获取MEMS传感器检测的第一旋转角速度/>和第二旋转角速度/>并将/>的计算结果再进行微分运算，得到目标物体的姿态信息其中，目标物体的姿态信息/>为离散时间形式。
在一种实现方式中，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、第一速度和姿态信息之后包括如下步骤：基于所述第一位置、所述第一速度和所述姿态信息获取惯导误差向量。
在本实施例中，惯导误差向量即组合导航中的状态误差向量为：δx＝[(δpn)1×3 (δvn)1×3 ψ1×3 (εg)1×3 (εa)1×3]T
式中，δp为位置误差，δv为速度误差，ψ姿态误差代表的是横滚角，俯仰角和航向角的误差，用以修正姿态信息εg速度零偏，εa角加速度零偏。后续只需将惯导误差向量进行优化，就能得到更精确的定位位置。
在一种实现方式中，状态误差预测为：
式中，和/>为地球参数相关定义，τbg和τba分别表示陀螺仪和加速度计零偏随时间变化的相关参数，wbg和wba表示陀螺仪和加速度计零偏的量测噪声。
在一种实现方式中，所述基于惯性导航机械编排算法，如图2所示，通过MEMS传感器计算目标物体的第一位置、第一速度和姿态信息之后还包括如下步骤：获取第一参数误差、第二参数误差、第三参数误差和第四参数误差，并将所述第一参数误差、所述第二参数误差、所述第三参数误差和所述第四参数误差修正传感器的累积误差。
在本实施例中，第一参数误差为加速度观测修正姿态误差，公式如下：
式中，为预测的加速度，/>为预测的姿态矩阵，na为量测误差，fn为三轴陀螺仪获取的本地重力加速度。ψ×表示姿态误差构造的对角矩阵，/>为三轴陀螺仪获取的当前时刻姿态矩阵，fb为三轴加速度计检测的加速度值。第二参数误差为零速更新修正静态下的速度发散误差，公式如下：
其中，为惯性导航机械编排算法输出的速度向量，/>为零向量(向量中的值为0)。δvn表示速度的观测残差，nv为速度观测误差。第三参数误差为零角速度更新修正静态下的航向角发散误差，公式为：
式中，为惯导输出的航向角，/>为提取的准静态第一时刻的参考航向角。δθ为航向观测残差，nθ为航向观测误差。第四参数误差为非完整性约束修正侧向速度发散误差，用于修正行人行走过程中由于手机手持姿态转变带来的测向误差，具体如下公式：
其中，为由步长计算出的行走速度，/>表示载体坐标系和东北天坐标系之间的转换矩阵，/>为手持模式的相关联矩阵，/>为东北天坐标系与导航坐标系之间的转换矩阵。整体的增强的非完整性约束涉及的四种支持的手持模式如图3所示。
此外，使用伪观测和伪速度值修正运动误差，基于惯性导航机械编排算法，两次采样之间载体的位置和线速度变化有限的原理构建观测方程如下：
式中，和/>为惯导机械编排输出的位置和速度，/> 为常量，/>和/>为位置观测的残差和速度观测的残差，np为位置观测对应的高斯白噪声，nv为速度观测对应的高斯白噪声。设备(传感器)参数，即为惯导误差向量δx＝[(δpn)1×3 (δvn)1×3 ψ1×3 (εg)1×3(εa)1×3]T。将所述第一参数误差、所述第二参数误差、所述第三参数误差和所述第四参数误差修正MEMS传感器的累积误差；也就是说可以将所述加速度观测修正姿态误差、所述零速更新修正静态下的速度发散误差、所述零角速度更新修正静态下的航向角发散误差和所述非完整性约束修正侧向速度发散误差修正MEMS传感器参数的累积误差。
得到目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息后，就可以执行如图1所示的如下步骤：S200、获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差。相应的，若干所述约束信息具体为速度约束值、位置约束值、三维高度约束值和准静态磁场约束值；所述根据若干所述约束信息得到目标物体的观测量和状态量包括如下步骤：将所述速度约束值减去所述速度，得到速度观测量；将所述位置约束值减去所述第一位置，得到二维位置观测量；将所述二维位置观测量和所述三维高度约束值融合，得到三维位置观测量；将所述准静态磁场约束值减去预设的磁场参考值，得到磁场观测量；将所述速度观测量、所述三维位置观测量和所述磁场观测量融合得到目标物体的观测量；基于所述观测量获取状态量。
具体地，速度约束值为：
式中Ls为行人步长值，t0和t1表示每一步开始和结束时刻的时间戳。将所述速度约束值减去所述速度，得到速度观测量：
式中，vINS为惯导机械编排算法提供的速度值，δv为速度观测残差，nwalking为速度观测残差对应的高斯白噪声。位置约束值为：
式中，rx(t)和ry(t)表示通过行人航迹推算算法得出的位置坐标，Ls(t)和θ(t)表示当前时刻的步长值和航向值。将所述位置约束值减去所述第一位置，得到二维位置观测量，公式如下：
式中，PINS为惯导机械编排提供的位置值，δP为位置残差，nr为位置残差对应的高斯白噪声。三维高度约束值为：
式中，βb由气压更新计算出的高度更新值，γ为气压计输出值，γ0为参考气压值。将所述二维位置观测量和所述三维高度约束值融合，得到三维位置观测量准静态磁场约束值为/>其中，/>为姿态矩阵，/>为磁力计检测值。将所述准静态磁场约束值减去预设的磁场参考值，得到磁场观测量，公式如下：
式中，为磁场参考值，nm为量测误差。
最后，将所述速度观测量所述三维位置观测量/>和所述磁场观测量/>融合得到目标物体的观测量δzt。在本实施例中，组合导航状态更新和观测更新方程为：
式中，δxt和δzt分别为状态量和观测量，Ft-1,t和Gt分别为状态矩阵和观测矩阵，νt和分别为状态误差和观测误差。基于目标物体的观测量δzt，就可以通过观测更新方程获取状态量δxt。
得到目标物体的观测量和状态量后，就可以执行如图1所示的如下步骤：S300、基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；相应的，所述基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离包括如下步骤：获取基站接收的第二距离；获取基站接收的信号强度值，并将所述信号强度值映射为第三距离；将所述第二距离和所述第三距离进行融合，得到第一距离；获取预设的地标点位置，将所述地标点位置和所述第一距离进行动态时间规整匹配，得到目标物体的第二位置。
具体地，获取基站接收的第二距离，在本实施例中，第二距离为接收端得到的测距距离值，用Lobserved表示。然后获取基站接收的信号强度值，并将所述信号强度值映射为第三距离；在本实施例中，将所述信号强度值映射为第三距离用Pr(d)表示。将所述第二距离和所述第三距离进行融合，得到第一距离；第一距离通过Wi-Fi精密测距的融合模型构造，第一距离公式如下：
ΔDFused＝δ1·Lobserved+δ2·Pr(d)
式中，δ1和δ2为可调参数，如δ1可以取值为0.7和δ2可以取值为0.3，ΔDFused为最终融合结果，也即是第一距离。接着获取预设的地标点位置，将所述地标点位置和所述第一距离进行动态时间规整匹配，得到目标物体的第二位置。
在一种实现方式中，通过建立模型的方式得到第一距离Lobserved＝LFTM+dbias+dN+drandom，此处的Lobserved相当于上述计算方式中的ΔDFused；其中，LFTM为测距真实值，dbias为测距零偏值，dN为非视距误差，drandom为随机误差。
在本实施例中，动态时间规整(DTW)算法匹配模型如下：
DTW(drefer,dk)＝Dist(bn,cm)+min[D(bn-1,cm),D(bn,cm-1),D(bn-1,cm-1)]
其中，dk即为第一距离ΔDFused，DTW(drefer,dk)表示计算得出的相似距离值，drefer表
示参考距离值，drefer包含bn项，dk为实时距离值，dk包含cm等项，其中，实时距离值由Wi-Fi精密测距的融合模型提供，bn和cm分别表示实时序列和参考序列中的每一个值，D()表示括号中两个参数之间的差值的绝对值。预设的地标点位置可以通过Wi-Fi精密测距的融合模型检测获取，将所述地标点位置和所述第一距离进行动态时间规整匹配，得到目标物体的第二位置，也即得到基于Wi-Fi地标点的位置坐标
得到目标物体的第二位置和预设位置与所述第二位置之间的第一距离后，就可以执行如图1所示的如下步骤：S400、基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。
具体地，获取一段观测时间，将观测时间与第一速度相乘可以计算出MEMS传感器位置更新得到的距离值dMEMS,m，Wi-Fi精密测距的融合模型提供的距离值dwifi,m，也即第一距离，先将MEMS传感器位置更新得到的距离值dMEMS,m和Wi-Fi精密测距的融合模型提供的距离值进行融合，融合距离公式如下：
式中，δzm,range表示距离量测的残差，dMEMS,m表示由传感器位置更新得到的距离值，dwifi,m表示Wi-Fi精密测距的融合模型提供的距离值。然后将所述第一位置和所述第二位置进行融合，融合位置公式如下：
式中，为检测到的基于Wi-Fi的地标点位置坐标，也即第二位置，/>惯导机械编排得到的位置坐标，也即第一位置。δp表示位置的观测残差，nwifi表示位置的观测误差，其中，nwifi是自适应调整Wi-Fi地标点提供的位置的权重值，计算公式为：因为Wi-Fi指标点位置的误差nwifi是变化的，因此需要实时地去估计大小；其中约等于可以看作等号。/>为位置观测的残差，δzd为距离观测的残差，将距离观测的残差除以时间，可以得到速度观测的残差/>基于融合距离和融合位置可以获取到目标物体的融合姿态信息。在本实施例中，使用正向的无迹自适应卡尔曼滤波算法对所有的状态量(根据前述的观测更新方程、位置观测的残差和速度观测的残差，就可以得到位置状态量和速度状态量)和观测量(位置观测的残差和速度观测的残差)进行融合和更新，步骤如下：
1)无迹卡尔曼状态初始化：式中，x0和P0表示初始状态分布和对应的协方差矩阵；/>表示初始状态分布对应的sigma点集。
2)计算初始状态分布： 为上一时刻的更新值，Pk-1为上一时刻协方差矩阵，γ为固定参数；
3)每一个sigma点的状态更新：φ为状态矩阵，/>为上一时刻sigma点状态；
4)每一个sigma点的量测更新：为状态更新值，/>为观测函数；
5)卡尔曼增益计算：式中/>为互相关协方差矩阵，/>为自相关协方差矩阵的逆矩阵，Kk为卡尔曼增益；
6)多源融合状态更新：式中/>为观测更新量，/>为最终的融合状态量；zk并表示实时的融合观测值，/>表示融合观测值的预测值。
接着，将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息又会和wifi基站测得的第二距离和第一距离进行融合，如此反复迭代，以便消除MEMS传感器的误差。此外，考虑到融合后的融合位置和融合姿态信息在融合过程中也会存在误差，为了消除这部分误差，提高定位精度，将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。
在一种实现方式中，所述基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置包括如下步骤：基于双向平滑滤波算法，对所述融合状态量进行反向滤波平滑，得到修正后的融合状态量；基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。
在本实施例中，对前向滤波(自适应无迹卡尔曼滤波算法)的融合状态量，进行固定区间平滑，以进一步消除融合过程中的累积误差和发散误差，主要步骤为：
1)对保存下来的融合状态量进行反向滤波平滑：式中Pk-1和/>分别表示正向滤波保存下来的协方差矩阵和状态量的更新值；
2)对保存下来的协方差矩阵进行反向滤波平滑
Pk-1|k＝Pk-1-(Pk-1φk T(Pk -)-1)(Pk-Pk -)·(Pk-1φk T(Pk -)-1)T，式中，φ为状态矩阵，
Pk -为保存下来的协方差矩阵的预测值。
通过上述步骤得到修正后的融合状态量基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。也即，将修正后的融合状态量δxt，根据惯导误差向量的公式：δx＝[(δpn)1×3 (δvn)1×3 ψ1×3 (εg)1×3 (εa)1×3]T，就可以求解出位置误差δp，姿态误差ψ，再根据位置误差δp，姿态误差ψ，修正所述融合位置和所述融合姿态信息，最终得到精准的目标位置。本发明采取双向平滑滤波算法可以得到更高的定位精度。
在一种实现方式中，所述基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置还包括如下步骤：构建步长航向模型，基于所述步长航向模型构建所述融合状态量和所述融合观测量的代价函数；基于梯度下降算法中的所述代价函数，通过所述融合观测量获取修正后的融合观测量获取修正后的融合状态量；基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。
在本实施例中，采用梯度下降算法的计算复杂度降低，主要步骤为：
1)构建步长航向模型(也即行人位置更新的步长航向模型)：
式中，P0为上一参考点位置，Li和θi为轨迹中包含的每一个步长值和对应的航向值。
2)基于所述步长航向模型构建所述融合状态量和所述融合观测量的代价函数：
式中R为融合观测量协方差矩阵，z为融合观测量，x为融合状态量；
3)对融合观测量进行线性泰勒级数展开：
式中，G为驱动矩阵，为误差项；/>
4)对融合状态量进行迭代求解：δx＝(GTR-1G)-1GTR-1δz，式中δz为融合观测量的残差量。最终迭代公式为：这样就可以得到修正后的融合状态量/>基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。也即，将修正后的融合状态量δxt，根据惯导误差向量的公式：δx＝[(δpn)1×3 (δvn)1×3 ψ1×3 (εg)1×3(εa)1×3]T，就可以求解出位置误差δp，姿态误差ψ，再根据位置误差δp，姿态误差ψ，修正所述融合位置和所述融合姿态信息，最终得到精准的目标位置。本发明采取梯度下降算法可以降低数据复杂度。
在实际中，可以根据对定位精度和计算复杂度的要求来选择双向平滑滤波算法或者梯度下降算法。
通过比较本发明提出的基于MEMS传感器的航向解算精度和基于陀螺仪和磁力计的航向解算精度，可以发现本发明实施例得到了较优的航向解算结果。同理，比较基于微传感器的位置解算精度和传统航迹推算算法的位置解算精度，也取得了较优的结果。航向解算和定位结果对比如图4和如图5所示。
图6和图7进一步给出了使用本发明所涉及的几种不同的手持模式的定位轨迹和相应的定位精度对比。从图7可以看出，四种不同的手持模式呈现出了不同的定位精度，其中阅读模式下的二维定位精度最高，达到百分之七十五情况下误差小于0.98米，手摇模式下精度最低，为百分之七十五情况下误差小于1.77米。
图8和图9分别展示了基于动态时间规整算法的Wi-Fi基站地标点检测算法的序列匹配图和最终实现的精度对比图，并与现存的算法做了实验对比，验证了本发明提出的地标点检测算法的高精度。从图9可以看出，本发明提出的基于动态时间规整算法的Wi-Fi基站地标点检测算法检测误差在百分之七十五的情况下小于0.26m，远高于同类算法。
实验场景一中进行的二维，三维定位和轨迹误差优化效果如图10，图11所示。由图10和图11可以看出，使用本发明提供的两种误差优化算法均可以获得比单一的正向融合轨迹更高的二维和三维定位精度。实验场景二中进行的二维，三维定位和轨迹误差优化效果如图12，图13所示。此外，本文中提出的算法与两种同类算法在定位精度上进行比较，均取得了较高的定位精度，具体对比如图14所示。
本发明与现有技术相比，本发明实施例综合考虑惯性导航算法，行人导航算法，稀疏地标检测，以及多源融合三维室内定位和误差优化问题：
1)采用了基于惯性导航算法，行人导航算法，磁场观测量和气压观测量的三维位置基础坐标更新模型，并使用自适应无迹卡尔曼滤波将惯导机械编排,行人导航机械编排,磁场模型，气压计高度更新模型有机融合起来。
2)提取行人运动特征和传感器输出特征作为观测量，进一步对MEMS传感器定位产生的累积误差进行约束，进一步提升使用MEMS传感器单独定位时的定位精度和稳定性。
3)通过采用时间序列匹配算法实现基于蓝牙或Wi-Fi基站的稀疏地标点检测，检测到的三维位置坐标和Wi-Fi测距结果被进一步与MEMS传感器的定位结果进行多源融合，以实现轻量级室内高精度定位系统。
4)采用了基于双向平滑算法和梯度下降算法的定位误差优化算法，最终实现包含稀疏分布基站的室内场景下的高精度三维定位与误差优化。
通过以上四点，本发明实施例能够取得较优的室内三维定位结果。
示例性设备
如图15中所示，本发明实施例提供一种基于MEMS传感器和稀疏地标点的室内定位及优化装置，该装置包括目标物体的移动信息获取模块501、目标物体的观测量和状态量获取模块502、目标物体的第二位置和第一距离的获取模块503和目标位置获取模块504，其中：目标物体的移动信息获取模块501，用于基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；
目标物体的观测量和状态量获取模块502，用于获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；
目标物体的第二位置和第一距离的获取模块503，用于基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；
目标位置获取模块504，用于基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。
基于上述实施例，本发明还提供了一种智能终端，其原理框图可以如图16所示。该智能终端包括通过系统总线连接的处理器、存储器、网络接口、显示屏、温度传感器。其中，该智能终端的处理器用于提供计算和控制能力。该智能终端的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该智能终端的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于MEMS传感器和稀疏地标点的室内定位及优化方法。该智能终端的显示屏可以是液晶显示屏或者电子墨水显示屏，该智能终端的温度传感器是预先在智能终端内部设置，用于检测内部设备的运行温度。
本领域技术人员可以理解，图16中的原理图，仅仅是与本发明方案相关的部分结构的框图，并不构成对本发明方案所应用于其上的智能终端的限定，具体的智能终端可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种智能终端，包括有存储器，以及一个或者一个以上的程序，其中一个或者一个以上程序存储于存储器中，且经配置以由一个或者一个以上处理器执行所述一个或者一个以上程序包含用于进行以下操作的指令：基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；
获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；
基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；
基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本发明所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
综上所述，本发明公开了基于MEMS传感器和稀疏地标点的室内定位及优化方法，所述方法包括：本发明实施例首先基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；然后获取目标物体的若干约束信息，并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；接着基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；最后基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置。可见，本发明实施例中基于惯性导航机械编排算法，通过将MEMS传感器得到第一位置和基于稀疏地标点得到第二位置进行无迹卡尔曼滤波融合，然后对融合的位置进行进一步的定位优化，消除融合计算导致的累积误差和发散误差，同时由于无迹卡尔曼滤波会将状态量和观测量反馈至惯性导航机械编排算法进一步消除MEMS传感器计算误差，从而得到精准的定位位置。
基于上述实施例，本发明公开了一种基于MEMS传感器和稀疏地标点的室内定位及优化方法，应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (10)

1.一种基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述方法包括：
基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息；其中，所述姿态信息用于表征目标物体移动过程中产生的航向角、横滚角和俯仰角；
获取目标物体的若干约束信息，若干所述约束信息包括：速度约束值、位置约束值、三维高度约束值和准静态磁场约束值；
并根据若干所述约束信息得到目标物体的观测量和状态量；其中，所述约束信息用于表征目标物体的移动属性；所述观测量用于表征目标物体移动时形成的真实误差；所述状态量用于表征目标物体移动时估算的预测误差；
基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离；
基于自适应无迹卡尔曼滤波算法，将若干所述约束信息、所述观测量、所述第一位置、所述第一速度、所述姿态信息、第二位置和所述第一距离进行融合，得到目标物体的融合观测量、融合状态量、融合位置和融合姿态信息；并将所述融合观测量和融合状态量反馈至基于惯性导航机械编排算法，通过MEMS传感器计算目标物体更新的第一位置、目标物体更新的第一速度和目标物体更新的姿态信息，同时基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置；
所述预设的滤波算法包括：双向平滑算法和梯度下降算法。
2.根据权利要求1所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、目标物体的第一速度和目标物体的姿态信息包括：
获取MEMS传感器检测的运动矩阵和姿态矩阵，并将所述运动矩阵和所述姿态矩阵进行相乘，得到初始位置；
将所述初始位置进行微分运算，得到目标物体的第一位置；
获取MEMS传感器检测的第二速度、姿态矩阵、第一旋转矩阵、第二旋转矩阵、加速度向量和重力向量，并将所述第二速度、姿态矩阵、第一旋转速度、第二旋转速度、加速度向量和重力向量进行微分运算得到目标物体的第一速度；
获取MEMS传感器检测的第一旋转角速度和第二旋转角速度，并将所述姿态矩阵、所述第一旋转角速度和所述第二旋转角速度进行微分运算得到目标物体的姿态信息。
3.根据权利要求1所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、第一速度和姿态信息之后包括：
基于所述第一位置、所述第一速度和所述姿态信息获取惯导误差向量。
4.根据权利要求3所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述基于惯性导航机械编排算法，通过MEMS传感器计算目标物体的第一位置、第一速度和姿态信息之后还包括：
获取第一参数误差、第二参数误差、第三参数误差和第四参数误差，并将所述第一参数误差、所述第二参数误差、所述第三参数误差和所述第四参数误差修正传感器的累积误差。
5.根据权利要求1所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述根据若干所述约束信息得到目标物体的观测量和状态量包括：
将所述速度约束值减去所述速度，得到速度观测量；
将所述位置约束值减去所述第一位置，得到二维位置观测量；
将所述二维位置观测量和所述三维高度约束值融合，得到三维位置观测量；
将所述准静态磁场约束值减去预设的磁场参考值，得到磁场观测量；
将所述速度观测量、所述三维位置观测量和所述磁场观测量融合得到目标物体的观测量；
基于所述观测量获取状态量。
6.根据权利要求1所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述基于稀疏地标点的预设位置，计算目标物体的第二位置以及预设位置与所述第二位置之间的第一距离包括：
获取基站接收的第二距离；
获取基站接收的信号强度值，并将所述信号强度值映射为第三距离；
将所述第二距离和所述第三距离进行融合，得到第一距离；
获取预设的地标点位置，将所述地标点位置和所述第一距离进行动态时间规整匹配，得到目标物体的第二位置。
7.根据权利要求1所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置包括：
基于双向平滑滤波算法，对所述融合状态量进行反向滤波平滑，得到修正后的融合状态量；
基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。
8.根据权利要求1所述的基于MEMS传感器和稀疏地标点的室内定位及优化方法，其特征在于，所述基于预设的滤波算法，对所述融合位置和所述融合姿态信息进行误差优化计算，得到目标位置还包括：
构建步长航向模型，基于所述步长航向模型构建所述融合状态量和所述融合观测量的代价函数；
基于梯度下降算法中的所述代价函数，通过所述融合观测量获取修正后的融合观测量获取修正后的融合状态量；
基于修正后的融合状态量修正所述融合位置和所述融合姿态信息，得到目标位置。
9.一种智能终端，其特征在于，包括有存储器，以及一个或者一个以上的程序，其中一个或者一个以上程序存储于存储器中，且经配置以由一个或者一个以上处理器执行所述一个或者一个以上程序包含用于执行如权利要求1-8中任意一项所述的方法。
10.一种非临时性计算机可读存储介质，其特征在于，当所述存储介质中的指令由电子设备的处理器执行时，使得电子设备能够执行如权利要求1-8中任意一项所述的方法。
CN202111470294.0A 2021-12-03 2021-12-03 基于mems传感器和稀疏地标点的室内定位及优化方法 Active CN114383605B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111470294.0A CN114383605B (zh) 2021-12-03 2021-12-03 基于mems传感器和稀疏地标点的室内定位及优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111470294.0A CN114383605B (zh) 2021-12-03 2021-12-03 基于mems传感器和稀疏地标点的室内定位及优化方法

Publications (2)

Publication Number Publication Date
CN114383605A CN114383605A (zh) 2022-04-22
CN114383605B true CN114383605B (zh) 2024-04-02

Family

ID=81195746

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111470294.0A Active CN114383605B (zh) 2021-12-03 2021-12-03 基于mems传感器和稀疏地标点的室内定位及优化方法

Country Status (1)

Country Link
CN (1) CN114383605B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116105731B (zh) * 2023-04-07 2023-06-20 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质
CN116182873B (zh) * 2023-05-04 2023-07-11 长沙驰芯半导体科技有限公司 室内定位方法、系统及计算机可读介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107289933A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN108519090A (zh) * 2018-03-27 2018-09-11 东南大学—无锡集成电路技术研究所 一种基于优化的ukf算法的双通道组合定姿算法的实现方法
CN109579836A (zh) * 2018-11-21 2019-04-05 阳光凯讯(北京)科技有限公司 一种基于mems惯性导航的室内行人方位校准方法
CN110702113A (zh) * 2019-05-09 2020-01-17 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN111854762A (zh) * 2020-08-05 2020-10-30 翟瑞永 一种基于卡尔曼滤波算法的三维定位方法及其定位系统
CN113295158A (zh) * 2021-05-14 2021-08-24 江苏大学 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107289933A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN108519090A (zh) * 2018-03-27 2018-09-11 东南大学—无锡集成电路技术研究所 一种基于优化的ukf算法的双通道组合定姿算法的实现方法
CN109579836A (zh) * 2018-11-21 2019-04-05 阳光凯讯(北京)科技有限公司 一种基于mems惯性导航的室内行人方位校准方法
CN110702113A (zh) * 2019-05-09 2020-01-17 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN111854762A (zh) * 2020-08-05 2020-10-30 翟瑞永 一种基于卡尔曼滤波算法的三维定位方法及其定位系统
CN113295158A (zh) * 2021-05-14 2021-08-24 江苏大学 一种融合惯性数据、地图信息以及行人运动状态的室内定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种基于改进UKF滤波的GPS+PDR组合定位方法;牛欢等;测绘通报(第07期);5-9 *
融合地图信息与WiFi地标的室内粒子滤波定位算法;朱晨迪等;山东科技大学学报(自然科学版);第39卷(第01期);91-99 *

Also Published As

Publication number Publication date
CN114383605A (zh) 2022-04-22

Similar Documents

Publication Publication Date Title
CN113091709B (zh) 一种新型gnss接收机倾斜测量方法
CN114383605B (zh) 基于mems传感器和稀疏地标点的室内定位及优化方法
CN112577521B (zh) 一种组合导航误差校准方法及电子设备
CN102322858B (zh) 用于地磁/捷联惯导组合导航系统的地磁匹配导航方法
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN106153069B (zh) 自主导航系统中的姿态修正装置和方法
CN105103089B (zh) 用于基于视频输入生成准确传感器校正的系统和方法
CN109059907A (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
CN112562077B (zh) 一种融合pdr和先验地图的行人室内定位方法
KR101576424B1 (ko) 실내 측위를 위한 지자기 센서 자동 보정 방법
CN103438890B (zh) 基于tds与图像测量的行星动力下降段导航方法
Hasan et al. Smart phone based sensor fusion by using Madgwick filter for 3D indoor navigation
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN116642482A (zh) 基于固态激光雷达和惯性导航的定位方法、设备和介质
CN110260860B (zh) 基于足部惯性传感器的室内移动测量定位定姿方法及系统
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN112985392A (zh) 基于图优化框架的行人惯性导航方法和装置
CN109682372B (zh) 一种结合建筑物结构信息与rfid标定的改进型pdr方法
JP6494552B2 (ja) フロア間の遷移に基づいて位置の補正が可能な位置推定装置、プログラム及び方法
Zhang et al. Mag-ODO: Motion speed estimation for indoor robots based on dual magnetometers
CN114353787B (zh) 一种多源融合定位方法
RU2629539C1 (ru) Способ измерения магнитного курса подвижного объекта
CN114719858B (zh) 一种基于imu和楼层高度目标补偿的3维定位方法
CN114088095B (zh) 一种基于光电二极管的三维室内定位方法
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant