CN105928519A - Navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer - Google Patents

Abstract

The invention discloses a navigation algorithm based on INS inertial navigation, GPS navigation and a magnetometer. The navigation algorithm can accurately reflect the attitude and position information of an unmanned plane. The navigation algorithm employs the advantage of long-term high positioning precision of GPS navigation to compensate for the disadvantage of divergence of cumulative errors of INS inertial navigation along with passing of time; the navigation algorithm also employs the characteristics of no proneness to external interference and continuous output of navigation information of INS inertial navigation to compensate for the disadvantages of proneness to interference and limited output frequency of GPS navigation; moreover, the navigation algorithm overcomes the problems that true north cannot be found via a yaw angle calculated through inertial navigation and great drift occurs; the navigation algorithm utilizes a yaw angle calculated by using the magnetometer for correction so as to obtain a geographical true north direction and a stable yaw angle; in addition, the navigation algorithm has good stability and can output satisfactory navigation positioning information, so the navigation algorithm can accurately reflect the attitude and position information of a carrier and is suitable for promotion and application in the field of navigation technology.

Description

Navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer

Technical Field

The invention relates to the technical field of navigation, in particular to a navigation algorithm based on INS inertial navigation, GPS navigation and a magnetometer.

Background

In the aspect of navigation technology, the most applications are achieved at present, and the most mature navigation modes comprise INS inertial navigation and satellite navigation. The GPS satellite navigation has the advantages of being global, all-weather and high in long-time positioning accuracy, but has the defects that signals are easy to interfere and shield, the signal quality is poor in a strong electromagnetic environment and when high-rise shielding exists, the output frequency is limited, generally 1-10 Hz, the output is discontinuous, and the defects of the GPS satellite navigation are highlighted in occasions needing quick information updating, such as unmanned aerial vehicle systems with high requirements on maneuverability and real-time performance. The INS inertial navigation system is a fully autonomous navigation mode, so that the system has strong concealment and anti-interference capability, continuous output information and high positioning accuracy in a short time. However, due to the characteristics of the MEMS-INS device, the gyroscope and the accelerometer have errors such as initial zero offset and random drift, and the errors become larger and larger with the cumulative effect of time, so that the positioning accuracy is poor for a long time, and finally the attitude and position information of the unmanned aerial vehicle cannot be accurately reflected.

Disclosure of Invention

The invention aims to solve the technical problem of providing a navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer, which can accurately obtain the attitude and position information of a carrier.

The technical scheme adopted by the invention for solving the technical problems is as follows: the navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer comprises the following steps:

A. measuring carrier angular velocity parameters by using gyroscope of INS inertial navigation systemSubstituting into quaternion differential equation to obtain quaternion q₀，q₁，q₂，q₃(ii) a WhereinThe angular velocity information of three axes measured by a gyroscope under the coordinate system of the carrier;

the quaternion differential equation is:

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=0.5\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right];$

B. q solved in step A₀，q₁，q₂，q₃Substituting the following formula to obtain an attitude matrix

$C_n^b=\left[\begin{array}{ccc}{q}_0+q_1+q_2+q_3& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0-q_1+q_2-q_3& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0-q_1-q_2+q_3\end{array}\right]$

According to the followingRelation with direction cosine

Calculating the attitude angles theta, gamma and gamma of the INS inertial navigation module of the carrier,

C. Yaw angle measured by magnetometerReplacing the calculated yaw angle in the step B

D. Measuring an acceleration parameter f of an accelerometer of an INS inertial navigation system^bAnd B, solving to obtain an attitude matrixSubstituting the velocity information v of the carrier in the east, north and sky directions under an INS inertial navigation coordinate system to obtain velocity information v_Nv_Ev_UThe differential equation is:

${\stackrel{\·}{v}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n$

wherein v isⁿ＝[v_Nv_Ev_U]' are the velocities in the east, north and sky directions of the INS inertial navigation coordinate system respectively,is the angular velocity of the earth's rotation,is the angular velocity g of the carrier in each axial direction around the INS inertial navigation coordinate systemⁿIs the acceleration of gravity;

E. v calculated in step D_Nv_Ev_URespectively substituting the position information of the carrier in the INS inertial navigation system into the following formula to obtain the position information of the carrier in the INS inertial navigation system, wherein L is latitude, lambda is longitude, and h is height,

h＝h(0)+∫v_Udt, where L (0) represents the latitude value of the initial position of the carrier, λ (0) represents the longitude value of the initial position of the carrier, and h (0) represents the initial height of the carrier from the earth's surface. R_MRepresenting the radius of curvature, R, in the meridian of the earth_NRepresenting the radius of curvature over a circle of latitude;

F. establishing an equation of stateAnd observation equation Z (t) ═ H (t) X_I(t)+V(t)，

X_I(t) represents the error state of the INS inertial navigation system at time t, which is a 15-dimensional vector as follows:

v_x,v_y,v_zthe velocity error of the INS inertial navigation system along the east, north and sky directions; phi is a_x,φ_y,φ_zIs the attitude angle error of the carrier; l, lambda and h respectively represent latitude, longitude and altitude errors of the carrier;_x,_y,_zrespectively representing the random drift of the gyroscope;respectively, random drift of the accelerometer, whereinIs a matrix of 15 × 15, wherein F_N(t) corresponds to v_x,v_y,v_z，φ_x,φ_y,φ_zThe non-zero elements of the INS inertial navigation system matrix of the 9 parameters L, λ, h are as follows:

$\begin{array}{cc}F(1,1)=\frac{v_y\tan L}{R_N+h}-\frac{v_z}{R_N+h}& F(1,2)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_x\tan L}{R_N+h}\end{array}$

F(1,5)＝-f_zF(1,6)＝f_y

$F(1,7)=(2{\mathrm{\ω}}_{ie}{v}_z\sin L+2{\mathrm{\ω}}_{ie}{v}_y\cos L+\frac{v_x{v}_y{\sec }^{2}L}{R_N+h})$

$\begin{array}{cc}F(2,1)=-(\frac{2v_x\tan L}{R_N+h}+2{\mathrm{\ω}}_{ie}\sin L)& F(2,2)=-\frac{v_z}{R_M+h}\end{array}$

F(2,4)＝f_zF(2,6)＝-f_x

$\begin{array}{cc}F(2,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_x{\sec }^{2}L}{R_N+h})v_x& F(3,1)=(2{\mathrm{\ω}}_{ie}\cos L+\frac{2v_x}{R_M+h})\end{array}$

F(3,4)＝-f_yF(3,5)＝f_xF(3,7)＝-2ω_iev_xsinL

$\begin{array}{ccc}F(3,9)=\frac{2g_0}{R_M}& F(4,2)=-\frac{1}{R_M+h}& F(4,5)={\mathrm{\ω}}_{ie}\sin L+\frac{v_x}{R_N+h}\tan L\end{array}$

$\begin{array}{cc}F(4,6)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_x}{R_N+h}& F(5,1)=\frac{1}{R_N+h}\end{array}$

F(5,7)＝-ω_iesinL

$\begin{array}{ccc}F(6,1)=\frac{tgL}{R_N+h}& F(6,4)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}& F(6,5)=\frac{v_y}{R_M+h}\end{array}$

$\begin{array}{ccc}F(6,7)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L& F(7,2)=\frac{1}{R_M+h}& F(8,1)=\frac{\sec L}{R_N+h}\end{array}$

F(9,3)＝1

F_S(t) is v_x,v_y,v_z，φ_x,φ_y,φ_zThe transformation matrix between the 9 parameters L, λ, h and the gyro and accelerometer drifts, whose dimensions are 9 × 6,

F_M(t) is_x,_y,_z，The INS inertial navigation system matrix, corresponding to the gyroscope and accelerometer drifts, is a diagonal matrix with dimensions 6 × 6, as follows:

F_M(t)＝diag[-1/T_gx-1/T_gy-1/T_gz-1/T_ax-1/T_ay-1/T_az](ii) a Wherein, T_gxTime constant, T, of error model representing x-axis of gyroscope_gyTime constant, T, of error model representing y-axis of gyroscope_gzTime constant, T, of error model representing z-axis of gyroscope_axTime constant, T, representing an error model of the accelerometer x-axis_ayRepresenting time of accelerometer y-axis error modelConstant, T_azA time constant representing a z-axis error model of the accelerometer;

G_I(t)＝diag[1 1 1......1 1]_15×15；

W_I(t) is a 15-dimensional vector, as follows:

W_I(t)＝[a₁a₂a₃a₄a₅a₆a₇a₈a₉a₁₀a₁₁a₁₂a₁₃a₁₄a₁₅]，

a₁a₂a₃a₄a₅a₆a₇a₈a₉a₁₀a₁₁a₁₂a₁₃a₁₄a₁₅representing a systematic process noise sequence;

z (t) is the difference value of the position and the velocity information of the carrier in the INS inertial navigation system and the position and the velocity information of the carrier in the GPS navigation system, is a 6-dimensional vector,

Z(t)＝[v_x+N_vxv_y+N_vyv_z+N_vz(R_M+h)L+N_y(R_M+h)cosLλ+N_xh+N_h]^Twherein N is_vxIndicating the speed error of the GPS navigation system in the x direction, N_vyIndicating the speed error of the GPS navigation system in the y-direction, N_vzRepresenting the speed error of the GPS navigation system in the z direction, N_xIndicating the position error of the GPS navigation system in the x direction, N_yIndicating the position error of the GPS navigation system in the y-direction, N_hIndicating a position error of the GPS navigation system in the z direction;

wherein

V_v(t)＝[N_vxN_vyN_vz]^T

V_p(t)＝[N_xN_yN_z]^T

G. Using the above obtained continuous equation of stateDiscretizing to obtain X_k＝Φ_k,k-1X_k-1+W_k-1Wherein

The continuous observation equation z (t) ═ h (t) X obtained above_IDiscretizing (t) + V (t) to obtain Z_k＝H_kX_k+V_k；

Wherein I is an identity matrix, F is a state transition matrix of the INS inertial navigation system, and delta t is sampling time of the discretized INS inertial navigation system;

H. subtracting the position and speed information of the carrier in the INS inertial navigation system from the position and speed information of the carrier in the GPS navigation system to obtain observation information z of Z (t) at the moment k;

I. calculating the optimal estimated value of the state of the INS inertial navigation system at the k moment Wherein, for an optimal estimate of the state of the INS inertial navigation system at time k-1,Q_k-1is the noise matrix of the INS inertial navigation system, the size of which is determined by the performance of the INS inertial navigation elements, R_kis a variance matrix of system measurement noise, the size of which is determined by the performance of the GPS receiver;

J. will be calculatedThe value is differed with the position and speed information of the carrier in the INS inertial navigation system to obtain the optimal navigation parameter;

K. and repeating the steps H-J to obtain the navigation information parameters of the continuous carrier.

The invention has the beneficial effects that: the navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer utilizes the method of INS inertial navigation and GPS navigation combined navigation to solve the defects that a single GPS navigation technology is easy to interfere and shield, the short-time positioning precision is not high, the output frequency is limited and the output is discontinuous; meanwhile, the defects that the accumulated error of a single INS inertial navigation parameter is larger and the long-time positioning accuracy is dispersed are solved, and the defect that the accumulated error of the INS inertial navigation is dispersed along with the increase of time is made up by utilizing the advantage that the GPS navigation has high positioning accuracy for a long time; the method overcomes the defects that the GPS is easy to interfere and the output frequency is limited by utilizing the characteristics that the INS inertial navigation is not interfered by the outside and the output navigation information is continuous, and corrects the navigation angle calculated by the magnetometer to obtain the geographical true north direction and the stable navigation angle in order to solve the problems that the true north cannot be found by the navigation angle calculated by the inertial navigation and the drift is large.

Drawings

FIG. 1 is a latitude error value calculated by a navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer according to the present invention;

FIG. 2 is a latitude error variance value calculated by the navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer according to the present invention;

FIG. 3 is a longitude error value calculated based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention;

FIG. 4 is a longitude error variance calculated based on the inertial navigation system of INS, the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention;

FIG. 5 is a calculated altitude error value based on the INS inertial navigation, the GPS navigation, and the magnetometer navigation algorithm of the present invention;

FIG. 6 is a calculated altitude error variance value based on the INS inertial navigation, the GPS navigation, and the navigation algorithm of the magnetometer, in accordance with the present invention;

FIG. 7 is an east velocity error value calculated by the navigation algorithm based on INS inertial navigation, GPS navigation, and magnetometer, in accordance with the present invention;

FIG. 8 is an east velocity error variance value calculated by the navigation algorithm based on INS inertial navigation, GPS navigation, and magnetometer, in accordance with the present invention;

FIG. 9 is a north velocity error value calculated based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention;

FIG. 10 is a calculated variance value of the north velocity error based on the INS inertial navigation, the GPS navigation, and the navigation algorithm of the magnetometer, in accordance with the present invention;

FIG. 11 is a calculated value of the sky-direction velocity error based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer;

FIG. 12 is a direction of the sky velocity error variance calculated based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention.

Detailed Description

The navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer comprises the following steps:

A. measuring carrier angular velocity parameters by using gyroscope of INS inertial navigation systemSubstituting into quaternion differential equation to obtain quaternion q₀，q₁，q₂，q₃(ii) a WhereinThe angular velocity information of three axes measured by a gyroscope under the coordinate system of the carrier;

the quaternion differential equation is:

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=0.5\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right];$

B. q solved in step A₀，q₁，q₂，q₃Substituting the following formula to obtain an attitude matrix

$C_n^b=\left[\begin{array}{ccc}{q}_0+q_1+q_2+q_3& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0-q_1+q_2-q_3& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0-q_1-q_2+q_3\end{array}\right]$

According to the followingRelation with direction cosine

Calculating the attitude angles theta, gamma and gamma of the INS inertial navigation module of the carrier,

C. Yaw angle measured by magnetometerReplacing the calculated yaw angle in the step B

D. Measuring an acceleration parameter f of an accelerometer of an INS inertial navigation system^bAnd B, solving to obtain an attitude matrixSubstituting the velocity information v of the carrier in the east, north and sky directions under an INS inertial navigation coordinate system to obtain velocity information v_Nv_Ev_UThe differential equation is:

${\stackrel{\·}{v}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n$

wherein v isⁿ＝[v_Nv_Ev_U]' respectivelyThe velocity in the middle east, north and sky directions of the INS inertial navigation coordinate system,is the angular velocity of the earth's rotation,is the angular velocity g of the carrier in each axial direction around the INS inertial navigation coordinate systemⁿIs the acceleration of gravity;

E. v calculated in step D_Nv_Ev_URespectively substituting the position information of the carrier in the INS inertial navigation system into the following formula to obtain the position information of the carrier in the INS inertial navigation system, wherein L is latitude, lambda is longitude, and h is height,

h＝h(0)+∫v_Udt, where L (0) represents the latitude value of the initial position of the carrier, λ (0) represents the longitude value of the initial position of the carrier, and h (0) represents the initial height of the carrier from the earth's surface. R_MRepresenting the radius of curvature, R, in the meridian of the earth_NRepresenting the radius of curvature over a circle of latitude;

F. establishing an equation of stateAnd observation equation Z (t) ═ H (t) X_I(t)+V(t)，

X_I(t) represents the error state of the INS inertial navigation system at time t, which is a 15-dimensional vector as follows:

v_x,v_y,v_zthe velocity error of the INS inertial navigation system along the east, north and sky directions; phi is a_x,φ_y,φ_zIs the attitude angle error of the carrier; l, lambda and h respectively represent latitude, longitude and altitude errors of the carrierA difference;_x,_y,_zrespectively representing the random drift of the gyroscope;respectively, random drift of the accelerometer, whereinIs a matrix of 15 × 15, wherein F_N(t) corresponds to v_x,v_y,v_z，φ_x,φ_y,φ_zThe non-zero elements of the INS inertial navigation system matrix of the 9 parameters L, λ, h are as follows:

$\begin{array}{cc}F(1,1)=\frac{v_y\tan L}{R_N+h}-\frac{v_z}{R_N+h}& F(1,2)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_x\tan L}{R_N+h}\end{array}$

F(1,5)＝-f_zF(1,6)＝f_y

$F(1,7)=(2{\mathrm{\ω}}_{ie}{v}_z\sin L+2{\mathrm{\ω}}_{ie}{v}_y\cos L+\frac{v_x{v}_y{\sec }^{2}L}{R_N+h})$

$\begin{array}{cc}F(2,1)=-(\frac{2v_x\tan L}{R_N+h}+2{\mathrm{\ω}}_{ie}\sin L)& F(2,2)=-\frac{v_z}{R_M+h}\end{array}$

F(2,4)＝f_zF(2,6)＝-f_x

$\begin{array}{cc}F(2,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_x{\sec }^{2}L}{R_N+h})v_x& F(3,1)=(2{\mathrm{\ω}}_{ie}\cos L+\frac{2v_x}{R_M+h})\end{array}$

F(3,4)＝-f_yF(3,5)＝f_xF(3,7)＝-2ω_iev_xsinL

$\begin{array}{ccc}F(3,9)=\frac{2g_0}{R_M}& F(4,2)=-\frac{1}{R_M+h}& F(4,5)={\mathrm{\ω}}_{ie}\sin L+\frac{v_x}{R_N+h}\tan L\end{array}$

$\begin{array}{cc}F(4,6)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_x}{R_N+h}& F(5,1)=\frac{1}{R_N+h}\end{array}$

F(5,7)＝-ω_iesinL

$\begin{array}{ccc}F(6,1)=\frac{tgL}{R_N+h}& F(6,4)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}& F(6,5)=\frac{v_y}{R_M+h}\end{array}$

$\begin{array}{ccc}F(6,7)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L& F(7,2)=\frac{1}{R_M+h}& F(8,1)=\frac{\sec L}{R_N+h}\end{array}$

F(9,3)＝1

F_S(t) is v_x,v_y,v_z，φ_x,φ_y,φ_zThe transformation matrix between the 9 parameters L, λ, h and the gyro and accelerometer drifts, whose dimensions are 9 × 6,

F_M(t) is_x,_y,_z，The INS inertial navigation system matrix, corresponding to the gyroscope and accelerometer drifts, is a diagonal matrix with dimensions 6 × 6, as follows:

F_M(t)＝diag[-1/T_gx-1/T_gy-1/T_gz-1/T_ax-1/T_ay-1/T_az](ii) a Wherein, T_gxTime constant, T, of error model representing x-axis of gyroscope_gyTime constant, T, of error model representing y-axis of gyroscope_gzTime constant, T, of error model representing z-axis of gyroscope_axTime constant, T, representing an error model of the accelerometer x-axis_ayTime constant, T, representing the accelerometer y-axis error model_azA time constant representing a z-axis error model of the accelerometer;

G_I(t)＝diag[1 1 1......1 1]_15×15；

W_I(t) is a 15-dimensional vector, as follows:

W_I(t)＝[a₁a₂a₃a₄a₅a₆a₇a₈a₉a₁₀a₁₁a₁₂a₁₃a₁₄a₁₅]，

a₁a₂a₃a₄a₅a₆a₇a₈a₉a₁₀a₁₁a₁₂a₁₃a₁₄a₁₅representing a systematic process noise sequence;

z (t) is the difference value of the position and the velocity information of the carrier in the INS inertial navigation system and the position and the velocity information of the carrier in the GPS navigation system, is a 6-dimensional vector,

Z(t)＝[v_x+N_vxv_y+N_vyv_z+N_vz(R_M+h)L+N_y(R_M+h)cosLλ+N_xh+N_h]^Twherein N is_vxIndicating the speed error of the GPS navigation system in the x direction, N_vyIndicating the speed error of the GPS navigation system in the y-direction, N_vzRepresenting the speed error of the GPS navigation system in the z direction, N_xIndicating the position error of the GPS navigation system in the x direction, N_yIndicating the position error of the GPS navigation system in the y-direction, N_hIndicating a position error of the GPS navigation system in the z direction;

wherein

V_v(t)＝[N_vxN_vyN_vz]^T

V_p(t)＝[N_xN_yN_z]^T

G. Using the above obtained continuous equation of stateDiscretizing to obtain X_k＝Φ_k,k-1X_k-1+W_k-1Wherein

The continuous observation equation z (t) ═ h (t) X obtained above_IDiscretizing (t) + V (t) to obtain Z_k＝H_kX_k+V_k；

Wherein I is an identity matrix, F is a state transition matrix of the INS inertial navigation system, and delta t is sampling time of the discretized INS inertial navigation system;

H. subtracting the position and speed information of the carrier in the INS inertial navigation system from the position and speed information of the carrier in the GPS navigation system to obtain observation information z of Z (t) at the moment k;

I. calculating the optimal estimated value of the state of the INS inertial navigation system at the k moment Wherein, for optimal estimation of INS inertial navigation system state at time k-1，Q_k-1Is the noise matrix of the INS inertial navigation system, the size of which is determined by the performance of the INS inertial navigation elements, R_kis a variance matrix of system measurement noise, the size of which is determined by the performance of the GPS receiver;

J. will be calculatedThe value is differed with the position and speed information of the carrier in the INS inertial navigation system to obtain the optimal navigation parameter;

K. and repeating the steps H-J to obtain the navigation information parameters of the continuous carrier.

The invention has the beneficial effects that: the navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer utilizes the method of INS inertial navigation and GPS navigation combined navigation to solve the defects that a single GPS navigation technology is easy to interfere and shield, the short-time positioning precision is not high, the output frequency is limited and the output is discontinuous; meanwhile, the defects that the accumulated error of a single INS inertial navigation parameter is larger and the long-time positioning accuracy is dispersed are solved, and the defect that the accumulated error of the INS inertial navigation is dispersed along with the increase of time is made up by utilizing the advantage that the GPS navigation has high positioning accuracy for a long time; the method overcomes the defects that the GPS is easy to interfere and the output frequency is limited by utilizing the characteristics that the INS inertial navigation is not interfered by the outside and the output navigation information is continuous, and corrects the navigation angle calculated by the magnetometer to obtain the geographical true north direction and the stable navigation angle in order to solve the problems that the true north cannot be found by the navigation angle calculated by the inertial navigation and the drift is large.

FIG. 1 is a latitude error value calculated by a navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer according to the present invention; FIG. 2 is a latitude error variance value calculated by the navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer according to the present invention; FIG. 3 is a longitude error value calculated based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention; FIG. 4 is a longitude error variance calculated based on the inertial navigation system of INS, the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention; FIG. 5 is a calculated altitude error value based on the INS inertial navigation, the GPS navigation, and the magnetometer navigation algorithm of the present invention; FIG. 6 is a calculated altitude error variance value based on the INS inertial navigation, the GPS navigation, and the navigation algorithm of the magnetometer, in accordance with the present invention; FIG. 7 is an east velocity error value calculated by the navigation algorithm based on INS inertial navigation, GPS navigation, and magnetometer, in accordance with the present invention; FIG. 8 is an east velocity error variance value calculated by the navigation algorithm based on INS inertial navigation, GPS navigation, and magnetometer, in accordance with the present invention; FIG. 9 is a north velocity error value calculated based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention; FIG. 10 is a calculated variance value of the north velocity error based on the INS inertial navigation, the GPS navigation, and the navigation algorithm of the magnetometer, in accordance with the present invention; FIG. 11 is a calculated value of the sky-direction velocity error based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer; FIG. 12 is a direction of the sky velocity error variance calculated based on the Inertial Navigation System (INS), the GPS navigation system, and the navigation algorithm of the magnetometer, in accordance with the present invention;

as can be seen from the test result graph, the error variances of the longitude, the latitude and the altitude obtained by the navigation algorithm based on the INS inertial navigation, the GPS navigation and the magnetometer can be quickly converged to smaller values; the navigation information such as position, speed and the like can be filtered and smoothed, large jump cannot be generated, and the stability of the algorithm is strong.

Claims (1)

1. The navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer is characterized by comprising the following steps:

A. measuring carrier angular velocity parameters by using gyroscope of INS inertial navigation systemSubstituting into quaternion differential equation to obtain quaternion q₀，q₁，q₂，q₃(ii) a WhereinThe angular velocity information of three axes measured by a gyroscope under the coordinate system of the carrier;

the quaternion differential equation is:

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=0.5\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbx}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right];$

B. q solved in step A₀，q₁，q₂，q₃Substituting the following formula to obtain an attitude matrix

$C_n^b=\left[\begin{array}{ccc}{q}_0+q_1+q_2+q_3& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0-q_1+q_2-q_3& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0-q_1-q_2+q_3\end{array}\right]$

According to the followingRelation with direction cosine

Calculating the attitude angles theta, gamma and gamma of the INS inertial navigation module of the carrier,

C. Yaw angle measured by magnetometerReplacing the calculated yaw angle in the step B

D. Measuring an acceleration parameter f of an accelerometer of an INS inertial navigation system^bAnd B, solving to obtain an attitude matrixSubstituting the velocity information v of the carrier in the east, north and sky directions under an INS inertial navigation coordinate system to obtain velocity information v_Nv_Ev_UThe differential equation is:

${\stackrel{\·}{v}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v^n+g^n$

wherein v isⁿ＝[v_Nv_Ev_U]' are the velocities in the east, north and sky directions of the INS inertial navigation coordinate system respectively,is the angular velocity of the earth's rotation,is the angular velocity g of the carrier in each axial direction around the INS inertial navigation coordinate systemⁿIs the acceleration of gravity;

E. v calculated in step D_Nv_Ev_URespectively substituting the position information of the carrier in the INS inertial navigation system into the following formula to obtain the position information of the carrier in the INS inertial navigation system, wherein L is latitude, lambda is longitude, and h is height,

h＝h(0)+∫v_Udt_，wherein L (0) represents the latitude value of the initial position of the carrier, λ (0) represents the longitude value of the initial position of the carrier, and h (0) represents the initial height of the carrier from the earth's surface. R_MRepresenting the radius of curvature, R, in the meridian of the earth_NRepresenting the radius of curvature over a circle of latitude;

F. establishing an equation of stateAnd observation equation Z (t) ═ H (t) X_I(t)+V(t)，

X_I(t) shows the error state of the INS inertial navigation system at time tThe state, which is a 15-dimensional vector, is as follows:

v_x,v_y,v_zthe velocity error of the INS inertial navigation system along the east, north and sky directions; phi is a_x,φ_y,φ_zIs the attitude angle error of the carrier; l, lambda and h respectively represent latitude, longitude and altitude errors of the carrier;_x,_y,_zrespectively representing the random drift of the gyroscope;respectively, random drift of the accelerometer, whereinIs a matrix of 15 × 15, wherein F_N(t) corresponds to v_x,v_y,v_z，φ_x,φ_y,φ_z，L,_λH INS inertial navigation system matrix of these 9 parameters, whose non-zero elements are as follows:

$\begin{array}{cc}F(1,1)=\frac{v_y\tan L}{R_N+h}-\frac{v_z}{R_N+h}& F(1,2)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_x\tan L}{R_N+h}\end{array}$

$\begin{array}{ccc}F(1,3)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h})& F(1,5)=-f_z& F(1,6)=f_y\end{array}$

$F\left(\mathrm{1,7}\right)=({2\mathrm{\ω}}_{\mathrm{ie}}{v}_z\sin L+{2\mathrm{\ω}}_{\mathrm{ie}}{v}_y\cos L+\frac{v_x{v}_y{\sec }^{2}L}{R_N+h})$

$\begin{array}{cc}F(2,1)=-(\frac{2v_x\tan L}{R_N+h}+2{\mathrm{\ω}}_{ie}\sin L)& F(2,2)=-\frac{v_z}{R_M+h}\end{array}$

$\begin{array}{ccc}F(2,3)=-\frac{v_y}{R_M+h}& F(2,4)=f_z& F(2,6)=-f_x\end{array}$

$\begin{array}{cc}F(2,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_x{\sec }^{2}L}{R_N+h})v_x& F(3,1)=(2{\mathrm{\ω}}_{ie}\cos L+\frac{2v_x}{R_M+h})\end{array}$

F(3,4)＝-f_yF(3,5)＝f_xF(3,7)＝-2ω_iev_xsinL

$\begin{array}{ccc}F(3,9)=\frac{2g_0}{R_M}& F(4,2)=-\frac{1}{R_M+h}& F(4,5)={\mathrm{\ω}}_{ie}\sin L+\frac{v_x}{R_N+h}\tan L\end{array}$

$\begin{array}{cc}F(4,6)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_x}{R_N+h}& F(5,1)=\frac{1}{R_N+h}\end{array}$

$\begin{array}{ccc}F(5,4)=-{\mathrm{\ω}}_{ie}\sin L-\frac{v_x}{R_N+h}\tan L& F(5,6)=-\frac{v_y}{R_M+h}& F(5,7)=-{\mathrm{\ω}}_{ie}\sin L\end{array}$

$\begin{array}{ccc}F(6,1)=\frac{tgL}{R_N+h}& F(6,4)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}& F(6,5)=\frac{v_y}{R_M+h}\end{array}$

$\begin{array}{ccc}F(6,7)={\mathrm{\ω}}_{ie}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L& F(7,2)=\frac{1}{R_M+h}& F(8,1)=\frac{\sec L}{R_N+h}\end{array}$

$\begin{array}{cc}F(8,7)=\frac{v_x}{R_N+h}\sec L\tan L& F(9,3)=1\end{array}$

F_S(t) is v_x,v_y,v_z，φ_x,φ_y,φ_zThe transformation matrix between the 9 parameters L, λ, h and the gyro and accelerometer drifts, whose dimensions are 9 × 6,

F_M(t) is_x,_y,_z，The INS inertial navigation system matrix, corresponding to the gyroscope and accelerometer drifts, is a diagonal matrix with dimensions 6 × 6, as follows:

F_M(t)＝diag[-1/T_gx-1/T_gy-1/T_gz-1/T_ax-1/T_ay-1/T_az](ii) a Wherein, T_gxTime constant, T, of error model representing x-axis of gyroscope_gyTime constant, T, of error model representing y-axis of gyroscope_gzTime constant, T, of error model representing z-axis of gyroscope_axTime constant, T, representing an error model of the accelerometer x-axis_ayRepresenting an accelerometer yTime constant of axis error model, T_azA time constant representing a z-axis error model of the accelerometer;

G_I(t)＝diag[1 1 1......1 1]_15×15；

W_I(t) is a 15-dimensional vector, as follows:

W_I(t)＝[a₁a₂a₃a₄a₅a₆a₇a₈a₉a₁₀a₁₁a₁₂a₁₃a₁₄a₁₅]，

a₁a₂a₃a₄a₅a₆a₇a₈a₉a₁₀a₁₁a₁₂a₁₃a₁₄a₁₅representing a systematic process noise sequence;

z (t) is the difference value of the position and the velocity information of the carrier in the INS inertial navigation system and the position and the velocity information of the carrier in the GPS navigation system, is a 6-dimensional vector,

Z(t)＝[v_x+N_vxv_y+N_vyv_z+N_vz(R_M+h)L+N_y(R_M+h)cosLλ+N_xh+N_h]^Twherein N is_vxIndicating the speed error of the GPS navigation system in the x direction, N_vyIndicating the speed error of the GPS navigation system in the y-direction, N_vzRepresenting the speed error of the GPS navigation system in the z direction, N_xIndicating the position error of the GPS navigation system in the x direction, N_yIndicating the position error of the GPS navigation system in the y-direction, N_hIndicating a position error of the GPS navigation system in the z direction;

wherein

V_v(t)＝[N_vxN_vyN_vz]^T

V_p(t)＝[N_xN_yN_z]^T

G. Using the above obtained continuous equation of stateDiscretizing to obtain X_k＝Φ_k,k-1X_k-1+W_k-1Wherein

The continuous observation equation z (t) ═ h (t) X obtained above_IDiscretizing (t) + V (t) to obtain Z_k＝H_kX_k+V_k；

Wherein I is an identity matrix, F is a state transition matrix of the INS inertial navigation system, and delta t is sampling time of the discretized INS inertial navigation system;

H. subtracting the position and speed information of the carrier in the INS inertial navigation system from the position and speed information of the carrier in the GPS navigation system to obtain observation information z of Z (t) at the moment k;

I. calculating the optimal estimated value of the state of the INS inertial navigation system at the k moment Wherein, for an optimal estimate of the state of the INS inertial navigation system at time k-1,Q_k-1is the noise matrix of the INS inertial navigation system, the size of which is determined by the performance of the INS inertial navigation elements, R_kis a variance matrix of system measurement noise, the size of which is determined by the performance of the GPS receiver;

J. will be calculatedThe value is differed with the position and speed information of the carrier in the INS inertial navigation system to obtain the optimal navigation parameter;

K. and repeating the steps H-J to obtain the navigation information parameters of the continuous carrier.