Gyroscope attitude estimation and error compensation method and system based on so(3) complementary filter

Abstract

This invention relates to a method and system for gyroscope attitude estimation and error compensation based on SO(3) complementary filtering. The method includes: establishing an attitude estimation model on the SO(3) group, representing the carrier attitude as an estimated rotation matrix satisfying orthogonal constraints, and designing a cost function that includes accelerometer and magnetometer information; collecting sensor data from the inertial measurement unit; calculating the corrected angular velocity based on the cost function and the current sensor measurement values, and converting the sensor measurement deviation into a correction amount on the SO(3) group using vector cross product operation; using the corrected angular velocity to estimate and update the gyroscope zero bias online, and performing real-time tracking and compensation for gyroscope drift; performing matrix exponential mapping operation on the compensated angular velocity using the Rodrigues formula to achieve incremental updates of the rotation matrix while maintaining SO(3) manifold constraints; and outputting the estimated attitude information and gyroscope zero bias. This invention improves numerical stability and computational accuracy.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation technology; and more particularly to a method and system for gyroscope attitude estimation and error compensation based on SO(3) complementary filtering. Background Technology

[0002] Inertial measurement units (IMUs) are widely used for attitude measurement and motion tracking in drones, robots, and autonomous vehicles. An IMU typically consists of a three-axis gyroscope and a three-axis accelerometer, and some also include a three-axis magnetometer to measure the vehicle's angular velocity, acceleration, and geomagnetic field direction. Attitude estimation is one of the core problems of inertial navigation systems; its mathematical essence lies in describing the evolution of the rotation matrix over time on a special orthogonal group, driven by the vehicle's angular velocity.

[0003] However, low-cost inertial devices suffer from problems such as gyroscope drift, accelerometer interference sensitivity, magnetometer susceptibility to interference, and difficulty in multi-sensor fusion in practical applications. In existing technologies, nonlinear complementary filtering based on quaternions does not consider SO(3) manifold constraints and has accumulated normalization errors. The method of fusing Kalman filtering relies on external aids such as GPS and cannot be applied to pure inertial measurement scenarios. The method based on quaternions and Runge-Kutta integrals lacks a rigorous mathematical foundation on the SO(3) group. The classic Mahony complementary filter does not fully utilize magnetometer information and has limited zero bias compensation capability.

[0004] Therefore, there is an urgent need for an attitude estimation method that can be directly computed on the SO(3) manifold, make full use of multi-sensor information, realize zero bias online estimation and compensation, and does not require external assistance. Summary of the Invention

[0005] In view of the above analysis, the present invention aims to disclose a gyroscope attitude estimation and error compensation method and system based on SO(3) complementary filtering, which solves the technical problems of gyroscope drift accumulation, accelerometer dynamic interference sensitivity, magnetometer susceptibility to magnetic interference and difficulty in multi-sensor fusion in attitude estimation of low-cost inertial devices.

[0006] This invention discloses a gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering, comprising: S1. Establish an attitude estimation model on the SO(3) group, express the carrier attitude as an estimated rotation matrix that satisfies orthogonal constraints, and design a cost function that includes accelerometer and magnetometer information. S2. Collect sensor data from the inertial measurement unit, including angular velocity measured by the gyroscope, gravity direction measured by the accelerometer, and geomagnetic field direction measured by the magnetometer. S3. Calculate the correction angular velocity based on the cost function and the current sensor measurement value, and use the vector cross product operation to convert the sensor measurement deviation into a correction amount on the SO(3) group; S4. The gyroscope zero bias is estimated and updated online using the corrected angular velocity to track and compensate for gyroscope drift in real time. S5. The compensated angular velocity is subjected to matrix exponential mapping operation through the Rodrigues formula to realize incremental update of the rotation matrix and maintain SO(3) manifold constraint. S6. Output the estimated attitude information and gyroscope zero bias; Repeat steps S2-S6 to continuously perform attitude estimation in real time.

[0007] Furthermore, in S1, the estimated rotation matrix is ​​established in the attitude estimation model. Satisfying orthogonal constraints: and ;in, This represents the transpose of the estimated rotation matrix, used to rotate the inertial frame reference vector to the carrier frame; It is the identity matrix; Represents the determinant operation of a matrix; Represents a special orthogonal group in three dimensions; The estimated gravity direction obtained from the attitude estimation model Estimate the geomagnetic direction ; This is the normalized representation of the gravity reference vector in the inertial frame. This is the normalized representation of the geomagnetic field reference vector in the inertial frame.

[0008] Furthermore, in S1, the cost function designed includes information from the accelerometer and magnetometer: ; in, , These are the weighting coefficients for the accelerometer and magnetometer, respectively. This indicates a vector inner product operation; a is the accelerometer measurement value, and m is the magnetometer measurement value.

[0009] Furthermore, in S3, the corrected angular velocity is calculated based on the parameters determined by the cost function. : ; in, For proportional gain; " is the cross product operator.

[0010] Furthermore, in S4, the corrected angular velocity is used to estimate and update the gyroscope zero bias online. The zero-bias update law is: ; in, For integral gain, For time.

[0011] Furthermore, in S5, the rotation matrix is ​​updated using the Rodriguez formula, including: 1) Construct the compensated angular velocity vector: ;in, Let ω be the angular velocity of the carrying system relative to the inertial frame. 2) The estimated equation for constructing the rotation matrix is: ; in, This represents the operation of converting a three-dimensional vector into an antisymmetric matrix; 3) Update the rotation matrix using the Rodriguez formula: The rate of change of the rotation matrix over time is equal to the product of the currently estimated rotation matrix and the antisymmetric matrix formed by the compensated angular velocity; ;in, The sampling time interval; The Rodriguez formula is used to calculate the matrix exponential mapping, thereby realizing the rotation matrix update in discrete time while maintaining orthogonality constraints.

[0012] Further, the variable optimization step; in this step, the variables to be optimized include the initial zero-bias estimate, the proportional gain, and the integral gain; The Nelder-Mead simplex method is used for variable optimization, with the goal of minimizing the cumulative cost function. The method constructs simplex vertices and performs reflection, expansion, contraction and compression operations to iteratively update the parameters until the convergence condition is met, and automatically finds the optimal initial zero bias and gain parameters.

[0013] Furthermore, while updating the rotation matrix using the Rodriguez formula and converting the compensated angular velocity into an incremental update of the rotation matrix through matrix exponential mapping to achieve the temporal evolution of attitude estimation, the Wahba problem is solved in parallel to obtain a drift-free reference rotation matrix, which is used to correct and fuse with the estimated attitude to suppress accumulated errors. The analytical solution to the Wahba problem is obtained by performing singular value decomposition on the matrix composed of accelerometer and magnetometer measurements. The reference rotation matrix is ​​equal to the product of the left singular vector matrix, the corrected diagonal matrix, and the transpose of the right singular vector matrix.

[0014] This invention also discloses an attitude estimation system employing the gyroscope attitude estimation and error compensation method described above, comprising: The model building module is used to build an attitude estimation model on the SO(3) group, represent the carrier attitude as a rotation matrix that satisfies orthogonal constraints, and design a cost function that includes accelerometer and magnetometer information. The data acquisition module is used to collect sensor data from the inertial measurement unit, including angular velocity measured by the gyroscope, gravity direction measured by the accelerometer, and geomagnetic field direction measured by the magnetometer. The complementary filtering module is used to calculate the correction angular velocity based on the cost function and the current sensor measurement value, and to convert the sensor measurement deviation into a correction amount on the SO(3) group using the vector cross product operation; The zero-bias compensation module is used to estimate and update the gyroscope zero bias online using the corrected angular velocity; The attitude update module is used to perform matrix exponential mapping calculation on the compensated angular velocity using the Rodrigues formula to update the rotation matrix; The output module is used to output the estimated attitude information and gyroscope zero bias.

[0015] Furthermore, it also includes: The parameter optimization module is used to minimize the cumulative cost function using the Nelder-Mead simplex method, thereby achieving automatic optimization of the initial zero bias and gain parameters.

[0016] Compared with the prior art, the present invention has at least the following advantages: 1. The attitude estimation model is directly established on the special orthogonal group SO(3). The attitude is updated by the exponential mapping of the rotation matrix. The orthogonality constraint is strictly maintained, avoiding the accumulation of quaternion normalization error and Euler angle singularity problem, thus improving numerical stability and computational accuracy. 2. By designing a cost function based on accelerometers and magnetometers, the gyroscope measurements for corrected angular velocity compensation are calculated, and the gyroscope bias is estimated and updated online, achieving real-time tracking and suppression of drift error; the estimation error is less than 0.5 degrees, and the drift is less than 2 degrees. 3. By integrating the high-frequency dynamic response of the gyroscope, the pitch and roll reference of the accelerometer, and the heading reference of the magnetometer, and adjusting the weighting coefficients, accurate attitude estimation can still be maintained in dynamic environments. 4. The Nelder-Mead simplex method is used to automatically optimize the initial zero bias and gain parameters, avoiding the tedious process of manual parameter tuning and improving the ease of use and adaptability of the algorithm. Attached Figure Description

[0017] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts. Figure 1This is a flowchart of the gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering in an embodiment of the present invention; Figure 2 This is a comparison diagram of roll angles in an embodiment of the present invention (unfiltered vs. error-type complementary filter vs. passive complementary filter). Figure 3 This is a comparison diagram of pitch angles in an embodiment of the present invention (unfiltered vs. error-type complementary filter vs. passive complementary filter). Figure 4 This is a comparison diagram of yaw angles in embodiments of the present invention (unfiltered vs. error-type complementary filter vs. passive complementary filter). Figure 5 This is a diagram showing the online estimation results of gyroscope zero bias in an embodiment of the present invention; Figure 6 This is a convergence curve of the cost function in an embodiment of the present invention; Figure 7 This is a schematic diagram showing the components and connections of the attitude estimation system in an embodiment of the present invention. Detailed Implementation

[0018] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and, together with the embodiments of the present invention, serve to illustrate the principles of the present invention.

[0019] One embodiment of the present invention discloses a gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering, such as... Figure 1 As shown, it includes: S1. Establish an attitude estimation model on the SO(3) group, express the carrier attitude as an estimated rotation matrix that satisfies orthogonal constraints, and design a cost function that includes accelerometer and magnetometer information. S2. Collect sensor data from the inertial measurement unit, including angular velocity measured by the gyroscope, gravity direction measured by the accelerometer, and geomagnetic field direction measured by the magnetometer. S3. Calculate the correction angular velocity based on the cost function and the current sensor measurement value, and use the vector cross product operation to convert the sensor measurement deviation into a correction amount on the SO(3) group; S4. The gyroscope zero bias is estimated and updated online using the corrected angular velocity to track and compensate for gyroscope drift in real time. S5. The compensated angular velocity is subjected to matrix exponential mapping operation through the Rodrigues formula to realize incremental update of the rotation matrix and maintain SO(3) manifold constraint. S6. Output the estimated attitude information and gyroscope zero bias; Repeat steps S2-S6 to continuously perform attitude estimation in real time.

[0020] Specifically, in S1, Define the normalized representation of the gravity reference vector and the geomagnetic field reference vector in the inertial frame. and , Define accelerometer measurements and magnetometer measurement values The projection of the corresponding reference vector onto the loading system; The accelerometer measurement corresponds to the component of the gravity vector in the load system after attitude rotation, and the magnetometer measurement corresponds to the component of the geomagnetic field vector in the load system after attitude rotation. Establish a realistic measurement model; , ; In the model, the true rotation matrix Let be the true rotation matrix from the carrier frame to the inertial frame, satisfying: and ; This refers to the accelerometer measurement, specifically the direction of gravity as measured by the accelerometer within the load system. The normalized representation of the gravity reference vector in the inertial frame is always... , This refers to the magnetometer measurement, specifically the direction of the Earth's magnetic field as measured by the magnetometer within the carrying system. This is the normalized representation of the geomagnetic field reference vector in the inertial frame.

[0021] Establish an attitude estimation model; in the model, the estimated rotation matrix Satisfying orthogonal constraints: and ;in, This represents the transpose of the estimated rotation matrix, used to rotate the inertial frame reference vector to the load system.

[0022] The attitude estimation model obtains the estimated gravity direction. And estimate the geomagnetic direction , and the measured value , The comparison is used to quantify the attitude estimation error.

[0023] In S1, the cost function designed includes information from the accelerometer and magnetometer: ; in, , These are the weighting coefficients for the accelerometer and magnetometer, respectively. This represents the vector inner product operation.

[0024] The cost function quantifies the attitude estimation error by measuring the deviation of the inner product between the reference vector under the estimated attitude and the corresponding sensor measured vector. When the attitude is accurately estimated, the inner product is 1 and the cost function is 0.

[0025] Specifically, in S2, the collected sensor data includes: The gyroscope measurement data is as follows: angular velocity of the carrying system relative to the inertial frame ; These are the angular velocities along the x, y, and z axes, respectively. Accelerometer measurement data consists of the specific force output in the measurement system, which is then low-pass filtered and normalized to obtain the unit vector of gravity. ; The magnetometer measurement data consists of the geomagnetic field strength within the measurement system, which, after hard iron / soft iron compensation and normalization, yields the unit vector of the geomagnetic direction. ; Preferably, the acquired sensor data is further preprocessed, including low-pass filtering to remove high-frequency vibration noise, temperature compensation to eliminate sensor temperature drift, and outlier removal to eliminate outliers, in order to improve data quality.

[0026] Specifically, in S3, the corrected angular velocity is calculated based on the parameters determined by the cost function. : ; in, For proportional gain; " is the cross product operator.

[0027] The corrected angular velocity uses vector cross product operation to convert the sensor measurement deviation into a correction quantity on the SO(3) group, realizing the complementary fusion of the low-frequency absolute reference provided by the accelerometer and magnetometer and the high-frequency dynamic information of the gyroscope.

[0028] Specifically, in S4, The corrected angular velocity is used to estimate and update the gyroscope zero bias online. The zero-bias update law is: ; in This is the integral gain.

[0029] The continuous-time expression for zero bias in a gyroscope is as follows: ;in, This is the initial value for zero bias of the gyroscope; This proportional-integral structure enables the zero-biased estimate to converge rapidly: When there is an error in the attitude estimation, a non-zero correction angular velocity ω is generated, which drives the zero-bias estimation to adjust towards the true value; Once the error is eliminated, ω approaches zero, and the zero-biased estimate remains stable.

[0030] Specifically, in S5, the rotation matrix is ​​updated using the Rodriguez formula, including: 1) Construct the compensated angular velocity vector: ; 2) The estimated equation for constructing the rotation matrix is: ; in, This represents the operation of converting a three-dimensional vector into an antisymmetric matrix.

[0031] 3) Update the rotation matrix using the Rodriguez formula: The rate of change of the rotation matrix over time is equal to the product of the currently estimated rotation matrix and the antisymmetric matrix formed by the compensated angular velocity; ;in, The sampling time interval; The Rodriguez formula is used to calculate the matrix exponential mapping, thereby realizing the rotation matrix update in discrete time while maintaining orthogonality constraints.

[0032] This update preserves the SO(3) manifold constraint, i.e., satisfies and This enables incremental updates of the rotation matrix.

[0033] Specifically, in S6, the output includes the estimated attitude information and the gyroscope zero bias. .

[0034] The attitude information includes roll angle, pitch angle, and yaw angle, all based on the estimated rotation matrix. The elements were calculated to obtain the result, where, Rotation matrix ; Roll angle ; Pitch angle ; Yaw angle .

[0035] Specifically, this embodiment also includes a variable optimization step; in this step, the optimized variables include the initial zero bias estimate, the proportional gain, and the integral gain; Preferably, the Nelder-Mead simplex method is used for parameter optimization, with the goal of minimizing the cumulative cost function. The method involves constructing simplex vertices and performing reflection, expansion, contraction, and compression operations to iteratively update the parameters until the convergence condition is met, thereby automatically finding the optimal initial zero bias and gain parameters.

[0036] Specifically, in this embodiment, while updating the rotation matrix using the Rodriguez formula and converting the compensated angular velocity into an incremental update of the rotation matrix through matrix exponential mapping to achieve the time evolution of attitude estimation, the Wahba problem is solved in parallel to obtain a drift-free reference rotation matrix, which is used to correct and fuse with the estimated attitude to suppress accumulated errors. The analytical solution to the Wahba problem is obtained by performing singular value decomposition on the matrix composed of accelerometer and magnetometer measurements. The reference rotation matrix is ​​equal to the product of the left singular vector matrix, the corrected diagonal matrix, and the transpose of the right singular vector matrix. A matrix composed of accelerometer and magnetometer measurements ; Reference rotation matrix for: ; In the formula, , Singular value decomposition The resulting left and right singular vector matrices, , Each is a matrix , The determinant, To correct the diagonal matrix.

[0037] In summary, compared with the prior art, the present invention has at least the following advantages: 1. The attitude estimation model is directly established on the special orthogonal group SO(3). The attitude is updated by the exponential mapping of the rotation matrix. The orthogonality constraint is strictly maintained, avoiding the accumulation of quaternion normalization error and Euler angle singularity problem, thus improving numerical stability and computational accuracy. 2. By designing a cost function based on accelerometers and magnetometers, the gyroscope measurement values ​​for corrected angular velocity compensation are calculated, and the gyroscope zero bias is estimated and updated online, thus realizing real-time tracking and suppression of drift error; 3. By integrating the high-frequency dynamic response of the gyroscope, the pitch and roll reference of the accelerometer, and the heading reference of the magnetometer, and adjusting the weighting coefficients, accurate attitude estimation can still be maintained in dynamic environments. 4. The Nelder-Mead simplex method is used to automatically optimize the initial zero bias and gain parameters, avoiding the tedious process of manual parameter tuning and improving the ease of use and adaptability of the algorithm.

[0038] To verify the effectiveness of the method of the present invention, a simulation experiment was conducted. The experimental data simulated the data acquired by the gyroscope, accelerometer, and magnetometer of an actual IMU, with a sampling period of T=0.01s and a simulation duration of 40 seconds.

[0039] (1) Roll Angle Estimation Results Figure 2 The comparison of roll angle estimation results is shown. The blue curve represents unfiltered data (pure gyro integral), the orange curve represents the result using the error-type complementary filter of this invention, and the green dashed line represents the result using the minimization-type passive filter. It can be seen that the orange curve and the green dashed line largely overlap, indicating that the method of this invention can effectively track roll angle changes with an estimation error of less than 0.5 degrees.

[0040] (2) Pitch estimation results Figure 3 The comparison of pitch angle estimation results is shown. The pitch angle exhibits significant dynamic changes (approximately ±15 degrees) during the simulation. The method of this invention can accurately track these changes, with the orange curve and green dashed line essentially overlapping, demonstrating good dynamic response performance.

[0041] (3) Yaw Angle Estimation Results Figure 4 The comparison of yaw angle estimation results is shown, which is the most effective indicator of gyroscope drift suppression. Unfiltered data (blue) drifted more than 150 degrees within 40 seconds due to the presence of zero gyroscope bias; while using the method of this invention (orange and green), the yaw angle remained near the true value, with a drift of less than 2 degrees, verifying the effectiveness of this invention in suppressing gyroscope drift.

[0042] (4) Gyroscope zero bias estimation results Figure 5 The online estimation results of the three-axis gyroscope's zero bias are shown. Solid lines represent estimated values, and dashed lines represent true values. As can be seen from the figure, the zero bias estimate converges to near the true value within approximately 5 seconds. The x-axis zero bias is approximately -3.3 deg / s (blue dashed line), the y-axis zero bias is approximately 0.3 deg / s (the red dashed line is covered by the estimated red solid line), and the z-axis zero bias is approximately 3.8 deg / s (green dashed line), consistent with the optimization results [-0.058, 0.005, 0.066] rad / s (i.e., [-3.3, 0.3, 3.8] deg / s) in the reference file.

[0043] (5) Cost function convergence curve Figure 6 The convergence process of the cost function is demonstrated. The cost function rapidly decreases from its initial value of approximately 0.5 and converges to a stable value of approximately 0.05 after about 10 seconds, indicating that the algorithm can quickly converge to the optimal attitude estimate.

[0044] Simulation results show that: (1) Zero drift suppression: The optimized error-type complementary filter can effectively suppress gyroscope zero drift. During the 40-second simulation time, the yaw angle drift of the unfiltered data exceeded 150 degrees, while the yaw angle drift was less than 2 degrees after using the method of this invention.

[0045] (2) Parameter optimization effect: Through the Nelder-Mead optimization algorithm, the initial zero bias was optimized from [0, 0, 0] to [-0.058, 0.005, 0.066] rad / s, and the cumulative cost function value was significantly reduced, showing obvious optimization effect.

[0046] (3) Dynamic response performance: During the movement of the carrier, the method of the present invention can accurately track attitude changes, and the estimation error of pitch angle and roll angle is less than 1 degree.

[0047] (4) Zero-bias estimation convergence: The zero-bias estimation of the gyroscope can converge to the true value in about 5 seconds. It has a fast convergence speed and high estimation accuracy.

[0048] This embodiment also discloses an attitude estimation system employing the gyroscope attitude estimation and error compensation method described above, such as... Figure 7 As shown, it includes: The model building module is used to build an attitude estimation model on the SO(3) group, represent the carrier attitude as a rotation matrix that satisfies orthogonal constraints, and design a cost function that includes accelerometer and magnetometer information. The data acquisition module is used to collect sensor data from the inertial measurement unit, including angular velocity measured by the gyroscope, gravity direction measured by the accelerometer, and geomagnetic field direction measured by the magnetometer. The complementary filtering module is used to calculate the correction angular velocity based on the cost function and the current sensor measurement value, and to convert the sensor measurement deviation into a correction amount on the SO(3) group using the vector cross product operation; The zero-bias compensation module is used to estimate and update the gyroscope zero bias online using the corrected angular velocity; The attitude update module is used to perform matrix exponential mapping calculation on the compensated angular velocity using the Rodrigues formula to update the rotation matrix; The output module is used to output the estimated attitude information and gyroscope zero bias.

[0049] Preferably, it also includes a parameter optimization module for minimizing the cumulative cost function using the Nelder-Mead simplex method, thereby achieving automatic optimization of the initial zero bias and gain parameters.

[0050] The specific technical features and corresponding beneficial effects of the system in this embodiment are the same as those of the method in the embodiment. Please refer to them for details, and they will not be repeated here.

[0051] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for gyroscope attitude estimation and error compensation based on SO(3) complementary filtering, characterized in that, include: S1. Establish an attitude estimation model on the SO(3) group, express the carrier attitude as an estimated rotation matrix that satisfies orthogonal constraints, and design a cost function that includes accelerometer and magnetometer information. S2. Collect sensor data from the inertial measurement unit, including angular velocity measured by the gyroscope, gravity direction measured by the accelerometer, and geomagnetic field direction measured by the magnetometer. S3. Calculate the correction angular velocity based on the cost function and the current sensor measurement value, and use the vector cross product operation to convert the sensor measurement deviation into a correction amount on the SO(3) group; S4. The gyroscope zero bias is estimated and updated online using the corrected angular velocity to track and compensate for gyroscope drift in real time. S5. The compensated angular velocity is subjected to matrix exponential mapping operation through the Rodrigues formula to realize incremental update of the rotation matrix and maintain SO(3) manifold constraint. S6. Output the estimated attitude information and gyroscope zero bias; Repeat steps S2-S6 to continuously perform attitude estimation in real time.

2. The gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering according to claim 1, characterized in that, In S1, the estimated rotation matrix is ​​part of the established attitude estimation model. Satisfying orthogonal constraints: and ;in, This represents the transpose of the estimated rotation matrix, used to rotate the inertial frame reference vector to the carrier frame; It is the identity matrix; Represents the determinant operation of a matrix; Represents a special orthogonal group in three dimensions; The estimated gravity direction obtained from the attitude estimation model Estimate the geomagnetic direction ; This is the normalized representation of the gravity reference vector in the inertial frame. This is the normalized representation of the geomagnetic field reference vector in the inertial frame.

3. The gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering according to claim 2, characterized in that, In S1, the cost function designed includes information from the accelerometer and magnetometer: ； in, , These are the weighting coefficients for the accelerometer and magnetometer, respectively. This indicates a vector inner product operation; a is the accelerometer measurement value, and m is the magnetometer measurement value.

4. The gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering according to claim 3, characterized in that, In S3, the corrected angular velocity is calculated based on the parameters determined by the cost function. : ； in, For proportional gain; " is the cross product operator.

5. The gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering according to claim 4, characterized in that, In S4, the corrected angular velocity is used to estimate and update the gyroscope zero bias online. The zero-bias update law is: ； in, For integral gain, For time.

6. The gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering according to claim 5, characterized in that, In S5, the rotation matrix is ​​updated using the Rodriguez formula, including: 1) Construct the compensated angular velocity vector: ;in, Let ω be the angular velocity of the carrying system relative to the inertial frame. 2) The estimated equation for constructing the rotation matrix is: ； in, This represents the operation of converting a three-dimensional vector into an antisymmetric matrix; 3) Update the rotation matrix using the Rodriguez formula: The rate of change of the rotation matrix over time is equal to the product of the currently estimated rotation matrix and the antisymmetric matrix formed by the compensated angular velocity; ;in, The sampling time interval; The Rodriguez formula is used to calculate the matrix exponential mapping, thereby realizing the rotation matrix update in discrete time while maintaining orthogonality constraints.

7. The gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering according to claim 6, characterized in that, Also includes: Variable optimization steps; In this step, the variables to be optimized include the initial zero-bias estimate, the proportional gain, and the integral gain; The Nelder-Mead simplex method is used for variable optimization, with the goal of minimizing the cumulative cost function. The method constructs simplex vertices and performs reflection, expansion, contraction and compression operations to iteratively update the parameters until the convergence condition is met, and automatically finds the optimal initial zero bias and gain parameters.

8. The gyroscope attitude estimation and error compensation method based on SO(3) complementary filtering according to claim 6, characterized in that, While using the Rodriguez formula to update the rotation matrix, the compensated angular velocity is converted into an incremental update of the rotation matrix through matrix exponential mapping to realize the time evolution of attitude estimation. At the same time, the Wahba problem is solved in parallel to obtain a drift-free reference rotation matrix, which is used to correct and fuse with the estimated attitude to suppress accumulated error. The analytical solution to the Wahba problem is obtained by performing singular value decomposition on the matrix composed of accelerometer and magnetometer measurements. The reference rotation matrix is ​​equal to the product of the left singular vector matrix, the corrected diagonal matrix, and the transpose of the right singular vector matrix.

9. An attitude estimation system employing the gyroscope attitude estimation and error compensation method as described in any one of claims 1-8, characterized in that, include: The model building module is used to build an attitude estimation model on the SO(3) group, represent the carrier attitude as a rotation matrix that satisfies orthogonal constraints, and design a cost function that includes accelerometer and magnetometer information. The data acquisition module is used to collect sensor data from the inertial measurement unit, including angular velocity measured by the gyroscope, gravity direction measured by the accelerometer, and geomagnetic field direction measured by the magnetometer. The complementary filtering module is used to calculate the correction angular velocity based on the cost function and the current sensor measurement value, and to convert the sensor measurement deviation into a correction amount on the SO(3) group using the vector cross product operation; The zero-bias compensation module is used to estimate and update the gyroscope zero bias online using the corrected angular velocity; The attitude update module is used to perform matrix exponential mapping calculation on the compensated angular velocity using the Rodrigues formula to update the rotation matrix; The output module is used to output the estimated attitude information and gyroscope zero bias.

10. The attitude estimation system according to claim 9, characterized in that, Also includes: The parameter optimization module is used to minimize the cumulative cost function using the Nelder-Mead simplex method, thereby achieving automatic optimization of the initial zero bias and gain parameters.

Images