Attitude estimation method based on multi-sensor fusion and adaptive filtering

Abstract

The invention relates to the technical field of inertial navigation and sensor fusion, in particular to a multi-sensor fusion and adaptive filtering attitude estimation method, which comprises the following steps: S1, acquiring sensor data of a gyroscope, a magnetometer and an accelerometer, and respectively acquiring angular velocity, magnetic field direction and gravitational acceleration information; s2, angle information is obtained, and magnetometer and accelerometer sensor data are converted into attitude information; s3, carrying out weighting processing on the angle information and the attitude information through a fading memory weighting method and a limited memory weighting method; S4, carrying out filtering fusion on the weighted angle information and attitude information through an RAEKF filtering fusion algorithm; s5, an adaptive factor and a robust factor are introduced into the RAEKF filtering fusion algorithm; s6, outputting attitude angle estimation; according to the method, the theoretical contradiction that long-term drifting and stability cannot be achieved at the same time is fundamentally solved, stable precision is kept under abnormal data interference, and the robust performance is remarkably improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of inertial navigation and sensor fusion, in particular to a multi-sensor fusion and adaptive filtering attitude estimation method. BACKGROUND

[0002] With the continuous progress of sensor technology and the continuous expansion of application scenarios, the precision and reliability of attitude estimation methods are continuously improved, and attitude estimation technology has a wide range of applications in unmanned aerial vehicle flight control, automatic driving and other fields, and is one of the key technologies to realize autonomous control and navigation. The application range of attitude estimation technology will be further expanded and become an indispensable part of various intelligent systems.

[0003] In the traditional attitude estimation method, mainly adopts the fusion method of accelerometer sensor and magnetometer, gyroscopic integration method, the principle of the fusion method of accelerometer sensor and magnetometer is: through the three-axis accelerometer to measure the gravity vector component, combined with the magnetometer to obtain the direction of the geomagnetic field, the direction cosine matrix is used to establish the conversion relationship between the carrier coordinate system and the geographic coordinate system, and the static accuracy can reach ±0.5° (ISO 8728 standard test environment), and the azimuth repeatability error is <1° in the non-magnetic interference scene; application limitations: dynamic response delay reaches 200-300ms (ISO 15066 motion test), magnetic interference sensitivity: in the steel structure environment, the error increases to 10°-30°, motion acceleration interference: when the carrier acceleration is >0.5g, the attitude angle error increases exponentially. The gyroscopic integration method is based on the angular rate differential equation to update the quaternion, and the Runge-Kutta method or the Pica algorithm is used to realize the discretization integration, the dynamic response time is <50ms (100Hz sampling rate), there is no magnetic field dependence, and it is suitable for special scenes such as mines and tunnels, the zero drift instability: the typical value is 0.005-0.01° / s (MEMS gyroscope), the temperature drift: 0.02-0.05° / (s·℃) (Bosch BMI160 measured data), the cumulative error: the heading deviation reaches 3°-5° after running for 1 hour; the dynamic integration method based on the gyroscope: the heading change is calculated by integrating the angular rate.

[0004] The advantages of accelerometers and magnetometers lie in their ability to provide comprehensive motion information and accurate orientation data. Accelerometers use inertial forces and elastic elements to measure acceleration, while magnetometers use the Earth's magnetic field to determine direction. The combined use of these two sensors, through data fusion algorithms such as Kalman filters, provides more stable and reliable heading data. The advantage of calculating heading using angular rate integration lies in its directness and accuracy. Because gyroscopes measure angular velocity, they can reflect dynamic changes in real time in navigation, unaffected by other factors, thus providing more accurate heading data. Furthermore, this method does not rely on external references such as Earth's gravity or magnetic field, enabling it to provide stable heading data in various environments. However, the calculated heading values ​​from accelerometer and magnetometer sensor information are easily affected by the surrounding environment or other factors, causing the calculated values ​​to fluctuate significantly around the true value. The heading obtained by integrating the angular rate from the gyroscope is unreliable due to the accumulation of gyroscope sensor errors during the integration process in inertial navigation mechanization, especially during long-term operation.

[0005] Method type Advantages Disadvantages Gravity-magnetic fusion method High static accuracy (±0.5°), good reliability when the environmental magnetic field is stable Poor dynamic response (delay > 200 ms), susceptible to magnetic field interference (deviation up to 10°-30°) Gyroscope integration method Fast dynamic response (<50 ms), no magnetic field dependence Significant error accumulation (typical drift 0.5° / h), poor long-term stability

[0006] Traditional attitude estimation methods suffer from contradictions between static accuracy and dynamic response, short-term stability and long-term reliability, and anti-interference capability and computational complexity. Therefore, there is an urgent need for an attitude estimation method to resolve these contradictions. Summary of the Invention

[0007] The purpose of this invention is to provide a multi-sensor fusion and adaptive filtering attitude estimation method, which can effectively suppress the inherent sensor bias of the traditional gravity-magnetic fusion method and the zero bias accumulation effect of the gyroscope integration method, improve the accuracy of static angle measurement, significantly optimize dynamic response characteristics, maintain a stable attitude reference during continuous operation, greatly reduce the temperature drift coefficient, and make a qualitative leap in the anti-interference capability of the system; the computational complexity is controlled within the engineering applicability range, and the multipath effect and signal masking problem are effectively overcome.

[0008] To achieve the above objectives, the present invention provides the following technical solution: a multi-sensor fusion and adaptive filtering attitude estimation method, comprising the following steps: Step S1: Collect sensor data from the gyroscope, magnetometer, and accelerometer to obtain angular velocity, magnetic field direction, and gravitational acceleration information, respectively; Step S2: Integrate the angular velocity to obtain angle information, solve the problem using geometric relationships, and convert the data from the magnetometer and accelerometer sensors into attitude information; Step S3: The angle information and attitude information are weighted using the fading memory weighting method and the limited memory weighting method, and the weights are reasonably allocated according to the historical and current measurement values; Step S4: Filter and fuse the weighted angle and attitude information using the RAEKF filtering and fusion algorithm; Step S5: The RAEKF filtering fusion algorithm introduces an adaptive factor and a robust factor; Step S6: Output attitude angle estimate.

[0009] Preferably, in step S3, the model noise parameters from the start time to time k are calculated using the fading memory weighting method, and the noise covariance is calculated using the limited memory weighting method from time k+1.

[0010] Preferably, in step S3, the state noise covariance is calculated using the fading memory weighted method based on the Sage-Husa algorithm and the time-varying noise statistical estimator. The calculation formula is as follows:

[0011] in, These are parameters obtained through the forgetting factor. It is Kalman gain. It is filtered information. It is the error covariance. It is the propagation volume point. It is the state prediction vector.

[0012] Preferably, after time kw, the weight coefficients in the fading memory weighting method are replaced, and in the constrained memory weighted adaptive filter, the state noise covariance is... The calculation formula is:

[0013] in: It is Kalman gain. It is filtered information. It is the error covariance. It is the propagation volume point. It is the state prediction vector;

[0014] in: It is Kalman gain. It is system noise.

[0015] Preferably, in step S5, a three-segment function is used to construct an adaptive factor with a predicted state deviation statistic, and the calculation formula is as follows:

[0016] Where c0 and c1 are constants, which can be adjusted according to the actual situation. This is a state deviation statistic used to determine the error of the state model.

[0017] Preferably, in step S5, the model based on maximum likelihood estimation is combined with an adaptive factor, and the diagonal elements of the equal weight matrix are used. Off-diagonal elements The method for determining it is as follows:

[0018] in, and , respectively, are the diagonal and off-diagonal elements of the measurement noise covariance matrix, where c is a constant. This represents the standard residual.

[0019] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. At the algorithm architecture level, this invention integrates the fading memory weighting and the bounded memory mechanism, and combines them with a two-stage noise covariance estimation model to significantly improve the adaptability to dynamic noise. This topological fusion not only retains the asymptotic convergence characteristics of fading memory, but also provides bounded error protection for bounded memory, fundamentally solving the theoretical contradiction that long-term drift and stability cannot be achieved simultaneously.

[0020] 2. This invention develops an adaptive factor compensation system for typical scenarios such as irregular motion, sudden changes in equipment posture, and strong magnetic interference. Combined with a robust estimation mechanism, it effectively suppresses model mismatch problems, maintains stable accuracy under abnormal data interference, and has significantly better robustness than traditional methods.

[0021] 3. This invention constructs a robust adaptive extended Kalman filter framework, which significantly reduces computational complexity, enables high-frequency real-time processing on embedded platforms, significantly shortens the system's dynamic response delay, improves static accuracy and long-term operational stability, and demonstrates outstanding azimuth error control capabilities in environments with strong magnetic interference. Attached Figure Description

[0022] Figure 1 This is a flowchart of the method in this invention; Figure 2 This is a schematic diagram of the attitude estimation framework in this invention. Detailed Implementation

[0023] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0024] Please see Figures 1-2A multi-sensor fusion and adaptive filtering attitude estimation method includes the following steps: Step S1: Collect sensor data from the gyroscope, magnetometer, and accelerometer through the data acquisition and processing module, and obtain angular velocity, magnetic field direction, and gravitational acceleration information respectively.

[0025] Step S2: At the data processing level, the angular velocity is integrated to obtain angle information, and the data from the magnetometer and accelerometer sensors is converted into attitude information by using geometric relationships.

[0026] Step S3: The angle information and attitude information are weighted using the fading memory weighting method and the limited memory weighting method. The weights are reasonably allocated according to the historical and current measurement values ​​to optimize the data fusion effect.

[0027] The constrained memory weighting method reduces the bias of the filter estimation by increasing the weight of the latest observations and decreasing the weight of older information. Since the constrained memory weighting method requires knowledge of the covariance between the estimated and predicted values ​​at time k, the fading memory weighting method is used to calculate the model noise parameters from the initial time to time k. From time k+1 onwards, the noise covariance is calculated using the constrained memory weighting method. By combining the fading memory weighting method and the constrained memory weighting method, the model noise parameters are estimated and corrected, reducing the bias of the filter estimation. Based on the Sage-Husa algorithm and a time-varying noise statistical estimator, the state noise covariance of the fading memory weighting method is calculated. The calculation formula is as follows:

[0028] in, These are parameters obtained through the forgetting factor. It is Kalman gain. It is filtered information. It is the error covariance. It is the propagation volume point. It is the state prediction vector.

[0029] After time kw, the weight coefficients in the fading memory weighting method are replaced, and the state noise covariance is adjusted in the bounded memory weighted adaptive filter. The calculation formula is:

[0030] in: It is Kalman gain. It is filtered information. It is the error covariance. It is the propagation volume point. It is the state prediction vector;

[0031] in: It is Kalman gain. It is system noise.

[0032] Step S4: The weighted angle and attitude information are filtered and fused using the RAEKF filtering and fusion algorithm.

[0033] Step S5: The RAEKF filtering fusion algorithm introduces an adaptive factor and a robust factor. The adaptive factor automatically adjusts the filtering parameters according to the system state, improving filtering accuracy and enhancing the system's resistance to external interference and abnormal data, ensuring the stability and reliability of heading estimation. This invention effectively integrates the advantages of different sensors through multi-sensor data acquisition, weighted processing, and filtering fusion, and optimizes the estimation process using advanced algorithms, providing solid technical support for high-precision heading estimation. It is suitable for application scenarios with high requirements for attitude angle accuracy and has significant theoretical and practical application value.

[0034] An adaptive factor based on predicted state deviation statistics is employed to overcome the influence of filtering model errors and anomalous disturbances. To control the impact of dynamic model errors, an adaptive factor is used to correct the Kalman gain, mitigating the effects of measurement outliers and state model errors. When observation information is redundant, the state vector can be directly estimated using the observation information. To overcome the influence of filtering model errors and anomalous disturbances, an adaptive factor (α) based on partial state inconsistencies is used to overcome the anomalous effects of state disturbances. This invention uses a three-segment function to construct the adaptive factor with predicted state deviation statistics; the calculation formula is as follows:

[0035] Where c0 and c1 are constants, which can be adjusted according to the actual situation. This is a state deviation statistic used to determine the error of the state model.

[0036] Robustness factor: To control outliers, a model based on maximum likelihood estimation (M-estimation) is combined with an adaptive factor. In measurements, robust estimation of the equivalent weight matrix based on maximum likelihood estimation is used to control measurement outliers and improve the robustness of the filtering. The method for determining the diagonal and off-diagonal elements of the equal weight matrix is ​​as follows:

[0037] in, and , respectively, are the diagonal and off-diagonal elements of the measurement noise covariance matrix, where c is a constant. This represents the standard residual.

[0038] Step S6: Output attitude angle estimate.

[0039] This solution achieves a systemic breakthrough in high-precision attitude estimation under complex environments through multi-sensor fusion. At the algorithm architecture level, it integrates fading memory weighting and bounded memory mechanisms, coupled with a two-stage noise covariance estimation model, significantly improving dynamic noise adaptability. Traditional fading memory methods reduce the weight of old information through exponential decay of historical data, but cannot eliminate the persistent influence of initial errors; single bounded memory methods cause ill-conditioned distortion of the covariance matrix due to fixed window truncation. This invention proposes a dynamically transitioning two-stage architecture: during system initialization, fading memory is used to establish a noise statistical ground state, ensuring Lyapunov stability; once steady-state operation is achieved, it automatically switches to the bounded memory window, using a moving average mechanism to achieve smooth migration of old and new data. This topological fusion retains the asymptotic convergence characteristics of fading memory while providing bounded error protection for bounded memory, fundamentally resolving the theoretical contradiction between long-term drift and stability. Experimental verification shows that this design improves heading drift suppression capability to twice that of traditional methods.

[0040] For typical scenarios such as irregular motion, sudden changes in equipment attitude, and strong magnetic interference, an adaptive factor compensation system has been developed. Combined with a robust estimation mechanism, it effectively suppresses model mismatch problems, maintains stable accuracy under abnormal data interference, and exhibits significantly better robustness than traditional methods. Existing Huber-M estimation uses a fixed threshold to handle observational anomalies, and its single-channel architecture cannot distinguish between model mismatch and sensor failure. This scheme dynamically adjusts the Kalman gain based on the statistical characteristics of the innovation vector, automatically weakening prediction weights when model errors are detected; it also uses a continuous threshold function to reconstruct the covariance matrix, progressively suppressing measurements exceeding the confidence interval. These two approaches form an orthogonal interaction space: adaptive compensation addresses modeling errors in the state transition process, while robust control focuses on observational data anomalies, whereas Huber-M estimation suffers from error coupling due to its mixed processing. In strong magnetic interference environment testing, this scheme significantly improves the suppression efficiency of intermittent disturbances compared to traditional methods.

[0041] This invention constructs a robust adaptive extended Kalman filter framework, significantly reducing computational complexity and achieving high-frequency real-time processing capabilities on an embedded platform. The system's dynamic response latency is significantly shortened, and its static accuracy reaches industry-leading levels. Long-term operational stability is verified through field tests with mainstream sensors, and its azimuth error control capability is outstanding in environments with strong magnetic interference. In complex scenario tests, the system demonstrates excellent continuous positioning accuracy, successfully overcoming multipath effects and signal obstruction problems. This technology system forms a complete independent intellectual property layout, providing cost-effective navigation solutions for fields such as smart wearables and industrial robots. It combines low power consumption and cost advantages, showing broad industrialization prospects.

[0042] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0043] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A pose estimation method based on multi-sensor fusion and adaptive filtering, characterized in that: Includes the following steps: Step S1: Collect sensor data from the gyroscope, magnetometer, and accelerometer to obtain angular velocity, magnetic field direction, and gravitational acceleration information, respectively; Step S2: Integrate the angular velocity to obtain angle information, solve the problem using geometric relationships, and convert the data from the magnetometer and accelerometer sensors into attitude information; Step S3: The angle information and attitude information are weighted using the fading memory weighting method and the limited memory weighting method, and the weights are reasonably allocated according to the historical and current measurement values; Step S4: Filter and fuse the weighted angle and attitude information using the RAEKF filtering and fusion algorithm; Step S5: The RAEKF filtering fusion algorithm introduces an adaptive factor and a robust factor; Step S6: Output attitude angle estimate.

2. The attitude estimation method based on multi-sensor fusion and adaptive filtering according to claim 1, characterized in that: In step S3, the model noise parameters from the initial time to time k are calculated using the fading memory weighting method, and the noise covariance is calculated using the limited memory weighting method from time k+1.

3. The attitude estimation method based on multi-sensor fusion and adaptive filtering according to claim 2, characterized in that: In step S3, based on the Sage-Husa algorithm and the time-varying noise statistical estimator, the fading memory weighted state noise covariance is calculated. The calculation formula is as follows: ， in, These are parameters obtained through the forgetting factor. It is Kalman gain. It is filtered information. It is the error covariance. It is the propagation volume point. It is the state prediction vector.

4. The attitude estimation method based on multi-sensor fusion and adaptive filtering according to claim 3, characterized in that: After time kw, the weight coefficients in the fading memory weighting method are replaced, and the state noise covariance is determined in the bounded memory weighted adaptive filter. The calculation formula is: ， in: It is Kalman gain. It is filtered information. It is the error covariance. It is the propagation volume point. It is the state prediction vector; ， in: It is Kalman gain. It is system noise.

5. The attitude estimation method based on multi-sensor fusion and adaptive filtering according to claim 1, characterized in that: In step S5, a three-segment function is used to construct an adaptive factor with a predicted state deviation statistic, and the calculation formula is as follows: ， Where c0 and c1 are constants, which can be adjusted according to the actual situation. This is a state deviation statistic used to determine the error of the state model.

6. The attitude estimation method based on multi-sensor fusion and adaptive filtering according to claim 1, characterized in that: In step S5, the model based on maximum likelihood estimation is combined with an adaptive factor, and the diagonal elements of the equal weight matrix are used. Off-diagonal elements The method for determining it is as follows: ， in, and , respectively, are the diagonal and off-diagonal elements of the measurement noise covariance matrix, where c is a constant. This represents the standard residual.

Images