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

Abstract

The invention discloses an autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction. According to the autonomous navigation system positioning method, output data of an MEMS (Micro-electromechanical Systems) accelerometer and an MEMS magnetometer in a pedestrian autonomous navigation system is used for carrying out initial alignment on the system; a strapdown inertial navigation resolving algorithm is used for estimating the state of the pedestrian autonomous navigation system; when a static detecting algorithm detects that pedestrian footsteps are stopped, a zero-speed correction error compensator based on Kalman filtering is designed and a navigation resolving result of the pedestrian autonomous navigation system is corrected by adopting a manner of outputting correction; the defects that an MEMS inertia measurement device is low in precision and has a great positioning error when being used for long time are overcome; a high-precision positioning function of the pedestrian autonomous navigation system is realized under the condition of not increasing external cost. The autonomous navigation system positioning method is simple and high in stability and reliability; the use precision of a single-pawn autonomous navigation system is improved effectively; the autonomous navigation system positioning method has important meanings of realizing autonomous pedestrian navigation and positioning with the high positioning precision.

Description

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

Technical Field

The invention belongs to the technical field of navigation positioning, and particularly relates to an autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction.

Background

The pedestrian autonomous navigation system (comprising an MEMS three-axis magnetometer, an MEMS three-axis accelerometer and an MEMS three-axis gyroscope) is mainly used for autonomous navigation and real-time positioning of individuals under known or unknown conditions, and assists in completing various types of emergency rescue tasks. When emergencies such as fire disasters and earthquakes occur, conditions which are unfavorable for rescue, such as visibility reduction, inherent environment change and the like, may exist on the accident site, and rescue personnel cannot rapidly and accurately distinguish the self position. At the moment, the positioning information provided by the pedestrian navigation system can provide effective technical support for rescue workers.

Most of the existing products with individual navigation and positioning functions mainly depend on a GPS (Global positioning System) for positioning, but when a GPS signal is lost, the system cannot work, and then the autonomous, real-time and stable positioning requirements of the pedestrian autonomous navigation system cannot be met. Therefore, the research on the individual soldier autonomous positioning technology under the condition without the GPS has certain application value. The pedestrian autonomous navigation system based on the MEMS inertial measurement technology does not depend on any external information when working, and has good anti-interference performance, so that the research on the pedestrian autonomous navigation system based on the MEMS inertial measurement technology has good application value. However, the measurement accuracy of the existing MIMU is generally low, and the drift of the sensor error along with the time is serious, so that the navigation error accumulation of the MIMU is increased rapidly.

Disclosure of Invention

The embodiment of the invention aims to provide an MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction, and aims to solve the problems that an existing MEMS inertial measurement device is low in precision, and a pedestrian autonomous navigation system based on an MEMS inertial measurement technology is large in navigation positioning error during long-time use.

The embodiment of the invention is realized in such a way that an MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction comprises the following steps:

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: obtaining initial carrier attitude information by using output values of MEMS accelerometer and MEMS magnetometer and formula

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.

Further, in step one, the pedestrian navigation system includes: MEMS gyroscope, MEMS accelerometer, MEMS magnetometer.

Further, in step one, the system output information received at an arbitrary time k is y_k＝[f_k ω_k m_k]^T；

Wherein, <math> <mrow> <msub> <mi>&omega;</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>x</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>y</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math> angular rate information output by the MEMS triaxial gyroscope at the moment k; $f_k={\left[\begin{array}{ccc}{f}_k^x& f_k^y& f_k^z\end{array}\right]}^T$ specific force information output by the MEMS triaxial accelerometer at the moment k; $m_k={\left[\begin{array}{ccc}{m}_k^x& m_k^y& m_k^z\end{array}\right]}^T$ outputting information of the MEMS three-axis magnetometer at the moment k; t denotes a transpose operation.

Further, 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_b＝[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

Projection in a carrier coordinate system, m_b＝[m_bx m_by m_bz]^T。

Further, a differential equation is used in step three:

<math> <mrow> <msubsup> <mover> <mi>C</mi> <mo>&CenterDot;</mo> </mover> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>&Omega;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </math>

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,

<math> <mrow> <mi>&Omega;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>z</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>y</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>z</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>x</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>y</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>x</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

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

<math> <mrow> <mi>&omega;</mi> <mo>&ap;</mo> <msubsup> <mi>&omega;</mi> <mi>nb</mi> <mi>b</mi> </msubsup> </mrow> </math>

in the formula,

angular rate of rotation of a vehicle coordinate system of a pedestrian navigation system relative to a geographic coordinate system: <math> <mrow> <msup> <mi>&omega;</mi> <mi>b</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>&omega;</mi> <mi>nb</mi> <mi>b</mi> </msubsup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msup> <mi>&omega;</mi> <mi>bx</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <mi>by</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <mi>bz</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>;</mo> </mrow> </math>

further, in step four, the formula is utilized:

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

θ＝arcsin(Ｃ_３2)

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

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

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

estimating individual soldier system navigation stateIncluding three-dimensional position, velocity, attitude vectors of pedestrian navigation systems, i.e.

Wherein, C_ij(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ⁿ(0) Is the initial velocity, sⁿ(0) 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\left(t\right)=C_b^n\left(t\right)f^b\left(t\right)$

in the formula (f)^b(t)＝[f^bx(t) f^by(t) f^bz(t)]^TIs the specific force measured by the MEMS accelerometer.

Further, in step five, the formula is utilized:

<math> <mrow> <mfrac> <mn>1</mn> <mi>N</mi> </mfrac> <munder> <mi>&Sigma;</mi> <mrow> <mi>k</mi> <mo>&Element;</mo> <msub> <mi>&Omega;</mi> <mi>n</mi> </msub> </mrow> </munder> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <msubsup> <mi>&sigma;</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>&sigma;</mi> <mi>&omega;</mi> <mn>2</mn> </msubsup> </mfrac> <msup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>&omega;</mi> <mi>k</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mo>&lt;</mo> <msup> <mi>&gamma;</mi> <mo>&prime;</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,

i | · | | represents the norm, $T\left(z_n\right)=-\frac{2}{N}\ln L_G\left(z_n\right),$ γ′＝-(2/N)ln(γ)，

ln (-) represents the logarithm based on e,

respectively representing the noise variance of the MEMS accelerometer and the MEMS gyroscope, wherein 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>&gamma;</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>&alpha;</mi> <mo>.</mo> </mrow> </math>

further, the kalman filtering model used in step six is:

$\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.$

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:

<math> <mrow> <mi>Z</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>&delta;&psi;</mi> </mtd> </mtr> <mtr> <mtd> <mi>&delta;v</mi> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>&delta;&psi;</mi> </mtd> <mtd> <msub> <mi>v</mi> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mi>v</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>v</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math>

the measurement array is:

$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]$

the state transition matrix is:

<math> <mrow> <mi>F</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mi>&Delta;tS</mi> <mrow> <mo>(</mo> <msubsup> <mi>f</mi> <mi>t</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mi>&Delta;t</mi> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

wherein G is_k-1For system noiseAcoustically driven array, W_k-1For systematic excitation of noise sequences, H_kIs a measuring array; n is a radical of_kFor measuring noise sequences, [ phi ]^TIs an attitude error and phi^T＝[δ_ψ φ_y φ_z]Where δ y is ψ_Magnetometer-ψ_{Navigation solution}，δs^TFor position error, δ v^TIs a velocity error and δ v^T＝[v_x V_x v_z]The three error terms are all three-dimensional;

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

Further, the optimal estimation and mean square error of the kalman filter only performing time update in step six are:

$\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}$

<math> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>P</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <mi>&delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>&delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <mo>=</mo> <msub> <mi>F</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mi>k</mi> </msub> <msubsup> <mi>F</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>GQ</mi> <mi>k</mi> </msub> <msup> <mi>G</mi> <mi>T</mi> </msup> </mtd> </mtr> </mtable> </mfenced> </math>

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

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

according to the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction, output data of an MEMS accelerometer and an MEMS magnetometer in the pedestrian autonomous navigation system are adopted to carry out initial alignment on the system, the state of the pedestrian autonomous navigation system is initially estimated by using a strapdown inertial navigation solution algorithm, the step motion state of a pedestrian is detected by using a static detection algorithm, and when the step of the pedestrian is detected to be static, a zero-speed correction error compensator based on Kalman filtering is designed to correct the navigation solution result of the pedestrian autonomous navigation system by using an output correction mode, so that the defects that an MEMS inertial measurement device is low in precision, the positioning error is large when the device is used for a long time and the like are overcome, and the high-precision positioning function of the pedestrian autonomous navigation system is realized under the condition that the external cost is not increased. The method is simple, high in stability and reliability, improves a convenient navigation positioning method, effectively improves the use precision of an individual soldier autonomous navigation system, and has important significance for realizing pedestrian autonomous navigation positioning with higher positioning precision.

Drawings

FIG. 1 is a flow chart of a method for positioning an MEMS pedestrian autonomous navigation system based on strapdown inertial navigation solution and zero-speed correction according to an embodiment of the present invention;

FIG. 2 is a schematic diagram of a principle of a positioning method of an MEMS pedestrian autonomous navigation system based on strapdown inertial navigation solution and zero-speed correction according to an embodiment of the present invention;

FIG. 3 is a schematic diagram of a result of a method for positioning an MEMS pedestrian autonomous navigation system based on strapdown inertial navigation solution and zero-speed correction according to an embodiment of the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

The application of the principles of the present invention will be further described with reference to the accompanying drawings and specific embodiments.

As shown in fig. 1, the method for positioning an MEMS pedestrian autonomous navigation system based on strapdown inertial navigation solution and zero-speed correction according to the embodiment of the present invention includes the following steps:

s101: the handheld PDA receives measurement information output by the steps in the pedestrian autonomous navigation system in real time;

s102: obtaining initial carrier attitude information by using output values and formulas of the MEMS accelerometer and the MEMS magnetometer;

s103: an updated direction cosine matrix;

s104: estimating the navigation state of the individual soldier system by utilizing the attitude matrix and the formula of the pedestrian navigation system;

s105: judging a zero-speed interval of the autonomous navigation system of the trip person;

s106: and correcting the navigation calculation result in an output correction mode.

The method comprises the following specific steps:

the method comprises the following steps: fixing the pedestrian navigation system on the foot of the individual soldier, receiving and storing the measurement information output by the system in real time when the pedestrian moves by using the handheld PDA, wherein the system output information received at any time k is y_k，y_k＝[f_k ω_k m_k]^T；

Wherein, <math> <mrow> <msub> <mi>&omega;</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>x</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>y</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math> angular rate information output by the MEMS triaxial gyroscope at the moment k; $f_k={\left[\begin{array}{ccc}{f}_k^x& f_k^y& f_k^z\end{array}\right]}^T$ specific force information output by the MEMS triaxial accelerometer at the moment k; $m_k={\left[\begin{array}{ccc}{m}_k^x& m_k^y& m_k^z\end{array}\right]}^T$ outputting information of the MEMS three-axis magnetometer at the moment k; t represents a transpose operation;

step two: obtaining initial carrier attitude information by using output values and formulas of MEMS accelerometer and MEMS magnetometer

Outputting values and formulas by using the MEMS accelerometer and the MEMS magnetometer:

$g_b=C_b^n{g}_n$

$m_b=C_b^n{m}_n$

$r_b=C_b^n{r}_n$

get the initial loadBody posture 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

Projection in a carrier coordinate system, m_b＝[m_bx m_by m_bz]^T；

Step three: using the pedestrian autonomous navigation system output data stored in the step one and a differential equation:

<math> <mrow> <msubsup> <mover> <mi>C</mi> <mo>.</mo> </mover> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>&Omega;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </math>

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,

<math> <mrow> <mi>&Omega;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>z</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>y</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>z</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>x</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>y</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>x</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

ω is the output angular rate measured by the MEMS gyroscope in the pedestrian navigation system:

<math> <mrow> <mi>&omega;</mi> <mo>&ap;</mo> <msubsup> <mi>&omega;</mi> <mi>nb</mi> <mi>b</mi> </msubsup> </mrow> </math>

in the formula,

angular rate of a vehicle coordinate system of a pedestrian navigation system relative to a geographic coordinate system:

<math> <mrow> <msup> <mi>&omega;</mi> <mi>b</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>&omega;</mi> <mi>nb</mi> <mi>b</mi> </msubsup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msup> <mi>&omega;</mi> <mi>bx</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <mi>by</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <mi>bz</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>;</mo> </mrow> </math>

step four: attitude matrix of pedestrian navigation system obtained in step threeAnd 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.

α＝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

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

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

The element in (1) is that alpha is a roll angle of the IMU, theta is a pitch angle of the IMU, and psi is a course angle of the IMU; wherein v isⁿ(0) Is the initial velocity, sⁿ(0) Is an initial position, vⁿ(0)＝[0 0 0]^T，sⁿ(0)＝[0 0 0]^T；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\left(t\right)=C_b^n\left(t\right)f^b\left(t\right)$

in the formula (f)^b(t)＝[f^bx(t) f^by(t) f^bz(t)]^TSpecific force measured for the MEMS accelerometer;

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

<math> <mrow> <mfrac> <mn>1</mn> <mi>N</mi> </mfrac> <munder> <mi>&Sigma;</mi> <mrow> <mi>k</mi> <mo>&Element;</mo> <msub> <mi>&Omega;</mi> <mi>n</mi> </msub> </mrow> </munder> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <msubsup> <mi>&sigma;</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>&sigma;</mi> <mi>&omega;</mi> <mn>2</mn> </msubsup> </mfrac> <msup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>&omega;</mi> <mi>k</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mo>&lt;</mo> <msup> <mi>&gamma;</mi> <mo>&prime;</mo> </msup> </mrow> </math>

judging a zero-speed interval of the autonomous navigation system of the trip person, and if the above formula is established, making the IMU static;

wherein,

i | · | | represents the norm, $T\left(z_n\right)=-\frac{2}{N}\ln L_G\left(z_n\right),$ γ′＝-(2/N)ln(γ)，

ln (-) represents the logarithm based on e,respectively representing the noise variance of the MEMS accelerometer and the MEMS gyroscope, wherein 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>&gamma;</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> <mi>d</mi> <msub> <mi>z</mi> <mi>n</mi> </msub> <mo>=</mo> <mi>&alpha;</mi> <mo>;</mo> </mrow> </math>

step six: and sixthly, judging a zero-speed state by utilizing the step six, using a Kalman filtering-based zero-speed error corrector, and correcting a navigation resolving result by adopting an output correction mode, wherein a Kalman filtering model for zero-speed correction is as follows:

$\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.$

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:

<math> <mrow> <mi>Z</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>&delta;&psi;</mi> </mtd> </mtr> <mtr> <mtd> <mi>&delta;v</mi> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>&delta;&psi;</mi> </mtd> <mtd> <msub> <mi>v</mi> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mi>v</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>v</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math>

the measurement array is:

$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]$

the state transition matrix is:

<math> <mrow> <mi>F</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mi>&Delta;tS</mi> <mrow> <mo>(</mo> <msup> <msub> <mi>f</mi> <mi>t</mi> </msub> <mi>n</mi> </msup> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mi>&Delta;t</mi> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

wherein G is_k-1Driving the array for system noise; w_ｋ－１Exciting a noise sequence for the system; h_kIs a measuring array; n is a radical of_kTo measure the noise sequence; phi is a^TIs an attitude error and phi^T＝[δ_ψ φ_y φ_z]Wherein delta psi ═ psi_Magnetometer-ψ_{Navigation solution}，δs^TFor position error, δ v^TAnd δ v^T＝[v_x v_y v_z]For the speed error, the three error terms are all three-dimensional;

is an anti-symmetric array of carrier motion accelerations along the geographic system;

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 performs complete updating, time updating + measurement updating, and the Kalman filtering only performs time updating:

and (3) optimal estimation:

$\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}$

mean square error:

<math> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>P</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <mi>&delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>&delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <mo>=</mo> <msub> <mi>F</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mi>k</mi> </msub> <msubsup> <mi>F</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <mi>G</mi> <msub> <mi>Q</mi> <mi>k</mi> </msub> <msup> <mi>G</mi> <mi>T</mi> </msup> </mtd> </mtr> </mtable> </mfenced> </math>

wherein E [. cndot. ] represents the expectation;

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

mean square error:

$\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}$

and (3) optimal estimation:

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

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

The beneficial effects of the present invention are further illustrated in conjunction with the following experiments:

the method comprises the steps that a true dual-IMU system individual navigation system model is built by utilizing a self-developed triaxial inertia measurement assembly (integrating a micro-mechanical system triaxial magnetometer, an accelerometer and a gyroscope), equipment parameters are shown in a table 1, reliability, practicability and accuracy of an MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction are verified through reasonable tests, and a test scene is selected in an outdoor open Harbin engineering university military operation field;

TABLE 1 Performance indexes of sensors of inertia measurement assembly of self-grinding micro inertia measurement unit

The relevant parameters in the experimental process are set as follows:

sampling frequency of the individual navigation autonomous positioning system: 100Hz

Gyro standard deviation of micro-mechanical system: sigma_a＝0.01m/s²

Standard deviation of micro-mechanical accelerometer: sigma_g＝0.1*pi/180rad/s

Initial speed: v. ofⁿ(0)＝[0 0 0]^T

Initial position coordinates: sⁿ(0)＝[0 0 0]^T

Before the experiment begins, a tester carries out static preheating of the system for 15 minutes in an experiment field to finish the initial alignment of the system and the initialization of GPS positioning information; GPS positioning information is collected in real time in an experiment and used as a reference of a real track;

the experimental contents are as follows: a tester walks around a rectangular football field for a circle (about 500 meters), the walking time is about 300 seconds, the output values of all sensors in the pedestrian autonomous navigation system are collected in real time in the experimental process, and the collected experimental data are analyzed in an off-line manner; the experimental results are shown in fig. 3;

the positioning result of the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction is shown in FIG. 2, in order to more vividly illustrate the superiority of the MEMS pedestrian autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction provided by the present general invention, the table 2 gives the root mean square error RMS of the positioning result when the positioning method is used, wherein the calculation truth value is GPS positioning information, the positioning error is still kept within 4.2m and less than 1% of the walking distance of a single soldier under the condition that the walking time is more than 300 seconds, and experiments prove that the positioning result of the MEMS autonomous navigation system positioning method based on strapdown inertial navigation solution and zero-speed correction provided by the invention is more ideal, and can meet the use requirements of the single soldier fighters in a short time.

TABLE 2

The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents and improvements made within the spirit and principle of the present invention are intended to be included within the scope of the present invention.

Claims (9)

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, <math> <mrow> <msub> <mi>&omega;</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>x</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>y</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>&omega;</mi> <mi>k</mi> <mi>z</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math> angular rate information output by the MEMS triaxial gyroscope at the moment k; $f_k={\left[\begin{array}{ccc}{f}_k^x& f_k^y& f_k^z\end{array}\right]}^T$ specific force information output by the MEMS triaxial accelerometer at the moment k; $m_k={\left[\begin{array}{ccc}{m}_k^x& m_k^y& m_k^z\end{array}\right]}^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={\left[\begin{array}{ccc}{m}_{\mathrm{bx}}& m_{\mathrm{by}}& m_{\mathrm{bz}}\end{array}\right]}^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:

<math> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>&Omega;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </math>

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,

<math> <mrow> <mi>&Omega;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>z</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>y</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>z</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>x</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>y</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <msub> <mi>b</mi> <mi>x</mi> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

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

<math> <mrow> <mi>&omega;</mi> <mo>&ap;</mo> <msubsup> <mi>&omega;</mi> <mi>nb</mi> <mi>b</mi> </msubsup> </mrow> </math>

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

<math> <mrow> <msup> <mi>&omega;</mi> <mi>b</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>&omega;</mi> <mi>nb</mi> <mi>b</mi> </msubsup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msup> <mi>&omega;</mi> <mi>bx</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <mi>by</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> <mtd> <msup> <mi>&omega;</mi> <mi>bz</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>.</mo> </mrow> </math>

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

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

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\left(t\right)=C_b^n{f}^b\left(t\right)$

in the formula, $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$ 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>&Sigma;</mi> <mrow> <mi>k</mi> <mo>&Element;</mo> <msub> <mi>&Omega;</mi> <mi>n</mi> </msub> </mrow> </munder> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <msubsup> <mi>&sigma;</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>&sigma;</mi> <mi>&omega;</mi> <mn>2</mn> </msubsup> </mfrac> <msup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>&omega;</mi> <mi>k</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mo>&lt;</mo> <msup> <mi>&gamma;</mi> <mo>&prime;</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\left(z_n\right)=-\frac{2}{N}\ln L_G\left(z_n\right),$ γ′＝-(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>&gamma;</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>&alpha;</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:

$\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.$

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:

<math> <mrow> <mi>Z</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>&delta;&psi;</mi> </mtd> </mtr> <mtr> <mtd> <mi>&delta;v</mi> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>&delta;&psi;</mi> </mtd> <mtd> <msub> <mi>v</mi> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mi>v</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>v</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math>

the measurement array is:

$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]$

the state transition matrix is:

<math> <mrow> <mi>F</mi> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mi>&Delta;tS</mi> <mrow> <mo>(</mo> <msubsup> <mi>f</mi> <mi>t</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mi>&Delta;t</mi> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>I</mi> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

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{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}$

<math> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>P</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mi>E</mi> <mo>[</mo> <mi>&delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>&delta;</mi> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <mo>=</mo> <msub> <mi>F</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mi>k</mi> </msub> <msubsup> <mi>F</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <mi>G</mi> <msub> <mi>Q</mi> <mi>k</mi> </msub> <msup> <mi>G</mi> <mi>k</mi> </msup> </mtd> </mtr> </mtable> </mfenced> </math>

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}={\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}$

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.

Images