Fast complementary filtering method for low-cost MARG sensor for pose estimation

Abstract

The application discloses a fast complementary filter method for attitude estimation by using a low-cost MARG sensor, and comprises the following steps: 1) estimating an attitude quaternion by using data output by an accelerometer; 2) using a gravity filter for compensation of a gyroscope; and 3) fusing the gravity and the magnetometer. The application discloses the fast complementary filter method for attitude estimation by using the low-cost MARG sensor, and converts the attitude estimation problem based on the accelerometer into a linear problem in view of a nonlinear problem in the attitude quaternion calculation process. In order to improve the accuracy of MARG attitude estimation, a new complementary filter structure is designed, and the new complementary filter structure is characterized in that the gravity is estimated by fusing the measurements of the accelerometer and the gyroscope. Meanwhile, the magnetic distortion detection is introduced into the algorithm, and the output of the magnetometer is fused with the estimated gravity based on the Markley algorithm.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of fast complementary filter, and particularly relates to a fast complementary filter method for attitude estimation of a low-cost MARG sensor. BACKGROUND

[0002] A MARG sensor is composed of a MEMS triaxial gyroscope, a triaxial accelerometer and a triaxial magnetometer, and has been widely applied in fields requiring attitude determination, such as human motion tracking, unmanned aerial vehicle autonomous flight, walking navigation, etc. Unlike traditional inertial sensors such as fiber-optic gyroscopes and ring laser gyroscopes, MEMS inertial sensors have relatively large errors, such as nonlinearity, random walk, temperature drift, etc., which means that an accurate attitude estimation cannot be obtained only by relying on a single MEMS inertial sensor. In order to ensure a reliable attitude solution, a MARG sensor must use a sensor fusion algorithm. In the past few decades, sensor fusion has been well developed, resulting in many valuable algorithms, such as complementary filter algorithm and Kalman filter algorithm.

[0003] The existing MARG sensor fusion algorithms can be mainly divided into two categories: one category involves complementary filters, and the other category involves Kalman filters. Complementary filters make full use of the accuracy of gyroscopes in a short time and the characteristics that accelerometers can determine the attitude through gravity, and can achieve good attitude measurement results with less computing resources. Kalman filters can estimate the state of a dynamic system from a series of data with measurement noise, provided that the measurement variance is known. Complementary filters are relatively simple and easy to apply, while Kalman filters require more complex matrix operations and are difficult to implement on platforms with low hardware configuration.

[0004] Most of the existing fusion algorithms construct a nonlinear system and use iterative methods, such as Gauss-Newton method and Levenberg-Marquardt method, which not only require more computing resources, but also require more memory to save calculation data. In addition, the existing fusion algorithms have poor attitude estimation accuracy in the presence of large external acceleration or large magnetic interference. SUMMARY

[0005] In order to solve the above problems, the present application proposes a fast complementary filter method for attitude estimation of a low-cost MARG sensor, which converts the attitude estimation problem based on the accelerometer into a linear problem in view of the nonlinearity in the process of calculating the attitude quaternion. In order to improve the accuracy of MARG attitude estimation, a new complementary filter structure is designed, which is characterized by first estimating the gravity by fusing the measurements of the accelerometer and the gyroscope. At the same time, magnetic distortion detection is introduced in the algorithm, and the output of the magnetometer is fused with the gravity estimated based on the Markley algorithm.

[0006] To achieve the above object, the technical scheme adopted by the present application is:

[0007] The present application provides a fast complementary filter method for low-cost MARG sensor for attitude estimation, the specific steps are as follows:

[0008] Step 1) estimating the attitude quaternion using the data output by the accelerometer;

[0009] Step 2) using the gyroscope to compensate the gravity filter;

[0010] Step 3) fusion of gravity and magnetometer.

[0011] As a further improvement of the present application, the step 1) estimates the attitude quaternion using the data output by the accelerometer;

[0012] The specific steps are as follows:

[0013] The rotation equation of the accelerometer is expressed as follows:

[0014] A b =CA r (1)

[0015] Wherein, A r is the representation of the accelerometer measurement in the East North Up coordinate system, A b is the representation of the accelerometer measurement in the carrier coordinate system, and C is the rotation matrix;

[0016] The rotation matrix is expressed in the following form:

[0017] C=(C1,C2,C3) (2)

[0018] Wherein,

[0019]

[0020]

[0021]

[0022] Wherein, q0, q1, q2, q3 are the quaternions corresponding to the rotation matrix;

[0023] Decompose equation (1) to obtain:

[0024]

[0025] Wherein, q a represents the quaternion obtained from the acceleration, and this equation is converted to:

[0026]

[0027] where, represents the Moore-Penrose generalized inverse matrix;

[0028] Using Equation (7) becomes:

[0029]

[0030] Define W a as:

[0031]

[0032] Equation (8) becomes:

[0033] W a q a = q a (10)

[0034] It can be easily seen that, Equation (10) is satisfied for any chosen q0;

[0035] Equation (10) is regarded as an iterative equation for q:

[0036] q a (t) = W a q a (t-1) (11)

[0037] It is noted that:

[0038]

[0039] Equation (11) is regarded as a linear discrete constant coefficient system, which is converted into a continuous system, according to modern control theory, a linear discrete constant coefficient system is converted into:

[0040]

[0041] where,

[0042]

[0043] where, T represents the sampling interval, since there are zero terms in W a , the natural logarithm of W a may contain -∞ terms, therefore, in order to approximately obtain H a , the first order Taylor expansion of is used:

[0044] W a = I 4×4 + Ha T (15)

[0045] Thus, the approximation H a is given by:

[0046]

[0047] where the time interval T is chosen to be very small, so that the continuous form of equation (11) becomes:

[0048]

[0049] The solution to this continuous system is derived as:

[0050]

[0051] where the exponential part is calculated as:

[0052]

[0053]

[0054]

[0055] In this way, is rewritten as:

[0056]

[0057] Setting

[0058]

[0059] This gives:

[0060]

[0061] Thus:

[0062]

[0063] When t→+∞, equation (24) becomes:

[0064]

[0065] Given any q0, the solution is given by:

[0066]

[0067] When q0= (1,0,0,0) T , equation (26) becomes:

[0068]

[0069] After normalization, we have:

[0070]

[0071] i.e. the quaternion from the accelerometer can be computed immediately using equation (28);

[0072] The measurement error is given by:

[0073]

[0074] The quaternion increment is given by:

[0075]

[0076] As a further improvement of the application, the step 2) using the gyroscope to compensate the gravity filter has the following specific steps: the projection of the gravity in the carrier coordinate system at time t is calculated using the following formula:

[0077]

[0078] where represents the fusion direction rate obtained by the accelerometer and the gyroscope, which is defined by the following formula:

[0079]

[0080] where γ represents the gain of the complementary filter, and equation (32) is further expressed as:

[0081]

[0082] The obtained quaternion is normalized:

[0083]

[0084] During the update, the quaternion q est,aω,t is estimated to obtain the gravity

[0085]

[0086] As a further improvement of the application, the step 3) fusion of the gravity and the magnetometer has the following specific steps:

[0087] The closed-form expression of the quaternion q opt is:

[0088]

[0089] where the parameters are:

[0090]

[0091]

[0092]

[0093] where represents the weight of the vector observation.

[0094] In this case, the vectors b1, r1, b2, r2, r × and b × are defined as:

[0095]

[0096] where, G b represents the gravity vector compensated by the complementary filter, which is obtained by using the information extracted from sensor fusion:

[0097]

[0098] However, due to the noise of the magnetometer, the calculated quaternion is not smooth enough, in which case a filter should be used to smooth the estimate, so that the gravity-magnetic quaternion is q gm The final attitude estimator generates q amω , and the quaternion based on the magnetometer is filtered in the following way:

[0099]

[0100] where, γ m represents the filter gain, and the result is normalized to generate the final attitude estimate:

[0101]

[0102] The invention is a fast complementary filtering method for low-cost MARG sensor attitude estimation, and the design points are as follows:

[0103] 1. The attitude estimation problem based on the accelerometer is converted into a linear problem, and the stable solution obtained is used as the measurement of the complementary filter;

[0104] 2. A new complementary filter structure is designed, characterized in that the gravity is first estimated by fusing the measurements of the accelerometer and the gyroscope;

[0105] 3. Magnetic distortion detection is introduced, and the output of the magnetometer is fused with the estimated gravity based on the Markley algorithm.

[0106] The present application is aimed at the nonlinear problem in quaternion solution in a pose fusion algorithm based on a MARG sensor, converts an attitude estimation problem based on an accelerometer into a linear problem, and uses a stable solution as a measurement of a complementary filter. In addition, in order to improve the precision of the fusion algorithm, a new complementary filter structure is designed, which is characterized in that the gravity is estimated by fusing the measurements of the accelerometer and the gyroscope, and the output of the magnetometer is fused with the estimated gravity based on the Markley algorithm. BRIEF DESCRIPTION OF DRAWINGS

[0107] Figure 1 The algorithm flowchart of the present application is shown in the figure. DETAILED DESCRIPTION

[0108] The present application will be further described in detail below in combination with the drawings and specific embodiments:

[0109] The fast complementary filter method for attitude estimation of a low-cost MARG sensor of the present application is shown in the algorithm flowchart of Figure 1 .

[0110] Step 1) estimating the attitude quaternion using the data output by the accelerometer.

[0111] The rotation equation of the accelerometer can be expressed as follows:

[0112] A b =CA r (1)

[0113] Wherein, A r is the representation of the accelerometer measurement in the East North Up coordinate system, A b is the representation of the accelerometer measurement in the carrier coordinate system, and C is a rotation matrix.

[0114] The rotation matrix can be expressed as follows:

[0115] C=(C1,C2,C3) (2)

[0116] Wherein,

[0117]

[0118]

[0119]

[0120] Wherein, q0, q1, q2, q3 are the quaternions corresponding to the rotation matrix.

[0121] The equation (1) is decomposed to obtain:

[0122]

[0123] where q a represents the quaternion obtained from the acceleration. This equation can be converted to:

[0124]

[0125] where, represents the Moore-Penrose generalized inverse matrix.

[0126] Using Equation (7) can be changed to:

[0127]

[0128] Define W a as:

[0129]

[0130] Equation (8) can be changed to:

[0131] W a q a = q a (10)

[0132] It can be easily seen that, Equation (10) is satisfied for any chosen q0.

[0133] Equation (10) can be regarded as an iterative equation for q:

[0134] q a (t) = W a q a (t - 1) (11)

[0135] It should be noted that:

[0136]

[0137] Obviously, equation (11) cannot be used to calculate the quaternion. In fact, equation (11) can be regarded as a linear discrete constant coefficient dynamic system. Therefore, it can be converted to a continuous system. According to modern control theory, a linear discrete constant coefficient system can be converted to:

[0138]

[0139] where,

[0140]

[0141] where T represents the sampling interval. Since in W aThere is a zero term, W a The natural logarithm may contain -∞ terms, for H a This is meaningless. Therefore, in order to approximate H... a ,use First-order Taylor expansion:

[0142] W a =I 4×4 +H a T (15)

[0143] This produces H. a Approximate value:

[0144]

[0145] Here, the time interval T is chosen to be very small, so the continuous form of equation (11) becomes:

[0146]

[0147] The solution to this continuous system can be derived as follows:

[0148]

[0149] The exponent portion is calculated as follows:

[0150]

[0151] in,

[0152]

[0153] In this way, It can be rewritten as:

[0154]

[0155] set up

[0156]

[0157] This allows us to obtain:

[0158]

[0159] therefore:

[0160]

[0161] As t→+∞, equation (24) becomes:

[0162]

[0163] Given any q0, its solution can be given by

[0164]

[0165] Note that when q0= (1, 0, 0, 0) T , equation (26) becomes:

[0166]

[0167] After normalization, we get:

[0168]

[0169] That is, the quaternion from the accelerometer can be immediately computed using equation (28).

[0170] The measurement error can be given by:

[0171]

[0172] The quaternion increment is given by:

[0173]

[0174] 2) Gravity filter with compensation using gyroscope;

[0175] The projection of gravity in the body frame at time t can be computed using the following equation:

[0176]

[0177] where is the fused direction rate obtained from the accelerometer and gyroscope, which is defined by:

[0178]

[0179] where γ represents the gain of the complementary filter, equation (32) can be further expressed as:

[0180]

[0181] The resulting quaternion is normalized:

[0182]

[0183] During the update, the quaternion q est,aω,t is estimated to obtain the gravity

[0184]

[0185] 3) Fusion of gravity and magnetometer;

[0186] The closed form of the quaternion q opt may be expressed as:

[0187]

[0188] where the parameters are:

[0189]

[0190]

[0191]

[0192] where represents the weight of the vector observation.

[0193] In this case, the vectors b1, r1, b2, r2, r × and b × are defined as:

[0194]

[0195] where G b represents the gravity vector compensated by the complementary filter. This fusion method requires a highly accurate measurement of the magnetic field direction in the reference frame M r . A common solution is to compensate the reference vector with a known magnetic map. However, in some special environments, this is not easily available. Using the information extracted from the sensor fusion, one can obtain:

[0196]

[0197] This problem can be solved without the reference vector. Using the method described above, one can obtain the quaternion from the gravity and the magnetic field. However, due to the noise of the magnetometer, the calculated quaternion is not smooth enough. In this case, a filter should be used to smooth the estimate. Let the gravity-magnetic quaternion be q gm and the final attitude estimator produces q amω . Then the quaternion based on the magnetometer is filtered by:

[0198]

[0199] where γ m represents the filter gain. The result is normalized to produce the final estimate of the attitude:

[0200]

[0201] It is noted that the gravity-magnetometer fusion is only performed if the magnetic field is stable. Distortions of the external magnetic field can be detected by the model of the magnetic field.

[0202] The above descriptions are only the preferred embodiments of the present application, not intended to limit the present application in any manner. Any modification or equivalent variation made according to the technical essence of the present application shall still fall within the scope of the present application.

Claims

1. A fast complementary filtering method for attitude estimation using a low-cost MARG sensor, comprising the following steps, characterized in that: Step 1) Estimate the attitude quaternion using the data output from the accelerometer; The specific steps are as follows: The rotation equation of the accelerometer is expressed as follows: A b =CA r (1) Among them, A r It is the representation of accelerometer measurements in the northeast-northeast coordinate system, A b It is the representation of the accelerometer measurement in the carrier coordinate system, and C is the rotation matrix; The rotation matrix is ​​represented in the following form: C = (C1, C2, C3) (2) in, Where q0, q1, q2, and q3 are the quaternions corresponding to the rotation matrix; Decomposing equation (1) yields: Where, q a Representing the quaternion derived from acceleration, this equation is transformed into: in, Represents the Moore-Penrose generalized inverse matrix; use Equation (7) becomes: Define W a for: Then equation (8) becomes: W a q a =q a (10) in, For any chosen q0, equation (10) is satisfied; Equation (10) is considered as an iterative equation for q: q a (t)=W a q a (t-1) (11) It is important to note that: Equation (11) is considered as a linear discrete constant coefficient dynamic system, which is transformed into a continuous system. According to modern control theory, a linear discrete constant coefficient system is transformed into: in, Where T represents the sampling interval, since in W a There is a zero term, W a The natural logarithm of H may contain -∞ terms; therefore, in order to approximate H... a ,use First-order Taylor expansion: W a =I 4×4 +H a T (15) This produces H. a Approximate value: Here, the time interval T is chosen to be very small, so the continuous form of equation (11) becomes: The solution to this continuous system is derived as follows: The exponent portion is calculated as follows: in, In this way, Rewritten as: set up Thus we obtain: therefore: As t→+∞, equation (24) becomes: Given any q0, the solution is given by the following formula: When q0 = (1,0,0,0) T Equation (26) becomes: After normalization, we get: The quaternion from the accelerometer is immediately calculated using equation (28); The measurement error is given by the following formula: The quaternion increment is given by the following formula: Step 2) Gravity filter with gyroscope compensation; Step 3) Integration of gravity and magnetometer.

2. The fast complementary filtering method for attitude estimation of a low-cost MARG sensor according to claim 1, characterized in that: The specific steps for the gravity filter using a gyroscope for compensation in step 2) are as follows: The projection of gravity in the carrier coordinate system at time t is calculated using the following formula: in The velocity in the fusion direction, obtained from the accelerometer and gyroscope, is defined by the following formula: Where γ represents the gain of the complementary filter, and equation (32) is further expressed as: Normalize the resulting quaternion: During the update, the quaternion q is used. est,aω,t The estimated gravity 3. The fast complementary filtering method for attitude estimation of a low-cost MARG sensor according to claim 1, characterized in that: The specific steps for integrating gravity and magnetometer in step 3) are as follows: Quaternion q opt The closed form is represented as: The parameters are: in Represents the weights of a single vector observation; In this case, vectors b1, r1, b2, r2, r × and b × Defined as: Among them, G b The gravity vector, compensated by a complementary filter, is obtained using information extracted from sensor fusion: However, due to magnetometer noise, the calculated quaternion is not smooth enough. In this case, a filter should be used to smooth the estimate so that the gravity-magnetism quaternion is q. gm The final attitude estimator produces q amω Therefore, the quaternions based on the magnetometer are filtered in the following way: Where, γ m Representing the filter gain, the result is normalized to produce an estimate of the final attitude:

Images