CN103776446A - 一种基于双mems-imu的行人自主导航解算算法 - Google Patents

Abstract

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

Description

一种基于双MEMS-IMU的行人自主导航解算算法

技术领域：

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

背景技术：

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

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

发明内容：

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

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

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

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

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

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

$\left\{\begin{array}{c}{\mathrm{\δX}}_k={\mathrm{F\δX}}_{k-1}+W_{k-1}\\ Z_k={\mathrm{HX}}_k+N_k\end{array}\right.$

得到修正后的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

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

优选的，在所述的步骤一中，任意时刻k接收到的两个IMU输出信息为： 

$y_k^i={\left[\begin{array}{cc}{f}_k^i& {\mathrm{\ω}}_k^i\end{array}\right]}^T$

其中，i＝1、2，表示双MEMS-IMU行人自主导航系统中的IMU1系统、IMU2系统； ${\mathrm{\ω}}_k^i={\left[\begin{array}{ccc}{\mathrm{\ω}}_k^{\mathrm{ix}}& {\mathrm{\ω}}_k^{\mathrm{iy}}& {\mathrm{\ω}}_k^{\mathrm{iz}}\end{array}\right]}^T$ 为MEMS三轴陀螺仪输出的角速率信息；  $f_k^i={\left[\begin{array}{ccc}{f}_k^{\mathrm{ix}}& f_k^{\mathrm{iy}}& f_k^{\mathrm{iz}}\end{array}\right]}^T$ 为MEMS三轴加速度计输出的比力信息；T表示转置操作。 

优选的，在所述的步骤二中，任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态为： 

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

优选的，在所述的步骤三中，零速校正卡尔曼滤波模型中，δX_k为被估计状态向量： 

δX＝[Φ^T δω^T δr^T δv^T δa^T] 

其中，φ^T为姿态误差、δω^T为陀螺漂移、δr^T为位置误差、δv^T为速度误差、δa^T为加速度计输出误差，以上每一项都是三维的；W_k-1为系统噪声序列；量测量Z_k为IMU静止时，其输出量经惯导解算得到的速度分量；N_k为量测噪声序列；H为量测阵： 

H＝[0 0 I 0 0]^T

F为状态转移矩阵: 

$F=\left[\begin{array}{ccccc}I& -{\mathrm{\ΔtC}}_b^n& 0& 0& 0\\ 0& I& 0& 0& 0\\ 0& 0& I& \mathrm{\ΔtI}& 0\\ \mathrm{\ΔtS}\left(f^n\right)& 0& 0& I& {\mathrm{\ΔtC}}_b^n\\ 0& 0& 0& 0& I\end{array}\right]$

式中，I(0)为单位(零)阵；

为沿地理系的载体运动加速度的反对称阵； 

为载体坐标系相对于导航坐标系的状态转移矩阵。 

优选的，在所述的步骤四中，利用不等式： 

||L·x_k||²≤γ²

判断双IMU输出是否满足约束条件，若k时刻导航估计状态存在

则执行步骤五，若k时刻导航估计状态||L·x_k||²≤γ²,则返回步骤三； 

其中，γ为单兵导航系统使用者行走或跑步时双脚间的最大步长；k∈N⁺； 

,I表示单位阵，O表示零阵；||·||表示求范数；x_k为k时刻两个IMU导航系统真实状态的联合，

为k时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息， ${\hat{x}}_k={\left[\begin{array}{cc}{\left({\hat{x}}_k^{\left(1\right)}\right)}^T& {\left({\hat{x}}_k^{\left(2\right)}\right)}^T\end{array}\right]}^T;x_k^{\left(i\right)}\∈R^{n_i},{\hat{x}}_k^{\left(i\right)}\∈R^{n_i},x_k,{\hat{x}}_k\∈R^m(n_1+n_2=m).$

优选的，在所述的步骤五中，利用公式 

$p\left(\hat{x}\right)\stackrel{\mathrm{eqv}}{=}\mathrm{\Π}\left({\mathrm{\λ}}^{*}\right){\hat{x}}_k$

求取映射方程

其中， 

$\mathrm{\Π}\left({\mathrm{\λ}}^{*}\right)\stackrel{\mathrm{def}}{=}{(P_k^{-1}+{\mathrm{\λ}}^{*}{L}^{T}L)}^{-1}{P}_k^{-1}$

式中，

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

$J(x,\mathrm{\λ})\stackrel{\mathrm{def}}{=}{{|\hat{x}}_k-\left|x\right||}_{P_k^{-1}}^2+\mathrm{\λ\ψ}\left(x\right)$

的驻点； 

其中，

λ为拉格朗日乘子，λ＞0。 

优选的，在所述的步骤六中，利用公式： 

$p\left({\hat{x}}_k\right)=\underset{x}{\arg \min }\left({\left|\right|{\hat{x}}_k-x_k\left|\right|}_{P_k^{-1}}^2\right)$

将此时行人自主导航系统导航状态的估计值约束到子空间{x∈R^m:||L·x||²≤γ²}中，得到双MEMS-IMU系统最大步长约束限制后的状态约束值

则为满足最大步长约束限制的状态量； 

其中，

P_k表示双导航系统状态估计值的协方差矩阵。 

优选的，在所述的步骤七中，通过公式： 

$P_k^{*}=\▿p\·P\·{(\▿p)}^T$

计算双MEMS-IMU系统状态约束后的协方差阵

以更新卡尔曼滤波的协方差阵； 

其中，▽p为约束函数p(x)围绕原始状态估计

的雅可比矩阵，即： 

$\▿p\stackrel{\mathrm{def}}{=}[\frac{\∂p\left(x\right)}{{\∂\left[x\right]}_1}{|}_{x={\hat{x}}_k}...\frac{\∂p\left(x\right)}{{\∂\left[x\right]}_m}{|}_{{x=\hat{x}}_k}];$

式中，

表示

时p(x)对[x]_m的导数。 

优选的，在所述的步骤八中，利用双MEMS-IMU的行人自主导航系统导航解算联合误差传播特性方程： 

δx_k＝F_kδx_k-1+G_kw_k

得到行人自主导航系统状态的最优估计值； 

其中，δx_k为基于双MEMS-IMU的行人自主导航系统导航解算联合误差： 

${\mathrm{\δx}}_k=\left[\begin{array}{cc}{\mathrm{\δx}}_k^{\left(1\right)}& {\mathrm{\δx}}_k^{\left(2\right)}\end{array}\right]$

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

$F_k=\left[\begin{array}{cc}{F}_k^{\left(1\right)}& 0_{\mathrm{9,9}}\\ 0_{\mathrm{9,9}}& F_k^{\left(2\right)}\end{array}\right]$

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

$G_k=\left[\begin{array}{cc}{G}_k^{\left(1\right)}& 0_{\mathrm{9,6}}\\ 0_{\mathrm{9,6}}& G_k^{\left(2\right)}\end{array}\right]$

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

$w_k=\left[\begin{array}{c}{\left(w_k^{\left(1\right)}\right)}^T\\ {\left(w_k^{\left(2\right)}\right)}^T\end{array}\right]$

式中，

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

${\mathrm{\δx}}_k^{\left(i\right)}=F_k^{\left(i\right)}{\mathrm{\δx}}_{k-1}^{\left(i\right)}+G_k^{\left(i\right)}{w}_k^{\left(i\right)}$

其中，

表示第i个IMU导航系统的状态转移矩阵；表示i个IMU导航系 统的噪声驱动阵； 

其中，

表示过程噪声，为零均值高斯白噪声，其方差为Qⁱ： 

$Q=\left[\begin{array}{cc}{Q}^1& 0_{\mathrm{6,6}}\\ 0_{\mathrm{6,6}}& Q^2\end{array}\right]$

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

$-H{\hat{x}}_k=\mathrm{H\δ}{\hat{x}}_k+v_k$

其中， 

式中，H＝[0_3,3 I₃ 0_3,3 ]；和η⁽ⁱ⁾用于判断第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为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为： 

$\left\{\begin{array}{c}{\mathrm{\δX}}_k={\mathrm{F\δX}}_{k-1}+W_{k-1}\\ Z_k={\mathrm{HX}}_k+N_k\end{array}\right.$

得到修正后的IMU状态

两个IMU独立进行零速校正； 

步骤四：利用步骤三中估计出的基于双MEMS-IMU的行人自主导航系统状态 

及最大步长约束不等式，判断系统解算结果是否满足约束，若不满足则执行步骤五，若满足约束则返回步骤三； 

步骤五：利用公式求取将不满足约束条件的系统解算结果映射到满足映射条件的范围内的映射方程

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

步骤七：利用公式计算经状态约束后，行人自主导航系统导航解算结果的协方差阵

以更新卡尔曼滤波的协方差阵； 

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

δx_k＝F_kδx_k-1+G_kw_k

得到基于双MEMS-IMU的行人自主导航系统的导航状态最优估计值。 

优选的，在所述的步骤一中，任意时刻k接收到的两个IMU输出信息为： 

$y_k^i={\left[\begin{array}{cc}{f}_k^i& {\mathrm{\ω}}_k^i\end{array}\right]}^T$

其中，i＝1、2，表示双系统中的IMU1系统、IMU2系统； ${\mathrm{\ω}}_k^i={\left[\begin{array}{ccc}{\mathrm{\ω}}_k^{\mathrm{ix}}& {\mathrm{\ω}}_k^{\mathrm{iy}}& {\mathrm{\ω}}_k^{\mathrm{iz}}\end{array}\right]}^T$ 为MEMS三轴陀螺仪输出的角速率信息； $f_k^i={\left[\begin{array}{ccc}{f}_k^{\mathrm{ix}}& f_k^{\mathrm{iy}}& f_k^{\mathrm{iz}}\end{array}\right]}^T$ 为MEMS三轴加速度计输出的比力信息；T表示转置操作。 

优选的，在所述的步骤二中，任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态

为： 

其中，v、s、

三维位置向量、速度向量、姿态向量，即

优选的，在所述的步骤三中，零速校正卡尔曼滤波模型中，δX_k为被估计状态向量： 

δX＝[Φ^T δω^T δr^T δv^T δa^T] 

其中，φ^T为姿态误差、δω^T为陀螺漂移、δr^T为位置误差、δv^T为速度误差、δa^T为加速度计输出误差，以上每一项都是三维的；W_k-1为系统噪声序列；量测量Z_k为IMU静止时，IMU输出量经惯导解算得到的速度分量；N_k为量测噪声序列；H为量测阵： 

H＝[0 0 I 0 0]^T

F为状态转移矩阵: 

$F=\left[\begin{array}{ccccc}I& -{\mathrm{\ΔtC}}_b^n& 0& 0& 0\\ 0& I& 0& 0& 0\\ 0& 0& I& \mathrm{\ΔtI}& 0\\ \mathrm{\ΔtS}\left(f^n\right)& 0& 0& I& {\mathrm{\ΔtC}}_b^n\\ 0& 0& 0& 0& I\end{array}\right]$

式中，I(0)为单位(零)阵；

为沿地理系的载体运动加速度的反对称阵； 

为载体坐标系相对于导航坐标系的状态转移矩阵。 

优选的，在所述的步骤四中，利用不等式： 

||L·x_k||²≤γ²

判断基于双MEMS-IMU的行人自主导航系统的导航解算状态是否满足约束条件，若k时刻导航估计状态满足不等式则执行步骤五，若k时刻导航估计状态||L·x_k||²≤γ²,则返回步骤三； 

其中，γ为行人自主导航系统使用者行走或跑步时双脚间的最大步长；k∈N⁺；

I表示单位阵，O表示零阵；||·||表示求范数；x_k为k时刻两个IMU导航系统真实状态的联合，

为k时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息， ${\hat{x}}_k={\left[\begin{array}{cc}{\left({\hat{x}}_k^{\left(1\right)}\right)}^T& {\left({\hat{x}}_k^{\left(2\right)}\right)}^T\end{array}\right]}^T;x_k^{\left(i\right)}\∈R^{n_i},{\hat{x}}_k^{\left(i\right)}\∈R^{n_i},x_k,{\hat{x}}_k\∈R^m(n_1+n_2=m).$

优选的，在所述的步骤五中，利用公式 

$p\left(\hat{x}\right)\stackrel{\mathrm{eqv}}{=}\mathrm{\Π}\left({\mathrm{\λ}}^{*}\right){\hat{x}}_k$

求取映射方程

其中， 

$\mathrm{\Π}\left({\mathrm{\λ}}^{*}\right)\stackrel{\mathrm{def}}{=}{(P_k^{-1}+{\mathrm{\λ}}^{*}{L}^{T}L)}^{-1}{P}_k^{-1}$

${\mathrm{\λ}}^{*}\stackrel{\mathrm{def}}{=}\{\mathrm{\λ}\∈R^{+}:\mathrm{\ψ}\left(\mathrm{\Π}\left(\mathrm{\λ}\right){\hat{x}}_k\right)=0\}$

式中，

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

$J(x,\mathrm{\λ})\stackrel{\mathrm{def}}{=}{{|\hat{x}}_k-\left|x\right||}_{P_k^{-1}}^2+\mathrm{\λ\ψ}\left(x\right)$

的驻点； 

其中，

λ为拉格朗日乘子，λ＞0。 

优选的，在所述的步骤六中，利用公式： 

$p\left({\hat{x}}_k\right)=\underset{x}{\arg \min }\left({\left|\right|{\hat{x}}_k-x_k\left|\right|}_{P_k^{-1}}^2\right)$

将当前时刻不满足约束条件的行人自主导航系统导航状态的估计值约束到子空间{x∈R^m:||L·x||²≤γ²}中，得到满足最大步长约束限制的双MEMS-IMU系统状态约束值

则为满足最大步长约束限制的状态量； 

其中，

P_k表示双导航系统状态估计值的协方差矩阵。 

优选的，在所述的步骤七中，通过公式： 

$P_k^{*}=\▿p\·P\·{(\▿p)}^T$

计算状态约束后基于双MEMS-IMU的行人自主导航的导航解算结果的协方差阵

以更新卡尔曼滤波的协方差阵； 

其中，▽p为约束函数p(x)围绕原始状态估计

的雅可比矩阵，即： 

$\▿p\stackrel{\mathrm{def}}{=}[\frac{\∂p\left(x\right)}{{\∂\left[x\right]}_1}{|}_{x={\hat{x}}_k}...\frac{\∂p\left(x\right)}{{\∂\left[x\right]}_m}{|}_{{x=\hat{x}}_k}];$

式中，

表示

时p(x)对[x]_m的导数。 

优选的，在所述的步骤八中，利用双MEMS-IMU导航系统的导航解算联合误 差传播特性方程： 

δx_k＝F_kδx_k-1+G_kw_k

得到行人自主导航系统状态的最优估计值； 

其中，δx_k为基于双MEMS-IMU的行人自主导航系统的导航解算联合误差： 

${\mathrm{\δx}}_k=\left[\begin{array}{cc}{\mathrm{\δx}}_k^{\left(1\right)}& {\mathrm{\δx}}_k^{\left(2\right)}\end{array}\right]$

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

$F_k=\left[\begin{array}{cc}{F}_k^{\left(1\right)}& 0_{\mathrm{9,9}}\\ 0_{\mathrm{9,9}}& F_k^{\left(2\right)}\end{array}\right]$

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

$G_k=\left[\begin{array}{cc}{G}_k^{\left(1\right)}& 0_{\mathrm{9,6}}\\ 0_{\mathrm{9,6}}& G_k^{\left(2\right)}\end{array}\right]$

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

$w_k=\left[\begin{array}{c}{\left(w_k^{\left(1\right)}\right)}^T\\ {\left(w_k^{\left(2\right)}\right)}^T\end{array}\right]$

式中，

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

满足： 

${\mathrm{\δx}}_k^{\left(i\right)}=F_k^{\left(i\right)}{\mathrm{\δx}}_{k-1}^{\left(i\right)}+G_k^{\left(i\right)}{w}_k^{\left(i\right)}$

其中，

表示第i个IMU导航系统的状态转移矩阵；

表示第i个IMU导航系统的噪声驱动阵； 

其中，

表示过程噪声，为零均值高斯白噪声，其方差为Qⁱ： 

$Q=\left[\begin{array}{cc}{Q}^1& 0_{\mathrm{6,6}}\\ 0_{\mathrm{6,6}}& Q^2\end{array}\right]$

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

$-H{\hat{x}}_k=\mathrm{H\δ}{\hat{x}}_k+v_k$

其中， 

式中，H＝[0_3,3 I₃ 0_3,3 ]；和η⁽ⁱ⁾用于判断第i个IMU是否满足触发零速校正卡尔曼滤波器的条件，如果

则表示第i个IMU不满足触发零速校正的条件；反之则表示满足，相应的量测噪声阵可表示为： 

结合以下实验对本发明的优益效果作进一步的说明： 

利用两套自研三轴惯性测量组件（集成了微机械系统三轴磁力计、加速度计、陀螺仪）搭建真实双IMU系统行人自主导航系统模型，设备参数如表1所示，通过合理的试验验证基于双MEMS-IMU的行人自主导航系统导航解算最大步长约束算法的可靠性、实用性、准确性，试验场景选在室外空旷的哈尔滨工程大学军工操场， 

表1 自研微型惯性测量单元惯性测量组件各传感器性能指标 

实验过程中相关参数设置如下： 

行人自主导航自主定位系统采样频率：100Hz 

最大步长限制γ：0.8m 

微机械系统陀螺标准偏差：σ_a=0.01m/s²

微机械加速度计标准偏差：σ_g=0.1*pi/180rad/s 

初始速度:vⁿ(0)=[0 0 0]^T

初始位置坐标:sⁿ(0)=[0 0 0]^T

实验开始前，测试者在实验场进行15分钟的系统静止预热，完成系统的初始对准和GPS定位信息的初始化；为了便于获取精确的参考信息，实验中实时采集了GPS定位信息作为真实轨迹的参考。然后围绕长方形足球场进行场地按照预定轨迹走一圈（约90米），行走时间约为2分钟。最后将采集得到的实验数据进行离线分析。 

定位结果如图3所示，为了更形象的说明定位误差，表2给出了使用本发明提出的基于双MEMS-IMU的行人自主导航系统最大步长约束导航解算算法的定位结果的均方根误差RMS，其中计算真值为GPS定位信息。在行走时间大于2分钟的情况下定位误差仍然保持在0.7m内，小于行人行走距离的1%。实验证明本发明提出的基于双MEMS-IMU的行人自主导航系统最大步长约束导航解算算法定位结果比较理想，可以满足短时间内行人作战人员的使用要求。 

表2 定位误差 

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

Claims (9)

1.一种基于双MEMS-IMU的行人自主导航解算算法，其特征在于，它由以下步骤实现： 

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

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

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

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

步骤四：利用步骤三中估计出的双IMU导航系统导航状态

及最大步长不等式，判断IMU输出是否满足最大步长约束，若不满足则执行步骤五，若满足约束则返回步骤三； 

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

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

步骤七：利用公式计算经状态约束后的双MEMS-IMU行人自主导航系统导航解算结果的协方差阵

以更新卡尔曼滤波的协方差阵； 

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

δx_k=F_kδx_k-1+G_kw_k

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

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的行人自主导航解算算法，其特征在于，在步骤三中，零速校正卡尔曼滤波模型中，δX_k为被估计状态向量： 

δX＝[Φ^T δω^T δr^T δv^T δa^T] 

其中，φ^T为姿态误差、δω^T为陀螺漂移、δr^T为位置误差、δv^T为 速度误差、δa^T为加速度计输出误差，以上每一项都是三维的； 

W_k-1为系统噪声序列；量测量Z_k为IMU静止时，双MEMS-IMU输出量经惯性导航解算算法得到的速度分量；N_k为量测噪声序列；H为量测阵： 

H＝[0 0 I 0 0]^T

F为状态转移矩阵: 

式中，I(0)为单位(零)阵；

为沿地理系的载体运动加速度的反对称阵；

为载体坐标系相对于导航坐标系的状态转移矩阵。 

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

||L·x_k||²≤γ²

判断双MEMS-IMU系统输出是否满足约束条件，若k时刻导航估计状态存在

则执行步骤五，若k时刻导航估计状态满足不等式||L·x_k||²≤γ²,则返回步骤三； 

其中，γ为行人自主导航系统使用者行走或跑步时双脚间的最大步长；k∈N⁺；L=[I₃O_3,n1-I₃O_3,n2]^T,I表示单位阵，O表示零阵；||·||表示求范数；x_k为k时刻两个IMU导航系统真实状态的联合， 

为k时刻两个导航系统导航估计状态的联合，其中包括三维速度信息、三维姿态信息、三维位置信息， 

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

求取映射方程

其中， 

式中，

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

的驻点； 

其中，

λ为拉格朗日乘子，λ＞0。 

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

将当前时刻不满足约束条件的行人自主导航系统导航状态的估计值约束到子空间{x∈R^m:||L·x||²≤γ²}中，得到满足最大步长约束限制的双MEMS-IMU系统状态约束值

则

为满足最大步长约束限制的状态量； 

其中，

P_k表示双MEMS-IMU导航系统状态估计值的协方差矩阵。 

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

计算状态约束后基于双MEMS-IMU的行人自主导航的导航解算结果的协方差阵

以更新卡尔曼滤波的协方差阵； 

其中，▽p为约束函数p(x)围绕原始状态估计的雅可比矩阵，即： 

式中，

表示

时p(x)对[x]_m的导数。 

9.如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤八中，利用双MEMS-IMU导航系统的导航解算联合误差传播特性方程： 

δx_k=F_kδx_k-1+G_kw_k

得到行人自主导航系统状态的最优估计值； 

其中，δx_k为双IMU导航系统的导航解算联合误差： 

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

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

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

式中，

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

满足： 

其中，

表示第i个IMU导航系统的状态转移矩阵；

表示第i个IMU导航系统的噪声驱动阵； 

其中，

表示过程噪声，为零均值高斯白噪声，其方差为Qⁱ： 

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

其中， 

式中，H=[0_3,3I₃0_3,3]；

和η⁽ⁱ⁾用于判断第i个IMU是否满足触发 零速校正卡尔曼滤波器的条件，如果

则表示第i个IMU不满足触发零速校正的条件；反之则表示满足，相应的量测噪声阵可表示为： 

Images