Accurate attitude control method for quad-rotor unmanned aerial vehicle under large maneuver

Abstract

The invention discloses an accurate attitude control method for a quad-rotor unmanned aerial vehicle under large maneuver, and the method comprises the steps: taking geometric control for large maneuver flight control as a basis, re-building an accurate attitude kinematics model described by a rotation matrix, designing an extended state observer on the basis of the model, and carrying out the precise attitude control of the quad-rotor unmanned aerial vehicle. Errors caused by interior and exterior of the unmanned aerial vehicle are observed; the controller adopts geometric cascade PID control, and the control algorithm not only keeps the simple structure, but also has good enough robustness, and is also easy to deploy on the actual unmanned aerial vehicle at the same time.

Description

Technical Field

[0001] The invention relates to a control method for a quad-rotor unmanned aerial vehicle, and in particular to a precise attitude control method for a quad-rotor unmanned aerial vehicle under large maneuvers. Background Art

[0002] The diversification of quadcopter application scenarios has put forward higher requirements for its flight performance. In recent years, the application scenarios of quadcopters have gradually transitioned from static or low-speed operations to high-speed and high-maneuverability scenarios.

[0003] High-dynamic scenes have two notable characteristics: First, the attitude changes greatly. The drone must maintain a large attitude to achieve high-speed flight. In addition, it is sometimes necessary to avoid obstacles by maintaining a large attitude. For example, in the above-mentioned drone passing through a narrow window to enter the room, the drone's roll angle can instantly reach 90°; second, the attitude changes quickly. High dynamics also means high acceleration. When the drone is traveling through complex scenes, it needs to be "fast when it should be fast, slow when it should be slow." In this case, higher requirements are also placed on the attitude change rate.

[0004] In the past decade, the development of traditional attitude control algorithms has been relatively mature, but there are significant defects in the control of large attitude angles in high-dynamic scenes, which cannot meet the control requirements. The attitude change of a quadrotor can be regarded as a motion on a special orthogonal group SO(3). The traditional algorithm decomposes the attitude of the drone along the coordinate axis of the aircraft system into three independent Euler angles for separate control. This method of expressing rotation in Euclidean space essentially ignores the manifold structure of the rotation and is only a local approximation on SO(3). This approximation is still widely used in attitude control. First, it is accurate enough under the condition of small angle deviation near the hovering point; second, the current mainstream research on quadrotor drone control algorithms focuses on drones weighing less than 1kg, and their flexibility masks the defects of traditional methods. In addition, when the attitude of the drone is too large, there is a singular point problem when using Euler angles to represent the attitude. When the pitch or roll of the drone reaches 90°, due to the gimbal lock problem of the Euler angle, the attitude cannot be solved normally and a sudden change occurs, which greatly affects the control quality.

[0005] In recent years, methods using unit quaternions to represent attitude and control have been vigorously developed. Unit quaternions are defined on the three-dimensional sphere S3 and can represent every attitude, but since the mapping between S3 and SO(3) is doubly covered, this means that each physical attitude of the quadrotor corresponds to two different quaternions. In this case, although the quaternion-based continuous controller can be designed to make the drone attitude converge to the equilibrium point from almost any initial position, the unwinding phenomenon will still occur, that is, the drone will rotate a large angle in the opposite direction to return to the equilibrium point, which will lead to unnecessary energy consumption and even cause the drone to become unstable.

[0006] Based on the above problems, the inventors have conducted in-depth research on the attitude control method of the UAV, hoping to design a precise attitude control method for the quad-rotor UAV under large maneuvers that can solve the above problems. Summary of the invention

[0007] In order to overcome the above problems, the inventors have conducted intensive research and designed a method for precise attitude control of a quadrotor UAV under large maneuvers. In this method, based on geometric control for large maneuver flight control, an accurate attitude kinematic model described by a rotation matrix is ​​re-established, and an extended state observer is designed on the basis of this model to observe errors caused by the inside and outside of the UAV. The controller uses geometric cascade PID control. This control algorithm not only maintains structural simplicity, but also has sufficiently good robustness, and is also easy to deploy on actual UAVs, thereby completing the present invention.

[0008] Specifically, the purpose of the present invention is to provide a method for precise attitude control of a quad-rotor drone under large maneuvers.

[0009] The method comprises the following steps:

[0010] Step 1: Get the expected posture in real time based on the task content, and transform the expected posture to get the expected rotation matrix.

[0011] Step 2, based on the desired rotation matrix, obtain the desired torque through the geometric cascade PID control law,

[0012] Step 3: Subtract the expected torque from the state disturbance to obtain a corrected expected torque, obtain the motor PWM value of the drone based on the corrected expected torque, control the drone motor according to the motor PWM value, and thus make the drone track the expected posture.

[0013] The beneficial effects of the present invention include:

[0014] (1) According to the precise attitude control method of the quad-rotor UAV under large maneuvers provided by the present invention, by setting an appropriate geometric cascade PID control law, the control torque on the UAV is small, the control energy consumed is small, and the control effect on the angle is more precise;

[0015] (2) The precise attitude control method for a quad-rotor UAV under large maneuvers provided by the present invention can ensure that the UAV will not lose control when the desired attitude angle suddenly changes, and can timely and effectively adjust and control the attitude of the UAV, so that the UAV can adjust its attitude to the desired attitude in the shortest time. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 The overall logic diagram of the method for precise attitude control of a quad-rotor drone under large maneuvers of the present invention is shown;

[0017] Figure 2 The actual posture diagram of the aircraft during the simulation is shown in Example 1 and Comparative Example 1;

[0018] Figure 3 A schematic diagram of the control torque on the drone during the simulation process is shown in Example 1 and Comparative Example 1;

[0019] Figure 4 A schematic diagram showing the observation effect of the finite-time extended state observer on the three-channel disturbance in Example 2;

[0020] Figure 5 A schematic diagram showing the observation effect of the finite time extended state observer on the three-channel posture in Example 2;

[0021] Figure 6 A schematic diagram showing the observation effect of the finite-time extended state observer on the three-channel angular velocity in Example 2 is shown. DETAILED DESCRIPTION

[0022] The present invention will be further described in detail below through the accompanying drawings and embodiments. Through these descriptions, the characteristics and advantages of the present invention will become more clear and distinct.

[0023] The word "exemplary" is used exclusively herein to mean "serving as an example, embodiment, or illustration." Any embodiment described herein as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments. Although various aspects of the embodiments are shown in the drawings, the drawings are not necessarily drawn to scale unless otherwise noted.

[0024] According to the present invention, a method for precise attitude control of a quad-rotor drone under large maneuvers is provided. Figure 1 As shown in , the method includes the following steps:

[0025] Step 1: Obtain the desired posture in real time based on the task content, and perform angle transformation on the desired posture to obtain the desired rotation matrix;

[0026] In this application, the expected attitude can be automatically generated based on the planned path, or can be manually input by the operator based on the flight control program. The expected attitude includes the expected Euler angles in the three directions that the drone can reach at the next moment, that is, the expected Euler angle φ in the rolling direction d , the expected Euler angle θ in the pitch direction d and the expected Euler angle ψ in the yaw direction d .

[0027] The desired rotation matrix R = [b1 b2 b3] T , b i Represents the projection of the three axes of the body coordinate system in the inertial system, i=1,2,3.

[0028] In step 1, the angle transformation process uses the relevant process in the full-authority "Multi-rotor Aircraft Design and Control".

[0029] Step 2, based on the desired rotation matrix, obtaining the desired torque through the geometric cascade PID control law;

[0030] In step 2, the geometric cascade PID control law obtains the desired torque in the Lagrangian domain through the following formula (1):

[0031] M C =PI(s)(-K R e R -ω ob )-D(s)ω ob (one)

[0032] Among them, M C represents the expected torque,

[0033] PI(s) represents the inner loop proportional and integral gain,

[0034] K R represents the observer gain,

[0035] e R represents the attitude tracking error,

[0036] ω ob represents the angular velocity error,

[0037] D(s) represents the inner loop differential gain.

[0038] Preferably, the PI(s) is obtained by the following formula (II), and D(s) is obtained by the following formula (III):

[0039]

[0040]

[0041] Among them, K p , K i , K d They independently represent the parameters of the extended state observer, and the preferred values ​​are:

[0042] s represents the Laplace operator,

[0043] N represents the differential filter constant.

[0044] Preferably, the posture tracking error e R Obtained by the following formula (IV):

[0045]

[0046] Among them, R d represents the desired drone attitude expressed as a rotation matrix,

[0047] R ob Represents the observed drone attitude expressed as a rotation matrix.

[0048] Mathematical transport symbol (x) in this application ∨ For details of the specific calculation process, please refer to D.Invernizzi, S.Panza, M.Lovera Robust Tuning of Geometric Attitude Controllers for MultirotorUnmanned Aerial Vehicles Journal of Guidance Control and Dynamics, Vol.43, N.7, 2020, p.1332-1343doi:10.2514 / 1.G004457.

[0049] Step 3: Subtract the expected torque from the state disturbance to obtain a corrected expected torque, obtain the motor PWM value of the drone based on the corrected expected torque, control the drone motor according to the motor PWM value, and thus make the drone track the expected posture.

[0050] In a preferred embodiment, the angular velocity error ω ob , R ob and the state disturbance d are obtained through the finite-time extended state observer.

[0051] The finite time extended state observer described in this application is obtained by iterating the following formula (V): and Again Integrate to get the current ω ob ,right Integrate to get the current R ob ,right Integrate to get d at the current moment;

[0052]

[0053] Among them, e ω represents the angular velocity error of the inner loop, preferably, obtained by the following formula (vi):

[0054] e ω =ω′ ob -R T R′ ob ω′ ob (six)

[0055] R′ ob Represents the R at the previous moment ob ,

[0056] ω′ ob represents the ω at the previous moment ob ,

[0057] M′ c Indicates the M at the previous moment C ,

[0058] d′ represents the d at the previous moment,

[0059] e′ R represents the e at the previous moment R ,

[0060] J represents the moment of inertia of the object, which is measured by the double cycloid method.

[0061] k R , k ω , k′ R , Υ each independently represent the observer gain, and the preferred value is:

[0062] β1, β2, and β3 each independently represent the parameters to be designed and need to satisfy the following constraints:

[0063] β1=β, 0<β<1; More preferably, the β value is 0.8.

[0064] Mathematical transport symbol (x) in this application ∧For details of the specific calculation process, please refer to D.Invernizzi, S.Panza, M.Lovera Robust Tuning of Geometric Attitude Controllers for MultirotorUnmanned Aerial Vehicles Journal of Guidance Control and Dynamics, Vol.43, N.7, 2020, p.1332-1343doi:10.2514 / 1.G004457.

[0065] The difference described in this application is to subtract the state disturbance from the expected torque. The state disturbance in this application is obtained through a limited time expanded state observer, which can include the external airflow disturbance to the drone during flight and the disturbance caused by the accuracy of the drone's own devices.

[0066] In a preferred embodiment, the motor PWM value of the drone is obtained by the following formula (VII):

[0067]

[0068] Among them, PWM represents the motor PWM value of the drone,

[0069] k represents the motor characteristic constant, preferably k=750;

[0070] A represents a matrix,

[0071] T represents the desired thrust of the drone, given by the position controller.

[0072] M x It represents the expected rolling moment of the UAV, which is the first item in the corrected expected moment;

[0073] M y It represents the expected pitch moment of the UAV, which is the second item in the corrected expected moment;

[0074] M z It represents the expected yaw moment of the UAV, which is the third item in the corrected expected moment.

[0075] Example 1

[0076] The MATLAB / SIMULINK simulation software is used to carry out simulation experiments on the rotary-wing UAV, in which the rotary-wing UAV is the DJI-F450 UAV, and its relevant parameters are shown in Table 1:

[0077] Table 1 DJI-F450 drone parameters

[0078]

[0079] The geometric cascade PID control law is installed on the drone, that is, the drone performs the following attitude processing process:

[0080] Step 1: Get the expected posture in real time based on the task content, and transform the expected posture to get the expected rotation matrix.

[0081] Step 2: Based on the desired rotation matrix, the desired torque is obtained by the geometric cascade PID control law. The geometric cascade PID control law obtains the desired torque by the following formula (1):

[0082] M C =PI(s)(-K R e R -ω ob )-D(s)ω ob (one)

[0083] The PI(s) is obtained by the following formula (II), and D(s) is obtained by the following formula (III):

[0084]

[0085]

[0086] Attitude tracking error e R Obtained by the following formula (IV):

[0087]

[0088] The angular velocity error ω ob , R ob and the state disturbance d are obtained through the finite-time extended state observer;

[0089] Step 3: Subtract the expected torque from the state disturbance to obtain a corrected expected torque, obtain the motor PWM value of the drone based on the corrected expected torque, control the drone motor according to the motor PWM value, and thus make the drone track the expected posture.

[0090] The motor PWM value of the drone is obtained by the following formula (VII):

[0091]

[0092] k represents a value of k=750;

[0093]

[0094] M x is the first term in the corrected desired torque,

[0095] M y is the second term in the corrected desired torque,

[0096] M z is the third term in the corrected desired torque.

[0097] A finite-time extended state observer is also added to the drone, that is, the derivative of the state disturbance d is obtained by monitoring through the following formula (V): Again Integrate to get d at the current moment;

[0098]

[0099] The values ​​of the parameters β1, β2, and β3 to be designed are as follows:

[0100] β1=β, The β value is 0.8.

[0101] The observer gain is

[0102] R ob ,ω ob 、M C , d, e R The initial value of is 0;

[0103] At the beginning of the simulation, the three channels are given [φ d θ d ψ d ]=[50° 70° 10°] of the expected Euler angle. During the simulation, the step size was selected as 0.01, the total simulation time was 4s, and a three-channel disturbance torque of Δ=[0.10.20.3]N·m was continuously applied to the rotorcraft from the second second. The actual attitude of the aircraft during the simulation was monitored, including the actual roll angle, actual pitch angle, and actual yaw angle, such as Figure 2 The control torque on the drone is shown as Figure 3 Shown by the dashed line.

[0104] Comparative Example 1

[0105] A basically identical drone to that in Example 1 is used to perform a basically identical drone control simulation experiment, the only difference being that the geometric cascade PID control law in the example is replaced by a traditional classical PID control rate, and the time-limited extended state observer in the example is replaced by a traditional classical extended state observer. The traditional classical PID control rate is selected from the traditional classical PID control rate recorded in S.Bouabdallah, A.Noth and R.Siegwart, "PID vs LQ control techniques applied to an indoor micro quadrotor," 2004IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat.No.04CH37566), Sendai, Japan, 2004, pp.2451-2456vol.3, doi:10.1109 / IROS.2004.1389776.; the traditional classical extended state observer is selected from the traditional classical extended state observer recorded in Herbst G.Transfer function analysis andimplementation of active disturbance rejection control[J].Control Theory and Technology, 2021, 19:19-34.

[0106] Monitor the actual attitude of the aircraft during the simulation, including the actual roll angle, actual pitch angle, and actual yaw angle, such as Figure 2 The control torque on the drone is shown as Figure 3 Shown by the solid line.

[0107] analyze Figure 2 It can be seen that the three-channel response results of the two methods are similar after 2s, which shows that the two methods have similar suppression effects on disturbances; but at the beginning of the simulation, the dynamic response of the control method of the present invention is significantly better than the method in the comparative example. This is due to the approximate processing made by the classical extended state observer using Euler angles to represent the attitude. This approximation is reasonable at small angles, but it is no longer valid when the drone performs large maneuvers as in this example. The geometric control represented by global coordinates can accurately represent the target attitude movement. Therefore, the dynamic response of the present invention is better than the classical control method, and the three-channel response can reach a steady-state value in about 0.5s.

[0108] analyze Figure 3It can be seen that the conventional control algorithm used in the comparison is not accurate enough in describing the posture due to the small angle assumption, thus generating a large unnecessary control torque within the time of 0-2s. The method of the present invention not only has a smaller control torque and consumes less control energy, but also has a more accurate control effect on the angle.

[0109] Example 2

[0110] The MATLAB / SIMULINK simulation software is used to carry out simulation experiments on the rotary-wing UAV, in which the rotary-wing UAV is the DJI-F450 UAV, and its relevant parameters are shown in Table 1:

[0111] Table 1 DJI-F450 drone parameters

[0112]

[0113]

[0114] Add a finite-time extended state observer to the drone, that is, monitor the derivative of the state disturbance d through the following formula (V): Again Integrate to get d at the current moment;

[0115]

[0116] The values ​​of the parameters β1, β2, and β3 to be designed are as follows:

[0117] β1=β, The β value is 0.8.

[0118] The observer gain is

[0119] R ob ,ω ob 、M C , d, e R The initial value of is 0;

[0120] In the simulation process, the step size is selected as 0.01, the total simulation time is 4s, and a three-channel disturbance torque of Δ=[0.1 0.2 0.3]N·m is continuously applied to the rotorcraft UAV from the second second. The state disturbance of the UAV during the simulation process is observed by the finite time extended state observer added to the UAV. The state disturbance includes the disturbance in the rolling direction, the disturbance in the pitch direction, and the disturbance in the yaw direction.

[0121] The observation effect of the finite-time extended state observer on the three-channel disturbance is as follows: Figure 4As shown by the three solid lines in the figure, the input Δ=[0.1 0.2 0.3]N·m disturbance is Figure 4 As shown by the three dotted lines in Figure 4 It can be seen that all three channels can reach the steady-state value in about 0.2s, and there is no steady-state error, which can achieve rapid tracking and convergence of the error.

[0122] The observation effect of the finite time extended state observer on the three-channel attitude is as follows: Figure 5 As shown by the solid line in , the observation effect of the three-channel angular velocity is as follows Figure 6 As shown by the solid line in , the actual value of the three-channel attitude is Figure 5 As shown by the dashed line in the middle, the actual values ​​of the angular velocities of the three channels are as follows Figure 6 As shown by the dotted line, the rotation matrix is ​​converted into three Euler angles for the convenience of representation. As can be seen from the figure, the observed value and the actual value are almost completely consistent, which fully demonstrates that the convergence speed of the observer provided by the present application is much faster than that of the controller. When the disturbance is applied at only 2s, the observed value lags slightly behind the true value, but it can also fully converge within 0.2s. The convergence speed of the state quantity observation value is almost the same as the convergence speed of the disturbance observation value.

[0123] The present invention has been described above in conjunction with preferred embodiments, but these embodiments are only exemplary and serve only as an illustration. On this basis, the present invention may be subjected to a variety of substitutions and improvements, all of which fall within the scope of protection of the present invention.

Claims

1. A precise attitude control method for a quadrotor UAV under large maneuvers, characterized in that the method comprises the following steps: Step 1, obtain the desired attitude in real time based on the task content, and perform an angle transformation on the desired attitude to obtain the desired rotation matrix. Step 2, based on the desired rotation matrix, obtain the desired torque through a geometric cascade PID control law. Step 3, subtract the state disturbance from the desired torque to obtain the corrected desired torque, obtain the motor PWM value of the UAV based on the corrected desired torque, and control the UAV motor according to the motor PWM value, so as to enable the UAV to track the desired attitude.

2. The precise attitude control method for a quadrotor UAV under large maneuvers according to claim 1, characterized in that in Step 2, the geometric cascade PID control law obtains the desired torque through the following formula (I) in the Laplace domain: MC = PI(s)(-KReR - ωob) - D(s)ωob (I) Among them, M C represents the expected torque, PI(s) represents the inner-loop proportional and integral gain. K R represents the outer loop proportional gain, e R represents the attitude tracking error, ω ob represents the angular velocity error, D(s) represents the inner-loop differential gain.

3. The precise attitude control method for a quadrotor UAV under large maneuvers according to claim 2, characterized in that the PI(s) is obtained through the following formula (II), and D(s) is obtained through the following formula (III): Among them, K p , K i , K d Each independently represents the extended state observer parameters, s represents the Laplace operator. N represents the differential term filtering constant.

4. The precise attitude control method for a quadrotor UAV under large maneuvers according to claim 2, characterized in that The posture tracking error e R Obtained by the following formula (IV): Among them, R d represents the desired drone attitude expressed as a rotation matrix, R ob Represents the observed drone attitude expressed as a rotation matrix.

5. The precise attitude control method for a quadrotor UAV under large maneuvers according to claim 4, characterized in that The angular velocity error ω ob , R ob and the state disturbance d are obtained through the finite-time extended state observer.

6. The precise attitude control method for a quadrotor UAV under large maneuvers according to claim 5, characterized in that The finite time extended state observer is obtained by iterating the following formula (5): and Again Integrate to get the current ω ob ,right Integrate to get the current R ob ,right Integrate to get d at the current moment; Among them, e ω represents the angular velocity error of the inner loop, preferably, obtained by the following formula (vi): e ω = ω o ′ b - R T R o ′ b ω o ′ b (Six) R o ' b Represents the R at the previous moment ob , ω o ' b represents the ω at the previous moment ob , M c ′ represents the M at the previous moment C , d′ represents d at the previous moment. e′ R represents the e at the previous moment R , J represents the moment of inertia of the UAV. k R , k ω , k′ R , Υ each independently represent the observer gain, β1, β2, and β3 each independently represent parameters to be designed.

7. The precise attitude control method for a quadrotor UAV under large maneuvers according to claim 1, characterized in that the motor PWM value of the UAV is obtained through the following formula (VII): where PWM represents the motor PWM value of the UAV. k represents the motor characteristic constant. A represents a matrix, T represents the expected thrust of the drone, M x Indicates the expected rolling moment of the drone, M y Denotes the expected pitch moment of the UAV, M z Indicates the desired yaw moment of the drone.

Images