A method for unmanned aerial vehicle motion planning and fault-tolerant control for power failure

Abstract

This invention provides a motion planning and fault-tolerant control method for unmanned aerial vehicles (UAVs) facing power failure. The method includes: performing dynamic modeling of the UAV and designing a motion planning method to achieve autonomous navigation and collision avoidance while satisfying the constraint of decreased maneuverability of the failed UAV; diagnosing whether the UAV has experienced a power failure using a designed dual-loop observer, and reconfiguring the controller to achieve fault-tolerant control when a power failure occurs; and modifying the UAV landing gear to design an anti-yawing moment plate to adjust the downwash airflow of the rotor and the deflection airflow during high-speed rotation of the UAV, thereby reducing the yawing moment acting on the UAV's rotational motion. This invention solves the problems of localization mapping and collision avoidance caused by the high-speed rotation of a failed UAV, as well as the need for rapid fault diagnosis and recovery of stable performance in the event of a sudden failure, thus improving the autonomous safety of the UAV in actual flight.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) fault-tolerant technology, and particularly relates to a method for motion planning and fault-tolerant control of UAVs for power failure faults. Background Technology

[0002] In recent years, traditional fault-tolerant theories and methods for unmanned aerial vehicles (UAVs) have encountered new challenges, mainly in the lack of autonomous navigation capabilities in denial environments and rapid adjustment capabilities in the event of power failure. Current technical bottlenecks include: due to decreased maneuverability, complex flight environments, limited visual perception distance, and limited onboard computing power, the challenges of optimal motion planning and fault-tolerant control after a power failure in UAVs have further increased. To ensure the autonomy, safety, and reliability of UAVs during flight after power failure, optimal motion planning, rapid fault diagnosis, and fault-tolerant control in denial environments are core research topics that urgently need to be addressed.

[0003] Given that most existing motion planning methods are applicable to fully attitude-controllable UAVs, designing the optimal motion planning method to address constraints such as degraded maneuverability, obstacle avoidance, and reduced localization and mapping performance after power failure remains a bottleneck affecting safety performance improvement. While current research provides a theoretical foundation for improving the flight safety of damaged UAVs, current UAV fault diagnosis methods in denied environments are suitable for precise estimation of actuator efficiency losses. However, works that simultaneously meet the dual requirements of speed and accuracy in fault diagnosis across multiple power failure modes in practical applications are still rare. After power failure, the control stability of UAVs severely deteriorates. Integrating UAV fault diagnosis results and residual dynamic constraints into controller design to improve fault tolerance and fault-tolerant control success rates under large attitude motions in power-failed UAVs remains a pressing issue.

[0004] To address the aforementioned issues, a highly autonomous, fast-response, and safe full-loop fault-tolerant system integrating "motion planning" and "fault diagnosis + fault-tolerant control" was invented. This system ensures that the UAV possesses autonomous navigation and safe flight capabilities in complex and unknown environments after a power failure. The system boasts low fault diagnosis time and high diagnostic accuracy; it employs a motion planning method based on spatiotemporal joint optimization and a highly reliable fault-tolerant control method. It enables rapid response and intelligent control of UAV power failures. Summary of the Invention

[0005] This invention addresses the shortcomings of existing technologies by providing a motion planning and fault-tolerant control method for unmanned aerial vehicles (UAVs) facing power failure. This method solves the problems of positioning and mapping and collision avoidance caused by the high-speed rotation of a faulty UAV, as well as the need for rapid fault diagnosis and recovery of stable performance in the event of a sudden failure, thereby improving the autonomous safety of UAVs in actual flight.

[0006] This invention is achieved through the following technical solution:

[0007] According to a second aspect of the present invention, a method for motion planning and fault-tolerant control of an unmanned aerial vehicle (UAV) oriented towards dynamic failure faults is provided, comprising the following steps:

[0008] S1: Perform dynamic modeling on the unmanned aerial vehicle that has failed to power, and establish kinematic and dynamic models;

[0009] S2: Based on the modeling results of S1, a motion planning method for UAVs based on acceleration adaptive optimization is designed, thereby...

[0010] To enable autonomous navigation and collision avoidance for malfunctioning drones;

[0011] S3: By designing an observer, estimate whether the resultant thrust and torque on the UAV match the resultant thrust and torque output by the controller, and diagnose whether the UAV has suffered a failure.

[0012] S4: Based on the fault diagnosis results of S3, design a fault-tolerant controller based on nonlinear model predictive control. When a fault occurs...

[0013] In the event of a malfunction, control of the yaw channel is abandoned, ensuring control of the pitch, roll, and altitude channels.

[0014] S5: By designing an aerodynamic structure to deflect the airflow, the torque change when the UAV fails is reduced, thereby reducing the UAV's spin speed and ensuring accurate positioning and stable control of the UAV in the event of a failure.

[0015] Furthermore, in step S1, a kinematic and dynamic model of the UAV is established, the expression of which is as follows:

[0016]

[0017] I v α B =-w B ×I v w B +τ-A(w)+τ a ;

[0018] In the kinematic equations, m and ξ represent the mass and center of gravity vectors of the UAV, and T is the three-dimensional vector of the resultant thrust in the body coordinate system. a This is an interference force caused by model mismatch, where g represents the gravitational acceleration vector, and z... B This represents the last column of the rotation matrix; in rotational dynamics, This represents quaternion multiplication, × represents cross product, q represents quaternion, and w represents quaternion multiplication. B=[p,q,r] T α represents angular velocity. B I represents angular acceleration. v Let τ represent the inertia matrix, and let τ represent the torque generated by the thrust. α This represents the disturbance torque caused by model mismatch, A(w)=[0,0,-γw,] T This represents the aerodynamic torque generated by air resistance;

[0019] The combined thrust and the torque caused by the rotors depend on the thrust of the four rotors, and its expression is as follows:

[0020]

[0021] Where, ||T|| represents the magnitude of the resultant thrust vector, and t = [T0, T1, T2, T3] T G represents the thrust generated by the four motors, and G represents the hybrid control matrix.

[0022]

[0023] Where r x i and r y i represents the half-shaft arm r i In the body coordinate system, the x and y components, k t Indicates the torque coefficient;

[0024] Because there is a motor speed response delay during actual flight of the UAV, it is necessary to consider the motor dynamics model. A first-order model is used to represent the motor response dynamics, and the expression is as follows:

[0025]

[0026] Where σ is the time constant calculated from experimental data, u i It is the input command for the i-th motor.

[0027] Further, step S2 specifically involves: using MINCO to generate the spatiotemporal trajectory with minimum control cost, achieving linear complexity spatiotemporal deformation of the planar output trajectory; then restoring the complete state from the planar output; for an N-dimensional M-segment polynomial trajectory with N = 2s⁻¹, where s is the order of the optimal solution state, a linear complexity mapping is constructed, expressed as follows:

[0028] c = M(q,T);

[0029] in It is a polynomial coefficient matrix, M(q,T) is the number of points from the path. and time allocation A smooth mapping to c; then optimizing the variable (q,T) to {q,T}; the i-th trajectory segment p i (t) is represented as In trajectory optimization, the cost function for each trajectory segment includes three constraints: flight time, state constraints, and obstacle avoidance.

[0030]

[0031] Where λ is the weight vector, used to weigh the various cost functions.

[0032] Furthermore, the three constraints—flight time, state constraint, and obstacle avoidance—are specifically as follows:

[0033] (4.1) Flight time cost J t To accelerate the navigation process, we first minimize the total time; the expression is as follows:

[0034]

[0035] (4.2) State constraint cost J s Limit the magnitude of linear velocity and acceleration to ensure feasibility; the formula for the cost function is as follows:

[0036] J s =J s,v +J s,a ;

[0037]

[0038] Where v max and a max It is the threshold for linear velocity and acceleration; J s,v It is the constraint cost of the velocity term, J s,a It is the constraint cost of the acceleration term;

[0039] Furthermore, acceleration reachability analysis is used to weigh the trade-offs between residual maneuverability and trajectory optimization parameters in order to maintain z I For the balance along the z-axis, neglecting horizontal air resistance, and assuming the UAV's acceleration along the z-axis is 0, we obtain:

[0040]

[0041] Where a des It is the horizontal acceleration of the drone, T s This represents the resultant force of the four motors acting on the drone; the output thrust of each motor satisfies 0 ≤ T ≤ T. max T max This represents the maximum thrust output by a single motor; and 0 ≤ T s ≤4Tmax If a power failure occurs and motor #1 fails (T1 = 0), then to maintain the drone's balance, its opposite motor (motor #0) needs to reduce its output thrust. Assuming T0 ≈ 0 at this point, the drone then satisfies 0 ≤ T. s ≤2T max The following expression is derived after the drone experiences a power failure:

[0042]

[0043] Further results were obtained:

[0044]

[0045] (1.3) Obstacle avoidance cost J c To ensure the generation of collision-free trajectories, an Euclidean symbolic distance field (ESDF) map is used to obtain the distances to adjacent obstacles and their corresponding gradient information; its expression is as follows:

[0046]

[0047] Where E(·) is the distance to the nearest obstacle obtained from the ESDF, d s It is the safety distance threshold; after the final trajectory is generated, the sampling points along the trajectory and their higher-order derivatives are sampled, and the flatness is differentiated to recover the expected complete state and input, which are used as the reference input for trajectory tracking control.

[0048] Furthermore, in step S4, the fault-tolerant controller based on nonlinear model predictive control is designed as follows:

[0049] Nonlinear model predictive control generates control laws by solving a finite-time optimal control problem in the rolling time domain; that is, by expressing the state... And control input u = [u1, u2, u3, u4] T Modeled as x in discrete time form k+1 =f(x) k ,u k For the reference trajectory, the cost function is the error between the reference state and the actual state within a finite time domain; N equal intervals t are used. N For each time interval ∈ [t, t+h], the step size is dt = h / N, where h represents the time interval length; therefore, the following constraint optimization expression is obtained:

[0050]

[0051] stx i+1 =f(x) i ,u i ), i = k, k+1, ..., k+N-1;

[0052]

[0053] Where k, N, x k:k+N Q, Q' represent the current time step, the sampling time step, and the predicted state trajectory, respectively; N R is the weight matrix; u , These are the upper and lower limits of the control law. w , These are the upper and lower limits of angular velocity; under normal flight conditions, the control input limits are set to... If the i-th power source fails and is detected by the fault detection module, the i-th element of u will be adjusted to...

[0054] The cost function consists of two parts: the first part is the accumulated running cost for each time step related to u and x, and the second part is the terminal cost depending on the terminal state; yaw control is performed using yaw torque, and the effectiveness of yaw control is about an order of magnitude lower than that of pitch and roll, making yaw control susceptible to motor saturation.

[0055] First, determine the attitude error:

[0056]

[0057] Where q r q is the reference pose, and q is the current pose;

[0058] The attitude error is decomposed into z and xy components:

[0059]

[0060] Construct the vector y of the running cost i And the diagonal weight matrix Q:

[0061]

[0062] Q = diag(Q) p Q xy Q z Q v Q w Q t Q u );

[0063] Q N =diag(Q p Q xy Q z Q v Q w Q t );

[0064] The reference value in the cost function is the hovering reference value or the sampling point in the trajectory at all time points in the prediction time domain; after the power failure diagnosis confirms the occurrence of the fault, the weight coefficient related to the yaw motion is set to zero, while the other weight coefficients remain unchanged; the thrust direction is forced to align with the reference, and the control of the yaw motion is abandoned.

[0065] Furthermore, when the drone's power fails, it is necessary to quickly diagnose the fault information. Based on the motor speed feedback information, the diagnosis is performed in the following way: First, according to the drone's dynamic equations, we know that:

[0066]

[0067] Where T f This is the estimated combined thrust, with each thrust calculated by combining the feedback motor speed with the thrust coefficient. This represents the acceleration estimated through a filter. Indicates the estimated external force; α represents the estimated external torque. B,f w B,f τ f Let represent the angular acceleration, angular velocity, and torque obtained through the filter, respectively; then, the sum of the external forces is solved, expressed as:

[0068]

[0069] Based on the mixing matrix, we obtain:

[0070]

[0071] in This represents the abnormal thrust values ​​among the four motors, combined with the control law t = [T0, T1, T2, T3] output by the controller. T The diagnosis will be performed according to the following rules:

[0072]

[0073] Therefore, when the ratio of the estimated thrust of a certain motor to the output of the controller exceeds the critical value of 0.8, the motor is considered to have suffered a power failure, and the UAV controller and planner will switch to fault-tolerant control mode.

[0074] Further, step S5 specifically involves: since the yaw torque becomes unbalanced and spins when the UAV's power fails; to reduce the torque generated by the rotation of each blade, i.e., to reduce the torque on the entire system when the blade fails, thereby reducing the system's spin speed; an aerodynamic structure is designed whose aerodynamic characteristics are related to the blade rotation direction. It is symmetrically installed on the UAV like the propeller, and can deflect the downwash airflow of the propeller to generate torque in the opposite direction to the propeller, which is used to offset part of the yaw torque generated by the propeller rotation, ultimately achieving control of the UAV system by the torque generated by the propeller rotation in the event of power failure.

[0075] According to a second aspect of the present invention, an electronic device is provided, characterized in that it comprises:

[0076] One or more processors;

[0077] Memory, used to store one or more programs;

[0078] When the one or more programs are executed by the one or more processors, the one or more processors implement the method for unmanned aerial vehicle motion planning and fault-tolerant control for power failure faults.

[0079] According to a third aspect of the present invention, a computer-readable storage medium is provided having computer instructions stored thereon, which, when executed by a processor, implement the steps of the aforementioned method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) oriented towards power failure faults.

[0080] The technical solutions provided by the embodiments of this application may include the following beneficial effects:

[0081] 1. This invention designs a position and attitude control algorithm applicable to failed unmanned aerial vehicles (UAVs), capable of solving for the desired attitude, thrust, and trajectory of the UAV given a preset target position. This method can improve the control accuracy and stability of UAVs in failure states and provides a technical foundation for position control and navigation of UAVs in failure states. Power failure is achieved by unloading one propeller blade.

[0082] 2. This invention aims to improve the controllability of unmanned aerial vehicles (UAVs) in failover conditions by designing a UAV navigation algorithm for denied environments. It achieves autonomous navigation in failover conditions without relying on external positioning and computing equipment, using only onboard sensors. Furthermore, the radar sensor used enables the UAV to navigate in strong light, low light, and outdoor environments.

[0083] 3. This invention designs a diagnostic algorithm that can quickly diagnose the location and extent of the failure of a UAV propeller blade after it has been damaged and lost power. It then integrates a fault-tolerant control algorithm into the UAV's control system, ensuring a safe and rapid transition from normal operation to fault-tolerant control after the propeller blades are damaged. In the experiment, the entire system's diagnostic time was 125ms, and it quickly recovered and continued flying along the predetermined trajectory after the failure diagnosis. Attached Figure Description

[0084] The accompanying drawings, which are incorporated herein by reference and constitute a part of this specification, illustrate embodiments consistent with this application and, together with the specification, serve to explain the principles of this application.

[0085] Figure 1 This is a structural diagram of an unmanned aerial vehicle (UAV) system.

[0086] Figure 2 This is a flowchart of the algorithm for a full-loop fault-tolerant system and method for powered failure UAVs based on lidar.

[0087] Figure 3 This is a schematic diagram of the forces acting on a drone during horizontal flight.

[0088] Figure 4 This is a diagram illustrating the physical injection failure mechanism of a drone.

[0089] Figure 5 This is a sequence diagram of an indoor trajectory tracking experiment for drones;

[0090] Figure 6 This is a 3D plot of the trajectory from an indoor drone trajectory tracking experiment.

[0091] Figure 7 This is a graph showing experimental data from an indoor drone trajectory tracking test.

[0092] Figure 8 This is a diagram of an indoor autonomous navigation experiment;

[0093] Figure 9 It includes the outdoor autonomous navigation experimental environment, point cloud, and trajectory map; Detailed Implementation

[0094] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application.

[0095] The terminology used in this application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. The singular forms “a,” “the,” and “the” used in this application and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0096] According to a first aspect of the present invention, a diagnostic, control, and motion planning system for unmanned aerial vehicles (UAVs) in the event of sudden power failure is proposed. This system can meet the control and intelligent navigation requirements of UAVs during sudden power failure. The system is designed to address the issues of abrupt attitude changes and insufficient control margin during power failure, ensuring the control stability and motion planning capabilities of the UAV under such circumstances.

[0097] The innovative aspects of the diagnostic, control, and motion planning system for unmanned aerial vehicles (UAVs) in the event of sudden power failure proposed in this invention include three points: 1. It can diagnose sudden power failures during normal flight of the UAV and initiate a fault-tolerant control state. 2. It enables the UAV to perform autonomous navigation in denied environments after a failure. 3. It designs a highly efficient dual-loop model predictive fault-tolerant controller.

[0098] This invention proposes a novel diagnostic, control, and motion planning system for unmanned aerial vehicles (UAVs) experiencing power failures. Its overall framework is as follows: Figure 2 As shown, the method includes the following steps.

[0099] S1: Perform dynamic modeling on a powered unmanned aerial vehicle (UAV).

[0100] S2: Based on dynamic modeling and analysis and UAV motion planning technology, motion planning and control algorithms were designed for failed UAVs. The calculated and optimized flight trajectory can be directly used as the input to the designed fault-tolerant controller, and the navigation function can work normally when the UAV is not failed.

[0101] S3: Controller design for a fault-tolerant control system for unmanned aerial vehicles (UAVs). By separating the yaw channel and modeling the inner and outer loops separately, a dual-loop model predictive controller was designed, resulting in a fault-tolerant controller that can operate stably when the UAV fails.

[0102] S4: Diagnostic method for drone failure. By designing an observer, the torque applied to the drone is observed to see if it is equivalent to the input control quantity. By comparing the input quantity with the observed actual torque, the failure status of the drone is determined, and the drone is diagnosed as having failed.

[0103] S5: Aerodynamic structure to reduce spin velocity in the event of drone failure. By designing an aerodynamic structure to deflect airflow, the torque change in the event of drone power failure is reduced, thereby reducing the drone's spin velocity.

[0104] The kinematics and dynamics model of the UAV is established, and its expression is as follows:

[0105]

[0106] I v α B =-w B ×I v w B +τ-A(w)+τ a ;

[0107] In the kinematic equations, m and ξ represent the mass and center of gravity vectors of the UAV, and T is the three-dimensional vector of the resultant thrust in the body coordinate system. a This is an interference force caused by model mismatch, where g represents the gravitational acceleration vector, and z... B This represents the last column of the rotation matrix. In rotational dynamics, This represents quaternion multiplication, × represents cross product, q represents quaternion, and w represents quaternion multiplication. B =[p,q,r] T α represents angular velocity. B I represents angular acceleration. v Let τ represent the inertia matrix, and let τ represent the torque generated by the thrust. α This represents the disturbance torque caused by model mismatch, A(w)=[0,0,-γw,] T This represents the aerodynamic torque generated by air resistance.

[0108] The combined thrust and the torque caused by the rotors depend on the thrust of the four rotors, and its expression is as follows:

[0109]

[0110] Where, ||T|| represents the magnitude of the resultant thrust vector, and t = [T0, T1, T2, T3] T G represents the thrust generated by the four motors, and G represents the hybrid control matrix.

[0111]

[0112] Where r x i and r y i represents the half-shaft arm r i In the body coordinate system, the x and y components, k t This represents the torque coefficient.

[0113] Because there is a motor speed response delay during actual flight of the drone, it is necessary to consider the motor dynamics model. A first-order model is used to represent the motor response dynamics:

[0114]

[0115] Where σ is the time constant calculated from experimental data, u i It is the input command for the i-th motor.

[0116] S2. Planning Algorithm under UAV Failure State This section describes the proposed flat trajectory used to generate a spatiotemporal trajectory with minimum control error in trajectory optimization.

[0117] MINCO is employed to generate the spatiotemporal trajectory with minimum control cost, achieving linear-complexity spatiotemporal deformation of the planar output trajectory; then, the complete state is recovered from the planar output; for an N-dimensional M-segment polynomial trajectory with N = 2s⁻¹, where s is the order of the optimal solution state, a linear-complexity mapping is constructed, expressed as follows:

[0118] c = M(q,T);

[0119] in It is a polynomial coefficient matrix, M(q,T) is the number of points from the path. and time allocation A smooth mapping to c; then optimizing the variable (q,T) to {q,T}; the i-th trajectory segment p i (t) is represented as In trajectory optimization, the cost function for each trajectory segment includes three constraints: flight time, state constraints, and obstacle avoidance.

[0120]

[0121] Where λ is the weight vector, used to weigh the various cost functions.

[0122] The flight time, state constraints, and obstacle avoidance constraints are specifically as follows:

[0123] (1) Flight time cost J t To accelerate the navigation process, we first minimize the total time; the expression is as follows:

[0124]

[0125] (2) State constraint cost J s Limit the magnitude of linear velocity and acceleration to ensure feasibility; the formula for the cost function is as follows:

[0126] J s =J s,v+J s,a ;

[0127]

[0128] Where v max and a max It is the threshold for linear velocity and acceleration; J s,v It is the constraint cost of the velocity term, J s,a It is the constraint cost of the acceleration term.

[0129] Furthermore, such as Figure 3 As shown, using the acceleration reachability analysis method, a trade-off is made between the residual maneuverability and trajectory optimization parameters in order to achieve the desired performance in z. I Maintaining balance along the z-axis, i.e., the drone's acceleration along the z-axis is 0, and ignoring horizontal air resistance, we obtain:

[0130]

[0131] Where a des It is the horizontal acceleration of the drone, T s This represents the resultant force of the four motors acting on the drone; the output thrust of each motor satisfies 0 ≤ T ≤ T. max T max This represents the maximum thrust output by a single motor; and 0 ≤ T s ≤4T max If a power failure occurs and motor #1 fails (T1 = 0), then to maintain the drone's balance, its opposite motor (motor #0) needs to reduce its output thrust. Assuming T0 ≈ 0 at this point, we can conclude that the drone satisfies 0 ≤ T. s ≤2T max The following expression is derived after the drone experiences a power failure:

[0132]

[0133] Further results were obtained:

[0134]

[0135] (3) Obstacle avoidance cost J c To ensure the generation of collision-free trajectories, an Euclidean symbolic distance field (ESDF) map is used to obtain the distances to adjacent obstacles and their corresponding gradient information; its expression is as follows:

[0136]

[0137] Where E(·) is the distance to the nearest obstacle obtained from the ESDF, d sIt is the safety distance threshold; after the final trajectory is generated, the sampling points along the trajectory and their higher-order derivatives are sampled, and the flatness is differentiated to recover the expected complete state and input, which are used as the reference input for trajectory tracking control.

[0138] S3, Controller Design of UAV Fault-Tolerant Control System

[0139] Nonlinear model predictive control generates control laws by solving a finite-time optimal control problem in the rolling time domain; that is, by expressing the state... And control input u = [u1, u2, u3, u4] T Modeled as x in discrete time form k+1 =f(x) k ,u k For the reference trajectory, the cost function is the error between the reference state and the actual state within a finite time domain; N equal intervals t are used. N For each time interval ∈ [t, t+h], the step size is dt = h / N, where h represents the time interval length; therefore, the following constraint optimization expression is obtained:

[0140]

[0141] stx i+1 =f(x) i ,u i ), i = k, k+1, ..., k+N-1;

[0142]

[0143] Where k, N, x k:k+N Q,Q represent the current time step, the sampling time step, and the predicted state trajectory, respectively. N R is the weight matrix; u , These are the upper and lower limits of the control law. w , These are the upper and lower limits of angular velocity; under normal flight conditions, the control input limits are set to... If the i-th power source fails and is detected by the fault detection module, the i-th element of u will be adjusted to...

[0144] The cost function comprises two parts: the first part is the accumulated runtime cost for each time step related to u and x, and the second part is the terminal cost dependent on the terminal state. Yaw control using yaw torque is about an order of magnitude less effective than pitch and roll control, making it susceptible to motor saturation.

[0145] First, determine the attitude error:

[0146]

[0147] Where q r q is the reference pose, and q is the current pose;

[0148] The attitude error is decomposed into z and xy components:

[0149]

[0150] Construct the vector y of the running cost i And the diagonal weight matrix Q:

[0151]

[0152] Q = diag(Q) p Q xy Q z Q v Q w Q t Q u );

[0153] Q N =diag(Q p Q xy Q z Q v Q w Q t );

[0154] The reference value in the cost function is the hovering reference value or the sampling point in the trajectory at all time points in the prediction time domain; after the power failure diagnosis confirms the occurrence of the fault, the weight coefficient related to the yaw motion is set to zero, while the other weight coefficients remain unchanged; the thrust direction is forced to align with the reference, and the control of the yaw motion is abandoned.

[0155] S4. UAV Fault Diagnosis Methods

[0156] Power failure is a common system malfunction that can occur unexpectedly due to propeller problems or other structural issues (such as motor arm or component breakage). They are particularly vulnerable because their actuators lack redundancy, making them susceptible to actuator failure. Effective control strategies implementing these proactive fault-tolerant methods to manage power failure require rapid detection of performance loss. Failure to identify and resolve power failure in a timely manner can lead to system instability or even catastrophic consequences. When power failure occurs, rapid fault diagnosis is necessary; therefore, based on motor speed feedback information, the following diagnostic scheme is adopted: First, according to the UAV dynamic equations, we know:

[0157]

[0158] Where T fThis is the estimated combined thrust, with each thrust calculated by combining the feedback motor speed with the thrust coefficient. This represents the acceleration estimated through a filter. Indicates the estimated external force; α represents the estimated external torque. B,f w B,f τ f Let represent the angular acceleration, angular velocity, and torque obtained through the filter, respectively; then the sum of the external forces can be solved, expressed as:

[0159]

[0160] Based on the mixing matrix, we obtain:

[0161]

[0162] in This represents the abnormal thrust values ​​among the four motors, combined with the control law t = [T0, T1, T2, T3] output by the controller. T The diagnosis will be performed according to the following rules:

[0163]

[0164] Therefore, when the ratio of the estimated thrust of a certain motor to the output of the controller exceeds the critical value of 0.8, the motor is considered to have suffered a power failure, and the UAV controller and planner will switch to fault-tolerant control mode.

[0165] S5. Aerodynamic structure for reducing the failure spin velocity of UAVs.

[0166] When a drone experiences power failure, it will spin due to an imbalance in yaw torque, and the upper limit of the spin speed is affected by the magnitude of the torque. For a single drone experiencing power failure, the magnitude of the torque it experiences is approximately the same as the torque generated by the rotation of two normally functioning diagonal propeller blades. Reducing the torque generated by the rotation of each propeller blade reduces the torque experienced by the entire system when a blade fails, thereby reducing the system's spin speed. Therefore, an aerodynamic structure was designed, whose aerodynamic characteristics are related to the direction of propeller rotation. It is symmetrically mounted on the drone, similar to the propeller, and can deflect the downwash airflow from the propeller, generating torque in the opposite direction to that of the propeller. This torque is used to counteract part of the yaw torque generated by the propeller rotation, ultimately reducing the impact of the propeller rotation torque on the drone system in the event of power failure. The aerodynamic structure is as follows: Figure 1 As shown.

[0167] This invention designs a position and attitude control algorithm applicable to failed unmanned aerial vehicles (UAVs), capable of solving for the desired attitude, thrust, and trajectory of the UAV given a preset target position. This method improves the control accuracy and stability of UAVs in failure states and provides a technical foundation for position control and navigation of UAVs in failure states. Power failure is achieved by unloading one propeller blade, such as... Figure 4 As shown. The experimental procedure is as follows. Figure 5 As shown, the flight trajectory is as follows Figure 6 As shown, the flight data is as follows Figure 7 As shown, the average positioning error during hovering is 0.056m, and the average tracking accuracy error during tracking is 0.153m.

[0168] This invention improves the controllability of unmanned aerial vehicles (UAVs) in failover conditions by designing a UAV navigation algorithm for denied environments. It achieves autonomous navigation in failover conditions using only onboard sensors, without relying on external positioning and computing equipment. Furthermore, the radar sensor used enables the UAV to navigate in strong light, low light, and outdoor environments; its navigation performance is as follows: Figure 8 , Figure 9 As shown.

[0169] According to a second aspect of the present invention, an electronic device is provided, characterized in that it comprises:

[0170] One or more processors;

[0171] Memory, used to store one or more programs;

[0172] When the one or more programs are executed by the one or more processors, the one or more processors implement the method for unmanned aerial vehicle motion planning and fault-tolerant control for power failure faults.

[0173] According to a third aspect of the present invention, a computer-readable storage medium is provided having computer instructions stored thereon, which, when executed by a processor, implement the steps of the aforementioned method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) oriented towards power failure faults.

[0174] Other embodiments of this application will readily occur to those skilled in the art upon consideration of the specification and practice of the disclosure herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein.

[0175] It should be understood that this application is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) facing dynamic failure, characterized in that, include: S1: Perform dynamic modeling on the unmanned aerial vehicle that has failed to power, and establish kinematic and dynamic models; S2: Based on the modeling results of S1, a motion planning method for unmanned aerial vehicles (UAVs) based on acceleration adaptive optimization is designed to achieve autonomous navigation and collision avoidance for failed UAVs. S3: By designing an observer, estimate whether the resultant thrust and torque on the UAV match the resultant thrust and torque output by the controller, and diagnose whether the UAV has suffered a failure. S4: Based on the fault diagnosis results of S3, a fault-tolerant controller based on nonlinear model predictive control is designed. When a fault occurs, the yaw channel control is abandoned to ensure control of the pitch, roll, and altitude channels; specifically: Nonlinear model predictive control generates control laws by solving a finite-time optimal control problem in the rolling time domain; that is, by expressing the state... and control input Modeled in discrete time as For the reference trajectory, the cost function is the error between the reference state and the actual state within a finite time domain; N equal intervals are used. Step size is ,in Let represent the length of the time period; therefore, the following constraint optimization expression is obtained: ; ; ;in These represent the current time step, the sampling time step, and the predicted state trajectory, respectively. It is a weight matrix; These are the upper and lower limits of the control law. These are the upper and lower limits of angular velocity; under normal flight conditions, the control input limits are set to... If the first A power failure was detected by the fault detection module. The Each element is adjusted to ; The cost function consists of two parts: the first part is related to... and The accumulated operating cost for each time step is the second part, which depends on the terminal state; yaw control is performed using yaw torque, but the effectiveness of yaw control is about an order of magnitude lower than that of pitch and roll, making yaw control susceptible to motor saturation. First, determine the attitude error: ;in It is a reference posture. This is the current stance; Decompose the attitude error into and Quantity: ； ； Construct a vector of runtime costs and diagonal weight matrix : ； ； ； The reference value in the cost function is the hovering reference value or the sampling point in the trajectory at all time points in the prediction time domain; after the power failure diagnosis confirms the occurrence of the fault, the weight coefficient related to yaw motion is set to zero, while the other weight coefficients remain unchanged; the thrust direction is forced to align with the reference, and the control of yaw motion is abandoned at the same time; S5: By designing an aerodynamic structure to deflect the airflow, the torque change when the UAV fails is reduced, thereby reducing the UAV's spin speed and ensuring accurate positioning and stable control of the UAV in the event of a failure.

2. The method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) oriented towards dynamic failure faults according to claim 1, characterized in that, In step S1, the kinematics and dynamics model of the UAV is established, and its expression is as follows: ; ; Among them, in the kinematic equations, and This represents the mass and center of gravity vector of the drone. It is the three-dimensional vector of the resultant thrust in the body coordinate system. The interference is caused by model mismatch. Represents the gravitational acceleration vector. This represents the last column of the rotation matrix; in rotational dynamics, To represent quaternion multiplication, This represents the cross product operation. Representing quaternions, Indicates angular velocity, Represents angular acceleration. Represents the inertia matrix. This represents the torque generated by the thrust. This indicates the disturbance torque caused by model mismatch. This represents the aerodynamic torque generated by air resistance; The combined thrust and the torque caused by the rotors depend on the thrust of the four rotors, and its expression is as follows: ； in, The magnitude of the resultant thrust vector. This represents the thrust generated by each of the four motors. Representing the control matrix: ;in and They are half-shaft arms In the body coordinate system and Quantity, Indicates the torque coefficient; Because there is a motor speed response delay during actual flight of the UAV, it is necessary to consider the motor dynamics model. A first-order model is used to represent the motor response dynamics, and the expression is as follows: ;in The time constant is calculated from experimental data. It is the first Input commands for each motor.

3. The method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) oriented towards dynamic failure faults according to claim 1, characterized in that, Step S2 specifically involves: using MINCO to generate a spatiotemporal trajectory with minimal control cost, achieving linear complexity spatiotemporal deformation of the planar output trajectory; then restoring the complete state from the planar output; for a given... of dimension Segment polynomial locus, where It is the order of the optimization solution state, which constructs a linear complexity mapping, as shown in the following expression: ;in It is a polynomial coefficient matrix. From the path point and time allocation arrive A smooth mapping; then optimize the variables. Convert to ;No. Segment trajectory fragment Represented as In trajectory optimization, the cost function for each trajectory segment includes three constraints: flight time, state constraints, and obstacle avoidance. ;in It is a weight vector used to make trade-offs among various cost functions.

4. The method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) oriented towards dynamic failure faults according to claim 3, characterized in that, The three constraints—flight time, state constraints, and obstacle avoidance—are specifically as follows: (4.1) Flight time cost To accelerate the navigation process, we first minimize the total time; the expression is as follows: ; (4.2) Cost of State Constraints Limit the magnitude of linear velocity and acceleration to ensure feasibility; the cost function formula is as follows: ; ; ;in and It is the threshold for linear velocity and acceleration; It is the constraint cost of the velocity term. It is the constraint cost of the acceleration term; Furthermore, acceleration reachability analysis is used to balance the remaining maneuverability and trajectory optimization parameters in order to maintain... For the balance along the z-axis, neglecting horizontal air resistance, and assuming the UAV's acceleration along the z-axis is 0, we obtain: ; in It is the horizontal acceleration of the drone. This represents the resultant force of the four motors acting on the drone; the output thrust of each motor satisfies... , This represents the maximum thrust output by a single motor; and ; If a power failure occurs, motor number 1 will fail. To maintain the drone's balance, its opposite motor, motor number 0, needs to reduce its output thrust. Let's assume that at this time... Then we get the condition that the drone satisfies the following: That is, after the drone experiences a power failure, the following expression is derived: Further, we obtain: ； (1.3) Obstacle Avoidance Cost To ensure the generation of collision-free trajectories, an Euclidean symbolic distance field (ESDF) map is used to obtain the distances to adjacent obstacles and their corresponding gradient information. Its expression is as follows: ; Where E(·) is the distance to the nearest obstacle obtained from ESDF. It is the safety distance threshold; after the final trajectory is generated, the sampling points along the trajectory and their higher-order derivatives are sampled, and the flatness is differentiated to recover the expected complete state and input, which are used as the reference input for trajectory tracking control.

5. The method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) oriented towards dynamic failure faults according to claim 1, characterized in that, When a drone experiences power failure, it is necessary to quickly diagnose the fault information. Based on the motor speed feedback information, the following method is used for diagnosis: First, according to the drone's dynamic equations, we know that: ; ;in This is the estimated combined thrust, with each thrust calculated by combining the feedback motor speed with the thrust coefficient. This represents the acceleration estimated through a filter. Indicates the estimated external force; This represents the estimated external torque. , , Let represent the angular acceleration, angular velocity, and torque obtained through the filter, respectively; then, the sum of the external forces is solved, expressed as: ; Based on the mixing matrix, we obtain: ;in This indicates the abnormal thrust values ​​among the four motors, combined with the control law output by the controller. The diagnosis will be performed according to the following rules; Therefore, when the ratio of the estimated thrust of a certain motor to the output of the controller exceeds the critical value of 0.8, the motor is considered to have experienced a power failure, and the UAV controller and planner will switch to fault-tolerant control mode.

6. The method for motion planning and fault-tolerant control of unmanned aerial vehicles (UAVs) oriented towards dynamic failure faults according to claim 1, characterized in that, Step S5 specifically involves the following: When the UAV's power fails, the yaw torque becomes unbalanced, causing it to spin. To reduce the torque generated by each blade's rotation, i.e., to reduce the torque experienced by the entire system when a blade fails, thereby reducing the system's spin speed, an aerodynamic structure is designed. Its aerodynamic characteristics are related to the blade's rotation direction. It is symmetrically installed on the UAV, just like the propeller, and can deflect the propeller's downwash airflow, generating torque in the opposite direction to the propeller. This torque is used to offset part of the yaw torque generated by the propeller's rotation, ultimately achieving control of the UAV system by the torque generated by the propeller's rotation in the event of power failure.

7. An electronic device, characterized in that, include: One or more processors; Memory, used to store one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the UAV motion planning and fault-tolerant control method for power failure faults as described in any one of claims 1-6.

8. A computer-readable storage medium storing computer instructions thereon, characterized in that, When executed by the processor, this instruction implements the steps of the UAV motion planning and fault-tolerant control method for power failure faults as described in any one of claims 1-6.

Images