CN103616030A

CN103616030A - Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction

Info

Publication number: CN103616030A
Application number: CN201310566710.6A
Authority: CN
Inventors: 于飞; 于春阳; 兰海钰; 周广涛; 卢宝峰; 史宏洋; 林萌萌; 张丽丽; 赵博; 李佳璇; 白红美
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2013-11-15
Filing date: 2013-11-15
Publication date: 2014-03-05

Abstract

The invention discloses a positioning method for an autonomous navigation system based on strapdown inertial navigation solution and zero-speed correction. The output data of the MEMS accelerometer and the MEMS magnetometer in the pedestrian autonomous navigation system are used to initially align the system, and the strapdown is used to The inertial navigation solution algorithm estimates the state of the pedestrian autonomous navigation system. When the pedestrian's footsteps are detected by the static detection algorithm, a zero-speed correction error compensator based on Kalman filter is designed to correct the pedestrian autonomous navigation system navigation solution by output correction. The calculation results are corrected, which overcomes the defects of low precision of MEMS inertial measurement devices and large positioning errors when used for a long time, and realizes the high-precision pedestrian autonomous navigation system positioning function without increasing external costs. The method of the invention is simple, high in stability and reliability, effectively improves the use accuracy of the individual soldier's autonomous navigation system, and has great significance for realizing pedestrian autonomous navigation and positioning with higher positioning accuracy.

Description

Positioning method of autonomous navigation system based on strapdown inertial navigation solution and zero speed correction

技术领域technical field

本发明属于导航定位技术领域，尤其涉及一种基于捷联惯导解算和零速校正的自主导航系统定位方法。The invention belongs to the technical field of navigation and positioning, and in particular relates to a positioning method for an autonomous navigation system based on strapdown inertial navigation solution and zero-speed correction.

背景技术Background technique

行人自主导航系统(包括MEMS三轴磁力计、MEMS三轴加速度计、MEMS三轴陀螺仪)主要用于个人在已知或未知条件下的自主导航和实时定位，辅助完成各类型的紧急救援任务。当火灾、地震等紧急意外发生时，事故发生现场可能存在可视性降低、固有环境改变等不利于救援的情况，营救人员无法快速准确的辨别自身位置。此时，行人导航系统提供的定位信息可为营救人员提供有效的技术保障。Pedestrian autonomous navigation system (including MEMS three-axis magnetometer, MEMS three-axis accelerometer, MEMS three-axis gyroscope) is mainly used for personal autonomous navigation and real-time positioning under known or unknown conditions, assisting in the completion of various types of emergency rescue tasks . When emergency accidents such as fires and earthquakes occur, there may be situations at the scene of the accident that are not conducive to rescue, such as reduced visibility and inherent environmental changes. Rescuers cannot quickly and accurately identify their own location. At this time, the positioning information provided by the pedestrian navigation system can provide effective technical support for rescuers.

现有的具有单兵导航定位功能的产品绝大数主要依靠GPS(GlobalPositioning System)进行定位，但当GPS信号缺失时，系统将无法工作，继而无法满足行人自主导航系统自主、实时、稳定的定位要求。因此，研究在无GPS情况下的单兵自主定位技术具有一定应用价值。基于MEMS惯性测量技术的行人自主导航系统工作时不依赖于任何外部信息，具有良好的抗干扰性，因此，研究基于MEMS惯性测量技术的行人自主导航系统具有良好的应用价值。但目前MIMU的测量精度总体上较低，传感器误差随时间的漂移比较严重，导致MIMU的导航误差积累急剧增大。The vast majority of existing products with individual navigation and positioning functions mainly rely on GPS (Global Positioning System) for positioning, but when the GPS signal is missing, the system will not work, and then cannot satisfy the autonomous, real-time, and stable positioning of pedestrian autonomous navigation systems Require. Therefore, it has certain application value to study the individual soldier's autonomous positioning technology without GPS. The pedestrian autonomous navigation system based on MEMS inertial measurement technology does not depend on any external information when it works, and has good anti-interference performance. Therefore, the study of pedestrian autonomous navigation system based on MEMS inertial measurement technology has good application value. However, at present, the measurement accuracy of MIMU is generally low, and the sensor error drifts seriously over time, which leads to a sharp increase in the accumulation of MIMU navigation errors.

发明内容Contents of the invention

本发明实施例的目的在于提供一种基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，旨在解决现有的MEMS惯性测量器件精度较低，基于MEMS惯性测量技术的行人自主导航系统长时间使用时导航定位误差较大的问题。The purpose of the embodiment of the present invention is to provide a MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction, aiming to solve the problem of low precision of existing MEMS inertial measurement devices and problems based on MEMS inertial measurement technology. The problem of large navigation and positioning errors when the pedestrian autonomous navigation system is used for a long time.

本发明实施例是这样实现的，一种基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，该基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法包括以下步骤：The embodiment of the present invention is achieved in this way, a MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation and zero-speed correction, the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation and zero-speed correction The method includes the following steps:

步骤一：将行人自主导航系统固定于单兵脚上，手持PDA实时接收并存储行人运动时系统输出的量测信息，任意时刻k接收到的系统输出信息为y_k；Step 1: Fix the pedestrian autonomous navigation system on the foot of the individual soldier, and the handheld PDA receives and stores the measurement information output by the system when the pedestrian is moving in real time, and the system output information received at any time k is y _k ;

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

Step 2: Use the output values of the MEMS accelerometer and MEMS magnetometer, and the formula to obtain the initial carrier attitude information

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

Step 3: Use the output data of the pedestrian autonomous navigation system stored in step 1, and the direction cosine matrix updated by the differential equation

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

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

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

Step 4: Use the attitude matrix of the pedestrian autonomous navigation system obtained in step 3

and the formula to estimate the navigation state of the individual soldier system

Contains the three-dimensional position vector, velocity vector and attitude vector of the pedestrian navigation system, namely

步骤五：利用步骤一中得到的行人导航系统中MEMS加速度计和MEMS陀螺仪的输出值及公式判断出行人自主导航系统的零速区间；Step 5: Use the output values and formulas of the MEMS accelerometer and MEMS gyroscope in the pedestrian navigation system obtained in step 1 to determine the zero-speed interval of the pedestrian autonomous navigation system;

步骤六：利用步骤五中判断出得零速状态，使用基于卡尔曼滤波的零速误差校正器，采用输出校正的方式对导航解算结果进行修正；Step 6: Use the zero-speed state judged in step 5, use the zero-speed error corrector based on Kalman filter, and use the output correction method to correct the navigation solution result;

通过以上循环即可估计出任意时刻k的状态量。Through the above cycle, the state quantity of k at any time can be estimated.

进一步，在步骤一中，行人导航系统包括：MEMS陀螺仪、MEMS加速度计、MEMS磁力计。Further, in step 1, the pedestrian navigation system includes: MEMS gyroscope, MEMS accelerometer, MEMS magnetometer.

进一步，在步骤一中，任意时刻k接收到的系统输出信息为y_k＝[f_k ω_k m_k]^T；Further, in step 1, the system output information received at any time k is y _k =[f _k ω _k m _k ] ^T ;

其中， $ω_{k} = {[\begin{matrix} ω_{k}^{x} & ω_{k}^{y} & ω_{k}^{z} \end{matrix}]}^{T}$ 为k时刻MEMS三轴陀螺仪输出的角速率信息； $f_{k} = {[\begin{matrix} f_{k}^{x} & f_{k}^{y} & f_{k}^{z} \end{matrix}]}^{T}$ 为k时刻MEMS三轴加速度计输出的比力信息； $m_{k} = {[\begin{matrix} m_{k}^{x} & m_{k}^{y} & m_{k}^{z} \end{matrix}]}^{T}$ 为k时刻MEMS三轴磁力计的输出信息；T表示转置操作。in, $ω_{k} = {[\begin{matrix} ω_{k}^{x} & ω_{k}^{the y} & ω_{k}^{z} \end{matrix}]}^{T}$ is the angular rate information output by the MEMS three-axis gyroscope at time k; $f_{k} = {[\begin{matrix} f_{k}^{x} & f_{k}^{the y} & f_{k}^{z} \end{matrix}]}^{T}$ is the specific force information output by the MEMS triaxial accelerometer at time k; $m_{k} = {[\begin{matrix} m_{k}^{x} & m_{k}^{the y} & m_{k}^{z} \end{matrix}]}^{T}$ is the output information of the MEMS three-axis magnetometer at time k; T represents the transpose operation.

进一步，在步骤二中利用MEMS加速度计和MEMS磁力计输出值及式：Further, use MEMS accelerometer and MEMS magnetometer output value and formula in step 2:

${g g}_{b b} = = {C C}_{b b}^{n no} {g g}_{n no}$

${m m}_{b b} = = {C C}_{b b}^{n no} {m m}_{n no}$

${r r}_{b b} = = {C C}_{b b}^{n no} {r r}_{n no}$

得初始载体姿态信息

Get the initial carrier attitude information

其中，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。Among them, g _n is the gravity vector

Projected in the geographic coordinate system, g _b = [0 0 g] ^T ; m _n is the geomagnetic vector

Projected in the geographic coordinate system, m _n = [m _ex m _ey m _ez ] ^T ; g _b is the gravity vector

Projection under the carrier coordinate system, g _b =[g _bx g _by g _bz ] ^T ; m _b is the geomagnetic vector

Projection in the carrier coordinate system, m _b =[m _bx m _by m _bz ] ^T .

进一步，在步骤三中使用微分方程：Further, use the differential equation in step three:

${\overset{\cdot \cdot}{C C}}_{b b}^{n no} ((t t)) = = {C C}_{b b}^{n no} ((t t)) Ω Ω ((t t))$

更新的方向余弦矩阵 updated direction cosine matrix

式中，为t时刻载体系到地理系的捷联矩阵，Ω(t)为向量ω^b(t)的反对称阵，In the formula, is the strapdown matrix carrying the system to the geographic system at time t, Ω(t) is the antisymmetric matrix of vector ω ^b (t),

$Ω Ω ((t t)) = = [\begin{matrix} 00 & - - {ω ω}^{{b b}_{z z}} ((t t)) & {ω ω}^{{b b}_{y the y}} ((t t)) \\ {ω ω}^{{b b}_{z z}} ((t t)) & 00 & - - {ω ω}^{{b b}_{x x}} ((t t)) \\ - - {ω ω}^{{b b}_{y the y}} ((t t)) & {ω ω}^{{b b}_{x x}} ((t t)) & 00 \end{matrix}]$

ω为行人导航系统中MEMS陀螺仪测得的角速率，ω is the angular rate measured by the MEMS gyroscope in the pedestrian navigation system,

$ω ω \approx \approx {ω ω}_{nb nb}^{b b}$

式中，

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

ω^{b} (t) = ω_{nb}^{b} = {[\begin{matrix} ω^{bx} (t) & ω^{by} (t) & ω^{bz} (t) \end{matrix}]}^{T};

In the formula,

is the angular rate at which the carrier coordinate system of the pedestrian navigation system rotates relative to the geographic coordinate system:

ω^{b} (t) = ω_{nb}^{b} = {[\begin{matrix} ω^{bx} (t) & ω^{by} (t) & ω^{bz} (t) \end{matrix}]}^{T};

进一步，在步骤四中利用公式：Further, use the formula in step four:

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

θ＝arcsin(Ｃ_３2)θ=arcsin(C ₃₂ )

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

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

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

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

Estimated Soldier System Navigation Status Contains the three-dimensional position vector, velocity vector and attitude vector of the pedestrian navigation system, namely

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

中的元素，α为惯性测量单元的横滚角，θ为惯性测量单元的俯仰角，ψ为惯性测量单元的航向角；其中vⁿ(0)为初始速度，sⁿ(0)为初始位置，aⁿ(t)为载体的运动所产生的加速度：Among them, C _ij (i, j=1, 2, 3) is expressed as a strapdown matrix

The elements in , α is the roll angle of the IMU, θ is the pitch angle of the IMU, and ψ is the heading angle of the IMU; where v ⁿ (0) is the initial velocity, and s ⁿ (0) is the initial position , a ⁿ (t) is the acceleration generated by the motion of the carrier:

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

其中，fⁿ(t)为MEMS加速度计测得的比力沿着地理系方向的投影：Among them, f ⁿ (t) is the projection of the specific force measured by the MEMS accelerometer along the direction of the geographic system:

${f f}^{n no} ((t t)) = = {C C}_{b b}^{n no} ((t t)) {f f}^{b b} ((t t))$

式中，f^b(t)＝[f^bx(t) f^by(t) f^bz(t)]^T为MEMS加速度计测得的比力。In the formula, f ^b (t) = [f ^bx (t) f ^by (t) f ^bz (t)] ^T is the specific force measured by the MEMS accelerometer.

进一步，在步骤五中利用公式：Further, use the formula in step five:

$\frac{11}{N N} \underset{k k &Element; &Element; {Ω Ω}_{n no}}{Σ Σ} ((\frac{11}{{σ σ}_{a a}^{22}} {| | | | {f f}_{k k} - - g g \frac{{\overset{&OverBar; &OverBar;}{f f}}_{n no}}{| | | | {\overset{&OverBar; &OverBar;}{f f}}_{n no} | | | |} | | | |}^{22} + + \frac{11}{{σ σ}_{ω ω}^{22}} {| | | | {ω ω}_{k k} | | | |}^{22})) < < {γ γ}^{' '}$

判断出行人自主导航系统的零速区间，若上式成立则行人导航系统使用者脚步静止；Judging the zero-speed range of the pedestrian autonomous navigation system, if the above formula is established, the footsteps of the pedestrian navigation system user are still;

其中，

||·||表示求范数，

T (z_{n}) = - \frac{2}{N} \ln L_{G} (z_{n}),

γ′＝-(2/N)ln(γ)，

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

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

||·|| means to find the norm,

T (z_{no}) = - \frac{2}{N} \ln L_{G} (z_{no}),

γ'=-(2/N)ln(γ),

ln( ) means to find the logarithm with e as the base,

Represent the noise variance of the MEMS accelerometer and MEMS gyroscope respectively, and the value of γ is determined by the following formula:

${P P}_{FA FA} = = {&Integral; &Integral;}_{{{{z z}_{n no} : : L L (({z z}_{n no})) > > γ γ}}} p p (({z z}_{n no};; {H h}_{00})) {dz dz}_{n no} = = α α . .$

进一步，在步骤六中使用的卡尔曼滤波模型为：Further, the Kalman filter model used in step six is:

$\{\begin{matrix} {X x}_{k k} = = {F f}_{k k,, k k - - 11} {X x}_{k k - - 11} + + {G G}_{k k - - 11} {W W}_{k k - - 11} \\ {Z Z}_{k k} = = {H h}_{k k} {X x}_{k k} + + {N N}_{k k} \end{matrix}$

被估计状态向量为：The estimated state vector is:

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

量测向量为零速状态时导航解算速度值与航向误差信息：When the measurement vector is at zero speed, the navigation calculation speed value and heading error information:

$Z Z = = [\begin{matrix} δψ δψ \\ δv δv \end{matrix}] = = {[\begin{matrix} δψ δψ & {v v}_{x x} & {v v}_{y the y} & {v v}_{z z} \end{matrix}]}^{T T}$

量测阵为：The measurement array is:

$H h = = [\begin{matrix} 00 & 00 & 00 & 11 & 00 & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 11 & 00 & 00 & 00 & 00 \\ 00 & 00 & 00 & 00 & 00 & 11 & 00 & 00 & 00 \\ 11 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \end{matrix}]$

状态转移矩阵为：The state transition matrix is:

$F f = = [\begin{matrix} {I I}_{33 \times \times 33} & 00_{33 \times \times 33} & 00_{33 \times \times 33} \\ ΔtS ΔtS (({f f}_{t t}^{n no})) & {I I}_{33 \times \times 33} & {I I}_{33 \times \times 33} \\ 00_{33 \times \times 33} & Δt Δt {I I}_{33 \times \times 33} & {I I}_{33 \times \times 33} \end{matrix}]$

其中，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]，三个误差项都是三维的；

为沿地理系的载体运动加速度的反对称阵。Among them, G _k-1 is the system noise driving array, W _k-1 is the system excitation noise sequence, H _k is the measurement array; N _k is the measurement noise sequence, φ ^T is the attitude error and φ ^T = [δ _ψ φ _y φ _z ], where δy=ψ _magnetometer -ψ _{navigation solution} , δs ^T is the position error, δv ^T is the velocity error and δv ^T =[v _x V _x v _z ], the three error terms are all three-dimensional;

is the antisymmetric matrix of the acceleration of the carrier motion along the geographic system.

进一步，在步骤六中卡尔曼滤波只做时间更新的最优估计和均方误差为：Further, in step 6, the optimal estimate and mean square error of Kalman filter only doing time update are:

$\begin{matrix} {\overset{^^}{X x}}_{k k + + 11 / / k k} = = E E. [[{X x}_{k k + + 11}]] \\ = = E E. [[{F f}_{k k + + 11,, k k} {X x}_{k k} + + {G G}_{k k} {W W}_{k k}]] \\ = = {F f}_{k k + + 11 . . k k} {\overset{^^}{X x}}_{k k} \end{matrix}$

$\begin{matrix} {\overset{^^}{P P}}_{k k + + 11 / / k k} = = E E. [[δ δ {\overset{^^}{X x}}_{k k + + 11 / / k k} {((δ δ {\overset{^^}{X x}}_{k k + + 11 / / k k}))}^{T T}]] \\ = = {F f}_{k k} {P P}_{k k} {F f}_{k k}^{T T} + + {GQ GQ}_{k k} {G G}^{T T} \end{matrix}$

其中，E[·]表述求取期望；Among them, E[ ] expresses the expectation;

时间更新+量测更新时最优估计和均方误差为：The optimal estimate and mean square error for time update + measurement update are:

${\overset{^^}{X x}}_{k k + + 11} = = {\overset{^^}{X x}}_{k k + + 11 / / k k} + + K K (({Z Z}_{k k + + 11} - - {H h}_{k k + + 11} {\overset{^^}{X x}}_{k k + + 11 / / k k}))$

${P P}_{k k + + 11}^{- - 11} = = {(({\overset{^^}{P P}}_{k k + + 11 / / k k}))}^{- - 11} + + {(({H h}_{k k + + 11}^{T T} {R R}_{k k + + 11}^{- - 11} {H h}_{k k + + 11}))}^{- - 11}$

其中， $K = P_{k + 1} H_{k + 1}^{T} R_{k + 1}^{- 1} .$ in, $K = P_{k + 1} h_{k + 1}^{T} R_{k + 1}^{- 1} .$

本发明提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法，通过采用行人自主导航系统中MEMS加速度计和MEMS磁力计的输出数据对系统进行初始对准，利用捷联惯导解算算法对行人自主导航系统的状态进行初始估计，通过静态检测算法当检测行人脚步运动状态，当检测到行人脚步静止时，设计基于卡尔曼滤波的零速校正误差补偿器采用输出校正的方式对行人自主导航系统导航解算结果进行校正，克服了MEMS惯性测量器件精度低、长时间使用时定位误差较大等缺陷，在不增加外在成本的条件下，实现了高精度的行人自主导航系统定位功能。本发明方法简单，稳定性和可靠性高，提高了一种便捷的导航定位方法，有效的提高了单兵自主导航系统的使用精度，对实现更高定位精度的行人自主导航定位具有重要意义。The MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction provided by the present invention uses the output data of the MEMS accelerometer and MEMS magnetometer in the pedestrian autonomous navigation system to initially align the system, and utilizes the fast The joint inertial navigation solution algorithm initially estimates the state of the pedestrian autonomous navigation system. When the static detection algorithm is used to detect the pedestrian's footstep movement state, when the pedestrian's footsteps are detected to be stationary, a zero-speed correction error compensator based on Kalman filter is designed to use the output The correction method corrects the navigation solution results of the pedestrian autonomous navigation system, overcomes the defects of low precision of MEMS inertial measurement devices and large positioning errors during long-term use, and realizes high-precision positioning without increasing external costs. Pedestrian autonomous navigation system positioning function. The method of the invention is simple, high in stability and reliability, improves a convenient navigation and positioning method, effectively improves the use accuracy of an individual soldier's autonomous navigation system, and is of great significance for realizing pedestrian autonomous navigation and positioning with higher positioning accuracy.

附图说明Description of drawings

图1是本发明实施例提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法流程图；Fig. 1 is the flow chart of the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction provided by the embodiment of the present invention;

图2是本发明实施例提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法原理示意图；Fig. 2 is a schematic diagram of the principle of the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction provided by the embodiment of the present invention;

图3是本发明实施例提供的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法结果示意图。Fig. 3 is a schematic diagram of the result of the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction provided by the embodiment of the present invention.

具体实施方式Detailed ways

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。In order to make the object, technical solution and advantages of the present invention more clear, the present invention will be further described in detail below in conjunction with the examples. It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention.

下面结合附图及具体实施例对本发明的应用原理作进一步描述。The application principle of the present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

如图1所示，本发明实施例的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法包括以下步骤：As shown in Figure 1, the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction of the embodiment of the present invention includes the following steps:

S101：手持PDA实时接收行人自主导航系统中脚步输出的量测信息；S101: The hand-held PDA receives the measurement information output by the footsteps of the pedestrian autonomous navigation system in real time;

S102：利用MEMS加速度计和MEMS磁力计输出值及式求得初始载体姿态信息；S102: Using the MEMS accelerometer and MEMS magnetometer output value and formula to obtain the initial carrier attitude information;

S103：更新的方向余弦矩阵；S103: updated direction cosine matrix;

S104：利用行人导航系统的姿态矩阵及公式估计出单兵系统导航状态；S104: Estimate the navigation state of the individual soldier system by using the attitude matrix and formula of the pedestrian navigation system;

S105：判断出行人自主导航系统的零速区间；S105: judging the zero-speed interval of the pedestrian autonomous navigation system;

S106：采用输出校正的方式对导航解算结果进行修正。S106: Correct the navigation calculation result by means of output correction.

本发明的具体步骤为：Concrete steps of the present invention are:

步骤一：将行人导航系统固定于单兵脚上，手持PDA实时接收并存储行人运动时系统输出的量测信息，任意时刻k接收到的系统输出信息为y_k，y_k＝[f_k ω_k m_k]^T；Step 1: Fix the pedestrian navigation system on the foot of the individual soldier, and the handheld PDA receives and stores the measurement information output by the system when the pedestrian is moving in real time. The system output information received at any time k is y _k , y _k = [f _k ω _k m _k ] ^T ;

其中， $ω_{k} = {[\begin{matrix} ω_{k}^{x} & ω_{k}^{y} & ω_{k}^{z} \end{matrix}]}^{T}$ 为k时刻MEMS三轴陀螺仪输出的角速率信息； $f_{k} = {[\begin{matrix} f_{k}^{x} & f_{k}^{y} & f_{k}^{z} \end{matrix}]}^{T}$ 为k时刻MEMS三轴加速度计输出的比力信息； $m_{k} = {[\begin{matrix} m_{k}^{x} & m_{k}^{y} & m_{k}^{z} \end{matrix}]}^{T}$ 为k时刻MEMS三轴磁力计的输出信息；T表示转置操作；in, $ω_{k} = {[\begin{matrix} ω_{k}^{x} & ω_{k}^{the y} & ω_{k}^{z} \end{matrix}]}^{T}$ is the angular rate information output by the MEMS three-axis gyroscope at time k; $f_{k} = {[\begin{matrix} f_{k}^{x} & f_{k}^{the y} & f_{k}^{z} \end{matrix}]}^{T}$ is the specific force information output by the MEMS triaxial accelerometer at time k; $m_{k} = {[\begin{matrix} m_{k}^{x} & m_{k}^{the y} & m_{k}^{z} \end{matrix}]}^{T}$ is the output information of the MEMS three-axis magnetometer at time k; T represents the transpose operation;

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

利用MEMS加速度计和MEMS磁力计输出值及式：Step 2: Use the MEMS accelerometer and MEMS magnetometer output value and formula to obtain the initial carrier attitude information

Using MEMS accelerometer and MEMS magnetometer output value and formula:

${g g}_{b b} = = {C C}_{b b}^{n no} {g g}_{n no}$

${m m}_{b b} = = {C C}_{b b}^{n no} {m m}_{n no}$

${r r}_{b b} = = {C C}_{b b}^{n no} {r r}_{n no}$

得初始载体姿态信息

Get the initial carrier attitude information

其中，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；Among them, g _n is the gravity vector

Projected in the geographic coordinate system, g _n = [0 0 g] ^T ; m _n is the geomagnetic vector

Projection under the carrier coordinate system, m _b =[m _bx m _by m _bz ] ^T ;

步骤三：使用步骤一中存储的行人自主导航系统输出数据，及微分方程：Step 3: Use the pedestrian autonomous navigation system output data stored in step 1, and the differential equation:

${\overset{. .}{C C}}_{b b}^{n no} ((t t)) = = {C C}_{b b}^{n no} ((t t)) Ω Ω ((t t))$

更新的方向余弦矩阵

updated direction cosine matrix

式中，

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

is the strapdown matrix carrying the system to the geographic system at time t, Ω(t) is the antisymmetric matrix of vector ω ^b (t),

ω为行人导航系统中MEMS陀螺仪测得的输出角速率：ω is the output angular rate measured by the MEMS gyroscope in the pedestrian navigation system:

$ω ω \approx \approx {ω ω}_{nb nb}^{b b}$

式中，

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

is the angular velocity of the carrier coordinate system of the pedestrian navigation system relative to the geographic coordinate system:

${ω ω}^{b b} ((t t)) = = {ω ω}_{nb nb}^{b b} = = {[\begin{matrix} {ω ω}^{bx bx} ((t t)) & {ω ω}^{by by} ((t t)) & {ω ω}^{bz bz} ((t t)) \end{matrix}]}^{T T};;$

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

Step 4: Use the attitude matrix of the pedestrian navigation system obtained in Step 3 and the formula to estimate the navigation state of the individual soldier system

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

θ＝arcsin(C₃₂)θ=arcsin(C ₃₂ )

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

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

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

估计出单兵系统导航状态

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

Estimated Soldier System Navigation Status

Including the three-dimensional position vector, velocity vector and attitude vector of the pedestrian navigation system, namely

其中，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)为载体的运动所产生的加速度：Among them, C _ij (i, j=1, 2, 3) is expressed as a strapdown matrix

The elements in , α is the roll angle of the IMU, θ is the pitch angle of the IMU, and ψ is the heading angle of the IMU; where v ⁿ (0) is the initial velocity, s ⁿ (0) is the initial position, v ⁿ (0) =[0 0 0] ^T , s ⁿ (0)=[0 0 0] ^T ; a ⁿ (t) is the acceleration produced by the movement of the carrier:

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

${f f}^{n no} ((t t)) = = {C C}_{b b}^{n no} ((t t)) {f f}^{b b} ((t t))$

式中，f^b(t)＝[f^bx(t) f^by(t) f^bz(t)]^T为MEMS加速度计测得的比力；In the formula, f ^b (t) = [f ^bx (t) f ^by (t) f ^bz (t)] ^T is the specific force measured by the MEMS accelerometer;

步骤五：利用步骤一中得到的行人导航系统中MEMS加速度计和MEMS陀螺仪的输出值及公式判断出行人自主导航系统的零速区间，利用步骤一中得到的行人导航系统中MEMS加速度计和MEMS陀螺仪的输出值及公式：Step 5: Use the output values and formulas of the MEMS accelerometer and MEMS gyroscope in the pedestrian navigation system obtained in step 1 to judge the zero-speed interval of the pedestrian autonomous navigation system, and use the MEMS accelerometer and MEMS gyroscope in the pedestrian navigation system obtained in step 1. The output value and formula of MEMS gyroscope:

判断出行人自主导航系统的零速区间，若上式成立则IMU静止；Judging the zero-speed interval of the pedestrian autonomous navigation system, if the above formula is true, the IMU is stationary;

其中，

||·||表示求范数，

T (z_{n}) = - \frac{2}{N} \ln L_{G} (z_{n}),

γ′＝-(2/N)ln(γ)，

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

||·|| means to find the norm,

T (z_{no}) = - \frac{2}{N} \ln L_{G} (z_{no}),

γ'=-(2/N)ln(γ),

ln( ) means to find the logarithm with e as the base, Represent the noise variance of the MEMS accelerometer and MEMS gyroscope respectively, and the value of γ is determined by the following formula:

${P P}_{FA FA} = = {&Integral; &Integral;}_{{{{z z}_{n no};; L L (({z z}_{n no})) > > γ γ | |} p p (({z z}_{n no};; {H h}_{00})) d d {z z}_{n no} = = α α;;$

步骤六：利用步骤六中判断出得零速状态，使用基于卡尔曼滤波的零速误差校正器，采用输出校正的方式对导航解算结果进行修正，零速校正卡尔曼滤波模型为：Step 6: Use the zero-speed state judged in step 6, use the zero-speed error corrector based on Kalman filter, and use the output correction method to correct the navigation calculation results. The zero-speed correction Kalman filter model is:

被估计状态向量为：The estimated state vector is:

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

量测阵为：The measurement array is:

状态转移矩阵为：The state transition matrix is:

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

为沿地理系的载体运动加速度的反对称阵；Among them, G _k-1 is the system noise drive array; W _k-1 is the system excitation noise sequence; H _k is the measurement array; N _k is the measurement noise sequence; φ ^T is the attitude error and φ ^T = [δ _ψ φ _y φ _z ], where δψ=ψ _magnetometer -ψ _{navigation solution} , δs ^T is the position error, δv ^T and δv ^T = [v _x v _y v _z ] is the velocity error, and the three error items are all three-dimensional;

is the antisymmetric matrix of the carrier motion acceleration along the geographic system;

存储各时刻的状态估计

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

Estimate the mean square error matrix P _k and filter gain K. If the MIMU zero-speed detection result of the footsteps at k+1 is in a motion state, the Kalman filter only performs time update.

若k+1时刻脚步MIMU零速检测结果为静止时，则KF做完整更新，时间更新+量测更新，卡尔曼滤波只做时间更新：If the footstep MIMU zero-speed detection result is static at time k+1, then KF will do a complete update, time update + measurement update, and Kalman filter will only do time update:

最优估计：Best estimate:

$\begin{matrix} {\overset{^^}{X x}}_{k k + + 11 / / k k} = = E E. [[{X x}_{k k + + 11}]] \\ = = E E. [[{F f}_{k k + + 11,, k k} {X x}_{k k} + + {G G}_{k k} {W W}_{k k}]] \\ = = {F f}_{k k + + 11,, k k} {\overset{^^}{X x}}_{k k} \end{matrix}$

均方误差：mean square error:

$\begin{matrix} {\overset{^^}{P P}}_{k k + + 11 / / k k} = = E E. [[δ δ {\overset{^^}{X x}}_{k k + + 11 / / k k} {((δ δ {\overset{^^}{X x}}_{k k + + 11 / / k k}))}^{T T}]] \\ = = {F f}_{k k} {P P}_{k k} {F f}_{k k}^{T T} + + G G {Q Q}_{k k} {G G}^{T T} \end{matrix}$

其中，E[·]表述求取期望；Among them, E[ ] expresses the expectation;

若k+1时刻脚步MIMU零速检测结果为静止时，则KF做完整更新，时间更新+量测更新：If the footstep MIMU zero-speed detection result is static at time k+1, then KF will do a complete update, time update + measurement update:

均方误差：mean square error:

$\begin{matrix} {P P}_{k k + + 11}^{- - 11} = = {(({\overset{^^}{P P}}_{k k + + 11} / / k k))}^{- - 11} + + {(({H h}_{k k + + 11}^{T T} {R R}_{k k + + 11}^{- - 11} {H h}_{k k + + 11}))}^{- - 11} \\ K K = = {P P}_{k k + + 11} {H h}_{k k + + 11}^{T T} {R R}_{k k + + 11}^{- - 11} \end{matrix}$

最优估计：Best estimate:

结合以下实验对本发明的优益效果做进一步的说明：The beneficial effect of the present invention is further described in conjunction with the following experiments:

利用自研三轴惯性测量组件(集成了微机械系统三轴磁力计、加速度计、陀螺仪)搭建真实双IMU系统单兵导航系统模型，设备参数如表1所示，通过合理的试验验证基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法的可靠性、实用性、准确性，试验场景选在室外空旷的哈尔滨工程大学军工操场；A self-developed three-axis inertial measurement component (integrated with a micromechanical system three-axis magnetometer, accelerometer, and gyroscope) is used to build a real dual-IMU system individual navigation system model. The reliability, practicability, and accuracy of the MEMS pedestrian autonomous navigation system positioning method calculated by strapdown inertial navigation and zero-speed correction, the test scene was selected in the open military playground of Harbin Engineering University;

表1自研微型惯性测量单元惯性测量组件各传感器性能指标Table 1 The performance indicators of each sensor of the self-developed miniature inertial measurement unit inertial measurement component

实验过程中相关参数设置如下：During the experiment, the relevant parameters were set as follows:

单兵导航自主定位系统采样频率：100HzIndividual navigation autonomous positioning system sampling frequency: 100Hz

微机械系统陀螺标准偏差：σ_a＝0.01m/s² MEMS gyro standard deviation: σ _a ＝0.01m/s ²

微机械加速度计标准偏差：σ_g＝0.1*pi/180rad/sMicromachined accelerometer standard deviation: σ _g = 0.1*pi/180rad/s

初始速度：vⁿ(0)＝[0 0 0]^T Initial velocity: v ⁿ (0) = [0 0 0] ^T

初始位置坐标：sⁿ(0)＝[0 0 0]^T Initial position coordinates: s ⁿ (0) = [0 0 0] ^T

实验开始前，测试者在实验场地进行15分钟的系统静止预热，完成系统的初始对准和GPS定位信息的初始化；实验中实时采集GPS定位信息作为真实轨迹的参考；Before the experiment started, the testers performed a 15-minute static warm-up of the system on the experimental site to complete the initial alignment of the system and the initialization of GPS positioning information; during the experiment, real-time GPS positioning information was collected as a reference for the real trajectory;

实验内容：测试者围绕长方形足球场进行场地行走一周(约500米)，行走时间约为300秒，实验过程中实时采集行人自主导航系统中各传感器的输出值，并对采集得到的实验数据进行离线分析；实验结果如图3所示；Experimental content: The tester walked around the rectangular football field for one week (about 500 meters), and the walking time was about 300 seconds. During the experiment, the output values of the sensors in the pedestrian autonomous navigation system were collected in real time, and the collected experimental data were analyzed Offline analysis; the experimental results are shown in Figure 3;

基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法的定位结果如图2所示，为了更形象的说明本大明提出的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法的优越性，表2给出了使用该定位方法时定位结果的均方根误差RMS，其中计算真值为GPS定位信息，在行走时间大于300秒的情况下定位误差仍然保持在4.2m内，小于单兵行走距离的1％，实验证明本发明提出的基于捷联惯导解算和零速校正的MEMS行人自主导航系统定位方法定位结果比较理想，可以满足短时间内单兵作战人员的使用要求。The positioning results of the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation and zero-speed correction are shown in Figure 2. In order to more vividly illustrate the MEMS based on strapdown inertial navigation and zero-speed correction proposed by Ben Daming The superiority of the positioning method of the pedestrian autonomous navigation system. Table 2 shows the root mean square error RMS of the positioning results when using this positioning method. The calculated true value is the GPS positioning information. The positioning error remains the same when the walking time is greater than 300 seconds It is kept within 4.2m, which is less than 1% of the walking distance of a single soldier. The experiment proves that the MEMS pedestrian autonomous navigation system positioning method based on the strapdown inertial navigation solution and zero-speed correction proposed by the present invention has ideal positioning results and can meet the short-term Requirements for use by individual combatants.

表2Table 2

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention should be included in the protection of the present invention. within range.

Claims

1. An autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction is characterized by comprising the following steps of:

the method comprises the following steps: the pedestrian autonomous navigation system is fixed on the foot of an individual soldier, the measurement information output by the system during the motion of the pedestrian is received and stored in real time by the handheld PDA, and the system output information received at any time k is y_k；

Step two: calculating initial carrier by using output values of MEMS accelerometer and MEMS magnetometer and formulaAttitude information

Step three: using the output data of the pedestrian autonomous navigation system stored in the step one and the direction cosine matrix updated by the differential equation

Step four: attitude matrix of pedestrian autonomous navigation system obtained in step three

And estimating the navigation state of the individual soldier system by a formula

Including three-dimensional position, velocity, attitude vectors of pedestrian navigation systems, i.e.

Step five: judging a zero-speed interval of the autonomous pedestrian navigation system by using the output values of the MEMS accelerometer and the MEMS gyroscope in the pedestrian navigation system obtained in the step one and a formula;

step six: judging a zero-speed state in the step five, and correcting a navigation resolving result by using a zero-speed error corrector based on Kalman filtering in an output correction mode;

the state quantity at any time k can be estimated through the above loop.

2. The method for MEMS pedestrian autonomous navigation system positioning based on strapdown inertial navigation solution and zero velocity correction of claim 1, wherein in step one, the pedestrian navigation system comprises: MEMS gyroscope, MEMS accelerometer, MEMS magnetometer.

3. The MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction as claimed in claim 1, wherein in step one, the system output information received at any time k is y_k＝[f_k ω_k m_k]^T；

Wherein,

angular rate information output by the MEMS triaxial gyroscope at the moment k;

f_{k} = {[\begin{matrix} f_{k}^{x} & f_{k}^{y} & f_{k}^{z} \end{matrix}]}^{T}

specific force information output by the MEMS triaxial accelerometer at the moment k;

m_{k} = {[\begin{matrix} m_{k}^{x} & m_{k}^{y} & m_{k}^{z} \end{matrix}]}^{T}

outputting information of the MEMS three-axis magnetometer at the moment k; t denotes a transpose operation.

4. The MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction as claimed in claim 1, wherein in step two, the output values and the formula of the MEMS accelerometer and the MEMS magnetometer are utilized:

g_{b} = C_{b}^{n} g_{n}

m_{b} = C_{b}^{n} m_{n}

r_{b} = C_{b}^{n} r_{n}

obtaining initial carrier attitude information

Wherein, g_nIs a gravity vector

Projection in a geographic coordinate system, g_n＝[0 0 g]^T；m_nAs a vector of geomagnetism

Projection in a geographic coordinate system, m_n=[m_ex m_ey m_ez]^T；g_bIs a gravity vector

Projection in a carrier coordinate system, g_b＝[g_bx g_by g_bz]^T；m_bAs a vector of geomagnetism

The projection in the carrier coordinate system is,

m_{b} = {[\begin{matrix} m_{bx} & m_{by} & m_{bz} \end{matrix}]}^{T} .

5. the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero velocity correction of claim 1, wherein differential equations are used in step three:

updated direction cosine matrix

In the formula,a strapdown matrix from a carrier system to a geographic system at the time t, wherein omega (t) is a vector omega^b(t) an anti-symmetric array of,

omega is the angular rate measured by the MEMS gyroscope in the pedestrian navigation system,

in the formula,angular rate of rotation of a vehicle coordinate system of a pedestrian navigation system relative to a geographic coordinate system:

6. the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction as claimed in claim 1, wherein in step four, the formula is utilized:

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

θ＝arcsin(C₃₂)

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

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

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

estimating individual soldier system navigation state

Wherein, C_i(i, j ═ 1, 2, 3) as a strapdown matrix

The alpha is a roll angle of the inertia measurement unit, the theta is a pitch angle of the inertia measurement unit, and the psi is a course angle of the inertia measurement unit; wherein v isⁿ(O) is the initial velocity, sⁿ(O) is an initial position, aⁿ(t) acceleration due to movement of the carrier:

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

wherein f isⁿ(t) is the projection of the specific force measured by the MEMS accelerometer along the geodetic direction:

f^{n} (t) = C_{b}^{n} f^{b} (t)

in the formula,

f^{b} (t) = {[\begin{matrix} f^{bx} (t) & f^{by} (t) & f^{bz} (t)] \end{matrix}]}^{T}

is the specific force measured by the MEMS accelerometer.

7. The MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction as claimed in claim 1, wherein in step five, the formula is utilized:

<math> <mrow> <mfrac> <mn>1</mn> <mi>N</mi> </mfrac> <munder> <mi>Σ</mi> <mrow> <mi>k</mi> <mo>&Element;</mo> <msub> <mi>Ω</mi> <mi>n</mi> </msub> </mrow> </munder> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <msubsup> <mi>σ</mi> <mi>a</mi> <mn>2</mn> </msubsup> </mfrac> <msup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>f</mi> <mi>k</mi> </msub> <mo>-</mo> <mi>g</mi> <mfrac> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>n</mi> </msub> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>n</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> </mfrac> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <mfrac> <mn>1</mn> <msubsup> <mi>σ</mi> <mi>ω</mi> <mn>2</mn> </msubsup> </mfrac> <msup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>ω</mi> <mi>k</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mo><</mo> <msup> <mi>γ</mi> <mo>′</mo> </msup> </mrow> </math>

judging a zero-speed interval of the autonomous navigation system of the traveling person, and if the above formula is established, enabling the steps of the user of the autonomous navigation system of the traveling person to be static;

wherein,

| l | · | | represents the norm calculation;

T (z_{n}) = - \frac{2}{N} \ln L_{G} (z_{n}),

γ′＝-(2/N)ln(γ)；

ln (·) represents the logarithm based on e;

representing the noise variance of the MEMS accelerometer and the MEMS gyroscope respectively; the value of gamma is determined by the following formula:

<math> <mrow> <msub> <mi>P</mi> <mi>FA</mi> </msub> <mo>=</mo> <msub> <mo>&Integral;</mo> <mrow> <mo>{</mo> <msub> <mi>z</mi> <mi>n</mi> </msub> <mo>:</mo> <mi>L</mi> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mo>></mo> <mi>γ</mi> <mo>}</mo> </mrow> </msub> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>n</mi> </msub> <mo>;</mo> <msub> <mi>H</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>dz</mi> <mi>n</mi> </msub> <mo>=</mo> <mi>α</mi> <mo>.</mo> </mrow> </math>

8. the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction as claimed in claim 1, wherein the Kalman filtering model used in the sixth step is:

\{\begin{matrix} X_{k} = F_{k, k - 1} X_{k - 1} + G_{k - 1} W_{k - 1} \\ Z_{k} = H_{k} X_{k} + N_{k} \end{matrix}

the estimated state vector is:

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

when the measurement vector is in a zero-speed state, the navigation resolving speed value and the course error information are as follows:

the measurement array is:

H = [\begin{matrix} 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{matrix}]

the state transition matrix is:

wherein G is_k-1For system noise-driven arrays, W_k-1For systematic excitation of noise sequences, H_kFor measurement array, N_kFor measuring noise sequences, [ phi ]^TIs an attitude error and phi^T＝[δ_ψφ_y φ_z]Wherein, delta psi ═ psi_Magnetometer-ψ_{Navigation solution}，δs^TFor position error, δ v^TIs a velocity error and δ v^T＝[v_x v_y v_z]The three error terms are all three-dimensional,

is an anti-symmetric matrix of carrier motion acceleration along the geographic system.

9. The MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction as claimed in claim 1, wherein the optimal estimation and mean square error of Kalman filtering only time update in step six are:

\begin{matrix} {\hat{X}}_{k + 1 / k} = E [X_{k + 1}] \\ = E [F_{k + 1, k} X_{k} + G_{k} W_{k}] \\ = F_{k + 1, k} {\hat{X}}_{k} \end{matrix}

wherein E [. cndot. ] represents the expectation;

the optimal estimation and mean square error in time update + measurement update is:

{\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} = {({\hat{P}}_{k + 1 / k})}^{- 1} + {(H_{k + 1}^{T} R_{k + 1}^{- 1} H_{k + 1})}^{- 1}

K = P_{k + 1} H_{k + 1}^{T} R_{k + 1}^{- 1}

storing state estimates for each time instant

Estimating mean square error matrix P_kAnd a filter gain K, if the step MIMU zero-speed detection result at the moment K +1 is in a motion state, the Kalman filter only updates the time,

if the step MIMU zero-speed detection result at the moment k +1 is static, the KF is completely updated, and the time and measurement are updated.