[发明专利]一种基于双MEMS-IMU的行人自主导航解算算法

说明书

技术领域：

本发明涉及的是一种导航解算算法，特别是涉及一种基于双MEMS-IMU（微机械系统-惯性测量单元）的行人自主导航解算算法。 

背景技术：

近年来，随着国内外MEMS惯性器件精度的提高，使得利用捷联惯性导航系统解算算法来进行行人航位推算成为可能，特别是利用捷联惯性导航解算算法可以提供更完备的导航信息。但是即便如此，若长时间工作，MEMS惯性器件误差还是会比较严重的发散，捷联惯性导航解算算法得到的行人航位推算结果验证了如果导航期间MEMS惯性器件误差不能得到有效补偿，位置误差会以时间三次方的趋势发散，系统将最终丧失导航功能。因此，捷联惯性导航解算算法应用于行人自主导航系统的最大难点在于设计有效的误差修正算法。 

现有的导航解算算法，主要以捷联惯性导航解算算法为基础，采用零速校正等误差补偿算法对导航结果进行实时修正。基于零速校正的误差补偿算法大都存在零速检测不准确、检测结果滞后、零速校正时间短等缺陷，虽然可以在一定范围内提高行人自主导航系统的导航精度，但是导航定位误差仍然较大，且仅能在短时间内使用。总而言之，现有的导航解算算法准确性差，难以满足行人自主导航精确可靠的要求。 

发明内容：

本发明的目的在于克服现有技术的不足，提供一种基于双MEMS-IMU的行人自主导航解算算法。 

为了解决背景技术所存在的问题，本发明采用以下技术方案： 

一种基于双MEMS-IMU的行人自主导航解算算法,它包括如下步骤： 

步骤一：将基于双MEMS-IMU的行人自主导航系统中的两个IMU系统分别固定于行人的两只脚上，手持PDA实时接收并存储行人运动时两个IMU系统输出的量测信息； 

步骤二：使用步骤一中存储的行人自主导航系统输出数据，利用捷联惯性导航系统导航解算方法求出任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态

步骤三：使用零速检测算法检测到IMU为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为： 

δXk=FδXk-1+Wk-1Zk=HXk+Nk]]>

得到修正后的IMU状态两个IMU独立进行零速校正； 

步骤四：利用步骤三中估计出的双MEMS-IMU导航系统导航状态及最大步长不等式，判断IMU输出是否满足最大步长约束，若不满足约束则执行步骤五，若满足约束则返回步骤三； 

步骤五：利用公式求取将不满足约束条件的双MEMS-IMU行人自主导航系统导航解算输出映射到满足映射条件的范围内的映射方程

步骤六：若双MEMS-IMU系统的导航解算结果不满足步骤四中的最大步长不 等式约束条件，则利用公式将此时行人自主导航系统导航状态的估计值约束到子空间{x∈R^m:||L·x||²≤γ²}中，得到双MEMS-IMU行人自主导航系统状态约束值 

步骤七：利用公式计算经状态约束后的双MEMS-IMU行人自主导航系统导航解算结果的协方差阵，以更新卡尔曼滤波的协方差阵； 

步骤八：构造卡尔曼滤波动态误差修正模型，利用行人自主导航系统导航解算联合误差传播特性方程： 

δx_k＝F_kδx_k-1+G_kw_k

得到行人自主导航参数的最优估计值。