CN109099913B - 一种基于mems惯性器件的穿戴式导航装置和方法 - Google Patents

一种基于mems惯性器件的穿戴式导航装置和方法 Download PDF

Info

Publication number
CN109099913B
CN109099913B CN201811177819.XA CN201811177819A CN109099913B CN 109099913 B CN109099913 B CN 109099913B CN 201811177819 A CN201811177819 A CN 201811177819A CN 109099913 B CN109099913 B CN 109099913B
Authority
CN
China
Prior art keywords
state
navigation
error
imu
static
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
CN201811177819.XA
Other languages
English (en)
Other versions
CN109099913A (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.)
Gewu Zhihang Shenzhen Technology Co ltd
Original Assignee
Gewu Zhihang Shenzhen Technology Co 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 Gewu Zhihang Shenzhen Technology Co ltd filed Critical Gewu Zhihang Shenzhen Technology Co ltd
Priority to CN201811177819.XA priority Critical patent/CN109099913B/zh
Publication of CN109099913A publication Critical patent/CN109099913A/zh
Application granted granted Critical
Publication of CN109099913B publication Critical patent/CN109099913B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明公开了一种基于MEMS惯性器件的穿戴式导航装置和方法，该装置主要由两组IMU架构组成，分别固定于行人的左右脚，两组IMU架构的结构相似，包括低成本的IMU阵列，微控制器以及蓝牙模块，本发明基于行人行走时脚部的运动特性，建立好联合导航系统的状态模型和误差模型，并求得载体导航状态。零速修正卡尔曼滤波器在相应设备处于静止状态时将对应采集得到的数据进行修正，然后借助于空间距离约束方法求得联合导航最优解，本发明极大地减少了导航系统的漂移，能够对导航进行实时修正，提高装置的导航精度。

Description

一种基于MEMS惯性器件的穿戴式导航装置和方法
技术领域
本发明涉及单兵作战、消防救援等领域，特别涉及一种基于MEMS惯性器件的穿戴式导航装置和方法。
背景技术
MEMS是微机电系统，随着GPS的发展，现如今打造的GPS导航系统可以向全球各地提供全时段，高精度的三维位置等信息的无线电导航定位系统。然而，在深山密林或者城市室内这些环境中，接收的GPS信号较弱或者缺失，这样用户将无法依靠GPS完成对自己当前位置的定位，以及前往目的地的导航规划。
因此，在卫星信号被密林或建筑物遮挡这些特殊环境下，便可以通过人员自主定位技术进行“补盲”。而惯性导航是一种自主性导航，通过固定于载体上的惯性测量器件，测得载体的角速度信息和加速度信息，并通过积分运算后求得载体的速度与位置信息，从而完成对载体的导航定位功能。由于惯性器件的复杂结构与高昂成本，限制了相关应用的发展。
随着微机电(MEMS)技术的发展，MEMS惯性器件逐渐取代了传统惯性器件，成为较为热门的研究对象。而基于MEMS的惯性测量单元(IMU)因其低廉的成本，小型化体积，功耗低以及极强的环境适用能力等优点，被广泛地用于军用，民用产业之中。因而使用基于MEMS惯性技术为核心的自主导航，便于安装与携带，自主性强，适用于各种野外荒地。并且其不依赖任何外部信息，也不向外辐射能量，具有较高的隐蔽性，也适用于特战，单兵潜入作战等方式。
基于MEMS的惯性测量单元也有缺陷，虽然在短程内具有较高的精度，然而随着时间的增长，因为积分运算以及测量元件的漂移，即使初始化修正完成，之后的误差仍然十分严重。
发明内容
本发明解决的技术问题是提供一种低成本、高精度的一种基于MEMS惯性器件的穿戴式导航装置。
本发明解决其技术问题所采用的技术方案是：一种基于MEMS惯性器件的穿戴式导航装置，包括数据测量模块，微控制器、蓝牙模块、PC机和存储器，所述数据测量模块和存储器与微控制器电连接，所述微控制器与PC机通过蓝牙连接，所述数据测量模块包括固联于左右双脚上的IMU测量阵列，所述IMU测量阵列包括电路板，所述电路板正反两面均以田字形排布，均匀、对称地分布有4个惯性测量单元；
所述每个惯性测量单元均由三轴加速度计，三轴陀螺仪以及磁力计组成；
IMU测量阵列输出的数据进行支持度方法测试，再采用加权平均算法完成数据融合得出导航数据。
本发明还公开了一种基于MEMS惯性器件的穿戴式导航方法，其步骤为：
(1)单组IMU架构导航状态及误差补偿；
建立单组IMU架构导航状态模型，根据静止探测器对载体运动状态的判断，分为运动与静止状态下的两类不同的状态模型，并在静止状态下对状态误差进行估计；状态误差估计结果返回系统中对导航状态量进行修正；
(2)基于双脚四种组合状态下的联合导航状态模型的建立；
双脚两组IMU架构可视为相互独立对象，针对左右双脚各自是否处于运动情况选取对应状态下的单脚状态模型，并组合建立联合导航状态模型；
从而搭建完成四种组合状态下的联合导航状态模型和组合误差模型；
(3)零速修正卡尔曼滤波；
利用零速探测器对测试值进行统计，当一个或两个系统的统计量小于阈值，则假设其处于静止状态；当零速探测器检测到单脚或双脚处于“零速”状态，则对联合导航状态下的误差量进行估计并反馈至状态量，完成导航状态的初步修正；
(4)建立并应用空间距离约束方法以优化联合导航解算；
利用双脚间最大距离作为约束条件，当不符合约束条件时，将导航状态量投射至满足约束条件的子空间，利用约束最小二乘法求得满足约束条件的导航状态量，并作为结果输出。
进一步的是：步骤(1)对两组IMU架构导航状态分析，具体步骤为：
(a)建立好单个IMU架构的状态方程；
惯性导航系统依据牛顿惯性原理，将加速度计、陀螺仪等惯性敏感元件测得的数据与初始位置信息结合以解算出此时系统的位置、速度和姿态信息；即当系统工作时，可以通过测量载体运动时的加速度以及角速度信息，通过积分运算求得导航数据；
针对惯性测量单元阵列，借助SINS导航算法将当前测量的加速度和角速度信息解算为载体此时位置，速度和姿态的状态信息；建立单个IMU架构的导航状态向量xk＝[pk vkqk]T，pk，vk，qk分别代表K时刻载体的位置，速度以及四元数信息。
则单个IMU架构的状态方程表示为：
Figure GDA0004191301330000021
fk，wk分别表示比力和角速度，即加速度计和陀螺仪的测量值，g＝[0 0 g]T表示重力，Ω(·)表示四元数更新矩阵；
随着导航系统的推演，加速度计和陀螺仪测量值存在的微小的误差会在积分运算的作用下快速增加，导致惯导系统不能够长时间工作；这里使用零速修正算法，通过将误差模型化后估算误差状态，再经由卡尔曼滤波器对误差进行处理；
(b)构建静止探测器；
对测试值进行统计，当系统的统计量低于阈值，则假设其处于静止状态；
建立姿态最优检测器方程：
Figure GDA0004191301330000031
(fk,ωk)W：表示在某个时间窗口W下的比力和角速度测量值；
Figure GDA0004191301330000032
表示在窗口w下比力的平均值；
σf，σω：表示测量误差标准偏差；
将该检测方程作为静止探测器使用，当给出零速检测阈值γ并满足以下条件
T{(fk,ωk)W}≤γ；
则IMU架构处于静止状态的假设成立；
(c)零速修正算法；
这里静止探测器与ZUPT探测器使用相同的判别结构，因此静止检测同样能够触发零速修正算法；设计零速修正卡尔曼滤波器，惯导系统误差的状态向量取为Δxk＝[Δpk Δvk Δθk]T，分别表示位置误差，速度误差和四元数误差；则一阶误差模型建立如下：
Δxk＝FK-1Δxk-1+Gk-1wk-1；
Figure GDA0004191301330000033
Fk-1，Gk-1：分别表示状态转移矩阵和噪声增益矩阵；
量测值取为静止状态下解算的速度误差Δvk，测量矩阵H＝[03 I3 03]，测量方程如下：
yk＝HΔxk+nk
nk为测量噪声，采用自适应算法求取，以提高滤波精度。速度误差Δvk只有在系统处于静止状态下才可以获取；所以当静止探测器检测为运动状态时，卡尔曼滤波器只做时间上的更新；当检测为静止状态时，卡尔曼滤波器进行完整更新，并且将估计结果返回系统中；
(d)更新载体处于静止状态时的状态方程；
对于双IMU架构系统，并不是所有的误差状态都可观察的；在连续的零速修正过程中，系统和可观测矩阵如下：
Figure GDA0004191301330000041
速度误差，翻滚角误差和俯仰角误差是可观测的，而位置误差和航向角误差是不可观测的；
在静止状态下以上参量仍然会有值产生，相较于理论上的无输入值，即产生了误差；针对这些参量，可观测参量误差可以通过卡尔曼滤波器进行修正，而不可观测参量误差则需要锁定，保证“零速”状态期间不会产生漂移影响整个导航系统正常工作；
对于位置误差，将变化量vk-1dtk设为0，可完成对位置的锁定；对于航向角误差，不能直接将航向角变化量独立出来，需要减去对航向角影响最大的垂直于水平面的角速度分量，完成对航向角的锁定；
Figure GDA0004191301330000042
静止状态下的载体状态方程及误差模型如下：
Figure GDA0004191301330000043
Δxk＝Fk-1,stillΔxk-1+Gk-1,stillwk-1
Figure GDA0004191301330000044
Fk-1,still，Gk-1,still：分别表示静止状态下的状态转移矩阵和噪声增益矩阵。
步骤(2)建立好联合导航状态模型以及误差模型；
双脚两组IMU架构视为相互独立对象，方便区分使用上标“1”和“2”各代表左脚和右脚处固联的惯性导航系统；因为采用了静态测试对导航状态进行锁定，所以联合导航状态将在四种不同的情况下单独讨论；
情况1：左右两组IMU架构都处于运动状态，则联合状态模型为：
Figure GDA0004191301330000051
两组惯性测量系统视为相互独立，所以误差状态方程建立如下：
Δxk＝Fk-1,jointΔxk-1+Gk-1,jointwk-1
Figure GDA0004191301330000052
情况2：左右两组IMU架构都处于静止状态，则联合状态模型为：
Figure GDA0004191301330000053
误差状态方程为：
Figure GDA0004191301330000054
情况3：左边惯性测量单元组处于静止状态，右边惯性测量单元组处于运动状态，则联合状态模型和误差状态方程：
Figure GDA0004191301330000055
Figure GDA0004191301330000056
情况4：左边惯性测量单元组处于运动状态，右边惯性测量单元组处于静止状态，则联合状态模型和误差状态方程：
Figure GDA0004191301330000057
Figure GDA0004191301330000058
后面三种情况下的静止状态，其中的过程噪声Wk-1中的协方差矩阵Qk-1,joint中对应航向过程噪音部分需要设置为0，
步骤(3)零速修正卡尔曼滤波器；
利用零速探测器对测试值进行统计，当一个或两个系统的统计量小于阈值，则假设其处于静止状态，因而对该部分进行卡尔曼增益的计算，求出误差估计量并对联合导航状态向量进行纠正，以及计算出后验协方差；
卡尔曼增益：
Figure GDA0004191301330000061
误差估计量：
Figure GDA0004191301330000062
纠正联合导航状态向量：
Figure GDA0004191301330000063
Figure GDA0004191301330000064
表示导航解通过扰动向量的修正得出真实导航状态；
计算后验协方差：
Pk←(I-KkHk)Pk
完成导航状态的初步修正；
步骤(4)空间范围约束
正常人类在行走的过程中，双脚之间的距离会随着行走姿势的不同而发生改变，但是双脚之间的距离是有上限的，可以通过对两个导航系统之间距离的最大距离约束对两个导航系统信息融合，求得最优导航解；
(a)建立约束式；
将左右两脚上的脚踏式惯性测量系统的状态分别设为
Figure GDA0004191301330000065
和/>
Figure GDA0004191301330000066
系统的联合导航状态量为/>
Figure GDA0004191301330000067
假设行人左右脚之间的最大距离为L，两个系统之间的范围约束式可以表达为：
||DXk||2≤l2，D＝[I3 03×7 -I3 03×7]
通过使用约束最小二乘法求得满足约束条件的联合导航状态；
对联合导航解施加约束的方法如下：
如果满足约束条件，将联合导航状态量输出；
如果不满足条件，即||DXk||2＞l2，将联合导航解投射到子空间{X∈R:||DX||2≤l2}，定义投影为：
Figure GDA0004191301330000071
Figure GDA0004191301330000072
Pk为联合导航解
Figure GDA0004191301330000073
的协方差；此投影可以解决不等式约束加权最小二问题，解为拉格朗日函数的一个驻点；
(b)对不等式约束最小二乘问题求解；
不等式约束加权最小二乘问题的解是拉格朗日函数的一个静止点，引入拉格朗日函数：
Figure GDA0004191301330000074
Figure GDA0004191301330000075
λ是拉格朗日乘子，即满足约束条件下求解拉格朗日函数的最小值；
则拉格朗日函数的驻点可由以下方程解求得：
Figure GDA0004191301330000076
如果约束最小二乘问题有唯一解，对应于最小二乘问题解的拉格朗日函数驻点是λ>0条件下的唯一驻点，该驻点表示为{λ*,X*}；
Figure GDA0004191301330000077
Figure GDA0004191301330000078
Figure GDA0004191301330000079
上面方程是λ的非线性多项式函数，使用牛顿法找到其根，得到λ，求解出联合导航解：
Figure GDA00041913013300000710
联合导航解的协方差
Figure GDA00041913013300000711
可以近似为：
Figure GDA00041913013300000712
Figure GDA00041913013300000713
是投影函数/>
Figure GDA00041913013300000714
的雅可比矩阵；
得到最优的导航信息。
本发明的有益效果是：
1、本发明对人员双脚上的惯性测量数据集合进行判别融合，确保用于解算的数据的鲁棒性和准确性；
2、本发明对左右脚上的两个系统施加状态空间约束，能够显著地降低最终位置的平均误差和协方差估计，并且通过约束完成两个导航系统信息的融合；
3、本发明通过设计静态探测器，融合加速度和角速度信息一起判断并确定系统的静止状态，从而通过零度更新下的卡尔曼滤波器完成对系统状态的矫正，为最终提高导航精度奠定了基础；
4、本发明对处于零速静止状态下的载体导航系统，通过卡尔曼滤波器完成可观测量误差的估计和联合导航状态的修正，以及在静止状态下对不可观测量进行锁定，避免航向角误差逐渐积累并发散等情况而带来严重的后果，保证系统完成高精度导航任务。
附图说明
图1是本发明的结构原理图；
图2是本发明的任务流程图；
图3是本发明的数据处理模块流程图；
图4是本发明的约束条件下数据重新解算模块流程图；
具体实施方式
下面结合附图和具体实施方式对本发明进一步说明。
如图1所示的一种基于MEMS惯性器件的穿戴式导航装置，其特征在于：包括数据测量模块，微控制器、蓝牙模块、PC机和存储器，所述数据测量模块和存储器与微控制器电连接，所述微控制器与PC机通过蓝牙连接，所述数据测量模块包括固联于左右双脚上的IMU测量阵列，所述IMU测量阵列包括电路板，所述电路板正反两面均以田字形排布，均匀、对称地分布有4个惯性测量单元，在系统运行中，由于有大量的惯性姿态数据采集与传输，故选择专门的存储器模块用作存储功能；微处理器作为惯性导航系统架构的主控芯片，将惯性传感器采集到的数据经过一系列算法的处理，从而输出修正完成的联合导航状态量，并通过蓝牙模块将数据传输到PC机，借助Matlab软件对传输数据处理并模拟出行人的行走轨迹。
所述的微处理器选择STM32，利用STM32的小巧轻便，能效比高。在稳定性，数据精度，工作温度，封装体积以及能耗等各方面因素有着较为明显的优势，所述每个惯性测量单元均由三轴加速度计，三轴陀螺仪以及磁力计组成；
导航装置的任务流程图如图2所示，其中，需要执行的任务按先后顺序分别为数据采集任务和数据处理任务；在数据采集任务中，左右双脚两组惯性测量单元组IMUs1,IMUs2传感器阵列测得的加速度与角速度数据形成集合并输出，经由数据融合单元对多项初始数据进行处理，利用支持度判别，采用加权平均法完成数据融合任务，将融合完成的数据作为当前时刻阵列传感器的输出均值，由数据传输模块将两组惯性测量单元组的输出均值传输到微处理器中，通过数据处理单元完成数据的解算以及状态模型的建立，具体可为：
路板正面记为A面，反面记为B面。则在K时刻传感器阵列输出8个角速度观测值，分别为：A组：
Figure GDA0004191301330000091
B组：/>
Figure GDA0004191301330000092
Figure GDA0004191301330000093
和/>
Figure GDA0004191301330000094
是电路板上同一点的正反两面安装的传感器输出的观测值；
针对两个观测值集合
Figure GDA0004191301330000095
和/>
Figure GDA0004191301330000096
引入支持度概念，若/>
Figure GDA0004191301330000097
和/>
Figure GDA0004191301330000098
相差较大，则支持度低；相反，支持度高；
针对两个观测值集合
Figure GDA0004191301330000099
和/>
Figure GDA00041913013300000910
引入支持度概念，若/>
Figure GDA00041913013300000911
和/>
Figure GDA00041913013300000912
相差较大，则支持度低；相反，支持度高；
对于传感器两个测量值
Figure GDA00041913013300000913
和/>
Figure GDA00041913013300000914
对于/>
Figure GDA00041913013300000915
的支持度rii'可表示为
Figure GDA00041913013300000916
其中0<rii'≤1，i＝1,2,3,4；
设定阈值R，若rii'>R，说明传感器观测值相差较小，可以利用加权平均算法对观测值初步融合求得角速度
Figure GDA00041913013300000917
若rii'＜R，则说明测量数据有较大偏差，则求得A,B两组输出观测值之间的支持度矩阵R,R'
Figure GDA00041913013300000918
则A组的第i个传感器测量数据的综合支持度
Figure GDA00041913013300000919
B组第i个传感器测量数据的综合支持度/>
Figure GDA00041913013300000920
数据综合支持度越大，则表明它与其余测量数据一致性越高；反之，数据一致性越低。选取综合支持度最高的一组数据用加权平均算法求得角速度
Figure GDA00041913013300000921
同理，可得加速度/>
Figure GDA00041913013300000922
通过融合处理后的传感器数据建立联合导航状态模型，利用零速更新下的卡尔曼滤波器反馈完成联合导航状态的纠正以及利用约束最小二乘法将不满足约束条件的状态向量重新解算。弥补了行走过程中误差累积造成的系统的不稳定，从而确保了该装置的高精度导航功能。
本发明还公开了一种基于MEMS惯性器件的穿戴式导航方法，其步骤为：
(1)单组IMU架构导航状态及误差补偿；
建立单组IMU架构导航状态模型，根据静止探测器对载体运动状态的判断，分为运动与静止状态下的两类不同的状态模型，并在静止状态下对状态误差进行估计；状态误差估计结果返回系统中对导航状态量进行修正；
(2)基于双脚四种组合状态下的联合导航状态模型的建立；
双脚两组IMU架构可视为相互独立对象，针对左右双脚各自是否处于运动情况选取对应状态下的单脚状态模型，并组合建立联合导航状态模型；
从而搭建完成四种组合状态下的联合导航状态模型和组合误差模型；
(3)零速修正卡尔曼滤波；
利用零速探测器对测试值进行统计，当一个或两个系统的统计量小于阈值，则假设其处于静止状态；当零速探测器检测到单脚或双脚处于“零速”状态，则对联合导航状态下的误差量进行估计并反馈至状态量，完成导航状态的初步修正；
(4)建立并应用空间距离约束方法以优化联合导航解算；
利用双脚间最大距离作为约束条件，当不符合约束条件时，将导航状态量投射至满足约束条件的子空间，利用约束最小二乘法求得满足约束条件的导航状态量，并作为结果输出。
具体为：
步骤(1)对两组IMU架构导航状态分析，具体步骤为：
(b)建立好单个IMU架构的状态方程；
惯性导航系统依据牛顿惯性原理，将加速度计、陀螺仪等惯性敏感元件测得的数据与初始位置信息结合以解算出此时系统的位置、速度和姿态信息；即当系统工作时，可以通过测量载体运动时的加速度以及角速度信息，通过积分运算求得导航数据；
针对惯性测量单元阵列，借助SINS导航算法将当前测量的加速度和角速度信息解算为载体此时位置，速度和姿态的状态信息；建立单个IMU架构的导航状态向量xk＝[pk vkqk]T，pk，vk，qk分别代表K时刻载体的位置，速度以及四元数信息。
Figure GDA0004191301330000101
fk，wk分别表示比力和角速度，即加速度计和陀螺仪的测量值，g＝[0 0 g]T表示重力，Ω(·)表示四元数更新矩阵；单个IMU架构的导航状态向量xk＝[pk vk qk]T，pk，vk，qk分别代表K时刻载体的位置，速度以及四元数信息，
随着导航系统的推演，加速度计和陀螺仪测量值存在的微小的误差会在积分运算的作用下快速增加，导致惯导系统不能够长时间工作；这里使用零速修正算法，通过将误差模型化后估算误差状态，再经由卡尔曼滤波器对误差进行处理；
所述步骤(1)中静止探测器的建立：
对测试值进行统计，当一个或两个系统的统计量小于阈值，则假设其处于静止状态；
建立姿态最优(SHOE)检测器方程：
Figure GDA0004191301330000111
(fk,ωk)W：表示在某个时间窗口W下的比力和角速度测量值；
Figure GDA0004191301330000112
表示在窗口w下比力的平均值；
σf，σω：表示测量误差标准偏差；
将该检测方程作为静止探测器使用，当给出零速检测阈值γ并满足条件T{(fk,ωk)W}≤γ，那么IMU架构处于静止状态的假设成立；
所述步骤(1)中误差模型的建立：
惯导系统误差的状态向量取为Δxk＝[Δpk Δvk Δθk]T，分别表示位置误差，速度误差和四元数误差。则一阶误差模型建立如下：
Δxk＝FK-1Δxk-1+Gk-1wk-1
Figure GDA0004191301330000113
Fk-1，Gk-1：分别表示状态转移矩阵和噪声增益矩阵；
量测值取为静止状态下解算的速度误差Δvk，测量矩阵H＝[03 I3 03]，测量方程如下：
yk＝HΔxk+nk；
nk为测量噪声，采用自适应算法求取，以提高滤波精度；而速度误差Δvk只有在系统处于静止状态下才可以获取；所以当静止探测器检测为运动状态时，卡尔曼滤波器只做时间上的更新；当检测为静止状态时，卡尔曼滤波器进行完整更新，并且将估计结果返回系统中；
所述步骤(1)中静止状态下的载体状态模型及误差模型如下：
Figure GDA0004191301330000121
Δxk＝Fk-1,stillΔxk-1+Gk-1,stillwk-1；
Figure GDA0004191301330000122
Fk-1,still，Gk-1,still：分别表示静止状态下的状态转移矩阵和噪声增益矩阵。
所述步骤(2)中联合导航状态和误差状态讨论如下：
双脚两组IMU架构视为相互独立对象，方便区分使用上标“1”和“2”各代表左脚和右脚处固联的惯性导航系统；因为采用了静态测试对导航状态进行锁定，所以联合导航状态将在四种不同的情况下单独讨论；
情况1：左右两组IMU架构都处于运动状态，则联合状态模型为：
Figure GDA0004191301330000123
两组惯性测量系统视为相互独立，所以误差状态方程建立如下：
Δxk＝Fk-1,jointΔxk-1+Gk-1,jointwk-1；
Figure GDA0004191301330000124
情况2：左右两组IMU架构都处于静止状态，则联合状态模型为：
Figure GDA0004191301330000125
误差状态方程为：
Figure GDA0004191301330000126
情况3：左边惯性测量单元组处于静止状态，右边惯性测量单元组处于运动状态，则联合状态模型和误差状态方程：
Figure GDA0004191301330000127
Figure GDA0004191301330000128
情况4：左边惯性测量单元组处于运动状态，右边惯性测量单元组处于静止状态，则联合状态模型和误差状态方程：
Figure GDA0004191301330000131
Figure GDA0004191301330000132
后面三种情况下的静止状态，其中的过程噪声Wk-1中的协方差矩阵Qk-1,joint中对应航向过程噪音部分需要设置为0。
所述步骤(4)中，利用双脚间最大距离的条件约束，可以将两个导航系统的信息进行融合并显著提高导航性能；
将左右两脚上的脚踏式惯性测量系统的状态分别设为
Figure GDA0004191301330000133
和/>
Figure GDA0004191301330000134
系统的联合导航状态量为/>
Figure GDA0004191301330000135
假设行人左右脚之间的最大距离为L，则两个系统之间的范围约束式可以表达为
||DXk||2≤l2，D＝[I3 03×7 -I3 03×7]；
通过使用约束最小二乘法求得联合导航状态量即可完成约束。
对联合导航解施加约束的方法如下：
如果满足约束条件，将联合导航状态量输出；
如果不满足条件，即||DXk||2＞l2，将联合导航解投射到子空间{X∈R:||DX||2≤l2}，定义投影为：
Figure GDA0004191301330000136
Figure GDA0004191301330000137
/>
Pk为联合导航解
Figure GDA0004191301330000138
的协方差；此投影可以解决不等式约束加权最小二问题，解为拉格朗日函数的一个驻点；
不等式约束加权最小二乘问题的解是拉格朗日函数的一个静止点，引入拉格朗日函数：
Figure GDA0004191301330000139
Figure GDA00041913013300001310
λ是拉格朗日乘子，即满足约束条件下求解拉格朗日函数的最小值；
则拉格朗日函数的驻点可由以下方程解求得：
Figure GDA0004191301330000141
如果约束最小二乘问题有唯一解，对应于最小二乘问题解的拉格朗日函数驻点是λ>0条件下的唯一驻点，该驻点表示为{λ*,X*}；
Figure GDA0004191301330000142
Figure GDA0004191301330000143
Figure GDA0004191301330000144
上面方程是λ的非线性多项式函数，使用牛顿法找到其根，得到λ，求解出联合导航解：
Figure GDA0004191301330000145
联合导航解的协方差
Figure GDA0004191301330000146
可以近似为：
Figure GDA0004191301330000147
Figure GDA0004191301330000148
是投影函数/>
Figure GDA0004191301330000149
的雅可比矩阵；
得到最优的导航信息。
本发明首先由融合处理后的惯性测量单元输出值对联合导航状态量进行更新，其次利用零速探测器对测试值进行统计，当一个或两个惯导系统的统计量小于阈值，则假设其处于静止状态，并对该部分进行卡尔曼增益的计算，求出误差估计量以及后验协方差，完成导航状态的初步修正；
最后，判断脚与脚之间的距离是否满足最大约束条件，满足，则输出联合导航状态量；若不满足，将联合导航解投射到满足约束条件的子空间进行重新解算，求出最优导航解并输出。
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于MEMS惯性器件的穿戴式导航方法，其步骤为：
(1)单组IMU架构导航状态及误差补偿；
建立单组IMU架构导航状态模型，根据静止探测器对载体运动状态的判断，分为运动与静止状态下的两类不同的状态模型，并在静止状态下对状态误差进行估计；状态误差估计结果返回系统中对导航状态量进行修正；
(2)基于双脚四种组合状态下的联合导航状态模型的建立；
双脚两组IMU架构可视为相互独立对象，针对左右双脚各自是否处于运动情况选取对应状态下的单脚状态模型，并组合建立联合导航状态模型；
从而搭建完成四种组合状态下的联合导航状态模型和组合误差模型；
(3)零速修正卡尔曼滤波；
利用零速探测器对测试值进行统计，当一个或两个系统的统计量小于阈值，则假设其处于静止状态；当零速探测器检测到单脚或双脚处于“零速”状态，则对联合导航状态下的误差量进行估计并反馈至状态量，完成导航状态的初步修正；
(4)建立并应用空间距离约束方法以优化联合导航解算；
利用双脚间最大距离作为约束条件，当不符合约束条件时，将导航状态量投射至满足约束条件的子空间，利用约束最小二乘法求得满足约束条件的导航状态量，并作为结果输出；
步骤（1）对两组IMU架构导航状态分析，具体步骤为：
(a)建立好单个IMU架构的状态方程；
惯性导航系统依据牛顿惯性原理，将加速度计和陀螺仪惯性敏感元件测得的数据与初始位置信息结合以解算出此时系统的位置、速度和姿态信息；即当系统工作时，通过测量载体运动时的加速度以及角速度信息，通过积分运算求得导航数据；
针对惯性测量单元阵列，借助SINS导航算法将当前测量的加速度和角速度信息解算为载体此时位置，速度和姿态的状态信息；建立单个IMU架构的导航状态向量
Figure QLYQS_1
，
Figure QLYQS_2
，/>
Figure QLYQS_3
，/>
Figure QLYQS_4
分别代表K时刻载体的位置，速度以及四元数信息；
则单个IMU架构的状态方程表示为：
Figure QLYQS_5
，/>
Figure QLYQS_6
，
Figure QLYQS_7
分别表示比力和角速度，即加速度计和陀螺仪的测量值，/>
Figure QLYQS_8
表示重力，/>
Figure QLYQS_9
表示四元数更新矩阵；
使用零速修正算法，通过将误差模型化后估算误差状态，再经由卡尔曼滤波器对误差进行处理；
（b）构建静止探测器；
对测试值进行统计，当系统的统计量低于阈值，则假设其处于静止状态；
建立姿态最优检测器方程：
Figure QLYQS_10
；
Figure QLYQS_11
：表示在某个时间窗口W下的比力和角速度测量值；
Figure QLYQS_12
：表示在窗口w下比力的平均值；
Figure QLYQS_13
，/>
Figure QLYQS_14
：表示测量误差标准偏差；
将该检测方程作为静止探测器使用，当给出零速检测阈值
Figure QLYQS_15
并满足以下条件/>
Figure QLYQS_16
；
则IMU架构处于静止状态的假设成立；
（c）零速修正算法；
静止探测器与ZUPT探测器使用相同的判别结构，因此静止检测同样能够触发零速修正算法；设计零速修正卡尔曼滤波器，惯导系统误差的状态向量取为
Figure QLYQS_17
，分别表示位置误差，速度误差和四元数误差；则一阶误差模型建立如下：/>
Figure QLYQS_18
；
Figure QLYQS_19
；/>
Figure QLYQS_20
；
Figure QLYQS_21
，/>
Figure QLYQS_22
：分别表示状态转移矩阵和噪声增益矩阵；
量测值取为静止状态下解算的速度误差
Figure QLYQS_23
，测量矩阵/>
Figure QLYQS_24
，测量方程如下：
Figure QLYQS_25
，
Figure QLYQS_26
为测量噪声，采用自适应算法求取，以提高滤波精度，速度误差/>
Figure QLYQS_27
只有在系统处于静止状态下才获取；所以当静止探测器检测为运动状态时，卡尔曼滤波器只做时间上的更新；当检测为静止状态时，卡尔曼滤波器进行完整更新，并且将估计结果返回系统中；
（d）更新载体处于静止状态时的状态方程；
对于双IMU架构系统，并不是所有的误差状态都可观察的；在连续的零速修正过程中，系统和可观测矩阵如下：
Figure QLYQS_28
，/>
Figure QLYQS_29
，/>
Figure QLYQS_30
；
速度误差，翻滚角误差和俯仰角误差是可观测的，而位置误差和航向角误差是不可观测的；
在静止状态下以上参量仍然会有值产生，相较于理论上的无输入值，即产生了误差；针对这些参量，可观测参量误差通过卡尔曼滤波器进行修正，而不可观测参量误差则需要锁定，保证“零速”状态期间不会产生漂移影响整个导航系统正常工作；
对于位置误差，将变化量
Figure QLYQS_31
设为0，可完成对位置的锁定；对于航向角误差，不能直接将航向角变化量独立出来，需要减去对航向角影响最大的垂直于水平面的角速度分量，完成对航向角的锁定；
Figure QLYQS_32
；；
静止状态下的载体状态方程及误差模型如下：
Figure QLYQS_33
，/>
Figure QLYQS_34
，
Figure QLYQS_35
，/>
Figure QLYQS_36
，
Figure QLYQS_37
，/>
Figure QLYQS_38
：分别表示静止状态下的状态转移矩阵和噪声增益矩阵。
2.如权利要求1所述的一种基于MEMS惯性器件的穿戴式导航方法，其特征在于：
步骤（2）建立好联合导航状态模型以及误差模型；
双脚两组IMU架构视为相互独立对象，方便区分使用上标“1”和“2”各代表左脚和右脚处固联的惯性导航系统；因为采用了静态测试对导航状态进行锁定，所以联合导航状态将在四种不同的情况下单独讨论；
情况1：左右两组IMU架构都处于运动状态，则联合状态模型为：
Figure QLYQS_39
，
两组惯性测量系统视为相互独立，所以误差状态方程建立如下：
Figure QLYQS_40
，/>
Figure QLYQS_41
，/>
Figure QLYQS_42
，
Figure QLYQS_43
；
情况2：左右两组IMU架构都处于静止状态，则联合状态模型为：
Figure QLYQS_44
，
误差状态方程为：
Figure QLYQS_45
；
情况3：左边惯性测量单元组处于静止状态，右边惯性测量单元组处于运动状态，则联合状态模型和误差状态方程：
Figure QLYQS_46
，/>
Figure QLYQS_47
；
情况4：左边惯性测量单元组处于运动状态，右边惯性测量单元组处于静止状态，则联合状态模型和误差状态方程：
Figure QLYQS_48
，/>
Figure QLYQS_49
；
后面三种情况下的静止状态，其中的过程噪声
Figure QLYQS_50
中的协方差矩阵/>
Figure QLYQS_51
中对应航向过程噪音部分需要设置为0，
步骤（3）零速修正卡尔曼滤波器；
利用零速探测器对测试值进行统计，当一个或两个系统的统计量小于阈值，则假设其处于静止状态，因而对该部分进行卡尔曼增益的计算，求出误差估计量并对联合导航状态向量进行纠正，以及计算出后验协方差；
卡尔曼增益：
Figure QLYQS_52
，
误差估计量：
Figure QLYQS_53
，
纠正联合导航状态向量：
Figure QLYQS_54
，，
Figure QLYQS_55
表示导航解通过扰动向量的修正得出真实导航状态；
计算后验协方差：
Figure QLYQS_56
，
完成导航状态的初步修正；
步骤（4）空间范围约束
正常人类在行走的过程中，双脚之间的距离会随着行走姿势的不同而发生改变，但是双脚之间的距离是有上限的，通过对两个导航系统之间距离的最大距离约束对两个导航系统信息融合，求得最优导航解；
（a）建立约束式；
将左右两脚上的脚踏式惯性测量系统的状态分别设为
Figure QLYQS_57
和/>
Figure QLYQS_58
，系统的联合导航状态量为/>
Figure QLYQS_59
；
假设行人左右脚之间的最大距离为L，两个系统之间的范围约束式表达为：
Figure QLYQS_60
，/>
Figure QLYQS_61
，
通过使用约束最小二乘法求得满足约束条件的联合导航状态；
对联合导航解施加约束的方法如下：
如果满足约束条件，将联合导航状态量输出；
如果不满足条件，即
Figure QLYQS_62
，将联合导航解投射到子空间/>
Figure QLYQS_63
定义投影为：
Figure QLYQS_64
，/>
Figure QLYQS_65
，
Figure QLYQS_66
为联合导航解/>
Figure QLYQS_67
的协方差；此投影解决不等式约束加权最小二问题，解为拉格朗日函数的一个驻点；
（b）对不等式约束最小二乘问题求解；
不等式约束加权最小二乘问题的解是拉格朗日函数的一个静止点，引入拉格朗日函数：
Figure QLYQS_68
；
Figure QLYQS_69
，/>
Figure QLYQS_70
是拉格朗日乘子，即满足约束条件下求解拉格朗日函数的最小值；
则拉格朗日函数的驻点可由以下方程解求得：
Figure QLYQS_71
；
如果约束最小二乘问题有唯一解，对应于最小二乘问题解的拉格朗日函数驻点是，λ>0条件下的唯一驻点，该驻点表示为
Figure QLYQS_72
；
Figure QLYQS_73
；
Figure QLYQS_74
；
Figure QLYQS_75
；
上面方程是λ的非线性多项式函数，使用牛顿法找到其根，得到λ，求解出联合导航解：
Figure QLYQS_76
；
联合导航解的协方差
Figure QLYQS_77
近似为：
Figure QLYQS_78
；
Figure QLYQS_79
：是投影函数/>
Figure QLYQS_80
的雅可比矩阵；
得到最优的导航信息。
3.一种基于MEMS惯性器件的穿戴式导航装置，使用权利要求2中所述的基于MEMS惯性器件的穿戴式导航方法，其特征在于：包括数据测量模块，微控制器、蓝牙模块、PC机和存储器，所述数据测量模块和存储器与微控制器电连接，所述微控制器与PC机通过蓝牙连接，所述数据测量模块包括固联于左右双脚上的IMU测量阵列，所述IMU测量阵列包括电路板，所述电路板正反两面均以田字形排布，均匀、对称地分布有4个惯性测量单元；
所述每个惯性测量单元均由三轴加速度计，三轴陀螺仪以及磁力计组成；
IMU测量阵列输出的数据进行支持度方法测试，再采用加权平均算法完成数据融合得出导航数据。
CN201811177819.XA 2018-10-10 2018-10-10 一种基于mems惯性器件的穿戴式导航装置和方法 Active CN109099913B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811177819.XA CN109099913B (zh) 2018-10-10 2018-10-10 一种基于mems惯性器件的穿戴式导航装置和方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811177819.XA CN109099913B (zh) 2018-10-10 2018-10-10 一种基于mems惯性器件的穿戴式导航装置和方法

Publications (2)

Publication Number Publication Date
CN109099913A CN109099913A (zh) 2018-12-28
CN109099913B true CN109099913B (zh) 2023-07-07

Family

ID=64868328

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811177819.XA Active CN109099913B (zh) 2018-10-10 2018-10-10 一种基于mems惯性器件的穿戴式导航装置和方法

Country Status (1)

Country Link
CN (1) CN109099913B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110221302B (zh) * 2019-05-24 2023-04-18 上海高智科技发展有限公司 环境探测装置及其修正方法、系统、便携设备及存储介质
CN110426033A (zh) * 2019-07-30 2019-11-08 上海理工大学 基于松耦合imu阵列导航系统的时间同步算法
CN110926464B (zh) * 2019-12-11 2020-10-16 中国人民解放军海军潜艇学院 一种基于双模式的惯性导航方法及系统
CN111121820B (zh) * 2019-12-16 2022-04-01 南京理工大学 基于卡尔曼滤波的mems惯性传感器阵列融合方法
CN111728618A (zh) * 2020-05-21 2020-10-02 深圳市千手千眼科技有限公司 一种人员定位人体运动步态检测方法
CN112066980B (zh) * 2020-08-31 2022-09-27 南京航空航天大学 一种基于人体四节点运动约束的行人导航定位方法
CN112857394A (zh) * 2021-01-05 2021-05-28 广州偶游网络科技有限公司 智能鞋及其动作识别方法、装置、存储介质
CN113390416A (zh) * 2021-06-15 2021-09-14 浙江奥新智能科技有限公司 一种单基站室内定位系统及定位方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102322861A (zh) * 2011-05-31 2012-01-18 电子科技大学 一种航迹融合方法
WO2013160286A1 (en) * 2012-04-23 2013-10-31 Deutsches Zentrum für Luft- und Raumfahrt e.V. Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian
JPWO2013046747A1 (ja) * 2011-09-30 2015-03-26 富士通株式会社 観測情報処理装置、観測情報処理プログラムおよび観測情報処理方法
CN106690637A (zh) * 2016-12-12 2017-05-24 李新亚 太空鞋磁系统
CN107218938A (zh) * 2017-05-22 2017-09-29 南京航空航天大学 基于人体运动模型辅助的穿戴式行人导航定位方法和设备

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102322861A (zh) * 2011-05-31 2012-01-18 电子科技大学 一种航迹融合方法
JPWO2013046747A1 (ja) * 2011-09-30 2015-03-26 富士通株式会社 観測情報処理装置、観測情報処理プログラムおよび観測情報処理方法
WO2013160286A1 (en) * 2012-04-23 2013-10-31 Deutsches Zentrum für Luft- und Raumfahrt e.V. Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian
CN106690637A (zh) * 2016-12-12 2017-05-24 李新亚 太空鞋磁系统
CN107218938A (zh) * 2017-05-22 2017-09-29 南京航空航天大学 基于人体运动模型辅助的穿戴式行人导航定位方法和设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种阵列式传感器数据融合方法的研究;杜胜雪 等;《中国科技论文》;第9卷(第07期);第794-797页 *
基于不等式约束卡尔曼滤波的双MIMU导航位置校正方法;时伟等;《中国惯性技术学报》;第25卷(第1期);第11-16页 *

Also Published As

Publication number Publication date
CN109099913A (zh) 2018-12-28

Similar Documents

Publication Publication Date Title
CN109099913B (zh) 一种基于mems惯性器件的穿戴式导航装置和方法
Ludwig et al. Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
Rohac et al. Calibration of low-cost triaxial inertial sensors
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
US8756995B2 (en) Device and method for combining samples from an inertial measurement sensor cluster
CN111024075B (zh) 一种结合蓝牙信标和地图的行人导航误差修正滤波方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN111121773B (zh) 一种mems惯性测量组合
CN107246872B (zh) 基于mems传感器和vlc定位融合的单粒子滤波导航装置和方法
Woyano et al. Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN112362057A (zh) 基于零速修正与姿态自观测的惯性行人导航算法
KR101301462B1 (ko) 저가형 관성 센서를 이용한 보행자 관성 항법 장치 및 그 항법
Montorsi et al. Design and implementation of an inertial navigation system for pedestrians based on a low-cost MEMS IMU
Geneva et al. Versatile 3d multi-sensor fusion for lightweight 2d localization
CN113532477A (zh) 一种骑行码表设备及骑行码表初始姿态自动校准方法
CN109612464B (zh) 基于iez框架下的多算法增强的室内导航系统及方法
CN116817905A (zh) 一种可穿戴多mems准实时协同导航系统及方法
CN115523919A (zh) 一种基于陀螺漂移优化的九轴姿态解算方法
CN112284388B (zh) 一种无人机多源信息融合导航方法
CN113029173A (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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20230523

Address after: B814, Key Laboratory Platform Building, Virtual University Park, No. 1 Yuexing Second Road, High tech Zone Community, Yuehai Street, Nanshan District, Shenzhen City, Guangdong Province, 518000

Applicant after: Gewu Zhihang (Shenzhen) Technology Co.,Ltd.

Address before: 518000 Changfu Jinmao Building, Shihua Road, Fubao Street, Futian District, Shenzhen City, Guangdong Province

Applicant before: GEWU SENSING (SHENZHEN) TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant