CN103616030A - 基于捷联惯导解算和零速校正的自主导航系统定位方法 - Google Patents

Abstract

本发明公开了一种基于捷联惯导解算和零速校正的自主导航系统定位方法，采用行人自主导航系统中MEMS加速度计和MEMS磁力计的输出数据对系统进行初始对准，利用捷联惯导解算算法对行人自主导航系统的状态进行估计，通过静态检测算法检测到行人脚步静止时，设计基于卡尔曼滤波的零速校正误差补偿器采用输出校正的方式对行人自主导航系统导航解算结果进行校正，克服了MEMS惯性测量器件精度低、长时间使用时定位误差较大的缺陷，在不增加外在成本的条件下，实现了高精度的行人自主导航系统定位功能。本发明方法简单，稳定性和可靠性高，有效的提高了单兵自主导航系统的使用精度，对实现更高定位精度的行人自主导航定位具有重要意义。

Description

基于捷联惯导解算和零速校正的自主导航系统定位方法

技术领域

本发明属于导航定位技术领域，尤其涉及一种基于捷联惯导解算和零速校正的自主导航系统定位方法。

背景技术

行人自主导航系统(包括MEMS三轴磁力计、MEMS三轴加速度计、MEMS三轴陀螺仪)主要用于个人在已知或未知条件下的自主导航和实时定位，辅助完成各类型的紧急救援任务。当火灾、地震等紧急意外发生时，事故发生现场可能存在可视性降低、固有环境改变等不利于救援的情况，营救人员无法快速准确的辨别自身位置。此时，行人导航系统提供的定位信息可为营救人员提供有效的技术保障。

现有的具有单兵导航定位功能的产品绝大数主要依靠GPS(GlobalPositioning System)进行定位，但当GPS信号缺失时，系统将无法工作，继而无法满足行人自主导航系统自主、实时、稳定的定位要求。因此，研究在无GPS情况下的单兵自主定位技术具有一定应用价值。基于MEMS惯性测量技术的行人自主导航系统工作时不依赖于任何外部信息，具有良好的抗干扰性，因此，研究基于MEMS惯性测量技术的行人自主导航系统具有良好的应用价值。但目前MIMU的测量精度总体上较低，传感器误差随时间的漂移比较严重，导致MIMU的导航误差积累急剧增大。

发明内容

本发明实施例的目的在于提供一种基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，旨在解决现有的MEMS惯性测量器件精度较低，基于MEMS惯性测量技术的行人自主导航系统长时间使用时导航定位误差较大的问题。

本发明实施例是这样实现的，一种基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，该基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法包括以下步骤：

步骤一：将行人自主导航系统固定于单兵脚上，手持PDA实时接收并存储行人运动时系统输出的量测信息，任意时刻k接收到的系统输出信息为y_k；

步骤二：利用MEMS加速度计和MEMS磁力计输出值，及公式求得初始载体姿态信息

步骤三：使用步骤一中存储的行人自主导航系统输出数据，及微分方程更新的方向余弦矩阵

步骤四：利用步骤三中求出的行人自主导航系统的姿态矩阵

及公式估计出单兵系统导航状态

包含行人导航系统的三维位置向量、速度向量、姿态向量，即

步骤五：利用步骤一中得到的行人导航系统中MEMS加速度计和MEMS陀螺仪的输出值及公式判断出行人自主导航系统的零速区间；

步骤六：利用步骤五中判断出得零速状态，使用基于卡尔曼滤波的零速误差校正器，采用输出校正的方式对导航解算结果进行修正；

通过以上循环即可估计出任意时刻k的状态量。

进一步，在步骤一中，行人导航系统包括：MEMS陀螺仪、MEMS加速度计、MEMS磁力计。

进一步，在步骤一中，任意时刻k接收到的系统输出信息为y_k＝[f_k ω_k m_k]^T；

其中， ${\mathrm{\ω}}_k={\left[\begin{array}{ccc}{\mathrm{\ω}}_k^x& {\mathrm{\ω}}_k^y& {\mathrm{\ω}}_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴陀螺仪输出的角速率信息； $f_k={\left[\begin{array}{ccc}{f}_k^x& f_k^y& f_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴加速度计输出的比力信息； $m_k={\left[\begin{array}{ccc}{m}_k^x& m_k^y& m_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴磁力计的输出信息；T表示转置操作。

进一步，在步骤二中利用MEMS加速度计和MEMS磁力计输出值及式：

$g_b=C_b^n{g}_n$

$m_b=C_b^n{m}_n$

$r_b=C_b^n{r}_n$

得初始载体姿态信息

其中，g_n为重力向量

在地理坐标系中投影，g_b＝[0 0 g]^T；m_n为地磁向量

在地理坐标系中投影，m_n＝[m_ex m_ey m_ez]^T；g_b为重力向量

在载体坐标系下的投影，g_b＝[g_bx g_by g_bz]^T；m_b为地磁向量

在载体坐标系下的投影，m_b＝[m_bx m_by m_bz]^T。

进一步，在步骤三中使用微分方程：

${\stackrel{\·}{C}}_b^n\left(t\right)=C_b^n\left(t\right)\mathrm{\Ω}\left(t\right)$

更新的方向余弦矩阵

式中，为t时刻载体系到地理系的捷联矩阵，Ω(t)为向量ω^b(t)的反对称阵，

$\mathrm{\Ω}\left(t\right)=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}^{b_z}\left(t\right)& {\mathrm{\ω}}^{b_y}\left(t\right)\\ {\mathrm{\ω}}^{b_z}\left(t\right)& 0& -{\mathrm{\ω}}^{b_x}\left(t\right)\\ -{\mathrm{\ω}}^{b_y}\left(t\right)& {\mathrm{\ω}}^{b_x}\left(t\right)& 0\end{array}\right]$

ω为行人导航系统中MEMS陀螺仪测得的角速率，

$\mathrm{\ω}\≈{\mathrm{\ω}}_{\mathrm{nb}}^b$

式中，

为行人导航系统的载体坐标系相对于地理坐标系转动的角速率： ${\mathrm{\ω}}^b\left(t\right)={\mathrm{\ω}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}^{\mathrm{bx}}\left(t\right)& {\mathrm{\ω}}^{\mathrm{by}}\left(t\right)& {\mathrm{\ω}}^{\mathrm{bz}}\left(t\right)\end{array}\right]}^T;$

进一步，在步骤四中利用公式：

α＝arctan(-C₃₁/C₃₃)

θ＝arcsin(Ｃ_３2)

ψ＝arctan(-C₁₂/C₂₂)

vⁿ(t)＝vⁿ(0)+∫aⁿ(t)dt

sⁿ(t)＝sⁿ(0)+∫vⁿ(t)dt

估计出单兵系统导航状态包含行人导航系统的三维位置向量、速度向量、姿态向量，即

其中，C_ij(i，j＝1，2，3)表示为捷联矩阵

中的元素，α为惯性测量单元的横滚角，θ为惯性测量单元的俯仰角，ψ为惯性测量单元的航向角；其中vⁿ(0)为初始速度，sⁿ(0)为初始位置，aⁿ(t)为载体的运动所产生的加速度：

aⁿ(t)＝fⁿ(t)-gⁿ

其中，fⁿ(t)为MEMS加速度计测得的比力沿着地理系方向的投影：

$f^n\left(t\right)=C_b^n\left(t\right)f^b\left(t\right)$

式中，f^b(t)＝[f^bx(t) f^by(t) f^bz(t)]^T为MEMS加速度计测得的比力。

进一步，在步骤五中利用公式：

$\frac{1}{N}\underset{k\&Element;{\mathrm{\&Omega;}}_n}{\mathrm{\&Sigma;}}(\frac{1}{{\mathrm{\&sigma;}}_a^2}{\left|\right|f_k-g\frac{{\stackrel{\&OverBar;}{f}}_n}{\left|\right|{\stackrel{\&OverBar;}{f}}_n\left|\right|}\left|\right|}^2+\frac{1}{{\mathrm{\&sigma;}}_{\mathrm{\&omega;}}^2}{\left|\right|{\mathrm{\&omega;}}_k\left|\right|}^2)<{\mathrm{\&gamma;}}^{\&prime;}$

判断出行人自主导航系统的零速区间，若上式成立则行人导航系统使用者脚步静止；

其中，

||·||表示求范数， $T\left(z_n\right)=-\frac{2}{N}\ln L_G\left(z_n\right),$ γ′＝-(2/N)ln(γ)，

ln(·)表示求以e为底数的对数，

分别表示MEMS加速度计和MEMS陀螺仪的噪声方差，γ取值由下式确定：

$P_{\mathrm{FA}}={\&Integral;}_{\{z_n:L\left(z_n\right)>\mathrm{\&gamma;}\}}p(z_n;H_0){\mathrm{dz}}_n=\mathrm{\&alpha;}.$

进一步，在步骤六中使用的卡尔曼滤波模型为：

$\left\{\begin{array}{c}{X}_k=F_{k,k-1}{X}_{k-1}+G_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+N_k\end{array}\right.$

被估计状态向量为：

X＝[φ^T δv^T δs^T]^T

量测向量为零速状态时导航解算速度值与航向误差信息：

$Z=\left[\begin{array}{c}\mathrm{\&delta;\&psi;}\\ \mathrm{\&delta;v}\end{array}\right]={\left[\begin{array}{cccc}\mathrm{\&delta;\&psi;}& v_x& v_y& v_z\end{array}\right]}^T$

量测阵为：

$H=\left[\begin{array}{ccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

状态转移矩阵为：

$F=\left[\begin{array}{ccc}{I}_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ \mathrm{\&Delta;tS}\left(f_t^n\right)& I_{3\&times;3}& I_{3\&times;3}\\ 0_{3\&times;3}& \mathrm{\&Delta;t}{I}_{3\&times;3}& I_{3\&times;3}\end{array}\right]$

其中，G_k-1为系统噪声驱动阵，W_k-1为系统激励噪声序列，H_k为量测阵；N_k为量测噪声序列，φ^T为姿态误差且φ^T＝[δ_ψ φ_y φ_z]，其中δy＝ψ_磁力计-ψ_导航解算，δs^T为位置误差，δv^T为速度误差且δv^T＝[v_x V_x v_z]，三个误差项都是三维的；

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

进一步，在步骤六中卡尔曼滤波只做时间更新的最优估计和均方误差为：

$\begin{array}{c}{\hat{X}}_{k+1/k}=E\left[X_{k+1}\right]\\ =E[F_{k+1,k}{X}_k+G_k{W}_k]\\ =F_{k+1.k}{\hat{X}}_k\end{array}$

$\begin{array}{c}{\hat{P}}_{k+1/k}=E\left[\mathrm{\&delta;}{\hat{X}}_{k+1/k}{\left(\mathrm{\&delta;}{\hat{X}}_{k+1/k}\right)}^T\right]\\ =F_k{P}_k{F}_k^T+{\mathrm{GQ}}_k{G}^T\end{array}$

其中，E[·]表述求取期望；

时间更新+量测更新时最优估计和均方误差为：

${\hat{X}}_{k+1}={\hat{X}}_{k+1/k}+K(Z_{k+1}-H_{k+1}{\hat{X}}_{k+1/k})$

$P_{k+1}^{-1}={\left({\hat{P}}_{k+1/k}\right)}^{-1}+{\left(H_{k+1}^T{R}_{k+1}^{-1}{H}_{k+1}\right)}^{-1}$

其中， $K=P_{k+1}{H}_{k+1}^T{R}_{k+1}^{-1}.$

本发明提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，通过采用行人自主导航系统中MEMS加速度计和MEMS磁力计的输出数据对系统进行初始对准，利用捷联惯导解算算法对行人自主导航系统的状态进行初始估计，通过静态检测算法当检测行人脚步运动状态，当检测到行人脚步静止时，设计基于卡尔曼滤波的零速校正误差补偿器采用输出校正的方式对行人自主导航系统导航解算结果进行校正，克服了MEMS惯性测量器件精度低、长时间使用时定位误差较大等缺陷，在不增加外在成本的条件下，实现了高精度的行人自主导航系统定位功能。本发明方法简单，稳定性和可靠性高，提高了一种便捷的导航定位方法，有效的提高了单兵自主导航系统的使用精度，对实现更高定位精度的行人自主导航定位具有重要意义。

附图说明

图1是本发明实施例提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法流程图；

图2是本发明实施例提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法原理示意图；

图3是本发明实施例提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法结果示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图及具体实施例对本发明的应用原理作进一步描述。

如图1所示，本发明实施例的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法包括以下步骤：

S101：手持PDA实时接收行人自主导航系统中脚步输出的量测信息；

S102：利用MEMS加速度计和MEMS磁力计输出值及式求得初始载体姿态信息；

S103：更新的方向余弦矩阵；

S104：利用行人导航系统的姿态矩阵及公式估计出单兵系统导航状态；

S105：判断出行人自主导航系统的零速区间；

S106：采用输出校正的方式对导航解算结果进行修正。

本发明的具体步骤为：

步骤一：将行人导航系统固定于单兵脚上，手持PDA实时接收并存储行人运动时系统输出的量测信息，任意时刻k接收到的系统输出信息为y_k，y_k＝[f_k ω_k m_k]^T；

其中， ${\mathrm{\&omega;}}_k={\left[\begin{array}{ccc}{\mathrm{\&omega;}}_k^x& {\mathrm{\&omega;}}_k^y& {\mathrm{\&omega;}}_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴陀螺仪输出的角速率信息； $f_k={\left[\begin{array}{ccc}{f}_k^x& f_k^y& f_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴加速度计输出的比力信息； $m_k={\left[\begin{array}{ccc}{m}_k^x& m_k^y& m_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴磁力计的输出信息；T表示转置操作；

步骤二：利用MEMS加速度计和MEMS磁力计输出值及式求得初始载体姿态信息

利用MEMS加速度计和MEMS磁力计输出值及式：

$g_b=C_b^n{g}_n$

$m_b=C_b^n{m}_n$

$r_b=C_b^n{r}_n$

得初始载体姿态信息

其中，g_n为重力向量

在地理坐标系中投影，g_n＝[0 0 g]^T；m_n为地磁向量

在地理坐标系中投影，m_n＝[m_ex m_ey m_ez]^T；g_b为重力向量

在载体坐标系下的投影，g_b＝[g_bx g_by g_bz]^T；m_b为地磁向量

在载体坐标系下的投影，m_b＝[m_bx m_by m_bz]^T；

步骤三：使用步骤一中存储的行人自主导航系统输出数据，及微分方程：

${\stackrel{.}{C}}_b^n\left(t\right)=C_b^n\left(t\right)\mathrm{\&Omega;}\left(t\right)$

更新的方向余弦矩阵

式中，

为t时刻载体系到地理系的捷联矩阵，Ω(t)为向量ω^b(t)的反对称阵，

$\mathrm{\&Omega;}\left(t\right)=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}^{b_z}\left(t\right)& {\mathrm{\&omega;}}^{b_y}\left(t\right)\\ {\mathrm{\&omega;}}^{b_z}\left(t\right)& 0& -{\mathrm{\&omega;}}^{b_x}\left(t\right)\\ -{\mathrm{\&omega;}}^{b_y}\left(t\right)& {\mathrm{\&omega;}}^{b_x}\left(t\right)& 0\end{array}\right]$

ω为行人导航系统中MEMS陀螺仪测得的输出角速率：

$\mathrm{\&omega;}\&ap;{\mathrm{\&omega;}}_{\mathrm{nb}}^b$

式中，

为行人导航系统的载体坐标系相对于地理坐标系的角速率：

${\mathrm{\&omega;}}^b\left(t\right)={\mathrm{\&omega;}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\&omega;}}^{\mathrm{bx}}\left(t\right)& {\mathrm{\&omega;}}^{\mathrm{by}}\left(t\right)& {\mathrm{\&omega;}}^{\mathrm{bz}}\left(t\right)\end{array}\right]}^T;$

步骤四：利用步骤三中求出的行人导航系统的姿态矩阵及公式估计出单兵系统导航状态

包含行人导航系统的三维位置向量、速度向量、姿态向量，即

α＝arctan(-C₃₁/C₃₃)

θ＝arcsin(C₃₂)

ψ＝arctan(-C₁₂/C₂₂)

vⁿ(t)＝vⁿ(0)+∫aⁿ(t)dt

sⁿ(t)＝sⁿ(0)+∫vⁿ(t)dt

估计出单兵系统导航状态

包含行人导航系统的三维位置向量、速度向量、姿态向量即

其中，C_ij(i，j＝1，2，3)表示为捷联矩阵

中的元素，α为IMU的横滚角，θ为IMU的俯仰角，ψ为IMU的航向角；其中vⁿ(0)为初始速度，sⁿ(0)为初始位置，vⁿ(0)＝[0 0 0]^T，sⁿ(0)＝[0 0 0]^T；aⁿ(t)为载体的运动所产生的加速度：

aⁿ(t)＝fⁿ(t)-gⁿ

其中，fⁿ(t)为MEMS加速度计测得的比力沿着地理系方向的投影：

$f^n\left(t\right)=C_b^n\left(t\right)f^b\left(t\right)$

式中，f^b(t)＝[f^bx(t) f^by(t) f^bz(t)]^T为MEMS加速度计测得的比力；

步骤五：利用步骤一中得到的行人导航系统中MEMS加速度计和MEMS陀螺仪的输出值及公式判断出行人自主导航系统的零速区间，利用步骤一中得到的行人导航系统中MEMS加速度计和MEMS陀螺仪的输出值及公式：

$\frac{1}{N}\underset{k\&Element;{\mathrm{\&Omega;}}_n}{\mathrm{\&Sigma;}}(\frac{1}{{\mathrm{\&sigma;}}_a^2}{\left|\right|f_k-g\frac{{\stackrel{\&OverBar;}{f}}_n}{\left|\right|{\stackrel{\&OverBar;}{f}}_n\left|\right|}\left|\right|}^2+\frac{1}{{\mathrm{\&sigma;}}_{\mathrm{\&omega;}}^2}{\left|\right|{\mathrm{\&omega;}}_k\left|\right|}^2)<{\mathrm{\&gamma;}}^{\&prime;}$

判断出行人自主导航系统的零速区间，若上式成立则IMU静止；

其中，

||·||表示求范数， $T\left(z_n\right)=-\frac{2}{N}\ln L_G\left(z_n\right),$ γ′＝-(2/N)ln(γ)，

ln(·)表示求以e为底数的对数，分别表示MEMS加速度计和MEMS陀螺仪的噪声方差，γ取值由下式确定：

$P_{\mathrm{FA}}={\&Integral;}_{\{z_n;L\left(z_n\right)>\mathrm{\&gamma;}|}p(z_n;H_0)d{z}_n=\mathrm{\&alpha;};$

步骤六：利用步骤六中判断出得零速状态，使用基于卡尔曼滤波的零速误差校正器，采用输出校正的方式对导航解算结果进行修正，零速校正卡尔曼滤波模型为：

$\left\{\begin{array}{c}{X}_k=F_{k,k-1}{X}_{k-1}+G_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+N_k\end{array}\right.$

被估计状态向量为：

X＝[φ^T δv^T δs^T]^T

量测向量为零速状态时导航解算速度值与航向误差信息：

$Z=\left[\begin{array}{c}\mathrm{\&delta;\&psi;}\\ \mathrm{\&delta;v}\end{array}\right]={\left[\begin{array}{cccc}\mathrm{\&delta;\&psi;}& v_x& v_y& v_z\end{array}\right]}^T$

量测阵为：

$H=\left[\begin{array}{ccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

状态转移矩阵为：

$F=\left[\begin{array}{ccc}{I}_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ \mathrm{\&Delta;tS}\left({f_t}^n\right)& I_{3\&times;3}& I_{3\&times;3}\\ 0_{3\&times;3}& \mathrm{\&Delta;t}{I}_{3\&times;3}& I_{3\&times;3}\end{array}\right]$

其中，G_k-1为系统噪声驱动阵；Ｗ_ｋ－１为系统激励噪声序列；H_k为量测阵；N_k为量测噪声序列；φ^T为姿态误差且φ^T＝[δ_ψ φ_y φ_z]，其中δψ＝ψ_磁力计-ψ_导航解算，δs^T为位置误差，δv^T且δv^T＝[v_x v_y v_z]为速度误差，三个误差项都是三维的；

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

存储各时刻的状态估计

估计均方误差阵P_k和滤波增益K，若k+1时刻脚步MIMU零速检测结果为运动状态时，则卡尔曼滤波只做时间更新，

若k+1时刻脚步MIMU零速检测结果为静止时，则KF做完整更新，时间更新+量测更新，卡尔曼滤波只做时间更新：

最优估计：

$\begin{array}{c}{\hat{X}}_{k+1/k}=E\left[X_{k+1}\right]\\ =E[F_{k+1,k}{X}_k+G_k{W}_k]\\ =F_{k+1,k}{\hat{X}}_k\end{array}$

均方误差：

$\begin{array}{c}{\hat{P}}_{k+1/k}=E\left[\mathrm{\&delta;}{\hat{X}}_{k+1/k}{\left(\mathrm{\&delta;}{\hat{X}}_{k+1/k}\right)}^T\right]\\ =F_k{P}_k{F}_k^T+G{Q}_k{G}^T\end{array}$

其中，E[·]表述求取期望；

若k+1时刻脚步MIMU零速检测结果为静止时，则KF做完整更新，时间更新+量测更新：

均方误差：

$\begin{array}{c}{P}_{k+1}^{-1}={({\hat{P}}_{k+1}/k)}^{-1}+{\left(H_{k+1}^T{R}_{k+1}^{-1}{H}_{k+1}\right)}^{-1}\\ K=P_{k+1}{H}_{k+1}^T{R}_{k+1}^{-1}\end{array}$

最优估计：

${\hat{X}}_{k+1}={\hat{X}}_{k+1/k}+K(Z_{k+1}-H_{k+1}{\hat{X}}_{k+1/k})$

通过以上循环即可估计出任意时刻k的状态量。

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

利用自研三轴惯性测量组件(集成了微机械系统三轴磁力计、加速度计、陀螺仪)搭建真实双IMU系统单兵导航系统模型，设备参数如表1所示，通过合理的试验验证基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法的可靠性、实用性、准确性，试验场景选在室外空旷的哈尔滨工程大学军工操场；

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

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

单兵导航自主定位系统采样频率：100Hz

微机械系统陀螺标准偏差：σ_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定位信息作为真实轨迹的参考；

实验内容：测试者围绕长方形足球场进行场地行走一周(约500米)，行走时间约为300秒，实验过程中实时采集行人自主导航系统中各传感器的输出值，并对采集得到的实验数据进行离线分析；实验结果如图3所示；

基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法的定位结果如图2所示，为了更形象的说明本大明提出的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法的优越性，表2给出了使用该定位方法时定位结果的均方根误差RMS，其中计算真值为GPS定位信息，在行走时间大于300秒的情况下定位误差仍然保持在4.2m内，小于单兵行走距离的1％，实验证明本发明提出的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法定位结果比较理想，可以满足短时间内单兵作战人员的使用要求。

表2

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

Claims (9)

1.一种基于捷联惯导解算和零速校正的自主导航系统定位方法，其特征在于，该基于MEMS惯性测量行人自主导航系统定位的方法包括以下步骤：

步骤一：将行人自主导航系统固定于单兵脚上，手持PDA实时接收并存储行人运动时系统输出的量测信息，任意时刻k接收到的系统输出信息为y_k；

步骤二：利用MEMS加速度计和MEMS磁力计输出值，及公式求得初始载体姿态信息

步骤三：使用步骤一中存储的行人自主导航系统输出数据，及微分方程更新的方向余弦矩阵

步骤四：利用步骤三中求出的行人自主导航系统的姿态矩阵

及公式估计出单兵系统导航状态

包含行人导航系统的三维位置向量、速度向量、姿态向量，即

步骤五：利用步骤一中得到的行人导航系统中MEMS加速度计和MEMS陀螺仪的输出值及公式判断出行人自主导航系统的零速区间；

步骤六：利用步骤五中判断出得零速状态，使用基于卡尔曼滤波的零速误差校正器，采用输出校正的方式对导航解算结果进行修正；

通过以上循环即可估计出任意时刻k的状态量。

2.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位的方法，其特征在于，在步骤一中，行人导航系统包括：MEMS陀螺仪、MEMS加速度计、MEMS磁力计。

3.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，其特征在于，在步骤一中，任意时刻k接收到的系统输出信息为y_k＝[f_k ω_k m_k]^T；

其中， ${\mathrm{\&omega;}}_k={\left[\begin{array}{ccc}{\mathrm{\&omega;}}_k^x& {\mathrm{\&omega;}}_k^y& {\mathrm{\&omega;}}_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴陀螺仪输出的角速率信息； $f_k={\left[\begin{array}{ccc}{f}_k^x& f_k^y& f_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴加速度计输出的比力信息； $m_k={\left[\begin{array}{ccc}{m}_k^x& m_k^y& m_k^z\end{array}\right]}^T$ 为k时刻MEMS三轴磁力计的输出信息；T表示转置操作。

4.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，其特征在于，在步骤二中利用MEMS加速度计和MEMS磁力计输出值及式：

$g_b=C_b^n{g}_n$

$m_b=C_b^n{m}_n$

$r_b=C_b^n{r}_n$

得到初始载体姿态信息

其中，g_n为重力向量

在地理坐标系中投影，g_n＝[0 0 g]^T；m_n为地磁向量

在地理坐标系中投影，m_n=[m_ex m_ey m_ez]^T；g_b为重力向量

在载体坐标系下的投影，g_b＝[g_bx g_by g_bz]^T；m_b为地磁向量

在载体坐标系下的投影， $m_b={\left[\begin{array}{ccc}{m}_{\mathrm{bx}}& m_{\mathrm{by}}& m_{\mathrm{bz}}\end{array}\right]}^T.$

5.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，其特征在于，在步骤三中使用微分方程：

$C_b^n\left(t\right)=C_b^n\left(t\right)\mathrm{\&Omega;}\left(t\right)$

更新的方向余弦矩阵

式中，为t时刻载体系到地理系的捷联矩阵，Ω(t)为向量ω^b(t)的反对称阵，

$\mathrm{\&Omega;}\left(t\right)=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}^{b_z}\left(t\right)& {\mathrm{\&omega;}}^{b_y}\left(t\right)\\ {\mathrm{\&omega;}}^{b_z}\left(t\right)& 0& -{\mathrm{\&omega;}}^{b_x}\left(t\right)\\ -{\mathrm{\&omega;}}^{b_y}\left(t\right)& {\mathrm{\&omega;}}^{b_x}\left(t\right)& 0\end{array}\right]$

ω为行人导航系统中MEMS陀螺仪测得的角速率，

$\mathrm{\&omega;}\&ap;{\mathrm{\&omega;}}_{\mathrm{nb}}^b$

式中，为行人导航系统的载体坐标系相对于地理坐标系转动的角速率：

${\mathrm{\&omega;}}^b\left(t\right)={\mathrm{\&omega;}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\&omega;}}^{\mathrm{bx}}\left(t\right)& {\mathrm{\&omega;}}^{\mathrm{by}}\left(t\right)& {\mathrm{\&omega;}}^{\mathrm{bz}}\left(t\right)\end{array}\right]}^T.$

6.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，其特征在于，在步骤四中利用公式：

α＝arctan(-C₃₁/C₃₃)

θ＝arcsin(C₃₂)

ψ＝arctan(-C₁₂/C₂₂)

vⁿ(t)＝vⁿ(0)+∫aⁿ(t)dt

sⁿ＝(t)sⁿ(0)+∫vⁿ(t)dt

估计出单兵系统导航状态

包含行人导航系统的三维位置向量、速度向量、姿态向量，即

其中，C_i(i，j＝1，2，3)表示为捷联矩阵

中的元素，α为惯性测量单元的横滚角，θ为惯性测量单元的俯仰角，ψ为惯性测量单元的航向角；其中vⁿ(O)为初始速度，sⁿ(O)为初始位置，aⁿ(t)为载体的运动所产生的加速度：

aⁿ(t)＝fⁿ(t)-gⁿ

其中，fⁿ(t)为MEMS加速度计测得的比力沿着地理系方向的投影：

$f^n\left(t\right)=C_b^n{f}^b\left(t\right)$

式中， $f^b\left(t\right)={\left[\begin{array}{ccc}{f}^{\mathrm{bx}}\left(t\right)& f^{\mathrm{by}}\left(t\right)& f^{\mathrm{bz}}\left(t\right)]\end{array}\right]}^T$ 为MEMS加速度计测得的比力。

7.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，其特征在于，在步骤五中利用公式：

$\frac{1}{N}\underset{k\&Element;{\mathrm{\&Omega;}}_n}{\mathrm{\&Sigma;}}(\frac{1}{{\mathrm{\&sigma;}}_a^2}{\left|\right|f_k-g\frac{{\stackrel{\&OverBar;}{f}}_n}{\left|\right|{\stackrel{\&OverBar;}{f}}_n\left|\right|}\left|\right|}^2+\frac{1}{{\mathrm{\&sigma;}}_{\mathrm{\&omega;}}^2}{\left|\right|{\mathrm{\&omega;}}_k\left|\right|}^2)<{\mathrm{\&gamma;}}^{\&prime;}$

判断出行人自主导航系统的零速区间，若上式成立则行人导航系统使用者脚步静止；

其中，

||·||表示求范数； $T\left(z_n\right)=-\frac{2}{N}\ln L_G\left(z_n\right),$ γ′＝-(2/N)ln(γ)；

ln(·)表示求以e为底数的对数；

分别表示MEMS加速度计和MEMS陀螺仪的噪声方差；γ取值由下式确定：

$P_{\mathrm{FA}}={\&Integral;}_{\{z_n:L\left(z_n\right)>\mathrm{\&gamma;}\}}p(z_n;H_0){\mathrm{dz}}_n=\mathrm{\&alpha;}.$

8.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，其特征在于，在步骤六中使用的卡尔曼滤波模型为：

$\left\{\begin{array}{c}{X}_k=F_{k,k-1}{X}_{k-1}+G_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+N_k\end{array}\right.$

被估计状态向量为：

X＝[φ^T δv^T δs^T]^T

量测向量为零速状态时导航解算速度值与航向误差信息：

$Z=\left[\begin{array}{c}\mathrm{\&delta;\&psi;}\\ \mathrm{\&delta;v}\end{array}\right]={\left[\begin{array}{cccc}\mathrm{\&delta;\&psi;}& v_x& v_y& v_z\end{array}\right]}^T$

量测阵为：

$H=\left[\begin{array}{ccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

状态转移矩阵为：

$F=\left[\begin{array}{ccc}{I}_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ \mathrm{\&Delta;tS}\left(f_t^n\right)& I_{3\&times;3}& I_{3\&times;3}\\ 0_{3\&times;3}& \mathrm{\&Delta;t}{I}_{3\&times;3}& I_{3\&times;3}\end{array}\right]$

其中，G_k-1为系统噪声驱动阵，W_k-1为系统激励噪声序列，H_k为量测阵，N_k为量测噪声序列，φ^T为姿态误差且φ^T＝[δ_ψφ_y φ_z]，其中，δψ＝ψ_磁力计-ψ_导航解算，δs^T为位置误差，δv^T为速度误差且δv^T＝[v_x v_y v_z]，三个误差项都是三维的，

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

9.如权利要求1所述的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，其特征在于，在步骤六中卡尔曼滤波只做时间更新的最优估计和均方误差为：

$\begin{array}{c}{\hat{X}}_{k+1/k}=E\left[X_{k+1}\right]\\ =E[F_{k+1,k}{X}_k+G_k{W}_k]\\ =F_{k+1,k}{\hat{X}}_k\end{array}$

$\begin{array}{c}{\hat{P}}_{k+1/k}=E\left[\mathrm{\&delta;}{\hat{X}}_{k+1/k}{\left(\mathrm{\&delta;}{\hat{X}}_{k+1/k}\right)}^T\right]\\ =F_k{P}_k{F}_k^T+G{Q}_k{G}^k\end{array}$

其中，E[·]表述求取期望；

时间更新+量测更新时最优估计和均方误差为：

${\hat{X}}_{k+1}={\hat{X}}_{k+1/k}+K(Z_{k+1}-H_{k+1}{\hat{X}}_{k+1/k})$

$P_{k+1}^{-1}={\left({\hat{P}}_{k+1/k}\right)}^{-1}+{\left(H_{k+1}^T{R}_{k+1}^{-1}{H}_{k+1}\right)}^{-1}$

$K=P_{k+1}{H}_{k+1}^T{R}_{k+1}^{-1}$

存储各时刻的状态估计

估计均方误差阵P_k和滤波增益K，若k+1时刻脚步MIMU零速检测结果为运动状态时，则卡尔曼滤波只做时间更新，

若k+1时刻脚步MIMU零速检测结果为静止时，则KF做完整更新，时间更新+量测更新。

Images