AHRS design based on ARM and low-cost MEMS devices

introduction

The Attitude and Heading Reference System (AHRS) can provide attitude information such as heading, roll and rollover. High-precision inertial navigation devices such as mechanical gyroscopes and fiber optic gyroscopes are expensive and difficult to promote. Currently, MEMS sensors are widely used in consumer electronic products, but MEMS angular rate gyroscopes have serious zero drift and random errors, which will produce integral errors in the strapdown inertial navigation solution, making it difficult to achieve application accuracy. Accelerometers and magnetometers can measure two unrelated three-dimensional vectors, gravity acceleration and geomagnetic field, respectively, which can be used as observation vectors of platform attitude to calibrate gyroscopes. The extended Kalman filter can combine the characteristics of these sensors, use the angular rate measured by the gyroscope for prediction updates, and use gravity acceleration and magnetic field observation updates to obtain higher-precision attitude angle information.

1 Hardware Structure

The basic components of the AHRS hardware of MEMS devices are a three-axis angular rate gyroscope, a three-axis accelerometer, a three-axis magnetoresistive sensor, and an STM32 series microprocessor STM32F103U8T6. The hardware structure of the heading attitude reference system is shown in Figure 1.

IMU uses the MPU6000, which integrates a 16-bit three-axis gyroscope and a three-axis accelerometer. Compared with the multi-component solution, it effectively avoids the problem of inter-axis installation error when combining the gyroscope and the accelerometer, saving installation space. At the same time, it has a built-in 16-bit A/D converter, which simplifies the circuit design. The angular rate range of MPU6000 is ±250 °/s, ±500 °/s, ±1000 °/s and ±2000 °/s. The acceleration measurement range is ±2g, ±4g, ±8g and ±16g. It has a built-in 16-bit digital temperature sensor to facilitate temperature compensation of the sensor. Data can be transmitted through an I2C bus of up to 400 kHz or an SPI interface of up to 20 MHz, and the sampling update rate reaches 8 kHz, which can ensure the real-time measurement of the system.

Figure 1 Hardware structure of the heading attitude reference system

Honeywell HMC5883 is a three-axis 12-bit I2C bus digital output magnetoresistive sensor with a measurement range of ±1~±8 Gs and a data update rate of 80 Hz. It has a built-in OFFSET/SET/RESET circuit, which will not cause magnetic saturation and cumulative errors. It supports automatic calibration procedures, simplifies the use steps, and can meet the measurement requirements of the geomagnetic field. The STM32 series ARM processor STM32F103U8T6 based on the CortexM3 core is selected, with a main frequency of 72 MHz and 1.25 DMIPS/MHz; it has a hardware single-cycle multiplier to ensure the real-time update of the attitude; it has a rich peripheral interface, and can use the I2C bus interface to read data from the sensor and communicate with the host computer through the serial port.

2 Four-dimensional Extended Kalman Filter Algorithm

The Extended Kalman Filter (EKF) algorithm is a real-time recursive algorithm implemented by a computer. The object processed is a random signal. It uses the statistical characteristics of system noise and observation noise, takes the system's observed quantity as the filter input, and takes the required estimated value (system state variable) as the filter output. The filter input and output are linked by the time update and observation update algorithm. The signal to be processed is estimated based on the system's state equation and observation equation. The state variable of the AHRS Extended Kalman Filter algorithm uses four-dimensional quaternions. Compared with the use of Euler angles, it avoids the large number of trigonometric function operations involved in the calculation of Euler angles, ensures the update rate and real-time performance, and does not have the singularity that occurs when using Euler angles. The conversion relationship between Euler angles and quaternions is shown in equations (1) to (3).

The quaternion differential equation is shown in formula (4), and the quaternion attitude matrix differential is

The equation only requires solving four differential equations, which reduces a lot of calculations compared to the direction cosine attitude matrix differential equation, making it easier to implement on a microprocessor.

2.1 Time Update

The state equation of the system is shown in equation (5).

The state variable is the quaternion X = [q0, q1, q2, q3]T, and Wk-1 is the four-dimensional process noise. The matrix A can be obtained based on the three-axis angular rate [ωX, ωY, ωZ]T measured by the gyroscope, as shown in formula (6). Δt is the time elapsed between two time prediction updates.

The time update of state variables is shown in formula (7).

The covariance matrix P is predicted as shown in formula (8), where Q is the covariance of the four-dimensional process excitation noise.

2.2 Observation Update

The observation update of AHRS is to rotate the reference vector of gravity acceleration and geomagnetic field in the body coordinate system to the navigation coordinate system, and then compare it with the acceleration and magnetic field sensors to obtain the residual of the observed variables. The transfer matrix from the body system to the navigation system can be expressed as quaternion as Equation (9).

The three-dimensional reference vector v is transferred to the navigation system and can be expressed by the observation equation (10).

When the gravity acceleration observation is updated, the reference vector v is equal to the gravity acceleration reference vector (which can be set to When the platform is stationary and placed horizontally, the three-dimensional vector measured by the accelerometer is:

When the magnetic field observation is updated, v is equal to the magnetic field reference vector (which can be set to When the platform is stationary and horizontal and the heading points to due north, the three-dimensional vector measured by the magnetoresistive meter is:

H is the Jacobian matrix of the partial derivative of h with respect to X, as shown in formula (11).

The Kalman gain matrix Kk is shown in formula (12), where the R matrix is ​​the three-dimensional observation noise covariance matrix.

Observation Update:

When the gravity acceleration observation is updated, zk is the acceleration, and the three-dimensional vector zk=[aXaYaZ]T measured by the sensor. When the magnetic field observation is updated, zk is the magnetoresistive sensor, and the three-dimensional vector zk=[mXmYmZ]T measured is obtained. Covariance update:

3 Program Structure

The software design of AHRS is mainly divided into:

① Sensor initialization, including setting the sensor’s update rate and range.

② Initialize the correlation matrix of the Kalman filter and set the process excitation noise covariance matrix Q according to the characteristics of the sensor, which is set to a four-dimensional diagonal matrix with diagonal elements of 0.1.

③ If the gyroscope data is read successfully, the Kalman filter time is updated.

④ Collect data from the acceleration sensor and magnetoresistive sensor. If the reading is successful, perform observation update. The difference between the acceleration observation update and magnetic field observation update algorithms lies in the observation variance R. The corresponding value can be set according to the confidence of the two sensors. The program flow of the heading attitude reference system is shown in Figure 2.

Figure 2 Program flow of heading attitude reference system

4 Sensor Calibration

4.1 Gyroscope Temperature Compensation

Low-cost MEMS gyroscopes have large zero-point offsets, which can generally reach 1~3 °/s. This problem can be solved by increasing the dimension of the state variables of the extended Kalman filter, that is, adding three-dimensional gyroscope zero bias for data fusion. The advantage is that the gyroscope zero bias can be dynamically estimated, and it has strong adaptability. The disadvantage is that the calculation amount of the Kalman filter algorithm increases with the cube of the dimension, so the real-time performance will be reduced. When the attitude update rate is required to be high and the computing performance of the single-chip microcomputer is limited, the temperature compensation method can be used to solve it. The zero-point offset of the gyroscope is closely related to the sensor temperature and temperature gradient. When the temperature of the MPU6000 gyroscope changes slowly, it can be considered that its temperature zero-point drift corresponds to the temperature of the sensor at this time.

The temperature drift curve can be set as a cubic polynomial BX = At3 + Bt2 + Ct1 + D, and the coefficients can be obtained by least squares fitting. The gyroscope curve after temperature compensation is shown in Figure 3.

Figure 3 Angular velocity curve after subtracting temperature drift at rest

4.2 Hard magnetic and non-orthogonality correction

Under normal circumstances, the three-dimensional data measured by the geomagnetic field should have a standard sphere in space. However, due to the influence of the external magnetic field, the scale factor and non-orthogonality of the axes of the magnetoresistive sensor, the data measured by the magnetic field meter is distributed in the three-dimensional space. The envelope of the data collected by the sensor is an ellipsoidal surface with the center offset from the origin. The three-dimensional distribution of the bare magnetic field data is shown in Figure 4.

Figure 4 Three-dimensional distribution of raw magnetic field data

The ellipsoid constraint equation is shown in formula (15).

Where m is the three-dimensional magnetic field strength measured by the sensor, c is the three-dimensional vector of the spherical center offset, and U is the scale factor and non-orthogonal calibration matrix. The magnetic field strength has no practical significance. What we care about is the direction of the three-dimensional geomagnetic vector measured by the sensor, so the magnetic field vector modulus is set to 1. U and c can be calculated by the least squares method. The comparison of magnetic field data before and after correction is shown in Figure 5. The left and right figures are the projections of the data before and after correction on the XY plane.

Figure 5 Comparison of magnetic field data before and after correction

5 Experimental Results

The AHRS is placed on a rotating platform fixed to a 1024-line grating encoder to test the measurement accuracy and tracking performance of the pitch attitude angle. The comparison of the AHRS and encoder measurement curves is shown in Figure 6.

Figure 6 Comparison of AHRS and encoder measurement curves

The solid line in the figure is the measurement value of AHRS, and the dot-dash line is the measurement value of the encoder. When the test platform swings with an amplitude of about ±10°, the maximum time lag between the AHRS and encoder measurement data does not exceed 5 ms, and the peak-to-peak difference does not exceed 0.3°.

Conclusion

The AHRS based on the quaternion extended Kalman filter algorithm has the characteristics of high update rate, good real-time performance and low price. It can be widely used in consumer electronic products such as mobile phones and tablets, and can also meet the measurement needs of some robots for posture control.