阅读说明：本技术 基于mems传感器的四元数融合姿态估计方法 (Quaternion fusion attitude estimation method based on MEMS sensor ) 是由 陈光武 张琳婧 邢东峰 程鉴皓 李文元 李朋朋 刘射德 于 2019-09-10 设计创作，主要内容包括：本发明公开了一种基于MEMS传感器的四元数融合姿态估计方法,包括：将陀螺仪原始测量信号进行预处理,得到预处理后的高频陀螺输出；使用含遗忘因子的加权最小二乘算法对所述高频陀螺输出进行处理,得到去噪后的陀螺输出；基于获取的初始姿态四元数和所述去噪后的陀螺输出使用改进的最小二乘递推方法估计陀螺漂移；将估计得到的陀螺漂移输入最小二乘估计模型得到补偿后的姿态四元数；基于所述补偿后的姿态四元对陀螺仪的姿态进行更新。实现提高姿态估计精度的优点。(The invention discloses a quaternion fusion attitude estimation method based on an MEMS sensor, which comprises the following steps: preprocessing an original measurement signal of the gyroscope to obtain a preprocessed high-frequency gyroscope output; processing the high-frequency gyroscope output by using a weighted least square algorithm containing a forgetting factor to obtain denoised gyroscope output; estimating gyro drift by using an improved least square recursion method based on the obtained initial attitude quaternion and the denoised gyro output; inputting the gyro drift obtained by estimation into a least square estimation model to obtain a compensated attitude quaternion; and updating the attitude of the gyroscope based on the compensated attitude quaternion. The method has the advantage of improving the attitude estimation precision.)

1. A quaternion fusion attitude estimation method based on an MEMS sensor is characterized by comprising the following steps:

preprocessing an original measurement signal of the gyroscope to obtain a preprocessed high-frequency gyroscope output;

processing the high-frequency gyroscope output by using a weighted least square algorithm containing a forgetting factor to obtain denoised gyroscope output;

estimating gyro drift by using an improved least square recursion method based on the obtained initial attitude quaternion and the denoised gyro output;

inputting the gyro drift obtained by estimation into a least square estimation model to obtain a compensated attitude quaternion;

and updating the attitude of the gyroscope based on the compensated attitude quaternion.

2. The method of claim 1, wherein the step of preprocessing the raw gyroscope measurement signals is preceded by:

the raw measurement signal of the gyroscope is represented as:

s(k)＝h(k)+ε·e(k),k＝0,1,…,n-1，

wherein s (k) is the original measurement signal of the gyroscope, h (k) is the unbiased signal, e (k) is the noise, and epsilon is the standard deviation of the noise.

3. The quaternion fusion attitude estimation method based on MEMS sensor according to claim 1, characterized in that the weighted least squares algorithm with forgetting factor comprises:

setting a functional relation between input and output obeys;

establishing an error square sum equation of a model output value and an actual observation value based on the functional relation, and introducing a forgetting factor lambda into the error square sum equation;

obtaining a least square problem based on an error square sum equation of introduced forgetting factor lambda;

and carrying out weighting processing on the least square problem.

4. The quaternion fusion attitude estimation method based on MEMS sensor according to claim 3, characterized in that the weighted least squares algorithm with forgetting factor comprises:

the function relationship obeyed by the input and the output of the system is set as y ═ f (x, t)_i) Where y is the system output, t_iFor system input, x ∈ RⁿThe parameter is a undetermined parameter or a undetermined vector;

noting the sum of the squares of the errors based on the model output and the actual observed value as S, e.g.Wherein m is the actual number of observations, and the value of x is the general minimum when S is found to be the minimumThe second-order problem and introduces a forgetting factor λ into a general least-squares estimation criterion, as:

introducing a residual function r_i(x)＝y_i-f(x,t_i) 1,2, …, mThen the least squares problem is recorded as

Writing a system measurement equation into a matrix form z ═ Hx + v, and performing weighting processing on a least square estimation criterion, wherein the formula is as follows:

wherein z is a measurement vector, which is used as an indirect measurement that cannot obtain the true value of the vector x to be estimated, and each component of the true value x is selected for linear combination; h is a measurement matrix, v is random measurement noise, W is a weighted positive definite matrix, and the weighted least square estimation obtained is as follows:

the weighted least squares estimate residual is as follows:

wherein the measurement noise v satisfies that the mean is zero and the variance matrix is R, i.e. and satisfies that W ═ R^-1Then weighted least squares estimate is given by

5. The MEMS sensor-based quaternion fusion attitude estimation method of claim 1, wherein the improved least squares recursion method comprises:

establishing a least square discrete recursion equation:

wherein K is the filter gain, P is the mean square error matrix, I is the unit matrix, H is the measurement matrix, Z is the measurement vector,is a state vector, k is a natural number;

the state mean square error matrix of the least squares discrete recursion equation is updated with square root filtering as:

Δ is the square root of P.

6. The method of claim 1, wherein inputting the estimated gyro drift into a least squares estimation model to obtain compensated attitude quaternion comprises updating the attitude of the gyroscope using a quaternion-fused dynamically updated attitude approach,

the quaternion fused dynamic attitude updating method comprises the following steps:

setting the carrier coordinate system as a b system, the navigation coordinate system as an n system, and transforming the coordinate matrix from the b system to the n systemReferred to as the attitude transformation matrix;

expressing angular velocity information output by the gyroscope based on the attitude transformation matrix to obtain a gyroscope quaternion;

representing acceleration information output by the accelerometer based on the attitude transformation matrix to obtain an accelerometer quaternion;

taking the difference between the gyro quaternion and the accelerometer quaternion as a state quantity of filtering, and taking the accelerometer quaternion as an observed quantity to obtain a quaternion least square estimation model;

and obtaining a quaternion attitude compensation model based on the quaternion least square estimation model.

7. The quaternion fusion attitude estimation method based on the MEMS sensor according to claim 6, wherein the quaternion least squares estimation model is:

wherein q is_ωIs a gyro quaternion, q_aIs an accelerometer quaternion, T is an angular velocity sampling period, Q_kIs a quaternion matrix, omega, at the previous moment₀Is the gyro drift vector, q_a0The noise quaternion in the accelerometer solution, k being a natural number.Is a state estimation vector, consisting of quaternion errors; and z is an observation vector and consists of an accelerometer quaternion.

8. The method of claim 7, wherein the quaternion-fused attitude estimation method based on the MEMS sensor is characterized in that the attitude compensation model of the quaternion is as follows:

wherein Q_kIs the quaternion matrix of the last moment, k is a natural number, q_ωIs the gyro attitude quaternion at the current moment, and q is the complementAnd compensating the drift attitude quaternion.

9. The method of claim 6, wherein the acceleration information is derived from a gravity vector measured by an accelerometer.

Technical Field

The invention relates to the field of attitude estimation, in particular to a four-element number fusion attitude estimation method based on an MEMS sensor.

Background

Attitude estimation is an important link of sensing and is widely used in intelligent machines such as unmanned aerial vehicles and unmanned vehicles.

Nowadays, the consumer-grade unmanned market is gradually exploded, and especially, the unmanned aerial vehicle series as the pioneer of the unmanned market, since the MEMS (microelectromechanical systems) technology can integrate more sensors at low cost because of its unique advantages, thereby being beneficial to realizing accurate measurement and control of the attitude, and the measurable dynamic range of the MEMS sensor is also continuously expanded. The most intuitive application of the MEMS gyroscope in the unmanned series is attitude acquisition, and the continued application is smooth control and assisted navigation of a carrier, wherein the gyroscope is a key device. However, since the gyroscope cannot provide an absolute reference, it is important to research how to obtain more accurate and stable real-time attitude information from the output of the MEMS gyroscope with noise, and thus how to reduce noise of the MEMS gyroscope is more concerned about the accuracy of attitude estimation.

The expression method of the attitude is the basis of researching attitude estimation, and common attitude calculation methods comprise Euler angles, direction cosines, rotation vectors and quaternion differential equations. Due to the global and non-singular advantages and the characteristic of avoiding a large amount of triangular calculation, the quaternion representation method with small calculation amount is widely applied. Micro Inertial Measurement Unit (MIMU) based on MEMS accelerometer/gyroscope has advantages of low cost, small volume, and low power consumption, and is applied in the field of attitude estimation. In a long-time working period, because the precision of the accelerometer is higher than that of the gyroscope, the gyroscope has accumulated errors, attitude information with higher precision can be obtained by combining the methods of the accelerometer and the gyroscope, course angle correction can also be carried out by using a magnetometer-assisted mode, but the magnetometer is sensitive to an external magnetic field and is easy to interfere, and certain requirements are met on the driving environment of a carrier. Therefore, the method has important significance in researching the quaternion attitude updating mode of the micro-inertia attitude measurement unit based on the MEMS accelerometer/gyroscope, has low cost and wide applicable environment, has long service life and high integration level, and plays an important role in researching the attitude measurement based on the MEMS technology in an unmanned series sensing layer.

Especially, the application of the small unmanned aerial vehicle is limited due to the small size, the small unmanned aerial vehicle limits the application of high-precision attitude measurement devices such as a laser gyro and a fiber-optic gyro in a certain degree, and therefore the MEMS sensor becomes the first choice for attitude measurement and control of the small unmanned aerial vehicle. Because the measurement accuracy is improved by simply depending on the improvement of the accuracy of the inertial instrument, a higher technical level is required, and the research and development cost is correspondingly improved, the key for improving the attitude accuracy is on the selection and fusion mode of the algorithm under the condition that the hardware basis is unchanged. For example, the application of a bionic drone, which is a small and important application of the drone family, MEMS-based sensors are the most suitable kind for its stability control and attitude measurement.

Disclosure of Invention

The invention aims to provide a quaternion fusion attitude estimation method based on an MEMS sensor to achieve the advantage of improving the accuracy of attitude estimation.

In order to achieve the purpose, the embodiment of the invention adopts the technical scheme that:

a quaternion fusion attitude estimation method based on a MEMS sensor comprises the following steps:

preprocessing an original measurement signal of the gyroscope to obtain a preprocessed high-frequency gyroscope output;

processing the high-frequency gyroscope output by using a weighted least square algorithm containing a forgetting factor to obtain denoised gyroscope output;

estimating gyro drift by using an improved least square recursion method based on the obtained initial attitude quaternion and the denoised gyro output;

inputting the gyro drift obtained by estimation into a least square estimation model to obtain a compensated attitude quaternion;

and updating the attitude of the gyroscope based on the compensated attitude quaternion.

Further, before the step of preprocessing the raw measurement signal of the gyroscope, the method includes:

the raw measurement signal of the gyroscope is represented as:

s(k)＝h(k)+ε·e(k),k＝0,1,…,n-1，

wherein s (k) is the original measurement signal of the gyroscope, h (k) is the unbiased signal, e (k) is the noise, and epsilon is the standard deviation of the noise.

Further, the weighted least square algorithm with a forgetting factor includes:

setting a functional relation between input and output obeys;

establishing an error square sum equation of a model output value and an actual observation value based on the functional relation, and introducing a forgetting factor lambda into the error square sum equation;

obtaining a least square problem based on an error square sum equation of introduced forgetting factor lambda;

and carrying out weighting processing on the least square problem.

Further, the weighted least square algorithm with a forgetting factor includes:

the function relationship obeyed by the input and the output of the system is set as y ═ f (x, t)_i) Where y is the system output, t_iIs delivered to the systemIn, x ∈ RⁿThe parameter is a undetermined parameter or a undetermined vector;

noting the sum of the squares of the errors based on the model output and the actual observed value as S, e.g.Wherein m is the actual observation frequency, the problem that the value of x is a common least square when S is the minimum is solved, and a forgetting factor lambda is introduced into a common least square estimation criterion, as shown in the formula:

introducing a residual function r_i(x)＝y_i-f(x,t_i) 1,2, …, mThen the least squares problem is recorded as

Writing a system measurement equation into a matrix form z ═ Hx + v, and weighting the least square estimation criterion, wherein the formula is as follows:

wherein z is a measurement vector, which is used as an indirect measurement that cannot obtain the true value of the vector x to be estimated, and each component of the true value x is selected for linear combination; h is a measurement matrix, v is random measurement noise, W is a weighted positive definite matrix, and the weighted least square estimation obtained is as follows:

the weighted least squares estimate residual is as follows:

wherein the measurement noise v satisfies that the mean is zero and the variance matrix is R, i.e. and satisfies that W ═ R^-1Then weighted least squares estimate is given by

Further, the improved least squares recursion method comprises:

establishing a least square discrete recursion equation:

wherein K is the filter gain, P is the mean square error matrix, I is the unit matrix, H is the measurement matrix, Z is the measurement vector,is a state vector, k is a natural number;

the state mean square error matrix of the least squares discrete recursion equation is updated with square root filtering as:

Δ is the square root of P.

Further, inputting the estimated gyro drift into a least square estimation model to obtain a compensated attitude quaternion, including updating the attitude of the gyroscope by using a quaternion-fused dynamic attitude updating mode,

the quaternion fused dynamic attitude updating method comprises the following steps:

setting the carrier coordinate system as a b system, the navigation coordinate system as an n system, and transforming the coordinate matrix from the b system to the n systemReferred to as the attitude transformation matrix;

expressing angular velocity information output by the gyroscope based on the attitude transformation matrix to obtain a gyroscope quaternion;

based on the acceleration information output by the accelerometer represented by the attitude transformation matrix, obtaining an accelerometer four-element number;

taking the difference between the gyro quaternion and the accelerometer quaternion as a state quantity of filtering, and taking the accelerometer quaternion as an observed quantity to obtain a quaternion least square estimation model;

and obtaining a quaternion attitude compensation model based on the quaternion least square estimation model.

Further, the quaternion least squares estimation model is:

wherein q is_ωIs a gyro quaternion, q_aIs an accelerometer quaternion, T is an angular velocity sampling period, Q_kIs a quaternion matrix, omega, at the previous moment₀Is the gyro drift vector, q_a0The noise quaternion in the accelerometer solution, k being a natural number.Is a state estimation vector, consisting of quaternion errors; and z is an observation vector and consists of quaternions of the accelerometer.

Further, the attitude compensation model of the quaternion is as follows:

wherein Q_kIs the quaternion matrix of the last moment, k is a natural number, q_ωAnd q is a gyro attitude quaternion at the current moment, and q is an attitude quaternion for compensating drift.

Further, the acceleration information is obtained from a gravity vector measured by the accelerometer.

The technical scheme of the invention has the following beneficial effects:

1. according to the quaternion fusion attitude estimation method based on the MEMS sensor, the problem of serious drift of the gyroscope in a long-time working period is solved through an improved quaternion attitude updating method, the output of the gyroscope is optimally fitted and estimated by using an improved least square method, the accuracy of the gyroscope is improved, the output is smoother, and a part of sudden changes and singular values are reduced.

2. The quaternion fusion attitude estimation method based on the MEMS sensor, which is provided by the invention, weights the measured value to different degrees by utilizing the characteristics of small quaternion calculated amount and containing all attitude information, so that the information contained in the measured value can be fully utilized in the filtering process, and a part of calculated amount can be reduced.

3. According to the quaternion fusion attitude estimation method based on the MEMS sensor, the quaternion attitude updating method introduces the square root filtering thought in the filtering process, and the calculation error and the numerical truncation error in the iteration process can be effectively overcome.

4. The quaternion fusion attitude estimation method based on the MEMS sensor has the advantages that the quaternion fusion filtering can be carried out by respectively resolving the gravity vector measured by the accelerometer and the angular rate information measured by the gyroscope within a long time period, and the accuracy of attitude estimation is effectively improved.

The technical solution of the present invention is further described in detail by the accompanying drawings and embodiments.

Drawings

FIG. 1 is a flow chart of a quaternion fusion attitude estimation method based on MEMS sensors according to an embodiment of the present invention;

FIG. 2 is a block diagram of an improved least squares recursion method according to an embodiment of the present invention;

FIG. 3 is a block diagram of an attitude estimation update according to an embodiment of the present invention;

FIG. 4 is a block diagram of quaternion fusion according to an embodiment of the present invention;

fig. 5 is a graph showing the effect of the experiment according to the embodiment of the present invention.

Detailed Description

The preferred embodiments of the present invention will be described in conjunction with the accompanying drawings, and it should be understood that the preferred embodiments described herein are merely for purposes of illustration and explanation, and are not intended to limit the present invention.

As shown in fig. 1, a quaternion fusion attitude estimation method based on a MEMS sensor includes:

s101: preprocessing an original measurement signal of the gyroscope to obtain a preprocessed high-frequency gyroscope output;

s102: processing the high-frequency gyroscope output by using a weighted least square algorithm containing a forgetting factor to obtain denoised gyroscope output;

s103: estimating gyro drift by using an improved minimum-two-times recursion method based on the obtained initial attitude quaternion and the denoised gyro output;

s104: inputting the gyro drift obtained by estimation into a least square estimation model to obtain a compensated attitude four-element number;

s105: and updating the attitude of the gyroscope based on the compensated attitude quaternion.

Further, the weighted least square algorithm with a forgetting factor includes:

setting a functional relation between input and output obeys;

establishing an error square sum equation of a model output value and an actual observation value based on the functional relation, and introducing a forgetting factor lambda into the error square sum equation;

obtaining a least square problem based on an error square sum equation of introduced forgetting factor lambda;

and carrying out weighting processing on the least square problem.

Further, inputting the estimated gyro drift into a least square estimation model to obtain a compensated attitude quaternion, including updating the attitude of the gyroscope by using a quaternion-fused dynamic attitude updating mode,

the quaternion fused dynamic attitude updating method comprises the following steps:

setting the carrier coordinate system as a b system, the navigation coordinate system as an n system, and a coordinate transformation matrix C from the b system to the n system_bn is called a posture conversion matrix;

expressing angular velocity information output by the gyroscope based on the attitude transformation matrix to obtain a gyroscope quaternion;

based on the acceleration information output by the accelerometer represented by the attitude transformation matrix, obtaining an accelerometer four-element number;

taking the difference between the gyro quaternion and the accelerometer quaternion as a state quantity of filtering, and taking the accelerometer quaternion as an observed quantity to obtain a quaternion least square estimation model;

and obtaining a quaternion attitude compensation model based on the quaternion least square estimation model.

In a particular application scenario,

a quaternion fusion attitude estimation method based on an MEMS sensor comprises the steps of firstly expressing an original measurement signal of a gyroscope as a formula (1),

s(k)＝h(k)+ε·e(k),k＝0,1,…,n-1 (1)，

wherein s (k) is the original measurement signal of the gyroscope, h (k) is the unbiased signal, e (k) is the noise, and epsilon is the standard deviation of the noise. Firstly, preprocessing the gyroscope to obtain the preprocessed high-frequency gyroscope output.

The preprocessing is to make the data curve smoother and remove the obviously abnormal values, so as to prevent other steps from being polluted in the subsequent data processing. This implementation uses general data smoothing, but is not limited to this method.

The signal after gyro preprocessing is used as an initial value, and an improved least square recursion method is used, so that the gyro signal is reduced in abrupt change and singular value and is smoother.

A weighted least squares algorithm with forgetting factor comprising:

step 1: the function relationship obeyed by the input and the output of the system is set as y ═ f (x, t)_i) Where y is the system output, t_iFor system input, x ∈ RⁿAnd the parameter is a pending parameter or a pending vector. The system is a MEMS system.

Step 2: noting the sum of the squares of the errors based on the model output and the actual observed value as S, e.g.Where m is the actual number of observations, and the value of x is a general least square problem when S is found to be the minimum. Introducing a forgetting factor lambda into a general minimum-two-times estimation criterion on the basis of the step one, as shown in a formula (2)

Introducing a residual function r_i(x)＝y_i-f(x,t_i) 1,2, …, mThen the least squares problem is recorded as

And step 3: writing a system measurement equation into a matrix form z ═ Hx + v, and performing weighting processing on the least square estimation criterion, wherein the formula is (3):

wherein z is a measurement vector which is used as an indirect measurement that cannot obtain the true value of the vector x to be estimated, and each component of the true value x is selected for linear combination. H is a measurement matrix, v is random measurement noise, W is a weighted positive definite matrix, and the weighted least square estimation obtained is as the following formula (4):

the weighted least squares estimation residual is as in equation (5):

wherein the measurement noise v satisfies that the mean is zero and the variance matrix is R, i.e. and satisfies that W ═ R^-1Then the weighted least squares estimate is as in equation (6):

also known as markov estimation.

In the actual solving process, the weighted least square algorithm containing the forgetting factor overcomes the disadvantage that the common minimum quadratic sum recurrence estimation rule enables the square sum of all deviations to be minimum but has no difference in the use of each measurement value, and can fully utilize the information of the measurement values.

The weighted least squares algorithm with the forgetting factor is a markov estimation. So-called Markov estimation, from a certain state valueStarting from this, the state values are updated randomly and repeatedly as time changes, and finally the state values become a sample size closer to the target distribution.

An improved least squares recursion method comprising:

step 1: least-squares discrete recursion as in equation (7):

wherein K is the filter gain, P is the mean square error matrix, I is the unit matrix, H is the measurement matrix, Z is the measurement vector,is a state vector.

Step 2: and (3) introducing a square root filtering idea, and updating the state mean square error matrix of the formula (7), wherein the formula (8) is as follows:

delta is the square root of P, and the meaning of square root filtering is represented by Delta_kInstead of the recurrence relation of P.

The square root filtering idea is introduced, the problem of calculation divergence of a filter can be effectively solved by updating the state mean square error matrix, and the specific expression is that the problem that the error covariance matrix loses nonnegativity and symmetry can be solved.

The essence of updating the state mean square error matrix by introducing the square root filtering idea is that delta in the formula (8)_kBy recurrence relation of P_kThe recurrence relation of the method can effectively overcome calculation errors and numerical truncation errors.

Introducing square root filtering idea, and making initial input on state mean square error array into square root delta of state mean square error array₀The updated formula is as shown in formula (9):

wherein the content of the first and second substances,

Δ₀is Δ_kThe value of k in (1) is 0.

The quaternion fused dynamic attitude updating method comprises the following steps:

step 1: setting the carrier coordinate system as a b system, the navigation coordinate system as an n system, and transforming the coordinate matrix from the b system to the n systemReferred to as the attitude transformation matrix, also known as the mathematical platform. Definition of quaternion as given in equation (10):

wherein u isⁿThe direction of the rotation axis is shown, and θ represents the angle of rotation of the rotation axis, that is, the quaternion Q represented by equation (10) contains all the information of such equivalent rotation.

Step 2: the angular velocity information representing the gyro output is as follows:

ω＝[ω_x ω_y ω_z]^T (11)，

solving formula (10) gives formula (12):

calculating (12) formula-available gyro updated gyro quaternion q_ω＝[q₀,q₁,q₂,q₃]^TTaking formula (14) to obtain the attitude update matrix

The abbreviation formula (13) is formula (14):

the attitude solution (15) can be obtained from the conversion relation between the quaternion and the Euler angle,theta and gamma are respectively course, pitch and roll angles,

and step 3: the acceleration information representing the accelerometer output is as follows (16):

a＝[a_x a_y a_z]^T (16)，

quaternion obtained by gravity vector calculation as formula (17) from g_bObtaining a quaternion q of the current acceleration attitude_a，

And 4, step 4: and (3) taking the difference between the gyro quaternion and the accelerometer quaternion as a state quantity of filtering, and taking the accelerometer quaternion as an observed quantity to obtain a quaternion least square estimation model as shown in the formula (18):

wherein q is_ωIs a gyro quaternion, q_aIs an accelerometer quaternion, T is an angular velocity sampling period, Q_kIs a quaternion matrix, omega, at the previous moment₀Is the gyro drift vector, q_a0The noise quaternion in the accelerometer solution, k being a natural number.Is a state estimation vector, consisting of quaternion errors; and z is an observation vector and consists of quaternions of the accelerometer.

And 5: the quaternion attitude compensation model is as follows (19):

wherein Q_kIs the quaternion matrix of the last moment, k is a natural number, q_ωAnd q is a gyro attitude quaternion at the current moment, and q is an attitude quaternion for compensating drift.

The accelerometer quaternion is derived from the gravity vector measured by the accelerometer.

The quaternion difference value is used as a state quantity estimated value, and the accuracy of the accelerometer is higher than that of the gyroscope in a long-time working period and the accumulated error of the gyroscope does not exist, so that the filtering accuracy can be effectively improved by fusing quaternions.

According to the quaternion difference method, under the condition that only a gyroscope and an accelerometer sensor are used and no additional component is added, a link of directly using an accelerometer to solve the angle of a carrier is abandoned, a gravity quaternion model is established firstly, and after noise of the gyroscope is reduced, the attitude quaternion of the gyroscope is compensated.

Since the true carrier attitude at the current time is difficult to obtain, the quaternion least squares estimation model obtains the gyroscope drift angular rate value omega by using the formula (18)₀(k +1), and obtaining a compensated attitude quaternion according to a quaternion attitude equation (19) of the gyroscope.

As shown in fig. 2, the quaternion fusion attitude estimation method based on the MEMS sensor according to the present invention divides the gyro output into two parts to be processed; firstly, preprocessing the gyroscope, and mainly reducing the high-frequency noise of the gyroscope and reducing contained mutation and singular value because the noise of the gyroscope is mainly concentrated in a high-frequency band; and secondly, performing noise reduction estimation on the gyroscope output by using a weighted least square method with a forgetting factor, so that the gyroscope output has higher precision and is smoother.

According to the quaternion fusion attitude estimation method based on the MEMS sensor, the attitude updating process is shown in figure 3. And resolving the initial attitude quaternion by the output of the gyroscope, and resolving the gyroscope output after noise reduction to obtain the attitude quaternion after drift compensation. And using a least square error estimation model to perform fusion filtering on the accelerometer quaternion and the gyroscope quaternion to obtain a fused attitude quaternion, and finally performing attitude updating.

As shown in fig. 4, the quaternion fusion attitude estimation method based on the MEMS sensor according to the present invention obtains the acceleration and the angular velocity of the vehicle by using the accelerometer and the gyroscope sensor, respectively. Because the accuracy is reduced due to error accumulation caused by long-time work of the gyroscope and the long-time work accuracy of the accelerometer is higher, the output of the accelerometer is used as a measurement value, the quaternion obtained by resolving the output of the accelerometer and the quaternion obtained by resolving the output of the gyroscope are subjected to subtraction to be used as a state estimator of filtering, filtering fusion is carried out by using an improved recursive least square method, and an attitude estimation value is updated.

As shown in fig. 5, the diagram is an attitude angle estimation effect diagram, and a red line is an attitude value output by an attitude heading reference system of the experimental turntable; the first scheme is the updating of the traditional complementary filtering attitude; the second scheme is a fusion quaternion attitude estimation method based on the MEMS provided by the invention; it can be observed that the estimated attitude angle is converged faster in the second scheme than in the first scheme, and the attitude calculation accumulation error caused by the gyro drift can be remarkably reduced, and the attitude estimation precision can be stably improved by about 40%.

Finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that modifications may be made to the embodiments described above, or equivalents may be substituted for elements thereof. Any modification, equivalent replacement, or improvement made without departing from the spirit and principle of the present invention shall fall within the protection scope of the present invention.