一种基于双mems-imu的行人自主导航解算算法
【专利摘要】本发明公开了一种基于双MEMS-IMU的行人自主导航解算算法，将两个IMU系统同时固联于行人导航系统使用者的两只脚上，双系统分别进行捷联惯导解算算法和基于卡尔曼滤波的零速修正算法，再融合两只脚的定位信息，当双脚解算距离超过两脚间最大步长γ时，采用状态约束卡尔曼滤波算法对两个IMU的导航结果进行不等式约束，将模糊的人体生理特性问题转化为严格的数学问题，从而得到导航结果的最优估计，实现了更高精度的行人导航定位功能。
【专利说明】—种基于双MEMS-1MU的行人自主导航解算算法
【技术领域】:
[0001]本发明涉及的是一种导航解算算法，特别是涉及一种基于双MEMS-1MU (微机械系统-惯性测量单元)的行人自主导航解算算法。
【背景技术】:
[0002]近年来，随着国内外MEMS惯性器件精度的提高，使得利用捷联惯性导航系统解算算法来进行行人航位推算成为可能，特别是利用捷联惯性导航解算算法可以提供更完备的导航信息。但是即便如此，若长时间工作，MEMS惯性器件误差还是会比较严重的发散，捷联惯性导航解算算法得到的行人航位推算结果验证了如果导航期间MEMS惯性器件误差不能得到有效补偿，位置误差会以时间三次方的趋势发散，系统将最终丧失导航功能。因此，捷联惯性导航解算算法应用于行人自主导航系统的最大难点在于设计有效的误差修正算法。
[0003]现有的导航解算算法，主要以捷联惯性导航解算算法为基础，采用零速校正等误差补偿算法对导航结果进行实时修正。基于零速校正的误差补偿算法大都存在零速检测不准确、检测结果滞后、零速校正时间短等缺陷，虽然可以在一定范围内提高行人自主导航系统的导航精度，但是导航定位误差仍然较大，且仅能在短时间内使用。总而言之，现有的导航解算算法准确性差，难以满足行人自主导航精确可靠的要求。
【发明内容】
:
[0004]本发明的目的在于克服现有技术的不足，提供一种基于双MEMS-MU的行人自主导航解算算法。`
[0005]为了解决【背景技术】所存在的问题，本发明采用以下技术方案:
[0006]一种基于双MEMS-MU的行人自主导航解算算法，它包括如下步骤:
[0007]步骤一:将基于双MEMS-MU的行人自主导航系统中的两个MU系统分别固定于行人的两只脚上，手持PDA实时接收并存储行人运动时两个IMU系统输出的量测信息；
[0008]步骤二:使用步骤一中存储的行人自主导航系统输出数据，利用捷联惯性导航系统导航解算方法求出任意k时刻基于双MEMS-MU的行人自主导航系统中每个IMU系统的
状态对);
[0009]步骤三:使用零速检测算法检测到頂U为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为:
=FSX1 +1\ ,
[0010]\7 υν \:
[Zk =HXk+Nk
[0011]得到修正后的頂U状态if ,两个頂U独立进行零速校正；
[0012]步骤四:利用步骤三中估计出的双MEMS-MU导航系统导航状态及最大步长不等式，判断IMU输出是否满足最大步长约束，若不满足约束则执行步骤五，若满足约束则返回步骤三；
[0013]步骤五:利用公式求取将不满足约束条件的双MEMS-MU行人自主导航系统导航解算输出映射到满足映射条件的范围内的映射方程Mi);
[0014]步骤六:若双MEMS-1MU系统的导航解算结果不满足步骤四中的最大步长不等式约束条件，则利用公式将此时行人自主导航系统导航状态的估计值约束到子空间Ix e Rm:1 IL.XI 12≤Y2}中，得到双MEMS-1MU行人自主导航系统状态约束值Pih)；
[0015]步骤七:利用公式计算经状态约束后的双MEMS-MU行人自主导航系统导航解算结果的协方差阵<，以更新卡尔曼滤波的协方差阵；
[0016]步骤八:构造卡尔曼滤波动态误差修正模型，利用行人自主导航系统导航解算联合误差传播特性方程:
[0017]δ Xk = Fk δ Xk-JGkWk
[0018]得到行人自主导航参数的最优估计值。
[0019]优选的，在所述的步骤一中，任意时刻k接收到的两个IMU输出信息为:
[0020]
【权利要求】
1.一种基于双MEMS-1MU的行人自主导航解算算法，其特征在于，它由以下步骤实现: 步骤一:将基于双MEMS-MU的行人自主导航系统中的两个MU系统分别固定于行人的两只脚上，手持PDA实时接收并存储行人运动时两个IMU系统输出的量测信息； 步骤二:使用步骤一中存储的行人自主导航系统输出数据，利用捷联惯性导航系统导航解算方法求出任意k时刻基于双MEMS-MU的行人自主导航系统中每个IMU系统的状态 步骤三:使用零速检测算法检测到頂U为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为:
2.如权利要求1所述的基于双MEMS-1MU的行人自主导航解算算法，其特征在于，在步骤一中，任意时刻k接收到的两个IMU输出信息为:
3.如权利要求1所述的基于双MEMS-1MU的行人自主导航解算算法，其特征在于，在步骤二中，任意k时刻基于双MEMS-MU的行人自主导航系统中每个IMU系统的状态为:
4.如权利要求1所述的基于双MEMS-1MU的行人自主导航解算算法，其特征在于，在步骤三中，零速校正卡尔曼滤波模型中，δ Xk为被估计状态向量:
5.如权利要求1所述的基于双MEMS-1MU的行人自主导航解算算法，其特征在于，在步骤四中，利用不等式:
6.如权利要求1所述的基于双MEMS-MU的行人自主导航解算算法，其特征在于，在步骤五中，利用公式
7.如权利要求1所述的基于双MEMS-1MU的行人自主导航解算算法，其特征在于，在步骤六中，利用公式:
8.如权利要求1所述的基于双MEMS-1MU的行人自主导航解算算法，其特征在于，在步骤七中，通过公式:
9.如权利要求1所述的基于双MEMS-1MU的行人自主导航解算算法，其特征在于，在步骤八中，利用双MEMS-1MU导航系统的导航解算联合误差传播特性方程:
.5 Xk=Fk 5 Xk-1+GkWk 得到行人自主导航系统状态的最优估计值； 其中，δ Xk为IMU导航系统的导航解算联合误差:
【文档编号】G01C21/16GK103776446SQ201310520233
【公开日】2014年5月7日 申请日期:2013年10月29日 优先权日:2013年10月29日
【发明者】于飞, 于春阳, 兰海钰, 周广涛, 刘凤, 赵博, 李佳璇, 孙艳涛, 郝勤顺, 张丽丽, 梁宏 申请人:哈尔滨工程大学