Quadrotor unmanned aerial vehicle trajectory tracking and obstacle avoidance control method and system

Abstract

This invention provides a method and system for trajectory tracking and obstacle avoidance control of a quadcopter unmanned aerial vehicle (UAV). The method includes: acquiring the desired position trajectory of the UAV and the center position and equivalent radius of the obstacle; determining the nominal position control quantity of the UAV based on the desired position trajectory; constructing a control obstacle function based on the center position and equivalent radius of the obstacle; determining the final position control quantity of the UAV based on the control obstacle function and the nominal position control quantity; determining the desired attitude of the UAV based on the final position control quantity; determining the nominal control torque of the UAV based on the desired attitude; determining the final control torque of the UAV based on the nominal control torque; and determining the PWM duty cycle of the UAV based on the final control torque to drive each motor to drive the propeller for trajectory tracking and obstacle avoidance. This invention achieves effective synergy between tracking performance and obstacle avoidance safety, and has the advantages of fast convergence in finite time and strong robustness.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) control technology, and particularly relates to a method and system for trajectory tracking and obstacle avoidance control of a quadcopter UAV. Background Technology

[0002] Quadcopter drones, due to their simple structure, compact size, and ability to take off and land vertically and hover, have been widely used in aerial surveying, road and pipeline inspection, disaster emergency rescue, and environmental exploration and detection. This has placed higher demands on the control performance of drones: they not only need to achieve high-precision and fast trajectory tracking, but also need to have real-time obstacle avoidance capabilities to ensure flight safety.

[0003] Current mainstream quadrotor UAV flight control schemes include those based on model structure characteristics, dividing the control system into an outer position loop and an inner attitude loop, with separate controllers designed for each: The outer position loop uses methods such as proportional-integral-derivative (PID), dynamic surface control, or model predictive control to generate the desired attitude command; the inner attitude loop uses methods such as sliding mode control, adaptive control, or optimal control to track the desired attitude output by the outer position loop. A simple finite-time tracking control scheme focuses on the rapid convergence of the system state, designing controllers based on homogeneous system theory, power integral techniques, or terminal sliding mode theory: corresponding finite-time controllers are designed for the inner attitude loop and the outer position loop to fully utilize the UAV's hardware performance. An obstacle avoidance control scheme combining a control obstacle function (CBF) and a control Lyapunov function (CLF) focuses on both safety and controllability, using CBF to construct safety constraints to ensure obstacle avoidance and CLF to maintain UAV stability.

[0004] Existing technologies struggle to simultaneously achieve both safe obstacle avoidance and high-precision, fast-response trajectory tracking performance for UAVs. They often present an either-or dilemma: simple finite-time tracking control schemes optimize convergence speed and accuracy but neglect obstacles, posing a collision risk; while simple control-by-obstacle-function (CBF) obstacle avoidance schemes ensure safety but fail to optimize tracking performance, potentially leading to decreased tracking accuracy or lag. Secondly, traditional smooth control schemes (such as PID and adaptive control) are based on asymptotic stability theory, resulting in slower convergence speeds than finite-time control. Furthermore, some schemes employ linear feedback, exhibiting weak robustness and anti-interference capabilities in complex environments such as airflow disturbances, making them prone to increased tracking errors or attitude instability. Moreover, existing schemes often suffer from a disconnect between the control objectives of the position outer loop and attitude inner loop. For example, using different types of controllers (e.g., asymptotic stability for the outer loop and finite-time for the inner loop) prevents the overall system from achieving unified finite-time convergence. The lack of a unified design framework integrating finite-time tracking and obstacle avoidance complicates control logic, reduces real-time performance, and may result in trajectory planning that doesn't adequately consider UAV dynamics, leading to trajectory deviations and safety hazards. Finally, the existing safety constraint design is not flexible enough, making it difficult to dynamically adjust the safety distance according to the drone's motion state and obstacle characteristics, which affects the reliability of obstacle avoidance. Summary of the Invention

[0005] This invention provides a method and system for trajectory tracking and obstacle avoidance control of a quadcopter drone, which ensures that the quadcopter drone accurately tracks the desired trajectory within the target time and avoids obstacles in real time, thereby improving the flight safety, robustness and anti-interference ability of the drone to adapt to flight missions in complex environments.

[0006] In a first aspect, the present invention provides a method for trajectory tracking and obstacle avoidance control of a quadcopter unmanned aerial vehicle, comprising:

[0007] Obtain the desired trajectory of the drone, as well as the center position and equivalent radius of the obstacle;

[0008] Determine the nominal position control quantity of the UAV based on the UAV's desired position trajectory;

[0009] Construct a control obstacle function based on the center position and equivalent radius of the obstacle;

[0010] The final position control quantity of the UAV is determined based on the control obstacle function and the nominal position control quantity;

[0011] The desired attitude of the UAV is determined based on the final position control parameters of the UAV.

[0012] The nominal control torque of the drone is determined based on the drone's desired attitude.

[0013] Determine the final control torque of the UAV based on the nominal control torque;

[0014] The PWM duty cycle of the drone is determined based on the final control torque to drive each motor to drive the propeller for trajectory tracking and obstacle avoidance.

[0015] Optionally, determining the nominal position control quantity of the UAV based on the desired position trajectory of the UAV includes:

[0016] The nominal position control quantity of the UAV is calculated using the following formula. :

[0017] ;

[0018] in, This refers to the nominal position control value along the x-axis of the UAV. This is the nominal position control value along the y-axis of the UAV; The z-axis nominal position control value of the UAV; T is the total thrust of the UAV; k represents the desired acceleration of the drone along the x-axis. px sig is the first control gain of the nominal position controller for the x-axis. a (‧) is a non-smooth function; sig a (‧)=sign|‧| a , sign(‧) is the sign function; a is a constant, taking a positive value; |‧| is the absolute value; α1 is the first control gain of the nominal position controller; x d k represents the desired position of the drone on the x-axis; x represents the actual position of the drone on the x-axis; k dx α1 is the second control gain of the nominal position controller for the x-axis; α2 is the second control gain of the nominal position controller. Let x be the desired velocity of the drone on the x-axis; This represents the actual speed of the drone on the x-axis. Let k be the desired acceleration of the drone on the y-axis; py The first control gain of the nominal position controller for the y-axis; y d k represents the desired position of the drone on the y-axis; y represents the actual position of the drone on the y-axis; k dy The second control gain of the nominal position controller for the y-axis; Let be the desired velocity of the drone on the y-axis; This represents the actual speed of the drone on the y-axis. k represents the desired acceleration of the UAV along the z-axis. pz The first control gain of the nominal position controller for the z-axis; z d k represents the desired position of the drone on the z-axis; z represents the actual position of the drone on the z-axis; k dz The second control gain of the nominal position controller for the z-axis; Let be the desired velocity of the drone on the z-axis; This represents the actual speed of the drone on the z-axis.

[0019] Optionally, constructing the control obstacle function based on the center position and equivalent radius of the obstacle includes:

[0020] Constructing control barrier functions The expression:

[0021] ;

[0022] Where χ represents the actual position of the UAV in the inertial coordinate system; d0 is the center position of the obstacle; d0 is the equivalent radius of the obstacle; δ is a preset constant, which takes a positive value; denoted as , where is the actual velocity of the UAV in the inertial coordinate system; T is the total thrust of the UAV.

[0023] Optionally, determining the final position control quantity of the UAV based on the control obstacle function and the nominal position control quantity includes:

[0024] Differentiate the control barrier function to obtain the differentiated control barrier function;

[0025] The final position control quantity of the UAV is calculated based on the differentiated control obstacle function and the following formula.

[0026] ;

[0027] in, This refers to the nominal position control quantity of the drone; Correct control parameters for the drone's position safety; The actual velocity of the UAV in the inertial coordinate system is given by ; T is the total thrust of the UAV. This represents the actual position of the UAV in the inertial coordinate system. The center position of the obstacle; Γ(‧) represents the safety correction activation judgment function; γ is a preset constant, taking a positive value; This is the function for controlling the barrier.

[0028] Optionally, determining the desired attitude of the UAV based on the final position control value of the UAV includes:

[0029] The desired attitude Ф of the UAV is calculated using the following formula. d :

[0030] ;

[0031] in, θ is the expected roll angle of the drone. dThe desired pitch angle for the drone; U is the desired yaw angle of the UAV; m is the total weight of the UAV; u x u represents the position control variable of the UAV on the x-axis. y u is the position control variable for the UAV on the y-axis. z denoted as the position control variable of the UAV on the z-axis; g is the gravitational acceleration of the environment in which the UAV is located.

[0032] Optionally, determining the nominal control torque of the UAV based on the desired attitude of the UAV includes:

[0033] The nominal control torque τ of the UAV is calculated using the following formula. norm :

[0034] ;

[0035] Where J is the diagonal inertia matrix of the UAV in the body coordinate system; J=diag(J1,J2,J3); J1 is the first attitude rotational inertia of the UAV in the body coordinate system; J2 is the second attitude rotational inertia of the UAV in the body coordinate system; J3 is the third attitude rotational inertia of the UAV in the body coordinate system; W d The transformation matrix; Let a be the desired angular acceleration of the UAV in the inertial coordinate system; p sig is the first control gain of the attitude controller. a (‧) is a non-smooth function; sig a (‧)=sign|‧| a , sign(‧) is the sign function; a is a constant, taking a positive value; |‧| is the absolute value; β1 is the second control gain of the attitude controller; Ф d Let be the desired attitude of the UAV; Ф be the actual angle of the UAV in the inertial coordinate system; a d β1 is the third control gain of the attitude controller; β2 is the fourth control gain of the attitude controller. Let ω1 be the desired angular velocity of the UAV in the inertial coordinate system; Ω be the three-axis angular velocities in the body coordinate system; Ω = (ω1, ω2, ω3) T ω1 is the pitch rate in the body coordinate system; ω2 is the roll rate in the body coordinate system; ω3 is the yaw rate in the body coordinate system. For matrix The derivative with respect to time; Ω * It is a partially symmetric matrix; θ is the expected roll angle of the drone. d The desired pitch angle for the drone.

[0036] Optionally, determining the final control torque of the UAV based on the nominal control torque includes:

[0037] The final control torque τ of the UAV is calculated using the following formula:

[0038] ;

[0039] in, This is an estimate of the lumped disturbance.

[0040] Secondly, the present invention provides a trajectory tracking and obstacle avoidance control system for a quadcopter unmanned aerial vehicle (UAV), comprising:

[0041] The acquisition module is used to acquire the desired position trajectory of the drone, as well as the center position and equivalent radius of the obstacles;

[0042] The first determining module is used to determine the nominal position control quantity of the UAV based on the UAV's desired position trajectory.

[0043] The second determining module is used to construct a control obstacle function based on the center position and equivalent radius of the obstacle;

[0044] The third determination module is used to determine the final position control quantity of the UAV based on the control obstacle function and the nominal position control quantity;

[0045] The fourth determining module is used to determine the desired attitude of the UAV based on the final position control values ​​of the UAV.

[0046] The fifth determining module is used to determine the nominal control torque of the UAV based on the UAV's desired attitude;

[0047] The sixth determining module is used to determine the final control torque of the UAV based on the nominal control torque;

[0048] The seventh determining module is used to determine the PWM duty cycle of the UAV based on the final control torque, so as to drive each motor to drive the propeller for trajectory tracking and obstacle avoidance.

[0049] Thirdly, the present invention provides a computer device, including a processor and a memory; wherein, when the processor executes a computer program stored in the memory, it implements the steps of the quadcopter UAV trajectory tracking and obstacle avoidance control method described in the first aspect.

[0050] Fourthly, the present invention provides a computer-readable storage medium for storing a computer program; when the computer program is executed by a processor, it implements the steps of the quadcopter unmanned aerial vehicle trajectory tracking and obstacle avoidance control method described in the first aspect.

[0051] This invention provides a method and system for trajectory tracking and obstacle avoidance control of a quadrotor unmanned aerial vehicle (UAV). The method systematically solves the core contradiction of balancing performance and safety in complex environments by constructing an integrated control architecture of "finite-time tracking + CBF obstacle avoidance." The benefits are significant: First, it achieves dynamic coordination between tracking and obstacle avoidance. Through CBF safety correction based on quadratic programming, the trajectory is corrected in real time with minimal performance loss while ensuring absolute obstacle avoidance, overcoming the "either / or" defects of traditional solutions. This allows for rapid and smooth recovery of accurate tracking of the desired trajectory after obstacle avoidance. Second, based on homogeneous system theory, a finite-time controller is designed for the position and attitude loops, ensuring dynamic performance matching between the inner and outer loops. This enables the entire system to achieve rapid convergence within a finite time, with tracking speed and accuracy significantly superior to traditional asymptotically stable methods. Third, by introducing a passive anti-interference mechanism with non-smooth feedback and an active anti-interference mechanism with sliding mode observer feedforward compensation, a dual robustness enhancement design is formed, enabling the UAV to maintain excellent tracking accuracy and flight stability even in complex environments such as airflow disturbances. Finally, this scheme transforms the obstacle avoidance problem into an efficient online optimization with analytical solutions. The control logic is simple, does not rely on complex pre-planning modules, has a low computational burden, ensures the real-time performance of the system, and allows for flexible adjustment of safety strategies through parameters. Attached Figure Description

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

[0053] Figure 1 A flowchart illustrating a method for trajectory tracking and obstacle avoidance control of a quadcopter unmanned aerial vehicle (UAV) according to an embodiment of the present invention;

[0054] Figure 2 This is a schematic diagram of a quadcopter unmanned aerial vehicle (UAV) structure provided in an embodiment of the present invention;

[0055] Figure 3 This is a structural block diagram of the quadcopter UAV control scheme provided in an embodiment of the present invention;

[0056] Figure 4 The trajectory curve of a quadcopter UAV under no lumped disturbance provided in the embodiments of the present invention;

[0057] Figure 5 The trajectory curve of a quadcopter UAV under lumped disturbance conditions provided in the embodiments of the present invention;

[0058] Figure 6This is a position error curve of a quadcopter UAV under lumped disturbance conditions provided in an embodiment of the present invention;

[0059] Figure 7 This is an attitude error curve of a quadcopter UAV under lumped disturbance conditions provided in an embodiment of the present invention;

[0060] Figure 8 The observer lumped disturbance estimation curve of a quadcopter UAV under lumped disturbance conditions provided in the embodiments of the present invention;

[0061] Figure 9 This is a schematic diagram of a quadcopter unmanned aerial vehicle (UAV) trajectory tracking and obstacle avoidance control system provided in an embodiment of the present invention. Detailed Implementation

[0062] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. 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 are within the scope of protection of the present invention.

[0063] Example 1

[0064] like Figure 1 and Figure 3 As shown, this embodiment of the invention provides a method for trajectory tracking and obstacle avoidance control of a quadcopter unmanned aerial vehicle (UAV), including:

[0065] Step 101: Obtain the desired location trajectory of the drone and the center position and equivalent radius of the obstacle.

[0066] In this step, such as Figure 2 As shown, an inertial coordinate system is constructed with the coordinate origin and coordinate axes fixed to the ground. This is used to describe the three-axis position and three-axis attitude of a quadcopter drone; a body coordinate system is constructed using the coordinate origin and coordinate axes fixed to the drone's fuselage. This describes the angular velocity of the three-axis airframe; four propellers are driven independently, each generating thrust F. i With counter torque Q i , i=1,2,3,4.

[0067] Constructing the UAV position control model (1) and attitude control model (2):

[0068] (1)

[0069] (2)

[0070] Where χ=(x,y,z) T This indicates the actual position of the quadcopter drone in its inertial frame. This represents the actual Euler angle attitude of the quadcopter UAV in its inertial frame. For Rao x n The roll angle of the shaft (Roll); θ is the y-axis. n Pitch angle of the axis; For Rao z n Yaw is the yaw angle of the axis; m is the weight of the quadcopter drone; g is the gravitational acceleration of the environment in which the drone is located; T represents the total lift generated by the propeller of the quadcopter drone; Ω=(ω1,ω2,ω3) T ω1 represents the pitch angular velocity in the body coordinate system; ω2 represents the roll angular velocity in the body coordinate system; ω3 represents the yaw angular velocity in the body coordinate system; J=diag(J1,J2,J3) is the diagonal inertia matrix of the UAV in the body coordinate system; J1 is the moment of inertia of the UAV in its first attitude in the body coordinate system; J2 is the moment of inertia of the UAV in its second attitude in the body coordinate system; J3 is the moment of inertia of the UAV in its third attitude in the body coordinate system. This represents the torque generated by the rotation of the motor; This represents the unknown time-varying lumped disturbance experienced by the UAV, such as model parameter perturbations (internal parameter perturbations), wind disturbances (external disturbances), etc. (i.e., lumped disturbance = internal parameter perturbation + external disturbance); W is the transformation matrix, through which the angular velocity is transformed from the body coordinate system to the inertial coordinate system; Ω * It is a partially symmetric matrix;

[0071] .

[0072] .

[0073] For the position control model design of a quadcopter UAV, a nominal finite-time position controller was first designed to enable the quadcopter UAV to track the desired position from its actual position within a finite time. The stability of the position error system was then analyzed using a Lyapunov function. Next, considering the obstacle avoidance requirements of the UAV, a constraint-based optimization (CBF) was introduced to transform the obstacle avoidance problem into a constrained optimization problem, thus modifying the nominal controller to achieve finite-time tracking of the reference trajectory while ensuring the UAV's safety.

[0074] For the position control model (1), define the virtual control quantity. Defined as

[0075] (3)

[0076] Position control model (1) can be rewritten as

[0077] (4)

[0078] Let the desired position of the quadcopter UAV be... x d Let y be the desired position of the drone on the x-axis; d Let z be the desired position of the drone on the y-axis; d Let U be the desired position of the UAV on the z-axis; the position error can be defined as... ;e x e represents the positional error of the UAV on the x-axis. y e represents the position error of the UAV on the y-axis. z This represents the positional error of the UAV on the z-axis.

[0079] Step 102: Determine the nominal position control quantity of the UAV based on the UAV's desired position trajectory.

[0080] For example, the nominal position control quantity of the UAV is calculated according to the following formula. :

[0081] (5)

[0082] in, This refers to the nominal position control value along the x-axis of the UAV. This is the nominal position control value along the y-axis of the UAV; The z-axis nominal position control value of the UAV; T is the total thrust of the UAV; k represents the desired acceleration of the drone along the x-axis. px sig is the first control gain of the nominal position controller for the x-axis. a (‧) is a non-smooth function; sig a (‧)=sign|‧| a , sign(‧) is the sign function; a is a constant, taking a positive value; |‧| is the absolute value; α1 is the first control gain of the nominal position controller, i.e., the fractional power of the non-smooth function; x d k represents the desired position of the drone on the x-axis; x represents the actual position of the drone on the x-axis; k dx α1 is the second control gain of the nominal position controller on the x-axis; α2 is the second control gain of the nominal position controller, that is, the fractional power of the non-smooth function, and its value can be α2=2α1 / (1+α1); Let x be the desired velocity of the drone on the x-axis; This represents the actual speed of the drone on the x-axis. Let k be the desired acceleration of the drone on the y-axis; py The first control gain of the nominal position controller for the y-axis; y dk represents the desired position of the drone on the y-axis; y represents the actual position of the drone on the y-axis; k dy The second control gain of the nominal position controller for the y-axis; Let be the desired velocity of the drone on the y-axis; This represents the actual speed of the drone on the y-axis. k represents the desired acceleration of the UAV along the z-axis. pz The first control gain of the nominal position controller for the z-axis; z d k represents the desired position of the drone on the z-axis; z represents the actual position of the drone on the z-axis; k dz The second control gain of the nominal position controller for the z-axis; Let be the desired velocity of the drone on the z-axis; This represents the actual speed of the drone on the z-axis.

[0083] 0 < α1 < 1; k px >0; k dx >0; k py >0; k dy >0; k pz >0; k dz >0; This allows the quadcopter drone to track the desired trajectory within a limited time.

[0084] Taking the x-axis as an example, the same applies to the y-axis and z-axis.

[0085] Define error Combining equation (4), the x-axis error can be written as

[0086] (6)

[0087] The Lyapunov candidate function of equation (6) can be constructed as follows:

[0088] (7)

[0089] The derivative of V1 with respect to equation (6) satisfies

[0090] (8)

[0091] Based on Barbalat's lemma and the extended Barbalat's lemma, it can be concluded that the origin of equation (6) is globally asymptotically stable, that is, when Sometimes, .

[0092] Secondly, consider the homogeneity of equation (6). By definition, equation (6) is about the extension. A homogeneous system, whose homogeneity is .

[0093] According to the homogeneity finite-time lemma, the origin of equation (6) is a global finite-time equilibrium point, that is, under the action of equation (5), the UAV can track the desired trajectory in a finite time.

[0094] Step 103: Construct a control obstacle function based on the center position and equivalent radius of the obstacle.

[0095] To facilitate the design of the obstacle avoidance controller, it is assumed that the UAV can perceive the precise location of obstacles in real time, and that its initial state is not within the vicinity of the obstacles. The space occupied by the obstacles is considered a spherical region, and the set of occupied spaces is defined as...

[0096] (9)

[0097] in, Indicates the spatial location occupied by the obstacle. d0 represents the center position of the obstacle, d0 > 0, and d0 represents the maximum radius extending outward from the center of the obstacle.

[0098] According to equation (9) above, the safe set can be expressed in terms of Euclidean distance.

[0099] (10)

[0100] Based on equation (4), construct the control barrier function. The expression:

[0101] (11)

[0102] Where χ represents the actual position of the UAV in the inertial coordinate system; d0 is the center position of the obstacle; d0 is the equivalent radius of the obstacle; δ is an adjustable preset constant, which takes a positive value. By adjusting this parameter, the drone can maintain a sufficient safe distance from the obstacle. denoted as , where is the actual velocity of the UAV in the inertial coordinate system; T is the total thrust of the UAV.

[0103] Based on the control barrier function A safe set can be defined as Define the extended K-class functions as follows: , where γ>0 is the design constant to be determined.

[0104] Next, we can construct a set. Then, any element belonging to set K safe The continuous controller u can ensure the system state Keep it within set C1. It is obvious that... Therefore, if the initial value of the system is within C1, then the system state χ will always remain within the safe set C0, meaning the drone will not collide with obstacles.

[0105] Step 104: Determine the final position control quantity of the UAV based on the control obstacle function and the nominal position control quantity.

[0106] In this step, the derivative of the control barrier function is obtained:

[0107] (12)

[0108] Based on the nominal position control variable and the differentiated control obstacle function, construct the expression for the UAV quadratic route planning correction model:

[0109] (13)

[0110] in, The optimization cost function designed for the second correction; R 3 Represents the three-dimensional real number space; This refers to the nominal position control quantity of the drone; The actual velocity of the UAV in the inertial coordinate system is given by ; T is the total thrust of the UAV. This represents the actual position of the UAV in the inertial coordinate system.

[0111] Considering the obstacle avoidance task, based on equations (5) and (13), that is, according to the differentiated control obstacle function and the following formula, the final position control quantity of the UAV is calculated.

[0112] (14)

[0113] in, This refers to the nominal position control quantity of the drone; Correct control parameters for the drone's position safety; The actual velocity of the UAV in the inertial coordinate system is given by ; T is the total thrust of the UAV. The center position of the obstacle; Γ(‧) represents the safety correction activation judgment function; γ is a preset constant, taking a positive value; This is the function for controlling the barrier.

[0114] This allows the quadcopter drone to track the desired trajectory as closely as possible while completing obstacle avoidance tasks.

[0115] Based on the KKT conditions, consider the Lagrangian function.

[0116] (15)

[0117] in, It is a constant.

[0118] Equation (14) and Substituting the above Lagrange function (15), we can obtain

[0119] (16)

[0120] Using the KKT conditions, we have

[0121] (17)

[0122] Case 1: If and Then, by transforming equation (17), we can obtain .

[0123] Right now , and thus

[0124] (18)

[0125] Case 2: If λ=0 and From the transformation equation (17), we can see that... and .

[0126] In summary, under the action of equation (14), the UAV can track the desired location trajectory as much as possible while ensuring obstacle avoidance.

[0127] Step 105: Determine the desired attitude of the UAV based on the final position control values ​​of the UAV.

[0128] Before designing an attitude controller, it is first necessary to determine the attitude required by the aircraft to track the desired trajectory, i.e., the desired attitude information. The desired attitude information is defined as follows: Virtual control inputs were used in the design of the position controller. Therefore, according to the inverse operation of equation (3), the required attitude information is obtained from the virtual control input, that is, the desired attitude Ф of the UAV is calculated according to the following formula. d :

[0129] (19)

[0130] in, θ is the expected roll angle of the drone. d The desired pitch angle for the drone; U is the desired yaw angle of the UAV; m is the total weight of the UAV; u x u represents the position control variable of the UAV on the x-axis. yu is the position control variable for the UAV on the y-axis. z denoted as the position control variable of the UAV on the z-axis; g is the gravitational acceleration of the environment in which the UAV is located.

[0131] From the above transformation relationship (19), it can be concluded that the expected yaw angle is a free variable that can be set freely.

[0132] Step 106: Determine the nominal control torque of the UAV based on the desired attitude of the UAV.

[0133] For the UAV attitude system (2), without considering lumped disturbances (i.e., d=0), the nominal finite-time attitude controller can be designed as follows, that is, the nominal control torque τ of the UAV is calculated according to the following formula. norm :

[0134] (20)

[0135] Where J is the diagonal inertia matrix of the UAV in the body coordinate system; J=diag(J1,J2,J3); J1 is the first attitude rotational inertia of the UAV in the body coordinate system; J2 is the second attitude rotational inertia of the UAV in the body coordinate system; J3 is the third attitude rotational inertia of the UAV in the body coordinate system; W d The transformation matrix; Let a be the desired angular acceleration of the UAV in the inertial coordinate system; p sig is the first control gain of the attitude controller. a (‧) is a non-smooth function; sig a (‧)=sign|‧| a , sign(‧) is the sign function; a is a constant, taking a positive value; |‧| is the absolute value; β1 is the second control gain of the attitude controller; Ф d Let be the desired attitude of the UAV; Ф be the actual angle of the UAV in the inertial coordinate system; a d β1 is the third control gain of the attitude controller; β2 is the fourth control gain of the attitude controller. Let ω1 be the desired angular velocity of the UAV in the inertial coordinate system; Ω be the three-axis angular velocities in the body coordinate system; Ω = (ω1, ω2, ω3) T ω1 is the pitch rate in the body coordinate system; ω2 is the roll rate in the body coordinate system; ω3 is the yaw rate in the body coordinate system. For matrix The derivative with respect to time; Ω * It is a partially symmetric matrix; θ is the expected roll angle of the drone. d The desired pitch angle for the drone.

[0136] , , , Therefore, the drone's attitude can track the desired attitude within a finite time; that is, within a finite time, there is... .

[0137] Define the attitude tracking error of the UAV as Therefore, based on the attitude system equation (2) and the nominal attitude controller equation (20), the UAV attitude error system can be expressed as follows:

[0138] (twenty one)

[0139] Similar to the proof process of the nominal position finite-time controller, it can be concluded that under the action of the nominal finite-time attitude controller (21), the UAV can track the desired attitude within a finite time.

[0140] Step 107: Determine the final control torque of the UAV based on the nominal control torque.

[0141] To eliminate the impact of lumped disturbances on the UAV, a feedforward compensation strategy will be adopted in the control channel to compensate for the lumped disturbances. Next, the assumptions regarding the lumped disturbances are given, and a corresponding observer is designed to conduct fast and accurate observations of them.

[0142] Assume the lumped disturbance is continuously differentiable and its derivative is bounded. Specifically, there exist known positive constants L1 and L2, and...

[0143] (twenty two)

[0144] in, For the lumped disturbance in the pitch angle loop of the UAV; Let be the time derivative of the lumped disturbance on the pitch loop of the UAV; For the lumped disturbance on the roll angle loop of the UAV; Let the time derivative of the lumped disturbance on the roll angle loop of the UAV; For the lumped disturbance on the yaw angle loop of the UAV; Let be the time derivative of the lumped disturbance on the yaw angle loop of the UAV.

[0145] For the attitude system of a quadcopter UAV with lumped disturbances (2), under the premise of satisfying the above assumptions, a sliding mode observer can be designed as follows.

[0146] (twenty three)

[0147] in, and These represent the angular velocity and lumped disturbance in the UAV's body coordinate system, respectively. The observed values, and It is the observer gain, which satisfies the condition.

[0148] (twenty four)

[0149] This allows for accurate observation of lumped interference within a limited timeframe.

[0150] Based on the above sliding mode observer (23) and nominal finite-time attitude controller (20), further composite design can be carried out to obtain the following adaptive composite finite-time attitude controller.

[0151] For the attitude system of a quadrotor UAV with lumped disturbances (2), an adaptive composite finite-time attitude controller can be designed under the action of a sliding mode observer (23) as follows.

[0152] (25)

[0153] in, , , , Therefore, the drone can achieve tracking and control of the desired attitude within a limited time under disturbance conditions.

[0154] The sliding mode observer (23) can accurately and quickly observe unknown time-varying lumped disturbances within a finite time, i.e., there exists a finite time T1 for all , making .at this time( The closed-loop system under the action of the adaptive composite finite-time attitude controller (25) is equivalent to the closed-loop system under the action of the nominal finite-time attitude controller (20) (21). Therefore, the proof process is consistent with that of the nominal finite-time attitude controller (or the nominal finite-time position controller).

[0155] Step 108: Determine the PWM duty cycle of the drone based on the final control torque to drive each motor to drive the propeller for trajectory tracking and obstacle avoidance.

[0156] To make the solution of the present invention clearer, specific examples are further disclosed in the embodiments of the present invention.

[0157] The results were verified through MATLAB numerical simulation, as follows:

[0158] Expected trajectory: .

[0159] Obstacle parameters: , .

[0160] Controller parameters:

[0161] Adaptive Finite-Time Tracking and Obstacle Avoidance Control (CBF-AFC):

[0162] , , , , , , .

[0163] Finite-time tracking control (CBF-FC):

[0164] , , , , .

[0165] CBF+ Obstacle Avoidance Control (CBF-PD):

[0166] , , , .

[0167] Initial state: , .

[0168] System parameters: , , .

[0169] Comparison of obstacle avoidance objectives (under undisturbed conditions): Finite-time tracking and obstacle avoidance control (CBF-FC), simple finite-time control (FC), and simple proportional-derivative control (PD), such as... Figure 4 As shown, the method proposed in this embodiment has a faster response speed in the initial stage and can complete obstacle avoidance actions when facing obstacles.

[0170] To achieve the dual objectives of "tracking and obstacle avoidance" while balancing performance and safety, the following comparison objects are used: Adaptive Finite-Time Tracking and Obstacle Avoidance Control (CBF-AFC), Simple Finite-Time Tracking Control (CBF-FC), and Simple CBF Obstacle Avoidance Control (CBF-PD).

[0171] like Figure 5 As shown, the method proposed in this embodiment accomplishes the obstacle avoidance task and can track the reference trajectory both before and after obstacle avoidance. Figure 6 and Figure 7As shown, compared to conventional proportional-derivative control, finite-time control has better passive anti-interference capability. Finite-time control with an observer (i.e., the method proposed in this invention), due to the combination of active anti-interference technology, has even better anti-interference capability than the former two. For example... Figure 8 As shown, the observer can achieve fast and accurate estimation of lumped disturbances.

[0172] In summary, this embodiment provides a method for trajectory tracking and obstacle avoidance control of a quadrotor UAV. It uses CBF to describe obstacles and dynamically adapts to the motion state of the quadrotor UAV through quadruple programming. With the goal of "minimizing tracking performance loss", it ensures accuracy as much as possible while avoiding obstacles. It integrates homogeneous system theory and CBF and achieves the dual-objective optimization of "performance-safety" for the outer ring of the quadrotor position through "nominal finite time controller + quadruple programming safety correction". This solves the contradiction of "either / or" in most existing technologies.

[0173] Both the position controller and the attitude controller adopt a homogeneous design to ensure the consistency of the inner and outer loop finite-time control. The position outer loop and attitude inner loop controllers are designed in a unified manner based on the homogeneous system theory to achieve finite-time convergence of the entire system. The convergence speed is faster than that of traditional asymptotic stability schemes.

[0174] The non-smoothness of non-smooth functions, i.e., their high gain at the equilibrium point, can passively suppress the impact of disturbances on error convergence. By utilizing a sliding mode observer to quickly and accurately observe lumped disturbances and then performing feedforward compensation in the controller, active disturbance compensation is achieved, reducing their impact on control performance. The non-Lipschitz continuity of non-smooth functions enhances the system's passive anti-disturbance capability, while the observation of lumped disturbances via a sliding mode observer and feedforward compensation in the controller enhances the system's active anti-disturbance capability. These two aspects improve the overall robustness of the system, maintaining tracking accuracy and obstacle avoidance safety even in complex environments such as airflow disturbances.

[0175] Example 2

[0176] Based on the same inventive concept as Embodiment 1, this embodiment provides a quadcopter drone trajectory tracking and obstacle avoidance control system. Since the principle of this system in solving the problem is similar to that of the quadcopter drone trajectory tracking and obstacle avoidance control method described in Embodiment 1, the implementation of this system can refer to the implementation of the quadcopter drone trajectory tracking and obstacle avoidance control method.

[0177] like Figure 9 As shown, this embodiment provides a trajectory tracking and obstacle avoidance control system for a quadcopter unmanned aerial vehicle (UAV), including:

[0178] The acquisition module 10 is used to acquire the desired position trajectory of the UAV and the center position and equivalent radius of the obstacle.

[0179] The first determining module 20 is used to determine the nominal position control quantity of the UAV based on the UAV's desired position trajectory.

[0180] The second determining module 30 is used to construct a control obstacle function based on the center position and equivalent radius of the obstacle.

[0181] The third determining module 40 is used to determine the final position control quantity of the UAV based on the control obstacle function and the nominal position control quantity.

[0182] The fourth determining module 50 is used to determine the desired attitude of the UAV based on the final position control quantity of the UAV.

[0183] The fifth determining module 60 is used to determine the nominal control torque of the UAV based on the UAV's desired attitude.

[0184] The sixth determining module 70 is used to determine the final control torque of the UAV based on the nominal control torque.

[0185] The seventh determining module 80 is used to determine the PWM duty cycle of the UAV based on the final control torque, so as to drive each motor to drive the propeller for trajectory tracking and obstacle avoidance.

[0186] For more detailed information on the working process of each of the above modules, please refer to the relevant content disclosed in Example 1, which will not be repeated here.

[0187] Example 3

[0188] This embodiment provides a computer device, including a processor and a memory; wherein, when the processor executes the computer program stored in the memory, it implements the steps of the quadcopter UAV trajectory tracking and obstacle avoidance control method described in Embodiment 1.

[0189] For a more detailed explanation of the above method, please refer to the relevant content disclosed in Example 1, which will not be repeated here.

[0190] Example 4

[0191] This embodiment provides a computer-readable storage medium for storing a computer program; when the computer program is executed by a processor, it implements the steps of the quadcopter UAV trajectory tracking and obstacle avoidance control method described in Embodiment 1.

[0192] For a more detailed explanation of the above method, please refer to the relevant content disclosed in Example 1, which will not be repeated here.

[0193] Example 5

[0194] This embodiment provides a computer program product, including computer-executable instructions or a computer program. When the computer-executable instructions or the computer program are executed by a processor, they implement the steps of the quadcopter UAV trajectory tracking and obstacle avoidance control method described in Embodiment 1.

[0195] For a more detailed explanation of the above method, please refer to the relevant content disclosed in Example 1, which will not be repeated here.

[0196] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems, devices, storage media, and computer program products disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple, and relevant parts can be referred to the method section.

[0197] Those skilled in the art will clearly understand that the techniques in the embodiments of the present invention can be implemented using software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or certain parts of the embodiments of the present invention.

[0198] In some embodiments, computer-executable instructions may take the form of programs, software, software modules, scripts, or code, written in any form of programming language (including compiled or interpreted languages, or declarative or procedural languages), and may be deployed in any form, including as stand-alone programs or as modules, components, subroutines, or other units suitable for use in a computing environment.

[0199] As an example, computer-executable instructions may, but do not necessarily, correspond to files in a file system. They may be stored as part of a file that holds other programs or data, for example, in one or more scripts in a Hyper Text Markup Language (HTML) document, in a single file dedicated to the program in question, or in multiple co-located files (e.g., files that store one or more modules, subroutines, or code sections).

[0200] As an example, computer-executable instructions can be deployed to execute on a single electronic device, or on multiple electronic devices located at one location, or on multiple electronic devices distributed across multiple locations and interconnected via a communication network.

[0201] The present invention has been described in detail above with reference to specific embodiments and exemplary examples; however, these descriptions should not be construed as limiting the present invention. Those skilled in the art will understand that various equivalent substitutions, modifications, or improvements can be made to the technical solutions and embodiments of the present invention without departing from the spirit and scope of the invention, and all such modifications and improvements fall within the scope of the present invention. The scope of protection of the present invention is defined by the appended claims.

Claims

1. A method for trajectory tracking and obstacle avoidance control of a quadcopter unmanned aerial vehicle (UAV), characterized in that, include: Obtain the desired trajectory of the drone, as well as the center position and equivalent radius of the obstacle; Determine the nominal position control quantity of the UAV based on the UAV's desired position trajectory; Construct a control obstacle function based on the center position and equivalent radius of the obstacle; The final position control quantity of the UAV is determined based on the control obstacle function and the nominal position control quantity; The desired attitude of the UAV is determined based on the final position control parameters of the UAV. The nominal control torque of the drone is determined based on the drone's desired attitude. Determine the final control torque of the UAV based on the nominal control torque; The PWM duty cycle of the drone is determined based on the final control torque to drive each motor to drive the propeller for trajectory tracking and obstacle avoidance.

2. The quadcopter UAV trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, The determination of the nominal position control quantity of the UAV based on the UAV's desired position trajectory includes: The nominal position control quantity of the UAV is calculated using the following formula. : ； in, This refers to the nominal position control value along the x-axis of the UAV. This is the nominal position control value along the y-axis of the UAV; The z-axis nominal position control value of the UAV; T is the total thrust of the UAV; k represents the desired acceleration of the drone along the x-axis. px sig is the first control gain of the nominal position controller for the x-axis. a (‧) is a non-smooth function; sig a (‧)=sign|‧| a , sign(‧) is the sign function; a is a constant, taking a positive value; |‧| is the absolute value; α1 is the first control gain of the nominal position controller; x d k represents the desired position of the drone on the x-axis; x represents the actual position of the drone on the x-axis; k dx α1 is the second control gain of the nominal position controller for the x-axis; α2 is the second control gain of the nominal position controller. Let x be the desired velocity of the drone on the x-axis; This represents the actual speed of the drone on the x-axis. Let k be the desired acceleration of the drone on the y-axis; py The first control gain of the nominal position controller for the y-axis; y d k represents the desired position of the drone on the y-axis; y represents the actual position of the drone on the y-axis; k dy The second control gain of the nominal position controller for the y-axis; Let be the desired velocity of the drone on the y-axis; This represents the actual speed of the drone on the y-axis. k represents the desired acceleration of the UAV along the z-axis. pz The first control gain of the nominal position controller for the z-axis; z d k represents the desired position of the drone on the z-axis; z represents the actual position of the drone on the z-axis; k dz The second control gain of the nominal position controller for the z-axis; Let be the desired velocity of the drone on the z-axis; This represents the actual speed of the drone on the z-axis.

3. The quadcopter UAV trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, The construction of the control obstacle function based on the center position and equivalent radius of the obstacle includes: Constructing control barrier functions The expression: ； Where χ represents the actual position of the UAV in the inertial coordinate system; d0 is the center position of the obstacle; d0 is the equivalent radius of the obstacle; δ is a preset constant, which takes a positive value; denoted as , where is the actual velocity of the UAV in the inertial coordinate system; T is the total thrust of the UAV.

4. The quadcopter UAV trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, The process of determining the final position control quantity of the UAV based on the control obstacle function and the nominal position control quantity includes: Differentiate the control barrier function to obtain the differentiated control barrier function; The final position control quantity of the UAV is calculated based on the differentiated control obstacle function and the following formula. ； in, This refers to the nominal position control quantity of the drone; Correct control parameters for the drone's position safety; The actual velocity of the UAV in the inertial coordinate system is given by ; T is the total thrust of the UAV. This represents the actual position of the UAV in the inertial coordinate system. The center position of the obstacle; Γ(‧) represents the safety correction activation judgment function; γ is a preset constant, taking a positive value; This is the function for controlling the barrier.

5. The quadcopter UAV trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, Determining the desired attitude of the UAV based on its final position control parameters includes: The desired attitude Ф of the UAV is calculated using the following formula. d : ； in, θ is the expected roll angle of the drone. d The desired pitch angle for the drone; U is the desired yaw angle of the UAV; m is the total weight of the UAV; u x u represents the position control variable of the UAV on the x-axis. y u is the position control variable for the UAV on the y-axis. z denoted as the position control variable of the UAV on the z-axis; g is the gravitational acceleration of the environment in which the UAV is located.

6. The quadcopter UAV trajectory tracking and obstacle avoidance control method according to claim 1, characterized in that, Determining the nominal control torque of the UAV based on its desired attitude includes: The nominal control torque τ of the UAV is calculated using the following formula. norm : ； Where J is the diagonal inertia matrix of the UAV in the body coordinate system; J=diag(J1,J2,J3); J1 is the first attitude rotational inertia of the UAV in the body coordinate system; J2 is the second attitude rotational inertia of the UAV in the body coordinate system; J3 is the third attitude rotational inertia of the UAV in the body coordinate system; W d The transformation matrix; Let a be the desired angular acceleration of the UAV in the inertial coordinate system; p sig is the first control gain of the attitude controller. a (‧) is a non-smooth function; sig a (‧)=sign|‧| a , sign(‧) is the sign function; a is a constant, taking a positive value; |‧| is the absolute value; β1 is the second control gain of the attitude controller; Ф d Let be the desired attitude of the UAV; Ф be the actual angle of the UAV in the inertial coordinate system; a d β1 is the third control gain of the attitude controller; β2 is the fourth control gain of the attitude controller. Let ω1 be the desired angular velocity of the UAV in the inertial coordinate system; Ω be the three-axis angular velocities in the body coordinate system; Ω = (ω1, ω2, ω3) T ω1 is the pitch rate in the body coordinate system; ω2 is the roll rate in the body coordinate system; ω3 is the yaw rate in the body coordinate system. For matrix The derivative with respect to time; Ω * It is a partially symmetric matrix; θ is the expected roll angle of the drone. d The desired pitch angle for the drone.

7. The quadcopter UAV trajectory tracking and obstacle avoidance control method according to claim 6, characterized in that, The determination of the final control torque of the UAV based on the nominal control torque includes: The final control torque τ of the UAV is calculated using the following formula: ； in, This is an estimate of the lumped disturbance.

8. A trajectory tracking and obstacle avoidance control system for a quadcopter unmanned aerial vehicle (UAV), characterized in that, include: The acquisition module is used to acquire the desired position trajectory of the drone, as well as the center position and equivalent radius of the obstacles; The first determining module is used to determine the nominal position control quantity of the UAV based on the UAV's desired position trajectory. The second determining module is used to construct a control obstacle function based on the center position and equivalent radius of the obstacle; The third determination module is used to determine the final position control quantity of the UAV based on the control obstacle function and the nominal position control quantity; The fourth determining module is used to determine the desired attitude of the UAV based on the final position control values ​​of the UAV. The fifth determining module is used to determine the nominal control torque of the UAV based on the UAV's desired attitude; The sixth determining module is used to determine the final control torque of the UAV based on the nominal control torque; The seventh determining module is used to determine the PWM duty cycle of the UAV based on the final control torque, so as to drive each motor to drive the propeller for trajectory tracking and obstacle avoidance.

9. A computer device, characterized in that, It includes a processor and a memory; wherein, when the processor executes the computer program stored in the memory, it implements the steps of the quadcopter UAV trajectory tracking and obstacle avoidance control method according to any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, Used to store computer programs; when the computer programs are executed by a processor, they implement the steps of the quadcopter UAV trajectory tracking and obstacle avoidance control method according to any one of claims 1-7.

Images