WO2018028711A1

WO2018028711A1 - Method for estimating noise covariance of unmanned aerial vehicle

Info

Publication number: WO2018028711A1
Application number: PCT/CN2017/097384
Authority: WO
Inventors: 刘国良; 邬静云; 黄涛; 张瑞
Original assignee: 贵州火星探索科技有限公司
Priority date: 2016-08-12
Filing date: 2017-08-14
Publication date: 2018-02-15
Also published as: CN107729585B; CN107729585A

Abstract

A method for estimating a noise covariance of an unmanned aerial vehicle combines controlled variables and observation values by using an extended Kalman filter to linearize an unmanned aerial vehicle dynamic model to obtain a linearized state estimation equation. A coefficient matrix obtained from the linearized state estimation equation, a Kalman gain, and an innovation are substituted into an auto-covariance least squares algorithm to obtain a covariance matrix composed of an expected value of the innovation, and an estimated value of an auto-covariance matrix composed of the expected value of the innovation. Then, after employing a semidefinite programming algorithm, a process noise covariance and a measurement noise covariance are obtained. The method has high estimation accuracy for process noise covariance Q and measurement noise covariance R, and provides a numerical reference for operators. Thereby, it is more convenient and quicker for general operators to debug the process noise covariance Q and the measurement noise covariance R.

Description

Method for estimating noise covariance of drone

Technical field

The present invention relates to the field of unmanned aerial vehicles, and more particularly to a method for estimating the noise covariance of a drone.

Background technique

The unmanned aircraft, referred to as the "unmanned aerial vehicle", is a non-manned aircraft operated by radio remote control equipment and its own program control device. It is widely used in military and civilian fields, such as airborne warning, agriculture, geology. , weather, electricity, disaster relief, video shooting and other industries. As a drone operated by a self-contained program control device, it has strong coupling, nonlinearity, high control difficulty, and the dynamic modeling is complicated. The disturbances faced by the control and operation of the drone include: external disturbance torque such as gravity, and system noise in the drone dynamics model and observation noise in the measurement equation, etc. Interference needs to be estimated by high-precision models and algorithms to achieve high-precision control.

However, in the prior art, a filtering algorithm for processing process noise and measurement noise of a drone, such as a Kalman filter, generally obtains a covariance matrix value of noise using an empirical value or a method of debugging when the noise characteristic is unknown, that is, In the prior art, the covariance of the process noise in the UAV dynamics model and the covariance of the observed noise in the measurement equation need to be debugged to achieve the best effect. Therefore, the prior art has the following drawbacks: the covariance matrix of the noise needs to pass the empirical value or The method of debugging can be obtained, and the more experienced operators are required. The requirements of the operator are high, and if the noise covariance deviation given by the debugging is too large, the filtering value will directly lead to the recursive process. Even the whole process is more and more deviated from the actual value, causing the filter to diverge, causing the flight of the drone to deviate from the target result.

The self-covariance least squares theory proposed by Odelson in 2003 can effectively estimate the covariance matrix of noise and save debugging time. The auto-covariance least squares theory algorithm is currently used for noise estimation in the chemical industry and has not been used in the field of drones. Rajamani et al. proposed a linear time-varying auto-covariance least squares method for nonlinear systems in 2011. The invention combines the above method with the UAV dynamic model to estimate the process noise and observed noise of the UAV.

Summary of the invention

The technical problem of the present invention is to overcome the existing filtering algorithm for processing UAV process noise and measurement noise. When the noise characteristics are unknown, it is necessary to obtain the covariance matrix value of the noise by using the empirical value or by debugging. For the problem that the operator has higher requirements and is easy to cause deviation, an estimation method that can automatically estimate the process noise and the covariance matrix value caused by the observation is designed.

The technical solution adopted by the present invention to solve the technical problem thereof is to provide a method for estimating the noise covariance of a drone, which is characterized in that the self-covariance least squares theory method is used in the dynamic model of the drone The process noise and the characteristics of the observed noise in the measurement equation are estimated, including the following steps:

S1, establishing a drone dynamics model;

S2, using an extended Kalman filter combined with the control quantity and the observation value to linearize the UAV dynamic model to obtain a linearized state estimation equation; and according to the linearized state estimation equation Coefficient matrix, Kalman gain, and innovation;

S3. Substituting the coefficient matrix, the Kalman gain, and the innovation into the auto-covariance least squares algorithm to obtain a covariance matrix formed by the expected value of the innovation and the self-consistion formed by the expected value of the innovation. Estimate of the variance matrix;

S4. Using the covariance matrix formed by the expected value of the innovation and the estimated value of the autocorrelation matrix formed by the expected value of the innovation, the covariance of the process noise and the covariance of the measurement noise are obtained through a semi-definite programming algorithm. .

Further, the step S1 further includes the following steps:

S11. The linear motion equation established by the drone dynamics model in the ground coordinate system and the rotational equation established in the body coordinate system are:

Where m is the mass of the drone and X is the position vector of the drone.

For the acceleration vector of the drone, T is the thrust generated by the rotor, M is the torque generated by the rotor, J is the moment of inertia of the drone, and ω is the angular velocity of the drone.

For the angular acceleration of the drone, G is the gravity received by the drone, and F is the air resistance received by the drone.

Where L is the distance from the center of the rotor to the X-axis or the Y-axis, Ω ₁ , Ω ₂ , Ω ₃ , and Ω ₄ are the rotational speeds of the four blades, respectively, and b and d are the tensile and torque coefficients of the blade, respectively;

S12. The angle between the body axis X and the horizontal plane is defined as the pitch angle θ, and the head is positive; the angle between the projection of the body axis X on the horizontal plane and the X _E axis is defined as the yaw angle.

The right angle of the nose is positive; the angle between the body axis Z and the vertical plane passing through the X axis of the body is defined as the roll angle φ, and the drone is rightly turned to be positive, and the three angles are Euler angles;

make

Then the coordinate transformation matrix from the body coordinate system to the ground coordinate system is

Then the dynamic model of the drone is written as

among them,

The acceleration of the drone in the direction of the ground coordinate system X _E , Y _E , Z _E axis;

They are the angular accelerations of the UAV in the X, Y, and Z directions of the body coordinate system; I _xx , I _yy , and I _zz are the moments of inertia of the X-axis, Y-axis, and Z-axis of the UAV, respectively. F _fx , F _fy , and F _fz are the components of F on the three axes of the body coordinate system, and [ω _x , ω _y , ω _z ] are the components of ω on the three axes of the body coordinate system, [ω _x , ω _y , The relationship between ω _z ] and Euler angle is as follows

Further, the step S2 further includes the following steps:

S21. Write the state equation and the measurement equation of the system into the following general form:

Where x _k is a state quantity, y _k is the observation, the observation is obtained from the sensor, u _k is the control quantity, the control quantity is artificially set, and w _k is the process noise, v _k is the measured noise; w _k ～ N(0, Q) is not related to v _k ～N(0, R), and its state is estimated by equation (5),

Where L _k is the Kalman filter gain, and the innovation is

S22, the equation (5) is

Linearized

Where A _k , B _k , G _k , C _k are all the coefficient matrix,

S23. The linearized state estimation equation is:

Further, the step S3 further includes the following steps:

S31. The expected value of the used interest constitutes an autocovariance matrix.

S32, an estimate of the self-covariance matrix formed by [R(N)] _s and the expected value of the innovation

The square of the difference between the two norms is the optimization goal.

S.t. Q, R≥0, Q=Q', R=R'

among them,

Further, when the noise covariance of the line motion is estimated, the Euler angle and the rotational speed of the four blades are used as the control amount.

Further, when the noise covariance of the diagonal motion is estimated, the rotational speeds of the four blades are taken as the control amount.

One or more technical solutions provided by the present invention have at least the following technical effects or advantages:

The self-covariance least squares theory method can estimate the process noise and process modeling of the drone, and reduce the covariance matrix value of the debugging process noise and observation noise in the prior art, and the requirements for the empirical value and the operator. It can provide operators with a more accurate valuation interval, saving debugging time and ensuring the accuracy of system noise and observation noise.

DRAWINGS

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the embodiments or the description of the prior art will be briefly described below. Obviously, the drawings in the following description are only Is an embodiment of the present invention, and is not for those of ordinary skill in the art On the premise of creative work, other drawings can also be obtained from the provided drawings.

1 is a schematic diagram of a method for estimating noise covariance of a drone according to the present invention;

2 is a schematic view of a ground coordinate system and a body coordinate system in the present invention;

Figure 3 is a position curve of the X _E- axis direction in the ground coordinate system of the present invention;

Figure 4 is a velocity curve in the X _E- axis direction of the ground coordinate system of the present invention;

Figure 5 is a positional curve of the Y _E- axis direction in the ground coordinate system of the present invention;

Figure 6 is a velocity curve in the Y _E- axis direction of the ground coordinate system of the present invention;

Figure 7 is a positional curve of the Z _E- axis direction in the ground coordinate system of the present invention;

Figure 8 is a velocity curve in the Z _E- axis direction of the ground coordinate system of the present invention;

Figure 9 is a graph showing the variation of the roll angle φ of the present invention;

Figure 10 is the rate of change of roll angle of the present invention

Change curve

Figure 11 is a graph showing the variation of the pitch angle θ of the present invention;

Figure 12 is a graph showing the rate of change of the pitch angle of the present invention.

Change curve

Figure 13 is a yaw angle of the present invention

Change curve

Figure 14 is a yaw angle change rate of the present invention

Change curve

detailed description

FIG. 1 is a schematic diagram of a method for estimating noise covariance of a drone of the present invention. First, the present invention includes: step S1, establishing a drone dynamics model, and step S2, using an extended Kalman filter. The control quantity u and the observation value h(x) obtained by the sensor linearize the drone dynamics model; in step S3, the linearized coefficient matrix A, B, C, G, the Kalman gain L and the new interest y are substituted In the linear time-varying-auto-covariance least squares algorithm, the expected value of the obtained new interest constitutes the estimated value of the autocovariance matrix [R(N)] _s and the self-covariance matrix composed of the expected value of the innovation.

Step S4, the expected value of the obtained new interest constitutes an estimate of the autocovariance matrix [R(N)] _s and the self-covariance matrix formed by the expected value of the innovation.

Substituting into the semi-definite programming algorithm, the process noise covariance Q and the measurement noise covariance R are obtained.

Step S1 includes, as shown in FIG. 2, for the "X"-shaped quadrotor UAV, the ground coordinate system and the body coordinate system are established as shown in FIG. 2. Body coordinate system B-XYZ: The origin is taken at the centroid of the drone, and the coordinate system is fixed to the body. The X axis is parallel to the longitudinal axis of the fuselage design and is in the symmetry plane of the drone, pointing to the front; the Y axis is perpendicular to the symmetry plane of the drone pointing to the right; the Z axis is in the symmetry plane of the drone, and perpendicular to The X axis points down. The entire coordinate system conforms to the right-hand rule of the Euler coordinate system. Ground coordinate system EX _E Y _E Z _E : The northeast ground coordinate system is adopted, the X _E axis points to the north side, the Y _E axis points to the east side, and the Z _E axis points vertically downward.

In the case where only the gravity G of the drone, the pulling force T generated by the rotor, and the air resistance F received by the drone are considered, S1 further includes step S11, and the dynamic model of the drone is established as follows. The line motion equations are established in the ground coordinate system and the rotation equations are established in the body coordinate system as follows:

Where m is the mass of the drone and X is the position vector of the drone.

For the acceleration vector of the drone, M is the torque generated by the rotor, J is the moment of inertia of the drone, and ω is the angular velocity of the drone.

The angular acceleration for the drone,

Where L is the distance from the center of the rotor to the X-axis or the Y-axis, Ω ₁ , Ω ₂ , Ω ₃ , and Ω ₄ are the rotational speeds of the four blades, respectively, and b and d are the tensile and torque coefficients of the blade, respectively.

S1 further includes step S12, defining an angle between the body axis X and the horizontal plane as a pitch angle θ, and the head is positive; defining an angle between the projection of the body axis X on the horizontal plane and the X _E axis as a yaw angle

The right side of the nose is positive; the angle between the body axis Z and the vertical plane passing through the X-axis of the body is defined as the roll angle φ, and the drone is right-turned to be positive. These three corners are the Euler angles.

make

Then the dynamic model of the drone is written as

among them,

Step S2 further includes S21 writing the state equation and the measurement equation of the system into the following general form:

Estimating its state, where x _k is the state quantity, y _k is the observation, the observation is taken from the sensor, u _k is the control amount, the control amount is artificially set, w _k For the process noise, v _k is the measurement noise; w _k ～ N(0, Q) is not related to v _k ～N(0, R), and its state is estimated by equation (1),

Where L _k is the Kalman filter gain, innovation

Step S2 further includes S22, and the equation (5) is

Linearized

Where A _k , B _k , G _k , C _k are all the coefficient matrix,

Step S2 further includes S23, and the linearized state estimation equation is

State estimation error is expressed as

And

then

Step S3 further includes S31, and constructing an autocovariance matrix with the expected value of the innovation

Step S3 further includes S31 to

And an estimate of the autocovariance matrix formed by the expected value of the innovation

The square of the difference between the two norms is the optimization goal

S.t. Q, R≥0, Q=Q', R=R'

among them,

Embodiment 1

Since the UAV dynamics model is a 12-dimensional system of equations, when using the covariance least squares theory method to estimate the noise covariance in the model, to reduce the program running time, the noise covariance in the linear motion and the angular motion respectively. Make an estimate. The values of the parameters in the model used in the simulation are as shown in Table 1. Show.

Table 1 parameter values

参数名parameter name	参数值Parameter value	参数名parameter name	参数值Parameter value
无人机质量mDrone mass m	1.5kg1.5kg	重力加速度gGravity acceleration g	9.79m/s² 9.79m/s ²
x轴转动惯量I_xx X-axis moment of inertia I _xx	0.024kg·m² 0.024kg·m ²	y轴转动惯量I_yy Y-axis moment of inertia I _yy	0.024kg·m² 0.024kg·m ²
z轴转动惯量I_zz Z-axis moment of inertia I _zz	0.112kg·m² 0.112kg·m ²	LL	0.232m0.232m
桨叶拉力系数bBlade tension coefficient b	1.0643×10^-5 1.0643×10 ^-5	桨叶扭矩系数dBlade torque factor d	2.3528×10^-7 2.3528×10 ^-7
空气阻力系数kAir resistance coefficient k	-0.2334 -0.2334

1 line movement

The state equation in the linear motion is the linear motion equation of the drone dynamics model, ie

Among them, the state variable is X=[x,y,z,u,v,w] ^T . The Euler angle and the rotational speed of the four motors are taken as the control quantity, [φ, θ, φ] = [0, 0, 0], [Ω ₁ , Ω ₂ , Ω ₃ , Ω ₄ ] = [588.9737, 588.9737, 588.9737 , 588.9737]. The initial value of the state quantity

And set the measurement equation as follows

The zero-mean Gaussian white noise with covariance of Q=2.5×10 ^-5 is added to the online motion model, and Gaussian white noise with covariance of R=10 ^-3 I _6×6 is added to the observation equation, and the ODE45 function is used for 0.001 seconds. The fixed step size generates 10 seconds of observation data.

First, the coefficient matrix and the Kalman gain are obtained by using the extended Kalman filter algorithm combined with the state equation, the control amount and the observation data. Then, the theory of the self-covariance least squares algorithm is applied to give the self-covariance matrix composed of the expected value of the interest rate and the estimated value of the auto-covariance matrix composed of the expected value of the interest. Finally, the semi-definite programming method is used to solve the optimal problem in equation (14), and the obtained process noise covariance Q and measurement noise covariance R are obtained.

2-angle movement

The equation of state in angular motion is the angular motion equation of the dynamic model of the drone, ie

Where the state variable is

The amount of control in angular motion is the speed of four motors, [Ω ₁ , Ω ₂ , Ω ₃ , Ω ₄ ] = [481.5, 481.5, 481.5, 481.5].

The initial value of the state quantity

Set the measurement equation as follows

Zero-mean Gaussian white noise with covariance of Q=2.5×10 ^-5 is added to the online motion model, and Gaussian white noise with covariance of R=10 ^-3 I _6×6 is added to the observation equation, and the ODE45 function is used for 0.001 seconds. The fixed step size generates 10 seconds of observation data. The process of covariance estimation is the same as in line motion.

The simulation results of the first embodiment are as follows:

1 line movement

When using the covariance least squares theory method, the sampling time is set to 0.1 second, the initial estimate of Q is set to 8×10 ^-3 , and the initial estimate of R is set to 3×10 ^-3 I _6×6 with the smallest covariance. The result of the two-squares theoretical method identification is

2-angle movement

Using the covariance least squares theory method, the sampling time is set to 0.1 second, the initial estimation value of Q is set to 3×10 ^-3 , and the initial estimation value of R is set to 3×10 ^-3 I _6×6 . The result of the identification is

The estimated noise characteristics in the line motion and angular motion models are substituted into the extended Kalman filter for filtering. The comparison between the filtered variable curves and the real and observed values of each variable is shown in Figure 3-14. The observations, true values, and values filtered using the extended Kalman filter are plotted in -14. The gray scattered points in the figure are the observed values of the variables, and the black solid lines are the true values. It can be seen that there is an error in the observed values due to the presence of process noise and observed noise. The process noise covariance Q and the measurement noise covariance R identified by the covariance least squares theoretical method algorithm are substituted into the extended Kalman filter algorithm for filtering, and the filtered values indicated by the black dotted lines in the figure are obtained. Although the filtered value still has an error, it is very close to the true value, indicating the covariance least squares The covariance estimated by the method algorithm is more accurate.

In summary, the estimation accuracy of the process noise covariance Q and the measurement noise covariance R is high by the method for estimating the noise covariance of the drone in the present invention. At the same time, the estimation method provides a numerical reference for the operator, so the general operator is more convenient and quicker in debugging the process noise covariance Q and measuring the noise covariance R.

Claims

A method for estimating a noise covariance of a drone, characterized in that it comprises the following steps:

S1, establishing a drone dynamics model;

S2, linearizing the UAV dynamics model with an extended Kalman filter in combination with a control quantity and an observation value to obtain a linearized state estimation equation; and obtaining a coefficient matrix according to the linearized state estimation equation, Karl Man gain and new interest;

S3. Substituting the coefficient matrix, the Kalman gain, and the innovation into the auto-covariance least squares algorithm to obtain a covariance matrix formed by the expected value of the innovation and the self-consistion formed by the expected value of the innovation. Estimate of the variance matrix;

S4. Using the covariance matrix formed by the expected value of the innovation and the estimated value of the autocorrelation matrix formed by the expected value of the innovation, the covariance of the process noise and the covariance of the measurement noise are obtained through a semi-definite programming algorithm. .
The estimating method according to claim 1, wherein said step S1 further comprises the following steps:

S11. The linear motion equation established by the drone dynamics model in the ground coordinate system and the rotational equation established in the body coordinate system are:

Where m is the mass of the drone and X is the position vector of the drone.
For the acceleration vector of the drone, T is the thrust generated by the rotor, M is the torque generated by the rotor, J is the moment of inertia of the drone, and ω is the angular velocity of the drone.
For the angular acceleration of the drone, G is the gravity of the drone, and F is the air resistance of the drone.

Where L is the distance from the center of the rotor to the X-axis or the Y-axis, Ω 1 , Ω 2 , Ω 3 , and Ω 4 are the rotational speeds of the four blades, respectively, and b and d are the tensile and torque coefficients of the blade, respectively;

S12. The angle between the body axis X and the horizontal plane is defined as the pitch angle θ, and the head is positive; the angle between the projection of the body axis X on the horizontal plane and the X E axis is defined as the yaw angle.
The right angle of the nose is positive; the angle between the body axis Z and the vertical plane passing through the X axis of the body is defined as the roll angle φ, and the drone is rightly turned to be positive, and the three angles are Euler angles;

make
Then the coordinate transformation matrix from the body coordinate system to the ground coordinate system is

Then the dynamic model of the drone is written as

among them,
The acceleration of the drone in the direction of the ground coordinate system X E , Y E , Z E axis;

They are the angular accelerations of the UAV in the X, Y, and Z directions of the body coordinate system; I xx , I yy , and I zz are the moments of inertia of the X-axis, Y-axis, and Z-axis of the UAV, respectively. F fx , F fy , and F fz are components of F on the three axes of the body coordinate system, and [ω x , ω y , ω z ] are components of ω on the three axes of the body coordinate system, [ω x , ω The relationship between y , ω z ] and Euler angle is as follows
The estimating method according to claim 1, wherein said step S2 further comprises the following steps:

S21. According to the Kalman filter theory, the state equation and the measurement equation of the system are written as follows:

Where x k is a state quantity, y k is the observation, the observation is obtained from the sensor, u k is the control quantity, the control quantity is artificially set, and w k is the process noise, v k is the measured noise; w k ～ N(0, Q) is not related to v k ～N(0, R), and its state is estimated by equation (5),

Where L k is the Kalman filter gain, and the innovation is

S22, the equation (5) is
Linearized

Where A k , B k , G k , C k are all the coefficient matrix,

S23. The linearized state estimation equation is:
The estimating method according to claim 1, wherein said step S3 further comprises the following steps:

S31. The expected value of the used interest constitutes an autocovariance matrix.

S32, using [R(N)] s and the expected value of the used interest to form an estimate of the autocovariance matrix
The square of the difference between the two norms is the optimization goal.

among them,
The estimating method according to claim 2, wherein when the noise covariance of the line motion is estimated, the Euler angle and the rotational speed of the four blades are used as the control amount.
The estimation method according to claim 2, wherein when the noise covariance of the diagonal motion is estimated, the rotational speeds of the four blades are taken as the control amount.