Attitude estimation method in magnetic interference environment

Abstract

The invention relates to the technical field of attitude estimation, in particular to an attitude estimation method in a magnetic interference environment, which comprises the following steps of: obtaining a conversion relation between an Euler angle and a quaternion; calculating the quaternion at the next sampling moment; respectively solving a first state transition matrix, a first process noise matrix and a first prior covariance according to the first state equation; respectively solving a second state transition matrix, a second process noise matrix and a second prior covariance according to a second state equation; elements in the nb-series rotation matrix are used as observed quantities of state variables in the parameter-series rotation matrix, a corresponding Kalman gain is calculated, and a Jacobian matrix is designed by using the observed quantities; calculating an observation noise covariance matrix by using the Jacobian matrix; updating the Kalman gain by using the observation noise covariance matrix; and updating the state equation and the posterior covariance by using the updated Kalman gain and the observed quantity. The method solves the problem of how to prevent the calculation of the roll angle and the pitch angle from being influenced by magnetic interference when the magnetic interference exists.

Description

Technical Field

[0001] This invention relates to the field of attitude estimation technology, and in particular to an attitude estimation method under magnetic interference environment. Background Technology

[0002] MIMUS consists of a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer, and can be used for attitude estimation. Gyroscope integration can yield short-term, high-precision attitude data; however, gyroscopes suffer from integration drift, causing attitude errors to accumulate over time. Simultaneously, attitude can also be calculated using the accelerometer and magnetometer, but this relies on the accuracy of the sensor measurements; significant interference can render the calculated attitude unreliable.

[0003] During the movement of the carrier, if there is external magnetic field interference in the current environment, making the current magnetic field strength much greater than the Earth's magnetic field strength, the magnetometer's measurement value will lose its reference value. This will not only affect the accuracy of the heading angle calculation, but also reduce the accuracy of the roll and pitch angle calculations. How to solve this problem needs further research.

[0004] To address the issue that magnetic interference in the environment reduces the accuracy of roll and pitch angle calculations.

[0005] In existing technologies, a new vector is constructed by cross-product of the gravity vector and the geomagnetic vector, and this vector is used to replace the geomagnetic vector for observation. However, although this method can reduce the impact of magnetic interference on the roll and pitch angles, it is not completely immune to magnetic interference. There is also an error Kalman filter algorithm, but the main problem with this method is that the calculated attitude error represents three rotations, and each rotation will affect the overall attitude. Therefore, it cannot guarantee that the calculation of roll and pitch angles is completely immune to magnetic interference. Another example is the four-parameter complementary filter, which is a hybrid attitude representation method. However, the main problem with this method is that the differential equation representing the heading angle parameter is nonlinear. Solving this differential equation through a first-order approximation leads to a significant increase in attitude error during rotational motion. Summary of the Invention

[0006] To address the shortcomings of existing methods, this invention primarily solves the problem of how to ensure that the calculation of roll and pitch angles is unaffected by magnetic interference, while maintaining decoupling of roll and pitch angle calculations from magnetic interference, and obtaining high-precision attitude.

[0007] The technical solution adopted in this invention is: an attitude estimation method under magnetic interference environment, comprising the following steps: Step 1: Obtain the conversion relationship between Euler angles and quaternions using the rotation matrix and Euler angle matrix of the reference frame; calculate the quaternion at the next sampling time based on the measurement data of the gyroscope and the quaternion at a certain sampling time. In a preferred embodiment of the present invention, the rotation sequence of the Euler angles of the reference system includes: ZYX axis or ZXY axis.

[0008] Step 2: Construct the first state equation based on the first to third state variables of the rotation matrix of the parameter system, and solve the first state transition matrix, the first process noise matrix, and the first prior covariance according to the first state equation; construct the second state equation based on the fourth to fifth state variables of the rotation matrix of the parameter system, and solve the second state transition matrix, the second process noise matrix, and the second prior covariance according to the second state equation. As a preferred embodiment of the present invention, the first prior covariance The formula is: (10) in, A 1 is the first state transition matrix, and Q1 is the first process noise matrix.

[0009] As a preferred embodiment of the present invention, the second prior covariance The formula is: (14) in, A 2 is the second state transition matrix, and Q2 is the second process noise matrix.

[0010] Step 3, through mb System rotation matrix and nm The rotation matrix is ​​obtained nb System rotation matrix; with nb The elements in the rotation matrix are used as observations of the state variables in the rotation matrix of the reference system. The corresponding Kalman gain is calculated using the observations. The Jacobian matrix is ​​designed using the observations. The observation noise covariance matrix is ​​calculated using the Jacobian matrix. The Kalman gain is updated using the observation noise covariance matrix. The state variables and posterior covariance are updated using the updated Kalman gain and the observations. In a preferred embodiment of the present invention, step three specifically includes: Step 31 nb System rotation matrix , for nm System rotation matrix, for mb System rotation matrix; Step 32: Construct the observation matrix for the first state variable S1, the second state variable S2, and the third state variable S3.Z a Construct the observation matrices for the fourth state variable S4 and the fifth state variable S5. Z b ; , , ;in, a x , a y , a z These are the measurements taken by the accelerometer along the X, Y, and Z axes, respectively. m x , m y , m z These are the measured values ​​of the magnetometer along the X, Y, and Z axes, respectively. Step 33: Construct the Kalman gains for S1 to S3 K Kalman gain of S1 and S4 to S5 K 2; Step 34, using Z b The first element Z b1 and the second element Z b2 Constructing the Jacobian matrix J 1; Step 35: Calculate the observation noise covariance matrix R 2;Use R 2. Observation noise covariance matrix R 1. Update the Kalman gain of all state variables; As a preferred embodiment of the present invention, the observation noise covariance matrix R The formula for 2 is: (twenty one) in, σ mx , σ my , σ mz The following are the noise standard deviations for the X, Y, and Z axes of the magnetometer, respectively.

[0011] In a preferred embodiment of the present invention, the Kalman gain formula for all state variables is: (19) in, H 1 and H Both are identity matrices; ; σ ax , σay , σ az The following are the noise standard deviations for the X, Y, and Z axes of the accelerometer, respectively.

[0012] Step 36: Update the posterior covariance using the Kalman gains of all updated state variables.

[0013] In a preferred embodiment of the present invention, the formula for the updated empirical covariance is: (twenty three) in, , For the prior covariance of the state variables; , It is an identity matrix.

[0014] As a preferred embodiment of the present invention, it further includes: when the absolute value of the pitch angle is greater than 80°, constructing a first state equation based on the first to third state variables of the rotation matrix of the parameter system, and solving the first state transition matrix, the first process noise matrix and the first prior covariance according to the first state equation; constructing a third state equation based on the sixth to seventh state variables of the rotation matrix of the parameter system, and solving the third state transition matrix, the third process noise matrix and the third prior covariance according to the third state equation.

[0015] As a preferred embodiment of the present invention, the attitude estimation system under magnetic interference environment includes: a memory for storing instructions executable by a processor; and a processor for executing the instructions to implement the attitude estimation method under magnetic interference environment.

[0016] The beneficial effects of this invention are: 1. In this invention, the state variables related to roll angle and pitch angle are calculated using only gyroscopes and accelerometers, fundamentally separating the influence of magnetometers on roll angle and pitch angle; 2. The method of this invention can be combined with current anti-interference algorithms, such as the threshold method; 3. This invention is simple to implement and is suitable for real-time systems; 4. The roll and pitch angles calculated by the attitude estimation algorithm of this invention are completely immune to magnetic interference. Attached Figure Description

[0017] Figure 1 This is a flowchart of the attitude estimation method under magnetic interference environment of the present invention; Figure 2 This is a comparison chart of the roll angles of the four filters; Figure 3 This is a comparison chart of the pitch angles of the four filters; Figure 4This is a comparison chart of the average error of the heading angle of the four filters; Figure 5 for and Differences over time Figure 1 ; Figure 6 for and Differences over time Figure 2 ; Figure 7 for and Differences over time Figure 3 . Detailed Implementation

[0018] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0019] like Figure 1 As shown, an attitude estimation method under magnetic interference environment includes the following steps: Step 1: Construct the rotation matrix and Euler angle matrix of the reference system to obtain the conversion relationship between Euler angles and quaternions; calculate the quaternion at the next sampling time based on the gyroscope and the quaternion at a certain sampling time. Euler angles include either the ZYX axis or the ZXY axis.

[0020] The rotation matrix from the reference frame to the loading frame can be represented by a quaternion (parameter-loading frame rotation matrix) as follows: (1) in, q 0 is the real part of the quaternion; q 1, q 2, q 3 represents the imaginary part of the quaternion; Let be the rotation matrix of the reference system.

[0021] If a vector is represented in the reference coordinate system as V n In the carrier system, it is represented as V b Therefore, the relationship between the two can be described using a rotation matrix: .

[0022] Will Let it be denoted as the fifth state variable S5. Let it be denoted as the fourth state variable S4. Let it be denoted as the first state variable S1. Let them be the second state variables S2 and Let S3 be the third state variable; S1 to S5 are used as the state variables for the Kalman filter.

[0023] The rotation matrix from the reference frame to the load frame can be expressed as an Euler angle matrix in the ZYX rotation order (Euler angles of the ZYX axis of the load frame): (2) in, R x, R y, R z represents the rotation matrices about the x, y, and z axes, respectively; , , These represent the heading angle, pitch angle, and roll angle, respectively. c and s It is an abbreviation for the trigonometric functions cos and sin; that is... This is represented by rotating the heading angle around the z-axis, the pitch angle around the y-axis, and the roll angle around the x-axis in sequence.

[0024] Preferably, the rotation matrix from the reference frame to the load system can also be expressed as the ZXY rotation order Euler angle matrix (Euler angles of the ZXY axis of the load system): (3) The conversion relationship between Euler angles and quaternions under the ZYX rotation order (Euler angles and quaternions along the ZYX axis) is as follows:

[0025] (4) Preferably, under the ZXY rotation order, the conversion relationship between Euler angles and quaternions (Euler angles and quaternions along the ZXY axis) is as follows:

[0026] (5) Based on the measurement data and sampling time of the gyroscope k Quaternion calculation sampling time k Quaternions with +1, formula: (6) in, ω x , ω y , ω z These are the measurements from the gyroscope. This refers to the sampling interval for sensor data.

[0027] Step 2: Construct the first state equation based on the first to third state variables of the rotation matrix of the parameter system, and solve the first state transition matrix, the first process noise matrix, and the first prior covariance according to the first state equation; construct the second state equation based on the fourth to fifth state variables of the rotation matrix of the parameter system, and solve the second state transition matrix, the second process noise matrix, and the second prior covariance according to the second state equation. The formula for the first state equation: (7) Solve the first state transition matrix using the chain rule. A 1 is: (8) The noise matrix Q1 for the first process is: (9) in, ; R g = diag[σ ωx 2 ,σ ωy 2 , σ ωz 2 ], σ ωx , σ ωy , σ ωz These are the noise standard deviations for the X, Y, and Z axes of the gyroscope, respectively.

[0028] First prior covariance The formula is: (10) in, T For transpose, For the first k The first posterior covariance at time 1.

[0029] The formula for the second state equation: (11) The second state transition matrix A2 is obtained by using the chain rule: (12) The noise matrix Q2 for the second process is: (13) in, .

[0030] Second prior covariance The formula is: (14) in, T This is a transpose.

[0031] Step 3, through mb System rotation matrix and nm The rotation matrix is ​​obtained nb System rotation matrix; with nb The elements in the rotation matrix are used as observations of the state variables in the rotation matrix of the reference system. The corresponding Kalman gain is calculated using the observations. The Jacobian matrix is ​​designed using the observations. The observation noise covariance matrix is ​​calculated using the Jacobian matrix. The Kalman gain is updated using the observation noise covariance matrix. The state variables and posterior covariance are updated using the updated Kalman gain and the observations. Build m Temporary coordinate system n System and m The system has only one heading angle deviation (i.e. n The system rotates by a heading angle to obtain m (System); Based on the rotation sequence of ZYX, the representation is derived from the accelerometer measurements. m Tie b Rotation matrix of the system; in, n The coordinate system is the navigation (reference) coordinate system, namely the northeast-northeast coordinate system. n The coordinate system is a fixed coordinate system; b The system is the carrier coordinate system; m Tie b Tie( mb The formula for the rotation matrix of the system is: (15) in, a x , a y , a z These are the measurements taken by the accelerometer along the X, Y, and Z axes, respectively.

[0032] Derived from the magnetometer's measurements n Tie m Tie( nm The rotation matrix of the system is: (16) in, ; m x , m y , m z These are the measured values ​​of the magnetometer along the X, Y, and Z axes, respectively.

[0033] The values ​​from the accelerometer and magnetometer are obtained by matrix multiplication. n Tie b Tie( nb The rotation matrix of the system is: (17) For the observation Z of state variables S1 to S3 a The last column in equation (17) is taken as the observation; for the observations Z from S4 to S5 b Using the first row and first column and the first row and second column of equation (17) as the observations, they are respectively: (18) Kalman gain for S1 to S3 K 1 and Kalman gain for S4 to S5 K 2 are respectively: (19) in, H 1 and H Both are identity matrices; ; σ ax , σ ay , σ az The noise standard deviations for the accelerometer's X, Y, and Z axes are shown in order. R 1 and R 2 represents the observation noise covariance matrix.

[0034] Z b It is a nonlinear combination of accelerometer and magnetometer measurements, and the uncertainty can propagate through the Jacobian matrix; Z b The first and second elements are defined as Z. b1 and Z b2 Jacobian matrix J Formula 1 is: (20) get R 2 is: (twenty one) in, σ mx , σ my , σ mz The following are the noise standard deviations for the X, Y, and Z axes of the magnetometer, respectively.

[0035] The state equations from S1 to S5 are updated as follows: (twenty two) Posterior covariances of S1 to S5 and Updated to: (twenty three) The conversion relationship between Euler angles and quaternions using the ZYX rotation order is calculated by equation (4).

[0036] Preferably, when the pitch angle is close to 90 degrees, in order to avoid singularity, the rotation order needs to be changed; the rotation order is switched from ZYX to ZXY; the two state variables S4 and S5 will no longer be used, and the elements of the second row and first column and the second row and second column of formula (1) will be used as the sixth state variable S6 and the seventh state variable S7; the second state equation is constructed based on S6 and S7. The third state equation is: (twenty four) Solve the third state transition matrix using the chain rule. A 3 is: (25) Third process noise matrix Q 3 is: (26) in, .

[0037] Third prior covariance The formula is: (27) Following the ZXY rotation sequence, the values ​​measured by the accelerometer are used to derive the representation from... m Tie b The rotation matrix of the system is: (28) Derived from the output of the magnetometer n Tie m The rotation matrix of the system is: (29) in, .

[0038] The values ​​from the accelerometer and magnetometer are obtained by matrix multiplication. n Tie b The rotation matrix of the system is: (30) The elements in the second row, first column and the second row, second column of equation (30) are taken as the observations of S6 and S7, and denoted as Z. c Then Z c for: (31) For S6 and S7, the Kalman gain K 3 is: (32) H3 is the identity matrix.

[0039] Z c It is a nonlinear combination of accelerometer and magnetometer measurements, and its uncertainty can propagate through the Jacobian matrix. Z... c The first and second elements are defined as Z. c1 and Z c2 Jacobian matrix J 2 is: (33) so R 3 is: (34) S6 and S7 have been updated to: (35) Posterior covariance Updated to: (36) Euler angles and quaternions are calculated using equation (5).

[0040] Finally, it's important to note that the state variables for calculating the heading angle should be switched depending on whether the pitch angle is close to 90 degrees, and the gain matrix should also be adjusted accordingly. K 2 and K 3 also needs to be switched, but the discarded gain will continue to be updated.

[0041] The algorithm flow is as follows:

[0042] Experimental procedure: To verify the performance of this algorithm, Monte Carlo simulation was used to test it; gyroscope data was simulated using MATLAB. (37) in, a 1, a 2, a 3 represents the amplitude of the gyroscope's x, y, and z axis output signals, respectively. b 1,b 2, b 3 represents the motion frequencies of the gyroscope's x, y, and z axes, respectively.

[0043] The data generated from the inertial sensor and magnetometer are as follows: (38) Gravitational acceleration, geomagnetic field strength, magnetic inclination, sampling frequency, gyroscope noise standard deviation, accelerometer noise standard deviation, and magnetometer noise standard deviation are respectively set as follows: ||g||= 9.8 m / s 2 , ||m||= 0.5 G , β =53 deg, T =0 . 01s (39) σ ω =0 . 0.06 rad / s, σ a =0 . 008m / s 2 , σ m =0 . 001G. The experiment was repeated one thousand times, and in each run, t Increase from 0 to 60 in increments of 0.01; a i and b i (i=1, 2, 3) are randomly generated from uniform distributions of [0, 5] and [0, 0.5], respectively; (i= ω , a , m The values ​​are randomly generated from a uniform distribution in the range [0, 0.001]. a a and a m Randomly generated from uniform distributions of [0, 0.5] and [0.1, 0.25], respectively; b a and b mRandomly generated from a uniform distribution in the range [0, 0.5]; rotational motion, magnetic interference, and linear acceleration interference were introduced at sampling intervals of [500, 6000], [1500, 3500], and [3000, 4500], respectively. The algorithm was run twice in each iteration. In the first run, measurements from all sensors were used for attitude calculation; in the second run, only the gyroscope and accelerometer were used, excluding the magnetometer. This experimental procedure was also applied to three filters, EKF, EEKF, and FCF, for comparison.

[0044] Figure 2 , Figure 3 , Figure 4 The average error (AAE) of the roll angle, pitch angle, and yaw angle of these four filters is shown respectively. AAE is defined as: (40) in, = , , ; and They were respectively in the second j In the repeated run, the first k The estimated attitude and the true attitude at each moment; if the magnetometer is not used for attitude estimation Then replace with ; Figure 5 , Figure 6 , Figure 7 They were shown respectively and Differences that change over time.

[0045] Figure 4 This indicates that the FCF performs poorly in terms of heading angle during rotational motion; from Figure 5 and Figure 6 It can be seen that DOEA is not completely immune to magnetic interference when it is present; when linear acceleration interference is present, magnetic interference will still affect the roll and pitch angles of EEKF, while the proposed algorithm is completely immune to magnetic interference.

[0046] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

Claims

1. A method for pose estimation in a magnetic interference environment, the method comprising: The method comprises the following steps: Step one, obtaining the conversion relationship between Euler angles and quaternions by using the body-fixed coordinate system rotation matrix and the body-fixed coordinate system Euler angle matrix; calculating the quaternion at the next sampling time according to the measurement data of the gyroscope and the quaternion at a certain sampling time; Step two, constructing a first state equation based on the first to third state variables of the body-fixed coordinate system rotation matrix, and respectively solving a first state transition matrix, a first process noise matrix and a first prior covariance according to the first state equation; Constructing a second state equation based on the fourth to fifth state variables of the body-fixed coordinate system rotation matrix, and respectively solving a second state transition matrix, a second process noise matrix and a second prior covariance according to the second state equation; Step three, by mb the rotation matrix and nm the rotation matrix nb the rotation matrix; In nb The elements in the rotation matrix are taken as the observation of the state variables in the rotation matrix, the corresponding Kalman gain is calculated by using the observation, and the Jacobian matrix is designed by using the observation. Calculating the observation noise covariance matrix by using the Jacobian matrix; Updating the Kalman gain by using the observation noise covariance matrix; updating the state variable and the posterior covariance by using the updated Kalman gain and the observation.

2. The method of claim 1, wherein, first prior covariance The formula is: (10) wherein A 1 is a first state transition matrix and Q1 is a first process noise matrix.

3. The method of claim 1, wherein, Second prior covariance The formula is: (14) wherein A 2 is a second state transition matrix, Q2 is a second process noise matrix.

4. The method of claim 1, wherein, Step three specifically comprises: Step 31 nb is a rotation matrix , is nm is a rotation matrix, is mb is a rotation matrix; Step 32, constructing the observation matrix of the first state variable S1, the second state variable S2, the third state variable S3 Z a ; constructing the observation matrix of the fourth state variable S4, the fifth state variable S5 Z b ; , , ; wherein, a x , a y , a z are the measurements of the accelerometer X, Y, Z axes, m x , m y , m z are the measurements of the magnetometer X, Y, Z axes; Step 33, construct Kalman gain of S1 to S3 K 1 and Kalman gain of S4 to S5 K 2; Step 34, with Z b the first element Z b1 and the second element Z b2 constructing the Jacobian matrix J 1 ; Step 35, compute the observation noise covariance matrix R 2; utilize R 2 and the observation noise covariance matrix R 1 update the Kalman gain for all state variables; Step 36, updating the posterior covariance by using the Kalman gain of all updated state variables.

5. The method of pose estimation in a magnetic interference environment of claim 4, wherein, Observation noise covariance matrix R The formula for 2 is: (21) wherein σ mx , σ my , σ mz Noise standard deviation of magnetometer X, Y, Z axis, in sequence.

6. The method of claim 5, wherein, The formula of the Kalman gain of all state variables is: （19） wherein H 1 and H 2 are both identity matrices; .

7. The method of pose estimation in a magnetic interference environment of claim 6, wherein, The formula of the updated posterior covariance is: （23） wherein, , is the prior covariance of the state variable.

8. The method of claim 1, wherein, Further comprising: When the absolute value of the pitch angle is greater than 80°, constructing a first state equation based on the first to third state variables of the body-fixed coordinate system rotation matrix, and respectively solving a first state transition matrix, a first process noise matrix and a first prior covariance according to the first state equation; Constructing a third state equation based on the sixth to seventh state variables of the body-fixed coordinate system rotation matrix, and respectively solving a third state transition matrix, a third process noise matrix and a third prior covariance according to the third state equation.

9. The method of claim 1, wherein, The rotation order of the body-fixed coordinate system Euler angle comprises ZYX axis or ZXY axis.

10. A system for attitude estimation in a magnetic interference environment, characterized by It comprises: A memory for storing instructions executable by a processor; A processor for executing instructions to implement the attitude estimation method in a magnetic interference environment as claimed in any one of claims 1-8.

Images