Multi-sensor fusion and attitude calculation of UAV navigation system

In the field of drones, DJI has always been in the first queue, with various consumer drones emerging one after another, last year’s popular Mavic Air2, this year’s FJIFPV and DJI Air2S. DJI drones have a market share of 90% in China, 70% in the global market, and close to 80% in the US market, so that everyone can frequently see big shots in various Hollywood blockbusters. The figure of Xinjiang UAV.

In the field of drones, DJI has always been in the first queue, with various consumer drones emerging one after another, last year’s popular Mavic Air2, this year’s FJIFPV and DJI Air2S. DJI drones have a market share of 90% in China, 70% in the global market, and close to 80% in the US market, so that everyone can frequently see big shots in various Hollywood blockbusters. The figure of Xinjiang UAV.

Multi-sensor fusion and attitude calculation of UAV navigation system
(DJI UAV, source: https://www.dji.com/cn/air-2s)

The so-called unmanned aerial vehicle is an unmanned aircraft operated by radio remote control equipment and self-provided program control device. In recent years, UAVs have been widely used in civil, military and scientific research fields, such as reconnaissance and surveillance, ground attack, traffic patrol, etc. Among them, in the flight control of the UAV, the flight attitude is one of the important parameters, and the inertial navigation system is a more commonly used navigation method.

1. Sensor devices

For an aircraft, the calculation progress of the attitude angle is closely related to the navigation accuracy, so the attitude calculation and real-time update are the key points of the navigation system. Accelerometers and gyroscopes are the main components of attitude calculation.

Multi-sensor fusion and attitude calculation of UAV navigation system
(Accelerometer, source: Baidu Encyclopedia)

The accelerometer measures the magnitude of the acceleration force received by the carrier. When the accelerometer remains stable, the measured data is the magnitude of the reaction force generated by the projection of gravity on the three-dimensional plane. Only the three-axis accelerometer can be used to estimate the attitude angle, but due to the slow dynamic response of the three-axis accelerometer, if only the accelerometer is used to obtain the attitude angle, a large delay will occur, and during the movement process, the attitude calculation less accurate.

Multi-sensor fusion and attitude calculation of UAV navigation system
(Gyroscope, source: Baidu Encyclopedia)

The gyroscope measures the angular velocity of the carrier rotating around the axis of rotation, also known as the angular velocity meter. When the carrier moves at a certain rate, the angular rate of rotation in three-dimensional space can be obtained by measuring the Coriolis acceleration. Theoretically, integrating the angular rate can obtain the attitude angle information of the carrier, but since the measurement of the gyroscope will be affected by the drift error, for the regular drift, it can be compensated by establishing a mathematical model, but for the random drift error Compensation has not yet emerged as a particularly effective method. Therefore, it is not suitable to use only the gyroscope to calculate the attitude angle.

2. Inertial Navigation System

1. Working principle

The inertial navigation system obtains position and attitude information by integrating the total increment, which can be mainly divided into platform inertial navigation system and strapdown inertial navigation system.

In the platform inertial inertial navigation system, the accelerometer and gyroscope are installed on the navigation platform, and the navigation platform is used to simulate the navigation coordinate system. The acceleration in the navigation coordinate system, and then the geometric method is used to obtain the attitude and heading information of the aircraft from the platform.

The strapdown inertial navigation system is to directly connect the accelerometer and gyroscope to the aircraft, instead of using a mechanical gyro-stabilized platform. Determine the input axes of the accelerometer and gyroscope according to the directions of the roll axis, pitch axis and yaw axis of the aircraft,

Using the attitude matrix, the acceleration and angular velocity information of the aircraft along the axis of the body coordinate system measured by the three-axis accelerometer and the three-axis gyroscope are transformed into the navigation coordinate system, and then the speed, position and attitude information of the aircraft are obtained.

Compared with the two inertial navigation systems, the strapdown inertial navigation system has an autonomous navigation method, and the reliability of the system is higher than that of the platform inertial navigation system.

2. Navigation system fusion algorithm

In view of the complementary characteristics of the frequency domain characteristics of the accelerometer and the gyroscope, the original data of the two can be fused for attitude calculation. Two common fusion algorithms, complementary filtering and Kalman filtering, are introduced below.

1. Complementary filtering:

Complementary filters are usually used to fuse data with similar or the same physical meaning measured by different sensors. Corresponding preprocessing is required for multiple input quantities, that is, for input quantities with high-frequency noise, a low-pass filter needs to be used. filter. In aircraft attitude measurement, low-pass filtering is usually required for the variables input by the accelerometer, and high-pass filtering is required for the variables input by the gyroscope.

The following figure is a block diagram of a common second-order complementary filtering algorithm.

Multi-sensor fusion and attitude calculation of UAV navigation system

In the second-order complementary filtering, the difference between the output value x and the measurement value y1 of the accelerometer is made, and then the difference value is processed by the PI controller, and the measurement value of the gyroscope is corrected in the form of negative feedback, which simplifies the calculation of the attitude angle. The formula is

Multi-sensor fusion and attitude calculation of UAV navigation system

Among them, Anglek is the angle after the k-th filtering, Anglek-1 is the angle after the k-1 filtering, angle_rate is the angular velocity value measured by the gyroscope, and t is the sampling interval time of the gyroscope. Angle_acc is the angle measurement value of the accelerometer, a is the high-pass filter coefficient, and b is the low-pass filter coefficient, both of which are fixed values, and the weights will not change during system operation.

2. Kalman filter

Kalman filtering is the optimal estimation with the minimum mean square error as the standard. First, the control process of the discrete system is expressed by the following equation.

Multi-sensor fusion and attitude calculation of UAV navigation system

k represents discrete time, X(K) represents the state at time k, U(k) represents the control quantity at time k, and W(k) represents the disturbance quantity, which is expressed in the form of Gaussian white noise , and the covariance of the interference quantity is denoted as Q. M and N represent the inherent parameters of the system. When the system is a multi-model system, M and N are in the form of multi-dimensional vectors. Z(k) represents the measurement value at time k, V(k) represents the interference signal at the time of measurement, and its covariance is recorded as R, and P is an inherent parameter of the measurement system.

Kalman filter process:

First, estimate the state quantity x(k|k-1) at the next moment according to the state quantity X(k-1) of the system at time k-1. According to the overall operating mechanism of the system, there are:

Multi-sensor fusion and attitude calculation of UAV navigation system

Then estimate the covariance of x(k|k-1), S(k|k-1) represents the covariance of X(k|k-1), and S(k-|k-1) represents x( k-1|k-1) covariance.

Multi-sensor fusion and attitude calculation of UAV navigation system

fusing measured and estimated values,

Multi-sensor fusion and attitude calculation of UAV navigation system

x(k|k-1) represents the optimal state value calculated by the system at time k. G(k) is the Kalman gain, and its meaning is whether to trust the measured value or the estimated value more when guiding data fusion.

So how is G(k) calculated?

Multi-sensor fusion and attitude calculation of UAV navigation system

Different from complementary filtering, the parameters in Kalman filtering are not fixed, which are related to the inherent parameters of the system and the amount of interference.Further, in order to maintain the dynamic real-time update of the system, the covariance of x(k|k) needs to be updated, namely

Multi-sensor fusion and attitude calculation of UAV navigation system

3. Optical flow navigation system

In addition to the common strapdown inertial navigation system, the optical flow navigation system is also a navigation system that has been widely used and studied in recent years. The optical flow navigation system is a related algorithm designed to imitate the changes of the biological eyes to perceive the light. It associates the pixels in the two-dimensional image with the three-dimensional carrier through mathematical modeling, and calculates the motion information of the carrier through the changes of the continuous frames of the optical flow image. The research team of Brigham Young University in the United States applied optical flow navigation technology to drones for the first time, using optical mouse technology and laser ranging.

In the optical flow navigation system, when using computer vision for navigation, the relationship between the feature points of two consecutive frames is usually used to establish changing coordinates, thereby judging the motion information of the UAV. The key to position judgment is to solve the extraction and matching of feature points between images, and thus establish a position estimation equation based on feature points, and design a corresponding optical flow navigation algorithm.

Multi-sensor fusion and attitude calculation of UAV navigation system
(Optical flow sensor, source: https://www.jianshu.com/p/b7499050dc10)

The optical flow data can be obtained based on the feature point algorithm, but it is only the coordinate information, which needs to be calibrated and transformed accordingly.

In the optical flow sensor camera calibration, in order to obtain the transformation relationship between the actual object and the object in the image, it is necessary to obtain the internal parameter matrix and external parameter matrix of the camera, that is, use a camera of a certain size to take pictures of the same object from different perspectives , and then obtain the internal and external parameter matrix of the camera through these images. The internal parameter matrix is ​​determined by the camera itself, and the external parameter matrix is ​​determined by different angles of taking pictures.

Through the internal and external parameter matrix, the corresponding relationship between the optical flow sensor on the UAV and the optical flow field can be obtained, and the optical flow information can be converted into the speed information of the UAV. In order to obtain the motion information of the UAV, it is necessary to use the height information of the UAV and the transformation matrix of the carrier coordinates and the ground coordinates.

Because when the light conditions are not good, only using the optical flow sensor will lead to a large error. Therefore, the optical flow navigation system is often used in combination with the inertial navigation system, which can complement each other. The optical flow/inertial loose coupling model uses the optical flow to establish a relationship with the speed in the inertial navigation system, and uses the optical flow system to correct the data of the inertial navigation system, as shown in the figure below.

Multi-sensor fusion and attitude calculation of UAV navigation system

Using the difference between the speed of the optical flow navigation system and the speed of the inertial navigation system, the speed measurement equation is obtained

Multi-sensor fusion and attitude calculation of UAV navigation system

Zp is the observation matrix, that is, the data provided by the inertial navigation system and the optical flow navigation system, Hp is the measurement transition matrix, Vp is the measurement noise matrix, and X is the state vector at time t. VE-nE and VN-nN represent the speed difference between the inertial navigation system and the optical flow navigation system, and h-hn represent the position difference between the inertial navigation system and the optical flow navigation system.

Advances in theories such as navigation technology and MEMS technology have reduced the production, use and maintenance costs of UAVs, making the application scope of UAVs gradually expanded. At the same time, the safety issues of UAVs cannot be ignored, especially in complex situations. It is even more necessary to improve the stability of flight, and multi-sensor fusion and attitude calculation have important research and application significance.

The Links:   FLC48SXC8V-05 LM8V302R

Related Posts