CN114594675B - Four-rotor aircraft control system and method with improved PID - Google Patents

Abstract

The present invention relates to the field of aircraft control technology, and in particular to an improved PID quadcopter control system and method. The present invention provides an improved PID quadcopter control system and method, comprising a flight attitude detection module, a wireless communication module and a control module; the flight attitude detection module comprises an acceleration sensor, a gyroscope, a magnetometer, a barometric altimeter and a Beidou positioning system; the Beidou positioning system is used to measure the earth coordinate system and the aircraft coordinate system; the wireless communication module transmits information data; the control module performs attitude data calculation and drives the flight motor in a PWM mode. Based on the real-time data acquisition and quaternion actuation estimation of the quadcopter attitude micro-electromechanical detection system, the attitude angle data solution process has higher calculation accuracy and adaptability.

Description

A quadrotor aircraft control system and method based on improved PID

Technical Field

The present invention relates to the technical field of aircraft control, and in particular to an improved PID quadrotor aircraft control system and method.

Background technique

The traditional PID can only make the rate of change of the changing signal a constant, which is a " ramp signal ". When the rate of change is still a function of time, since the differential term in our controller is only a first-order differential, the signal after differentiation is still a changing signal, and the traditional PID cannot handle it. In addition, it is a linear controller , so it can only be used for a certain equilibrium point and its neighborhood, but the real system has nonlinear and strong coupling characteristics. Once the neighborhood is exceeded, the system will collapse.

In order to solve the problems of complex conditions for parameter adjustment and complexity of controlled objects, poor adaptability, and weak robustness of traditional PID control, it is a technical problem that needs to be solved urgently.

Summary of the invention

In order to overcome the shortcomings of poor adaptability and weak robustness of traditional PID control, the technical problem of the present invention is to provide an improved PID quadrotor aircraft control system and method.

The technical implementation scheme of the present invention is: an improved PID quadcopter control system, comprising: a flight attitude detection module, a wireless communication module and a control module; the flight attitude detection module comprises an acceleration sensor, a gyroscope, a magnetometer, a barometric altimeter and a Beidou positioning system; the Beidou positioning system is used to measure the earth coordinate system and the aircraft coordinate system; the wireless communication module transmits information data; the control module performs attitude data calculations and drives the flight motor in a PWM manner.

An improved PID quadrotor control method, comprising:

S1) Establishment of coordinate system and attitude expression; the geographic coordinate system (X _g , Y _g , Z _g ) is used to describe the position and attitude of the aircraft relative to the ground, and the body coordinate system (X _b , Y _b , Z _b ) is the basic coordinate system for UAV inertial navigation;

The geographic coordinate system can be obtained by rotating θ, γ, Coincident with the body coordinate system, its posture is expressed as: rotation matrix Just update /> Then the real-time calculation of θ, γ, /> Rotation Matrix/> It can be obtained by solving the quaternion.

S2) Acceleration sensor data acquisition; when the acceleration sensor is measuring, the three-axis components of the gravitational inertial mass R are recorded as Rx, Ry, and Rz;

The angles between the gravitational acceleration and the three coordinate axes are denoted as α, β, and γ.

S3) Gyroscope data acquisition: measuring the rate of change of the angle within a certain period of time, and adding the initial state angle and the integrated angle to obtain the incremental rotation angle θ;

And establish the gyroscope random drift mathematical model, the first-order Auto-Recessive (AR) model:

_Xt ＝ _{αiXt -1} + _εt .

S4) Magnetometer data acquisition; heading angle It is obtained by measuring the horizontal component of the magnetometer information.

S5) Kalman filter design: Based on the gyroscope noise estimation and the first-order (AR) model, a Kalman filter is designed to compensate the gyroscope output angular rate;

Kalman filter estimation can be divided into two stages: prediction and update;

predict:

renew:

S6) By using quaternions to solve the attitude rotation matrix, the result of the solution has been given according to the inference of Chen Mengyuan et al., and the new quaternion is converted into three Euler angles to obtain the flight attitude angle θ, γ;

S7) Design of a quadrotor controller based on attitude detection and improved PID.

S7) The steps are as follows:

1) Improve the differential term in the control process of the aircraft into a free vibration differential equation in a critical damping state;

The natural frequencies are expressed according to the stiffness and mass of the system;

Substituting the initial condition at time zero into the free vibration differential equation of the system, the expressions of amplitude and initial phase are obtained:

_Xt = Asin( _ω0t +δ).

2) The improved PID controller consists of a proportional unit, an integral unit and an improved vibration differential unit. With T as the sampling period, _Ti as the integral period, _Td as the differential period, and n as the sampling number, the improved PID algorithm can be expressed as:

The beneficial effects of the present invention are:

1. Based on the real-time data collection and quaternion calculation estimation of the attitude micro-electromechanical detection system of the quadrotor aircraft, the attitude angle data calculation process has higher calculation accuracy and adaptability.

2. Improve the differential term in the aircraft's PID control algorithm into a free vibration differential equation in a critical damping state, and then perform decoupling operations to reduce the system overshoot and make it more robust.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a schematic diagram showing the coordinate principle and attitude representation of a quadrotor aircraft of the present invention.

FIG. 2 is an algorithm block diagram of the improved PID differential term of the present invention.

FIG. 3 is a roll angle curve diagram of the present invention in a flight test.

FIG. 4 is a pitch angle curve diagram of the present invention during a flight test.

FIG. 5 is a graph showing the roll angle error of the present invention during a flight test.

FIG. 6 is a pitch angle error curve diagram of the present invention in a flight test.

Detailed ways

The technical solutions in the embodiments of the present invention are described clearly and completely below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

Example

The present invention is based on the existing aircraft hardware construction mode and is described from three aspects: flight attitude micro-electromechanical detection, improved PID control and critical damping free vibration differential equation solution:

1. Establish a micro-electromechanical detection system for the attitude of a quadrotor aircraft;

2. Based on micro-electromechanical real-time data acquisition and quaternion precise estimation, the controller is designed using the improved PID method to make it have higher calculation accuracy and stronger robustness.

3. In the differential term of the improved PID, the critical damping degree of freedom vibration differential equation is approximately established and solved based on the negative damping principle of aircraft proportional control and the aircraft dynamics model.

As shown in Figure 1, an improved quadcopter control system includes a flight attitude detection module, a wireless communication module and a control module; the flight attitude detection module includes an acceleration sensor, a gyroscope, a magnetometer, a barometric altimeter and a Beidou positioning system; the acceleration sensor and the gyroscope respectively measure the attitude data of the aircraft's X, Y, and Z axes, and perform complementary fusion to calculate the attitude angle (pitch angle, roll angle, yaw angle); the magnetic sensor monitoring data is used to eliminate the integral drift error caused by the gyroscope; the barometric altimeter measures the aircraft's altitude data; the Beidou positioning system is used to determine the geodetic coordinate system and the aircraft coordinate system; the wireless communication module transmits information data; the control module performs attitude data calculations and drives the flight motor in a PWM mode.

The present invention also relates to an improved quadrotor control method, which comprises the following steps:

S1) Establishment of coordinate system and attitude expression: As shown in Figure 1, the geographic coordinate system ( _Xg , _Yg , _Zg ) is used to describe the position and attitude of the aircraft relative to the ground. It takes the center of the aircraft as the origin, _Xg points to the geographic east, _Yg points to the geographic north, and _Zg points to the sky, that is, the northeast sky coordinate system;

The body coordinate system (X _b , Y _b , Z _b ) is the basic coordinate system of the UAV inertial navigation, which is fixed to the aircraft. It takes the center of the body as the origin, X _b points to the front of the body, Y _b points to the right of the body, and Z _{b is} perpendicular to the horizontal plane of the body and upward;

Attitude angle (pitch angle θ, roll angle γ, yaw angle ) is an angular parameter used to describe the motion of the aircraft. When the aircraft is working, the four rotating rotors generate a total lift F perpendicular to the horizontal plane of the aircraft and a three-axis rotational torque t ₁ , t ₂ , t ₃ along the aircraft coordinate system. The lift F causes the spatial position of the aircraft to change, and the rotational torque t changes the attitude of the aircraft. The geographic coordinate system can be changed by rotating θ, γ, / > Coincident with the body coordinate system, that is:

_Xg , _Yg , Y ₁ ,/> Y ₂ ,/> _Yb , _Zb

The rotation matrix from the geographic system to the machine system can be obtained

In the formula, only the update Then the real-time calculation of θ, γ, /> Rotation Matrix/> It can be obtained by solving matrix differential equations or quaternions.

S2) Acceleration sensor data acquisition; when the acceleration sensor is measuring, the three-axis components of the gravitational inertial mass R are recorded as Rx, Ry, and Rz; then the angles between the gravitational acceleration and the three coordinate axes can be calculated through trigonometric functions and other related knowledge and recorded as α, β, and γ;

S3) Gyroscope data acquisition: The gyroscope, also known as the angular velocity sensor, measures the rate of change of the angle within a certain period of time. To obtain the angle increment value, the time difference between two measurements can be integrated. A positive increment value indicates that the angle is increasing, and a negative increment value indicates that the angle is decreasing. The rotation angle θ after the increment can be obtained by adding the initial state angle and the integrated angle.

θ＝θ ₀ +∫ ₀ ^t ωdt

Where: θ - rotation angle value after increment; θ ₀ - angle value of the last rotation; ω - value measured by the angular velocity sensor; t - measurement interval;

Since the MEMS gyroscope has large output noise and the attitude solution has an integration process, the gyroscope output noise will accumulate over time, causing the final rotation angle of the solution to diverge, which is called random drift. Therefore, a mathematical model of gyroscope random drift is established to compensate its output angular rate to improve the accuracy of aircraft attitude solution. The first-order Auto-Recessive (AR) model demonstrated by "Huamingqian" et al. is used to describe the random drift of the MEMS gyroscope:

_Xt ＝ _{αiXt -1} + _εt

Where _Xt is the gyro drift at time t, _αi is a constant coefficient, _εt obeys a zero-mean Gaussian distribution, and the parameters _αi and _εt can be obtained by the Matlab function “aryule”.

S4) Magnetometer data collection; The magnetometer is used to obtain an estimate of the heading angle in the carrier attitude, and the heading angle is obtained by the component of the magnetometer measurement information in the horizontal direction; the measurement value of the three-axis magnetometer in the earth's magnetic field coordinate system is:

Among them, the superscript b indicates that the vector h is a vector in the Earth's magnetic field coordinate system, and the subscripts x, y, and z represent the components of each axis of the three-axis magnetometer. When the magnetometer is placed horizontally on a horizontal plane, the heading _hx and _hy of the horizontal components of the vector h can be used to calculate the angle between the magnetometer coordinate system x and the North Pole of the Earth's magnetic field, that is, the heading angle derivation formula is:

S5) Kalman filter design: Based on the gyroscope noise estimation and the first-order (AR) model, a Kalman filter is designed to compensate the gyroscope output angular rate;

The Kalman dynamic equation is divided into the state equation and the observation equation. The state equation is:

_Xt ＝At _{-1Xt- 1} +Bt _{-1Ut- 1} +Wt _-1

Observation equation: Y _t = H _t X _t + V _t

Where _Xt is the state vector at time t; _At-1 is the state transfer matrix; _Ut-1 is the control variable; Bt _-1 is the control input matrix; Wt _-1 is the process noise, which obeys the Gaussian distribution with zero mean and variance _Qt ; _Ht is the observation matrix; _Vt is the measurement noise, which obeys the Gaussian distribution with zero mean and variance _Rt ;

Kalman filter estimation can be divided into two stages: prediction and update. In the prediction stage, the state value estimated at the previous moment is used to predict the state value at the next moment. In the update stage, the state value at the current moment is updated by combining the current predicted value and the observed value.

predict:

renew:

In the formula is the state value and variance matrix updated by Kalman; _Yt is the measured value; _Kg is the Kalman gain, which ensures that the state value estimated by the Kalman algorithm is the optimal value with the minimum variance; the angular rate and random drift can be estimated in real time according to the prediction and update formula;

Before applying the Kalman algorithm, the noise is processed according to the first-order (AR) model in step 3) so that the gyroscope random drift is a part of the state vector, thereby satisfying the noise requirement of the Kalman estimation; the random drift of the gyroscope is then estimated by the Kalman algorithm to compensate for the three-axis angular rate output by the gyroscope; the dynamic equation of the Kalman algorithm is determined according to the first-order (AR) model for the gyroscope random drift;

Equation of state:

Observation equation: Y _t = [1 0] X _t + V _t , where

Where X _α and X _r represent the gyroscope random drift and output angular rate estimated by Kalman filter respectively.

S6) The attitude is solved by using quaternion rotation matrix to obtain the flight attitude angle; assuming that the current coordinate system is the body coordinate system, the quaternion vector is:

q＝[q ₀ q ₁ q ₂ q ₃ ] ^T ＝[0123] ^T

According to the solution results given in the inference of Chen Mengyuan et al., the process of converting the new quaternion into three Euler angles can be intuitively expressed as:

θ＝arcsin(-2(q ₁ q ₃ -q ₀ q ₂ ))

Real-time flight attitude data is then obtained and used to regulate the operating power of each motor of the quadrotor.

S7) Design of a quadcopter controller based on attitude detection and improved PID; Since the parameters of PID are to convert different dimensional data, its parameter acquisition depends on the data information of the flight attitude detection module; The differential term D controls whether a certain quantity reaches the target value quickly and whether it will overshoot. It is equivalent to a negative damping effect on the P parameter, which is mainly used to suppress the body oscillation overshoot. Increasing the differential D value can correct this jitter, so the differential needs to be adjusted together with the proportional term; As shown in Figure 2, the steps are as follows:

1) Improve the differential term in the aircraft's PID control algorithm to a free vibration differential equation in a critical damping state;

Knowing the stiffness and mass of the system, the natural frequency can be expressed;

Substituting the initial condition at time zero into the free vibration differential equation of the system, we can derive the expressions of amplitude and initial phase with respect to displacement:

_Xt ＝Asin( _ω0t +δ)

Where A is the amplitude (constant), δ is the initial phase;

2) The improved PID controller consists of a proportional unit, an integral unit and an improved vibration differential unit. According to the processing method of the digital PID controller, T is the sampling period, _Ti is the integral period, _Td is the differential period, and n is the sampling sequence number. The improved PID algorithm can be expressed as:

Where _Kp is the proportionality coefficient, is the integration coefficient, /> is the vibration differential coefficient combined with the natural frequency;

The deviation between the system output value and the expected value is obtained by negative feedback of the collected signal, and the system output control quantity is obtained through proportional conversion to eliminate the system output deviation; the integral control adjusts the accumulated deviation; the differential control controls the change trend of the error under the boundary conditions of critical damping free vibration, improves the rapidity of the output response, and reduces the system overshoot.

Experimental test

The experimental conditions are specifically given below to further illustrate the effectiveness of the adaptive aircraft control system fault compensation and disturbance suppression method according to the embodiment of the present invention, wherein the experimental conditions include:

Aircraft parameters:

The total mass of the machine is m = 1.5 kg, the opposite wheelbase is L = 50.0 × 10 ^-2 m, the lift coefficient is k _t = 6.15 × 10 ^-5 N·ms ² , the drag coefficient is k _d = 1.18 × 10 ^-6 N·ms ² , the X-axis moment of inertia is 6.25 × 10 ^-3 , the Y-axis moment of inertia is 5.15 × 10 ^-3 , the Z-axis moment of inertia is 10.45 × 10 ^-5 , and the motor moment of inertia is 1.19 × 10 ^-4 kg·m ² . The quadcopter micromotor system is built using the quaternion solver module, and an outdoor flight experiment is conducted at the college stadium. The flight attitude signal is given through the improved PID remote controller to control the flight attitude. The data is transmitted back to the host computer for analysis using wireless communication technology. Figure 3-4 shows the roll angle curve and pitch angle curve of the flight test. In Figure 3-4, the maximum roll angle does not exceed 10°, and the maximum pitch angle does not exceed 45°.

As shown in Figure 5-6, in the flight test, the roll angle has relatively large errors at several peaks, which is caused by its high-frequency large-amplitude changes. The amplitude of the pitch angle changes more slowly, and the corresponding error is relatively small. Although there is a certain relative error, when performing high-rate angular motion, the aircraft can well follow the reference value given by the controller, and the error can be controlled within a certain range. This proves the effectiveness of the attitude solution method in actual flight.

The above-mentioned embodiments only express the preferred implementation modes of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present invention. It should be pointed out that, for a person of ordinary skill in the art, several modifications, improvements and substitutions can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention patent shall be subject to the attached claims.

Claims (1)

1. The four-rotor aircraft control method of the improved PID is applied to a four-rotor aircraft control system of the improved PID, and is characterized in that the four-rotor aircraft control system of the improved PID comprises a flight attitude detection module, a wireless communication module and a control module; the flight attitude detection module comprises an acceleration sensor, a gyroscope, a magnetometer, an air pressure altimeter and a Beidou positioning system; the Beidou positioning system is used for measuring a geodetic coordinate system and an aircraft coordinate system; the wireless communication module transmits information data; the control module performs attitude data operation and drives the flying motor in a PWM mode;

The four-rotor aircraft control method of the improved PID comprises the following steps:

s1) establishing a coordinate system and representing the gesture; the geographic coordinate system (X _g,Y_g,Z_g) is used for describing the position and the gesture of the aircraft relative to the ground, and the machine body coordinate system (X _b,Y_b,Z_b) is a basic coordinate system of inertial navigation of the unmanned aerial vehicle;

The geographic coordinate system may be determined by rotating θ, γ, Coinciding with the machine body coordinate system, the posture of the machine body coordinate system is expressed as follows: rotation matrix/>Only update/>Can calculate theta, gamma,/>, in real timeRotation matrix/>The method can be obtained by solving quaternion;

S2) data acquisition of an acceleration sensor; the triaxial components of the gravity inertial mass R are recorded as Rx, ry and Rz when the acceleration sensor is used for measuring;

The included angles between the gravity acceleration and the three coordinate axes are marked as alpha, beta and gamma;

S3) gyroscope data acquisition; measuring the change rate of the angle in a certain period of time, and adding the initial state angle and the integrated angle to obtain an incremental rear rotation angle theta;

and establishing a random drift mathematical model of the gyroscope, wherein the first-order Auto-RECESSIVE model is as follows:

X_t＝α_iX_t-1+ε_t；

wherein X _t is gyro drift at t moment, alpha _i is a constant coefficient, epsilon _t obeys zero-mean Gaussian distribution, and the parameter alpha _i、ε_t can be obtained by Matlab function 'aryule';

s4) magnetic force count acquisition; course angle Obtaining the component of the measurement information in the horizontal direction for the magnetometer;

S5) Kalman filter design; on the basis of a first-order Auto-RECESSIVE model, a Kalman filter is designed to compensate the output angular rate of the gyroscope;

the Kalman filter estimation can be divided into two phases: predicting and updating;

and (3) predicting:

updating:

In the prediction phase,: a _t-1 is a state transition matrix; u _t-1 is a control quantity; b _t-1 is a control input matrix; q _t is variance;

In the update phase: k _g is Kalman gain, which ensures that the state value estimated by the Kalman algorithm is the optimal value with minimum variance; h _t is the observation matrix; r _t is variance;

State values and variance matrices updated for kalman; y _t is a measurement value; according to the prediction and updating formula, the angular rate and random drift can be estimated in real time;

S6) carrying out attitude calculation on a rotation matrix by adopting quaternions, and converting the new quaternions into three Euler angles so as to obtain the flying attitude angles θ，γ；

S7) four rotor aircraft controller design based on attitude detection and improved PID:

1) Improving a differential term in the control process of the aircraft into a free vibration differential equation in a critical damping state;

wherein k is the stiffness of the system; m is the mass of the system;

The natural frequency is represented according to the rigidity and the quality of the system; Is a natural frequency;

the initial condition of zero moment is brought into a free vibration differential equation of the system, and an expression of amplitude and initial phase relative to displacement is obtained:

X_t＝A sin(ω₀t+δ)；

wherein X _t is a displacement; a is an amplitude constant, and delta is an initial phase;

2) The improved PID controller consists of a proportional unit, an integral unit and an improved vibration differential unit, wherein the improved PID controller takes T as a sampling period, T _i as an integral period, T _d as a differential period and n as a sampling sequence number, and the improved PID algorithm can be expressed as follows:

wherein U (n) is the output value of the controller at the nth sampling point; k _p is the coefficient of proportionality, Is an integral coefficient,/>A vibration differential coefficient for combining the natural frequency; e _n is the error value at the nth sample point; the cumulative error from zero to the nth sampling point is the cumulative error of the integrating part in the PID controller.