CN107247459B - Anti-interference flight control method and device - Google Patents
Anti-interference flight control method and device Download PDFInfo
- Publication number
- CN107247459B CN107247459B CN201710605267.7A CN201710605267A CN107247459B CN 107247459 B CN107247459 B CN 107247459B CN 201710605267 A CN201710605267 A CN 201710605267A CN 107247459 B CN107247459 B CN 107247459B
- Authority
- CN
- China
- Prior art keywords
- angle
- error
- control
- attitude
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
Abstract
The invention provides an anti-interference flight control method and device, and relates to the technical field of unmanned aerial vehicles. According to the anti-interference flight control device provided by the embodiment of the invention, the position information and the attitude data of the unmanned aerial vehicle are acquired, the attitude angle is calculated according to the attitude data, the position error is calculated according to the position information and the set reference data, the position control quantity is calculated according to the position error, the expected attitude angle is obtained according to the position control quantity and the attitude angle, the angle error is calculated according to the calculated attitude angle and the expected attitude angle, the control quantity of the attitude angle is calculated according to the angle error, and the position and the attitude of the unmanned aerial vehicle are controlled according to the position control quantity and the control quantity of the attitude angle. The anti-interference flight control method and device provided by the invention can carry out algorithm inhibition on external deterministic interference and uncertain interference, so that the control precision is improved, the anti-interference capability is enhanced, and the flight of the unmanned aerial vehicle is enabled to reach stability rapidly.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicles, in particular to an anti-interference flight control method and device.
Background
The four-rotor unmanned aerial vehicle is an aircraft capable of realizing autonomous flight through remote control or based on own sensors, can adapt to various environments, and has wide application prospects in the military and civil fields.
Because the four-rotor unmanned aerial vehicle is a complex strong-coupling nonlinear underactuated control system, the gyroscope and the accelerometer are easily affected by vibration of the engine body, and the system has parameter uncertainty and is easily interfered by external environmental factors, so that the attitude control and the position control of the flight control system on the unmanned aerial vehicle are affected. The most commonly used control algorithm at present is a PID algorithm, but the PID algorithm has long adjustment time and large overshoot, and the control effect is usually not ideal enough for a strong coupling and nonlinear control system which is easy to be interfered by the outside, and has larger steady-state error.
In the actual flight process, the four-rotor unmanned aerial vehicle is often subjected to external interference, such as airflow disturbance, noise and the like, and the problems of poor anti-interference capability and unstable flight easily occur due to unsatisfactory control effects in the aspects of overshoot, adjustment time and robustness. Therefore, how to solve the above problems has been an important point of attention of those skilled in the art.
Disclosure of Invention
The invention aims to provide an anti-interference flight control method, which aims to solve the problems of non-ideal control effect, poor anti-interference capability and unstable flight of the existing unmanned aerial vehicle in the actual flight process.
The invention also aims to provide an anti-interference flight control device to solve the problems of unsatisfactory control effect, poor anti-interference capability and unstable flight of the existing unmanned aerial vehicle in the actual flight process.
In order to achieve the above object, the technical scheme adopted by the embodiment of the invention is as follows:
in a first aspect, an embodiment of the present invention provides an anti-interference flight control device, which is applied to an unmanned aerial vehicle. The anti-interference flight control method comprises the following steps: acquiring the position information of the unmanned aerial vehicle; acquiring attitude data of the unmanned aerial vehicle, and calculating an attitude angle according to the attitude data; calculating a position error according to the position information and the set reference data; calculating a position control amount according to the position error; obtaining a desired attitude angle according to the position control amount and the attitude angle; calculating an angle error according to the calculated attitude angle and the expected attitude angle; calculating the control quantity of the attitude angle according to the angle error; and controlling the position and the posture of the unmanned aerial vehicle according to the position control quantity and the control quantity of the posture angle.
In a second aspect, an embodiment of the present invention further provides an anti-interference flight control device, which is applied to an unmanned aerial vehicle, where the anti-interference flight control device includes a position information acquisition module, an attitude angle calculation module, a position error calculation module, a position control amount calculation module, an expected attitude angle calculation module, an angle error calculation module, an attitude angle control amount calculation module, and a control module. The position information acquisition module is used for acquiring the position information of the unmanned aerial vehicle; the attitude angle calculation module is used for acquiring attitude data of the unmanned aerial vehicle and calculating an attitude angle according to the attitude data; the position error calculation module is used for calculating a position error according to the position information and the set reference data; the position control quantity calculation module is used for calculating a position control quantity according to the position error; the expected attitude angle calculation module is used for obtaining an expected attitude angle according to the position control quantity and the attitude angle; the angle error calculation module is used for calculating an angle error according to the calculated attitude angle and the expected attitude angle; the attitude angle control quantity calculation module is used for calculating the control quantity of the attitude angle according to the angle error; the control module is used for controlling the position and the gesture of the unmanned aerial vehicle according to the position control quantity and the gesture angle control quantity.
Compared with the prior art, the invention has the following beneficial effects:
according to the anti-interference flight control device provided by the invention, the position information of the unmanned aerial vehicle is obtained, the attitude data of the unmanned aerial vehicle is obtained, the attitude angle is calculated according to the attitude data, the position error is calculated according to the position information and the set reference data, and the position control quantity is calculated according to the position error; and obtaining a desired attitude angle according to the position control amount and the attitude angle, calculating an angle error according to the calculated attitude angle and the desired attitude angle, calculating a control amount of the attitude angle according to the angle error, and controlling the position and the attitude of the unmanned aerial vehicle according to the position control amount and the control amount of the attitude angle. The anti-interference flight control method and device provided by the invention inhibit deterministic interference and nondeterminacy interference by adopting a backstepping robust self-adaption and stability-increasing integral backstepping self-adaption algorithm, so that the control precision is improved, the anti-interference capability is enhanced, and the flight of the unmanned aerial vehicle is enabled to reach stability rapidly.
In order to make the above objects, features and advantages of the present invention more comprehensible, preferred embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 shows a block diagram of a flight control system provided by an embodiment of the present invention.
Fig. 2 shows a control schematic of the main control module in fig. 1.
Fig. 3 shows a block diagram of an anti-interference flight control device according to an embodiment of the present invention.
Fig. 4 is a schematic flow chart of an anti-interference flight control method according to an embodiment of the present invention.
Icon: 100-flight control system; 200-an anti-interference flight control device; 110-a main control module; a 120-sensor module; 130-a positioning module; 140-electric modulation; 150-an electric motor; 160-a wireless communication module; 210-a position information acquisition module; 220-an attitude angle calculation module; 230-a position error calculation module; 240-a position control amount calculation module; 250-a desired attitude angle calculation module; 260-an angle error calculation module; 270-attitude angle control amount calculation module; 280-control module.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. The components of the embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the invention, as presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be made by a person skilled in the art without making any inventive effort, are intended to be within the scope of the present invention.
It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures. Meanwhile, in the description of the present invention, the terms "first", "second", and the like are used only to distinguish the description, and are not to be construed as indicating or implying relative importance.
Referring to fig. 1, a block diagram of a flight control system 100 according to an embodiment of the present invention is provided. In this embodiment, the flight control system 100 may be applied to an unmanned aerial vehicle, such as a quad-rotor aircraft, and includes a main control module 110, a sensor module 120, a positioning module 130, an electric regulator 140, a motor 150, and a wireless communication module 160, wherein the sensor module 120, the positioning module 130, and the wireless communication module 160 are all electrically connected to the main control module 110, and the motor 150 is electrically connected to the main control module 110 through the electric regulator 140.
The wireless communication module 160 is configured to be communicatively coupled to a remote control device for receiving reference data transmitted by the remote control device. Wherein the reference data can be set by the user himself and transmitted via the remote control device.
The sensor module 120 is configured to collect gesture data of the unmanned aerial vehicle, so that the main control module 110 reads the gesture data and calculates a gesture angle according to the gesture data. In this embodiment, the sensor module 120 may include a gyroscope, an accelerometer, a magnetic compass, a temperature barometer (or referred to as a barometric pressure compensator).
The temperature_barometer is used for collecting external environment temperature and redundant air pressure data, and the main control module 110 is used for reading the external environment temperature and redundant air pressure data and obtaining height compensation data according to the read external environment temperature and redundant air pressure data.
The positioning module 130 is configured to collect data when the unmanned aerial vehicle flies, so that the main control module 110 reads the data and obtains the position information of the unmanned aerial vehicle according to the data collected by the positioning module 130. In this embodiment, the position information includes displacements in x-axis, y-axis and z-axis directions in the three-dimensional coordinate system, and the above-mentioned height compensation data can be used to compensate the displacement in the z-axis direction (i.e. the height information) in the position information, so that the obtained height information is more accurate.
The main control module 110 is configured to calculate a position control amount and a control amount of an attitude angle according to the position information, the set reference data, and the calculated attitude angle of the unmanned aerial vehicle, and control the electric regulator 140 to regulate the rotation speed of the motor 150 according to the position control amount and the control amount of the attitude angle, thereby controlling the position and the attitude of the unmanned aerial vehicle, so as to make the unmanned aerial vehicle fly stably.
Preferably, in the present embodiment, the number of the electric power conditioner 140 and the motor 150 is 4.
In this embodiment, the attitude angle includes a roll angle, a pitch angle, and a yaw angle, the set reference data includes a desired yaw angle and desired displacements in x-axis, y-axis, and z-axis directions, and the position control amounts include control amounts of displacements in x-axis, y-axis, and z-axis directions.
Fig. 2 is a control schematic diagram of the main control module 110. The main control module 110 adopts double-loop control, the inner loop controls the flight attitude of the unmanned aerial vehicle, the outer loop controls the flight position of the unmanned aerial vehicle, and the instruction signal (i.e. the set reference data) input into the main control module 110 is set as (x) d ，y d ，z d ，ψ d ) Wherein x is d 、y d 、z d Representing the expected displacement in the x-axis, y-axis and z-axis directions, ψ d Representing a desired yaw angle, wherein the final output control amount of the dual loop control is
u 1 For the control quantity of the outer ring control output, the control quantity of the vertical axis of the unmanned aerial vehicle (similar to the throttle), u 2 、u 3 、u 4 The control amount of roll angle phi, pitch angle theta, yaw angle phi are represented as the output amount of inner loop control, respectively, and the 4 control amounts correspond to the 4 electric tones 140 of the flight control system 100 to control the electric tones 140 to adjust the rotational speed of the motor 150. Wherein the outer loop control loop is based on a back-stepping method, and introduces an integration link based on the current position (x, y, z) and the expected position (x) by combining the self-adaptive principle d 、y d 、z d ) The error of (2) is adopted to obtain a vertical axis control quantity u by adopting a stability-increasing integral back-stepping self-adaptive control algorithm 1 To control the rising speed of the unmanned aerial vehicle; simultaneously, the outer loop control loop calculates the expected roll angle phi d Desired pitch angle θ d An inner loop control loop is input, and the inner loop control loop outputs a desired roll angle phi of the outer loop control output d Desired pitch angle θ d The desired yaw angle ψ in the command signal d As the desired attitude angle (phi) d 、θ d 、ψ d ) Based on the calculated actual attitude angle (phi, theta, phi) and the desired attitude angle (phi) d 、θ d 、ψ d ) Adopts a backstepping robust self-adaptive control algorithm to calculate the control quantity u of the roll angle phi 2 Control amount u of pitch angle θ 3 Control amount u of yaw angle ψ 4 Further, the main control module 110 is configured to control the control unit according to the control amount u 1 、u 2 、u 3 、u 4 And the position and the gesture of the unmanned aerial vehicle are controlled. />
From the above, it can be seen that the 6 parameters of phi, theta, phi, x, y, z are controlled by only 4 control amounts u 1 、u 2 、u 3 、u 4 The control is performed, so that the quadrotor unmanned aerial vehicle is a strong coupling underactuated system, and the attitude control and the position control of the quadrotor unmanned aerial vehicle have a coupling relationship. In this embodiment, the basic idea of the back-stepping method is adopted to decompose a complex flying system into subsystems which do not exceed the system orderThen, a Lyapunov function and an intermediate virtual control quantity are respectively designed for each subsystem, and the design of all the control quantities is completed in a reverse pushing mode.
Referring to fig. 3, a block diagram of an anti-interference flight control device 200 according to an embodiment of the invention is shown. The anti-interference flight control device 200 can be applied to the above-mentioned main control module 110, and is used for controlling the stable flight of the unmanned aerial vehicle, and comprises a position information acquisition module 210, an attitude angle calculation module 220, a position error calculation module 230, a position control amount calculation module 240, a desired attitude angle calculation module 250, an angle error calculation module 260, an attitude angle control amount calculation module 270 and a control module 280. It is understood that the above-mentioned position information obtaining module 210, attitude angle calculating module 220, position error calculating module 230, position control amount calculating module 240, desired attitude angle calculating module 250, angle error calculating module 260, attitude angle control amount calculating module 270 and control module 280 may be software function modules and computer programs stored in a memory and may be executed by the main control module 110.
The location information obtaining module 210 is configured to obtain location information of the unmanned aerial vehicle.
In this embodiment, the position information obtaining module 210 calculates and converts the data collected by the positioning module 130 to obtain the position information of the unmanned aerial vehicle, where the position information includes displacements in x-axis, y-axis, and z-axis directions in a three-dimensional coordinate system, which may be expressed as (x, y, z).
The attitude angle calculation module 220 is configured to obtain attitude data of the unmanned aerial vehicle, and calculate an attitude angle according to the attitude data.
In this embodiment, the attitude data of the unmanned aerial vehicle is acquired by the sensor module 120, and the attitude angle calculation module 220 calculates an accurate attitude angle by adopting an attitude fusion algorithm of nonlinear complementary filtering to the attitude data. In the present embodiment, the attitude angle includes a roll angle Φ, a pitch angle θ, and a yaw angle ψ. In this embodiment, the attitude angle calculation module 220 fuses the attitude data in a nonlinear complementary filtering manner, so as to reduce the influence of zero drift and temperature drift on the flight control system 100 and improve the anti-interference capability.
The position error calculating module 230 is configured to calculate a position error according to the position information and the set reference data.
In this embodiment, the set reference data is (x d ，y d ，z d ，ψ d ) Wherein x is d ，y d ，z d As an input for the outer ring position control, the position error calculation module 230 calculates a position error based on the set reference data (x d ，y d ，z d ) And the position information (x, y, z) to obtain a position error. For example, according to the z-axis direction displacement z (which can be understood as the flying height of the unmanned aerial vehicle) and the expected z-axis direction displacement z in the current position information d Can obtain the displacement error e in the z-axis direction 7 In the same way, the displacement error e in the x-axis direction can be obtained 9 Displacement error e in y-axis direction 11 。
The position control amount calculating module 240 is configured to calculate a position control amount according to the position error.
In the present embodiment, a state variable is set:
definition x 1 ＝φ，/>
x 3 ＝θ，/>
x 5 ＝ψ，/>
x 7 ＝z，/>
x 9 ＝x，
x 11 ＝y，/>
/>
Then a mathematical model of the drone may be built:
wherein m represents the mass of the unmanned aerial vehicle, l represents the distance from the mass center of the unmanned aerial vehicle to the rotary wing rotation shaft, and Ω r Represents the angular velocity of the unmanned aerial vehicle, I x 、I y 、I z Respectively representing moment of inertia in 3 axial directions of x, y and z axes, g represents gravitational acceleration, I r Representing the rotational inertia of the unmanned aerial vehicle rotor around the motor shaft.
In the present embodiment, the position control mainly controls (x, y, z) 3 displacement variables, taking height control in the position control as an example. Definition of external interference R 2 ＝(R x ,R y ,R z ) (i.e., forces along three axes x, y, z), wherein the state equation of the dynamic model of the altitude channel can be expressed as:
due to R z Is unknown, so the estimated value +.>
Instead of. Defining an estimation error: />
In this embodiment, the uncertainty of the model is considered to be a constant change, there is +.>
In the present embodiment, an integral term is introduced
Wherein->
The value of the integrated function satisfies +.>
Because of the introduction of the integration link, if the error is relatively large, a relatively large value can be obtained after integration, so that the system generates relatively severe shaking, the overshoot is increased, and the response time of the system is delayed. Therefore, the integrated function in the integral term is improved by adopting the idea of switching control. Specifically, define x id (i=7, 8,9,10,11, 12) as a desired value, and the limit adjustment band is taken as (x) id -Δ,x id +Δ), when the actual value x i In the case of the predetermined defined adjustment band-shaped region (i.e. approaching this region), the integrand in the integral term +.>
The value is zero; when x is i When not in the preset defined adjustment band-shaped area (i.e. other cases), the integrand in the integral term +.>
Take the value as the corresponding error e 7 、e 8 、e 9 、e 10 、e 11 、e 12 . By improving the integral term, when the flight control system 100 receives a strong interference signal, the oscillation amplitude of the system can be effectively reduced, and the robustness of the system to external disturbance and model uncertainty can be improved.
Thereby, the control amount of displacement in the z-axis direction is obtained:
In the present embodiment, isObtaining an estimated value
Selecting a Lyapunov function:
selecting an estimated value +.>
Is the adaptive law of (1): />
Then it can be obtained
The control amount of displacement in the z-axis direction can be expressed as:
similarly, the control amounts of displacement in the x-axis and y-axis directions can be obtained as follows:
wherein x is 7d 、x 9d 、x 11d E, respectively, the desired displacement in the z-axis, x-axis and y-axis directions 7 、e 8 E is the displacement error and the velocity error in the z-axis direction respectively 9 、e 10 E is the displacement error and the speed error in the x-axis direction respectively 11 、e 12 Respectively, displacement and velocity error in y-axis direction, c 7 、c 8 、c 9 、c 10 、c 11 、c 12 、λ 1 、λ 2 、λ 3 、ε 1 、ε 2 、ε 3 All represent control parameter variables ρ 7 、ρ 8 、ρ 9 、ρ 10 、ρ 11 、ρ 12 Are integral terms.
Therefore, when the control amounts of displacement in the x, y and z axis directions are the control amounts u obtained z 、u x 、u y And in addition, displacement errors in the directions of the x axis, the y axis and the z axis can be ensured to be exponentially stable. In the present embodiment, the control amount u is calculated based on the control amount u z 、u x 、u y The vertical axis control amount u can be obtained 1 The vertical axis control amount u 1 Is a complex vector, it being understood that the vertical axis controls the amount u 1 Projection in three-dimensional coordinate system and u z 、u x 、u y The unmanned aerial vehicle position control system has relevance, and can control the rising speed of the unmanned aerial vehicle, so that the purpose of unmanned aerial vehicle position control is achieved.
The desired attitude angle calculation module 250 is configured to obtain a desired attitude angle according to the position control amount and the attitude angle.
Specifically, since the attitude control and the position control have a coupling relationship, the desired attitude angle calculation module 250 can calculate the control amount u of the displacement in the x-axis and y-axis directions according to the above-mentioned control amount u x 、u y The coupling relation between the attitude angles (phi, theta, phi) and the roll angles (phi ) is calculated to obtain the expected roll angle phi d Desired pitch angle θ d Thereby obtaining a desired attitude angle (phi) d 、θ d 、ψ d ) As input for the inner ring attitude control.
The angle error calculation module 260 is configured to calculate an angle error according to the calculated attitude angle and the expected attitude angle.
In the present embodiment, the desired attitude angle includes the desired roll angle φ d Desired pitch angle θ d The desired yaw angle ψ d The angle error calculation module 260 is configured to calculate a desired roll angle phi based on the attitude angles (phi, theta, phi) d Desired pitch angle θ d The desired yaw angle ψ d Respectively calculating the roll angle phi, the pitch angle theta and the angle error e of the yaw angle phi 1 、e 3 、e 5 。
The attitude angle control amount calculation module 270 is configured to calculate a control amount of the attitude angle according to the angle error.
In the present embodiment, the attitude control mainly controls (Φ, θ, ψ) 3 attitude angle variables, taking the control amount of calculating the roll angle Φ as an example. Definition of external interference R 1 ＝(R φ ,R θ ,R ψ )，e 2 For roll angular velocity error, the dynamic model of roll angle phi can be expressed as:
definition of a non-negative smooth function mu φ (x) So that |R φ |≤γ φ μ φ (x) A. The invention relates to a method for producing a fibre-reinforced plastic composite Introducing a virtual control quantity alpha of a backstepping function φ For suppressing uncertainty R φ The impact on the system is, among other things,
τ 1 >0. definition of the unknown constant gamma φ The estimated value of (2) is +.>
sat (·) represents the saturation function. />
Is +.>
c φ >0。
Definition of the unknown constant gamma φ The error of (2) is
Selecting Lyapunov function
According to the stability theorem of the Lyapunov function,/>
the error in roll angle phi is progressively stable at that time. Therefore, the robust self-adaptive control law of the roll angle phi can be obtained as follows:
similarly, the control rates of the pitch angle θ and the yaw angle ψ are respectively:
in the above calculation formula, x 1d 、x 3d 、x 5d Respectively representing the desired roll angle, the desired pitch angle and the desired yaw angle, x 2 、x 4 、x 6 Respectively represent roll angular velocity, pitch angular velocity and yaw angular velocity, e 1 、e 2 E represents the angle error and the roll angle speed error of the roll angle respectively 3 、e 4 E represents the angle error and pitch angle speed error of the pitch angle, respectively 5 、e 6 Respectively representing an angle error and a yaw rate error of a yaw angle, alpha φ 、α θ Alpha and alpha ψ Respectively represent the virtual control quantity of the backstepping function, a 1 、a 2 、a 3 、b 1 、b 2 、b 3 、c 1 、c 2 、c 3 、c 4 、c 5 、c 6 All representing control parameter variables.
When the control amounts of the attitude angles (phi, theta and phi) are respectively u 2 、u 3 、u 4 At this time, the error of the attitude angle is progressively stabilized according to the stability theorem of the Lyapunov function.
The control module 280 is configured to control the position and the posture of the unmanned aerial vehicle according to the position control amount and the control amount of the posture angle.
In the present embodiment, the control module 280 controls the displacement u according to the x, y, and z axes z 、u x 、u y The vertical control amount u can be obtained 1 Then according to the vertical control amount u 1 Control amount u of roll angle phi 2 Control amount u of pitch angle θ 3 Control amount u of yaw angle ψ 4 The control signal is output to control the electric regulator 140 to regulate the rotating speed of the motor 150, so as to control the position and the gesture of the unmanned aerial vehicle, so that the position error and the angular error of the gesture angle of the unmanned aerial vehicle are close to 0, the control precision is high, and the flight stability of the unmanned aerial vehicle is improved.
Referring to fig. 4, a flow chart of an anti-interference flight control method according to an embodiment of the invention is shown. It should be noted that, the anti-interference flight control method according to the embodiment of the present invention is not limited to the specific sequence shown in fig. 4 and described below, and the basic principle and the technical effects thereof are the same as those of the anti-interference flight control device 200 provided in the above embodiment. It should be understood that, in other embodiments, the order of some steps in the anti-interference flight control method according to the present invention may be interchanged according to actual needs, or some steps may be omitted or deleted. The anti-interference flight control method can be applied to the above-mentioned main control module 110, and the specific flow shown in fig. 4 will be described in detail below.
Step S101, acquiring position information of the unmanned aerial vehicle.
It is understood that this step S101 may be performed by the above-described position information acquisition module 210.
Step S102, acquiring attitude data of the unmanned aerial vehicle, and calculating an attitude angle according to the attitude data.
It is understood that this step S102 may be performed by the attitude angle calculation module 220 described above.
Step S103, calculating a position error according to the position information and the set reference data.
It is understood that this step S103 may be performed by the position error calculation module 230 described above.
Step S104, calculating the position control quantity according to the position error.
It is understood that this step S104 may be performed by the position control amount calculation module 240 described above.
And step 105, obtaining a desired attitude angle according to the position control amount and the attitude angle.
It is understood that this step S105 may be performed by the desired attitude angle calculation module 250 described above.
And S106, calculating an angle error according to the calculated attitude angle and the expected attitude angle.
It is understood that this step S106 may be performed by the angle error calculation module 260 described above.
Step S107, calculating the control quantity of the attitude angle according to the angle error.
It is understood that this step S107 may be performed by the attitude angle control amount calculation module 270 described above.
And step S108, controlling the position and the posture of the unmanned aerial vehicle according to the position control quantity and the control quantity of the posture angle.
It is understood that this step S108 may be performed by the control module 280 described above.
In summary, the anti-interference flight control method and device provided by the embodiment of the invention comprise outer ring position control and inner ring attitude control, wherein the outer ring adopts a stability-increasing integral back-step self-adaptive control algorithm, and the control quantity u of x, y and z axis direction displacement is calculated based on position errors x 、u y 、u z According to the control quantity u x 、u y 、u z Output of vertical axis control quantity u 1 And according to the control quantity u x 、u y The relation between the expected roll angle phi and the attitude angles phi, theta and phi is calculated d Desired pitch angle θ d . The inner loop adopts a robust self-adaptive control algorithm and is based on attitude angles (phi, theta, phi) and expected attitude angles (phi) d 、θ d 、ψ d ) Is a mistake in (2)The difference is calculated to obtain the control quantity u of the roll angle phi 2 Control amount u of pitch angle θ 3 Control amount u of yaw angle ψ 4 Control amount u according to the output vertical axis 1 Control amount u of roll angle phi 2 Control amount u of pitch angle θ 3 Control amount u of yaw angle ψ 4 The rotation speed of the motor is controlled and electrically regulated, so that the position error and the angle error of the attitude angle of the unmanned aerial vehicle are close to 0, and further the position and the attitude of the unmanned aerial vehicle are controlled, so that the unmanned aerial vehicle can fly stably. The anti-interference flight control method and device provided by the embodiment of the invention adopt a back-step robust self-adaption and stability-increasing integral back-step self-adaption algorithm aiming at external deterministic interference and non-deterministic interference to enable the flight of the control unmanned aerial vehicle to reach stability rapidly, have high control precision, enhance the anti-interference capability of a flight control system to the outside, and have better control effects in the aspects of overshoot, adjustment time and robustness.
It should be noted that in this document, relational terms such as "first" and "second" and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The above description is only of the preferred embodiments of the present invention and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention. It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures.
Claims (4)
1. The anti-interference flight control method is applied to an unmanned aerial vehicle and is characterized by comprising the following steps of:
acquiring the position information of the unmanned aerial vehicle;
acquiring attitude data of the unmanned aerial vehicle, and calculating an attitude angle according to the attitude data;
calculating a position error according to the position information and the set reference data;
calculating a position control amount according to the position error;
obtaining a desired attitude angle according to the position control amount and the attitude angle;
calculating an angle error according to the calculated attitude angle and the expected attitude angle;
calculating the control quantity of the attitude angle according to the angle error;
controlling the position and the posture of the unmanned aerial vehicle according to the position control quantity and the control quantity of the posture angle;
the attitude angle comprises a roll angle, a pitch angle and a yaw angle, the set reference data comprises expected yaw angle and expected displacement in the directions of an x axis, a y axis and a z axis, the position control quantity comprises control quantity of displacement in the directions of the x axis, the y axis and the z axis, and the step of obtaining the expected attitude angle according to the position control quantity and the attitude angle comprises the following steps: according to the control amounts of the displacement in the x-axis direction and the y-axis direction and the attitude angle, calculating to obtain a desired roll angle and a desired pitch angle, wherein the desired attitude angle comprises the desired roll angle, the desired pitch angle and the desired yaw angle;
the calculation formula of the control quantity of the roll angle is as follows:
the calculation formula of the control quantity of the pitch angle is as follows:
the calculation formula of the control quantity of the yaw angle is as follows:
wherein x is 1d 、x 3d 、x 5d Respectively representing the desired roll angle, the desired pitch angle and the desired yaw angle, x 2 、x 4 、x 6 Respectively represent roll angular velocity, pitch angular velocity and yaw angular velocity, e 1 、e 2 E represents the angle error and the roll angle speed error of the roll angle respectively 3 、e 4 E represents the angle error and pitch angle speed error of the pitch angle, respectively 5 、e 6 Respectively representing an angle error and a yaw rate error of a yaw angle, alpha φ 、α θ Alpha and alpha ψ Respectively represent the virtual control quantity of the backstepping function, a 1 、a 2 、a 3 、b 1 、b 2 、b 3 、c 1 、c 2 、c 3 、c 4 、c 5 、c 6 All represent control parameter variables, I r Representing moment of inertia, Ω r Representing an angular velocity of the drone;
the calculation formula of the control amount of the displacement in the z-axis direction is as follows:
the calculation formula of the control quantity of the displacement in the x-axis direction is as follows:
the calculation formula of the control quantity of the y-axis directional displacement is as follows:
wherein m represents the mass of the unmanned aerial vehicle, u 1 Representing the vertical axis control quantity, x, of the unmanned aerial vehicle 7d 、x 9d 、x 11d E, respectively, the desired displacement in the z-axis, x-axis and y-axis directions 7 、e 8 E is the displacement error and the velocity error in the z-axis direction respectively 9 、e 10 E is the displacement error and the speed error in the x-axis direction respectively 11 、e 12 Respectively, displacement and velocity error in y-axis direction, c 7 、c 8 、c 9 、c 10 、c 11 、c 12 、λ 1 、λ 2 、λ 3 、ε 1 、ε 2 、ε 3 All represent control parameter variables ρ 7 、ρ 8 、ρ 9 、ρ 10 、ρ 11 、ρ 12 Are integral terms.
2. The tamper resistant flight control method of claim 1, wherein the integral term is
Wherein (1)>
The value of the integrated function is as follows
3. An anti-interference flight control device is applied to unmanned aerial vehicle, its characterized in that, anti-interference flight control device includes:
the position information acquisition module is used for acquiring the position information of the unmanned aerial vehicle;
the attitude angle calculation module is used for acquiring the attitude data of the unmanned aerial vehicle and calculating an attitude angle according to the attitude data;
the position error calculation module is used for calculating a position error according to the position information and the set reference data;
a position control amount calculation module for calculating a position control amount according to the position error;
the expected attitude angle calculation module is used for obtaining an expected attitude angle according to the position control quantity and the attitude angle;
the angle error calculation module is used for calculating an angle error according to the calculated attitude angle and the expected attitude angle;
the attitude angle control quantity calculation module is used for calculating the control quantity of the attitude angle according to the angle error;
the control module is used for controlling the position and the gesture of the unmanned aerial vehicle according to the position control quantity and the gesture angle control quantity;
the attitude angle comprises a roll angle, a pitch angle and a yaw angle, the set reference data comprises expected yaw angles and expected displacements in x-axis, y-axis and z-axis directions, the position control quantity comprises control quantities of the displacements in the x-axis, y-axis and z-axis directions, and the expected attitude angle calculation module is used for calculating the expected roll angle and the expected pitch angle according to the control quantities of the displacements in the x-axis and y-axis directions and the attitude angle, wherein the expected attitude angle comprises the expected roll angle, the expected pitch angle and the expected yaw angle;
the attitude angle control quantity calculation module is used for calculating the attitude angle control quantity according to a formula
the position control quantity calculation module is used for calculating a position control quantity according to a formula
Obtaining the control quantity of the z-axis direction displacement, wherein the position control quantity calculation module is also used for calculating the control quantity according to a formula
Formula (I)
Respectively calculating control amounts of displacement in the x-axis direction and the y-axis direction, wherein m represents the mass of the unmanned aerial vehicle, and u 1 Representing the vertical axis control quantity, x, of the unmanned aerial vehicle 7d 、x 9d 、x 11d E, respectively, the desired displacement in the z-axis, x-axis and y-axis directions 7 、e 8 E is the displacement error and the velocity error in the z-axis direction respectively 9 、e 10 E is the displacement error and the speed error in the x-axis direction respectively 11 、e 12 Respectively, displacement and velocity error in y-axis direction, c 7 、c 8 、c 9 、c 10 、c 11 、c 12 、λ 1 、λ 2 、λ 3 、ε 1 、ε 2 、ε 3 All represent control parameter variables ρ 7 、ρ 8 、ρ 9 、ρ 10 、ρ 11 、ρ 12 Are integral terms.
4. The tamper resistant flight control device of claim 3, wherein the integral term is
Wherein (1)>
The value of the integrated function is as follows
/>
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710605267.7A CN107247459B (en) | 2017-07-24 | 2017-07-24 | Anti-interference flight control method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710605267.7A CN107247459B (en) | 2017-07-24 | 2017-07-24 | Anti-interference flight control method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107247459A CN107247459A (en) | 2017-10-13 |
| CN107247459B true CN107247459B (en) | 2023-06-09 |
Family
ID=60014080
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710605267.7A Active CN107247459B (en) | 2017-07-24 | 2017-07-24 | Anti-interference flight control method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107247459B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107943049B (en) * | 2017-12-13 | 2020-04-21 | 江西洪都航空工业集团有限责任公司 | Unmanned vehicle control method and unmanned mowing vehicle |
| WO2019205152A1 (en) * | 2018-04-28 | 2019-10-31 | 深圳市大疆创新科技有限公司 | Cradle head control method and cradle head |
| CN109947126B (en) * | 2019-03-07 | 2020-04-14 | 中国科学院深圳先进技术研究院 | Control method, device and equipment of quad-rotor unmanned aerial vehicle and readable medium |
| CN110058601B (en) * | 2019-03-19 | 2021-12-21 | 沈阳无距科技有限公司 | Method and device for rapidly judging take-off and landing of unmanned helicopter and electronic equipment |
| CN113031636B (en) * | 2021-03-01 | 2024-02-20 | 之江实验室 | Unmanned aerial vehicle control method, unmanned aerial vehicle control device, electronic equipment, unmanned aerial vehicle and storage medium |
| CN114237298B (en) * | 2021-12-21 | 2024-04-05 | 中国人民解放军海军航空大学 | Control method and system for unmanned aerial vehicle formation in-flight plane to follow long plane |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102830622A (en) * | 2012-09-05 | 2012-12-19 | 北京理工大学 | Auto-disturbance-rejection automatic flight control method for four-rotor aircraft |
| CN105607640A (en) * | 2016-01-20 | 2016-05-25 | 南京工业大学 | Pose controller for four-rotor aircraft |
| CN105676641A (en) * | 2016-01-25 | 2016-06-15 | 南京航空航天大学 | Nonlinear robust controller design method based on back-stepping and sliding mode control technologies and aimed at nonlinear model of quad-rotor unmanned plane |
| CN105759832A (en) * | 2016-05-20 | 2016-07-13 | 武汉科技大学 | Four-rotor aircraft sliding mode variable structure control method based on inversion method |
| WO2017025724A1 (en) * | 2015-08-13 | 2017-02-16 | Bae Systems Plc | Apparatus and method for communications management |
| CN106945046A (en) * | 2017-04-24 | 2017-07-14 | 华南理工大学 | Mechanical arm control system and its control method based on variation rigidity elastic driver |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110042510A1 (en) * | 2009-08-24 | 2011-02-24 | Bevirt Joeben | Lightweight Vertical Take-Off and Landing Aircraft and Flight Control Paradigm Using Thrust Differentials |
-
2017
- 2017-07-24 CN CN201710605267.7A patent/CN107247459B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102830622A (en) * | 2012-09-05 | 2012-12-19 | 北京理工大学 | Auto-disturbance-rejection automatic flight control method for four-rotor aircraft |
| WO2017025724A1 (en) * | 2015-08-13 | 2017-02-16 | Bae Systems Plc | Apparatus and method for communications management |
| CN105607640A (en) * | 2016-01-20 | 2016-05-25 | 南京工业大学 | Pose controller for four-rotor aircraft |
| CN105676641A (en) * | 2016-01-25 | 2016-06-15 | 南京航空航天大学 | Nonlinear robust controller design method based on back-stepping and sliding mode control technologies and aimed at nonlinear model of quad-rotor unmanned plane |
| CN105759832A (en) * | 2016-05-20 | 2016-07-13 | 武汉科技大学 | Four-rotor aircraft sliding mode variable structure control method based on inversion method |
| CN106945046A (en) * | 2017-04-24 | 2017-07-14 | 华南理工大学 | Mechanical arm control system and its control method based on variation rigidity elastic driver |
Non-Patent Citations (4)
| Title |
|---|
| Ziwei Song等.modeling and maneuvering control for tricopter based on the back-stepping method.2016IEEE CGNCC.2017,全文. * |
| 朱文武 ; 都海波 ; .基于有限时间姿态控制算法的四旋翼飞行器悬停控制.系统科学与数学.2017,(第04期),全文. * |
| 王健等.基于自适应模糊PID的四旋翼飞行器悬停控制.桂林电子科技大学学报.2016,第36卷(第36期),406-411. * |
| 蔡敏 ; 王思明 ; .抗时滞LADRC的六旋翼飞行器姿态控制.科学技术与工程.2017,(第06期),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107247459A (en) | 2017-10-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107247459B (en) | Anti-interference flight control method and device | |
| CN106959110B (en) | Cloud deck attitude detection method and device | |
| CN110081878B (en) | Method for determining attitude and position of multi-rotor unmanned aerial vehicle | |
| Hua et al. | Introduction to feedback control of underactuated VTOLvehicles: A review of basic control design ideas and principles | |
| Hong | Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (UAV) | |
| CN102830622B (en) | Auto-disturbance-rejection automatic flight control method for four-rotor aircraft | |
| Yang et al. | Optimal path following for small fixed-wing UAVs under wind disturbances | |
| Wang et al. | High-order nonlinear differentiator and application to aircraft control | |
| CN111221347B (en) | Acceleration compensation method and system in attitude estimation of vertical take-off and landing fixed wing unmanned aerial vehicle | |
| Stephan et al. | Linear parameter-varying control for quadrotors in case of complete actuator loss | |
| CN108592911A (en) | A kind of quadrotor kinetic model/airborne sensor Combinated navigation method | |
| CN110793515A (en) | Unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large-mobility condition | |
| CN113296525A (en) | Data-driven nonlinear control method for tilting three-rotor unmanned aerial vehicle | |
| CN115454113A (en) | Attitude stability control method of unmanned aerial vehicle in high-altitude environment | |
| Pollini et al. | Simulation and robust backstepping control of a quadrotor aircraft | |
| Ackerman et al. | Flight control methods for multirotor uas | |
| Lin et al. | Design of anti-interference system for fully autonomous UAV based on ADRC-EKF algorithm | |
| CN109521785A (en) | It is a kind of to clap Smart Rotor aerocraft system with oneself | |
| Zhan et al. | Control system design and experiments of a quadrotor | |
| CN108693372A (en) | A kind of course axis angular rate method of estimation of quadrotor | |
| Nguyen et al. | Aerodynamic characteristics of quadrotor helicopter | |
| Emran et al. | Hybrid low-cost approach for quadrotor attitude estimation | |
| Yang et al. | Model-free integrated navigation of small fixed-wing UAVs full state estimation in wind disturbance | |
| Wang et al. | Attitude estimation for UAV with low-cost IMU/ADS based on adaptive-gain complementary filter | |
| Li et al. | Quaternion-based robust extended kalman filter for attitude estimation of micro quadrotors using low-cost mems |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |