Safety obstacle avoidance tracking control method and system of unmanned aerial vehicle in complex disturbance environment

Abstract

This invention discloses a safe obstacle avoidance and tracking control method for unmanned aerial vehicles (UAVs) under complex disturbance environments, belonging to the field of computer application technology. It includes: designing a high-order sliding mode observer to estimate disturbances to address external interference problems; constructing a control barrier function (CBF) based on the velocity barrier method to ensure collision avoidance for the UAV in obstacle environments; embedding the CBF into an optimal control framework to synergistically optimize trajectory tracking performance and obstacle avoidance safety; using a single-network ADP algorithm to solve the HJB equations, generating the optimal position and attitude controllers in real time, and rigorously proving the consistent eventual boundedness (UUB) of position and attitude tracking errors. This invention also provides a safe obstacle avoidance and tracking control system for UAVs under complex disturbance environments. This invention effectively solves the problem of safe obstacle avoidance and tracking control for UAVs under complex disturbance environments.

Description

Technical Field

[0001] This invention relates to the field of computer application technology, and in particular to a method and system for safe obstacle avoidance and tracking control of unmanned aerial vehicles (UAVs) in complex disturbance environments. Background Technology

[0002] Quadcopter unmanned aerial vehicles (QUAVs) have been widely used in military reconnaissance, urban inspection, and agricultural spraying due to their superior maneuverability and vertical takeoff and landing capabilities. However, the system has strong nonlinear dynamic characteristics and faces challenges from external uncertainties (such as airflow interference) and complex obstacle environments (such as buildings and trees) in actual operation. This makes it difficult for traditional control methods (such as PID control and robust control) to simultaneously ensure trajectory tracking accuracy and obstacle avoidance safety.

[0003] Adaptive dynamic programming (ADP), as a data-driven optimization method for solving the Hamilton-Jacobi-Bellman (HJB) equations, provides a new approach for the approximate optimal control of nonlinear systems. However, existing research has the following shortcomings: (1) the handling of obstacle constraints often adopts a path planning and control separation approach, which makes it difficult to guarantee the unity of real-time performance and safety; (2) the compensation accuracy for external disturbances is insufficient, affecting control robustness; (3) the synergistic integration of online learning mechanisms and obstacle avoidance and disturbance compensation is not yet perfect. Therefore, constructing a unified control framework that integrates the control barrier function (CBF), sliding mode disturbance observer, and neural network has become the key to solving the problem of safe tracking control of UAVs in complex environments. Summary of the Invention

[0004] To address the aforementioned issues, this invention aims to propose a safe obstacle avoidance and tracking control method and system for unmanned aerial vehicles (UAVs) in complex disturbance environments. By integrating high-order sliding mode disturbance estimation, CBF obstacle avoidance mechanism, and ADP optimal control, it effectively solves the safe tracking problem of quadcopter UAVs in complex disturbance and obstacle environments, providing technical support for the reliable operation of UAVs in real-world scenarios.

[0005] To achieve the above objectives, the technical solution of the present invention is implemented as follows:

[0006] A method for safe obstacle avoidance and tracking control of a UAV in a complex disturbance environment includes the following steps:

[0007] Step S1: Establish a mathematical model of the quadcopter UAV: ​​Construct a nonlinear dynamic model in a six-degree-of-freedom space, which is divided into a position subsystem and an attitude subsystem. The position subsystem describes the position change of the UAV in the inertial coordinate system and is affected by the total thrust, gravity and external disturbances. The attitude subsystem describes the change of Euler angles and is affected by the control torque, Coriolis force and external disturbances.

[0008] Step S2: Design a high-order sliding mode observer to estimate external disturbances: For external disturbances of the position and attitude subsystem, a third-order sliding mode observer is designed to achieve real-time estimation; by constructing dynamic terms and sliding surfaces related to the system state, the disturbance estimation error is made to converge quickly, providing a basis for subsequent disturbance compensation.

[0009] Step S3: Calculate the obstacle avoidance penalty term: Design the control barrier function CBF based on the velocity obstacle method. By defining the relative position and velocity relationship between the UAV and the obstacle, construct the obstacle avoidance penalty term. When the UAV approaches the obstacle, the penalty term increases to "repel" the UAV from moving away from the obstacle area, ensuring collision avoidance safety. When moving away from the obstacle, the penalty term approaches 0 to reduce the impact on trajectory tracking.

[0010] Step S4: Update the position neural network weights and calculate the virtual position control: The single-network ADP algorithm is adopted to approximate the optimal value function through the neural network and embed the obstacle avoidance penalty term into the optimal control framework; the neural network weights are updated online based on the gradient descent method, the HJB equation is solved to obtain the nominal control law, and the virtual position control input is generated by combining the disturbance estimate to achieve the optimality of trajectory tracking;

[0011] Step S5: Calculate the attitude control input: Based on the virtual position control inverse solution of the desired attitude, define the attitude tracking error and construct its dynamic equation; adopt a control structure combining feedforward and feedback, the feedforward control compensates for the known dynamics of the system, and the feedback control achieves the convergence of the attitude error based on online learning of the neural network. At the same time, a disturbance estimate is introduced to improve robustness, and finally the attitude control input is output.

[0012] Furthermore, in step S1:

[0013] The location subsystem is ,in, The location coordinates of the drone. For the UAV attitude Euler angles, Let be the rotation matrix from the body coordinate system to the inertial coordinate system. For the quality of drones, , This represents the total thrust generated by the four rotors. It is the acceleration due to gravity. For external disturbances acting on the position subsystem, ;

[0014] Attitude subsystem is ,in, Here is the rotational inertia matrix. For nonlinear Coriolis terms, It controls the input torque. For attitude control input, , , For drone parameters, This refers to external disturbances acting on the attitude subsystem.

[0015] Furthermore, the rotation matrix

[0016]

[0017] From the body coordinate system to inertial coordinate system The rotation matrix, where, , .

[0018] Furthermore, the rotational inertia matrix .

[0019] Furthermore, in step S2:

[0020] To compensate for uncertain external disturbances in the system A third-order sliding mode observer is constructed for perturbation estimation: , , ,

[0021] , , , ,

[0022] in, , Design parameters related to the upper bound of the perturbation derivative, , , If is a state-dependent dynamic term in the system, then the external disturbance estimate is: ,in .

[0023] Furthermore, in step S3: calculate the obstacle avoidance penalty term. To achieve obstacle avoidance control, consider defining the first... k The location of the obstacle is ,in Then the obstacle avoidance penalty item ,in, Augmenting system state To track errors, , , For reference location trajectory, The obstacle avoidance penalty coefficient, For the scheduling function, we have

[0024]

[0025] Let be the control barrier function, where , , Let be the relative position vector between the drone and the obstacle. UAV velocity vector, for hour The value of when the drone approaches an obstacle. This will increase rapidly, thus creating obstacle items in optimized control and actively driving the drone to avoid obstacle areas.

[0026] Furthermore, in step S4:

[0027] First, considering the state of the augmented system, its dynamic equation is obtained as follows: ,in, , , , , The desired trajectory function , For virtual position control;

[0028] Then, a neural network is used to approximate the optimal value function. ,in , and To track performance design parameters, Let be the attenuation coefficient, and define the location neural network structure as follows: ,in, For the weight vector, For the activation function vector, For obstacles and penalties, , , for hour The value, For positive numbers, the nominal control law Define Bellman residuals: ;

[0029] Update the weights using gradient descent: , , For design parameters, , For learning rate, The attenuation coefficient;

[0030] The final calculated virtual position control input is .

[0031] Furthermore, in step S5: calculate attitude control. ;

[0032] First, obtain the desired attitude of the drone. When virtual location control After obtaining the desired yaw angle, input the desired yaw angle. Then the desired pitch angle and expected roll angle It can be done The inverse solution yields the result;

[0033] The tracking error is defined as , , The tracking error is dynamic. ,in, , , The feedforward attitude control section is , for The false reversal;

[0034] The feedback control law is: ,in, For activation function, , For design parameters, Let be the weight vector, and its update law be: ,in, , , , , For design parameters, ;

[0035] The final attitude control law for the UAV is .

[0036] To achieve the above objectives, the present invention also provides a safe collision avoidance and tracking control system for a quadcopter unmanned aerial vehicle, comprising the following modules:

[0037] Model building module: used to establish a six-degree-of-freedom spatial nonlinear dynamic model of a quadcopter UAV. The model includes a position subsystem and an attitude subsystem. The position subsystem describes the position change of the UAV in the inertial coordinate system, and its position change is affected by the total thrust, gravity and external disturbances. The attitude subsystem describes the change of Euler angles, and its Euler angle change is affected by the control torque, Coriolis force and external disturbances.

[0038] The disturbance estimation module is configured as a high-order sliding mode observer to perform real-time estimation of external disturbances to the position subsystem and attitude subsystem. The disturbance estimation module constructs dynamic terms and sliding surfaces related to the system state to enable the disturbance estimation error to converge quickly and outputs the disturbance estimate for subsequent disturbance compensation.

[0039] Obstacle avoidance penalty term generation module: Based on the velocity obstacle method, a control barrier function CBF is designed to calculate and output the obstacle avoidance penalty term according to the relative position and velocity relationship between the UAV and the obstacle. When the UAV approaches the obstacle, the obstacle avoidance penalty term increases to generate a "repulsion" effect to make the UAV move away from the obstacle area. When the UAV moves away from the obstacle, the obstacle avoidance penalty term approaches 0 to reduce the impact on trajectory tracking.

[0040] The virtual position control module includes a single-network ADP adaptive dynamic programming unit. The single-network ADP unit is configured to approximate the optimal value function through a neural network and embed the obstacle avoidance penalty term into the optimal control framework. The virtual position control module updates the neural network weights online based on the gradient descent method, solves the HJB equation to obtain the nominal control law, and generates and outputs the virtual position control input by combining the disturbance estimate output by the disturbance estimation module, thereby achieving optimal trajectory tracking.

[0041] Attitude control module: used to solve the desired attitude based on the virtual position control input output by the virtual position control module, define the attitude tracking error and construct its dynamic equation; the attitude control module adopts a feedforward-feedback combined control structure, where feedforward control is used to compensate for the known dynamics of the system, feedback control is based on online learning of neural networks to achieve convergence of attitude error, and the disturbance estimate output by the disturbance estimation module is introduced to improve control robustness, and finally outputs attitude control input.

[0042] To achieve the above objectives, the present invention also provides a computer-readable storage device storing a computer program that, when executed, implements the above-described quadcopter UAV safety collision avoidance and tracking control method.

[0043] Beneficial effects: This invention addresses the problem of external interference in the system by constructing a high-order sliding mode observer; based on the velocity obstacle avoidance method, a control obstacle function (CBF) is designed to enable QUAV to avoid collisions in obstacle environments; the CBF is introduced into the optimal control framework to achieve a balance between optimality and safety; the HJB equations are solved using the ADP algorithm to obtain the optimal position and attitude controller, ensuring that the position and attitude tracking errors are consistent and eventually bounded. Attached Figure Description

[0044] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings:

[0045] Figure 1 This is a flowchart of the safe obstacle avoidance and tracking control method for unmanned aerial vehicles (UAVs) under complex disturbance environments, as described in an embodiment of the present invention.

[0046] Figure 2 This is a diagram showing the position tracking trajectory and position error in the safe obstacle avoidance and tracking control method for UAVs under complex disturbance environments described in this embodiment of the invention.

[0047] Figure 3 This is a three-dimensional trajectory diagram of the QUAV in the safe obstacle avoidance and tracking control method for unmanned aerial vehicles in complex disturbance environments described in this embodiment of the invention;

[0048] Figure 4 This is a trajectory diagram of the scheduling function for each obstacle in the safe obstacle avoidance and tracking control method for UAVs in complex disturbance environments as described in this embodiment of the invention.

[0049] Figure 5 This is an attitude angle tracking error diagram in the safe obstacle avoidance and tracking control method for UAVs under complex disturbance environments described in this embodiment of the invention;

[0050] Figure 6 This is a convergence graph of the position and attitude neural network weights in the safe obstacle avoidance and tracking control method for UAVs under complex disturbance environments described in this embodiment of the invention.

[0051] Figure 7 This is a schematic diagram of the structure of the safe obstacle avoidance and tracking control system for UAVs in complex disturbance environments, as described in an embodiment of the present invention. Detailed Implementation

[0052] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0053] Example 1

[0054] See Figure 1 This embodiment of a method for safe obstacle avoidance and tracking control of a UAV in a complex disturbance environment includes the following steps:

[0055] Step S1: Establish a mathematical model of the quadcopter UAV: ​​Construct a nonlinear dynamic model in a six-degree-of-freedom space, which is divided into a position subsystem and an attitude subsystem. The position subsystem describes the position change of the UAV in the inertial coordinate system and is affected by the total thrust, gravity and external disturbances. The attitude subsystem describes the changes in Euler angles (roll angle, pitch angle and yaw angle) and is affected by the control torque, Coriolis force and external disturbances.

[0056] Step S2: Design a high-order sliding mode observer to estimate external disturbances: For external disturbances of the position and attitude subsystem, such as airflow interference, a third-order sliding mode observer is designed to achieve real-time estimation; by constructing dynamic terms and sliding surfaces related to the system state, the disturbance estimation error is made to converge quickly, providing a basis for subsequent disturbance compensation.

[0057] Step S3: Calculate the obstacle avoidance penalty term: Design the control barrier function CBF based on the velocity obstacle method. By defining the relative position and velocity relationship between the UAV and the obstacle, construct the obstacle avoidance penalty term. When the UAV approaches the obstacle, the penalty term increases to "repel" the UAV from moving away from the obstacle area, ensuring collision avoidance safety. When moving away from the obstacle, the penalty term approaches 0 to reduce the impact on trajectory tracking.

[0058] Step S4: Update the position neural network weights and calculate the virtual position control: The single-network ADP algorithm is adopted to approximate the optimal value function through the neural network and embed the obstacle avoidance penalty term into the optimal control framework; the neural network weights are updated online based on the gradient descent method, the HJB equation is solved to obtain the nominal control law, and the virtual position control input is generated by combining the disturbance estimate to achieve the optimality of trajectory tracking;

[0059] Step S5: Calculate the attitude control input: Based on the virtual position control inverse solution of the desired attitude, define the attitude tracking error and construct its dynamic equation; adopt a control structure combining feedforward and feedback, the feedforward control compensates for the known dynamics of the system, and the feedback control achieves the convergence of the attitude error based on online learning of the neural network. At the same time, a disturbance estimate is introduced to improve robustness, and finally the attitude control input is output.

[0060] This embodiment integrates a complete control method encompassing "modeling-disturbance estimation-obstacle avoidance-optimal control-attitude adjustment," achieving safe obstacle avoidance and trajectory tracking for UAVs in complex disturbance environments through a five-step collaborative process. Its core advantages are:

[0061] By integrating high-order sliding mode disturbance estimation, control barrier function (CBF) obstacle avoidance, and adaptive dynamic programming (ADP) optimal control, the problem that traditional methods cannot simultaneously guarantee tracking accuracy and obstacle avoidance safety is solved.

[0062] By strictly employing a mechanism of "disturbance compensation + obstacle avoidance penalty embedding + online learning", the position and attitude tracking errors are made consistent and ultimately bounded (UUB), ensuring system stability and robustness.

[0063] In a specific instance, in step S1:

[0064] The location subsystem is ,in, The location coordinates of the drone. For the UAV attitude Euler angles, Let be the rotation matrix from the body coordinate system to the inertial coordinate system. For the quality of drones, , This represents the total thrust generated by the four rotors. It is the acceleration due to gravity. For external disturbances acting on the position subsystem, ;

[0065] Attitude subsystem is ,in, Here is the rotational inertia matrix. For nonlinear Coriolis terms, It controls the input torque. For attitude control input, , , For drone parameters, This refers to external disturbances acting on the attitude subsystem.

[0066] This embodiment defines in detail the mathematical models of the position subsystem and attitude subsystem of the quadcopter UAV, and its advantages are:

[0067] It accurately characterizes the nonlinear dynamics of the UAV in six-degree-of-freedom space, and clearly incorporates the effects of total thrust, gravity, control torque, Coriolis force and external disturbances (such as airflow), providing an accurate theoretical basis for the design of subsequent control algorithms;

[0068] By using parameterized expressions (such as position coordinates, Euler angles, rotation matrices, etc.), the dynamic characteristics of the system can be quantified and calculated, laying a mathematical basis for disturbance estimation and control input design.

[0069] 3. In a specific example, the rotation matrix

[0070]

[0071] From the body coordinate system to inertial coordinate system The rotation matrix, where, , .

[0072] This embodiment clarifies the specific form of the rotation matrix from the body coordinate system to the inertial coordinate system, and its advantage is that:

[0073] It accurately describes the mathematical relationship between the UAV's attitude (Euler angles) and position coordinate transformation, ensuring that the mapping between the body motion and the inertial space motion is unambiguous;

[0074] The matrix form based on trigonometric functions (sin / cos) facilitates engineering implementation and provides an accurate conversion tool for the quantitative calculation of "total thrust-position change" in the position subsystem, thereby improving control accuracy.

[0075] In a specific instance, the moment of inertia matrix .

[0076] In this embodiment, the moment of inertia matrix is ​​defined as a diagonal matrix. Its advantages are:

[0077] It conforms to the physical characteristics of quadcopter UAVs (the rotational inertia about the three axes are independent of each other), simplifies the dynamic equations of the attitude subsystem, and reduces the computational complexity of the control algorithm;

[0078] The inverse of the diagonal matrix is ​​easy to solve, which facilitates the real-time calculation of "control torque-attitude change" in the attitude subsystem and improves the real-time performance of the algorithm.

[0079] In a specific instance, in step S2:

[0080] To compensate for uncertain external disturbances in the system A third-order sliding mode observer is constructed for perturbation estimation: , , ,

[0081] , , , ,

[0082] in, , Design parameters related to the upper bound of the perturbation derivative, , , If is a state-dependent dynamic term in the system, then the external disturbance estimate is: ,in .

[0083] This embodiment designs a third-order sliding mode observer for external disturbance estimation, which has the following advantages:

[0084] For external disturbances (such as airflow) to the position and attitude subsystem, real-time, high-precision estimation of the disturbance is achieved through dynamic terms and sliding surface design, and the estimation error converges quickly.

[0085] This provides an accurate basis for subsequent disturbance compensation, effectively suppresses the impact of external uncertainties on the system, and significantly improves the robustness of UAVs in complex disturbance environments.

[0086] In a specific example, in step S3: calculate the obstacle avoidance penalty. To achieve obstacle avoidance control, consider defining the first... k The location of the obstacle is ,in Then the obstacle avoidance penalty item ,in, Augmenting system state To track errors, , , For reference location trajectory, The obstacle avoidance penalty coefficient, For the scheduling function, we have

[0087]

[0088] Let be the control barrier function, where , , Let be the relative position vector between the drone and the obstacle. UAV velocity vector, for hour The value of when the drone approaches an obstacle. This will increase rapidly, thus creating obstacle items in optimized control and actively driving the drone to avoid obstacle areas.

[0089] This embodiment designs an obstacle avoidance penalty term (including a scheduling function S and a control barrier function) based on the speed obstacle method. Its advantages are:

[0090] Dynamically adjust the penalty term size: when the drone approaches an obstacle, the penalty term increases to produce a "repulsion" effect; when it moves away from an obstacle, the penalty term approaches 0, reducing interference with trajectory tracking and achieving coordinated optimization of "safe obstacle avoidance" and "trajectory tracking";

[0091] The scheduling function S uses distance segments (obstacle avoidance radius r) ak ), detection radius r bk To achieve adaptive obstacle perception and control the barrier function By quantifying obstacle avoidance risks using relative position and velocity, the accuracy and real-time nature of obstacle avoidance decisions can be ensured.

[0092] In a specific instance, in step S4:

[0093] First, considering the state of the augmented system, its dynamic equation is obtained as follows: ,in, , , , , The desired trajectory function , For virtual position control;

[0094] Then, a neural network is used to approximate the optimal value function. ,in , and To track performance design parameters, Let be the attenuation coefficient, and define the location neural network structure as follows: ,in, For the weight vector, For the activation function vector, For obstacles and penalties, , , for hour The value, For positive numbers, the nominal control law Define Bellman residuals: ;

[0095] Update the weights using gradient descent: , , For design parameters, , For learning rate, The attenuation coefficient;

[0096] The final calculated virtual position control input is .

[0097] This embodiment describes in detail the calculation of virtual position control (including dynamic equations, neural network approximation, and weight updates), and its advantages are:

[0098] The single-network ADP algorithm is adopted, and the optimal value function is approximated through the neural network. The obstacle avoidance penalty term is embedded in the HJB equation to solve the problem, thereby achieving the optimality of trajectory tracking.

[0099] By updating the neural network weights online using the gradient descent method and generating virtual control inputs by combining the disturbance estimate, the adaptiveness of the control is guaranteed, while the optimal control accuracy is improved by minimizing the Bellman residual, thus balancing "optimal tracking" and "safety constraints".

[0100] In a specific example, in step S5: calculate attitude control ;

[0101] First, obtain the desired attitude of the drone. When virtual location control After obtaining the desired yaw angle, input the desired yaw angle. Then the desired pitch angle and expected roll angle It can be done The inverse solution yields the result;

[0102] The tracking error is defined as , , The tracking error is dynamic. ,in, , , The feedforward attitude control section is , for The false reversal;

[0103] The feedback control law is: ,in, For activation function, , For design parameters, Let be the weight vector, and its update law be: ,in, , , , , For design parameters, ;

[0104] The final attitude control law for the UAV is .

[0105] This embodiment designs an attitude control input (including a feedforward-feedback structure), the advantages of which are:

[0106] The feedforward control compensation system has known dynamics (such as Coriolis force and moment of inertia), while the feedback control is based on neural network to learn attitude error online. The combination of the two improves the speed of attitude adjustment.

[0107] By introducing a perturbation estimate to further suppress external disturbances, and by using a weight update law to ensure the convergence of attitude tracking errors, high robustness and high precision of attitude control are ultimately achieved.

[0108] In the specific implementation, to prove that the position and attitude tracking errors of the UAV system using the control method of this embodiment are ultimately bounded, the specific process is as follows:

[0109] For a position system, the Lyapunov function is defined as follows: ,in, For ideal weights The estimation error.

[0110] right Differentiation has

[0111] Therefore, we can obtain

[0112] in, , , for The smallest eigenvalue, , , It is a positive parameter. , for The largest eigenvalue, , , , , , , The upper bound constant is to satisfy the following inequality: , , , , , , , It represents the residual.

[0113] Choosing appropriate parameters makes the matrix If it is positively determined, then it should be From time to time Therefore, the position tracking error is uniformly bounded.

[0114] For the attitude system, the Lyapunov function is defined as follows: , of which For ideal weights The estimation error.

[0115] right Differentiation has

[0116] Therefore, we can obtain

[0117] in, , , for The smallest eigenvalue, for The largest eigenvalue, , , , , , , The upper bound constant is to satisfy the following inequality: , , , , , , , It represents the residual.

[0118] If the following conditions are true

[0119]

[0120] have Then the attitude tracking error is uniformly bounded.

[0121] The following is a simulation example:

[0122] The drone's parameters are configured as follows: mass gravitational acceleration m / , Moment of inertia initial position initial velocity Initial attitude angle initial angular velocity In the position subsystem and attitude subsystem, the applied external disturbances are respectively set as follows: , .

[0123] In the position subsystem, the gain parameter of the higher-order sliding mode observer (HSMO) is selected as follows: The activation function vector of the neural network is chosen as Value function parameters are set to , , , In the attitude subsystem, the gain parameter of the higher-order sliding mode observer is set as follows: The activation function vector of a real neural network The attitude value function parameters are set as follows: , .

[0124] The reference trajectory was set , Two static spherical obstacles are deployed in the environment. Obstacle 1: Center coordinates Obstacle avoidance radius Detection radius 2m; Obstacle 2: Center Coordinates Obstacle avoidance radius Detection radius

[0125] Figure 2 The results show that this method enables the positioning system to track the desired trajectory in the absence of obstacles, and the tracking error gradually converges to a small neighborhood near the origin. Furthermore, from... Figure 2-3 It can be seen that the designed controller can achieve tracking control and obstacle avoidance. Figure 4 Provides scheduling functions for obstacles. The trajectory never reaches its maximum value during the avoidance process, which means that collision avoidance with obstacles has been achieved. Figure 5 This indicates that the attitude tracking error gradually converges to a small neighborhood near the zero point. Figure 6 It demonstrates that the weights of position neural networks and pose neural networks converge quickly to their optimal values.

[0126] Example 2

[0127] To achieve the above objectives, see Figure 7 This embodiment also provides a safe collision avoidance and tracking control system for quadcopter drones, including the following modules:

[0128] Model building module: used to establish a six-degree-of-freedom spatial nonlinear dynamic model of a quadcopter UAV. The model includes a position subsystem and an attitude subsystem. The position subsystem describes the position change of the UAV in the inertial coordinate system, and its position change is affected by the total thrust, gravity and external disturbances. The attitude subsystem describes the change of Euler angles, and its Euler angle change is affected by the control torque, Coriolis force and external disturbances.

[0129] The disturbance estimation module is configured as a high-order sliding mode observer to perform real-time estimation of external disturbances to the position subsystem and attitude subsystem. The disturbance estimation module constructs dynamic terms and sliding surfaces related to the system state to enable the disturbance estimation error to converge quickly and outputs the disturbance estimate for subsequent disturbance compensation.

[0130] Obstacle avoidance penalty term generation module: Based on the velocity obstacle method, a control barrier function CBF is designed to calculate and output the obstacle avoidance penalty term according to the relative position and velocity relationship between the UAV and the obstacle. When the UAV approaches the obstacle, the obstacle avoidance penalty term increases to generate a "repulsion" effect to make the UAV move away from the obstacle area. When the UAV moves away from the obstacle, the obstacle avoidance penalty term approaches 0 to reduce the impact on trajectory tracking.

[0131] The virtual position control module includes a single-network ADP adaptive dynamic programming unit. The single-network ADP unit is configured to approximate the optimal value function through a neural network and embed the obstacle avoidance penalty term into the optimal control framework. The virtual position control module updates the neural network weights online based on the gradient descent method, solves the HJB equation to obtain the nominal control law, and generates and outputs the virtual position control input by combining the disturbance estimate output by the disturbance estimation module, thereby achieving optimal trajectory tracking.

[0132] Attitude control module: used to solve the desired attitude based on the virtual position control input output by the virtual position control module, define the attitude tracking error and construct its dynamic equation; the attitude control module adopts a feedforward-feedback combined control structure, where feedforward control is used to compensate for the known dynamics of the system, feedback control is based on online learning of neural networks to achieve convergence of attitude error, and the disturbance estimate output by the disturbance estimation module is introduced to improve control robustness, and finally outputs attitude control input.

[0133] The quadcopter UAV safety collision avoidance and tracking control system of this embodiment has the same advantages as the quadcopter UAV safety collision avoidance and tracking control system described above compared with the prior art, and will not be repeated here.

[0134] Example 3

[0135] To achieve the above objectives, this embodiment also provides a computer-readable storage device storing a computer program that, when executed, implements the aforementioned quadcopter UAV safety collision avoidance and tracking control method.

[0136] Those skilled in the art will understand that all or part of the steps of the above method embodiments can be implemented by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it performs the steps of the above method embodiments. The aforementioned storage medium includes various media that can store program code, such as ROM, RAM, magnetic disk, or optical disk.

[0137] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A safe obstacle avoidance and tracking control method for unmanned aerial vehicles (UAVs) under complex disturbance environments, characterized in that, Includes the following steps: Step S1: Establish a mathematical model of the quadcopter UAV: ​​Construct a nonlinear dynamic model in a six-degree-of-freedom space, which is divided into a position subsystem and an attitude subsystem. The position subsystem describes the position change of the UAV in the inertial coordinate system and is affected by the total thrust, gravity and external disturbances. The attitude subsystem describes the change of Euler angles and is affected by the control torque, Coriolis force and external disturbances. Step S2: Design a high-order sliding mode observer to estimate external disturbances: For external disturbances of the position and attitude subsystem, a third-order sliding mode observer is designed to achieve real-time estimation; by constructing dynamic terms and sliding surfaces related to the system state, the disturbance estimation error is made to converge quickly, providing a basis for subsequent disturbance compensation. Step S3: Calculate the obstacle avoidance penalty term: Design the control barrier function CBF based on the velocity obstacle method. By defining the relative position and velocity relationship between the UAV and the obstacle, construct the obstacle avoidance penalty term. When the UAV approaches the obstacle, the penalty term increases to "repel" the UAV from the obstacle area and ensure collision avoidance safety. When moving away from the obstacle, the penalty term approaches 0 to reduce the impact on trajectory tracking. Step S4: Update the position neural network weights and calculate the virtual position control: The single-network ADP algorithm is adopted to approximate the optimal value function through the neural network and embed the obstacle avoidance penalty term into the optimal control framework; the neural network weights are updated online based on the gradient descent method, the HJB equation is solved to obtain the nominal control law, and the virtual position control input is generated by combining the disturbance estimate to achieve the optimality of trajectory tracking; Step S5: Calculate the attitude control input: Based on the virtual position control inverse solution of the desired attitude, define the attitude tracking error and construct its dynamic equation; adopt a control structure combining feedforward and feedback, the feedforward control compensates for the known dynamics of the system, and the feedback control achieves the convergence of the attitude error based on online learning of the neural network. At the same time, a disturbance estimate is introduced to improve robustness, and finally the attitude control input is output.

2. The method for safe obstacle avoidance and tracking control of a UAV under complex disturbance environments according to claim 1, characterized in that, In step S1: The location subsystem is ,in, The location coordinates of the drone. For the UAV attitude Euler angles, Let be the rotation matrix from the body coordinate system to the inertial coordinate system. For the quality of drones, , This represents the total thrust generated by the four rotors. It is the acceleration due to gravity. For external disturbances acting on the position subsystem, ; Attitude subsystem is ,in, Here is the rotational inertia matrix. For nonlinear Coriolis terms, It controls the input torque. For attitude control input, , , For drone parameters, This refers to external disturbances acting on the attitude subsystem.

3. The method for safe obstacle avoidance and tracking control of a UAV under complex disturbance environments according to claim 2, characterized in that, The rotation matrix From the body coordinate system to inertial coordinate system The rotation matrix, where, , .

4. The method for safe obstacle avoidance and tracking control of a UAV in a complex disturbance environment according to claim 2, characterized in that, The rotational inertia matrix .

5. The method for safe obstacle avoidance and tracking control of a UAV under complex disturbance environments according to claim 1, characterized in that, In step S2: To compensate for uncertain external disturbances in the system A third-order sliding mode observer is constructed for perturbation estimation: , , , ， ， ， ， in, , Design parameters related to the upper bound of the perturbation derivative, , , If is a state-dependent dynamic term in the system, then the external disturbance estimate is: ,in .

6. The method for safe obstacle avoidance and tracking control of a UAV in a complex disturbance environment according to claim 1, characterized in that, In step S3: Calculate the obstacle avoidance penalty. To achieve obstacle avoidance control, consider defining the first... k The location of the obstacle is ,in Then the obstacle avoidance penalty item ,in, Augmenting system state To track errors, , , For reference location trajectory, The obstacle avoidance penalty coefficient, For the scheduling function, we have Let be the control barrier function, where , , Let be the relative position vector between the drone and the obstacle. UAV velocity vector, for hour The value of when the drone approaches an obstacle. This will increase rapidly, thus creating obstacle items in optimized control and actively driving the drone to avoid obstacle areas.

7. The method for safe obstacle avoidance and tracking control of a UAV under complex disturbance environments according to claim 1, characterized in that, In step S4: First, considering the state of the augmented system, its dynamic equation is obtained as follows: ,in, , , , , The desired trajectory function , For virtual position control; Then, a neural network is used to approximate the optimal value function. ,in , and To track performance design parameters, Let be the attenuation coefficient, and define the location neural network structure as follows: ,in, For the weight vector, For the activation function vector, For obstacles and penalties, , , for hour The value, For positive numbers, the nominal control law Define Bellman residuals: ; Update the weights using gradient descent: , , For design parameters, , For learning rate, The attenuation coefficient; The final calculated virtual position control input is .

8. The method for safe obstacle avoidance and tracking control of a UAV under complex disturbance environments according to claim 1, characterized in that, In step S5: calculate attitude control ; First, obtain the desired attitude of the drone. When virtual location control After obtaining the desired yaw angle, input the desired yaw angle. Then the desired pitch angle and expected roll angle It can be done The inverse solution yields the result; The tracking error is defined as , , The tracking error is dynamic. ,in, , , The feedforward attitude control section is , for The false reversal; The feedback control law is: ,in, For activation function, , For design parameters, Let be the weight vector, and its update law be: ,in, , , , , For design parameters, ; The final attitude control law for the UAV is .

9. A safety collision avoidance and tracking control system for a quadcopter unmanned aerial vehicle (UAV), characterized in that, Includes the following modules: Model building module: used to establish a six-degree-of-freedom spatial nonlinear dynamic model of a quadcopter UAV. The model includes a position subsystem and an attitude subsystem. The position subsystem describes the position change of the UAV in the inertial coordinate system, and its position change is affected by the total thrust, gravity and external disturbances. The attitude subsystem describes the change of Euler angles, and its Euler angle change is affected by the control torque, Coriolis force and external disturbances. The disturbance estimation module is configured as a high-order sliding mode observer to perform real-time estimation of external disturbances to the position subsystem and attitude subsystem. The disturbance estimation module constructs dynamic terms and sliding surfaces related to the system state to enable the disturbance estimation error to converge quickly and outputs the disturbance estimate for subsequent disturbance compensation. Obstacle avoidance penalty term generation module: Based on the velocity obstacle method, a control barrier function CBF is designed to calculate and output the obstacle avoidance penalty term according to the relative position and velocity relationship between the UAV and the obstacle. When the UAV approaches the obstacle, the obstacle avoidance penalty term increases to generate a "repulsion" effect to make the UAV move away from the obstacle area. When the UAV moves away from the obstacle, the obstacle avoidance penalty term approaches 0 to reduce the impact on trajectory tracking. The virtual position control module includes a single-network ADP adaptive dynamic programming unit. The single-network ADP unit is configured to approximate the optimal value function through a neural network and embed the obstacle avoidance penalty term into the optimal control framework. The virtual position control module updates the neural network weights online based on the gradient descent method, solves the HJB equation to obtain the nominal control law, and generates and outputs the virtual position control input by combining the disturbance estimate output by the disturbance estimation module, thereby achieving optimal trajectory tracking. Attitude control module: used to solve the desired attitude based on the virtual position control input output by the virtual position control module, define the attitude tracking error and construct its dynamic equation; the attitude control module adopts a feedforward-feedback combined control structure, where feedforward control is used to compensate for the known dynamics of the system, feedback control is based on online learning of neural networks to achieve convergence of attitude error, and the disturbance estimate output by the disturbance estimation module is introduced to improve control robustness, and finally outputs attitude control input.

10. A computer-readable storage device storing a computer program, characterized in that, When the computer program is executed, it implements the safe collision avoidance and tracking control method for quadcopter unmanned aerial vehicles as described in any one of claims 1-8.

Images