An online estimation method for nonlinear bias of a micro-electro-mechanical system gyro

Abstract

The application discloses a kind of micro electro mechanical system gyroscopes nonlinear bias on-line estimation method, comprising the following steps: S1, using MEMS inertial sensor and external attitude sensor obtains gyroscope angular velocity and non-inertial attitude measurement value, and the static bias correction of gyroscope angular velocity measurement value is carried out, denoising and mapping preprocessing, and the mapping preprocessing of non-inertial attitude measurement value is carried out;S2, construct the nonlinear observer of on-line observation gyroscope bias, with the sensor information obtained in step S1 as input to carry out bias correction with attitude estimation and gyroscope bias estimation.The method is more in line with the actual situation for the assumption of model, and is more widely used, and has higher accuracy, robustness and stability than existing RHO, IEKF method.

Description

TECHNICAL FIELD

[0001] The present application relates to the integrated application of the fields of integrated navigation, information technology and automatic control, and in particular to a method for online estimation of the nonlinear bias of a micro-electro-mechanical system (MEMS) gyroscope using attitude information of an integrated navigation system. BACKGROUND

[0002] An inertial element (gyroscope and accelerometer) based on micro-electro-mechanical system (MEMS) technology and external auxiliary information (such as satellite navigation information, visual information, etc.) form a MEMS-based integrated navigation system, which is widely used in attitude and position measurement of various moving bodies (such as vehicles, pedestrians, robots, unmanned aerial vehicles, airships, etc.). With the rapid development of MEMS technology, navigation-grade MEMS inertial elements have emerged. However, in low-cost application scenarios, the precision of industrial-grade MEMS integrated navigation systems is still limited by the bias error in the measurement value of the MEMS gyroscope. The randomness error in the bias error of the MEMS gyroscope contains a large amount of sequential start bias and environmental coupling error. When the external auxiliary information is invalid, these random errors will cause the precision of the MEMS integrated navigation system to rapidly decrease. Therefore, online correction of the bias of the MEMS gyroscope is a key to improving the reliability and practicality of the MEMS integrated navigation system in low-cost application scenarios.

[0003] The multi-sensor information fusion method is one of the widely used online correction methods for the bias of the MEMS gyroscope. In this method, it is assumed that the linear dynamic model of the bias of the MEMS gyroscope and its noise characteristics are known, and a random state filter such as a nonlinear Kalman filter is used to realize online estimation and correction of the bias using external auxiliary information. In many studies, not small achievements have been made. Some studies have proposed a pose and gyroscope bias estimator based on a covariance inflation-multiplication extended Kalman filter, aiming at the initial alignment error of the pose, the uncertainty of the measurement value of the acceleration and magnetic field sensors. Some studies have proposed a robust pose and gyroscope bias estimator based on a two-step measurement update Kalman filter strategy. In addition, some studies have proposed a micro-inertial / visual integrated navigation algorithm for the random bias of the micro-inertial gyroscope coupled by vibration in the Mars star table constraint environment. Under the assumption that the Mars star table is planar, the pose measurement information of the unmanned aerial vehicle in the visual image is extracted, and the pose and gyroscope bias are estimated based on the extended Kalman filter. However, the integrated filter method is limited by the dynamic characteristics of the moving body and the task trajectory, and it is difficult to always ensure the observability of the bias of the MEMS gyroscope. At the same time, when the linear model of the bias of the MEMS gyroscope and the measurement noise characteristics are inaccurate, it is difficult to obtain satisfactory online correction results.

[0004] In recent years, in order to solve the problems of inaccurate measurement noise characteristics and poor anti-interference ability, a nonlinear observer is introduced into the MEMS gyro bias online correction process, which has shown good performance in many studies. Some people have proposed a motion body dynamics and kinematics model based on the attitude sliding mode observer for the problems of gyro bias dynamic linear model parameter variation and inaccurate measurement noise characteristics, which improves the robustness of gyro bias estimation. However, the attitude sliding mode observer has the problem of high accuracy requirement of motion body dynamics model. Some experts have proposed a nonlinear navigation observer based on the distance and attitude information output by the UWB and motion capture system under the condition of full state observability and trajectory acceleration constraint, which realizes the semi-global asymptotically stable estimation of linear gyro start-up bias. In addition, some people have proposed a semi-global stable nonlinear observer, which uses visual azimuth, velocity and other measurement information to realize the estimation of gyro bias and attitude. On this basis, some experts have developed a nonlinear observer for visual aided inertial navigation system, which ensures the almost global asymptotic stability and local exponential stability. Studies have shown that under the constraint conditions of known MEMS gyro bias dynamic linear model, high-precision observation information and specific flight trajectory, the gyro bias online correction method based on nonlinear observer has the advantages of small calculation amount, Lyapunov stability and robustness to uncertain disturbances. However, some experts point out that in practical application, the MEMS gyro bias characteristics coupled with environmental factors are difficult to describe with a specific linear model. To our knowledge, under the condition that the dynamic model of MEMS gyro bias is unknown, the gyro bias online correction method based on nonlinear observer has not been discussed. SUMMARY

[0005] In order to solve the above-mentioned problems of the prior art and improve the precision of low-cost integrated navigation system, a micro-electromechanical gyro nonlinear bias online estimation method is provided. The method uses commercial MEMS inertial sensors and external attitude sensors to obtain gyro angular velocity and non-inertial attitude measurement values, and proposes a nonlinear observer for online observation of gyro bias. The obtained sensor information is used as input for bias-corrected attitude estimation and gyro bias estimation, and is applied to a micro-inertial integrated navigation platform. The performance of the estimation method is tested in dynamic experiments. The specific technical scheme of the present application is as follows:

[0006] A micro-electromechanical gyro nonlinear bias online estimation method, comprising the following steps:

[0007] S1: using MEMS inertial sensors and external attitude sensors to obtain gyro angular velocity and non-inertial attitude measurement values, and performing static bias correction, denoising and mapping preprocessing on the gyro angular velocity measurement values, and performing mapping preprocessing on the non-inertial attitude measurement values;

[0008] S2: Construct a nonlinear observer for online observation of gyroscope bias, and use the sensor information obtained in step S1 as input to perform attitude estimation and gyroscope bias estimation with bias correction.

[0009] Furthermore, step S1 includes the following sub-steps:

[0010] S1-1: Construct a micro-inertial integrated navigation platform equipped with MEMS inertial sensors and external attitude sensors;

[0011] S1-2: Design the desired trajectory, complete the dynamic experiment, and use MEMS inertial sensors and external attitude sensors to collect gyro angular velocity and non-inertial attitude measurement values ​​in real time.

[0012] S1-3: Based on the moment of motion, extract the output of the gyroscope in the initial static phase of the gyroscope angular velocity in step S1-2, regard the average value of the gyroscope output in this phase as the initial deterministic bias of the MEMS gyroscope, and perform static bias correction on the gyroscope angular velocity in the motion phase.

[0013] S1-4: Use a moving average filter to remove vibration noise from the processing result of step S1-3;

[0014] S1-5: Define a hypercomplex mapping R = H(r), where H: In three-dimensional Euclidean space, r = [r x r y r z ] T For any three-dimensional space vector, R = 0 + ir x +jr y +kr z The quaternion after hypercomplex mapping, where i, j, k are imaginary units; the non-inertial attitude λ from step S1-2 is represented by the hypercomplex mapping H. c And the gyro angular velocity in steps S1-4 These are respectively mapped to the non-inertial attitude measurement quaternion Λ c Quaternion of gyroscope measurement value .

[0015] Furthermore, step S2 includes the following sub-steps:

[0016] S2-1: Taking the moment of motion initiation as the initial moment, determine the integration time t based on the minimum sampling time of the data from step S1-2, where t = 1, 2, ..., where the attitude estimation quaternion Λ is the initial time 0. o0 The value is equal to the non-inertial measurement quaternion Λ at the initial time 0. c0 The initial gyroscope bias estimate at time 0 satisfies

[0017] S2-2: Extract the non-inertial attitude measurement quaternion Λ at time t in step S1-5. ct The attitude estimation quaternion Λ at time (t-1) o(t-1) Taking the inverse, the attitude estimation feedback term e at time t is calculated according to the following formula. t :

[0018]

[0019] Where the subscript t or (t-1) represents the integration time, Let be the attitude estimation error quaternion. for The real part, for The vector part, Representative attitude estimation feedback term;

[0020] S2-3: Amplify the processing result of step S2-2 to obtain the attitude estimation correction term ke. t Simultaneously extract the processing results from steps S1-4 at time t. and the estimated gyroscope bias at time (t-1) The attitude estimation correction term ke at time t t gyroscope measurement at time t The estimated gyroscope bias at time (t-1) These three factors jointly drive the Poisson equation for attitude estimation, as shown in the following equation:

[0021]

[0022] This enables attitude estimation with bias correction, yielding the attitude estimation quaternion Λ at time t. ot ;

[0023] S2-4: Define the mapping Where h: Γ is any quaternion Γ = τ0 + iτ1 + jτ2 + kτ3, The column vector corresponding to this quaternion The processing result Λ of step S2-3 is mapped using the mapping h. ot Mapped to the attitude estimation quaternion vector λ at time t ot After passing through a first-order nonlinear feedforward amplification stage, it is as follows:

[0024]

[0025] The estimate of the gyroscope bias product coupling component at time t is obtained. in, It is the gain matrix of the first-order nonlinear feedforward amplifier at time t;

[0026] S2-5: Extract the quaternion of the gyroscope measurement value from step S1-5 at time t. The attitude estimation quaternion vector λ in step S2-4 ot Components coupled with gyroscope bias product The attitude-angular velocity driving term u at time t is synthesized together. t The input is fed into a first-order nonlinear feedback system to obtain an estimate of the gyroscope bias integral coupling component at time t. As shown in the following formula

[0027]

[0028] in, Let u be the gain matrix of the first-order nonlinear feedback system at time t. t ∈R 4 ;

[0029] S2-6: Extract the processing results of steps S2-4 and S2-5 at time t, and estimate the coupled components of the gyroscope bias product. Estimation of components coupled with gyroscope bias integral The summation yields the quaternion of the gyroscope bias estimate at time t. The gyroscope bias estimate at time t is obtained by using the inverse mapping after combining h and H. That is, satisfy

[0030]

[0031] S2-7: Repeat steps S2-2 to S2-6 until attitude estimation and gyroscope bias estimation with bias correction are completed at all time points.

[0032] The beneficial effects of this invention compared to the prior art are as follows:

[0033] 1. This invention designs an observer for online observation of the bias of commercial-grade MEMS gyroscopes using an external attitude sensor. Compared to the Robust Hybrid Attitude and Gyro-Bias Observer (RHO) and Iterated Extended Kalman Filter (IEKF) methods, which assume a linear correlation between the body's attitude and gyroscope drift, the proposed NRBO (Nonlinear Robust Bias Observer) method relaxes the model requirements for gyroscope drift. It no longer considers the gyroscope bias as a constant, but rather as a nonlinear function of the body's attitude. This makes the NRBO method's model assumptions more consistent with reality and its applicability wider.

[0034] 2. This invention designs the nonlinear model of gyroscope drift into two parts: a product-coupled component of gyroscope bias and an integral-coupled component of gyroscope bias. Online estimation of the gyroscope nonlinear bias can be achieved by adjusting the feedforward gain matrix and the feedback gain matrix. Compared to previous RHO and IEKF methods, the NRBO method demonstrates higher accuracy, robustness, and stability.

[0035] 3. The NRBO method proposed in this invention can be used for online estimation of accelerometer nonlinear bias, thereby further improving the accuracy of low-cost microelectromechanical integrated navigation systems. Attached Figure Description

[0036] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings used in the embodiments will be briefly described below. Referring to the accompanying drawings will provide a clearer understanding of the features and advantages of the present invention. The drawings are illustrative and should not be construed as limiting the present invention in any way. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort. Wherein:

[0037] Figure 1 This is a flowchart of an online estimation method for nonlinear bias of a microelectromechanical gyroscope according to the present invention;

[0038] Figure 2 This is a configuration diagram of a cable parallel robot platform according to an embodiment of the present invention;

[0039] Figure 3 It is an end effector of a cable-parallel robot according to an embodiment of the present invention;

[0040] Figure 4 This is a motion trajectory diagram of an end effector acquired by an external sensor according to an embodiment of the present invention;

[0041] Figure 5 This is an end effector attitude diagram acquired by an external sensor according to an embodiment of the present invention;

[0042] Figure 6 This is a data acquisition diagram from a MEMS inertial sensor according to an embodiment of the present invention;

[0043] Figure 7 This is a comparison of attitude estimation results using four attitude estimation methods: GI, RHO, IEKF, and NRBO, according to an embodiment of the present invention.

[0044] Figure 8 These are the root mean square error results of four methods for attitude error according to an embodiment of the present invention;

[0045] Figure 9The gyroscope bias is estimated using the RHO method according to an embodiment of the present invention;

[0046] Figure 10 The gyroscope bias is estimated using the IEKF method according to an embodiment of the present invention;

[0047] Figure 11 The gyroscope bias is estimated using the NRBO method according to an embodiment of the present invention;

[0048] Figure 12 The root mean square error results of the attitude errors of the RHO_GI, IEKF_GI and NRBO_GI methods according to an embodiment of the present invention are; Detailed Implementation

[0049] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other.

[0050] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.

[0051] This invention provides a nonlinear observer for online observation of MEMS gyroscope bias using non-inertial attitude measurements. Motivated by improving accuracy and robustness, it classifies and models the factors affecting gyroscope bias, achieving "high-quality" gyroscope bias correction, thereby improving the reliability and practicality of low-cost integrated navigation systems. Non-inertial attitude measurements can originate from visual navigation systems, MARG (Magnetic, Angular Rate, and Gravity) systems, or integrated navigation systems. The non-inertial attitude measurement quaternion and the gyroscope's measured angular velocity are used as inputs to the MEMS gyroscope bias nonlinear observer, first completing attitude estimation with bias correction. Then, the estimated attitude is input to the feedforward loop of the nonlinear amplification stage to estimate the gyroscope bias product coupling component. The amplification gain of this stage is designed to ensure that the instability of the gyroscope bias and attitude estimation errors during carrier motion are not affected, without impacting the robustness of the gyroscope bias observation results. Finally, the angular velocity measured by the MEMS gyroscope, the estimated attitude, and the gyroscope bias product coupling component are input into a first-order nonlinear feedback system to obtain the gyroscope bias integral coupling component. Adjusting the first-order nonlinear feedback gain can guarantee the global asymptotic stability of the state observer.

[0052] Specifically, such as Figure 1 The diagram shows a flowchart of an online method for estimating the nonlinear bias of a microelectromechanical gyroscope according to the present invention.Figure 2 As shown in the configuration diagram of a cable-parallel robot platform according to an embodiment of the present invention, an online estimation method for nonlinear bias of a microelectromechanical gyroscope includes the following steps:

[0053] S1: Use commercial-grade MEMS inertial sensors and external attitude sensors to acquire gyro angular velocity and non-inertial attitude measurement values, and perform static bias correction, noise reduction and mapping preprocessing on the gyro angular velocity measurement values, and perform mapping preprocessing on the non-inertial attitude measurement values.

[0054] Preferably, the specific steps of step S1 are as follows:

[0055] S1-1: Design a micro-inertial integrated navigation platform equipped with MEMS inertial sensors and external attitude sensors.

[0056] S1-2: Design the desired trajectory, complete the dynamic experiment, and use MEMS inertial sensors and external attitude sensors to collect gyro angular velocity and non-inertial attitude measurement values ​​in real time.

[0057] S1-3: Based on the moment of motion, extract the output of the gyroscope during the initial static phase of the gyroscope angular velocity in step S1-2, and take the average value of the gyroscope output during this phase as the initial deterministic bias of the MEMS gyroscope. Perform static bias correction on the gyroscope angular velocity during the motion phase.

[0058] S1-4: Remove vibration noise from the processing results of step S1-3 using a moving average filter with a window length of 10.

[0059] S1-5: Define a hypercomplex mapping R = H(r), where H: In three-dimensional Euclidean space, r = [r x r y r z ] T For any three-dimensional space vector, R = 0 + ir x +jr y +kr z Let be the quaternion after the hypercomplex mapping, where i, j, k are the imaginary units. The non-inertial attitude λ from step S1-2 is then mapped using the hypercomplex mapping H. c And the gyro angular velocity in steps S1-4 These are respectively mapped to the non-inertial attitude measurement quaternion Λ c Quaternion of gyroscope measurement value .

[0060] S2: Construct a nonlinear observer for online observation of gyroscope bias, and use the sensor information obtained in step S1 as input to perform attitude estimation and gyroscope bias estimation with bias correction.

[0061] Preferably, the specific steps of step S2 are as follows:

[0062] S2-1: Taking the moment of motion initiation as the initial moment, determine the integration time t based on the minimum sampling time of the data from step S1-2, where t = 1, 2, ..., where the attitude estimation quaternion Λ is the initial time 0. o0 The value is equal to the non-inertial measurement quaternion Λ at the initial time 0. c0 The initial gyroscope bias estimate at time 0 satisfies The attitude and gyroscope bias are estimated hour by hour, starting from time 1.

[0063] S2-2: Extract the non-inertial attitude measurement quaternion Λ at time t in step S1-5. ct The attitude estimation quaternion Λ at time (t-1) o(t-1) Taking the inverse, the attitude estimation feedback term e at time t is calculated according to the following formula. t :

[0064]

[0065] Wherein, the subscript t or (t-1) represents the integration time divided in step S2-1. Let be the attitude estimation error quaternion, which is the inverse of the attitude estimation quaternion. Non-inertial measurement quaternion Λ c Quaternion product, for The real part, for The vector part, This represents the attitude estimation feedback term.

[0066] S2-3: Amplify the processing result of step S2-2 to obtain the attitude estimation correction term ke. t Simultaneously extract the processing results from steps S1-4 at time t. and the estimated gyroscope bias at time (t-1) The attitude estimation correction term ke at time t t gyroscope measurement at time t The estimated gyroscope bias at time (t-1) These three factors jointly drive the Poisson equation for attitude estimation, as shown in the following equation:

[0067]

[0068] This enables attitude estimation with bias correction, yielding the attitude estimation quaternion Λ at time t. ot .

[0069] S2-4: Define the mapping Where h: Γ is any quaternion Γ = τ0 + iτ1 + jτ2 + kτ3, The column vector corresponding to this quaternion The processing result Λ of step S2-3 is mapped using the mapping h. ot Mapped to the attitude estimation quaternion vector λ at time t ot After passing through a first-order nonlinear feedforward amplification stage, it is as follows:

[0070]

[0071] The estimate of the gyroscope bias product coupling component at time t is obtained. in, It is the gain matrix of the first-order nonlinear feedforward amplifier at time t.

[0072] S2-5: Extract the quaternion of the gyroscope measurement value from step S1-5 at time t. The attitude estimation quaternion vector λ in step S2-4 ot Components coupled with gyroscope bias product The attitude-angular velocity driving term u at time t is synthesized together. t The input is fed into a first-order nonlinear feedback system to obtain an estimate of the gyroscope bias integral coupling component at time t. The specific formula is as follows.

[0073]

[0074] in, Let u be the gain matrix of the first-order nonlinear feedback system at time t. This matrix has one zero eigenvalue and three positive eigenvalues. t ∈R 4 .

[0075] S2-6: Extract the processing results of steps S2-4 and S2-5 at time t, and estimate the coupled components of the gyroscope bias product. Estimation of components coupled with gyroscope bias integral The summation yields the quaternion of the gyroscope bias estimate at time t. The gyroscope bias estimate at time t is obtained by using the inverse mapping after combining h and H. That is, satisfy

[0076]

[0077] in, This indicates the composition of mappings.

[0078] S2-7: Repeat steps S2-2 to S2-6 until attitude estimation and gyroscope bias estimation with bias correction are completed at all time points.

[0079] Based on this, the present invention utilizes a micro-inertial integrated navigation platform to conduct dynamic experiments under the influence of the carrier's own vibration environment, further verifying the observation accuracy of the nonlinear observer. The specific method employed is as follows: dynamic experiments are conducted using a micro-inertial integrated navigation platform equipped with MEMS inertial sensors and external attitude sensors. The output value of the high-precision external attitude sensor is used as the attitude reference value, and the accuracy of four attitude estimation methods is evaluated using the raw data from steps S1-2, including Gyro Integrating (GI), Robust Hybrid Observer (RHO), Iterative Extended Kalman Filter (IEKF), and the method proposed in this invention (NRBO). Since the true value of the gyro bias is unavailable, it is difficult to directly determine the gyro bias estimation accuracy of the RHO, IEKF, and NRBO methods. However, considering that the gyro output, after being corrected for the true gyro bias, can be input into the GI algorithm to obtain an accurate attitude, this method is feasible. Therefore, the gyroscope biases estimated by the RHO, IEKF, and NRBO methods are used as real-time corrections for the gyroscope inputs of the GI method. The accuracy of the gyroscope biases of the RHO, IEKF, and NRBO methods is indirectly judged based on the attitude calculated by the RHO_GI, IEKF_GI, and NRBO_GI methods.

[0080] The following specific embodiment illustrates the online estimation method for nonlinear bias of microelectromechanical gyroscopes according to the present invention and its accuracy verification.

[0081] The online estimation method for nonlinear bias of microelectromechanical gyroscopes relies on high-precision measurements from external attitude sensors. Therefore, a cable-parallel robot platform equipped with a MEMS inertial sensor and the OptiTrack motion capture system was designed to conduct dynamic experiments under the influence of the platform's own vibration. The cable-parallel robot experimental platform was chosen as the micro-inertial integrated navigation platform in this embodiment. This platform consists of a motor, a flexible cable, an end effector, etc. Figure 2 As shown. The motion control actuators of the cable-parallel robot are ESTUN servo motors. Four motors are located at the vertices of a 4.8m × 3.8m rectangle, 2.5m above the ground. Each motor is connected to the end effector via a flexible cable, as shown. Figure 3 As shown, a WCG-1 IMU developed by the Tsinghua Micromechanical Gyroscope Laboratory was installed on the end effector. Two Flex 13 motion capture cameras were installed between each adjacent motor, for a total of eight cameras forming the OptiTrack motion capture system. The specifications of the WCG-1 IMU and the OptiTrack motion capture system are shown in Table 1.

[0082] Table 1 Technical parameters of WCG-1 and OptiTrack systems

[0083]

[0084] By controlling a flexible cable with a motor, an end effector can complete arbitrary trajectory movements within the workspace. In this experiment, the end effector of the cable-connected robot was controlled to complete an approximate "Z" shaped trajectory. The end effector's movement lasted a total of 347.465 seconds. For the first 18 seconds, the end effector was stationary. From 18 to 25 seconds, the end effector moved vertically upwards. From 25 to 168 seconds, the end effector completed the "Z" shaped trajectory. From 168 to 320 seconds, the end effector returned to the starting point along the "Z" shaped trajectory, pausing at the starting point for 27 seconds before the experiment ended. At 25, 70, 121, 168, 215, 265, 311, and 320 seconds, the end effector reached different turning points on the "Z" shaped trajectory, each pausing for 2 to 3 seconds. During the end effector's movement, the OptiTrack system collected data at a frame rate of 120Hz. The captured end effector position and attitude information are shown below. Figure 4 , Figure 5 As shown; the WCG-1 model IMU acquires data at a frequency of 200Hz, and the output angular velocity information is as follows. Figure 6 As shown.

[0085] Depend on Figure 4 As can be seen, point A is the starting point, and points B, C, D, and E are the turning points. The end effector precisely completed the "Z" shaped trajectory movement. Figure 5 It can be seen that there are slight jumps in the attitude values ​​at 18s, 120s, 200s, and 230s. Spectral analysis reveals low-frequency noise of approximately 6Hz. This is because the cable-parallel robot is underconstrained and the flexible cable is elastic, indicating that the end effector's motion is in a vibration environment. Figure 6 It can be seen that the MEMS sensor output is almost zero at times of 25s, 70s, 121s, 168s, 215s, 265s, 311s, and 320s. This is because the end effector stops at turning points B, C, D, E, D, C, B, and A, respectively. Figure 6 Spectral analysis of the inertial information revealed that there was also low-frequency noise of about 6Hz in the output of the MEMS sensor, confirming that the end effector equipped with the MEMS sensor completed the predetermined trajectory under vibration.

[0086] Four attitude estimation algorithms, including GI, RHO, IEKF, and NRBO, were evaluated using experimental data. Considering that compensating for static gyroscope bias can effectively improve the convergence speed of these four algorithms, the mean value of the gyroscope output under a static base from 0 to 18 seconds was used as the static compensation value for the gyroscope bias, and a moving average filter with a window length of 10 was used for preprocessing.

[0087] The gain parameter k of the RHO algorithm is set to 2, and the nonlinear parameter of the NRBO algorithm is set to...

[0088]

[0089]

[0090] in and gyroscope measurement value The average value of the triaxial angular velocity after preprocessing.

[0091] Under these parameters, the end effector pose estimated by the four algorithms is as follows: Figure 7 As shown, the root mean square error (RMSE) of the attitude is as follows: Figure 8 As shown. By Figure 7 , Figure 8 It can be seen that the GI algorithm estimated drift in all three axes, with the most severe yaw angle RMSE exceeding 2°. The RHO, IEKF, and NRBO algorithms tracked the reference values ​​better, with all three algorithms achieving RMSEs below 0.5°. This is due to gyro drift during carrier motion, proving that the attitude accuracy of the RHO, IEKF, and NRBO algorithms is higher than that of the GI algorithm. Furthermore, from... Figure 7 Enlarged view of a part Figure 8 It can be seen that, compared with the RHO and IEKF algorithms, the NRBO algorithm estimates the three-axis pose more closely to the reference value and has a smaller RMSE. This proves that when the pose estimation of the RHO, IEKF and NRBO algorithms is relatively accurate, the NRBO algorithm performs better.

[0092] The RHO, IEKF, and NRBO algorithms, while estimating relatively accurate attitudes, can also estimate gyroscope biases online, as shown below. Figure 9 , Figure 10 and Figure 11 As shown. By Figures 9-11 It can be seen that during the ascent of the end effector, the elasticity of the flexible cable causes significant swaying of the end effector. The estimated gyroscope bias exhibits considerable fluctuation during this stage, indicating that the gyroscope bias is sensitive to the motion state of the carrier, verifying the viewpoint pointed out by experts that "commercial-grade MEMS exhibit dynamic sensitivity." Furthermore, the gyroscope bias changes continuously throughout the entire motion of the end effector, contradicting the assumption in the RHO algorithm that the gyroscope bias has a zero time derivative. Therefore, it is difficult to describe gyroscope drift using a linear model.

[0093] Since the true value of the gyroscope bias is unavailable, it is difficult to directly determine the bias value. Figures 9-11The accuracy of gyroscope bias estimation using the RHO, IEKF, and NRBO algorithms is assessed. However, some experts point out that after the gyroscope output is corrected for the actual gyroscope bias, it can be input into the GI algorithm to obtain a precise attitude. Therefore, the process of using the gyroscope bias estimated by the RHO, IEKF, and NRBO algorithms to correct the gyroscope output in real time, and then integrating the corrected gyroscope output to obtain the attitude, is respectively called the RHO_GI, IEKF_GI, and NRBO_GI algorithms. By comparing the attitude errors estimated by the RHO_GI, IEKF_GI, and NRBO_GI algorithms, the accuracy of the gyroscope bias in the RHO, IEKF, and NRBO algorithms can be indirectly demonstrated. Using the high-precision end effector attitude captured by the Optitrack system as a reference, the root mean square error (RMSE) of the attitude estimated by the RHO_GI, IEKF_GI, and NRBO_GI algorithms is as follows: Figure 12 As shown.

[0094] Depend on Figure 12 It can be seen that the root mean square error of the three-axis attitude of the RHO_GI algorithm exceeds 10°, and the attitude accuracy is actually worse than that of the RHO_GI algorithm. Figure 8 The GI algorithm indicates overcompensation during the gyroscope output correction process, meaning the RHO algorithm has significant room for improvement in online gyroscope bias estimation accuracy. The IEKF_GI algorithm's root mean square error for three-axis attitude is between 0.6° and 3°, with the end effector roll and pitch angles showing lower accuracy than... Figure 8 The GI algorithm in the text indicates that the IEKF algorithm's estimation of gyro biases in these two axes is inaccurate, resulting in overcompensation. In contrast, the NRBO_GI algorithm achieves root mean square errors of less than 0.6° for all three axes, representing an average improvement of approximately 70 times over the RHO_GI algorithm, 3 times over the IEKF_GI algorithm, and significantly better than... Figure 8 The GI algorithm in the paper improves upon the performance by 1.2 times, which demonstrates the superiority of the NRBO algorithm in online estimation of gyroscope bias when the gyroscope drift model is unknown, and verifies the hypothesis of this invention.

Claims

1. A method for online estimation of nonlinear bias in a microelectromechanical gyroscope, characterized in that, Includes the following steps: S1: Use MEMS inertial sensors and external attitude sensors to obtain gyro angular velocity and non-inertial attitude measurement values, and perform static bias correction, noise reduction and mapping preprocessing on the gyro angular velocity measurement values, and perform mapping preprocessing on the non-inertial attitude measurement values. S2: Construct a nonlinear observer for online observation of gyroscope bias, and use the sensor information obtained in step S1 as input to perform attitude estimation and gyroscope bias estimation with bias correction; Step S1 includes the following sub-steps: S1-1: Construct a micro-inertial integrated navigation platform equipped with MEMS inertial sensors and external attitude sensors; S1-2: Design the desired trajectory, complete the dynamic experiment, and use MEMS inertial sensors and external attitude sensors to collect gyro angular velocity and non-inertial attitude measurement values ​​in real time. S1-3: Based on the moment of motion, extract the output of the gyroscope in the initial static phase of the gyroscope angular velocity in step S1-2, regard the average value of the gyroscope output in this phase as the initial deterministic bias of the MEMS gyroscope, and perform static bias correction on the gyroscope angular velocity in the motion phase. S1-4: Use a moving average filter to remove vibration noise from the processing result of step S1-3; S1-5: Define a hypercomplex mapping R = H(r), where In three-dimensional Euclidean space, r = [r x r y r z ] T For any three-dimensional space vector, R = 0 + ir x +jr y +kr z The quaternion after hypercomplex mapping, where i, j, k are imaginary units; the non-inertial attitude λ from step S1-2 is represented by the hypercomplex mapping H. c And the gyro angular velocity in steps S1-4 These are respectively mapped to the non-inertial attitude measurement quaternion Λ c Quaternion of gyroscope measurement value Step S2 includes the following sub-steps: S2-1: Taking the moment of motion initiation as the initial moment, determine the integration time t based on the minimum sampling time of the data from step S1-2, where t = 1, 2, ..., where the attitude estimation quaternion Λ is the initial time 0. o0 The value is equal to the non-inertial measurement quaternion Λ at the initial time 0. c0 The initial gyroscope bias estimate at time 0 satisfies S2-2: Extract the non-inertial attitude measurement quaternion Λ at time t in step S1-5. ct The attitude estimation quaternion Λ at time (t-1) o(t-1) Taking the inverse, the attitude estimation feedback term e at time t is calculated according to the following formula. t : Where the subscript t or (t-1) represents the integration time, Let be the attitude estimation error quaternion. for The real part, for The vector part, Representative attitude estimation feedback term; S2-3: Amplify the processing result of step S2-2 to obtain the attitude estimation correction term ke. t Simultaneously extract the processing results from steps S1-4 at time t. and the estimated gyroscope bias at time (t-1) The attitude estimation correction term ke at time t t gyroscope measurement at time t The estimated gyroscope bias at time (t-1) These three factors jointly drive the Poisson equation for attitude estimation, as shown in the following equation: This enables attitude estimation with bias correction, yielding the attitude estimation quaternion Λ at time t. ot ; S2-4: Define the mapping in Γ is any quaternion The column vector corresponding to this quaternion Use mapping h to map the processing result Λ of step S2-3. ot Mapped to the attitude estimation quaternion vector λ at time t ot After passing through a first-order nonlinear feedforward amplification stage, it is as follows: The estimate of the gyroscope bias product coupling component at time t is obtained. in, It is the gain matrix of the first-order nonlinear feedforward amplifier at time t; S2-5: Extract the quaternion of the gyroscope measurement value from step S1-5 at time t. The attitude estimation quaternion vector λ in step S2-4 ot Components coupled with gyroscope bias product The attitude-angular velocity driving term u at time t is synthesized together. t The input is fed into a first-order nonlinear feedback system to obtain an estimate of the gyroscope bias integral coupling component at time t. As shown in the following formula in, Let u be the gain matrix of the first-order nonlinear feedback system at time t. t ∈R 4 ; S2-6: Extract the processing results of steps S2-4 and S2-5 at time t, and estimate the coupled components of the gyroscope bias product. Estimation of components coupled with gyroscope bias integral The summation yields the quaternion of the gyroscope bias estimate at time t. The gyroscope bias estimate at time t is obtained by using the inverse mapping after combining h and H. That is, satisfy S2-7: Repeat steps S2-2 to S2-6 until attitude estimation and gyroscope bias estimation with bias correction are completed at all time points.

Images