A nine-axis attitude solution method based on gyro drift optimization

Abstract

The application discloses a nine-axis attitude solution method based on gyro drift optimization. First, based on the requirement of low calculation amount, a Madgwick filter of a nine-axis sensor is used for attitude solution. Then, a new gyro drift optimization method is provided, and when the gyroscope does not move, the rotation angle is not calculated additionally through the method of intelligent threshold judgment. The output angular velocity after the method processing is used as the input of the Madgwick filter, so as to realize the optimized estimation of the attitude. For the traditional nine-axis attitude solution method, the method can effectively solve the heading angle solution problem caused by the gyro drift, and can complete the attitude solution in different environments.

Description

Technical Field

[0001] This invention belongs to the field of inertial navigation, specifically relating to a nine-axis attitude calculation method based on gyroscope drift optimization. Background Technology

[0002] Attitude calculation algorithms are one of the key technologies in robotics. Micro inertial sensors, with their advantages of small size, low power consumption, and low cost, have become one of the important sensors for attitude angle calculation and orientation perception solutions for industrial robots. Typically, micro inertial sensors are mounted on the robot's arm and wrist to detect pitch angles (vertical bending) and yaw angles (lateral movement).

[0003] Accelerometers, by detecting the gravity vector, can be combined with gyroscopes for sensor fusion to calculate precise pitch angles. Heading angle calculation requires high-precision pre-calibration of the magnetometer to ensure accurate angle calculation. However, the complex calculations involved in magnetometer pre-calibration lead to increased computer power consumption, and magnetometers are easily affected by environmental magnetic fields, such as in industrial plants or robot operating rooms. Satellite navigation systems, such as GPS (Global Positioning System) and GNSS (Global Navigation Satellite System), are not suitable for indoor environments; therefore, heading angle calculation in attitude determination remains one of the challenges in the robotics industry.

[0004] Gyroscopes can provide angular rates for calculating heading angles. However, using a gyroscope as the sole data source for heading angle calculation is susceptible to accumulated errors in integration operations, and gyroscope readings drift over time, causing the attitude calculation results to deviate from the actual angle estimate. Various sensor fusion techniques, such as Kalman filtering, complementary filtering, and Madgwick filtering, are commonly used filters. Madgwick filtering can combine accelerometers, gyroscopes, and magnetometers to calculate heading angles. However, calculating heading angles becomes challenging in the absence of a magnetic sensor. Summary of the Invention

[0005] To address the aforementioned issues, this invention discloses a nine-axis attitude calculation method based on gyroscope drift optimization. This method solves the problem of calculating the heading angle in attitude calculation caused by the drift of the micro-inertial sensor gyroscope, thereby improving the robustness and accuracy of the heading angle calculation in the attitude calculation algorithm.

[0006] To achieve the above objectives, the technical solution of the present invention is as follows:

[0007] A nine-axis attitude calculation method based on gyroscope drift optimization includes the following steps:

[0008] Step 1) Measure the three-axis angular velocity in the carrier coordinate system using the gyroscope module in the micro-inertial sensor. The quaternion form of the angular velocity is defined as follows:

[0009]

[0010] Where s is a quaternion, w is the angular velocity, and x, y, z represent the three axes in the carrier coordinate system;

[0011] Step 2), the quaternion form of the rotation matrix from the Earth coordinate system to the carrier coordinate system is defined as follows:

[0012]

[0013] Where S is the carrier coordinate system, E is the Earth coordinate system, and q is a quaternion.

[0014] Step 3) Differentiate the quaternion to obtain the rotation rate of the Earth coordinate system relative to the carrier coordinate system:

[0015]

[0016] Step 4), introduce time t, and construct the recurrence relation for the quaternion:

[0017]

[0018]

[0019] t represents the time point, and Δt represents the time interval;

[0020] Step 5), the triaxial acceleration in the carrier coordinate system is measured by the accelerometer module in the micro inertial sensor, and the triaxial magnetic intensity is measured by the magnetometer. The quaternion form is defined as follows:

[0021]

[0022]

[0023] Where a is the accelerometer, m is the magnetometer, and mx, my, and mz are the three components of the magnetic vector;

[0024] Step 6), the main calculation steps of the Madgwick filter are divided into the following four steps:

[0025] Step 6.1), the first step, calculate the quaternion change using the angular velocity measurement value:

[0026]

[0027] Step 6.2), the second step, uses the gradient descent algorithm to apply the two reference vectors of gravity and magnetism to the quaternion used to correct the angular velocity calculation. The acceleration reference vector is defined as follows:

[0028] E a=[0,0,0,g] (9)

[0029] The rotation correction F is calculated by calculating the Jacobian matrix:

[0030]

[0031]

[0032] Where E is the reference vector, g is the Earth's gravitational acceleration, F is the correction, I is the unit vector, and J is the Jacobian matrix;

[0033] Step 6.3): Multiply the obtained quaternion correction by an adjustable correlation coefficient and add it to the quaternion variable:

[0034] δq′=δq-βδs (12)

[0035]

[0036] q t =q t-1 +δq′△t (14)

[0037] Step 6.4), finally calculate the heading angle:

[0038]

[0039] Step 7): After the gyroscope is powered on, execute the gyroscope drift optimization algorithm under stable attitude. The main calculation steps of this algorithm are divided into the following three steps:

[0040] Step 7.1), the first step, calculate the zero-bias compensation variable, and subtract this variable from the original gyroscope output to obtain the compensated angular velocity output:

[0041]

[0042]

[0043] Where TO represents zero bias upon power-on, and k represents the value at time k;

[0044] Step 7.2), the second step, calculate the motion detection threshold by reading i quantitative data points and finding the maximum absolute value among them as the threshold:

[0045] Z th =m i ax{|Z gy [1]|,|Z gy [2]|,...,|Z gy [i]|} (18)

[0046] After calculating the threshold, the compensated angular velocity output is compared with the threshold. If it is greater than the threshold, it is considered to be in motion, and the original data is input into the subsequent filtering. If it is less than or equal to the threshold, it is determined to be noise in a stationary state.

[0047] Where th represents the threshold, gy represents the gyroscope output, and Z represents the Z-axis;

[0048] Step 7.3), the third step, determines the vector length for data storage, and calculates the required static time based on the required vector length:

[0049] A≥W+N (19)

[0050] Where A is the vector length, W is the zero-bias computation length, and N is the threshold computation length;

[0051] Finally, the optimized angular velocity obtained by the algorithm is input into the Madgwick filter for attitude calculation.

[0052] The beneficial effects of this invention are:

[0053] (1) This invention solves the problem of calculating heading angle without magnetic assistance, and the algorithm has strong stability. Furthermore, it ensures the robustness of the system while maintaining the accuracy of the original algorithm under both dynamic and static conditions. It has good application prospects in Industry 4.0 applications where magnetometers cannot be used due to external environmental interference.

[0054] (2) This invention employs an optimized algorithm for gyroscope drift in micro-inertial sensors, providing a new method for calculating heading angles in the absence of magnetometers and GNSS. This technique is used as a preprocessing input for the Madgwick filter, without relying on external sensor inputs such as magnetometers.

[0055] (3) This invention employs the Madgwick attitude calculation algorithm, using acceleration and magnetic field measured by sensors to correct gyroscope drift. Compared to complex linear filtering algorithms, this significantly improves the processor's computational efficiency, and the overall computational load of the algorithm is small, making it suitable for industrial robots that require multi-point deployment of micro-inertial sensor modules. Attached Figure Description

[0056] Figure 1 This is a flowchart of the nine-axis attitude calculation method based on gyroscope drift optimization of the present invention.

[0057] Figure 2 This is a framework diagram of the gyroscope drift optimization algorithm of the present invention. Detailed Implementation

[0058] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0059] As shown in the figure, the nine-axis attitude calculation method based on gyroscope drift optimization described in this invention includes the following steps:

[0060] like Figure 1 As shown, in step 1), the three-axis angular velocity in the carrier coordinate system is measured by the gyroscope module in the micro inertial sensor. The quaternion form of the angular velocity is defined as follows:

[0061]

[0062] Where s is a quaternion, w is the angular velocity, and x, y, z represent the three axes in the carrier coordinate system;

[0063] Step 2), the quaternion form of the rotation matrix from the Earth coordinate system to the carrier coordinate system is defined as follows:

[0064]

[0065] Where S is the carrier coordinate system, E is the Earth coordinate system, q is a quaternion, t is a time marker, and Δt is a time interval;

[0066] Step 3) Differentiate the quaternion to obtain the rotation rate of the Earth coordinate system relative to the carrier coordinate system:

[0067]

[0068] Step 4), introduce time t, and construct the recurrence relation for the quaternion:

[0069]

[0070]

[0071] Step 5), the triaxial acceleration in the carrier coordinate system is measured by the accelerometer module in the micro inertial sensor, and the triaxial magnetic intensity is measured by the magnetometer. The quaternion form is defined as follows:

[0072]

[0073]

[0074] Where a is the accelerometer, m is the magnetometer, and mx, my, and mz are the three components of the magnetic vector;

[0075] Step 6), the main calculation steps of the Madgwick filter are divided into the following four steps:

[0076] Step 6.1), the first step, calculate the quaternion change using the angular velocity measurement value:

[0077]

[0078] Step 6.2), the second step, uses the gradient descent algorithm to apply the two reference vectors of gravity and magnetism to the quaternion used to correct the angular velocity calculation. The acceleration reference vector is defined as follows:

[0079] E a =[0,0,0,g] (9)

[0080] The rotation correction F is calculated by calculating the Jacobian matrix:

[0081]

[0082]

[0083] Where E is the reference vector, g is the Earth's gravitational acceleration, F is the correction, I is the unit vector, and J is the Jacobian matrix;

[0084] Step 6.3): Multiply the obtained quaternion correction by an adjustable correlation coefficient and add it to the quaternion variable:

[0085] δq′=δq-βδs (12)

[0086]

[0087] q t =q t-1 +δq′△t (14)

[0088] Step 6.4), finally calculate the heading angle:

[0089]

[0090] like Figure 2 As shown in step 7), after the gyroscope is powered on, the gyroscope drift optimization method is executed under a stable attitude. The main calculation steps of this method are divided into the following three steps:

[0091] Step 7.1), the first step, calculate the zero-bias compensation variable, and subtract this variable from the original gyroscope output to obtain the compensated angular velocity output:

[0092]

[0093]

[0094] Where TO represents zero bias upon power-on, and k represents the value at time k;

[0095] Step 7.2), the second step, calculate the motion detection threshold by reading i quantitative data points and finding the maximum absolute value among them as the threshold:

[0096]

[0097] After calculating the threshold, the compensated angular velocity output is compared with the threshold. If it is greater than the threshold, it is considered to be in motion, and the original data is input into the subsequent filtering. If it is less than or equal to the threshold, it is determined to be noise in a stationary state.

[0098] Where th represents the threshold, gy represents the gyroscope output, and Z represents the Z-axis;

[0099] Step 7.3), the third step, determines the vector length for data storage, and calculates the required static time based on the required vector length:

[0100] A≥W+N (19)

[0101] Where A is the vector length, W is the zero-bias computation length, and N is the threshold computation length;

[0102] Finally, the optimized angular velocity obtained by the algorithm is input into the Madgwick filter for attitude calculation.

[0103] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A nine-axis attitude calculation method based on gyroscope drift optimization, characterized in that: Includes the following steps: Step 1), establish a quaternion model of the micro-inertial sensor measurement values; Step 2), derive the rotational quaternion differential-integral equation; Step 3), establish quaternion models for gravity reference vector and magnetic reference vector; Step 4), construct the gyroscope drift optimization algorithm; The gyroscope drift optimization algorithm is executed under stable attitude after the gyroscope is powered on and started. Step 41: Calculate the zero-bias compensation variable, and subtract this variable from the original gyroscope output to obtain the compensated angular velocity output: (1)； (2)； Step 42: Calculate the motion detection threshold. Read a set of i data points and find the maximum absolute value among them as the threshold. (3)； After calculating the threshold, the compensated angular velocity output is compared with the threshold. If it is greater than the threshold, it is considered to be in motion and the original data is input into the subsequent filtering. If it is less than or equal to the threshold, it is determined to be noise in a stationary state. Where th represents the threshold, gy represents the gyroscope output, and Z represents the Z-axis; Step 43: Determine the vector length for data storage, and calculate the required static time based on the required vector length: (4)； Where A is the vector length, W is the zero-bias computation length, and N is the threshold computation length; Finally, the optimized angular velocity obtained by the algorithm is input into the Madgwick filter for attitude calculation; Step 5) Construct the overall attitude calculation algorithm by inputting the angular velocity value optimized by gyroscope drift and the accelerometer and magnetometer data into the Madgwick filter to calculate the attitude angle; Step 51: Calculate the quaternion change using the angular velocity measurement: (5)； Step 52: Using the gradient descent algorithm, apply the two reference vectors of gravity and magnetism to the quaternion used to correct the angular velocity calculation. The acceleration reference vector is defined as follows: (6)； The rotation correction F is calculated by calculating the Jacobian matrix: (7)； (8)； Where E is the reference vector, g is the Earth's gravitational acceleration, F is the correction, I is the unit vector, and J is the Jacobian matrix; Step 53: Multiply the obtained quaternion correction by an adjustable correlation coefficient and add it to the quaternion variable: (9)； (10)； (11)； Step 54: Finally, calculate the heading angle: (12)； Where β is the filter coefficient and θ is the attitude angle.

2. The nine-axis attitude calculation method based on gyroscope drift optimization according to claim 1, characterized in that: Step 1) establishes a quaternion model of the measured values ​​from the micro-inertial sensor. The gyroscope module in the micro-inertial sensor measures the three-axis angular velocity in the carrier coordinate system. The quaternion form of the angular velocity is defined as follows: (13)； Where s is a quaternion, w is the angular velocity, and x, y, z represent the three axes in the carrier coordinate system. The angular velocity along the x-axis is... The angular velocity along the y-axis is... ω is the angular velocity along the y-axis.

3. The nine-axis attitude calculation method based on gyroscope drift optimization according to claim 1, characterized in that: Step 2) involves solving the quaternion differential equation to obtain the rotation rate of the Earth coordinate system relative to the carrier coordinate system: (14)； Introducing time t, we construct the recurrence relation for quaternions: (15)； (16)； Where S is the carrier coordinate system, E is the Earth coordinate system, and q is a quaternion. Let be the rotation quaternion from the Earth coordinate system to the carrier coordinate system, and t be the time marker. For time intervals.

4. The nine-axis attitude calculation method based on gyroscope drift optimization according to claim 1, characterized in that: Step 3) Using the micro-inertial sensor accelerometer module and magnetometer module, the quaternion form of the triaxial acceleration and triaxial magnetic intensity in the carrier coordinate system is defined as follows: (17)； (18)； in, denoted as accelerometer, m as magnetometer, and mx, my, and mz as the three components of the magnetic vector.

Images