Four-rotor unmanned aerial vehicle parameter identification method based on adaptive extended Kalman filtering

Abstract

A quad-rotor unmanned aerial vehicle parameter identification method based on adaptive extended Kalman filtering collects flight data through an unmanned aerial vehicle self-loaded sensor, and performs parameter identification work by using actual flight data in combination with an adaptive extended Kalman filtering algorithm inferred by variational Bayesian. The method comprises the steps of firstly converting a nonlinear system state equation and an observation equation into linear equations, then deriving a state, a prediction error and a measurement noise covariance matrix based on a variational Bayesian (VB) machine learning method by selecting inverse Warshard (IW) prior, and finally correcting an updating process of an EKF algorithm by utilizing the obtained covariance. According to the method, a relatively low parameter identification error can be achieved under the condition that a measurement error is unknown.

Description

Technical Field

[0001] The present invention relates to the field of parameter identification of quadrotor drones, and specifically provides a method for parameter identification of quadrotor drones based on variational Bayesian inference adaptive extended Kalman filtering (VBAEKF). The method improves the EKF algorithm by combining the Bayesian machine learning method, and mainly identifies the three-axis moment of inertia, lift coefficient and anti-torque coefficient of the quadrotor drone. Background Art

[0002] In recent years, UAV technology has developed rapidly and has been widely used in various industries. This has also put forward higher requirements on the control accuracy of UAVs. The premise of improving control accuracy is to have an accurate mathematical model of the UAV. Therefore, accurate modeling of UAVs is a key technical issue in achieving precise control of UAVs.

[0003] Quadcopter drones are typical nonlinear, strongly coupled underactuated systems. In the past, scholars used the least squares method and maximum likelihood estimation method for parameter identification. These methods usually use small perturbation linearization methods to establish the mathematical model of quadcopter drones. Some researchers have also proposed frequency domain and time domain identification methods to identify drone systems. Without exception, these technologies do not consider the nonlinear characteristics of drones, and the identified linear system parameters only conform to the characteristics of drones to a certain extent. Therefore, in order to solve the identification problem of such nonlinear systems, many system identification methods based on intelligent algorithms have been proposed, such as genetic algorithms and hybrid frog leaping algorithms. These methods usually only consider the identification of drones in common working states such as hovering states, cannot adapt to other flight states, and have a slow convergence speed. In addition to the above methods, the augmented Kalman filter algorithm is also used for helicopter parameter identification. It obtains the state space model through balancing and linearization, but this method cannot cope with the uncertainty of process noise and measurement noise, and often requires a lot of time to try the error covariance matrix. Summary of the invention

[0004] In order to solve the problems of insufficient identification accuracy, slow convergence speed and noise uncertainty in the above-mentioned prior art, the present invention combines the Bayesian machine learning method to provide a quadrotor unmanned aerial vehicle parameter identification method based on adaptive extended Kalman filtering, and uses actual flight data combined with an adaptive extended Kalman filtering algorithm of variational Bayesian inference to perform parameter identification. This method is suitable for situations where measurement errors are unknown. First, the nonlinear system state equation and observation equation are converted into linear equations, and then the state, prediction error and measurement noise covariance matrices are derived based on the variational Bayesian (VB) method by selecting the inverse Wishart (IW) prior. Finally, the obtained covariance is used to correct the update process of the traditional EKF algorithm, and finally the unmanned aerial vehicle parameters are identified.

[0005] In order to achieve the above purpose, the technical solution adopted by this application is:

[0006] A method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter comprises the following steps:

[0007] 1) Establish a mathematical model of the UAV;

[0008] 2) Determine the UAV parameters that need to be identified based on the established UAV mathematical model;

[0009] 3) Establish the UAV state augmentation vector and collect UAV flight data;

[0010] 4) The adaptive extended Kalman filter algorithm based on variational Bayesian inference is combined with UAV flight data to identify the UAV parameters and finally obtain the identification results.

[0011] The present invention utilizes the flight data of a quad-rotor UAV and identifies the physical parameters of the UAV based on the proposed variational Bayesian inference adaptive extended Kalman filter algorithm.

[0012] Compared with the prior art, the present invention has the following technical effects: 1. The drone flight data can be directly obtained by using the drone onboard sensors without adding additional costs; 2. This method can estimate the sensor measurement noise covariance matrix and can better cope with external noise. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 It is a flow chart of the parameter identification method of a quadrotor UAV based on adaptive extended Kalman filter.

[0014] Figure 2 This is a comparison chart between VBAEKF and traditional EKF methods for estimating the x-axis moment of inertia of a quadrotor drone.

[0015] Figure 3 This is a comparison chart between VBAEKF and traditional EKF methods for estimating the y-axis moment of inertia of a quadrotor drone.

[0016] Figure 4 This is a comparison chart between VBAEKF and traditional EKF methods for estimating the z-axis moment of inertia of a quadrotor drone.

[0017] Figure 5 It is a comparison chart between VBAEKF and traditional EKF methods for estimating lift coefficient b of quadrotor UAV.

[0018] Figure 6 It is a comparison chart between VBAEKF and traditional EKF methods for estimating the anti-torque coefficient k of a quadrotor UAV. DETAILED DESCRIPTION

[0019] The present invention will be further described below in conjunction with the accompanying drawings.

[0020] Reference Figure 1 to Figure 6 A quadrotor UAV parameter identification method based on adaptive extended Kalman filter can identify the x-axis moment of inertia, y-axis moment of inertia, z-axis moment of inertia, lift coefficient b and anti-torque coefficient k of the quadrotor UAV.

[0021] The method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter in this embodiment comprises the following steps:

[0022] 1) Establish a mathematical model of a quadrotor drone;

[0023] Based on the mechanism method, the quadrotor drone is modeled. According to the Newton-Euler law, the following quadrotor drone mathematical model is established:

[0024]

[0025] Where m is the total mass of the UAV, x, y, and z are the three-axis positions of the UAV in the inertial coordinate system, φ, θ, and ψ are the roll angle, pitch angle, and yaw angle of the UAV, respectively. xx ,I yy ,I zz are the moments of inertia of the drone around the x, y, and z axes, respectively; p, q, and r are the roll, pitch, and yaw angular velocities of the drone, respectively. are the accelerations of the drone in the x, y, and z axes, g is the acceleration of gravity, U1 is the total lift generated by the drone, U2, U3, and U4 are the rolling moment, pitch moment, and yaw moment of the drone, respectively. The expressions are as follows:

[0026]

[0027] Where b is the lift coefficient of the drone, k is the anti-torque coefficient of the drone, d is the distance between the center of the propeller and the center of the drone, ω i The rotation speeds of the drone’s four propellers, i=1, 2, 3, 4.

[0028] 2) Determine the UAV parameters that need to be identified based on the established UAV mathematical model;

[0029] The total mass m of the drone and the distance d between the propeller center and the mass center of the drone are obtained by direct measurement; the acceleration of gravity g is obtained by searching on the Internet; the three-axis moment of inertia of the drone I xx ,I yy ,I zzIt is related to the shape and mass distribution of the aircraft, and generally cannot be directly measured. It is necessary to build a corresponding experimental device and the cost of obtaining accurate numerical values ​​is extremely high. The lift coefficient and anti-torque coefficient are related to the size, structure and air density of the propeller, and are not easy to obtain. Therefore, the three-axis rotation inertia I of the UAV is xx ,I yy ,I zz , the lift coefficient b and anti-torque coefficient k of the UAV are identified;

[0030] 3) Establish the UAV state augmented vector and collect UAV flight data. The process is as follows:

[0031] 3.1) Based on the analysis in 2), the following drone state augmentation vector is established:

[0032] x=[xyzuvw φ θ ψ pqr I xx I yy I zz bk] T

[0033] Among them, u, v, and w are the linear velocities of the drone in the x, y, and z axes respectively;

[0034] 3.2) Collection of drone data: Usually, drones are equipped with a set of sensors, which at least include an inertial measurement unit (IMU) and a visual positioning system or a GPS system or an optical flow positioning system or even a motion capture system. The IMU usually includes an accelerometer, a gyroscope, a magnetometer and a barometer. The data required for parameter identification in the present invention include the drone position, linear velocity, attitude angle, angular velocity data and the propeller speed of the drone during flight.

[0035] 4) The adaptive extended Kalman filter algorithm based on variational Bayesian inference is used to identify the UAV parameters in combination with the UAV flight data. The process is as follows:

[0036] 4.1) Consider the following nonlinear system, whose state equation and observation equation are as follows:

[0037]

[0038] Among them, f k-1 is the state transfer function, h k is the measurement function, u k-1 is the input vector, x k is the system state vector, w k-1 is the process noise, v k To measure the noise, the noise conforms to the Gaussian distribution;

[0039] According to the above nonlinear system equation, solve its Jacobian matrix: Assume that the process noise w at each moment k-1 and measurement noise v k are all 0, then we get:

[0040]

[0041] Expand the state function and measurement function according to the first-order Taylor formula to obtain the following Jacobian matrix:

[0042]

[0043] Among them, F k-1 and w k-1 is the Jacobian matrix of the process at k-1 time, is the state posterior value at time k-1, H k and V k is the measurement Jacobian matrix at time k, is the state prior value at time k. The required Jacobian matrix is ​​used in the subsequent prediction and correction processes;

[0044] 4.2) Prediction process:

[0045]

[0046] in, is the state prior value at time k, is the state posterior value at time k-1, F k-1 is the Jacobian matrix of the process at k-1 moment; is the prior error covariance calibration value at time k, P k-1∣k-1 is the posterior error covariance at time k-1, is the process noise covariance calibration value at time k-1;

[0047] 4.3) Variational measurement update process

[0048] Before the update process, initialize the following parameters:

[0049]

[0050]

[0051] in, is the initial value of the state, is the initial value of the error covariance, is the inverse Wishart distribution (IW) parameter value for updating the error covariance, is the parameter value of the inverse Wishart distribution (IW) to update the measurement noise covariance, n is the system state dimension, τ is the adjustment parameter, and m is the observation dimension;

[0052] The process of 4.3) is as follows:

[0053] 4.3.1) IW distribution parameter update of error covariance

[0054]

[0055] Among them, P k∣k-1 is the prior error covariance at time k-1, q (i+1) (P k∣k-1 ) indicates that after the i+1th iteration, P k∣k-1 The probability density function of , IW represents the inverse Wishart distribution, are all IW distribution parameters, represents the prior value of the t parameter at time k, Represents the prior value of the T parameter at time k. (i=1,2,3...N-1) This part prepares for the subsequent update of the error covariance;

[0056] 4.3.2) IW distribution parameter update of measurement noise covariance

[0057]

[0058] Among them, R k is the measurement noise covariance at time k, q (i+1) (R k ) indicates that R k The probability density function of , IW represents the inverse Wishart distribution, are all IW distribution parameters, represents the prior value of the u parameter at time k, Represents the prior value of the U parameter at time k. This part prepares for the subsequent update of the measurement noise covariance;

[0059] 4.3.3) Error covariance, measurement noise covariance and state estimate update;

[0060] The error covariance is updated as follows:

[0061]

[0062]

[0063] in, Indicates P k∣k-1 The inverse, Indicates that after the i+1th iteration The expected value of represents the error covariance after the i+1th iteration at time k-1;

[0064] The update of the measurement noise covariance is updated as follows:

[0065]

[0066] in, Represents R k The inverse, Indicates that after the i+1th iteration The expected value of represents the measurement noise covariance after the i+1th iteration at time k;

[0067] Kalman gain solution:

[0068]

[0069] State estimate update:

[0070]

[0071] in, Represents the state estimate after the i+1th iteration at time k. Represents the error covariance estimate after the i+1th iteration at time k;

[0072] When i=N-1, it means the current iteration is over, and the following output is obtained:

[0073]

[0074] in, That is, the estimated value of the state quantity at the current moment, P k∣k is the estimated value of the error covariance at the current moment, is the estimated value of the IW distribution parameter at the current moment, which serves as the initial value of the iteration at the next moment.

[0075] The contents described in the embodiments of this specification are merely enumerations of implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter, characterized in that: The method comprises the following steps: 1) Establish a mathematical model of the UAV; 2) Determine the UAV parameters that need to be identified based on the established UAV mathematical model; 3) Establish the UAV state augmentation vector and collect UAV flight data; 4) The adaptive extended Kalman filter algorithm based on variational Bayesian inference is combined with UAV flight data to identify the UAV parameters and finally obtain the identification results.

2. The method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter according to claim 1, characterized in that: The process of step 1) is as follows; According to Newton-Euler law, the following mathematical model of quadrotor drone is established Where m is the total mass of the UAV, x, y, and z are the three-axis positions of the UAV in the inertial coordinate system, φ, θ, and ψ are the roll angle, pitch angle, and yaw angle of the UAV, respectively. xx ,I yy ,I zz are the moments of inertia of the drone around the x, y, and z axes, respectively; p, q, and r are the roll, pitch, and yaw angular velocities of the drone, respectively. are the accelerations of the drone in the x, y, and z directions, g is the acceleration of gravity, U1 is the total lift generated by the drone, I2, I3, and U4 are the rolling moment, pitch moment, and yaw moment of the drone, respectively. The expressions are as follows: Where b is the lift coefficient of the drone, k is the anti-torque coefficient of the drone, d is the distance between the center of the propeller and the center of the drone, ω i is the rotation speed of the four propellers of the drone, i=1, 2, 3, 4.

3. The method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter according to claim 1 or 2, characterized in that: The process of step 2) is as follows: Through the model analysis of step 1), the three-axis rotational inertia of the drone I xx ,I yy ,I zz , lift coefficient and anti-torque coefficient are not easy to obtain or the cost of obtaining is high; therefore, the three-axis rotation inertia I xx ,I yy ,I zz , lift coefficient b and anti-torque coefficient k are identified.

4. The method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter according to claim 1 or 2, characterized in that: The process of step 3) is as follows: 3.1) Establish the following drone state augmentation vector: x=[x y z u v w φ θ ψ p q r I xx I yy I zz b k] T The parameters to be identified are regarded as the drone state, and the other parameters to be identified are regarded as constants, so their derivatives are all 0; 3.2) UAV data collection: UAV data collection can be completed only through airborne sensors. The required data includes the UAV position, linear speed, attitude angle, angular velocity data and propeller speed during the flight of the UAV.

5. The method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter according to claim 1 or 2, characterized in that: The process of step 4) is as follows: S4.1) Consider the following nonlinear system, whose state equation and observation equation are as follows: Among them, f k-1 is the state transfer function, h k is the measurement function, u k-1 is the input vector, x k is the system state vector, w k-1 is the process noise, v k To measure the noise, the noise conforms to the Gaussian distribution; According to the above nonlinear system equation, solve its Jacobian matrix: Assume that the process noise w at each moment k-1 and measurement noise v k are all 0, then we get: Expand the state function and measurement function according to the first-order Taylor formula to obtain the following Jacobian matrix: Among them, F k-1 and W k-1 is the Jacobian matrix of the process at k-1 time, is the posterior estimate of the state at time k-1, H k and V k is the measurement Jacobian matrix at time k, is the state prior value at time k, and the required Jacobian matrix is ​​used in the subsequent prediction and correction processes; S4.2) Prediction process: in, is the state prior value at time k, is the state posterior value at time k-1, F k-1 is the Jacobian matrix of the process at k-1 moment; is the prior error covariance calibration value at time k, P k-1∣k-1 is the posterior error covariance at time k-1, is the process noise covariance calibration value at time k-1; 4.3) Variational measurement update process Before the update process, initialize the following parameters: in, is the initial value of the state, is the initial value of the error covariance, is the inverse Wishart distribution (IW) parameter value for updating the error covariance, is the inverse Wishart distribution IW parameter value that updates the measurement noise covariance, n is the system state dimension, τ is the adjustment parameter, and m is the observation dimension.

6. The method for identifying parameters of a quadrotor drone based on an adaptive extended Kalman filter as claimed in claim 5, characterized in that: The process of 4.3) is as follows: 4.3.1) IW distribution parameter update of error covariance in, P k∣k-1 is the prior error covariance at time k-1, q (i+1) (P k∣k-1 ) indicates that after the i+1th iteration, P k∣k-1 The probability density function of , IW represents the inverse Wishart distribution, are all IW distribution parameters, represents the prior value of the t parameter at time k, Represents the prior value of the T parameter at time k, i = 1, 2, 3...N-1, this part is prepared for the subsequent update of the error covariance; 4.3.2) Update of IW distribution parameters of measurement noise covariance Among them, R k is the measurement noise covariance at time k, q (i+1) (R k ) indicates that R k The probability density function of , IW represents the inverse Wishart distribution, are all IW distribution parameters, represents the prior value of the u parameter at time k, Represents the prior value of the U parameter at time k, which is used to prepare for the subsequent update of the measurement noise covariance; 4.3.3) Error covariance, measurement noise covariance and state estimate update: The error covariance is updated as follows: in, Indicates P k∣k-1 The inverse, Indicates that after the i+1th iteration The expected value of represents the error covariance after the i+1th iteration at time k-1; The update of the measurement noise covariance is updated as follows: in, Represents R k The inverse, Indicates that after the i+1th iteration The expected value of represents the measurement noise covariance after the i+1th iteration at time k; Kalman gain solution: State estimate update: in, represents the estimated state value after the i+1th iteration at time k, represents the error covariance estimate after the i+1th iteration at time k; When i=N-1, it means the current iteration is over, and the following output is obtained: in, That is, the estimated value of the state quantity at the current moment, P k∣k is the estimated value of the error covariance at the current moment, is the estimated value of the IW distribution parameter at the current moment, which serves as the initial value of the iteration at the next moment.

Images