CN105890593A - MEMS inertial navigation system and track reconstruction method based on same - Google Patents

Abstract

The invention discloses an MEMS inertial navigation system and a track reconstruction method based on the same. A six-position method is applied to error factor identification on a gyroscope and an accelerometer, a static error model is obtained, and therefore static errors are compensated; the random error of the gyroscope and the accelerometer is remarkably reduced through filtering of a low pass filter, noise is lowered, and output precision is improved. A complementary filter algorithm is adopted for effectively combining the updated attitude angles of the gyroscope and the accelerometer to obtain an attitude matrix. The attitude matrix is used for acceleration conversion and gravity compensation, and then the triaxial acceleration of a navigation module in an inertial coordinate system is obtained. Finally, track and speed information is obtained through iterative dual integration. The independently solved attitude angles of the accelerometer and the gyroscope are combined through the complementary filter algorithm, divergency of drifting of the attitude angles can be restrained, and attitude calculation is greatly improved in a dynamic state and a static state.

Description

MEMS inertial navigation system and track reconstruction method based on same

Technical Field

The invention belongs to the technical field of inertial navigation, and particularly relates to a track reconstruction method based on an MEMS inertial navigation system.

Technical Field

Micro Electro Mechanical Systems (MEMS) are an extension and development of Micro electronic technology, and have been rapidly developed at the end of the twentieth century and in the decades of this century. Due to the rapid development of the electronics industry and computer technology and the continuous reduction of the manufacturing cost of electronic devices, some high-end technologies in military science and technology are beginning to be applied to the consumer electronics field, wherein the application of MEMS in human-computer interaction is attracting more and more attention. The MEMS inertial sensor is a product of MEMS application in the inertial navigation field and is composed of a MEMS accelerometer and a MEMS gyroscope. The technology not only has great application in the military fields of aerospace, national defense science and technology and the like, but also has great development slowly in some civil fields, such as somatosensory games, human motion perception, air mouse systems and the current more popular VR equipmentThe above. In the track reconstruction technology of the MEMS-based inertial navigation system, the requirement on the precision of the inertial sensor is very high, because the precision of the reconstructed track of the inertial navigation system is directly influenced. In view of the current technical development trend, the MEMS inertial sensor is developed along the trend of high precision and high integration, and the precision of the existing MEMS acceleration sensor can reach 1 & 10^-4g and there is still further room for lift. The limit precision of the existing MEMS gyroscope can reach below 0.01/h, and the random walk coefficient can be controlled below 1/h. Due to cost limitations in the consumer electronics field, the precision of the selected MEMS gyroscope is generally in the range of 10-100/h. The reduction in accuracy therefore requires special algorithms to ensure the accuracy of the reconstructed trajectory.

The application of MEMS inertial sensors to track reconstruction in the field of consumer electronics has become a hotspot in current research and development. The research on the MEMS sensor before 05 years is mainly focused on the aspects of manufacturing research of the MEMS sensor, calibration error processing method research of the MEMS sensor and the like. In recent years, the application of the MEMS sensor gradually enters the lives of ordinary people in response to the continuous improvement of the living quality of people and the rise of smart phones. Such as inertial sensors in the equipment currently offered by IBM for VR gaming experience, body motion recognition techniques offered by XSENS corporation, uk, x-io Technologies Limited, and WSENS corporation in the background, which undoubtedly perfect MEMS technology applications and enrich people's lives.

Disclosure of Invention

The invention aims to provide a track reconstruction method based on an MEMS inertial navigation system, which reduces navigation errors by correcting a model of a sensor and adopting a filtering algorithm, and provides a motion attitude and a motion track of the system in a range so as to meet the navigation or tracking requirements of high-precision motion tracks in a small range.

In order to achieve the purpose, the technical scheme adopted by the invention is as follows: an MEMS inertial navigation system comprises an inertial sensor, a first level conversion module, a power supply module, a second level conversion module and an upper computer; the inertial sensor consists of an accelerometer and a gyroscope; a wireless transmission module is carried on the first level conversion module; a wireless receiving module is carried on the second level conversion module; the accelerometer is used for measuring the three-axis acceleration of the sensor, and the gyroscope is used for measuring the three-axis angular velocity of the sensor; the accelerometer and the gyroscope are both connected with the first level conversion module; the wireless transmission module and the wireless receiving module transmit data wirelessly; the second level conversion module is connected with the upper computer through a USB; and the upper computer performs static error compensation and filtering on the received signals, calculates and updates the attitude through an analysis processing algorithm, and finally performs gravity compensation and double integration on the instantaneous attitude to obtain an inertial navigation track in the time period so as to realize track reconstruction.

A track reconstruction method of the system specifically comprises the following steps:

(1) the accelerometer measures the three-axis acceleration of the sensor, and the gyroscope measures the three-axis angular velocity of the sensor;

(2) the first level conversion module receives triaxial acceleration measured by an accelerometer and triaxial angular velocity measured by a gyroscope, and transmits the triaxial acceleration and the triaxial angular velocity to a wireless receiving module carried on a second level conversion module through a wireless transmission module carried on the first level conversion module;

(3) sending the acceleration and angular velocity signals obtained in the step (2) after the optimized compensation to a filtering module of an upper computer, wherein the filtering module performs digital low-pass filtering on the signals to eliminate high-frequency noise signals and reduce random errors of the signals;

(4) solving the attitude angle of the accelerometer through the relation of three-axis components of the digital low-pass filtered acceleration signal obtained in the step (3);

(5) solving the attitude angle of the gyroscope by the digital low-pass filtered angular velocity signal obtained in the step 3 through an Euler method;

(6) sending the attitude angle of the accelerometer and the attitude angle of the gyroscope obtained in the steps 4 and 5 to a complementary filtering module, and complementing the attitude angles of the accelerometer and the gyroscope to obtain the optimal attitude angle of the inertial sensor;

(7) obtaining an optimal attitude matrix of the inertial sensor from the optimal attitude angle of the inertial sensor obtained in the step 6, and transforming the triaxial acceleration of the accelerometer obtained in the step 3 into a coordinate system through the attitude matrix to obtain triaxial acceleration under an inertial system; and then, gravity compensation is carried out on the triaxial acceleration, and double integration is carried out on the compensated triaxial acceleration to obtain the position and speed information of the object, so that track reconstruction is realized.

Further, the processing steps of the error compensation module are as follows:

establishing an accelerometer error model equation, wherein the model equation is as follows:

$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{c}{a}_{x0}\\ a_{y0}\\ a_{z0}\end{array}\right]+\left[\begin{array}{ccc}{S}_x& K_{yx}& K_{zx}\\ K_{xy}& S_y& K_{zy}\\ K_{xz}& K_{yz}& S_z\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{ccc}{K}_{x2}& 0& 0\\ 0& K_{y2}& 0\\ 0& 0& K_{z2}\end{array}\right]\left[\begin{array}{c}{a}_x^2\\ {a_y}^2\\ {a_z}^2\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(1\right)$

wherein A is_x、A_y、A_zRespectively generation by generationOutput of accelerometer of meter in x, y, z axes, a_x0、a_y0、a_z0Respectively representing zero offset, S, of the three axes x, y, z of the accelerometer_x、S_y、S_zRepresenting scale factors on the x, y, z axes of the accelerometer, respectively, K_xyRepresenting the coupling error factor, K, between the x and y axes_yxRepresenting the coupling error factor between the y and x axes; k_yzRepresenting the coupling error factor, K, between the y and z axes_zyRepresenting the coupling error factor between the z and y axes; k_zxRepresenting the coupling error factor, K, between the z and x axes_xzRepresenting the coupling error factor, K, between the x and z axes_x2、K_y2、K_z2All represent error coefficients of the three axes of the accelerometer with respect to the second power, v_x、v_y、v_zRespectively representing random errors of three axes x, y and z of the accelerometer;

establishing an error model equation of the gyroscope, wherein the model equation is as follows:

$\left[\begin{array}{c}{G}_x\\ G_y\\ G_z\end{array}\right]=\left[\begin{array}{c}{g}_{x0}\\ g_{y0}\\ g_{z0}\end{array}\right]+\left[\begin{array}{ccc}{S}_x& K_{yx}& K_{zx}\\ K_{xy}& S_y& K_{zy}\\ K_{xz}& K_{yz}& S_z\end{array}\right]\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]+\left[\begin{array}{ccc}{K}_{x2}& 0& 0\\ 0& K_{y2}& 0\\ 0& 0& K_{z2}\end{array}\right]\left[\begin{array}{c}{g}_x^2\\ g_y^2\\ g_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(2\right)$

wherein G is_x、G_y、G_zRepresenting outputs of gyroscopes in x, y, z axes, g_x0、g_y0、g_z0Respectively representing zero offset, S, of the three x, y and z axes of the gyroscope_x、S_y、S_zRespectively representing scale factors on the x, y and z axes of the gyroscope,K_xyrepresenting the coupling error factor, K, between the x and y axes_yxRepresenting the coupling error factor between the y and x axes; k_yzRepresenting the coupling error factor, K, between the y and z axes_zyRepresenting the coupling error factor between the z and y axes; k_zxRepresenting the coupling error factor, K, between the z and x axes_xzRepresenting the coupling error factor, K, between the x and z axes_x2、K_y2、K_z2Respectively representing the error coefficients of the three axes of the gyroscope with respect to the square, v_x、v_y、v_zRespectively representing random errors of x, y and z three axes of the gyroscope;

after error model equations of the MEMS accelerometer and the gyroscope are established, model error coefficients of the accelerometer and the gyroscope are calibrated by a six-position calibration method, wherein the model error coefficients are respectively as follows:

the accelerometer is the same as the gyroscope in the solution of the error coefficient, taking the accelerometer as an example, substituting the data detected by the accelerometer into an accelerometer error model equation according to the calibration process of the accelerometer to obtain the related error coefficient in the accelerometer error model, namely

An accelerometer X-axis:

A_x1＝a_x0+S_x+K_x2,A_x2＝a_x0+K_yx,A_x3＝a_x0+K_zx

A_x4＝a_x0-S_x+K_x2,A_x5＝a_x0-K_yx,A_x6＝a_x0-K_zx

accelerometer Y-axis:

A_y1＝a_y0+K_xy,A_y2＝a_y0+S_y+K_y2,A_y3＝a_y0+K_zy

A_y4＝a_y0-K_xy,A_y5＝a_y0-S_y+K_y2,A_y6＝a_y0-K_zy

accelerometer Z-axis:

A_z1＝a_z0+K_xz,A_y2＝a_z0+K_yz,A_z3＝a_z0+S_z+K_z2

A_z4＝a_z0-K_xz,A_z5＝a_z0-K_zy,A_z6＝a_z0-S_z+K_z2

solving the three groups of equations to obtain all model error coefficients; and obtaining a related error coefficient in the gyroscope error model in the same way.

Further, the processing steps of the filtering module are as follows:

a finite impulse response digital filter based on a Kaiser window is adopted to suppress random errors of a gyroscope and an accelerometer and compensate lagging phases.

Further, the step 4 specifically includes:

expanding the formula (3) and the formula (4) to obtain a formula (5) and a formula (6), and substituting the acceleration signal obtained in the step (3) after digital low-pass filtering into the formula (5) and the formula (6) to obtain the attitude angle of the accelerometer;

q＝90-a_y(3)

cosqcosg＝cosa_z(4)

$q=\arcsin \frac{g_x}{\sqrt{g_x^2+g_y^2+g_z^2}}---\left(5\right)$

$g=\arctan -\frac{g_x}{g_z}---\left(6\right)$

wherein, α_x、α_y、α_zrespectively the directional cosine of the X, Y, Z axis, g_x、g_y、g_zThe components of gravity on three axes respectively; theta is the roll angle of the accelerometer and gamma is the pitch angle.

Further, the step 5 specifically comprises the following steps:

substituting the digital low-pass filtered angular velocity signal obtained in the step (3) into a formula (7) to obtain an attitude angle of the gyroscope;

$\left[\begin{array}{c}\stackrel{\·}{y}\\ \stackrel{\·}{q}\\ \stackrel{\·}{g}\end{array}\right]=\left[\begin{array}{ccc}-\frac{\sin g}{\cos q}& 0& \frac{\cos g}{\cos q}\\ \cos g& 0& \sin g\\ \sin g\tan q& 1& -\cos g\tan q\end{array}\right]\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]---\left(7\right)$

whereinRespectively representing the differential, omega, of the course angle, roll angle, pitch angle of the carrier_x、ω_y、ω_zRespectively, representing the three-axis angular velocities of the gyroscope.

Further, the processing steps of the complementary filtering module are as follows:

substituting the attitude angle of the accelerometer and the attitude angle of the gyroscope obtained in the steps 4 and 5 into a formula (8) to obtain the optimal attitude angle of the inertial sensor;

j_K＝K_j(j_A-j_G)+j_G(8)

q_K＝K_q(q_A-q_G)+q_G(9)

g_K＝K_g(g_A-g_G)+g_G(10) wherein j is_K、q_K、g_KThree optimal attitude angles for the inertial sensor; k_j、K_q、K_gAll three attitude angles are fused gain values;θ_A、γ_Aall represent attitude angles calculated by the accelerometer,θ_G、γ_Gand representing the attitude angle obtained by the gyroscope through calculation.

Further, the step 7 specifically includes:

obtaining an optimal attitude matrix of the inertial sensor from the optimal attitude angle of the inertial sensor obtained in the step 6, and transforming the triaxial acceleration of the accelerometer obtained in the step 3 into a coordinate system through the attitude matrix to obtain triaxial acceleration under an inertial system; and then, gravity compensation is carried out on the triaxial acceleration, and double integration is carried out on the compensated triaxial acceleration to obtain the position and speed information of the object, so that track reconstruction is realized.

Substituting the optimal attitude angle of the inertial sensor obtained in the step 6 into a formula (11) to obtain an optimal attitude matrix of the inertial sensor;

$C_n^b=\left[\begin{array}{ccc}\cos g& 0& -\sin g\\ 0& 1& 0\\ \sin g& 0& \cos g\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos q& \sin q\\ 0& -\sin q& \cos q\end{array}\right]\left[\begin{array}{ccc}\cos j& \sin j& 0\\ -\sin j& \cos j& 0\\ 0& 0& 1\end{array}\right]---\left(11\right)$

and transforming the triaxial acceleration to an inertial coordinate system through the optimal attitude matrix, and performing gravity acceleration compensation, wherein the formula is as follows:

wherein x is_b、y_b、z_bRepresenting the acceleration signals of three axes in a carrier coordinate system; x is the number of_n、y_n、z_nRepresenting the acceleration signals of three axes in an inertial coordinate system, g being the gravity vector, x_a、y_a、z_aThe acceleration signals of the three axes after gravity compensation;

suppose from t₀The sampling is started at the moment t, and the displacement s (t) and the speed v (t) of the carrier at the moment t are expressed as follows:

$s\left(t\right)=\underset{t_0}{\overset{t}{\∫}}v\left(t\right)dt+s_0---\left(14\right)$

$v\left(t\right)=\underset{t_0}{\overset{t}{\∫}}a\left(t\right)dt+v_0---\left(15\right)$

wherein s is₀、v₀Representing the displacement and speed of the carrier at the initial moment; d τ is a small quantity, such that

dt＝t_i-t_i-1＝Dt,t₀￡t_i￡t (16)

Wherein i is a positive integer;

rewriting formula (14) and formula (15) into an accumulative formula to obtain formula (17) and formula (18):

$s\left(n\right)=\underset{i=1}{\overset{n}{\Σ}}\frac{v\left(i\right)+v(i-1)}{2}Dt+s_0---\left(17\right)$

$v\left(n\right)=\underset{i=1}{\overset{n}{\Σ}}\frac{a\left(i\right)+a(i-1)}{2}Dt+v_0---\left(18\right)$

wherein a (i), v (i) represent the acceleration and speed of the carrier at time i; n is a positive number; s (n) is the final displacement of the carrier; v (n) is the final velocity of the carrier; dt is a time interval;

subtracting s (n) from s (n-1) by equation (17), and subtracting v (n) from v (n-1) by equation (18) yields the recursion:

$s\left(n\right)=\frac{v\left(n\right)+v(n-1)}{2}Dt+s(n-1)---\left(19\right)$

$v\left(n\right)=\frac{a\left(n\right)+a(n-1)}{2}Dt+v(n-1)---\left(20\right)$

and obtaining a final displacement and speed calculation formula of the carrier according to the formula (19) and the formula (20):

$v\left(n\right)=\frac{a\left(n\right)+a(n-1)}{2}Dt+v(n-1)---\left(21\right)$

$s\left(n\right)=\frac{a\left(n\right)+a(n-1)}{4}{\mathrm{Dt}}^2+v(n-1)Dt+s(n-1).---\left(22\right)$

compared with the prior art, the technical scheme of the invention has the following advantages:

1. the invention has lower requirement on hardware, and only needs one inertial navigation module with multiple sensors and a signal remote transmission module. The signal processing process is mainly completed in the upper computer, so that the volume of the inertial navigation system is small, the inertial navigation track reconstruction can be realized in a tiny working area, and the accuracy cannot be influenced by too large volume of the inertial navigation system.

2. The invention tests the error model according to the characteristics of the sensor. A large amount of data of a sensor carried in a system is collected through a six-position method, zero offset and the like of the sensor are calculated through error analysis of the data, an error compensation matrix is established only aiming at compensation data of the sensor, and the method has better specificity instead of adopting a common correction model. Therefore, the sensor can be properly corrected according to the structural characteristics and the precision level of the sensor.

3. The invention utilizes a complementary filtering method to determine the optimal estimation value of the attitude angle at the current moment, has clear concept, relatively accords with the practical application of engineering, has better compensation effect and higher practical value. The gyroscope and the accelerometer can independently obtain the attitude angle of the carrier, but the two signals have larger errors, and if the signals of the two sensors are fused together, the defects of the two signals can be complemented with each other, so that the error of the algorithm is converged, and the accuracy of attitude calculation is greatly improved.

Drawings

FIG. 1 is a structural flow chart of a track reconstruction algorithm based on an MEMS inertial navigation system according to the present invention;

FIG. 2 is a block diagram of a complementary filtering algorithm for accelerometer and gyroscope data fusion;

FIG. 3 is a schematic block diagram of a strapdown inertial navigation system.

Detailed Description

The invention will be further described with reference to the accompanying drawings.

An MEMS inertial navigation system comprises an inertial sensor, a first level conversion module, a power supply module, a second level conversion module and an upper computer; the inertial sensor consists of an accelerometer and a gyroscope; a wireless transmission module is carried on the first level conversion module; a wireless receiving module is carried on the second level conversion module; the accelerometer is used for measuring the three-axis acceleration of the sensor, and the gyroscope is used for measuring the three-axis angular velocity of the sensor; the accelerometer and the gyroscope are both connected with the first level conversion module; the wireless transmission module and the wireless receiving module transmit data wirelessly; the second level conversion module is connected with the upper computer through a USB; and the upper computer performs static error compensation and filtering on the received signals, calculates and updates the attitude through an analysis processing algorithm, and finally performs gravity compensation and double integration on the instantaneous attitude to obtain an inertial navigation track in the time period so as to realize track reconstruction.

A track reconstruction method of the system specifically comprises the following steps:

(1) the accelerometer measures the three-axis acceleration of the sensor, and the gyroscope measures the three-axis angular velocity of the sensor;

(2) the first level conversion module receives triaxial acceleration measured by an accelerometer and triaxial angular velocity measured by a gyroscope, and transmits the triaxial acceleration and the triaxial angular velocity to a wireless receiving module carried on a second level conversion module through a wireless transmission module carried on the first level conversion module;

(3) sending the acceleration and angular velocity signals obtained in the step (2) after the optimized compensation to a filtering module of an upper computer, wherein the filtering module performs digital low-pass filtering on the signals to eliminate high-frequency noise signals and reduce random errors of the signals;

(4) solving the attitude angle of the accelerometer through the relation of three-axis components of the digital low-pass filtered acceleration signal obtained in the step (3);

(5) solving the attitude angle of the gyroscope by the digital low-pass filtered angular velocity signal obtained in the step 3 through an Euler method;

(6) sending the attitude angle of the accelerometer and the attitude angle of the gyroscope obtained in the steps 4 and 5 to a complementary filtering module, and complementing the attitude angles of the accelerometer and the gyroscope to obtain the optimal attitude angle of the inertial sensor;

(7) obtaining an optimal attitude matrix of the inertial sensor from the optimal attitude angle of the inertial sensor obtained in the step 6, and transforming the triaxial acceleration of the accelerometer obtained in the step 3 into a coordinate system through the attitude matrix to obtain triaxial acceleration under an inertial system; and then, gravity compensation is carried out on the triaxial acceleration, and double integration is carried out on the compensated triaxial acceleration to obtain the position and speed information of the object, so that track reconstruction is realized.

Analysis of an error model: analyzing the cause of error formation of the MEMS accelerometer can obtain an error model equation as follows:

$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{c}{a}_{x0}\\ a_{y0}\\ a_{z0}\end{array}\right]+\left[\begin{array}{ccc}{S}_x& K_{yx}& K_{zx}\\ K_{xy}& S_y& K_{zy}\\ K_{xz}& K_{yz}& S_z\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{ccc}{K}_{x2}& 0& 0\\ 0& K_{y2}& 0\\ 0& 0& K_{z2}\end{array}\right]\left[\begin{array}{c}{a}_x^2\\ {a_y}^2\\ {a_z}^2\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(1\right)$

wherein A is_x、A_y、A_zRepresenting the outputs of the accelerometers in the x, y, z axes, a_x0、a_y0、a_z0Respectively representing zero offset, S, of the three axes x, y, z of the accelerometer_x、S_y、S_zRepresenting scale factors on the x, y, z axes of the accelerometer, respectively, K_xyRepresenting the coupling error factor, K, between the x and y axes_yxRepresenting the coupling error factor between the y and x axes; k_yzRepresenting the coupling error factor, K, between the y and z axes_zyRepresenting the coupling error factor between the z and y axes; k_zxRepresenting the coupling error factor, K, between the z and x axes_xzRepresenting the coupling error factor, K, between the x and z axes_x2、K_y2、K_z2All represent error coefficients of the three axes of the accelerometer with respect to the second power, v_x、v_y、v_zRespectively representing random errors of three axes x, y and z of the accelerometer;

similar to the MEMS accelerometer, the error model equation for the gyroscope is also established as follows:

$\left[\begin{array}{c}{G}_x\\ G_y\\ G_z\end{array}\right]=\left[\begin{array}{c}{g}_{x0}\\ g_{y0}\\ g_{z0}\end{array}\right]+\left[\begin{array}{ccc}{S}_x& K_{yx}& K_{zx}\\ K_{xy}& S_y& K_{zy}\\ K_{xz}& K_{yz}& S_z\end{array}\right]\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]+\left[\begin{array}{ccc}{K}_{x2}& 0& 0\\ 0& K_{y2}& 0\\ 0& 0& K_{z2}\end{array}\right]\left[\begin{array}{c}{g}_x^2\\ g_y^2\\ g_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(3\right)$

wherein G is_x、G_y、G_zRespectively representing outputs of the gyroscope on three axes, g_x0、g_y0、g_z0Respectively representing zero offset, S, of three axes of the gyroscope_x、S_y、S_zRespectively representing scale factors, K, on three axes of the gyroscope_yx、K_xyRepresenting the coupling between the X and Y axesAn error factor; k_zy、K_yzRepresenting the coupling error factor between the Y and Z axes; k_xz、K_zxRepresenting the coupling error factor, K, between the Z and X axes_x2、K_y2、K_z2Respectively representing the error coefficient of the gyroscope with respect to the square, v_x、v_y、v_zRespectively representing the random error of the three axes of the gyroscope.

After error model equations of the MEMS accelerometer and the gyroscope are established, model error coefficients of the two sensors can be calibrated through a position calibration method. The invention adopts a six-position calibration method according to the number of unknown coefficients of an error model, which respectively comprises the following steps:

the only difference between the accelerometer and the gyroscope in the experiment is that the accelerometer needs constant g input, the module needs to keep a static state, and static measurement can be performed on the platform; however, the gyroscope is not output in a static state, the module needs to be fixed on the turntable to keep the coincidence of the rotating shaft of the turntable and the coordinate axes of the module, and the test is completed by means of the uniform rotation of the turntable.

And (3) solving the error coefficient, wherein the accelerometer is the same as the gyroscope, the accelerometer is taken as an example, and data measured by the accelerometer is substituted into an error model of the accelerometer according to a specified MEMS accelerometer calibration process to obtain the following three groups of equations:

x-axis of accelerometer:

A_x1＝a_x0+S_x+K_x2,A_x2＝a_x0+K_yx,A_x3＝a_x0+K_zx

A_x4＝a_x0-S_x+K_x2,A_x5＝a_x0-K_yx,A_x6＝a_x0-K_zx

y-axis of accelerometer:

A_y1＝a_y0+K_xy,A_y2＝a_y0+S_y+K_y2,A_y3＝a_y0+K_zy

A_y4＝a_y0-K_xy,A_y5＝a_y0-S_y+K_y2,A_y6＝a_y0-K_zy

z-axis of accelerometer:

A_z1＝a_z0+K_xz,A_y2＝a_z0+K_yz,A_z3＝a_z0+S_z+K_z2

A_z4＝a_z0-K_xz,A_z5＝a_z0-K_zy,A_z6＝a_z0-S_z+K_z2

the three equations can respectively obtain the related error coefficients in the error model of the acceleration sensor:

an accelerometer X-axis:

$a_{x0}=\frac{A_{x2}+A_{x3}+A_{x5}+A_{x6}}{4},S_x=\frac{A_{x1}-A_{x4}}{2},K_{yx}=\frac{A_{x2}-A_{x5}}{2}$

$K_{zx}=\frac{A_{x3}-A_{x6}}{2},K_{x2}=\frac{2(A_{x1}+A_{x4})-A_{x2}-A_{x3}-A_{x5}-A_{x6}}{4}$

accelerometer Y-axis:

$a_{y0}=\frac{A_{y1}+A_{y3}+A_{y4}+A_{y6}}{4},S_y=\frac{A_{y2}-A_{y5}}{2},K_{xy}=\frac{A_{y1}-A_{y4}}{2}$

$K_{zy}=\frac{A_{y3}-A_{y6}}{2},K_{y2}=\frac{2(A_{y2}+A_{y5})-A_{y1}-A_{y3}-A_{y4}-A_{y6}}{4}$

accelerometer Z-axis:

$a_{z0}=\frac{A_{z1}+A_{z2}+A_{z4}+A_{z5}}{4},S_z=\frac{A_{z3}-A_{z6}}{2},K_{xz}=\frac{A_{z1}-A_{z4}}{2}$

$K_{yz}=\frac{A_{z2}-A_{z5}}{2},K_{z2}=\frac{2(A_{z3}+A_{z6})-A_{z1}-A_{z2}-A_{a4}-A_{z5}}{4}$

b, a low-pass filtering module: after error models of the accelerometer and gyroscope are built, the output of the accelerometer and gyroscope can be statically compensated, but random errors of the sensor cannot be determined by an experimental method. Since the mean value is used for each test point value when calibrating the accelerometer and gyroscope error models in A, the random error v is ignored in solving the parameters_x、v_y、v_z. The invention relates to a method for processing random errors, which comprises low-pass filtering, Kalman filtering and heuristic subtraction filtering (HDR), wherein the invention uses a finite impulse response digital filter based on Kaiser window to restrain the random errors of three axes of a gyroscope and an accelerometer and compensate lagging phase.

And C, a posture updating module: after correcting zero offset errors and random noises of the three-axis angular velocity and the three-axis acceleration, calculating the differential of the module attitude angle at the moment by an Euler method according to the three-axis angular velocity; the triaxial acceleration expresses the current attitude angle through coordinate transformation.

Taking gravity as an example, in a three-dimensional space, the components of gravity on three coordinate axes have the following relations with the directions of gravity and the directions of the three coordinate axes:

q＝90-a_y(3)

cosqcosg＝cosa_z(4)

equations (5) and (6), i.e., roll and pitch expressions, are derived from equations (3) and (4):

$q=\arcsin \frac{g_x}{\sqrt{g_x^2+g_y^2+g_z^2}}---\left(5\right)$

$g=\arctan -\frac{g_x}{g_z}---\left(6\right)$

wherein, α_x、α_y、α_zare respectivelyX, Y, Z direction cosine of axis, g_x、g_y、g_zThe components of gravity on three axes respectively; theta is the roll angle of the accelerometer and gamma is the pitch angle.

The Euler angle attitude updating algorithm utilizes a differential equation relationship to obtain the differentials of the three attitude angles through the angular velocity, and the final result of the equation is as follows:

$\left[\begin{array}{c}\stackrel{\·}{y}\\ \stackrel{\·}{q}\\ \stackrel{\·}{g}\end{array}\right]=\left[\begin{array}{ccc}-\frac{\sin g}{\cos q}& 0& \frac{\cos g}{\cos q}\\ \cos g& 0& \sin g\\ \sin g\tan q& 1& -\cos g\tan q\end{array}\right]\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]---\left(7\right)$

wherein,respectively indicating course angle and roll angle of the carrierDifferential of pitch angle, ω_x、ω_y、ω_zRepresenting the three-axis angular velocities of the gyroscope and compensated for yaw rate and associated errors.

D, a multi-sensor complementary fusion filtering module: the accelerometer and the gyroscope can independently calculate the attitude angle of the carrier, the accelerometer and the gyroscope are fused by a complementary filtering module through a gain value changing along with the motion state to obtain the optimal estimation value of the current attitude angle of the carrier, and the equation of complementary filtering is as follows:

j_K＝K_j(j_A-j_G)+j_G(8)

q_K＝K_q(q_A-q_G)+q_G(9)

g_K＝K_g(g_A-g_G)+g_G(10) wherein,θ_K、γ_Kthree optimal attitude angles for the inertial sensor;K_θ、K_γis a gain value fused by three attitude angles, and is related to the current motion state of the carrier.θ_A、γ_ARepresenting the attitude angle resolved by the accelerometer,θ_G、γ_Gand representing the attitude angle obtained by the gyroscope through calculation.

The acceleration conversion and gravity compensation module: obtaining an attitude matrix of a current moment module by obtaining the current moment optimal attitude angle through complementary filtering calculation:

$C_n^b=\left[\begin{array}{ccc}\cos g& 0& -\sin g\\ 0& 1& 0\\ \sin g& 0& \cos g\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos q& \sin q\\ 0& -\sin q& \cos q\end{array}\right]\left[\begin{array}{ccc}\cos j& \sin j& 0\\ -\sin j& \cos j& 0\\ 0& 0& 1\end{array}\right]---\left(11\right)$

and transforming the triaxial acceleration to an inertial coordinate system through the optimal attitude matrix, and performing gravity acceleration compensation, wherein the formula is as follows:

wherein x is_b、y_b、z_bRepresenting an acceleration signal in a carrier coordinate system; x is the number of_n、y_n、z_nRepresenting acceleration signals in an inertial coordinate system, g being a gravity vector, x_a、y_a、z_aThe acceleration signal is an acceleration signal after gravity compensation;

an integral link and track reconstruction module: the acceleration signal converted by the coordinate system and filtered by gravity represents the triaxial acceleration of the carrier under the inertial system at the current moment, and the position and speed information of the carrier can be obtained after double integration of the triaxial acceleration, so that the track reconstruction of the carrier is realized.

Suppose from t₀The sampling is started at the moment t, and the displacement s (t) and the speed v (t) of the carrier at the moment t are expressed as follows:

$s\left(t\right)=\underset{t_0}{\overset{t}{\∫}}v\left(t\right)dt+s_0---\left(14\right)$

$v\left(t\right)=\underset{t_0}{\overset{t}{\∫}}a\left(t\right)dt+v_0---\left(15\right)$

wherein s is₀、v₀Indicating the displacement and velocity of the carrier at the initial moment, d τIs a small amount, order

dt＝t_i-t_i-1＝Dt,t₀￡t_i￡t (16)

Wherein i is a positive integer;

rewriting formula (14) and formula (15) into an accumulative formula to obtain formula (17) and formula (18):

$s\left(n\right)=\underset{i=1}{\overset{n}{\Σ}}\frac{v\left(i\right)+v(i-1)}{2}Dt+s_0---\left(17\right)$

$v\left(n\right)=\underset{i=1}{\overset{n}{\Σ}}\frac{a\left(i\right)+a(i-1)}{2}Dt+v_0---\left(18\right)$

wherein a (i), v (i) represent the acceleration and speed of the carrier at time i; n is a positive number; s (n) is the final displacement of the carrier; v (n) is the final velocity of the carrier; dt is a time interval;

subtracting s (n) from s (n-1) by equation (17), and subtracting v (n) from v (n-1) by equation (18) yields the recursion:

$s\left(n\right)=\frac{v\left(n\right)+v(n-1)}{2}Dt+s(n-1)---\left(19\right)$

$v\left(n\right)=\frac{a\left(n\right)+a(n-1)}{2}Dt+v(n-1)---\left(20\right)$

and obtaining a final displacement and speed calculation formula of the carrier according to the formula (19) and the formula (20):

$v\left(n\right)=\frac{a\left(n\right)+a(n-1)}{2}Dt+v(n-1)---\left(21\right)$

$s\left(n\right)=\frac{a\left(n\right)+a(n-1)}{4}{\mathrm{Dt}}^2+v(n-1)Dt+s(n-1).---\left(22\right)$

Claims (8)

1. An MEMS inertial navigation system is characterized by comprising an inertial sensor, a first level conversion module, a power supply module, a second level conversion module and an upper computer; the inertial sensor consists of an accelerometer and a gyroscope; a wireless transmission module is carried on the first level conversion module; a wireless receiving module is carried on the second level conversion module; the accelerometer is used for measuring the three-axis acceleration of the sensor, and the gyroscope is used for measuring the three-axis angular velocity of the sensor; the accelerometer and the gyroscope are both connected with the first level conversion module; the wireless transmission module and the wireless receiving module transmit data wirelessly; the second level conversion module is connected with the upper computer through a USB; and the upper computer performs static error compensation and filtering on the received signals, calculates and updates the attitude through an analysis processing algorithm, and finally performs gravity compensation and double integration on the instantaneous attitude to obtain an inertial navigation track in the time period so as to realize track reconstruction.

2. A trajectory reconstruction method using the system of claim 1, comprising the steps of:

(1) the accelerometer measures the three-axis acceleration of the sensor, and the gyroscope measures the three-axis angular velocity of the sensor;

(2) the first level conversion module receives triaxial acceleration measured by an accelerometer and triaxial angular velocity measured by a gyroscope, and transmits the triaxial acceleration and the triaxial angular velocity to a wireless receiving module carried on a second level conversion module through a wireless transmission module carried on the first level conversion module;

(3) sending the acceleration and angular velocity signals obtained in the step (2) after the optimized compensation to a filtering module of an upper computer, wherein the filtering module performs digital low-pass filtering on the signals to eliminate high-frequency noise signals and reduce random errors of the signals;

(4) solving the attitude angle of the accelerometer through the relation of three-axis components of the digital low-pass filtered acceleration signal obtained in the step (3);

(5) solving the attitude angle of the gyroscope by the digital low-pass filtered angular velocity signal obtained in the step 3 through an Euler method;

(6) sending the attitude angle of the accelerometer and the attitude angle of the gyroscope obtained in the steps 4 and 5 to a complementary filtering module, and complementing the attitude angles of the accelerometer and the gyroscope to obtain the optimal attitude angle of the inertial sensor;

(7) obtaining an optimal attitude matrix of the inertial sensor from the optimal attitude angle of the inertial sensor obtained in the step 6, and transforming the triaxial acceleration of the accelerometer obtained in the step 3 into a coordinate system through the attitude matrix to obtain triaxial acceleration under an inertial system; and then, gravity compensation is carried out on the triaxial acceleration, and double integration is carried out on the compensated triaxial acceleration to obtain the position and speed information of the object, so that track reconstruction is realized.

3. The trajectory reconstruction method according to claim 2, wherein the error compensation module specifically comprises the following processing steps:

establishing an accelerometer error model equation, wherein the model equation is as follows:

$\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=\left[\begin{array}{c}{a}_{x0}\\ a_{y0}\\ a_{z0}\end{array}\right]+\left[\begin{array}{ccc}{S}_x& K_{yx}& K_{zx}\\ K_{xy}& S_y& K_{zy}\\ K_{xz}& K_{yz}& S_z\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{ccc}{K}_{x2}& 0& 0\\ 0& K_{y2}& 0\\ 0& 0& K_{z2}\end{array}\right]\left[\begin{array}{c}{a}_x^2\\ {a_y}^2\\ {a_z}^2\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(1\right)$

wherein A is_x、A_y、A_zRepresenting the outputs of the accelerometers in the x, y, z axes, a_x0、a_y0、a_z0Respectively representing zero offset, S, of the three axes x, y, z of the accelerometer_x、S_y、S_zRepresenting scale factors on the x, y, z axes of the accelerometer, respectively, K_xyRepresenting the coupling error factor, K, between the x and y axes_yxRepresenting the coupling error factor between the y and x axes; k_yzRepresenting the coupling error factor, K, between the y and z axes_zyRepresenting the coupling error factor between the z and y axes; k_zxIndicating errors in coupling between the z and x axesDifference factor, K_xzRepresenting the coupling error factor, K, between the x and z axes_x2、K_y2、K_z2All represent error coefficients of the three axes of the accelerometer with respect to the second power, v_x、v_y、v_zRespectively representing random errors of three axes x, y and z of the accelerometer;

establishing an error model equation of the gyroscope, wherein the model equation is as follows:

$\left[\begin{array}{c}{G}_x\\ G_y\\ G_z\end{array}\right]=\left[\begin{array}{c}{g}_{x0}\\ g_{y0}\\ g_{z0}\end{array}\right]+\left[\begin{array}{ccc}{S}_x& K_{yx}& K_{zx}\\ K_{xy}& S_y& K_{zy}\\ K_{xz}& K_{yz}& S_z\end{array}\right]\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]+\left[\begin{array}{ccc}{K}_{x2}& 0& 0\\ 0& K_{y2}& 0\\ 0& 0& K_{z2}\end{array}\right]\left[\begin{array}{c}{g}_x^2\\ g_y^2\\ g_z^2\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]---\left(2\right)$

wherein G is_x、G_y、G_zRepresenting outputs of gyroscopes in x, y, z axes, g_x0、g_y0、g_z0Respectively representing zero offset, S, of the three x, y and z axes of the gyroscope_x、S_y、S_zRespectively representing scale factors on the x, y and z axes of the gyroscope, K_xyRepresenting the coupling error factor, K, between the x and y axes_yxRepresenting the coupling error factor between the y and x axes; k_yzRepresenting the coupling error factor, K, between the y and z axes_zyRepresenting the coupling error factor between the z and y axes; k_zxRepresenting the coupling error factor, K, between the z and x axes_xzRepresenting the coupling error factor, K, between the x and z axes_x2、K_y2、K_z2Respectively representing the error coefficients of the three axes of the gyroscope with respect to the square, v_x、v_y、v_zRespectively representing random errors of x, y and z three axes of the gyroscope;

after error model equations of the MEMS accelerometer and the gyroscope are established, model error coefficients of the accelerometer and the gyroscope are calibrated by a six-position calibration method, wherein the model error coefficients are respectively as follows:

the accelerometer is the same as the gyroscope in the solution of the error coefficient, taking the accelerometer as an example, substituting the data detected by the accelerometer into an accelerometer error model equation according to the calibration process of the accelerometer to obtain the related error coefficient in the accelerometer error model, namely

An accelerometer X-axis:

A_x1＝a_x0+S_x+K_x2,A_x2＝a_x0+K_yx,A_x3＝a_x0+K_zx

A_x4＝a_x0-S_x+K_x2,A_x5＝a_x0-K_yx,A_x6＝a_x0-K_zx

accelerometer Y-axis:

A_y1＝a_y0+K_xy,A_y2＝a_y0+S_y+K_y2,A_y3＝a_y0+K_zy

A_y4＝a_y0-K_xy,A_y5＝a_y0-S_y+K_y2,A_y6＝a_y0-K_zy

accelerometer Z-axis:

A_z1＝a_z0+K_xz,A_y2＝a_z0+K_yz,A_z3＝a_z0+S_z+K_z2

A_z4＝a_z0-K_xz,A_z5＝a_z0-K_zy,A_z6＝a_z0-S_z+K_z2

solving the three groups of equations to obtain all model error coefficients; and obtaining a related error coefficient in the gyroscope error model in the same way.

4. The trajectory reconstruction method according to claim 2, wherein the filtering module specifically comprises the following processing steps:

a finite impulse response digital filter based on a Kaiser window is adopted to suppress random errors of a gyroscope and an accelerometer and compensate lagging phases.

5. The trajectory reconstruction method according to claim 2, wherein the step 4 is specifically as follows:

expanding the formula (3) and the formula (4) to obtain a formula (5) and a formula (6), and substituting the acceleration signal obtained in the step (3) after digital low-pass filtering into the formula (5) and the formula (6) to obtain the attitude angle of the accelerometer;

q＝90-a_y(3)

cosqcosg＝cosa_z(4)

$q=\arcsin \frac{g_x}{\sqrt{g_x^2+g_y^2+g_z^2}}---\left(5\right)$

$g=\arctan -\frac{g_x}{g_z}---\left(6\right)$

wherein, α_x、α_y、α_zrespectively the directional cosine of the X, Y, Z axis, g_x、g_y、g_zThe components of gravity on three axes respectively; theta is the roll angle of the accelerometer and gamma is the pitch angle.

6. The trajectory reconstruction method according to claim 2, wherein the step 5 is as follows:

substituting the digital low-pass filtered angular velocity signal obtained in the step (3) into a formula (7) to obtain an attitude angle of the gyroscope;

$\left[\begin{array}{c}\stackrel{\·}{y}\\ \stackrel{\·}{q}\\ \stackrel{\·}{g}\end{array}\right]=\left[\begin{array}{ccc}-\frac{\sin g}{\cos q}& 0& \frac{\cos g}{\cos q}\\ \cos g& 0& \sin g\\ \sin g\tan q& 1& -\cos g\tan q\end{array}\right]\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]---\left(7\right)$

whereinRespectively representing the differential, omega, of the course angle, roll angle, pitch angle of the carrier_x、ω_y、ω_zRespectively, representing the three-axis angular velocities of the gyroscope.

7. The trajectory reconstruction method according to claim 2, wherein the complementary filtering module comprises the following processing steps:

substituting the attitude angle of the accelerometer and the attitude angle of the gyroscope obtained in the steps 4 and 5 into a formula (8) to obtain the optimal attitude angle of the inertial sensor;

j_K＝K_j(j_A-j_G)+j_G(8)

q_K＝K_q(q_A-q_G)+q_G(9)

g_K＝K_g(g_A-g_G)+g_G(10)

wherein j is_K、q_K、g_KThree optimal attitude angles for the inertial sensor; k_j、K_q、K_gAll three attitude angles are fused gain values;θ_A、γ_Aall represent attitude angles calculated by the accelerometer,θ_G、γ_Gand representing the attitude angle obtained by the gyroscope through calculation.

8. The trajectory reconstruction method according to claim 2, wherein the step 7 specifically comprises:

obtaining an optimal attitude matrix of the inertial sensor from the optimal attitude angle of the inertial sensor obtained in the step 6, and transforming the triaxial acceleration of the accelerometer obtained in the step 3 into a coordinate system through the attitude matrix to obtain triaxial acceleration under an inertial system; and then, gravity compensation is carried out on the triaxial acceleration, and double integration is carried out on the compensated triaxial acceleration to obtain the position and speed information of the object, so that track reconstruction is realized.

Substituting the optimal attitude angle of the inertial sensor obtained in the step 6 into a formula (11) to obtain an optimal attitude matrix of the inertial sensor;

$C_n^b=\left[\begin{array}{ccc}\cos g& 0& -\sin g\\ 0& 1& 0\\ \sin g& 0& \cos g\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos q& \sin q\\ 0& -\sin q& \cos q\end{array}\right]\left[\begin{array}{ccc}\cos j& \sin j& 0\\ -\sin j& \cos j& 0\\ 0& 0& 1\end{array}\right]---\left(11\right)$

and transforming the triaxial acceleration to an inertial coordinate system through the optimal attitude matrix, and performing gravity acceleration compensation, wherein the formula is as follows:

wherein x is_b、y_b、z_bRepresenting the acceleration signals of three axes in a carrier coordinate system; x is the number of_n、y_n、z_nRepresenting the acceleration signals of three axes in an inertial coordinate system, g being the gravity vector, x_a、y_a、z_aThe acceleration signals of the three axes after gravity compensation;

suppose from t₀The sampling is started at the moment t, and the displacement s (t) and the speed v (t) of the carrier at the moment t are expressed as follows:

$s\left(t\right)=\underset{t_0}{\overset{t}{\∫}}v\left(t\right)dt+s_0---\left(14\right)$

$v\left(t\right)=\underset{t_0}{\overset{t}{\∫}}a\left(t\right)dt+v_0---\left(15\right)$

wherein s is₀、v₀Representing the displacement and speed of the carrier at the initial moment; d τ is a small quantity, such that

Wherein i is a positive integer;

rewriting formula (14) and formula (15) into an accumulative formula to obtain formula (17) and formula (18):

$s\left(n\right)=\underset{i=1}{\overset{n}{\Σ}}\frac{v\left(i\right)+v(i-1)}{2}Dt+s_0---\left(17\right)$

$v\left(n\right)=\underset{i=1}{\overset{n}{\Σ}}\frac{a\left(i\right)+a(i-1)}{2}Dt+v_0---\left(18\right)$

wherein a (i), v (i) represent the acceleration and speed of the carrier at time i; n is a positive number; s (n) is the final displacement of the carrier; v (n) is the final velocity of the carrier; dt is a time interval;

subtracting s (n) from s (n-1) by equation (17), and subtracting v (n) from v (n-1) by equation (18) yields the recursion:

$s\left(n\right)=\frac{v\left(n\right)+v(n-1)}{2}Dt+s(n-1)---\left(19\right)$

$v\left(n\right)=\frac{a\left(n\right)+a(n-1)}{2}Dt+v(n-1)---\left(20\right)$

and obtaining a final displacement and speed calculation formula of the carrier according to the formula (19) and the formula (20):

$v\left(n\right)=\frac{a\left(n\right)+a(n-1)}{2}Dt+v(n-1)---\left(21\right)$

$s\left(n\right)=\frac{a\left(n\right)+a(n-1)}{4}{\mathrm{Dt}}^2+v(n-1)Dt+s(n-1).---\left(22\right)$