An obstacle avoidance control method for an underactuated quadrotor unmanned aerial vehicle

Abstract

The application provides an under-actuated four-rotor unmanned aerial vehicle obstacle avoidance control method with motor failure fault and interference, the control method comprises an obstacle avoidance loop and a tracking loop, an improved obstacle avoidance strategy is designed in the obstacle avoidance loop by introducing a height factor in an artificial potential field function, so that the unmanned aerial vehicle can be prevented from falling into a local optimal trap when encountering an obstacle; meanwhile, the motor failure fault of the unmanned aerial vehicle is considered, an adaptive technology is used to estimate fault parameters on line, so as to inhibit the influence of the motor failure fault in the tracking loop. Each subsystem is stabilized through backstepping technology, and the stability of the closed-loop unmanned aerial vehicle system is ensured; compared with a traditional obstacle avoidance strategy, the technical scheme can not only solve the problem of falling into a local optimum, but also compensate the influence of the motor failure fault on the unmanned aerial vehicle.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned aerial vehicle obstacle avoidance and unmanned aerial vehicle fault-tolerant technology, in particular, especially relates to an underactuated quadrotor unmanned aerial vehicle obstacle avoidance control method with motor failure fault and external disturbance. BACKGROUND

[0002] When the unmanned aerial vehicle performs flight tasks, due to the complexity of the flight environment and the diversity of obstacles in its path, it needs to have the ability of autonomous obstacle avoidance. In view of the above problems, various obstacle avoidance schemes have been proposed, including the geometric method proposed by X. Wang, the force field method proposed by Z. Pan, the optimization-based method proposed by D. Wang, the perception and obstacle avoidance method proposed by Hyeon-Cheol Lee, etc. Among them, the force field method is also called artificial potential field method. This method makes the unmanned aerial vehicle become a particle in multiple force fields, so as to avoid obstacles by manipulating attractive and repulsive forces. Z. Pan et al. introduced a rotating potential field to solve the local minimum problem of the artificial potential field method, so that the unmanned aerial vehicle can get rid of the common local minimum and oscillation. J. Feng et al. considered static and dynamic obstacles at the same time, and proposed a new dynamic path planning algorithm based on obstacle position prediction and APF correction. O. Montiel et al. proposed a method to find the optimal path in an environment with static and dynamic obstacles. L. Zhu et al. proposed a 3D collision avoidance strategy to solve the obstacle avoidance problem in dynamic environment. The above researches put forward some solutions to the local minimum problem, such as introducing rotating potential field, new virtual obstacle method, etc. However, the above methods have certain limitations. For example, the rotating potential field can only make the unmanned aerial vehicle pass through a narrow channel, and may not work for dense obstacles. The virtual obstacle method fills the local minimum point with virtual obstacles, which may increase the tracking error of the unmanned aerial vehicle. Therefore, it is urgent to propose a new type of obstacle avoidance method that can overcome the above limitations.

[0003] On the other hand, the failure of the UAV itself during the task is also an important factor leading to the failure of the task. Due to the characteristics of the reduction of the size of the equipment, the reduction of the switching energy and the acceleration of the operation rate of the equipment, the quad-rotor UAV is more likely to encounter motor failure and other problems. Once the motor fails, the obstacle avoidance strategy may fail in the obstacle avoidance loop. In order to solve these problems, fault-tolerant control emerges as the times require. H. Ma et al. proposed an adaptive controller based on an observer to estimate and compensate for faults without adding sensors or actuators to increase hardware redundancy. S. Sun et al. designed a sensor-based fault-tolerant controller for a quad-rotor UAV with a flight speed greater than 8 m / s and two rotor failures. For the problem of multi-UAV fault-tolerant cooperative control with actuator failure and input saturation, Z. Yu et al. used dynamic surface control to construct a distributed fault-tolerant control scheme. However, when the UAV encounters obstacles, the above fault-tolerant methods may fail. Therefore, the obstacle avoidance strategy not only has the ability to avoid obstacles, but also needs to have fault-tolerant capability, so the invention will study the control of heterogeneous vehicle fleet. SUMMARY

[0004] According to the above-mentioned technical problems that the obstacle avoidance strategy cannot handle dense obstacles and increase the tracking error of the UAV, and the traditional obstacle avoidance strategy cannot handle motor failure, an obstacle avoidance control method for an under-actuated quad-rotor UAV with motor failure and external disturbance is provided. Compared with the traditional obstacle avoidance strategy, the technical scheme of the invention not only solves the problem of falling into local optimization, but also compensates for the influence of motor failure on the UAV.

[0005] The technical means adopted by the invention are as follows:

[0006] An obstacle avoidance control method for an under-actuated quad-rotor UAV with motor failure and external disturbance, characterized in that it comprises the following steps:

[0007] S1, force analysis is performed on the movement of the UAV, and a UAV dynamics model under motor failure is established in combination with a motor failure model;

[0008] S2, obstacle information is obtained, and an improved artificial potential field obstacle avoidance strategy is constructed;

[0009] S3, based on the obstacle avoidance strategy constructed in step S2, a gravitational field function and a repulsive field function are established;

[0010] S4, based on the gravitational field function and the repulsive field function established in step S3, appropriate potential field parameters are selected to obtain corresponding gravitational force, repulsive force and UAV reference position.

[0011] S5, based on the expected path obtained in step S4, an adaptive backstepping fault-tolerant control strategy is constructed. An appropriate Lyapunov function is selected, a fault-tolerant controller and an adaptive update rate are designed, and the stability of the system is proved.

[0012] The specific process of the step S1 is as follows:

[0013] S11, define the dynamic model of the unmanned aerial vehicle, as follows:

[0014]

[0015]

[0016]

[0017]

[0018] Wherein, p, v, η, ω respectively represent the position, speed, angle and angular acceleration of the unmanned aerial vehicle, and m is the mass of the unmanned aerial vehicle; K is an unknown aerodynamic parameter; e3 = [0 0 1] T ; g is the acceleration of gravity; d = [d1 d2 d3] T is an unknown time-varying external disturbance; η is an extended attitude angle; ω is an extended attitude angular velocity; h1(η), h2(ω) are system nonlinear terms; B is a constant matrix; f is the lift provided by the four motors of the unmanned aerial vehicle; Ω is a virtual control quantity; h1(η) and h2(ω) are nonlinear terms;

[0019] S12, combine the actuator fault model to establish a vehicle longitudinal dynamics model under actuator fault; the actuator fault model is specifically:

[0020] f i F = ρ i f i

[0021] Wherein, f i F is the control input under motor fault, and ρ i represents the fault factor of the motor;

[0022] The actuator fault model is brought into the unmanned aerial vehicle dynamics model to obtain the unmanned aerial vehicle dynamics model under motor fault as follows:

[0023]

[0024]

[0025]

[0026]

[0027] The specific process of the step S2 is as follows:

[0028] S21. Define the obstacle avoidance target as follows:

[0029] ||p(t)-p oi (t)||>d safe

[0030] Where, p i Let be the location of the drone, and be the location of the i-th obstacle. safe Safe distance between drones and obstacles;

[0031] The specific process of step S3 is as follows:

[0032] S31. To achieve the obstacle avoidance objective, the repulsive force field function and the gravitational force field function are constructed as follows:

[0033]

[0034]

[0035] Where, p oi =[x oi y oi z oi ] T p ixy =[xy 0] T -[x oi y oi 0] T ;p iz =[00z oi ] T -[00z] T ;0 3 =

[000] T ;x oi y oi and z oi Let r represent the coordinates and height of the i-th obstacle, respectively. p This refers to the horizontal safe distance between the drone and the center of the obstacle during flight. c Z is the obstacle collision distance during drone flight. xy It is the horizontal distance between the drone and the moving target on the ground;

[0036] The specific process of step S4 is as follows:

[0037] S41. The expressions for attraction and repulsion are as follows:

[0038]

[0039]

[0040] Among them, Fattract It is the expression for gravity; F repel This is the repulsive force expression; k2 is the gravitational coefficient; z1 is the difference between the actual position and the target position; z2 is the velocity error; v d The desired speed for the drone;

[0041] S42. By substituting the gravitational and repulsive forces as the resultant forces acting on the UAV into the UAV dynamics model, the UAV's reference position p can be obtained. d .

[0042] The specific process of step S5 is as follows:

[0043] S51. Based on the UAV reference position obtained in step S4, the error between the UAV reference position and the actual position is defined as:

[0044] z3=pp d

[0045] The S52 PD position controller is designed as follows:

[0046]

[0047] S53. Based on the nonlinear adaptive backstepping method, the following error variables were designed:

[0048]

[0049] z5=ω-ω d

[0050] Where, η d ω is the desired attitude angle. d a1 and a2 are the desired angular velocity; a1 and a2 are the gain coefficients.

[0051] S54. Design adaptive error, specifically:

[0052]

[0053] In the formula, It is the estimation error of ρ. yes The estimation error;

[0054] S55. Design an adaptive update law, specifically:

[0055]

[0056]

[0057] S56. Combining the backstepping control method and the adaptive method to process the entire closed-loop system, an adaptive backstepping fault-tolerant controller is obtained, specifically:

[0058]

[0059] in, This is an estimate of the upper limit of external disturbances.

[0060] S57. Construct a Lyapunov function with the following function expression:

[0061]

[0062] Differentiating the Lyapunov function and substituting the adaptive update law and control law into the formula after differentiating the Lyapunov function, we obtain:

[0063]

[0064] According to Lyapunov stability theory, the closed-loop system is stable.

[0065] Furthermore, after step S5, the method further includes:

[0066] S6. A simulation verification study was conducted on the UAV dynamics model, backstepping fault-tolerant controller, and adaptive update law of the obstacle avoidance control method for underactuated quadrotor UAVs with motor failure faults and external disturbances. The results were compared with conventional methods to further verify the effectiveness and superiority.

[0067] Compared with the prior art, the present invention has the following advantages:

[0068] 1. Existing obstacle avoidance strategies may fall into the trap of local optima. To avoid this, this paper introduces an altitude factor into the artificial potential field function to generate new paths. Therefore, the obstacle avoidance control strategy ensures that by increasing the drone's altitude, the path can escape local optima, thus enabling the drone to climb over obstacles.

[0069] 2. When a UAV encounters obstacles, the existing controller performs satisfactorily in terms of trajectory tracking. However, when a malfunction occurs, the original obstacle avoidance strategy may fail. Therefore, this paper proposes combining UAV obstacle avoidance with fault tolerance to achieve autonomous obstacle avoidance in the event of a motor failure.

[0070] 3. This invention takes into account the occurrence of motor faults, establishes a Lyapunov function containing fault information, and obtains the asymptotic stability conditions of the entire closed-loop system based on the backstepping control method and adaptive compensation technology.

[0071] Based on the above reasons, this invention can be widely applied in fields such as obstacle avoidance for unmanned aerial vehicles. Attached Figure Description

[0072] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0073] Figure 1 This is a flowchart of the method of the present invention.

[0074] Figure 2 A schematic diagram of a drone provided for an embodiment of the present invention.

[0075] Figure 3 A simulation diagram showing the error between the actual position and the target position provided for an embodiment of the present invention.

[0076] Figure 4 A simulation diagram showing the error between the reference position and the actual position provided in an embodiment of the present invention.

[0077] Figure 5 The simulation diagram of attitude angle error provided for the embodiment of the present invention. Detailed Implementation

[0078] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0079] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0080] like Figure 1 As shown, the present invention provides an obstacle avoidance control method for an underactuated quadcopter unmanned aerial vehicle (UAV) with motor failure and external disturbances, comprising the following steps:

[0081] S1. Perform force analysis on the motion of the UAV and, in conjunction with the motor fault model, establish a dynamic model of the UAV under motor fault.

[0082] S2, obstacle information, constructing an improved artificial potential field obstacle avoidance strategy;

[0083] S3. Based on the obstacle avoidance strategy constructed in step S2, establish the gravitational field function and the repulsive field function;

[0084] S4. Based on the gravitational field function and repulsive field function established in step S3, select appropriate potential field parameters to obtain the corresponding gravitational, repulsive and UAV reference positions.

[0085] S5. Based on the desired path obtained in step S4, construct an adaptive backstepping fault-tolerant control strategy. Select a suitable Lyapunov function, design a fault-tolerant controller and an adaptive update rate, and prove the stability of the system.

[0086] The specific process of step S1 is as follows:

[0087] S11. Define the dynamic model of the UAV as follows:

[0088]

[0089]

[0090]

[0091]

[0092] Where p, v, η, and ω represent the position, velocity, angle, and angular acceleration of the unmanned aerial vehicle (UAV), respectively, η is the mass of the UAV; K is an unknown aerodynamic parameter; e3 = [0 0 1] T g is the acceleration due to gravity; d = [d1 d2 d3] T For unknown time-varying external disturbances; η is the extended attitude angle; ω is the extended attitude angular velocity; h1(η) and h2(ω) are the system nonlinear terms; B is a constant matrix; f is the lift provided by the four motors of the UAV; Ω is the virtual control quantity; h1(η) and h2(ω) are nonlinear terms;

[0093] S12. Based on the actuator failure model, establish a longitudinal dynamics model of the vehicle under actuator failure; the actuator failure model is specifically as follows:

[0094] f i F =ρ i f i

[0095] Among them, f iF It is the control input under motor failure, ρ i Fault factors representing the motor;

[0096] Substituting the actuator failure model into the UAV dynamics model, the UAV dynamics model under the motor failure is obtained as follows:

[0097]

[0098]

[0099]

[0100]

[0101] The specific process of step S2 is as follows:

[0102] S21. Define the obstacle avoidance target as follows:

[0103] ||p(t)-p oi (t)||>d safe

[0104] Where, p i Let be the location of the drone, and be the location of the i-th obstacle. safe Safe distance between drones and obstacles;

[0105] Based on Example 2, the specific process of step S3 is as follows:

[0106] S31. To achieve the obstacle avoidance objective, the repulsive force field function and the gravitational force field function are constructed as follows:

[0107]

[0108]

[0109] Where, p oi =[x oi y oi z oi ] T p ixy =[xy 0] T -[x oi y oi 0] T ;p iz =[00z oi ] T -[00z] T ;0 3 =

[000] T ;x oi yoi and z oi Let r represent the coordinates and height of the i-th obstacle, respectively. p This refers to the horizontal safe distance between the drone and the center of the obstacle during flight. c Z is the obstacle collision distance during drone flight. xy It is the horizontal distance between the drone and the moving target on the ground;

[0110] The specific process of step S4 is as follows:

[0111] S41. The expressions for attraction and repulsion are as follows:

[0112]

[0113]

[0114] Among them, F attract It is the expression for gravity; F repel This is the repulsive force expression; k2 is the gravitational coefficient; z1 is the difference between the actual position and the target position; z2 is the velocity error; v d The desired speed for the drone;

[0115] S42. By substituting the gravitational and repulsive forces as the resultant forces acting on the UAV into the UAV dynamics model, the UAV's reference position p can be obtained. d .

[0116] The specific process of step S5 is as follows:

[0117] S51. Based on the UAV reference position obtained in step S4, the error between the UAV reference position and the actual position is defined as:

[0118] z3=pp d

[0119] The S52 PD position controller is designed as follows:

[0120]

[0121] S53. Based on the nonlinear adaptive backstepping method, the following error variables were designed:

[0122]

[0123] z5=ω-ω d

[0124] Where, η d ω is the desired attitude angle. d a1 and a2 are the desired angular velocity; a1 and a2 are the gain coefficients.

[0125] S54. Design adaptive error, specifically:

[0126]

[0127] In the formula, It is the estimation error of ρ. yes The estimation error;

[0128] S55. Design an adaptive update law, specifically:

[0129]

[0130]

[0131] S56. Combining the backstepping control method and the adaptive method to process the entire closed-loop system, an adaptive backstepping fault-tolerant controller is obtained, specifically:

[0132]

[0133] in, This is an estimate of the upper limit of external disturbances.

[0134] S57. Construct a Lyapunov function with the following function expression:

[0135]

[0136] Differentiating the Lyapunov function and substituting the adaptive update law and control law into the formula after differentiating the Lyapunov function, we obtain:

[0137]

[0138] According to Lyapunov stability theory, the closed-loop system is stable.

[0139] The method also includes:

[0140] S6. A simulation verification study was conducted on the UAV dynamics model, backstepping fault-tolerant controller, and adaptive update law under fault conditions using an obstacle avoidance control scheme for an underactuated quadrotor UAV with motor failure and external disturbances. The results were compared with conventional methods to further verify the effectiveness and superiority.

[0141] To verify the effectiveness of the obstacle avoidance control method for underactuated quadrotor UAVs with motor failure and external disturbances provided in this embodiment, a simulation experiment was conducted using MATLAB, and a detailed explanation was provided.

[0142] like Figure 2As shown, this is the UAV model provided in this embodiment. Taking into account motor failures and external disturbances, backstepping control technology and adaptive technology are adopted to design a fault-tolerant controller, which enables the closed-loop system to gradually stabilize, has good tracking performance, and has a certain robustness to motor failures and good suppression of external disturbances.

[0143] Specifically, the physical parameters of the drone selected in this embodiment are as follows: drone mass m = 0.55 kg; distance from the motor to the drone's center of mass l = 0.11 m; torque coefficient c = 1.1905 * 10 -2 N*ms 2 / rad 2 The three-dimensional rotational inertia of the UAV is J. x =1.9*10 -3 N*ms 2 / rad 2 J y =1.9*10 -3 N*ms 2 / rad 2 J z =2.4*10 -3 N*ms 2 / rad 2 The trajectory of the ground moving target selected in this paper is as follows:

[0144] x(t)=0

[0145] y(t)=0.5*t

[0146] The motor failure faults of the drone are as follows:

[0147]

[0148] The external disturbances to the drone are as follows:

[0149]

[0150] In this embodiment, three obstacles are selected, with the following physical parameters: Obstacle 1 has a height of 12m, an x-axis position of -1m, and a y-axis position of 2m; Obstacle 2 has a height of 12m, an x-axis position of 1m, and a y-axis position of 2m; Obstacle 3 has a height of 12m, an x-axis position of 0.1m, and a y-axis position of 2m; The horizontal safe distance r between the UAV and the center of the obstacle is [missing information]. p =2m; is the obstacle collision distance r during drone flight. c =1m.

[0151] Based on the above parameters, the obstacle avoidance control method for underactuated quadrotor UAVs with motor failure and external disturbances proposed in this invention is verified through simulation.Figures 3-5 As shown. Among them, Figure 3 The error between the actual position and the target position decreases and eventually converges to near 0, and autonomous obstacle avoidance is achieved when encountering obstacles; Figure 4 It displays the error between the reference position and the actual position, and gradually converges to near 0; Figure 5 The attitude angle error was shown and gradually converged to near 0, demonstrating excellent tracking performance. This completes the digital simulation of the algorithm, verifying its effectiveness.

[0152] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0153] In the above embodiments of the present invention, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0154] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for obstacle avoidance control of an underactuated quadrotor unmanned aerial vehicle (UAV) with motor failure and external disturbances, characterized in that, Includes the following steps: S1: Perform force analysis on the motion of the UAV and establish a dynamic model of the UAV under motor failure by combining the motor failure model; S2: Constructing an improved artificial potential field obstacle avoidance strategy based on obstacle information; S3: Establish the gravitational field function and repulsive field function based on the obstacle avoidance strategy; S4: Based on the gravitational field function and the repulsive field function, select appropriate potential field parameters to obtain the corresponding gravitational force, repulsive force and the expected path of the UAV; S5: Construct an adaptive backstepping fault-tolerant control strategy based on the UAV's desired path, select a suitable Lyapunov function to design a fault-tolerant controller and adaptive update rate, and prove the stability of the system; The specific process of step S3 is as follows: S31. To achieve the obstacle avoidance objective, the repulsive force field function and the gravitational force field function are constructed as follows: in, , ; ; ; , and They represent the first The coordinates and height of each obstacle. It is the horizontal safe distance between the drone and the center of the obstacle during flight. It is the obstacle collision distance during drone flight. It is the horizontal distance between the drone and the moving target on the ground.

2. The obstacle avoidance control method for an underactuated quadrotor UAV with motor failure and external disturbances as described in claim 1, characterized in that: When establishing the UAV dynamics model under motor failure: Define the UAV dynamic model, combine the actuator failure model to establish the UAV dynamics model under actuator failure, and then input the actuator failure model into the UAV dynamics model to obtain the UAV dynamics model under motor failure.

3. The obstacle avoidance control method for an underactuated quadrotor UAV with motor failure and external disturbances according to claim 2, characterized in that, The specific process of step S2 is as follows: S21. Define the obstacle avoidance target as follows: in, Location of the drone. It is the first The location of the obstacle Safe distance between drones and obstacles.

4. The obstacle avoidance control method for an underactuated quadrotor UAV with motor failure and external disturbances as described in claim 1, characterized in that, The specific process of step S4 is as follows: S41. The expressions for attraction and repulsion are as follows: in, It is the expression for gravity. It is a repulsive force expression. It is the gravitational coefficient. This is the difference between the actual position and the target position. For speed error, The desired speed for the drone; S42. By substituting the gravitational and repulsive forces as the resultant forces acting on the UAV into the UAV dynamics model, the UAV's reference position can be obtained. .

5. The obstacle avoidance control method for an underactuated quadrotor UAV with motor failure and external disturbances according to claim 4, characterized in that, The specific process of step S5 is as follows: Based on the UAV's reference position, the error between the UAV's reference position and its actual position is defined as: The position controller is designed as follows: Based on the nonlinear adaptive backstepping method, the following error variables are designed: in, The desired attitude angle; The desired angular velocity; , Gain coefficient The design incorporates adaptive error, specifically: ， In the formula, yes The estimation error, yes The estimation error; The adaptive update law is designed as follows: By combining backstepping control and adaptive methods to process the entire closed-loop system, an adaptive backstepping fault-tolerant controller is obtained, specifically: in, This is an estimate of the upper limit of external disturbances; Construct a Lyapunov function, whose function expression is as follows: Differentiating the Lyapunov function and substituting the adaptive update law and control law into the formula after differentiating the Lyapunov function, we obtain: According to Lyapunov stability theory, the closed-loop system is stable.

Images