一种基于双MEMSIMU的行人自主导航解算算法.pdf

摘要
申请专利号：	CN201310520233.X	申请日：	2013.10.29
公开号：	CN103776446A	公开日：	2014.05.07
当前法律状态：	终止	有效性：	无权
法律详情：	未缴年费专利权终止IPC(主分类):G01C 21/16申请日:20131029授权公告日:20170104终止日期:20171029\|\|\|授权\|\|\|实质审查的生效IPC(主分类):G01C 21/16申请日:20131029\|\|\|公开
IPC分类号：	G01C21/16	主分类号：	G01C21/16
申请人：	哈尔滨工程大学
发明人：	于飞; 于春阳; 兰海钰; 周广涛; 刘凤; 赵博; 李佳璇; 孙艳涛; 郝勤顺; 张丽丽; 梁宏
地址：	150001 黑龙江省哈尔滨市南通大街145号
优先权：
专利代理机构：	北京科亿知识产权代理事务所(普通合伙) 11350	代理人：	汤东凤
PDF下载：	PDF下载

内容摘要

本发明公开了一种基于双MEMS-IMU的行人自主导航解算算法，将两个IMU系统同时固联于行人导航系统使用者的两只脚上，双系统分别进行捷联惯导解算算法和基于卡尔曼滤波的零速修正算法，再融合两只脚的定位信息，当双脚解算距离超过两脚间最大步长γ时，采用状态约束卡尔曼滤波算法对两个IMU的导航结果进行不等式约束，将模糊的人体生理特性问题转化为严格的数学问题，从而得到导航结果的最优估计，实现了更高精度的行人导航定位功能。

权利要求书

权利要求书
1.  一种基于双MEMS-IMU的行人自主导航解算算法，其特征在于，它由以下步骤实现：
步骤一：将基于双MEMS-IMU的行人自主导航系统中的两个IMU系统分别固定于行人的两只脚上，手持PDA实时接收并存储行人运动时两个IMU系统输出的量测信息；
步骤二：使用步骤一中存储的行人自主导航系统输出数据，利用捷联惯性导航系统导航解算方法求出任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态
步骤三：使用零速检测算法检测到IMU为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为：

得到修正后的IMU状态两个IMU独立进行零速校正；
步骤四：利用步骤三中估计出的双IMU导航系统导航状态及最大步长不等式，判断IMU输出是否满足最大步长约束，若不满足则执行步骤五，若满足约束则返回步骤三；
步骤五：利用公式求取将不满足约束条件的双MEMS-IMU行人自主导航系统的导航解算输出映射到满足映射条件的范围内的映射方程
步骤六：利用公式将不满足步骤四中最大步长不等式约束的双MEMS-IMU系统导航状态的估计值约束到子空间{x∈Rm:||L·x||2≤γ2}中，得到双MEMS-IMU行人自主导航系统状态约束值
步骤七：利用公式计算经状态约束后的双MEMS-IMU行人自主导航系统导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵；
步骤八：构造卡尔曼滤波动态误差修正模型，利用基于双MEMS-IMU的行人自主导航系统的导航解算联合误差传播特性方程：
δxk=Fkδxk-1+Gkwk
得到行人自主导航参数的最优估计值。

2.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤一中，任意时刻k接收到的两个IMU输出信息为：

其中，i＝1、2，表示双系统中的IMU1系统、IMU2系统；为MEMS三轴陀螺仪输出的角速率信息；为MEMS三轴加速度计输出的比力信息；T表示转置操作。

3.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤二中，任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态为：

其中，v、s、三维位置向量、速度向量、姿态向量，即

4.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤三中，零速校正卡尔曼滤波模型中，δXk为被估计状态向量：
δX＝[ΦT δωT δrT δvT δaT]
其中，φT为姿态误差、δωT为陀螺漂移、δrT为位置误差、δvT为速度误差、δaT为加速度计输出误差，以上每一项都是三维的；
Wk-1为系统噪声序列；量测量Zk为IMU静止时，双MEMS-IMU输出量经惯性导航解算算法得到的速度分量；Nk为量测噪声序列；H为量测阵：
H＝[0 0 I 0 0]T
F为状态转移矩阵:

式中，I(0)为单位(零)阵；为沿地理系的载体运动加速度的反对称阵；为载体坐标系相对于导航坐标系的状态转移矩阵。

5.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤四中，利用不等式：
||L·xk||2≤γ2
判断双MEMS-IMU系统输出是否满足约束条件，若k时刻导航估计状态存在则执行步骤五，若k时刻导航估计状态满足不等式||L·xk||2≤γ2,则返回步骤三；
其中，γ为行人自主导航系统使用者行走或跑步时双脚间的最大步长；k∈N+；L=[I3O3,n1-I3O3,n2]T,I表示单位阵，O表示零阵；||·||表示求范数；xk为k时刻两个IMU导航系统真实状态的联合，为k时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息，

6.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤五中，利用公式

求取映射方程
其中，

式中，为满秩的；（x*,λ*）为拉格朗日方程：

的驻点；
其中，λ为拉格朗日乘子，λ＞0。

7.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤六中，利用公式：

将当前时刻不满足约束条件的行人自主导航系统导航状态的估计值约束到子空间{x∈Rm:||L·x||2≤γ2}中，得到满足最大步长约束限制的双MEMS-IMU系统状态约束值则为满足最大步长约束限制的状态量；
其中，Pk表示双MEMS-IMU导航系统状态估计值的协方差矩阵。

8.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤七中，通过公式：

计算状态约束后基于双MEMS-IMU的行人自主导航的导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵；
其中，▽p为约束函数p(x)围绕原始状态估计的雅可比矩阵，即：

式中，表示时p(x)对[x]m的导数。

9.  如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤八中，利用双MEMS-IMU导航系统的导航解算联合误差传播特性方程：
δxk=Fkδxk-1+Gkwk
得到行人自主导航系统状态的最优估计值；
其中，δxk为双IMU导航系统的导航解算联合误差：

基于双MEMS-IMU的行人自主导航系统导航解算联合误差的状态转移矩阵为：

基于双MEMS-IMU的行人自主导航系统导航解算联合误差的噪声驱动阵为：

基于双MEMS-IMU的行人自主导航系统导航解算联合误差的噪声为：

式中，为基于双MEMS-IMU的行人自主导航系统导航解算误差，i(i=1,2)表示第i个IMU导航系统，且满足：

其中，表示第i个IMU导航系统的状态转移矩阵；表示第i个IMU导航系统的噪声驱动阵；
其中，表示过程噪声，为零均值高斯白噪声，其方差为Qi：

系统量测更新模型表示为：

其中，

式中，H=[03,3I303,3]；和η(i)用于判断第i个IMU是否满足触发零速校正卡尔曼滤波器的条件，如果则表示第i个IMU不满足触发零速校正的条件；反之则表示满足，相应的量测噪声阵可表示为：

说明书

说明书一种基于双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∈Rm:||L·x||2≤γ2}中，得到双MEMS-IMU行人自主导航系统状态约束值
步骤七：利用公式计算经状态约束后的双MEMS-IMU行人自主导航系统导航解算结果的协方差阵，以更新卡尔曼滤波的协方差阵；
步骤八：构造卡尔曼滤波动态误差修正模型，利用行人自主导航系统导航解算联合误差传播特性方程：
δxk＝Fkδxk-1+Gkwk
得到行人自主导航参数的最优估计值。
优选的，在所述的步骤一中，任意时刻k接收到的两个IMU输出信息为：
yki=fkiωkiT]]>
其中，i＝1、2，表示双MEMS-IMU行人自主导航系统中的IMU1系统、IMU2系统；ωki=ωkixωkiyωkizT]]>为MEMS三轴陀螺仪输出的角速率信息； fki=fkixfkiyfkizT]]>为MEMS三轴加速度计输出的比力信息；T表示转置操作。
优选的，在所述的步骤二中，任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态为：

其中，v、s、三维位置向量、速度向量、姿态向量，即
优选的，在所述的步骤三中，零速校正卡尔曼滤波模型中，δXk为被估计状态向量：
δX＝[ΦT δωT δrT δvT δaT]
其中，φT为姿态误差、δωT为陀螺漂移、δrT为位置误差、δvT为速度误差、δaT为加速度计输出误差，以上每一项都是三维的；Wk-1为系统噪声序列；量测量Zk为IMU静止时，其输出量经惯导解算得到的速度分量；Nk为量测噪声序列；H为量测阵：
H＝[0 0 I 0 0]T
F为状态转移矩阵:
F=I-ΔtCbn0000I00000IΔtI0ΔtS(fn)00IΔtCbn0000I]]>
式中，I(0)为单位(零)阵；为沿地理系的载体运动加速度的反对称阵；为载体坐标系相对于导航坐标系的状态转移矩阵。
优选的，在所述的步骤四中，利用不等式：
||L·xk||2≤γ2
判断双IMU输出是否满足约束条件，若k时刻导航估计状态存在则执行步骤五，若k时刻导航估计状态||L·xk||2≤γ2,则返回步骤三；
其中，γ为单兵导航系统使用者行走或跑步时双脚间的最大步长；k∈N+； ,I表示单位阵，O表示零阵；||·||表示求范数；xk为k时刻两个IMU导航系统真实状态的联合，为k时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息，x^k=(x^k(1))T(x^k(2))TT;xk(i)&Element;Rni,x^k(i)&Element;Rni,xk,x^k&Element;Rm(n1+n2=m).]]>
优选的，在所述的步骤五中，利用公式
p(x^)=eqvΠ(λ*)x^k]]>
求取映射方程
其中，
Π(λ*)=def(Pk-1+λ*LTL)-1Pk-1]]>

式中，为满秩的；（x*,λ*）为拉格朗日方程：
J(x,λ)=def|x^k-|x||Pk-12+λψ(x)]]>
的驻点；
其中，λ为拉格朗日乘子，λ＞0。
优选的，在所述的步骤六中，利用公式：
p(x^k)=argminx(||x^k-xk||Pk-12)]]>
将此时行人自主导航系统导航状态的估计值约束到子空间{x∈Rm:||L·x||2≤γ2}中，得到双MEMS-IMU系统最大步长约束限制后的状态约束值则为满足最大步长约束限制的状态量；
其中，Pk表示双导航系统状态估计值的协方差矩阵。
优选的，在所述的步骤七中，通过公式：
Pk*=&dtri;p·P·(&dtri;p)T]]>
计算双MEMS-IMU系统状态约束后的协方差阵以更新卡尔曼滤波的协方差阵；
其中，▽p为约束函数p(x)围绕原始状态估计的雅可比矩阵，即：
&dtri;p=def[&PartialD;p(x)&PartialD;[x]1|x=x^k...&PartialD;p(x)&PartialD;[x]m|x=x^k];]]>
式中，表示时p(x)对[x]m的导数。
优选的，在所述的步骤八中，利用双MEMS-IMU的行人自主导航系统导航解算联合误差传播特性方程：
δxk＝Fkδxk-1+Gkwk
得到行人自主导航系统状态的最优估计值；
其中，δxk为基于双MEMS-IMU的行人自主导航系统导航解算联合误差：
δxk=δxk(1)δxk(2)]]>
基于双MEMS-IMU的行人自主导航系统导航解算联合误差的状态转移矩阵为：
Fk=Fk(1)09,909,9Fk(2)]]>
基于双MEMS-IMU的行人自主导航系统导航解算联合误差的噪声驱动阵为：
Gk=Gk(1)09,609,6Gk(2)]]>
基于双MEMS-IMU的行人自主导航系统导航解算联合误差的噪声为：
wk=(wk(1))T(wk(2))T]]>
式中，为基于双MEMS-IMU的行人自主导航系统导航解算误差，i(i=1,2)表示第i个IMU导航系统，且满足：
δxk(i)=Fk(i)δxk-1(i)+Gk(i)wk(i)]]>
其中，表示第i个IMU导航系统的状态转移矩阵；表示i个IMU导航系统的噪声驱动阵；
其中，表示过程噪声，为零均值高斯白噪声，其方差为Qi：
Q=Q106,606,6Q2]]>
系统量测更新模型表示为：
-Hx^k=Hδx^k+vk]]>
其中，

式中，H＝[03,3 I3 03,3 ]；和η(i)用于判断第i个IMU是否满足触发零速校正卡尔曼滤波器的条件，如果则表示第i个IMU不满足触发零速校正的条件；如果则表示第i个IMU满足触发零速校正的条件，相应的量测噪声阵可表示为：

本发明对比现有技术有如下的有益效果：通过利用双IMU系统输出信息，使用捷联惯性导航解算算法及零速校正算法对导航结果进行初步估计，对估计后的双IMU定位信息进行信息融合，利用人体运动时存在最大步长γ设计状态约束科尔曼滤波算法，对基于双MEMS-IMU的行人自主导航系统的导航结果进行不等式约束，将模糊的人体生理特性问题转化为严格的数学问题，克服了传统单脚定位时约束条件少、精确度差、系统可使用时间短等不足，在小成本的条件下，提高了检测的精度。本发明方法简单，稳定性和可靠性高，有效的提高了行人自主导航系统的使用精度。
附图说明：
图1是基于双MEMS-IMU的行人自主导航解算原理图。
图2是基于双MEMS-IMU的行人自主导航解算算法中最大步长取值图。
图3是本发明实施例提供的基于双MEMS-IMU的行人自主导航系统实验时导航解算结果图。
具体实施方式：
下面结合附图和具体实施方式对本发明作进一步描述：
图1是基于双MEMS-IMU的行人自主导航解算原理图,图2是基于双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的行人自主导航系统状态及最大步长约束不等式，判断系统解算结果是否满足约束，若不满足则执行步骤五，若满足约束则返回步骤三；
步骤五：利用公式求取将不满足约束条件的系统解算结果映射到满足映射条件的范围内的映射方程
步骤六：若基于双MEMS-IMU的行人自主导航系统导航解算结果不满足步骤四中的不等式最大步长约束，则利用公式将此时行人自主导航系统导航状态的估计值约束到子空间{x∈Rm:||L·x||2≤γ2}中，得到满足最大步长约束限制的导航状态值
步骤七：利用公式计算经状态约束后，行人自主导航系统导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵；
步骤八：构造卡尔曼滤波动态误差修正模型，利用基于双MEMS-IMU的行人自主导航系统的导航解算联合误差传播特性方程：
δxk＝Fkδxk-1+Gkwk
得到基于双MEMS-IMU的行人自主导航系统的导航状态最优估计值。
优选的，在所述的步骤一中，任意时刻k接收到的两个IMU输出信息为：
yki=fkiωkiT]]>
其中，i＝1、2，表示双系统中的IMU1系统、IMU2系统；ωki=ωkixωkiyωkizT]]>为MEMS三轴陀螺仪输出的角速率信息；fki=fkixfkiyfkizT]]>为MEMS三轴加速度计输出的比力信息；T表示转置操作。
优选的，在所述的步骤二中，任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态为：

其中，v、s、三维位置向量、速度向量、姿态向量，即
优选的，在所述的步骤三中，零速校正卡尔曼滤波模型中，δXk为被估计状态向量：
δX＝[ΦT δωT δrT δvT δaT]
其中，φT为姿态误差、δωT为陀螺漂移、δrT为位置误差、δvT为速度误差、δaT为加速度计输出误差，以上每一项都是三维的；Wk-1为系统噪声序列；量测量Zk为IMU静止时，IMU输出量经惯导解算得到的速度分量；Nk为量测噪声序列；H为量测阵：
H＝[0 0 I 0 0]T
F为状态转移矩阵:
F=I-ΔtCbn0000I00000IΔtI0ΔtS(fn)00IΔtCbn0000I]]>
式中，I(0)为单位(零)阵；为沿地理系的载体运动加速度的反对称阵；为载体坐标系相对于导航坐标系的状态转移矩阵。
优选的，在所述的步骤四中，利用不等式：
||L·xk||2≤γ2
判断基于双MEMS-IMU的行人自主导航系统的导航解算状态是否满足约束条件，若k时刻导航估计状态满足不等式则执行步骤五，若k时刻导航估计状态||L·xk||2≤γ2,则返回步骤三；
其中，γ为行人自主导航系统使用者行走或跑步时双脚间的最大步长；k∈N+；I表示单位阵，O表示零阵；||·||表示求范数；xk为k时刻两个IMU导航系统真实状态的联合，为k时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息，x^k=(x^k(1))T(x^k(2))TT;xk(i)&Element;Rni,x^k(i)&Element;Rni,xk,x^k&Element;Rm(n1+n2=m).]]>
优选的，在所述的步骤五中，利用公式
p(x^)=eqvΠ(λ*)x^k]]>
求取映射方程
其中，
Π(λ*)=def(Pk-1+λ*LTL)-1Pk-1]]>
λ*=def{λ&Element;R+:ψ(Π(λ)x^k)=0}]]>
式中，为满秩的；（x*,λ*）为拉格朗日方程：
J(x,λ)=def|x^k-|x||Pk-12+λψ(x)]]>
的驻点；
其中，λ为拉格朗日乘子，λ＞0。
优选的，在所述的步骤六中，利用公式：
p(x^k)=argminx(||x^k-xk||Pk-12)]]>
将当前时刻不满足约束条件的行人自主导航系统导航状态的估计值约束到子空间{x∈Rm:||L·x||2≤γ2}中，得到满足最大步长约束限制的双MEMS-IMU系统状态约束值则为满足最大步长约束限制的状态量；
其中，Pk表示双导航系统状态估计值的协方差矩阵。
优选的，在所述的步骤七中，通过公式：
Pk*=&dtri;p·P·(&dtri;p)T]]>
计算状态约束后基于双MEMS-IMU的行人自主导航的导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵；
其中，▽p为约束函数p(x)围绕原始状态估计的雅可比矩阵，即：
&dtri;p=def[&PartialD;p(x)&PartialD;[x]1|x=x^k...&PartialD;p(x)&PartialD;[x]m|x=x^k];]]>
式中，表示时p(x)对[x]m的导数。
优选的，在所述的步骤八中，利用双MEMS-IMU导航系统的导航解算联合误差传播特性方程：
δxk＝Fkδxk-1+Gkwk
得到行人自主导航系统状态的最优估计值；
其中，δxk为基于双MEMS-IMU的行人自主导航系统的导航解算联合误差：
δxk=δxk(1)δxk(2)]]>
基于双MEMS-IMU的行人自主导航系统导航解算联合误差的状态转移矩阵为：
Fk=Fk(1)09,909,9Fk(2)]]>
基于双MEMS-IMU的行人自主导航系统导航解算联合误差的噪声驱动阵为：
Gk=Gk(1)09,609,6Gk(2)]]>
基于双MEMS-IMU的行人自主导航系统导航解算联合误差的噪声为：
wk=(wk(1))T(wk(2))T]]>
式中，为基于双MEMS-IMU的行人自主导航系统导航解算误差，i(i=1,2)表示第i个IMU导航系统，且满足：
δxk(i)=Fk(i)δxk-1(i)+Gk(i)wk(i)]]>
其中，表示第i个IMU导航系统的状态转移矩阵；表示第i个IMU导航系统的噪声驱动阵；
其中，表示过程噪声，为零均值高斯白噪声，其方差为Qi：
Q=Q106,606,6Q2]]>
系统量测更新模型表示为：
-Hx^k=Hδx^k+vk]]>
其中，

式中，H＝[03,3 I3 03,3 ]；和η(i)用于判断第i个IMU是否满足触发零速校正卡尔曼滤波器的条件，如果则表示第i个IMU不满足触发零速校正的条件；反之则表示满足，相应的量测噪声阵可表示为：

结合以下实验对本发明的优益效果作进一步的说明：
利用两套自研三轴惯性测量组件（集成了微机械系统三轴磁力计、加速度计、陀螺仪）搭建真实双IMU系统行人自主导航系统模型，设备参数如表1所示，通过合理的试验验证基于双MEMS-IMU的行人自主导航系统导航解算最大步长约束算法的可靠性、实用性、准确性，试验场景选在室外空旷的哈尔滨工程大学军工操场，
表1 自研微型惯性测量单元惯性测量组件各传感器性能指标

实验过程中相关参数设置如下：
行人自主导航自主定位系统采样频率：100Hz
最大步长限制γ：0.8m
微机械系统陀螺标准偏差：σa=0.01m/s2
微机械加速度计标准偏差：σg=0.1*pi/180rad/s
初始速度:vn(0)=[0 0 0]T
初始位置坐标:sn(0)=[0 0 0]T
实验开始前，测试者在实验场进行15分钟的系统静止预热，完成系统的初始对准和GPS定位信息的初始化；为了便于获取精确的参考信息，实验中实时采集了GPS定位信息作为真实轨迹的参考。然后围绕长方形足球场进行场地按照预定轨迹走一圈（约90米），行走时间约为2分钟。最后将采集得到的实验数据进行离线分析。
定位结果如图3所示，为了更形象的说明定位误差，表2给出了使用本发明提出的基于双MEMS-IMU的行人自主导航系统最大步长约束导航解算算法的定位结果的均方根误差RMS，其中计算真值为GPS定位信息。在行走时间大于2分钟的情况下定位误差仍然保持在0.7m内，小于行人行走距离的1%。实验证明本发明提出的基于双MEMS-IMU的行人自主导航系统最大步长约束导航解算算法定位结果比较理想，可以满足短时间内行人作战人员的使用要求。
表2 定位误差

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

资源描述

《一种基于双MEMSIMU的行人自主导航解算算法.pdf》由会员分享，可在线阅读，更多相关《一种基于双MEMSIMU的行人自主导航解算算法.pdf（19页珍藏版）》请在专利查询网上搜索。

1、(10)申请公布号 CN 103776446 A (43)申请公布日 2014.05.07 CN 103776446 A (21)申请号 201310520233.X (22)申请日 2013.10.29 G01C 21/16(2006.01) (71)申请人哈尔滨工程大学地址 150001 黑龙江省哈尔滨市南通大街 145 号 (72)发明人于飞于春阳兰海钰周广涛刘凤赵博李佳璇孙艳涛郝勤顺张丽丽梁宏 (74)专利代理机构北京科亿知识产权代理事务所 ( 普通合伙 ) 11350 代理人汤东凤 (54) 发明名称一种基于双 MEMS-IMU 的行人自主导航解算。

2、算法 (57) 摘要本发明公开了一种基于双 MEMS-IMU 的行人自主导航解算算法，将两个 IMU 系统同时固联于行人导航系统使用者的两只脚上，双系统分别进行捷联惯导解算算法和基于卡尔曼滤波的零速修正算法，再融合两只脚的定位信息，当双脚解算距离超过两脚间最大步长时，采用状态约束卡尔曼滤波算法对两个 IMU 的导航结果进行不等式约束，将模糊的人体生理特性问题转化为严格的数学问题，从而得到导航结果的最优估计，实现了更高精度的行人导航定位功能。 (51)Int.Cl. 权利要求书 5 页说明书 11 页附图 2 页 (19)中华人民共和国国家知识产权局 (。

3、12)发明专利申请权利要求书5页说明书11页附图2页 (10)申请公布号 CN 103776446 A CN 103776446 A 1/5 页 2 1. 一种基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，它由以下步骤实现：步骤一：将基于双MEMS-IMU的行人自主导航系统中的两个IMU系统分别固定于行人的两只脚上，手持 PDA 实时接收并存储行人运动时两个 IMU 系统输出的量测信息；步骤二：使用步骤一中存储的行人自主导航系统输出数据，利用捷联惯性导航系统导航解算方法求出任意 k 时刻基于双 MEMS-IMU 的行人自主导航系统中每个 IMU。

4、系统的状态步骤三：使用零速检测算法检测到 IMU 为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为：得到修正后的 IMU 状态两个 IMU 独立进行零速校正；步骤四：利用步骤三中估计出的双 IMU 导航系统导航状态及最大步长不等式，判断 IMU 输出是否满足最大步长约束，若不满足则执行步骤五，若满足约束则返回步骤三；步骤五：利用公式求取将不满足约束条件的双 MEMS-IMU 行人自主导航系统的导航解算输出映射到满足映射条件的范围内的映射方程步骤六：利用公式将不满足步骤四中。

5、最大步长不等式约束的双 MEMS-IMU 系统导航状态的估计值约束到子空间 x Rm:|Lx|2 2 中，得到双 MEMS-IMU 行人自主导航系统状态约束值步骤七：利用公式计算经状态约束后的双 MEMS-IMU 行人自主导航系统导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵；步骤八：构造卡尔曼滤波动态误差修正模型，利用基于双 MEMS-IMU 的行人自主导航系统的导航解算联合误差传播特性方程： xk=Fkxk-1+Gkwk 得到行人自主导航参数的最优估计值。 2. 如权利要求 1 所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤一。

6、中，任意时刻 k 接收到的两个 IMU 输出信息为：其中， i 1、 2，表示双系统中的 IMU1 系统、 IMU2 系统；为 MEMS 三轴陀螺仪输出的角速率信息；为 MEMS 三轴加速度计输出的比力信息； T 表示转置操作。 3. 如权利要求 1 所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤二中，任意 k 时刻基于双 MEMS-IMU 的行人自主导航系统中每个 IMU 系统的状态为：权利要求书 CN 103776446 A 2 2/5 页 3 其中， v、 s、三维位置向量、速度向量、姿态向量，即 4. 如权利要。

7、求 1 所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤三中，零速校正卡尔曼滤波模型中， Xk为被估计状态向量： X T T rT vT aT 其中， T为姿态误差、 T为陀螺漂移、 rT为位置误差、 vT为速度误差、 aT为加速度计输出误差，以上每一项都是三维的； Wk-1为系统噪声序列；量测量 Zk为 IMU 静止时，双 MEMS-IMU 输出量经惯性导航解算算法得到的速度分量； Nk为量测噪声序列； H 为量测阵： H 0 0 I 0 0T F 为状态转移矩阵 : 式中， I(0) 为单位 ( 零 ) 阵；为沿地理系的载体运动加。

8、速度的反对称阵；为载体坐标系相对于导航坐标系的状态转移矩阵。 5. 如权利要求 1 所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤四中，利用不等式： |Lxk|2 2 判断双 MEMS-IMU 系统输出是否满足约束条件，若 k 时刻导航估计状态存在则执行步骤五，若 k 时刻导航估计状态满足不等式 |Lxk|2 2, 则返回步骤三；其中，为行人自主导航系统使用者行走或跑步时双脚间的最大步长； k N+； L=I3O3,n1-I3O3,n2T,I 表示单位阵， O 表示零阵； | 表示求范数； xk为 k 时刻两个 IMU 导航系统真。

9、实状态的联合，为 k 时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息， 6. 如权利要求 1 所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤五中，利用公式求取映射方程其中，权利要求书 CN 103776446 A 3 3/5 页 4 式中，为满秩的；（x*,*）为拉格朗日方程：的驻点；其中，为拉格朗日乘子， 0。 7. 如权利要求 1 所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤六中，利用公式：将当前时刻不满足约束条件的行人自主导航系统导航状。

10、态的估计值约束到子空间 x Rm:|Lx|2 2 中，得到满足最大步长约束限制的双 MEMS-IMU 系统状态约束值则为满足最大步长约束限制的状态量；其中，Pk表示双MEMS-IMU导航系统状态估计值的协方差矩阵。 8. 如权利要求 1 所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤七中，通过公式：计算状态约束后基于双 MEMS-IMU 的行人自主导航的导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵；其中， p 为约束函数 p(x) 围绕原始状态估计的雅可比矩阵，即：式中，表示时 p(x) 对 xm的导数。 9. 如权利要求 1 。

11、所述的基于双 MEMS-IMU 的行人自主导航解算算法，其特征在于，在步骤八中，利用双 MEMS-IMU 导航系统的导航解算联合误差传播特性方程： xk=Fkxk-1+Gkwk 得到行人自主导航系统状态的最优估计值；其中， xk为双 IMU 导航系统的导航解算联合误差：基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的状态转移矩阵为：权利要求书 CN 103776446 A 4 4/5 页 5 基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的噪声驱动阵为：基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的噪声为：式中，。

12、为基于双 MEMS-IMU 的行人自主导航系统导航解算误差， i(i=1,2) 表示第 i 个 IMU 导航系统，且满足：其中，表示第 i 个 IMU 导航系统的状态转移矩阵；表示第 i 个 IMU 导航系统的噪声驱动阵；其中，表示过程噪声，为零均值高斯白噪声，其方差为 Qi：系统量测更新模型表示为：其中，式中， H=03,3I303,3 ；和 (i)用于判断第 i 个 IMU 是否满足触发零速校正卡尔曼滤波器的条件，如果则表示第 i 个 IMU 不满足触发零速校正的条件；反之则表示满足，相应的量测噪声阵可表示为：权利要求书 CN 1037。

13、76446 A 5 5/5 页 6 权利要求书 CN 103776446 A 6 1/11 页 7 一种基于双 MEMS-IMU 的行人自主导航解算算法技术领域： 0001 本发明涉及的是一种导航解算算法，特别是涉及一种基于双 MEMS-IMU（微机械系统 - 惯性测量单元）的行人自主导航解算算法。背景技术： 0002 近年来，随着国内外 MEMS 惯性器件精度的提高，使得利用捷联惯性导航系统解算算法来进行行人航位推算成为可能，特别是利用捷联惯性导航解算算法可以提供更完备的导航信息。但是即便如此，若长时间工作， MEMS 惯性器件误差还是会比较严重的发散，捷。

14、联惯性导航解算算法得到的行人航位推算结果验证了如果导航期间 MEMS 惯性器件误差不能得到有效补偿，位置误差会以时间三次方的趋势发散，系统将最终丧失导航功能。因此，捷联惯性导航解算算法应用于行人自主导航系统的最大难点在于设计有效的误差修正算法。 0003 现有的导航解算算法，主要以捷联惯性导航解算算法为基础，采用零速校正等误差补偿算法对导航结果进行实时修正。基于零速校正的误差补偿算法大都存在零速检测不准确、检测结果滞后、零速校正时间短等缺陷，虽然可以在一定范围内提高行人自主导航系统的导航精度，但是导航定位误差仍然较大，且仅能在短时间内使用。总而言之，现有的导。

15、航解算算法准确性差，难以满足行人自主导航精确可靠的要求。发明内容： 0004 本发明的目的在于克服现有技术的不足，提供一种基于双 MEMS-IMU 的行人自主导航解算算法。 0005 为了解决背景技术所存在的问题，本发明采用以下技术方案： 0006 一种基于双 MEMS-IMU 的行人自主导航解算算法 , 它包括如下步骤： 0007 步骤一：将基于双MEMS-IMU的行人自主导航系统中的两个IMU系统分别固定于行人的两只脚上，手持 PDA 实时接收并存储行人运动时两个 IMU 系统输出的量测信息； 0008 步骤二：使用步骤一中存储的行人自主导航系统输出数据，。

16、利用捷联惯性导航系统导航解算方法求出任意 k 时刻基于双 MEMS-IMU 的行人自主导航系统中每个 IMU 系统的状态 0009 步骤三：使用零速检测算法检测到 IMU 为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为： 0010 0011 得到修正后的 IMU 状态两个 IMU 独立进行零速校正； 0012 步骤四：利用步骤三中估计出的双 MEMS-IMU 导航系统导航状态及最大步长不等式，判断 IMU 输出是否满足最大步长约束，若不满足约束则执行步骤五，若满足约束则返说明书 C。

17、N 103776446 A 7 2/11 页 8 回步骤三； 0013 步骤五：利用公式求取将不满足约束条件的双 MEMS-IMU 行人自主导航系统导航解算输出映射到满足映射条件的范围内的映射方程 0014 步骤六：若双 MEMS-IMU 系统的导航解算结果不满足步骤四中的最大步长不等式约束条件，则利用公式将此时行人自主导航系统导航状态的估计值约束到子空间 x Rm:|Lx|2 2 中，得到双 MEMS-IMU 行人自主导航系统状态约束值 0015 步骤七：利用公式计算经状态约束后的双 MEMS-IMU 行人自主导航系统导航解算结果的协方差阵，以更新卡尔曼滤波的协方。

18、差阵； 0016 步骤八：构造卡尔曼滤波动态误差修正模型，利用行人自主导航系统导航解算联合误差传播特性方程： 0017 xk Fkxk-1+Gkwk 0018 得到行人自主导航参数的最优估计值。 0019 优选的，在所述的步骤一中，任意时刻 k 接收到的两个 IMU 输出信息为： 0020 0021 其中， i 1、 2，表示双 MEMS-IMU 行人自主导航系统中的 IMU1 系统、 IMU2 系统；为 MEMS 三轴陀螺仪输出的角速率信息；为 MEMS 三轴加速度计输出的比力信息； T 表示转置操作。 0022 优选的，在所述的步骤二中，任意 k 时刻基于。

19、双 MEMS-IMU 的行人自主导航系统中每个 IMU 系统的状态为： 0023 0024 其中， v、 s、三维位置向量、速度向量、姿态向量，即 0025 优选的，在所述的步骤三中，零速校正卡尔曼滤波模型中， Xk为被估计状态向量： 0026 X T T rT vT aT 0027 其中， T为姿态误差、 T为陀螺漂移、 rT为位置误差、 vT为速度误差、 aT 为加速度计输出误差，以上每一项都是三维的； Wk-1为系统噪声序列；量测量 Zk为 IMU 静止时，其输出量经惯导解算得到的速度分量； Nk为量测噪声序列； H 为量测阵： 0028 H 0 0。

20、 I 0 0T 0029 F 为状态转移矩阵 : 0030 说明书 CN 103776446 A 8 3/11 页 9 0031 式中， I(0) 为单位 ( 零 ) 阵；为沿地理系的载体运动加速度的反对称阵；为载体坐标系相对于导航坐标系的状态转移矩阵。 0032 优选的，在所述的步骤四中，利用不等式： 0033 |Lxk|2 2 0034 判断双 IMU 输出是否满足约束条件，若 k 时刻导航估计状态存在则执行步骤五，若 k 时刻导航估计状态 |Lxk|2 2, 则返回步骤三； 0035 其中，为单兵导航系统使用者行走或跑步时双脚间的最大步长； k N+； ,I 。

21、表示单位阵， O 表示零阵； | 表示求范数； xk为 k 时刻两个 IMU 导航系统真实状态的联合，为 k 时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息， 0036 优选的，在所述的步骤五中，利用公式 0037 0038 求取映射方程 0039 其中， 0040 0041 0042 式中，为满秩的；（x*,*）为拉格朗日方程： 0043 0044 的驻点； 0045 其中，为拉格朗日乘子， 0。 0046 优选的，在所述的步骤六中，利用公式： 0047 0048 将此时行人自主导航系统导航状。

22、态的估计值约束到子空间 x Rm:|Lx|2 2 中，得到双 MEMS-IMU 系统最大步长约束限制后的状态约束值则为满足最大步长约束限制的状态量； 0049 其中，Pk表示双导航系统状态估计值的协方差矩阵。说明书 CN 103776446 A 9 4/11 页 10 0050 优选的，在所述的步骤七中，通过公式： 0051 0052 计算双 MEMS-IMU 系统状态约束后的协方差阵以更新卡尔曼滤波的协方差阵； 0053 其中， p 为约束函数 p(x) 围绕原始状态估计的雅可比矩阵，即： 0054 0055 式中，表示时 p(x) 对 xm的导。

23、数。 0056 优选的，在所述的步骤八中，利用双 MEMS-IMU 的行人自主导航系统导航解算联合误差传播特性方程： 0057 xk Fkxk-1+Gkwk 0058 得到行人自主导航系统状态的最优估计值； 0059 其中， xk为基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差： 0060 0061 基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的状态转移矩阵为： 0062 0063 基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的噪声驱动阵为： 0064 0065 基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的噪声为。

24、： 0066 0067 式中，为基于双 MEMS-IMU 的行人自主导航系统导航解算误差， i(i=1,2) 表示第 i 个 IMU 导航系统，且满足： 0068 0069 其中，表示第 i 个 IMU 导航系统的状态转移矩阵；表示 i 个 IMU 导航系统的噪声驱动阵； 0070 其中，表示过程噪声，为零均值高斯白噪声，其方差为 Qi： 0071 0072 系统量测更新模型表示为：说明书 CN 103776446 A 10 5/11 页 11 0073 0074 其中， 0075 0076 0077 式中， H 03,3 I3 03,3 ；和 (i)用于判断第 i 个。

25、 IMU 是否满足触发零速校正卡尔曼滤波器的条件，如果则表示第i个IMU不满足触发零速校正的条件；如果则表示第 i 个 IMU 满足触发零速校正的条件，相应的量测噪声阵可表示为： 0078 0079 本发明对比现有技术有如下的有益效果：通过利用双 IMU 系统输出信息，使用捷联惯性导航解算算法及零速校正算法对导航结果进行初步估计，对估计后的双 IMU 定位信息进行信息融合，利用人体运动时存在最大步长设计状态约束科尔曼滤波算法，对基于双 MEMS-IMU 的行人自主导航系统的导航结果进行不等式约束，将模糊的人体生理特性问题转化为严格的数学问题，克服了传统单。

26、脚定位时约束条件少、精确度差、系统可使用时间短等不足，在小成本的条件下，提高了检测的精度。本发明方法简单，稳定性和可靠性高，有效的提高了行人自主导航系统的使用精度。附图说明： 0080 图 1 是基于双 MEMS-IMU 的行人自主导航解算原理图。 0081 图 2 是基于双 MEMS-IMU 的行人自主导航解算算法中最大步长取值图。 0082 图 3 是本发明实施例提供的基于双 MEMS-IMU 的行人自主导航系统实验时导航解算结果图。说明书 CN 103776446 A 11 6/11 页 12 具体实施方式： 0083 下面结合附图和具体实施方式对本发明作。

27、进一步描述： 0084 图 1 是基于双 MEMS-IMU 的行人自主导航解算原理图 , 图 2 是基于双 MEMS-IMU 的行人自主导航解算算法中最大步长取值图。 0085 一种基于双 MEMS-IMU 的行人自主导航解算算法 , 它包括如下步骤： 0086 步骤一：将基于双MEMS-IMU的行人自主导航系统中的两个IMU系统分别固定于行人的两只脚上，手持 PDA 实时接收并存储行人运动时两个 IMU 系统输出的量测信息； 0087 步骤二：使用步骤一中存储的行人自主导航系统输出数据，利用捷联惯性导航系统导航解算方法求出任意 k 时刻基于双 MEMS-IMU 的行。

28、人自主导航系统中每个 IMU 系统的状态 0088 步骤三：使用零速检测算法检测到 IMU 为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为： 0089 0090 得到修正后的 IMU 状态两个 IMU 独立进行零速校正； 0091 步骤四：利用步骤三中估计出的基于双 MEMS-IMU 的行人自主导航系统状态及最大步长约束不等式，判断系统解算结果是否满足约束，若不满足则执行步骤五，若满足约束则返回步骤三； 0092 步骤五：利用公式求取将不满足约束条件的系统解算结果映射到满足映射条件。

29、的范围内的映射方程 0093 步骤六：若基于双 MEMS-IMU 的行人自主导航系统导航解算结果不满足步骤四中的不等式最大步长约束，则利用公式将此时行人自主导航系统导航状态的估计值约束到子空间 x Rm:|Lx|2 2 中，得到满足最大步长约束限制的导航状态值 0094 步骤七：利用公式计算经状态约束后，行人自主导航系统导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵； 0095 步骤八：构造卡尔曼滤波动态误差修正模型，利用基于双 MEMS-IMU 的行人自主导航系统的导航解算联合误差传播特性方程： 0096 xk Fkxk-1+Gkwk 0097 得到基于双。

30、 MEMS-IMU 的行人自主导航系统的导航状态最优估计值。 0098 优选的，在所述的步骤一中，任意时刻 k 接收到的两个 IMU 输出信息为： 0099 0100 其中， i 1、 2，表示双系统中的 IMU1 系统、 IMU2 系统；为 MEMS 三轴陀螺仪输出的角速率信息；为 MEMS 三轴加速度计输出的比说明书 CN 103776446 A 12 7/11 页 13 力信息； T 表示转置操作。 0101 优选的，在所述的步骤二中，任意 k 时刻基于双 MEMS-IMU 的行人自主导航系统中每个 IMU 系统的状态为： 0102 0103 其中， v、 s。

31、、三维位置向量、速度向量、姿态向量，即 0104 优选的，在所述的步骤三中，零速校正卡尔曼滤波模型中， Xk为被估计状态向量： 0105 X T T rT vT aT 0106 其中， T为姿态误差、 T为陀螺漂移、 rT为位置误差、 vT为速度误差、 aT 为加速度计输出误差，以上每一项都是三维的； Wk-1为系统噪声序列；量测量 Zk为 IMU 静止时， IMU 输出量经惯导解算得到的速度分量； Nk为量测噪声序列； H 为量测阵： 0107 H 0 0 I 0 0T 0108 F 为状态转移矩阵 : 0109 0110 式中， I(0) 为单位 ( 零 )。

32、阵；为沿地理系的载体运动加速度的反对称阵；为载体坐标系相对于导航坐标系的状态转移矩阵。 0111 优选的，在所述的步骤四中，利用不等式： 0112 |Lxk|2 2 0113 判断基于双 MEMS-IMU 的行人自主导航系统的导航解算状态是否满足约束条件，若 k 时刻导航估计状态满足不等式则执行步骤五，若 k 时刻导航估计状态 |Lxk|2 2, 则返回步骤三； 0114 其中，为行人自主导航系统使用者行走或跑步时双脚间的最大步长； k N+； I 表示单位阵， O 表示零阵； | 表示求范数； xk为 k 时刻两个 IMU 导航系统真实状态的联合，为 k 时刻两。

33、个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息， 0115 优选的，在所述的步骤五中，利用公式 0116 说明书 CN 103776446 A 13 8/11 页 14 0117 求取映射方程 0118 其中， 0119 0120 0121 式中，为满秩的；（x*,*）为拉格朗日方程： 0122 0123 的驻点； 0124 其中，为拉格朗日乘子， 0。 0125 优选的，在所述的步骤六中，利用公式： 0126 0127 将当前时刻不满足约束条件的行人自主导航系统导航状态的估计值约束到子空间 x Rm:|Lx|2 2 中，得到。

34、满足最大步长约束限制的双 MEMS-IMU 系统状态约束值则为满足最大步长约束限制的状态量； 0128 其中，Pk表示双导航系统状态估计值的协方差矩阵。 0129 优选的，在所述的步骤七中，通过公式： 0130 0131 计算状态约束后基于双 MEMS-IMU 的行人自主导航的导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵； 0132 其中， p 为约束函数 p(x) 围绕原始状态估计的雅可比矩阵，即： 0133 0134 式中，表示时 p(x) 对 xm的导数。 0135 优选的，在所述的步骤八中，利用双 MEMS-IMU 导航系统的导航解算联合误差传播特性方程。

35、： 0136 xk Fkxk-1+Gkwk 0137 得到行人自主导航系统状态的最优估计值； 0138 其中， xk为基于双 MEMS-IMU 的行人自主导航系统的导航解算联合误差： 0139 0140 基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的状态转移矩阵为：说明书 CN 103776446 A 14 9/11 页 15 0141 0142 基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的噪声驱动阵为： 0143 0144 基于双 MEMS-IMU 的行人自主导航系统导航解算联合误差的噪声为： 0145 0146 式中，为基于双 MEMS-。

36、IMU 的行人自主导航系统导航解算误差， i(i=1,2) 表示第 i 个 IMU 导航系统，且满足： 0147 0148 其中，表示第 i 个 IMU 导航系统的状态转移矩阵；表示第 i 个 IMU 导航系统的噪声驱动阵； 0149 其中，表示过程噪声，为零均值高斯白噪声，其方差为 Qi： 0150 0151 系统量测更新模型表示为： 0152 0153 其中， 0154 0155 0156 式中， H 03,3 I3 03,3 ；和 (i)用于判断第 i 个 IMU 是否满足触发零速校正卡尔曼滤波器的条件，如果则表示第i个IMU不满足触发零速校正的条件；反之说明。

37、书 CN 103776446 A 15 10/11 页 16 则表示满足，相应的量测噪声阵可表示为： 0157 0158 结合以下实验对本发明的优益效果作进一步的说明： 0159 利用两套自研三轴惯性测量组件（集成了微机械系统三轴磁力计、加速度计、陀螺仪）搭建真实双 IMU 系统行人自主导航系统模型，设备参数如表 1 所示，通过合理的试验验证基于双 MEMS-IMU 的行人自主导航系统导航解算最大步长约束算法的可靠性、实用性、准确性，试验场景选在室外空旷的哈尔滨工程大学军工操场， 0160 表 1 自研微型惯性测量单元惯性测量组件各传感器性能指标 0161 01。

38、62 0163 实验过程中相关参数设置如下： 0164 行人自主导航自主定位系统采样频率： 100Hz 0165 最大步长限制： 0.8m 0166 微机械系统陀螺标准偏差： a=0.01m/s2 0167 微机械加速度计标准偏差： g=0.1*pi/180rad/s 0168 初始速度 :vn(0)=0 0 0T 0169 初始位置坐标 :sn(0)=0 0 0T 说明书 CN 103776446 A 16 11/11 页 17 0170 实验开始前，测试者在实验场进行 15 分钟的系统静止预热，完成系统的初始对准和GPS定位信息的初始化；为了便于获取精确的参考信息，。

39、实验中实时采集了GPS定位信息作为真实轨迹的参考。然后围绕长方形足球场进行场地按照预定轨迹走一圈（约 90 米），行走时间约为 2 分钟。最后将采集得到的实验数据进行离线分析。 0171 定位结果如图3所示，为了更形象的说明定位误差，表2给出了使用本发明提出的基于双 MEMS-IMU 的行人自主导航系统最大步长约束导航解算算法的定位结果的均方根误差RMS，其中计算真值为GPS定位信息。在行走时间大于2分钟的情况下定位误差仍然保持在 0.7m 内，小于行人行走距离的 1%。实验证明本发明提出的基于双 MEMS-IMU 的行人自主导航系统最大步长约束导航解算算法定位结果比较理想，可以满足短时间内行人作战人员的使用要求。 0172 表 2 定位误差 0173 0174 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。说明书 CN 103776446 A 17 1/2 页 18 图 1 图 2 说明书附图 CN 103776446 A 18 2/2 页 19 图 3 说明书附图 CN 103776446 A 19 。

展开阅读全文