A dynamic adaptive pose estimation method based on inertial sensors

Abstract

The application discloses a dynamic self-adaptive attitude estimation method based on an inertial sensor, and comprises the following steps: acquiring carrier maneuvering state data based on an inertial sensor and an attitude estimation result; acquiring adaptive compensation coefficients and gyroscope zero offset estimation data from the carrier maneuvering state data; processing the adaptive compensation coefficients and the gyroscope zero offset estimation data to generate a complementary filtering algorithm; and acquiring an adaptive attitude estimation result based on the complementary filtering algorithm. Under static conditions, the method can achieve good attitude estimation effect; under dynamic conditions, the method has higher precision than traditional complementary filtering; and the method has smaller influence from maneuvering than traditional complementary filtering.

Description

A Dynamic Adaptive Attitude Estimation Method Based on Inertial Sensors Technical Field

[0001] This invention belongs to the field of sensor adaptive estimation, and in particular relates to a dynamic adaptive attitude estimation method based on inertial sensors. Background Technology

[0002] Inertial sensor-based attitude measurement systems are widely used in motion capture, virtual reality, robotics, and other fields due to their autonomous, low-cost, and compact characteristics. They obtain the horizontal attitude angle of the vehicle by fusing the outputs of gyroscopes and accelerometers.

[0003] While gyroscopes provide high accuracy for attitude angles obtained through short-time integration, they accumulate errors during prolonged motion. When the vehicle is stationary or in uniform motion, accelerometer errors, such as zero bias, are disregarded, as the accelerometer only senses gravitational acceleration, allowing for accurate calculation of the horizontal attitude angle. However, in motion, the presence of motion acceleration increases the error in the calculated horizontal attitude angle. Therefore, improving the attitude estimation accuracy of the vehicle under prolonged motion by utilizing information from gyroscopes and accelerometers is a crucial research area for attitude measurement systems. Summary of the Invention

[0004] The purpose of this invention is to provide a dynamic adaptive attitude estimation method based on inertial sensors to solve the problems existing in the prior art.

[0005] To achieve the above objectives, this invention provides a dynamic adaptive attitude estimation method based on inertial sensors, comprising:

[0006] The vehicle's maneuvering state data is obtained based on inertial sensor and attitude estimation results;

[0007] Adaptive compensation coefficients and gyroscope zero-bias estimation data are obtained from the carrier's maneuvering state data;

[0008] The adaptive compensation coefficients and gyroscope bias estimation data are processed to generate a complementary filtering algorithm;

[0009] The adaptive attitude estimation result is obtained based on the complementary filtering algorithm.

[0010] Preferably, the process of acquiring the carrier maneuver status data includes:

[0011] Acquire inertial sensor data and attitude estimation results from the previous moment;

[0012] The current maneuvering state of the vehicle is evaluated based on the inertial sensor data and the attitude estimation result of the previous moment, thereby obtaining the vehicle maneuvering state data.

[0013] Preferably, the process of assessing the current maneuvering state of the carrier includes:

[0014] Obtain the magnitude values ​​of the accelerometer output and the gyroscope output;

[0015] A preset threshold is set, and the magnitude values ​​output by the accelerometer and the gyroscope are calculated and compared with the preset threshold to divide the current maneuvering state of the carrier into a non-maneuvering state and a maneuvering state.

[0016] The pitch angle error estimate and roll angle error estimate are obtained, and the maneuvering state is evaluated based on the pitch angle error estimate and roll angle error estimate to obtain the maneuvering state data of the vehicle.

[0017] Preferably, the process of obtaining the adaptive compensation coefficient includes:

[0018] Obtain the angle between the theoretical gravitational acceleration output and the actual output of the accelerometer;

[0019] Select a compensation coefficient and a compensation threshold, and obtain the adaptive compensation coefficient based on the compensation coefficient, the compensation threshold, and the included angle.

[0020] Preferably, the process of acquiring gyroscope bias estimation data includes:

[0021] Based on a preset threshold and the carrier's maneuvering state data, the magnitude values ​​output by the accelerometer and the gyroscope are judged to obtain a judgment result.

[0022] Based on the judgment result, the gyroscope output is stored in a sliding window for collection and distribution to obtain the zero bias of the gyroscope;

[0023] The zero bias of the gyroscope is updated to obtain the zero bias estimation data of the gyroscope.

[0024] Preferably, the process of generating the complementary filter algorithm includes:

[0025] Select a coordinate system and obtain the attitude quaternion based on the coordinate system;

[0026] Three-dimensional vector data is obtained based on accelerometer measurement data and the attitude quaternion;

[0027] The vector product error is obtained by calculating the three-dimensional vector data.

[0028] After adjusting the vector product error based on the adaptive compensation coefficient, complementary filtering is then performed based on the gyroscope zero-bias estimation data to generate the complementary filtering algorithm.

[0029] Preferably, the process of obtaining the adaptive compensation coefficients further includes:

[0030] To determine the carrier's state, when the carrier is in a non-motorized state, the compensation coefficient is adjusted based on the angle between the accelerometer's output and the actual output of the accelerometer in the non-motorized state.

[0031] When the carrier is in a maneuvering state, the compensation coefficient is adjusted based on the estimated pitch angle error and the estimated roll angle error.

[0032] Preferably, the process of obtaining the adaptive pose estimation result includes:

[0033] The compensated angular velocity is obtained by taking the angle between the accelerometer output and the actual accelerometer output in non-motorized state, the original angular velocity output by the gyroscope, and compensating for the gyroscope output.

[0034] The quaternion is updated based on the compensated angular velocity;

[0035] The adaptive pose estimation result is obtained based on the updated quaternion result.

[0036] The technical effects of this invention are as follows:

[0037] (1) Under static conditions, the method proposed in this invention can achieve good attitude estimation results;

[0038] (2) Under dynamic conditions, the method proposed in this invention has better accuracy than traditional complementary filtering;

[0039] (3) The method proposed in this invention is less affected by motion than traditional complementary filtering. Attached Figure Description

[0040] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0041] Figure 1 is a schematic diagram of the overall algorithm framework in an embodiment of the present invention;

[0042] Figure 2 is a graph showing the variation of K with respect to ||E||2 in the non-motorized state according to an embodiment of the present invention;

[0043] Figure 3 is a graph showing the variation of K with respect to ρ under maneuvering conditions in an embodiment of the present invention;

[0044] Figure 4 shows the static test results of pitch angle and roll angle in an embodiment of the present invention.

[0045] Figure 5 shows the test results of pitch angle and pitch angle error in an embodiment of the present invention;

[0046] Figure 6 shows the roll angle and roll angle error test results in an embodiment of the present invention. Detailed Implementation

[0047] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0048] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0049] Example 1

[0050] As shown in Figures 1-2, this embodiment provides a dynamic adaptive attitude estimation method based on an inertial sensor, including:

[0051] First, the current maneuvering state of the vehicle is evaluated based on the inertial sensor readings and the attitude estimation results from the previous moment. If the vehicle is in a maneuvering state, the current horizontal attitude angle error is calculated. If the vehicle is determined to be in a non-maneuvering state, gyroscope data is added to a sliding window to estimate the gyroscope's zero bias. Simultaneously, the compensation coefficients are adaptively adjusted based on the current maneuvering state and the horizontal attitude error angle. In the complementary filter, acceleration information is used to compensate for angular velocity, and the final attitude estimation result is obtained by integrating the angular velocity.

[0052] Further optimization of the scheme involves using the three-dimensional vector product error formed by the gravitational acceleration measured by the accelerometer and the current attitude quaternion. This error is then adjusted by adaptive compensation coefficients and complementarily filtered with the gyroscope output to obtain the current attitude quaternion. The key to the adaptive complementary filtering attitude estimation algorithm lies in the adaptive adjustment strategy of the compensation coefficients. This embodiment proposes to evaluate the carrier's maneuvering state using the carrier's own motion information and to adopt corresponding adaptive adjustment strategies for the compensation coefficients for different maneuvering states. Simultaneously, a gyroscope zero-bias estimation algorithm based on a sliding window is proposed to reduce the gyroscope's integration error.

[0053] Further optimizing the scheme, without considering the Earth's rotation, when the carrier is in a non-motorized state, the magnitude of the accelerometer output is the local gravitational acceleration, and the gyroscope output is 0. Let n f n is the magnitude of the accelerometer output. ω The magnitude of the gyroscope output:

[0054]

[0055]

[0056] In the formula These represent the gyroscope output ω. b Components on the x, y, and z axes These represent the accelerometer output f. b Components on the x, y, and z axes.

[0057] Considering the errors in the outputs of the accelerometer and gyroscope, set appropriate thresholds:

[0058]

[0059] In the formula T f and T ω This represents the set threshold, where g is the magnitude of the local gravitational acceleration.

[0060] When n f and n ω When formula (3) is satisfied, the carrier is considered to be in a non-motorized state; otherwise, the carrier is considered to be in a motorized state. Compared to using only n... f n ω To determine the motion state of the carrier, this embodiment proposes to simultaneously use n f and n ω To determine the motion state of the carrier, it can effectively avoid misjudging the motion state of the carrier.

[0061] When n f and n ω When formula (3) is not satisfied, the carrier is considered to be in a maneuvering state. At this time, it is necessary to analyze the attitude error caused by non-gravitational acceleration.

[0062] Let the output of the accelerometer in the navigation coordinate system be f. n The accelerometer output in the body coordinate system is f b The local gravitational acceleration is g n g n =[0 0 -g] T .

[0063] When the carrier is in a non-motorized state, f b It can be represented as:

[0064]

[0065] The expression for the rotation matrix from the n-system to the b-system:

[0066]

[0067] Substituting formula (5) into formula (4), we get:

[0068]

[0069] The expressions for the roll angle γ and pitch angle θ can be obtained from formula (6):

[0070]

[0071] Let Δθ and Δγ be the pitch and roll errors caused by non-gravitational acceleration, respectively, and V x V y V z These represent the normalized gravitational acceleration V components along the x, y, and z axes, respectively. Δa x , Δa y , Δa z These represent the normalized non-gravitational acceleration components on the x, y, and z axes, respectively.

[0072] Δθ=arcsin(V y +Δa y )-arcsin(V y (8)

[0073]

[0074] Δa x , Δa y , Δa z Since it cannot be obtained directly, this embodiment obtains the estimated value Δa of non-gravitational acceleration by subtracting the accelerometer output from the theoretical gravitational acceleration under the current attitude. x , Δa y , Δa z .

[0075]

[0076] Formulas (8) and (9) can be rewritten as:

[0077] Δθ=arcsin(V y +Δa y )-arcsin(V y (11)

[0078]

[0079] In the formula, Δθ and Δγ represent the estimated pitch angle error and roll angle error, respectively. Larger values ​​of Δθ and Δγ indicate greater vehicle maneuverability, while smaller values ​​indicate smaller vehicle maneuverability.

[0080] To further optimize the scheme, the traditional complementary filtering algorithm does not estimate the zero bias of the gyroscope, which increases the error generated by the gyroscope integration. Therefore, this embodiment proposes a zero bias estimation algorithm based on a sliding window.

[0081] When n f and n ω When formula (3) is satisfied, ω b Store the data in a sliding window, with a length of N, and calculate the gyroscope's zero bias:

[0082]

[0083] In the formula, This represents the data from the i-th gyroscope within the sliding window.

[0084] Update the gyroscope output:

[0085]

[0086] In the formula, This is the compensated gyroscope output.

[0087] The scheme was further optimized by selecting the Northeast-Sky (ENU) geographic coordinate system n for navigation and the right-front-upper coordinate system b for the body coordinate system.

[0088] This embodiment uses quaternions for attitude update. Compared to the Euler angle method and the direction cosine method, the quaternion method can work across all attitudes and has high computational efficiency.

[0089] The expression for the quaternion q:

[0090] q=q0+q1i+q2j+q3k (15)

[0091] Differentiating formula (13) with respect to time t, we get:

[0092]

[0093] In the formula, ω x ω y ω z These represent the components of angular velocity along the x, y, and z axes, respectively.

[0094] The discrete update formula for quaternions:

[0095]

[0096] In the formula, Δt is the sampling time.

[0097] Formula (5) can also be written in quaternion form

[15] :

[0098]

[0099] The quaternion expression for the attitude angle can be obtained from formulas (4) and (16):

[0100]

[0101] Let V be the output of the accelerometer in the non-motorized state.

[0102]

[0103] The actual output of the accelerometer is f b When the carrier is in a mobile state, V≠f b Let E be the sum of V and f. b The angle between them; when the angle is small, we can let E = sinE, and E can be obtained from f. b Cross product with V yields:

[0104]

[0105] In the formula, V x V y V z These are the components of V on the x, y, and z axes, respectively.

[0106] Let ω b 0 represents the raw angular velocity output by the gyroscope, ω. b 1 represents the compensated angular velocity, and K is the compensation coefficient, which can be used to compensate the gyroscope output via E.

[0107] ω b 1=ω b 0+K*E (22)

[0108] Finally, the current attitude angle can be solved using formulas (16), (17) and (19).

[0109] To further optimize the scheme, in traditional complementary filtering, K is a fixed value, which can lead to increased attitude estimation errors when non-gravity acceleration is too large. Therefore, this embodiment judges the motion state of the carrier and adopts different compensation strategies for different motion states.

[0110] After evaluating the maneuverability of the carrier using the carrier maneuverability assessment method, the compensation coefficient K is determined based on the carrier's maneuverability.

[0111] When the carrier is in a non-motorized state, in order to ensure rapid convergence of the attitude angle and avoid overshoot of the attitude angle during accelerometer calibration, this embodiment adaptively adjusts K according to the magnitude of E:

[0112]

[0113] In the formula, K1 and K2 represent the selected compensation coefficients, e1 and e2 represent the selected threshold values, and ||E||2 represents the L2 norm of E. The curve showing the variation of K with respect to ||E||2 is shown in Figure 2.

[0114] When the carrier is in a maneuvering state, if the attitude angle error increases, the compensation of the accelerometer to the gyroscope should be reduced. Therefore, this embodiment proposes to adaptively adjust the value of K by using Δθ and Δγ.

[0115]

[0116]

[0117] In the formula, K α This represents the selected compensation coefficient, and k is the attenuation factor.

[0118] The curve of K versus ρ at this point is shown in Figure 3. To further optimize the scheme, and to verify the performance of the proposed method in a static environment, a static environment simulation was performed.

[0119] The simulation parameters are set as follows: initial pitch angle is 20°; initial roll angle is 30°; gyroscope bias is 0.1° / s; system noise variance of the gyroscope is 0.01° / s; system noise variance of the accelerometer is 10... -3 g; sampling frequency is 200 Hz, and the total simulation time is 300 s.

[0120] Under simulation conditions, the attitude was calculated using the traditional complementary filtering algorithm and the method proposed in this embodiment, and the results were compared. The results are shown in Figure 4. The pitch angle error and roll angle error under simulation conditions are shown in Table 1.

[0121] Table 1

[0122]

[0123] As can be seen from Figure 4 and Table 1, under static conditions, both complementary filtering and the method proposed in this embodiment can achieve good attitude estimation results. The method proposed in this embodiment, due to the addition of the zero bias estimation process, can reduce the integral error of the gyroscope after the zero bias estimation is completed, so the attitude estimation effect is better than that of traditional complementary filtering.

[0124] To further optimize the scheme and verify the effectiveness of the method proposed in this embodiment, a prototype dynamic attitude measurement system based on STM32 and IMU was built. The IMU uses the SCHA634 chip, which integrates a three-axis accelerometer and a three-axis gyroscope—a MEMS sensor. 2 The C-bus transmits data to the main control chip. The prototype system transmits raw sensor data and attitude data to the host computer for display and recording via serial port. The parameters of the SHCA634 are shown in Table 2.

[0125] Table 2

[0126]

[0127] The reference benchmark is selected from the angle output of the dynamic attitude measurement station. The dynamic attitude measurement station consists of modules such as a rotation mechanism, a drive motor, and an angle encoder. The angle data is transmitted to the host computer via a serial port for display and recording. The output angle error is within 0.05°.

[0128] To verify the performance of the proposed method under maneuvering conditions, six sets of comparative experiments were conducted. In the first three sets of experiments, the prototype was vertically mounted on the control surface of the angle sensor to test the pitch angle calculation accuracy. In the latter three sets of experiments, the prototype was horizontally mounted on the control surface of the angle sensor to test the roll angle calculation accuracy. The motor was started, causing the angle sensor to rotate at low speed (maximum speed 50° / s), medium speed (maximum speed 100° / s), and high speed (maximum speed 200° / s), respectively, and the outputs of the angle sensor and the prototype were recorded. Finally, the attitude was calculated using a traditional complementary filtering algorithm and the proposed method, and the results were compared.

[0129] The results of the pitch angle and pitch angle error tests are shown in Figure 5, and the results of the roll angle and roll angle error tests are shown in Figure 6. The root mean square error and maximum error of the two algorithms under dynamic testing are shown in Table 3.

[0130] Table 3

[0131]

[0132] From Figures 5 and 6 and Table 3, we can conclude that:

[0133] Under dynamic conditions, the attitude estimation accuracy of the method proposed in this embodiment is superior to that of the traditional complementary filtering algorithm. At low, medium, and high speeds, the root mean square errors of the method proposed in this embodiment are 82%, 40%, and 32% of those of the traditional complementary filtering algorithm, respectively, with the maximum errors being 31%, 44%, and 39% of those of the traditional complementary filtering algorithm.

[0134] The attitude estimation accuracy of the method proposed in this embodiment is less affected by maneuvering than that of the traditional complementary filtering algorithm. When the prototype is in high-speed motion, compared with low-speed motion, the pitch angle error (RMS) of the traditional complementary filtering algorithm increases by 0.55° and the roll angle error (RMS) increases by 0.81°. The pitch angle error (RMS) of the method proposed in this embodiment increases by only 0.07° and the roll angle error (RMS) increases by only 0.19°.

[0135] The above description is merely a preferred embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A dynamic adaptive attitude estimation method based on inertial sensors, characterized in that, Includes the following steps: The vehicle's maneuvering state data is obtained based on inertial sensor and attitude estimation results; Adaptive compensation coefficients and gyroscope zero-bias estimation data are obtained from the carrier's maneuvering state data; The adaptive compensation coefficients and gyroscope bias estimation data are processed to generate a complementary filtering algorithm; The adaptive attitude estimation result is obtained based on the complementary filtering algorithm described above; The process of acquiring carrier maneuvering state data includes: acquiring inertial sensor data and the attitude estimation result of the previous moment; evaluating the current maneuvering state of the carrier based on the inertial sensor data and the attitude estimation result of the previous moment to obtain the carrier maneuvering state data; the process of evaluating the current maneuvering state of the carrier includes: acquiring the magnitude values ​​output by the accelerometer and the gyroscope; setting a preset threshold, calculating the magnitude values ​​output by the accelerometer and the gyroscope and comparing them with the preset threshold to classify the current maneuvering state of the carrier into a non-maneuvering state and a maneuvering state; acquiring pitch angle error estimates and roll angle error estimates, evaluating the maneuvering state based on the pitch angle error estimates and roll angle error estimates to obtain the carrier maneuvering state data; the process of acquiring adaptive compensation coefficients includes: acquiring the angle between the theoretical gravitational acceleration output and the actual output of the accelerometer; selecting a compensation coefficient... The process of obtaining gyroscope zero-bias estimation data includes: judging the magnitude of the accelerometer output and the magnitude of the gyroscope output based on the preset threshold and the carrier maneuvering state data, and obtaining the judgment result; storing the gyroscope output in a sliding window for collection and distribution based on the judgment result, and obtaining the gyroscope zero-bias; updating the gyroscope zero-bias to obtain the gyroscope zero-bias estimation data; the process of generating complementary filtering algorithm includes: selecting a coordinate system and obtaining attitude quaternions based on the coordinate system; obtaining three-dimensional vector data based on the accelerometer measurement data and the attitude quaternions; calculating the vector product error based on the three-dimensional vector data; adjusting the vector product error based on the adaptive compensation coefficient, and then performing complementary filtering based on the gyroscope zero-bias estimation data to generate the complementary filtering algorithm.

2. The dynamic adaptive attitude estimation method based on inertial sensors according to claim 1, characterized in that, The process of obtaining the adaptive compensation coefficient also includes: determining the carrier state; when the carrier is in a non-maneuvering state, adjusting the compensation coefficient based on the angle between the accelerometer output and the actual accelerometer output in the non-maneuvering state; when the carrier is in a maneuvering state, adjusting the compensation coefficient based on the pitch angle error estimate and the roll angle error estimate.

3. The dynamic adaptive attitude estimation method based on inertial sensors according to claim 1, characterized in that, The process of obtaining the adaptive attitude estimation result includes: obtaining the compensated angular velocity by using the angle between the accelerometer output and the actual accelerometer output in the non-maneuvering state, the original angular velocity output by the gyroscope, and compensating the gyroscope output; updating the quaternion based on the compensated angular velocity; and obtaining the adaptive attitude estimation result based on the updated quaternion result.

Images