Iterative attitude solution filtering method for multi-rotor unmanned aerial vehicle

Abstract

The invention discloses an iterative attitude solution filtering method for a multi-rotor unmanned aerial vehicle, and belongs to the technical field of unmanned aerial vehicle control. The problems that an existing unmanned aerial vehicle attitude filtering calculation method is low in precision, and stable flight and control of an aircraft are difficult to achieve are solved. The invention provides a method for adaptively adjusting a filtering parameter noise covariance. The method is used for adaptively estimating two noise covariances Q and R which are originally uncertain. According to the method, the principle of a small-batch gradient descent method is introduced into the UKF algorithm, so that Q and R in the UKF algorithm are changed into optimal values from empirical estimation values, the real-time estimation precision and stability of noise are improved, and the attitude calculation precision and robustness of a traditional UKF algorithm are greatly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle control, and in particular to a novel multi-rotor unmanned aerial vehicle attitude calculation and filtering method. Background Art

[0002] Attitude information is one of the most critical parameters in the navigation system. Attitude measurement systems are widely used in aerospace, drones, robots, motion tracking and other fields. Attitude solution is the basis for aircraft navigation and positioning. To achieve stable flight and control, the accuracy of attitude solution must be guaranteed.

[0003] The attitude calculation of multi-rotor UAVs is carried out by accelerometers, gyroscopes, magnetometers and other sensors to build an attitude calculation platform to solve the real-time attitude parameters of the UAV. However, gyroscope measurement will produce drift, poor long-term static properties, and insignificant short-term drift errors; accelerometer measurement will produce high-frequency noise, resulting in poor short-term accuracy; magnetometers are subject to greater external interference and have poor stability. As time accumulates, the system's own errors are large, which seriously affects the accuracy of the calculation.

[0004] In order to solve the noise problem of sensors, filtering algorithms are usually used to solve attitude information and reduce the interference of noise errors on the attitude calculation of multi-rotor unmanned aerial vehicles. Therefore, scholars at home and abroad have studied many filtering algorithms.

[0005] In 2018, Yang Qingquan et al. from Zhejiang University published a paper titled "AFast Adaptive-Gain Complementary Filter Algorithm for Attitude Estimation of an Unmanned Aerial Vehicle" in the Journal of Navigation. The paper proposed a new fast adaptive gain complementary filtering algorithm for attitude estimation of unmanned aerial vehicles (UAVs), which reduces attitude errors and computational complexity. However, the complementary filtering algorithm has difficulty in selecting the frequencies of high-pass and low-pass filters, so the accuracy of attitude solution is not high.

[0006] In 2017, Daero Lee et al. from York University in Canada published a paper titled "Robust Adaptive Unscented Kalman Filter for Spacecraft Attitude Estimation Using Quaternion Measurements" in the Journal of Aerospace Engineering. The paper proposed a multiplicative adaptive unscented Kalman filter algorithm that uses quaternions to represent attitude, which is used for spacecraft attitude estimation under measurement failure conditions.

[0007] Xin Yanfeng and others from Hefei Saiwei Intelligent Co., Ltd. published the invention patent "A method and system for calculating the attitude of unmanned aerial vehicles based on least squares and EKF" (patent number: CN202010982173.3), which imports the IMU data processed by the least squares method into the extended Kalman filter algorithm module to calculate the attitude of the unmanned aerial vehicle. However, EKF ignores the high-order terms of the nonlinear function after Taylor expansion, and the algorithm accuracy is limited.

[0008] Cai Anjiang and others from Xi'an University of Architecture and Technology published a method for solving the attitude calculation of a small multi-rotor UAV (patent number: CN201810275931.0). The method uses an improved UKF filtering algorithm to process acceleration data and magnetometer data in two stages, thereby reducing the dimension of the measurement vector and the amount of calculation. However, this method has the problem of sensor noise interference.

[0009] In summary, the existing technology still has the problems of low calculation accuracy and poor adaptability. Since the statistical characteristics of system noise and measurement noise are unknown and time-varying, the accuracy of conventional filtering algorithms is reduced, the algorithm optimality is lost, and even the filtering diverges. Summary of the invention

[0010] In view of the defects and shortcomings of the above-mentioned prior art, the present invention proposes an iterative attitude solution filtering method. It mainly includes the following steps:

[0011] First, the North-East right-hand coordinate system is used to build the UAV attitude solution system model, and the quaternion attitude matrix q of the gyroscope data is used as the state vector of the system Taking into account the gyro offset caused by environmental factors, the state equation is established , expressed as:

[0012] (1)

[0013] (2)

[0014] In the formula, is the state estimate at time k, A is the state transfer matrix, and Gyroscope data , , and the gyroscope offset. is the process noise vector that follows a Gaussian distribution.

[0015] The observation equation is established based on the data of the three-axis accelerometer and the three-axis magnetometer to correct the cumulative error of the attitude angle caused by the gyroscope drift. The three-axis accelerometer is used to measure the three-axis acceleration a of the carrier in motion, and the gravity acceleration in the geographic coordinate system is ; The three-axis magnetometer can measure the three components of the Earth's magnetic field strength in the body coordinate system The outputs of the accelerometer and magnetometer are expressed as:

[0016] (3)

[0017] in, is the rotation matrix from the geographic coordinate system to the body coordinate system, select the observation Build z a represents the output of the accelerometer, a n is the gravity component, n a is the zero-mean white noise of the accelerometer; z m represents the output of the magnetometer, m n is the magnetic field intensity component, n m is the zero-mean white noise of the magnetometer.

[0018] The observation equation of the system , specifically expressed as:

[0019] (4)

[0020] Where H is the measurement coefficient matrix, is the observation noise vector that follows a Gaussian distribution;

[0021] in, and are mutually uncorrelated zero-mean Gaussian white noises, and their noise covariance matrices are , .

[0022] Second, for the two noise covariances we need to optimize =Qsum =R, and perform a small batch gradient descent algorithm to obtain the optimal value. First, the root mean square error (RMSE) obtained by comparing the reference data of the posture with the solution data obtained through the experiment is selected as the objective function:

[0023] (5)

[0024] In the formula, is the number of sampling points, is the solved attitude angle, is the reference attitude angle.

[0025] The objective function obtains the partial derivatives of the two parameters we need to solve and the gradients are:

[0026] (6)

[0027] In the formula, is the number of sampling points, , is the i-th input x value when taking partial derivatives of two parameters respectively.

[0028] Using the obtained gradient to iteratively update the parameters Q and R, we have:

[0029] (7)

[0030] When the objective function converges to the minimum value, the optimal value can be obtained. and .

[0031] Third, by referring to the unscented transformation principle in the unscented Kalman filter, the optimal covariance obtained by iteration is propagated.

[0032] (1) According to the state equation set in the first step, determine the initial state estimate and its covariance According to the dimension of the state vector, a set of sigma sampling points and their corresponding weights are obtained through UT transformation.

[0033] (8)

[0034] In the formula, is the scaling parameter, , , It is a parameter to be selected. represents the ith column of the matrix square root.

[0035] (9)

[0036] In the formula, The sampling point weights, The variance weight of the sampling point.

[0037] (2) Calculate the one-step prediction of 2n+1 sigma point sets and get (i=1,2,...,2n+1); According to the weighted summation of the predicted values ​​of the Sigma point set, the one-step prediction and covariance matrix of the system state quantity are obtained:

[0038] (10)

[0039] (3) Based on the one-step predicted value of the system state quantity, the UT transformation is used again to generate a new sigma point set.

[0040] (11)

[0041] (4) Substitute the predicted sigma point set into the observation equation to obtain the predicted observation quantity:

[0042] (12)

[0043] (5) The predicted value and covariance matrix of the observed variables are obtained by weighted summation:

[0044] (13)

[0045] (6) Calculate the Kalman filter gain:

[0046] (14)

[0047] (7) Finally, calculate the state update and covariance update:

[0048] (15)

[0049] In the formula, is the actual measurement value of the accelerometer, , only update the quaternion, is the optimal attitude angle after filtering.

[0050] The benefit of the present invention lies in that it innovatively proposes a method for adaptively adjusting the noise covariance of the filter parameters, and adaptively estimates the two uncertain noise covariances Q and R. By introducing the principle of mini-batch gradient descent into the UKF algorithm, Q and R in the UKF algorithm are changed from empirical estimates to optimal values, the real-time estimation accuracy and stability of the noise are improved, and the attitude solution accuracy and robustness of the traditional UKF algorithm are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 This is a schematic diagram of the multi-rotor UAV attitude solution method of the present invention.

[0052] Figure 2 This is an experimental flow chart of the multi-rotor UAV attitude solution method of the present invention.

[0053] Figure 3 It is an experimental platform for the attitude calculation method of multi-rotor UAV of the present invention.

[0054] Figure 4 This is the experimental site of the multi-rotor UAV attitude solution method of the present invention.

[0055] Figure 5 This is the experimental result of the multi-rotor UAV attitude solution method of the present invention. DETAILED DESCRIPTION

[0056] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, preferred embodiments are given below and the specific implementation of the present invention is further described in detail in conjunction with the accompanying drawings.

[0057] Example:

[0058] Combined with Figure 1 ,The small batch gradient descent unscented Kalman filter solution method for the attitude of a multi-rotor UAV includes the following steps:

[0059] (1) Collect the flight angular velocity, acceleration and magnetic declination state parameter values ​​of the UAV through the IMU sensor (i.e., a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer integrated chip), and set the data collection cycle of the inertial sensor;

[0060] (2) Based on the previous system state equation and measurement equation, a state space model is established with the attitude quaternion as the state vector:

[0061]

[0062] The state quantity is , and are mutually uncorrelated zero-mean Gaussian white noises, and their noise covariance matrices are , .

[0063] (3) Set the initial state quaternion , the initial state error covariance matrix ;

[0064] (4) Read gyroscope data , , , the value obtained after the quaternion update is taken as the predicted value and is substituted into formula (1) (2) to calculate the state transfer matrix ; Use the k-1 time state to predict the system prior state estimate at time k , the prior state estimation error covariance ;

[0065] (5) Initialized system status A set of sigma sampling points is obtained by formula (8) ;

[0066] (6) Calculate the one-step prediction of 2n+1 sigma point sets according to formula (1) ; Calculate the mean weight using formula (9) and covariance weights , we get the one-step prediction value of the system state quantity in formula (9): and the covariance matrix ;

[0067] (7) Based on the one-step prediction value of the system state quantity in the previous step, the UT transformation is used again to generate a new sigma point set according to formula (9): ;

[0068] (8) Substitute the sigma point set predicted in the previous step into the observation equation of formula (4) to obtain the predicted observation value ; and obtain the predicted value of the observed variable through weighted summation ;

[0069] (9) Read the accelerometer and magnetometer data and obtain according to formula (3): According to formula (5), (6) and (7), the total observation noise covariance is obtained: ;

[0070] (10) Calculate the covariance of the predicted value of the observed variable according to formula (13): and the cross-covariance between the prior state estimate and the predicted value ;

[0071] (11) Calculate the Kalman filter gain matrix according to formula (14): , update the state vector at time k+1 and covariance :

[0072] (12) In the above process, the process noise covariance matrix Q and the observation noise covariance matrix R are updated by formula (5) (6) (7); During the algorithm optimization process, the two parameters are compared with the set threshold value, and the iteration is repeated until the parameter is smaller than the set threshold value, indicating that the global optimal value has been found. The iteration stops and the optimal objective function value at this time is output; the optimal value is obtained and .

[0073] (13) Output attitude angle information. The optimized attitude can be expressed by quaternion method, and the root mean square error RMSE of the objective function is updated, thereby reflecting the solution effect of the adaptive filtering algorithm.

[0074] The above describes in detail the technical scheme of the present invention in combination with the accompanying drawings. The technical scheme of the present invention proposes a small-batch gradient descent unscented Kalman filter solution method for a multi-rotor UAV attitude. Based on the unscented Kalman filter model, the noise covariance is adaptively optimized by the small-batch gradient descent method. The accurate values ​​of the noise covariance Q and the observed noise covariance R are obtained through repeated iterations. The uncertainty of the parameters of the traditional unscented Kalman filter calculation method for multi-rotor UAV data attitude solution is optimized to a certain extent, and a more complete adaptive unscented Kalman filter calculation method model is established.

[0075] The above embodiments are only used to illustrate the present invention, but not to limit the present invention. A person skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, all equivalent technical solutions also belong to the scope of the present invention, and the patent protection scope of the present invention shall be defined by the claims.

[0076] The technical contents not described in detail in the present invention are all known technologies.

Claims

1. An iterative attitude solution filtering method for a multi-rotor UAV adopts quaternion as state vector, uses gyroscope data to establish state equation to update the state vector, uses acceleration data and magnetometer data to establish observation equation, and corrects the cumulative error of attitude angle caused by gyroscope drift; uses small batch gradient descent method to adaptively estimate the two uncertain noise covariances Q and R, so that they change from empirical estimated values ​​to optimal values; uses an improved unscented Kalman filtering method to solve the UAV attitude, propagates the iterative optimal covariance, updates the quaternion attitude, and obtains the solved attitude angle of the quadrotor UAV.

2. The iterative attitude solution filtering method for a multi-rotor UAV according to claim 1, characterized in that: Attitude quaternion A state space model is established by combining the state equation and the observation equation as the state vector. The roll angle, pitch angle and yaw angle obtained by the accelerometer and magnetometer are used to correct the attitude angle updated by the gyroscope. This model can reveal the changing law of the system state. （1） Among them, the quaternion method has the advantages of small calculation amount, easy operation and no singularity problem in the Euler angle method, which reduces the computing load of the processor.

3. The iterative attitude solution filtering method for a multi-rotor UAV according to claim 1, characterized in that: The root mean square error (RMSE) obtained by comparing the reference data of the posture with the solution data obtained through the experiment is selected as the objective function, and the noise covariance Q and R are subjected to the small batch gradient descent algorithm to obtain the optimal value （2） in, is the number of sampling points; The mini-batch gradient descent method not only reduces the amount of calculation and controls possible oscillations, but also has a fast calculation speed and can converge more stably. This method is used to improve the real-time estimation accuracy and stability of noise.

4. The iterative attitude solution filtering method for a multi-rotor UAV according to claim 1, characterized in that: By referring to the unscented transformation principle in the unscented Kalman filter, the optimal covariance obtained by iteration is propagated; the state vector Get a set of sigma sampling points through UT transformation and its weights; （3） In the formula, is the scaling parameter, , , is the parameter to be selected; （4） In the formula, The sampling point weights, Sampling point variance weight; Calculate the one-step prediction of 2n+1 sigma point sets and get (i=1,2,...,2n+1), calculate the one-step prediction value and covariance matrix of the system state quantity: （5） Similarly, in calculating the covariance of the predicted value of the observed variable, the optimal covariance obtained by iteration is propagated and ; Based on the prediction of a set of sigma points, the weighted mean of them is calculated to obtain a one-step prediction of the system state quantity; by combining the small batch gradient descent method and the unscented transformation, the overall final filtering effect is better, and the attitude solution accuracy and robustness of the traditional UKF algorithm are improved.

Images