CN-115523919-B - Nine-axis attitude calculation method based on gyro drift optimization

CN115523919BCN 115523919 BCN115523919 BCN 115523919BCN-115523919-B

Abstract

The invention discloses a nine-axis attitude resolving method based on gyro drift optimization. Firstly, based on the requirement of low calculation amount, a Madgwick filter of a nine-axis sensor is adopted for gesture calculation. Secondly, a brand new gyro drift optimization method is provided, and the rotation angle is not calculated additionally when the gyroscope does not move through an intelligent threshold judgment method. And taking the output angular velocity processed by the method as Madgwick filter input to realize optimal estimation of the attitude. Aiming at the traditional nine-axis gesture resolving method, the method can effectively solve the course angle resolving difficulty caused by gyroscope drift, and can complete gesture resolving under different environments.

Inventors

YANG HAOMING
ZHAO LIYE
GAO ANG
LI HONGSHENG
HUANG LIBIN
DING XUJIE
CHEN RUIFENG

Assignees

东南大学

Dates

Publication Date: 20260508
Application Date: 20221108

Claims (4)

1. A nine-axis attitude resolving method based on gyro drift optimization is characterized by comprising the following steps: step 1), establishing a quaternion model of the measured value of the micro inertial sensor; Step 2), deducing a rotation quaternion differential integral equation; step 3), establishing a gravity reference vector and a magnetic reference vector quaternion model; Step 4), constructing a gyro drift optimization algorithm; The gyro drift optimization algorithm is executed under a stable posture after the gyroscope is powered on; Step 41, calculating a zero offset compensation variable, and subtracting the variable from the original output of the gyroscope to obtain a compensated angular velocity output: (1); (2); Step 42, calculating a threshold value of motion detection, reading quantitative i data and taking the maximum absolute value of the quantitative i data as the threshold value: (3); after calculating the threshold value, comparing the compensated angular velocity output with the threshold value, wherein when the angular velocity output is larger than the threshold value, the motion state is obtained, the original data is input into the subsequent filtering, and when the original data is smaller than or equal to the threshold value, the noise in the stationary state is judged; wherein th represents a threshold value, gy represents a gyroscope output, and Z represents a Z axis; step 43, calculating the required rest time according to the required vector length by the data storage vector length: (4); Wherein A is the vector length, W is the zero offset calculation length, and N is the threshold calculation length; Finally, inputting the optimized angular velocity obtained by the algorithm into a Madgwick filter to perform attitude calculation; Step 5), constructing an integral attitude calculation algorithm, and inputting a gyro drift optimized angular velocity value and accelerometer magnetometer data into Madgwick filters to calculate attitude angles; Step 51, calculating quaternion variation through angular velocity measurement value: (5); Step 52, applying two reference vectors of gravity and magnetic force to the quaternion calculated by correcting the angular velocity through a gradient descent algorithm, wherein the acceleration reference vector is defined as follows: (6); By calculating the jacobian matrix, a rotation correction amount F is calculated: (7); (8); wherein E is a reference vector, g is earth gravitational acceleration, F is correction amount, I is a unit vector, and J is a Jacobian matrix; Step 53, multiplying the obtained quaternion correction by an adjustable correlation coefficient and adding the quaternion correction to a quaternion variable: (9); (10); (11); Step 54, finally calculating the course angle: (12); where β is the filter coefficient and θ is the attitude angle.
2. The nine-axis attitude calculation method based on gyro drift optimization according to claim 1, wherein in step 1), a quaternion model of the measured value of the micro inertial sensor is built, the three-axis angular velocity under the carrier coordinate system is measured through a gyro module in the micro inertial sensor, and the quaternion form of the angular velocity is defined as follows: (13); Wherein s is a quaternion, w is an angular velocity, x, y and z respectively represent three axes in a carrier coordinate system, For the angular velocity in the x-axis, For the angular velocity in the y-axis, Is the angular velocity in the y-axis.
3. The nine-axis attitude solving method based on gyro drift optimization according to claim 1, wherein step 2) is characterized in that the solving of the quaternion differential equation obtains a rotation rate of an earth coordinate system relative to a carrier coordinate system: (14); Introducing time t, and constructing a recursive equation of the quaternion: (15); (16); wherein S is a carrier coordinate system, E is an earth coordinate system, q is a quaternion, A rotation quaternion from an earth coordinate system to a carrier coordinate system, t is a time mark, Is a time interval.
4. The nine-axis attitude calculation method based on gyro drift optimization according to claim 1, wherein step 3) is characterized in that quaternion forms of triaxial acceleration and triaxial magnetic strength under a measuring carrier coordinate system through a micro inertial sensor accelerometer module and a magnetometer module are defined as follows: (17); (18); Wherein, the For the accelerometer, m is the magnetometer, mx, my, mz are the three components of the magnetic vector.

Description

Nine-axis attitude calculation method based on gyro drift optimization Technical Field The invention belongs to the field of inertial navigation, and particularly relates to a nine-axis attitude calculation method based on gyro drift optimization. Background The gesture resolving algorithm is one of key technologies of the robot technology. The micro inertial sensor has the advantages of small size, low power consumption and low cost, and has become one of the important sensors of the industrial robot attitude angle calculation and direction sensing solutions. Generally, micro inertial sensors are installed at the arm and wrist of a robot to detect pitch angle (up-down bending) and heading angle (left-right movement). The accelerometer can be combined with the gyroscope to be fused with the sensor by detecting the gravity vector, so that the accurate pitch angle can be calculated. The course angle calculation requires a high-precision calibration of the magnetometer in advance to ensure a correct solution angle. However, complex calculations for pre-calibration of magnetometers can lead to increased computer power consumption and magnetometers are susceptible to environmental magnetic fields, such as in industrial plants or robotic workshops. Satellite navigation systems, such as GPS (global positioning system) and GNSS (global navigation satellite system), are not suitable for indoor environments, and thus course angle resolution in attitude resolution will become one of the difficulties in the robot industry. The gyroscope may provide an angular rate to calculate the heading angle, and using the gyroscope as the sole source of data for the heading angle solution may be affected by accumulated errors in the integration operation, and the gyroscope readings may drift over time, which may cause the attitude solution to deviate from the range of the actual angle estimate. In various types of sensor fusion, such as kalman filtering, complementary filtering, madgwick filtering are common filters, madgwick filtering can be combined with accelerometers, gyroscopes and magnetometers to calculate heading angle. However, without a magnetic sensor, the resolution of its heading angle would be a challenge. Disclosure of Invention In order to solve the problems, the invention discloses a nine-axis attitude resolving method based on gyro drift optimization, which solves the difficulty in calculating an attitude resolving course angle caused by the drift of a gyroscope of a micro inertial sensor and improves the robustness and the accuracy of the attitude resolving algorithm course angle resolving. In order to achieve the above purpose, the technical scheme of the invention is as follows: A nine-axis attitude resolving method based on gyro drift optimization comprises the following steps: step 1), measuring the triaxial angular velocity under a carrier coordinate system through a gyroscope module in the micro inertial sensor, wherein the quaternion form of the angular velocity is defined as follows: wherein s is a quaternion, w is an angular velocity, and x, y and z respectively represent three axes in a carrier coordinate system; step 2), the quaternion form of a rotation matrix from the earth coordinate system to the carrier coordinate system is defined as follows: wherein S is a carrier coordinate system, E is an earth coordinate system, q is a quaternion, Step 3), differentiating the quaternion to obtain the rotation rate of the earth coordinate system relative to the carrier coordinate system: step 4), introducing time t, and constructing a recursive equation of the quaternion: t is a time mark, and Δt is a time interval; Step 5), measuring triaxial acceleration under a carrier coordinate system and triaxial magnetic intensity quaternion form measured by a magnetometer through an accelerometer module in the micro inertial sensor, wherein the quaternion form is defined as follows: Where a is the accelerometer, m is the magnetometer, mx, my, mz are the three components of the magnetic vector; Step 6), the main calculation steps of Madgwick filters are divided into the following four steps: step 6.1), in a first step, calculating the quaternion change by means of the angular velocity measurement: Step 6.2), in the second step, two reference vectors of gravity and magnetic force are applied to correct quaternion of angular velocity calculation through a gradient descent algorithm, and the acceleration reference vector is defined as follows: Ea=[0,0,0,g] (9) By calculating the jacobian matrix, a rotation correction amount F is calculated: wherein E is a reference vector, g is earth gravitational acceleration, F is correction amount, I is a unit vector, and J is a Jacobian matrix; step 6.3), multiplying the obtained quaternion correction by an adjustable correlation coefficient and adding the quaternion correction to a quaternion variable: δq′=δq-βδs (12) qt＝qt-1+δq′△t (14) Step 6.4), finally calculating a course angle: A