An IMU Adaptive Attitude Estimation Method Based on Kalman Filter

Abstract

A Kalman filter-based IMU adaptive attitude estimation method relates to the field of robot motion tracking, navigation and positioning technology. Accelerometer and gyroscope measurement data are modeled, the IMU attitude is predicted using the angular velocity measurement of the gyroscope, the state to be estimated is extracted according to the relationship between the attitude and the attitude angle, and the covariance matrix of the state prior estimation is obtained, the acceleration measurement of the accelerometer is used to build the attitude observation model, the acceleration fine-tuning factor is defined to compensate the acceleration of each axis, the Kalman filter gain is calculated according to the acceleration fine-tuning factor to update the mean and covariance matrix of the state estimation, the final estimated state and the covariance matrix of the state posterior estimation are obtained, and then the attitude angle of the IMU is solved. The adaptive adjustment of the filter gain and the dynamic compensation of the external acceleration are realized, which can effectively improve the robustness and accuracy of the IMU attitude estimation.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion tracking, navigation and positioning, and specifically to an IMU adaptive attitude estimation method based on a Kalman filter. Background Art

[0002] The problem of attitude estimation of an Inertial Measurement Unit (IMU) has always been an important research direction in the fields of robot navigation and positioning, motion tracking, etc. A robot obtains its own motion information through the accelerometer and gyroscope of the IMU, and infers its own attitude based on this. Traditional attitude estimation methods usually use gyroscope integration to predict the attitude of the IMU within the framework of a Kalman filter, and correct the prediction result by combining the absolute attitude measurement provided by the accelerometer. In order to use the accelerometer to provide an absolute observation of the attitude of the IMU, it is necessary to estimate the actual external acceleration of the IMU, which is usually modeled as a first-order low-pass filtering process, but the influence of the actual external acceleration noise is not considered. Therefore, these methods only have relatively good attitude estimation accuracy in some low-dynamic scenarios and cannot be used in high-dynamic motion situations because the gain of the Kalman filter cannot well correct the attitude prediction value, which seriously reduces the accuracy of IMU attitude estimation. Summary of the Invention

[0003] To solve the deficiencies in the background art, the present invention provides an IMU adaptive attitude estimation method based on a Kalman filter, which realizes the adaptive adjustment of the filter gain and the dynamic compensation of the external acceleration, and can effectively improve the robustness and accuracy of IMU attitude estimation.

[0004] To achieve the above object, the present invention adopts the following technical solution: An IMU adaptive attitude estimation method based on a Kalman filter includes the following steps:

[0005] Step 1: Input the accelerometer measurement data a m and gyroscope measurement data ω m ;

[0006] Step 2: Model the accelerometer measurement data a m,k and gyroscope measurement data ω m,k at time k as follows:

[0007]

[0008] ω m,k = ω k + b g (2)

[0009] where a k-1Denote the external acceleration of the IMU at time k - 1, ω k Denote the angular velocity of the IMU at time k, ξ is a constant, ξ ∈ [0, 1], Denote the attitude of the IMU in the navigation coordinate system at time k, g is the local gravity acceleration vector, b a and b g Denote the biases of the accelerometer and gyroscope respectively, ε k Denote the external acceleration noise of the IMU at time k;

[0010] Step 3: Use the angular velocity measurement of the gyroscope to predict the attitude of the IMU, which is expressed as follows:

[0011]

[0012] In the formula, ω k-1 and Denote the angular velocity of the IMU and the attitude of the IMU in the navigation coordinate system at time k - 1 respectively, I 3 Denote the 3×3 identity matrix, Δt is the data sampling time interval of the IMU, [·] × Denote the cross product operation symbol;

[0013] Step 4: The attitude Consists of three attitude angles: roll angle ψ k , pitch angle θ k and yaw angle φ k , and the rotation order is z - y - x. The relationship between the attitude and the attitude angles is as follows:

[0014]

[0015] Take the third row element of the attitude As the state x to be estimated k :

[0016] x k = [-sinθ k , sinψ k cosθ k , cosψ k cosθ k T (5)

[0017] Transform formula (3) into:

[0018]

[0019] In the formula, Denote the prior estimate of the state x k at time k, ​The prior estimate of the state x at time k-1 k-1 According to formula (6), the covariance matrix of the state prior estimate is obtained as follows:

[0020]

[0021] where P k-1 is the covariance matrix of the state posterior estimate at time k-1, and Σ g is the covariance matrix of the gyroscope noise;

[0022] Step Five: Use the acceleration measurement of the accelerometer to construct an attitude observation model, and according to formula (1), we get:

[0023] a m,k -ξa k-1 =||g||x k +ε k +b a (8)

[0024] where ||g|| represents the magnitude of the local gravitational acceleration vector, and is defined as:

[0025]

[0026] where a k represents the external acceleration of the IMU at time k;

[0027] Step Six: When the system is fault-free, the actual error e of the Kalman filter e should be less than its theoretical error e t , that is, it satisfies the following formula:

[0028]

[0029] where Σ a is the covariance matrix of the accelerometer noise, and Σ ε is the external acceleration noise;

[0030] Step Seven: According to the analysis of the system fault cause, the acceleration fine-tuning factor λ is defined by formula (10) k as follows:

[0031]

[0032] Acceleration compensation is performed on each axis through the acceleration fine-tuning factor;

[0033] Step Eight: Ensure the non-negativity of the accelerometer fine-tuning factor, that is:

[0034] λ k (i,j)=max{0,λ k(i,j)}, where i = 1, 2, 3; j = 1, 2, 3 (12)

[0035] In the formula, λ k (i,j) represents the element value of the i-th row and j-th column of the acceleration fine-tuning factor;

[0036] Step Nine: Calculate the Kalman filter gain K according to the acceleration fine-tuning factor k :

[0037]

[0038] The acceleration fine-tuning factor enables the Kalman filter to dynamically adjust the filter gain;

[0039] Step Ten: Update the mean and covariance matrix of the state estimate according to the Kalman filter gain to obtain the finally estimated state x k and the covariance matrix P of the state posterior estimate k as follows:

[0040]

[0041]

[0042] After that, according to the finally estimated state x k Solve the attitude angle of the IMU as follows:

[0043]

[0044]

[0045] In the formula, x k (l), where l = 1, 2, 3 represents the l-th element value of the state x k of.

[0046] Compared with the prior art, the beneficial effects of the present invention are: The IMU attitude estimation method proposed by the present invention, when an external acceleration is detected, automatically adjusts the Kalman filter gain by estimating the additional covariance caused by the external acceleration, realizes the adaptive adjustment of the filter gain and the dynamic compensation for the external acceleration, and can effectively improve the robustness and accuracy of the IMU attitude estimation. Description of the Drawings

[0047] Figure 1 is the technical roadmap of the present invention;

[0048] Figure 2 is the IMU attitude estimation result diagram of the embodiment. Detailed Embodiment

[0049] Next, the technical solutions in the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0050] As Figures 1 to 2 shown, an IMU adaptive attitude estimation method based on a Kalman filter, the technical route is combined with Figure 1 shown, and includes the following steps:

[0051] Step 1: Input the accelerometer measurement data a m of the IMU and the gyroscope measurement data ω m ;

[0052] Step 2: Model the accelerometer measurement data a m,k and the gyroscope measurement data ω m,k at time k as follows:

[0053]

[0054] ω m,k = ω k + b g (2)

[0055] In the formula, a k-1 represents the external acceleration of the IMU at time k - 1, ω k represents the angular velocity of the IMU at time k, ξ is a constant, ξ ∈ [0, 1], and in one embodiment, ξ = 0.9, represents the attitude of the IMU in the navigation coordinate system at time k, g is the local gravitational acceleration vector, and in one embodiment, g = [0, 0, 9.8066] T , b a and b g respectively represent the zero biases of the accelerometer and the gyroscope, and in one embodiment, b a = [2.0000e - 03, 2.0000e - 03, 2.0000e - 03] T , b g = [1.6968e - 04, 1.6968e - 04, 1.6968e - 04] T , ε k represents the external acceleration noise of the IMU at time k;

[0056] Step 3: Use the angular velocity measurement of the gyroscope to predict the IMU attitude, which is expressed as follows:

[0057]

[0058] Where ω k-1 and represent the IMU angular velocity and the attitude of the IMU in the navigation coordinate system at the (k - 1)th moment respectively, I 3 represents the 3×3 identity matrix, Δt is the data sampling time interval of the IMU, and in one embodiment, Δt = 0.005 s, [·] × represents the cross product operation symbol;

[0059] Step Four: Attitude is composed of three attitude angles: roll angle ψ k , pitch angle θ k and yaw angle φ k , and the rotation order is z - y - x. The relationship between the attitude and the attitude angles is as follows:

[0060]

[0061] Take the third row element of the attitude as the state x k to be estimated:

[0062] x k = [-sinθ k , sinψ k cosθ k , cosψ k cosθ k T (5)

[0063] Transform formula (3) into:

[0064]

[0065] Where represents the prior estimate of the state x k at the kth moment, is the prior estimate of the state x k-1 at the (k - 1)th moment. According to formula (6), the covariance matrix of the state prior estimate is as follows:

[0066]

[0067] Where P k-1 is the covariance matrix of the state posterior estimate at the (k - 1)th moment, and Σ g is the covariance matrix of the gyroscope noise;

[0068] Step Five: Construct an attitude observation model using the acceleration measurement of the accelerometer. According to formula (1), we get: ​

[0069] a m,k -ξa k-1 = ||g||x k + ε k + b a (8)

[0070] In the formula, ||g|| represents the magnitude of the local gravitational acceleration vector. In one embodiment, ||g|| = 9.8066 is taken. It is defined that:

[0071]

[0072] In the formula, a k represents the external acceleration of the IMU at time k;

[0073] Step Six: When the system is fault-free, the actual error e of the Kalman filter e should be less than its theoretical error e t , that is, the following formula is satisfied:

[0074]

[0075] In the formula, Σ a is the covariance matrix of the accelerometer noise, and Σ ε is the external acceleration noise. When the external acceleration is too large, that is, e e > e t , it indicates that the system has a fault, and the influence of the system fault caused by this external acceleration must be considered in the Kalman filter gain;

[0076] Step Seven: According to the analysis of the system fault cause, the acceleration fine-tuning factor λ is defined by formula (10) k as follows:

[0077]

[0078] Acceleration compensation is performed on each axis through the acceleration fine-tuning factor. When external acceleration is detected, the Kalman filter gain will decrease as the acceleration fine-tuning factor increases, and the accuracy of attitude estimation is improved by reducing the influence of the external acceleration;

[0079] Step Eight: Ensure the non-negative condition of the accelerometer fine-tuning factor. When the accelerometer fine-tuning factor is less than zero, it will amplify the influence of the external acceleration. Therefore, the non-negativity of λ k must be ensured, that is:

[0080] λ k (i,j) = max{0, λ k (i,j)}, i = 1, 2, 3; j = 1, 2, 3 (12)

[0081] In the formula, λk (i,j) represents the element value of the i-th row and j-th column of the acceleration fine-tuning factor;

[0082] Step Nine: Calculate the Kalman filter gain K according to the acceleration fine-tuning factor k :

[0083]

[0084] Due to the existence of the acceleration fine-tuning factor, the Kalman filter can dynamically adjust the filter gain, improving the robustness of attitude estimation;

[0085] Step Ten: Update the mean and covariance matrix of the state estimation according to the Kalman filter gain to obtain the finally estimated state x k and the covariance matrix P of the posterior state estimation k as follows:

[0086]

[0087]

[0088] After that, according to the finally estimated state x k Solve the attitude angle of the IMU as follows:

[0089]

[0090]

[0091] In the formula, x k (l), l = 1, 2, 3 represents the l-th element value of the state x k of.

[0092] Finally, combined with Figure 2 shown, the IMU attitude estimation result obtained in this embodiment can be seen. Through the IMU attitude estimation method proposed by the present invention, the attitude estimation errors of the roll angle and pitch angle are both within 1°, with high accuracy, verifying the superiority of the IMU attitude estimation method proposed by the present invention.

[0093] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above exemplary embodiments, and without departing from the spirit or basic characteristics of the present invention, the present invention can be implemented in other forms. Therefore, from any point of view, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, it is intended to embrace all changes falling within the meaning and scope of the equivalent conditions of the claims in the present invention. Any reference signs in the claims should not be regarded as limiting the claims involved.

[0094] In addition, it should be understood that although this specification is described in terms of embodiments, not every embodiment contains only an independent technical solution. This narrative manner of the specification is only for clarity. Those skilled in the art should regard the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. An IMU adaptive attitude estimation method based on Kalman filter, characterized in that: The following steps are involved: Step 1: Input the IMU accelerometer measurement data a m And gyroscope measurement dataω m ; Step 2: Measure the accelerometer data a at time k m,k and gyroscope measurement dataω m,k The modeling is as follows: oh m,k =ω k +b g (2) In the formula, a k-1 represents the IMU external acceleration at time k-1, ω k represents the IMU angular velocity at time k, ξ is a constant, ξ∈[0,1], represents the attitude of the IMU in the navigation coordinate system at time k, g is the local gravity acceleration vector, b a and b g Denote the zero bias of the accelerometer and gyroscope, ε k represents the IMU external acceleration noise at time k; Step 3: Use the angular velocity measurement of the gyroscope to predict the IMU attitude, as shown below: In the formula, ω k-1 and They represent the IMU angular velocity and the IMU posture in the navigation coordinate system at time k-1, I3 represents the unit matrix of size 3×3, Δt is the data sampling time interval of IMU, [·] × Represents the cross product operator symbol; Step 4: Posture It is composed of three attitude angles: roll angle ψ k , pitch angle θ k and yaw angle φ k , the rotation order is zyx, the posture The relationship with the attitude angle is as follows: Take a stance The third row element is the state x to be estimated k : x k =[-sinθ k ,sinψ k cosθ k ,cosψ k cosθ k ] T (5) Formula (3) is transformed into: In the formula, Indicates the state x at time k k A priori estimate of At time k-1, the state x k-1 The prior estimate of the state is obtained according to formula (6): as follows: Where P k-1 is the covariance matrix of the state posterior estimate at time k-1, Σ g is the covariance matrix of the gyroscope noise; Step 5: Use the acceleration measurement of the accelerometer to build a posture observation model, and according to formula (1) we get: a m,k -xa k-1 =gx k +e k +b a (8) Where g represents the modulus of the local gravity acceleration vector, defined as: In the formula, a k Represents the IMU external acceleration at time k; Step 6: When the system is fault-free, the actual error e of the Kalman filter is e Should be less than its theoretical error e t , that is, satisfying the following formula: In the formula, Σ a is the covariance matrix of the accelerometer noise, Σ ε is the external acceleration noise; Step 7: Based on the analysis of the cause of the system failure, the acceleration fine-tuning factor λ is defined by formula (10): k as follows: Acceleration compensation is performed on each axis through the acceleration fine-tuning factor; Step 8: Ensure the non-negativity of the accelerometer fine-tuning factor, that is: l k (i,j)=max{0,λ k (i,j)},i=1,2,3;j=1,2,3 (12) In the formula, λ k (i,j) represents the value of the element in the i-th row and j-th column of the acceleration fine-tuning factor; Step 9: Calculate the Kalman filter gain K based on the acceleration fine-tuning factor k : The acceleration fine-tuning factor enables the Kalman filter to dynamically adjust the filter gain; Step 10: Update the mean and covariance matrix of the state estimate according to the Kalman filter gain to obtain the final estimated state x k And the covariance matrix P of the state posterior estimate k as follows: Then according to the final estimated state x k The attitude angle of the IMU is calculated as follows: In the formula, x k (l), l=1,2,3 represents the state x k The lth element value of .

Images