Safe obstacle avoidance tracking control method and system for unmanned aerial vehicle in complex disturbance environment

Abstract

The invention discloses a safe obstacle avoidance tracking control method for an unmanned aerial vehicle in a complex disturbance environment, and belongs to the technical field of computer application. Comprising the following steps: designing a high-order sliding-mode observer to realize disturbance estimation for the external disturbance problem of a system; a control barrier function (CBF) is constructed based on a speed obstacle method, and collision avoidance of the unmanned aerial vehicle in an obstacle environment is guaranteed; the CBF is embedded into an optimal control framework, and trajectory tracking performance and obstacle avoidance safety are cooperatively optimized; a single-network ADP algorithm is adopted to solve an HJB equation, an optimal position and attitude controller is generated in real time, and the UUB of position and attitude tracking errors is strictly proved. The invention further provides a safe obstacle avoidance tracking control system for the unmanned aerial vehicle in the complex disturbance environment. The problem of safe obstacle avoidance tracking control of the unmanned aerial vehicle in a complex disturbance environment is effectively solved.

Description

Technical Field

[0001] The present invention relates to the field of computer application technology, and in particular to a safe obstacle avoidance and tracking control method and system for a drone in a complex disturbance environment. Background Art

[0002] Quadrotor unmanned aerial vehicles (QUAVs), with their exceptional maneuverability and vertical take-off and landing capabilities, have found widespread application in military reconnaissance, urban inspections, and agricultural spraying. However, these systems exhibit strong nonlinear dynamics and face challenges in operational environments, including external uncertainties (such as airflow disturbances) and complex obstacles (such as buildings and trees). 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) equation, provides a new approach for approximate optimal control of nonlinear systems. However, existing research has the following shortcomings: (1) obstacle constraints are often handled by separating path planning and control, which makes it difficult to ensure the unity of real-time performance and safety; (2) the compensation accuracy for external disturbances is insufficient, which affects the robustness of control; (3) the coordinated integration of online learning mechanisms with 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 unmanned aerial vehicles in complex environments. Summary of the Invention

[0004] To solve the above problems, the present invention aims to propose a safe obstacle avoidance and tracking control method and system for UAVs in complex disturbance environments. By integrating high-order sliding mode disturbance estimation, CBF obstacle avoidance mechanism and ADP optimal control, the safe tracking problem of quadrotor UAVs in complex disturbance and obstacle environments is effectively solved, providing technical support for the reliable operation of UAVs in actual scenarios.

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

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

[0007] Step S1, establishing a mathematical model of the quadrotor drone: constructing 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 drone in the inertial coordinate system, which is affected by the total thrust, gravity, and external disturbances; the attitude subsystem describes the change of the Euler angle, which 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: Design a third-order sliding mode observer to achieve real-time estimation of external disturbances in the position and attitude subsystems. By constructing dynamic terms and sliding mode surfaces related to the system state, the disturbance estimation error is quickly converged, providing a basis for subsequent disturbance compensation.

[0009] Step S3, calculating the obstacle avoidance penalty term: Based on the speed barrier method, a control barrier function (CBF) is designed. By defining the relative position and speed relationship between the UAV and the obstacle, an obstacle avoidance penalty term is constructed. When the UAV approaches an obstacle, the penalty term increases to "repel" the UAV away from the obstacle area to ensure collision avoidance safety. When the UAV is far away from the obstacle, the penalty term approaches 0 to reduce the impact on trajectory tracking.

[0010] Step S4: Update the weights of the position neural network and calculate the virtual position control: Use a single-network ADP algorithm to approximate the optimal value function through the neural network, and embed the obstacle avoidance penalty term into the optimal control framework; update the neural network weights online based on the gradient descent method, solve the HJB equation to obtain the nominal control law, and combine the disturbance estimator to generate the virtual position control input to achieve optimal trajectory tracking;

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

[0012] Furthermore, in step S1:

[0013] The location subsystem is in is the position coordinate of the UAV, is the Euler angle of the drone attitude, R(Θ) is the rotation matrix from the body coordinate system to the inertial coordinate system, m is the mass of the drone, U=[0,0,U F ] T , U F represents the total thrust generated by the four rotors, g is the acceleration due to gravity, is the external disturbance acting on the position subsystem, c 3,3 =[0,0,1] T ;

[0014] The attitude subsystem is Among them J Θ is the moment of inertia matrix, is the nonlinear Coriolis term, τ=[lk ω u φ ,lk ω uθ ,k τ u ψ ] T is the control input torque, u Θ =[u φ ,u θ ,u ψ ] T is the posture control input, l, k ω , k τ are the drone parameters, is the external disturbance acting on the attitude subsystem.

[0015] Furthermore, the rotation matrix

[0016]

[0017] ε is the slave body coordinate system B To the inertial coordinate system ε I The rotation matrix, where c x =cosx,s x =sinx.

[0018] Furthermore, the moment of inertia matrix J Θ =diag(J φ ,J θ ,J ψ ).

[0019] Furthermore, in step S2:

[0020] To compensate for the uncertain external disturbance d in the system p ,d Θ , construct a third-order sliding mode observer for disturbance estimation:

[0021] dξ,i=zξ,i2,νξ,i1=-3Lξ,i1 / 3zξ,i1-ξi2 / 3signzξ,i1-ξi+zξ,i2, where ξ∈{p,Θ},L ξ,i is a design parameter related to the upper bound of the perturbation derivative, w p =R(Θ)U-gc 3,3 , is the dynamic term related to the state in the system, then the external disturbance estimator is where ξ∈{p,Θ}.

[0022] Furthermore, in step S3: calculate the obstacle avoidance penalty term N p (z p ), in order to achieve obstacle avoidance control, consider defining the position of the kth obstacle as in The obstacle avoidance penalty in, Augmented system state, e p =x p -x pr is the tracking error, p r is the reference position trajectory, a>0 obstacle avoidance penalty coefficient, S(z p ,s k ) is the scheduling function,

[0023]

[0024] is the control barrier function, where h(x p ,s k )=(∥d o ∥-rakeh1xp,sk, do=sk-p is the relative position vector between the UAV and the obstacle, UAV velocity vector, h(0,s k ) is x p = 0 when h(x p ,s k ) value, when the drone approaches an obstacle, B k (z p ) will increase rapidly, thus forming an obstacle term in the optimization control, actively driving the UAV to avoid the obstacle area.

[0025] Furthermore, in step S4:

[0026] First, for the state of the augmented system, its dynamic equation is obtained as follows: in H p is the expected trajectory function u p =R(Θ)U-gc 3,3 For virtual position control;

[0027] Then use the neural network to approximate the optimal value function where Q p =diag{Q p,e ,0 6×6}, and Design parameters for tracking performance, γ o >0 is the attenuation coefficient, and the position neural network structure is defined as in is the weight vector, σ p (zp ) is the activation function vector, is the obstacle penalty term, b′ k (0) is z p = 0 when b′ k (z p ), μ is a positive constant, the nominal control law Define Bellman residual:

[0028] Update the weights using gradient descent: Q p =diag{Q p,e ,0 6×6}, is the design parameter, α p >0 is the learning rate, γ p >0 is the attenuation coefficient;

[0029] The final calculated virtual position control input is

[0030] Furthermore, in step S5: calculate the posture control u Θ ;

[0031] First, obtain the desired posture of the drone Θ r =[φ r ,θ r ,ψ r ] T , when the virtual position control u p After obtaining, input the desired yaw angle ψ r , then the desired pitch angle θ r and the desired roll angle φ r You can use u p =R(Θ r 0U / m-gc 3,3 The inverse solution is:

[0032] Define the tracking error as e Θ =x Θ -x Θr ,, The tracking error dynamics is in The feedforward attitude control part is g Θ (x Θ )’s pseudo-inverse;

[0033] The feedback control law is: in is the activation function, G Θ =g Θ (x Θ ), is the design parameter, is the weight vector, and its update law is: where α Θ >0, Q Θ is the design parameter,

[0034] The final UAV attitude control law is

[0035] In order to achieve the above objectives, the present invention also provides a quadrotor UAV safety collision avoidance tracking control system, comprising the following modules:

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

[0037] Disturbance estimation module: configured as a high-order sliding mode observer, used to estimate the external disturbances to the position subsystem and attitude subsystem in real time; the disturbance estimation module constructs dynamic terms and sliding mode surfaces related to the system state to enable rapid convergence of the disturbance estimation error, and outputs the disturbance estimate for subsequent disturbance compensation;

[0038] Obstacle avoidance penalty generation module: Based on the speed barrier method, a control barrier function (CBF) is designed to calculate and output the obstacle avoidance penalty based on the relative position and speed relationship between the drone and the obstacle. When the drone approaches an obstacle, the obstacle avoidance penalty increases to create a "repulsion" effect, forcing the drone to stay away from the obstacle area. When the drone moves away from the obstacle, the obstacle avoidance penalty approaches 0 to minimize the impact on trajectory tracking.

[0039] A virtual position control module includes a single-network ADP adaptive dynamic programming unit 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 a gradient descent method, solves the HJB equation to obtain a nominal control law, and generates and outputs a virtual position control input in combination with the disturbance estimate output by the disturbance estimation module to achieve optimal trajectory tracking.

[0040] Attitude control module: used to inversely 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, in which feedforward control is used to compensate for the known dynamics of the system, and feedback control is based on neural network online learning to achieve convergence of attitude error. At the same time, the disturbance estimation output by the disturbance estimation module is introduced to improve control robustness, and finally the attitude control input is output.

[0041] In order to achieve the above-mentioned purpose, the present invention also provides a computer-readable storage device, which stores a computer program. When the computer program is executed, it implements the above-mentioned four-rotor drone safe collision avoidance tracking control method.

[0042] Beneficial effects: The present invention addresses the problem of external interference in the system by constructing a high-order sliding mode observer; based on the speed obstacle avoidance method, a control obstacle function (CBF) is designed to enable the QUAV to avoid collisions in an obstacle environment; the CBF is introduced into the optimal control framework to achieve a balance between optimality and safety; the ADP algorithm is used to solve the HJB equation to obtain the optimal position and attitude controller, ensuring that the position and attitude tracking errors are consistent and ultimately bounded. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The accompanying drawings, which constitute part of the present invention, are provided to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are provided to explain the present invention and do not constitute an undue limitation of the present invention. In the accompanying drawings:

[0044] Figure 1 This is a flow chart of a safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to an embodiment of the present invention;

[0045] Figure 2 The position tracking trajectory and position error map of the safe obstacle avoidance tracking control method for a UAV in a complex disturbance environment according to an embodiment of the present invention;

[0046] Figure 3 A three-dimensional trajectory diagram of the QUAV in the safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to an embodiment of the present invention;

[0047] Figure 4 This is a trajectory diagram of the scheduling function for each obstacle in the safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to an embodiment of the present invention;

[0048] Figure 5 This is a graph of attitude angle tracking error in the safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to an embodiment of the present invention;

[0049] Figure 6This is a convergence diagram of the position and attitude neural network weights in the safe obstacle avoidance and tracking control method for a drone in a complex disturbance environment according to an embodiment of the present invention;

[0050] Figure 7 This is a schematic diagram of the structure of a safe obstacle avoidance and tracking control system for a UAV in a complex disturbance environment according to an embodiment of the present invention. DETAILED DESCRIPTION

[0051] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0052] Example 1

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

[0054] Step S1, establishing a mathematical model of a quadrotor drone: constructing 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 drone in the inertial coordinate system, which is affected by the total thrust, gravity, and external disturbances; the attitude subsystem describes the change of the Euler angles (roll angle, pitch angle, yaw angle), which is affected by the control torque, Coriolis force, and external disturbances;

[0055] 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 disturbances, a third-order sliding mode observer is designed to achieve real-time estimation. By constructing dynamic terms and sliding mode surfaces related to the system state, the disturbance estimation error is quickly converged, providing a basis for subsequent disturbance compensation.

[0056] Step S3, calculating the obstacle avoidance penalty term: Based on the speed barrier method, a control barrier function (CBF) is designed. By defining the relative position and speed relationship between the UAV and the obstacle, an obstacle avoidance penalty term is constructed. When the UAV approaches an obstacle, the penalty term increases to "repel" the UAV away from the obstacle area to ensure collision avoidance safety. When the UAV is far away from the obstacle, the penalty term approaches 0 to reduce the impact on trajectory tracking.

[0057] Step S4: Update the weights of the position neural network and calculate the virtual position control: Use a single-network ADP algorithm to approximate the optimal value function through the neural network, and embed the obstacle avoidance penalty term into the optimal control framework; update the neural network weights online based on the gradient descent method, solve the HJB equation to obtain the nominal control law, and combine the disturbance estimator to generate the virtual position control input to achieve optimal trajectory tracking;

[0058] Step S5, calculate the attitude control input: inversely solve the desired attitude based on the virtual position control, define the attitude tracking error and construct its dynamic equation; adopt a feedforward-feedback combined control structure, feedforward control compensates for the known dynamics of the system, and feedback control achieves convergence of the attitude error based on neural network online learning. At the same time, a disturbance estimator is introduced to improve robustness, and finally the attitude control input is output.

[0059] This embodiment integrates a complete control method of "modeling - disturbance estimation - obstacle avoidance - optimal control - attitude adjustment" to achieve safe obstacle avoidance and trajectory tracking of drones in complex disturbance environments through a five-step collaborative process. Its core advantages are:

[0060] The integration of high-order sliding mode disturbance estimation, control barrier function (CBF) obstacle avoidance and adaptive dynamic programming (ADP) optimal control solves the problem that traditional methods are difficult to simultaneously ensure tracking accuracy and obstacle avoidance safety;

[0061] Through the strict "disturbance compensation + obstacle avoidance penalty embedding + online learning" mechanism, the consistent ultimate bounded (UUB) of position and attitude tracking errors is achieved, ensuring the stability and robustness of the system.

[0062] In a specific example, in step S1:

[0063] The location subsystem is in is the position coordinate of the UAV, is the Euler angle of the drone attitude, R(Θ) is the rotation matrix from the body coordinate system to the inertial coordinate system, m is the mass of the drone, U=[0,0,U F ] T , U F represents the total thrust generated by the four rotors, g is the acceleration due to gravity, is the external disturbance acting on the position subsystem, c 3,3 =[0,0,1] T ;

[0064] The attitude subsystem is Among them J Θ is the moment of inertia matrix, is the nonlinear Coriolis term, τ=[lk ω u φ ,lk ω u θ ,k τ u ψ ] T is the control input torque, u Θ =[u φ ,u θ ,u ψ ] T is the posture control input, l, kω , k τ are the drone parameters, is the external disturbance acting on the attitude subsystem.

[0065] This embodiment defines in detail the mathematical models of the position subsystem and attitude subsystem of the quadrotor drone, which has the following advantages:

[0066] The nonlinear dynamic characteristics of the UAV in the six-degree-of-freedom space are accurately characterized, and the effects of total thrust, gravity, control torque, Coriolis force, and external disturbances (such as airflow) are clearly incorporated, providing an accurate theoretical basis for the subsequent control algorithm design.

[0067] Through parametric expression (such as position coordinates, Euler angles, rotation matrices, etc.), the system dynamic characteristics can be quantified and calculated, laying a mathematical basis for disturbance estimation and control input design.

[0068] In one specific example, the rotation matrix

[0069]

[0070] ε is the slave body coordinate system B To the inertial coordinate system ε I The rotation matrix, where c x =cosx,s x =sinx.

[0071] This embodiment clarifies the specific form of the rotation matrix from the body coordinate system to the inertial coordinate system, which has the following advantages:

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

[0073] 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.

[0074] In a specific example, the moment of inertia matrix J Θ =diag(J φ ,J θ ,J ψ ).

[0075] In this embodiment, the moment of inertia matrix is ​​defined as the diagonal matrix J Θ =diag(J φ ,J θ ,J ψ ), its advantages are:

[0076] It complies with the physical characteristics of quadrotor drones (the moments of inertia around 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;

[0077] The inverse matrix 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.

[0078] In a specific example, in step S2:

[0079] To compensate for the uncertain external disturbance d in the system p ,d Θ , construct a third-order sliding mode observer for disturbance estimation:

[0080] where ξ∈{p,Θ},L ξ,i is a design parameter related to the upper bound of the perturbation derivative, w p =R(Θ)U-gc 3,3 , is the dynamic term related to the state in the system, then the external disturbance estimator is where ξ∈{p,Θ}.

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

[0082] For external disturbances (such as airflow) in the position and attitude subsystems, dynamic terms and sliding surface design are used to achieve real-time, high-precision estimation of disturbances, and the estimation error converges quickly.

[0083] It provides an accurate basis for subsequent disturbance compensation, effectively suppresses the impact of external uncertainty on the system, and significantly improves the robustness of the UAV in complex disturbance environments.

[0084] In a specific example, in step S3: calculate the obstacle avoidance penalty term N p (z p ), in order to achieve obstacle avoidance control, consider defining the position of the kth obstacle as in The obstacle avoidance penalty in, Augmented system state, e p =x p -x pr is the tracking error, p r is the reference position trajectory, a>0 obstacle avoidance penalty coefficient, S(z p ,sk ) is the scheduling function,

[0085]

[0086] is the control barrier function, where h(x p ,s k )=(∥d o ∥-rakeh1xp,sk, do=sk-p is the relative position vector between the UAV and the obstacle, UAV velocity vector, h(0,s k ) is x p = 0 when h(x p ,s k ) value, when the drone approaches an obstacle, B k (z p ) will increase rapidly, thus forming an obstacle term in the optimization control, actively driving the UAV to avoid the obstacle area.

[0087] This embodiment designs the obstacle avoidance penalty term based on the speed barrier method (including the scheduling function S and the control barrier function B k , its advantages are:

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

[0089] The scheduling function S is segmented by distance (obstacle avoidance radius r ak ), detection radius r bk ) realizes adaptive perception of obstacles and controls barrier function B k Quantify obstacle avoidance risks through relative position and speed to ensure the accuracy and real-time performance of obstacle avoidance decisions.

[0090] In a specific example, in step S4:

[0091] First, for the state of the augmented system, its dynamic equation is obtained as follows: in H p is the expected trajectory function u p =R(Θ)U-gc 3,3 For virtual position control;

[0092] Then use the neural network to approximate the optimal value function , where Q p =diag{Q p,e ,06×6}, and Design parameters for tracking performance, γ p >0 is the attenuation coefficient, and the position neural network structure is defined as in is the weight vector, σ p (z p ) is the activation function vector, is the obstacle penalty term, bk′0 is the value of bk′zp when zp=0, μ is a positive constant, and the nominal control law Define Bellman residual:

[0093] Update the weights using gradient descent: Q p =diag{Q p,e ,0 6×6}, is the design parameter, α p >0 is the learning rate, γ p >0 is the attenuation coefficient;

[0094] The final calculated virtual position control input is

[0095] This embodiment describes in detail the calculation of virtual position control (including dynamic equations, neural network approximation, and weight update), which has the following advantages:

[0096] A single-network ADP algorithm is used to approximate the optimal value function through a neural network, and the obstacle avoidance penalty term is embedded in the HJB equation to achieve optimal trajectory tracking.

[0097] The neural network weights are updated online based on the gradient descent method, and virtual control inputs are generated in combination with disturbance estimators. This not only ensures the adaptability of the control, but also improves the optimal control accuracy by minimizing the Bellman residual, and balances "optimal tracking" and "safety constraints."

[0098] In a specific example, in step S5: calculate the posture control u Θ ;

[0099] First, obtain the desired posture of the drone Θ r =[φ r ,θ r ,ψ r ] T , when the virtual position control u p After obtaining, input the desired yaw angle ψ r , then the desired pitch angle θr and the desired roll angle φ r You can use u p =R(Θ r )U / m-gc 3,3 The inverse solution is:

[0100] Define the tracking error as e Θ =x Θ -x Θr ,, The tracking error dynamics is in The feedforward attitude control part is g Θ (x Θ )’s pseudo-inverse;

[0101] The feedback control law is: in is the activation function, G Θ =g Θ (x Θ ), is the design parameter, is the weight vector, and its update law is: where α Θ >0, Q Θ is the design parameter,

[0102] The final UAV attitude control law is

[0103] This embodiment designs a posture control input (including a feedforward-feedback structure), which has the following advantages:

[0104] Feedforward control compensates for known system dynamics (such as Coriolis force and moment of inertia), while feedback control uses a neural network to learn attitude errors online. The combination of the two improves the rapidity of attitude adjustment.

[0105] The disturbance estimator is introduced to further suppress external interference, and the convergence of attitude tracking error is ensured through the weight update law, ultimately achieving high robustness and high precision of attitude control.

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

[0107] For the position system, define the Lyapunov function as in is the ideal weight W pThe estimation error.

[0108] To L p The derivative is

[0109] Then we can get

[0110] in M p =diag{m1,m2},m1=λ min (Q p,e ) is Q p,e The minimum eigenvalue of η is a positive parameter, λ max (R p ) is R p The maximum eigenvalue of b p,g , b p,k , ε p,HM , δ p,d To satisfy the upper bound constant of the following inequality: ∥G p ∥≤b p,g ,∥K p ∥≤b p,k , ∥ε p,H ∥≤ε p,HM , ε p,H is the residual.

[0111] Choose appropriate parameters so that the matrix M p If it is positive, then Sometimes Then the position tracking error is uniformly eventually bounded.

[0112] For the attitude system, the Lyapunov function is defined as Among them is is the ideal weight W Θ The estimation error.

[0113] To L Θ The derivative is

[0114] Then we can get

[0115] in, λ min (Q Θ ) is Q ΘThe minimum eigenvalue of max (R Θ ) is R Θ The maximum eigenvalue of b Θ,g , b Θ,k , ε Θ,HM , δ Θ,d To satisfy the upper bound constant of the following inequality: ∥G Θ ∥≤b Θ,g ,∥k Θ ∥≤b Θ,k , ∥ε Θ,H ∥≤ε Θ,HM , ε Θ,H is the residual.

[0116] If the following conditions are met

[0117]

[0118] have Then the pose tracking error is uniformly and ultimately bounded.

[0119] The following is a simulation example:

[0120] The drone parameters are: mass m = 1kg, gravity acceleration g = 9.8m / s 2 , B τ =diag{41.07,41.24,113.51}, moment of inertia J Θ =diag{4.88,4.85,8.84}×10 -3 kg·m 2 , initial position Initial velocity Initial attitude angle Initial angular velocity In the position subsystem and attitude subsystem, the external disturbances applied are set as

[0121] In the position subsystem, the gain parameter of the high-order sliding mode observer (HSMO) is selected as: L p,1 =L p,2 =L p,3 =20, the neural network activation function vector is selected as The value function parameter is set to Q p,e =10I,R p =I,γ p =0.1, a=40. In the attitude subsystem, the gain parameter of the high-order sliding mode observer is set to: L Θ,1 =L Θ,2=L Θ,3 =100, the activation function vector of the real neural network is selected as σ Θ (e Θ )=[e Θ1 e Θ1 ,e Θ1 e Θ4 ,e Θ 4e Θ4 ,e Θ2 e Θ2 ,e Θ2 e Θ5 ,e Θ5 e Θ5 ,e Θ3 e Θ3 ,e Θ3 e Θ6 ,e Θ6 e Θ6 ] T The parameters of the attitude value function are set to: Q Θ =10I,R Θ =I.

[0122] The reference trajectory is set ψ r = 0deg. Two static spherical obstacles are deployed in the environment, obstacle 1: center coordinate Obstacle avoidance radius Detection radius r b1 =2m; Obstacle 2: center coordinates Obstacle avoidance radius r a2 =0.9m, detection radius r b2 =2m.

[0123] Figure 2 The results show that this method can enable the positioning system to track the desired trajectory when there are no obstacles, and the tracking error gradually converges to a small neighborhood near the origin. Figure 2-3 It can be seen that the designed controller can achieve tracking control and obstacle avoidance. Figure 4 Provides a scheduling function S(z p ,s k ) trajectory, which never reaches its maximum value during the avoidance process, which means that collision avoidance with the obstacle is achieved. Figure 5 It shows that the pose tracking error gradually converges to a small neighborhood around zero. Figure 6 It shows that the weights of the position neural network and the pose neural network converge quickly to their optimal values.

[0124] Example 2

[0125] To achieve the above purpose, see Figure 7This embodiment also provides a quad-rotor UAV safety collision avoidance tracking control system, including the following modules:

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

[0127] Disturbance estimation module: configured as a high-order sliding mode observer, used to estimate the external disturbances to the position subsystem and attitude subsystem in real time; the disturbance estimation module constructs dynamic terms and sliding mode surfaces related to the system state to enable rapid convergence of the disturbance estimation error, and outputs the disturbance estimate for subsequent disturbance compensation;

[0128] Obstacle avoidance penalty generation module: Based on the speed barrier method, a control barrier function (CBF) is designed to calculate and output the obstacle avoidance penalty based on the relative position and speed relationship between the drone and the obstacle. When the drone approaches an obstacle, the obstacle avoidance penalty increases to create a "repulsion" effect, forcing the drone to stay away from the obstacle area. When the drone moves away from the obstacle, the obstacle avoidance penalty approaches 0 to minimize the impact on trajectory tracking.

[0129] A virtual position control module includes a single-network ADP adaptive dynamic programming unit 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 a gradient descent method, solves the HJB equation to obtain a nominal control law, and generates and outputs a virtual position control input in combination with the disturbance estimate output by the disturbance estimation module to achieve optimal trajectory tracking.

[0130] Attitude control module: used to inversely 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, in which feedforward control is used to compensate for the known dynamics of the system, and feedback control is based on neural network online learning to achieve convergence of attitude error. At the same time, the disturbance estimation output by the disturbance estimation module is introduced to improve control robustness, and finally the attitude control input is output.

[0131] The advantages of the quadrotor drone safety collision avoidance tracking control system of this embodiment are the same as those of the above-mentioned quadrotor drone safety collision avoidance tracking control system over the prior art, and will not be repeated here.

[0132] Example 3

[0133] In order to achieve the above-mentioned purpose, this embodiment further provides a computer-readable storage device, which stores a computer program. When the computer program is executed, it implements the above-mentioned four-rotor drone safe collision avoidance tracking control method.

[0134] Those skilled in the art will understand that all or part of the steps of implementing the above-mentioned method embodiment can be completed by hardware related to program instructions, and the aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps of the above-mentioned method embodiment; and the aforementioned storage medium includes: ROM, RAM, disk or optical disk, etc. Various media that can store program codes.

[0135] 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 in the scope of protection of the present invention.

Claims

1. A safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment, characterized by: The following steps are involved: Step S1, establishing a mathematical model of the quadrotor drone: constructing 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 drone in the inertial coordinate system, which is affected by the total thrust, gravity, and external disturbances; the attitude subsystem describes the change of the Euler angle, which is affected by the control torque, Coriolis force, and external disturbances; Step S2: Design a high-order sliding mode observer to estimate external disturbances: Design a third-order sliding mode observer to achieve real-time estimation of external disturbances in the position and attitude subsystems. By constructing dynamic terms and sliding mode surfaces related to the system state, the disturbance estimation error is quickly converged, providing a basis for subsequent disturbance compensation. Step S3: Calculate the obstacle avoidance penalty term: Design the control barrier function (CBF) based on the speed barrier method. By defining the relative position and speed relationship between the UAV and the obstacle, an obstacle avoidance penalty term is constructed. When the UAV approaches the obstacle, the penalty term increases to "repel" the UAV away from the obstacle area to ensure collision avoidance safety. When the UAV is far away from the obstacle, the penalty term approaches 0 to reduce the impact on trajectory tracking. Step S4: Update the weights of the position neural network and calculate the virtual position control: Use a single-network ADP algorithm to approximate the optimal value function through the neural network, and embed the obstacle avoidance penalty term into the optimal control framework; update the neural network weights online based on the gradient descent method, solve the HJB equation to obtain the nominal control law, and combine the disturbance estimator to generate the virtual position control input to achieve optimal trajectory tracking; Step S5, calculate the attitude control input: inversely solve the desired attitude based on the virtual position control, define the attitude tracking error and construct its dynamic equation; adopt a feedforward-feedback combined control structure, feedforward control compensates for the known dynamics of the system, and feedback control achieves convergence of the attitude error based on neural network online learning. At the same time, a disturbance estimator is introduced to improve robustness, and finally the attitude control input is output.

2. The safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to claim 1 is characterized in that: In step S1: The position subsystem is in is the position coordinate of the UAV, is the Euler angle of the drone attitude, R(Θ) is the rotation matrix from the body coordinate system to the inertial coordinate system, m is the mass of the drone, U=[0,0,U F ] T , U F represents the total thrust generated by the four rotors, g is the acceleration due to gravity, is the external disturbance acting on the position subsystem, c 3,3 =[0,0,1] T ; The attitude subsystem is Among them J Θ is the moment of inertia matrix, is the nonlinear Coriolis term, τ=[lk ω u φ ,lk ω u θ ,k τ u ψ ] T is the control input torque, u Θ =[u φ ,u θ ,u ψ ] T is the posture control input, l, k ω , k τ are the drone parameters, is the external disturbance acting on the attitude subsystem.

3. The safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to claim 2 is characterized in that: The rotation matrix ε is the slave body coordinate system B To the inertial coordinate system ε I The rotation matrix, where c x =cosx,s x =sinx.

4. The safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to claim 2 is characterized in that: The moment of inertia matrix J Θ =diag(J φ ,J θ ,J ψ ).

5. The safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to claim 1 is characterized in that: In step S2: To compensate for the uncertain external disturbance d in the system p ,d Θ , construct a third-order sliding mode observer for disturbance estimation: where ξ∈{p,Θ},L ξ,i is the design parameter related to the upper bound of the perturbation derivative, w ξ =[w ξ,1 ,w ξ,2 ,w ξ,3 ] T , w p =R(Θ)U-gc 3,3 , is the dynamic term related to the state in the system, then the external disturbance estimator is where ξ∈{p,Θ}.

6. The safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to claim 1 is characterized in that: In step S3: calculate the obstacle avoidance penalty term N p (z p ), in order to achieve obstacle avoidance control, consider defining the position of the kth obstacle as in The obstacle avoidance penalty in, Augmented system state, e p =x p -x pr is the tracking error, p r is the reference position trajectory, a>0 obstacle avoidance penalty coefficient, S(z p ,s k ) is the scheduling function, is the control barrier function, where h(x p ,s k )=(∥d o ∥-rakeh1xp,sk, do=sk-p is the relative position vector between the UAV and the obstacle, UAV velocity vector, h(0,s k ) is x p = 0 when h(x p ,s k ) value, when the drone approaches an obstacle, B k (z p ) will increase rapidly, thus forming an obstacle term in the optimization control, actively driving the UAV to avoid the obstacle area.

7. The safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to claim 1 is characterized in that: In step S4: First, for the state of the augmented system, its dynamic equation is obtained as follows: in H p is the expected trajectory function u p =R(Θ)U-gc 3,3 For virtual position control; Then use the neural network to approximate the optimal value function where Q p =diag{Q p,e ,0 6×6 }, and Design parameters for tracking performance, γ p >0 is the attenuation coefficient, and the position neural network structure is defined as in is the weight vector, σ p (z p ) is the activation function vector, is the obstacle penalty term, b ′ k (0) is z p = 0 when b ′ k (z p ), μ is a positive constant, the nominal control law Define Bellman residual: Update the weights using gradient descent: Q p =diag{Q p,e ,0 6×6 }, is the design parameter, is the learning rate, γ p >0 is the attenuation coefficient; The final calculated virtual position control input is 8. The safe obstacle avoidance and tracking control method for a UAV in a complex disturbance environment according to claim 1 is characterized in that: In step S5: calculate the posture control u Θ ; First, obtain the desired posture of the drone Θ r =[φ r ,θ r ,ψ r ] T , when the virtual position control u p After obtaining, input the desired yaw angle ψ r , then the desired pitch angle θ r and the desired roll angle φ r You can use u p =R(Θ r )U / m-gc 3,3 The inverse solution is: The tracking error is defined as The tracking error dynamics is in The feedforward attitude control part is g Θ (x Θ )’s pseudo-inverse; The feedback control law is: in is the activation function, G Θ =g Θ (x Θ ), is the design parameter, is the weight vector, and its update law is: in is the design parameter, The final UAV attitude control law is 9. A four-rotor UAV safety collision avoidance tracking control system, characterized in that: Includes the following modules: Model building module: used to establish a six-degree-of-freedom spatial nonlinear dynamic model of the quadrotor drone. The model includes a position subsystem and an attitude subsystem. The position subsystem is used to describe the position change of the drone in the inertial coordinate system, and its position change is affected by the total thrust, gravity and external disturbances. The attitude subsystem is used to describe the change of the Euler angle, and its Euler angle change is affected by the control torque, Coriolis force and external disturbances. Disturbance estimation module: configured as a high-order sliding mode observer, used to estimate the external disturbances to the position subsystem and attitude subsystem in real time; the disturbance estimation module constructs dynamic terms and sliding mode surfaces related to the system state to enable rapid convergence of the disturbance estimation error, and outputs the disturbance estimate for subsequent disturbance compensation; Obstacle avoidance penalty generation module: Based on the speed barrier method, a control barrier function (CBF) is designed to calculate and output the obstacle avoidance penalty based on the relative position and speed relationship between the drone and the obstacle. When the drone approaches an obstacle, the obstacle avoidance penalty increases to create a "repulsion" effect, forcing the drone to stay away from the obstacle area. When the drone moves away from the obstacle, the obstacle avoidance penalty approaches 0 to minimize the impact on trajectory tracking. A virtual position control module includes a single-network ADP adaptive dynamic programming unit 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 a gradient descent method, solves the HJB equation to obtain a nominal control law, and generates and outputs a virtual position control input in combination with the disturbance estimate output by the disturbance estimation module to achieve optimal trajectory tracking. Attitude control module: used to inversely 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, in which feedforward control is used to compensate for the known dynamics of the system, and feedback control is based on neural network online learning to achieve convergence of attitude error. At the same time, the disturbance estimation output by the disturbance estimation module is introduced to improve control robustness, and finally the attitude control input is output.

10. A computer-readable storage device storing a computer program, characterized in that: When the computer program is executed, the safe collision avoidance and tracking control method for a quadrotor drone as described in any one of claims 1 to 8 is implemented.

Images