Unmanned aerial vehicle motion planning and fault-tolerant control method for dynamic failure fault

Through dynamic modeling and nonlinear model prediction control, combined with aerodynamic structure deflection of airflow, the problem of rapid diagnosis and stable control of the drone under power failure failure is solved, and autonomous navigation and safe flight in complex environments are achieved.

CN119088085AActive Publication Date: 2024-12-06ZHEJIANG UNIV +1

Patent Information

Application Number
CN202411151042.5
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-21
Publication Date
2024-12-06
Estimated Expiration
2044-08-21

AI Technical Summary

Technical Problem

Existing UAV technology is difficult to achieve rapid fault diagnosis and fault-tolerant control after power failure, resulting in a decrease in control stability and it is difficult to ensure autonomous navigation and safe flight in complex environments.

Method used

A drone motion planning and fault-tolerant control method for dynamic failure is designed, and the rapid fault diagnosis and control of airflow through dynamic modeling, acceleration adaptive optimization motion planning, nonlinear model prediction control and aerodynamic structure deflection airflow are achieved. Stable control.

Benefits of technology

It achieves low fault diagnosis time and high diagnostic accuracy, improves the autonomous navigation and safe flight capabilities of the drone in power failure state, and ensures the success rate of stable control and fault-tolerant control in complex environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119088085A_ABST
    Figure CN119088085A_ABST
Patent Text Reader

Abstract

The invention provides an unmanned aerial vehicle motion planning and fault-tolerant control method for a dynamic failure fault. The method comprises the following steps: carrying out dynamic modeling on an unmanned aerial vehicle and designing an unmanned aerial vehicle motion planning method, so that autonomous navigation and collision avoidance are realized while the maneuverability descending constraint of the faulted unmanned aerial vehicle is met; whether the unmanned aerial vehicle has a power failure fault or not is diagnosed through a designed double-ring observer, and when the power failure fault occurs, a controller is reconstructed to realize fault-tolerant control; and a foot stand of the unmanned aerial vehicle is modified to design an anti-yaw moment plate, the downwash airflow of the rotor wings and the deflection airflow during high-speed rotation of the unmanned aerial vehicle are adjusted, and the yaw moment of the unmanned aerial vehicle acting on rotation movement is reduced. The problems of positioning mapping and collision avoidance caused by high-speed rotation of the fault unmanned aerial vehicle are solved, the requirements for rapid fault diagnosis and stable performance recovery of sudden faults are met, and the autonomous safety of actual flight of the unmanned aerial vehicle is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of UAV fault tolerance, and in particular relates to a UAV motion planning and fault tolerance control method for power failure. Background Art

[0002] In recent years, traditional UAV fault tolerance theory and methods have encountered new challenges, mainly manifested in the lack of autonomous navigation capabilities to cope with denied environments and the ability to quickly adjust to power failures. The current technical bottlenecks include: due to the decline in maneuverability, complex flight environment, limited visual perception distance, and limited onboard computing power, the challenges of optimal motion planning and fault-tolerant control of UAVs after power failures have further increased. In order to ensure the autonomy, safety, and reliability of the flight process of UAVs with power failures, their optimal motion planning, rapid fault diagnosis, and fault-tolerant control in denied environments are core topics that need to be studied urgently.

[0003] Considering that most existing motion planning methods are applicable to fully attitude controllable UAVs, how to design the optimal motion planning method for constraints such as degradation of UAV maneuverability, obstacle avoidance, and decreased positioning and mapping performance after power failure is a bottleneck problem that affects the improvement of its safety performance. Although current research work provides a certain theoretical basis for improving the flight safety of damaged UAVs, the current UAV fault diagnosis method under power failure in a denied environment is suitable for the fine estimation of the partial loss of actuator working efficiency, but there are few works that simultaneously meet the dual requirements of rapidity and accuracy of fault diagnosis under multiple power failure modes in practical applications. After the power failure occurs, the control stability of the UAV is seriously reduced. How to incorporate the UAV fault diagnosis results and the remaining power constraints into the controller design to improve the fault adjustment ability and fault-tolerant control success rate of the large-attitude motion of the UAV with power failure is still an urgent problem to be solved.

[0004] Based on the above problems, a full-loop fault-tolerant system of "motion planning" + "fault diagnosis + fault-tolerant control" with strong autonomy, fast response and high safety was invented to ensure that the UAV has the ability to autonomously navigate and fly safely in complex and unknown environments after a power failure occurs. The system has a low fault diagnosis time and a high diagnosis accuracy; a motion planning method based on spatiotemporal joint optimization and a highly reliable fault-tolerant control method. It can achieve rapid response and intelligent control of UAV power failure. Summary of the invention

[0005] In view of the shortcomings of the prior art, the present invention provides a method for UAV motion planning and fault-tolerant control for power failure faults. The method solves the problems of positioning and mapping, collision avoidance caused by the high-speed rotation of the faulty UAV, as well as the need for rapid fault diagnosis and stable recovery performance due to sudden faults, thereby improving the autonomous safety of the actual flight of the UAV.

[0006] The present invention is achieved through the following technical solutions:

[0007] According to a second aspect of the present invention, a method for motion planning and fault-tolerant control of a UAV facing power failure is provided, comprising the following steps:

[0008] S1: Dynamic modeling of the UAV with power failure and establishment of kinematic and dynamic models;

[0009] S2: Based on the modeling results of S1, a UAV motion planning method based on acceleration adaptive optimization is designed to

[0010] Realize autonomous navigation and collision avoidance of disabled drones;

[0011] S3: By designing an observer, it is estimated whether the combined thrust and torque of the UAV match the combined thrust and torque output by the controller, and whether the UAV suffers from a failure.

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

[0013] In case of failure, the control of the yaw channel is abandoned to ensure the control of the three channels of pitch, roll and altitude;

[0014] S5: By designing aerodynamic structures to deflect airflow, the torque change when the UAV power fails can be 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, the kinematic and dynamic models of the UAV are established, and the expressions thereof are as follows:

[0016]

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

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

[0019] The combined thrust and torque caused by the rotors depends on the thrust of the four rotors and is expressed as follows:

[0020]

[0021] Where ‖T‖ represents the modulus of the resultant thrust vector, t = [T0, T1, T2, T3] T Represents the thrust generated by the four motors respectively, and G represents the mixing matrix

[0022]

[0023] where r x ,i and r y ,i are the half-axis arms r i The x and y components in the body coordinate system, k t represents the moment coefficient;

[0024] Since the UAV has a motor speed response delay during actual flight, the motor dynamics model needs to be considered. The first-order model is used to represent the motor response dynamics. The expression is as follows:

[0025]

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

[0027] Furthermore, the step S2 is specifically as follows: MINCO is used to generate the space-time trajectory with the minimum control cost, and the linear complexity space-time deformation of the plane output trajectory is realized; then the complete state is restored from the plane output; for an N-dimensional M-segment polynomial trajectory with N=2s-1, where s is the order of the optimal solution state, a linear complexity mapping is constructed, and the expression is as follows:

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

[0029] in is the polynomial coefficient matrix, M(q,T) is the matrix from path points and time allocation to c; then the optimization variable (q,T) is transformed into {q,T}; the i-th trajectory segment p i (t) is expressed as In trajectory optimization, the cost function of each trajectory includes three constraints: flight time, state constraints, and obstacle avoidance:

[0030]

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

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

[0033] (4.1) Flight time cost J t : In order to speed up 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 linear velocity and acceleration to ensure feasibility; the formula of the cost function is as follows:

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

[0037]

[0038] where v max and a max is the threshold value of linear velocity and acceleration; J s,v is the constraint cost of the speed term, J s,a is the constraint cost of the acceleration term;

[0039] Furthermore, the acceleration reachability analysis is used to balance the remaining maneuverability and trajectory optimization parameters to maintain the z I The balance in the axis direction, ignoring the horizontal air resistance, the acceleration of the drone in the z-axis is 0, then we get:

[0040]

[0041] where a des is the horizontal acceleration of the drone, T s Represents the combined force of the four motors acting on the drone; the output thrust of each motor satisfies 0≤T≤T max , T max Indicates the maximum thrust output by a single motor; and 0≤T s ≤4Tmax ; If a power failure occurs, the No. 1 motor fails T1 = 0, then in order to ensure the balance of the drone, the opposite motor, that is, the No. 0 motor, needs to reduce the output thrust. Assuming that T0≈0 at this time, the drone satisfies 0≤T s ≤2T max , that is, after the UAV fails to function, the following expression is derived:

[0042]

[0043] Further we get:

[0044]

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

[0046]

[0047] where E(·) is the distance to the nearest obstacle obtained from ESDF, d s is the safety distance threshold; after the final trajectory is generated, the sampling points and their high-order derivatives along the trajectory are taken, and the differential flatness is used to restore the desired complete state and input as the reference input for trajectory tracking control.

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

[0049] The control law is generated by solving the finite-time optimal control problem in the receding time domain using nonlinear model predictive control; that is, by expressing the state And control input u=[u1,u2,u3,u4] T , modeled in discrete time form as x 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 in the finite time domain; N steps are equally spaced t N ∈[t,t+h], the step size is dt=h / N, where h represents the length of the time period; therefore, the following constrained 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 Represent the current time step, sampling time step and predicted state trajectory respectively; Q, Q N ,R is the weight matrix; u , are the upper and lower limits of the control law, w , are the upper and lower limits of the angular velocity; under normal flight conditions, the limits on control input are set to If the i-th power fails and is identified by the fault detection module, the i-th element of u is adjusted to

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

[0055] First determine the attitude error:

[0056]

[0057] where q r is the reference posture, q is the current posture;

[0058] Decompose the attitude error into z and xy components:

[0059]

[0060] Construct the vector y of 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 at all time points in the predicted time domain or the sampling points in the trajectory; after the power failure diagnosis confirms the failure, the weight coefficient related to the yaw motion is set to zero, and the other weight coefficients remain unchanged; the thrust direction is forced to be aligned with the reference, and the control of the yaw motion is abandoned.

[0065] Furthermore, when the UAV power fails, 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 UAV dynamics equation:

[0066]

[0067] Where T f is the estimated total thrust, each thrust is obtained by combining the feedback motor speed with the thrust coefficient. represents the acceleration estimated by the filter, represents the estimated external force; represents the estimated external moment, α B,f 、w B,f , τ f They represent the angular acceleration, angular velocity, and torque obtained by the filter respectively; and then the sum of the external forces is solved, which is expressed as:

[0068]

[0069] According to the mixing matrix, we can get:

[0070]

[0071] in Indicates the abnormal value of thrust in the four motors, combined with the control law output by the controller t=[T0,T1,T2,T3] T , the diagnosis is implemented according to the following rules:

[0072]

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

[0074] Furthermore, the step S5 is specifically as follows: when the power of the UAV fails, the yaw torque will be unbalanced and the UAV will enter spin; in order to reduce the torque generated by the rotation of each blade, that is, to reduce the torque applied to the entire system when the blade fails, thereby reducing the spin speed of the system; and then an aerodynamic structure is designed, whose aerodynamic characteristics are related to the rotation direction of the blades. The structure is symmetrically installed on the UAV like a propeller, and can deflect the downwash airflow of the propeller to generate a torque in the opposite direction of the propeller, which is used to offset a part of the yaw torque generated by the rotation of the propeller, and finally realize the control of the UAV system by the torque generated by the rotation of the propeller in the case of power failure.

[0075] According to a second aspect of the present invention, there is provided an electronic device, comprising:

[0076] one or more processors;

[0077] A memory for storing 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 of UAV motion planning and fault-tolerant control for power failure.

[0079] According to a third aspect of the present invention, there is provided a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the steps of the method for UAV motion planning and fault-tolerant control for power failure faults.

[0080] The technical solution provided by the embodiments of the present application may have the following beneficial effects:

[0081] 1. The present invention designs a position attitude control algorithm that can be applied to a failed UAV, which can solve the expected attitude, thrust and trajectory of the UAV by giving a preset target position. This method can improve the control accuracy and stability of the UAV in a failed state, and provide a technical basis for the position control and navigation of the UAV in a failed state. The power failure is achieved by unloading one blade.

[0082] 2. In order to improve the controllability of the drone in a failed state, the present invention designs a drone navigation algorithm in a denied environment. Autonomous navigation of the drone in a failed state can be achieved only through onboard sensors without relying on external positioning and computing equipment. In addition, the radar sensor used enables the drone to navigate in strong light, dark light, and outdoor environments.

[0083] 3. The present invention designs a diagnostic algorithm that can quickly diagnose the position and degree of failure of the UAV blades after the UAV blades are damaged and lose power, and enables the fault-tolerant control algorithm to intervene in the UAV control to ensure that the UAV blades are safely and quickly transferred from the normal state to the fault-tolerant control state after being damaged. In the experiment, the diagnosis time of the entire system is 125ms, and it quickly recovers after failure diagnosis and continues to fly according to the predetermined trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0085] Figure 1 It is the structure diagram of the UAV system;

[0086] Figure 2 It is a flowchart of the algorithm of the full-loop fault-tolerant system and method of the UAV with power failure based on laser radar;

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

[0088] Figure 4 This is the way to physically inject failure into a drone.

[0089] Figure 5 This is a sequence diagram of the indoor trajectory tracking experiment of a drone;

[0090] Figure 6 It is a three-dimensional graph of the indoor trajectory tracking experiment trajectory of the UAV;

[0091] Figure 7 This is the indoor trajectory tracking experimental data graph of the drone;

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

[0093] Fig. 9 It is the outdoor autonomous navigation experimental environment, point cloud, and trajectory map; DETAILED DESCRIPTION

[0094] Here, exemplary embodiments are described in detail, and examples thereof are shown in the accompanying drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application.

[0095] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "said" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0096] According to the first aspect of the present invention, the present invention proposes a diagnosis, control and motion planning system for sudden power failure of UAVs, which can meet the control requirements and intelligent navigation requirements of UAVs in the event of sudden power failure. The design is based on the problem of sudden attitude change and insufficient control margin when the UAV power fails, so as to ensure the control stability and motion planning ability of the UAV in this case.

[0097] The innovative features of the diagnosis, control and motion planning system for sudden power failure of UAVs proposed in this invention include three points: 1. It can diagnose sudden power failure of UAVs during normal flight and enter the fault-tolerant control state. 2. It can enable UAVs to perform autonomous navigation in a denied environment after failure. 3. A set of efficient dual-loop model predictive fault-tolerant controllers is designed.

[0098] The present invention proposes a novel diagnosis, control and motion planning system for UAV power failure, and its overall framework is as follows: Figure 2 As shown, the method includes the following steps.

[0099] S1: Dynamic modeling of a powered-off drone.

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

[0101] S3: Controller design for the fault-tolerant control system of the UAV. By separating the yaw channel and modeling the inner and outer loops respectively, a dual-loop model predictive controller is designed. A fault-tolerant controller that can work stably when the UAV fails is designed.

[0102] S4: A diagnostic method for UAV failure is to design an observer to observe whether the torque applied to the UAV is equivalent to the input control quantity. The failure of the UAV is determined by comparing the input quantity with the observed actual torque to diagnose whether the UAV has failed.

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

[0104] The kinematic and dynamic models of the UAV are established, and their expressions are as follows:

[0105]

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

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

[0108] The combined thrust and torque caused by the rotors depends on the thrust of the four rotors and is expressed as follows:

[0109]

[0110] Where ‖T‖ represents the modulus of the resultant thrust vector, t = [T0, T1, T2, T3] T Represents the thrust generated by the four motors respectively, and G represents the mixing matrix

[0111]

[0112] where r x ,i and r y ,i are the half-axis arms r i The x and y components in the body coordinate system, k t Represents the torque coefficient.

[0113] Since the UAV has a motor speed response delay during actual flight, the motor dynamics model needs to be considered. The first-order model is used to represent the motor response dynamics:

[0114]

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

[0116] S2. Planning Algorithm for Failed UAVs This section illustrates the proposed flat trajectory for generating spatiotemporal trajectories with minimum control errors in trajectory optimization.

[0117] MINCO is used to generate the space-time trajectory with the minimum control cost, and the linear complexity space-time deformation of the plane output trajectory is realized; then the complete state is restored from the plane output; for an N-dimensional M-segment polynomial trajectory with N=2s-1, where s is the order of the optimal solution state, a linear complexity mapping is constructed, which is expressed as follows:

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

[0119] in is the polynomial coefficient matrix, M(q,T) is the matrix from path points and time allocation to c; then the optimization variable (q,T) is transformed into {q,T}; the i-th trajectory segment p i (t) is expressed as In trajectory optimization, the cost function of each trajectory includes three constraints: flight time, state constraint, and obstacle avoidance.

[0120]

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

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

[0123] (1) Flight time cost J t : In order to speed up the navigation process, we first minimize the total time; the expression is as follows:

[0124]

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

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

[0127]

[0128] where v max and a max is the threshold value of linear velocity and acceleration; J s,v is the constraint cost of the speed term, J s,a is the constraint cost of the acceleration term.

[0129] Furthermore, if Figure 3 As shown in the figure, the acceleration reachability analysis method is used to balance the remaining maneuverability and trajectory optimization parameters in order to I Keep the balance in the axis direction, that is, the acceleration of the drone in the z-axis is 0, and ignore the air resistance in the horizontal direction, then we get:

[0130]

[0131] where a des is the horizontal acceleration of the drone, T s Represents the combined force of the four motors acting on the drone; the output thrust of each motor satisfies 0≤T≤T max , T max Indicates the maximum thrust output by a single motor; and 0≤T s ≤4T max ; If a power failure occurs, the No. 1 motor fails T1 = 0, then in order to ensure the balance of the drone, the opposite motor, that is, the No. 0 motor, needs to reduce the output thrust. Assuming that T0≈0 at this time, it can be obtained that the drone satisfies 0≤T s ≤2T max , that is, after the UAV fails to function, the following expression is derived:

[0132]

[0133] Further we get:

[0134]

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

[0136]

[0137] where E(·) is the distance to the nearest obstacle obtained from ESDF, d sis the safety distance threshold; after the final trajectory is generated, the sampling points and their high-order derivatives along the trajectory are taken, and the differential flatness is used to restore the desired complete state and input as the reference input for trajectory tracking control.

[0138] S3. Controller design for fault-tolerant control system of UAV

[0139] The control law is generated by solving the finite-time optimal control problem in the receding time domain using nonlinear model predictive control; that is, by expressing the state And control input u=[u1,u2,u3,u4] T , modeled in discrete time form as x 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 in the finite time domain; N steps are equally spaced t N ∈[t,t+h], the step size is dt=h / N, where h represents the length of the time period; therefore, the following constrained 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 Represent the current time step, sampling time step and predicted state trajectory respectively. Q,Q N ,R is the weight matrix; u , are the upper and lower limits of the control law, w , are the upper and lower limits of the angular velocity; under normal flight conditions, the limits on control input are set to If the i-th power fails and is identified by the fault detection module, the i-th element of u is adjusted to

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

[0145] First determine the attitude error:

[0146]

[0147] where q r is the reference posture, q is the current posture;

[0148] Decompose the attitude error into z and xy components:

[0149]

[0150] Construct the vector y of 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 at all time points in the predicted time domain or the sampling points in the trajectory; after the power failure diagnosis confirms the failure, the weight coefficient related to the yaw motion is set to zero, and the other weight coefficients remain unchanged; the thrust direction is forced to be aligned with the reference, and the control of the yaw motion is abandoned.

[0155] S4. UAV fault diagnosis method

[0156] Power failure is a common system failure that may occur unexpectedly due to propeller problems or other structural problems (such as broken motor arms or components). UAVs are particularly vulnerable because their actuators lack redundancy, making them susceptible to actuator failures. In order to manage power failure, effective control strategies that implement these active fault-tolerant methods require rapid detection of the loss of effectiveness. If power failure is not identified and resolved in time, it may lead to system instability or even catastrophic consequences. When power fails, fault information needs to be diagnosed quickly. Therefore, based on the motor speed feedback information, the following diagnostic scheme is adopted; first, according to the UAV dynamics equation:

[0157]

[0158] Where T fis the estimated total thrust, each thrust is obtained by combining the feedback motor speed with the thrust coefficient. represents the acceleration estimated by the filter, represents the estimated external force; represents the estimated external moment, α B,f 、w B,f , τ f They represent the angular acceleration, angular velocity, and torque obtained by the filter respectively; and then the sum of the external forces can be solved, which is expressed as:

[0159]

[0160] According to the mixing matrix, we can get:

[0161]

[0162] in Indicates the abnormal value of thrust in the four motors, combined with the control law output by the controller t=[T0,T1,T2,T3] T , the diagnosis is implemented according to the following rules:

[0163]

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

[0165] S5. Aerodynamic structure for reducing the spin speed of unmanned aerial vehicle failure

[0166] When the power of a drone fails, it will enter a spin due to an unbalanced yaw torque, and the upper limit of the spin speed is affected by the size of the torque. For a drone with a single power failure, the torque it is subject to is approximately the same as the torque generated by the rotation of two normally working diagonal blades. Reducing the torque generated by the rotation of each blade can reduce the torque applied to the entire system when the blade fails, thereby reducing the spin speed of the system. Therefore, an aerodynamic structure is designed, whose aerodynamic characteristics are related to the direction of blade rotation. It is installed symmetrically on the drone like a propeller, and can deflect the propeller's downwash airflow to generate a torque in the opposite direction of the propeller, which is used to offset a portion of the yaw torque generated by the propeller rotation, ultimately reducing the impact of the torque generated by the propeller rotation on the drone system in the event of power failure. Aerodynamic structures such as Figure 1 shown.

[0167] The present invention designs a position attitude control algorithm that can be applied to a failed UAV, which can solve the expected attitude, thrust and trajectory of the UAV by giving a preset target position. This method can improve the control accuracy and stability of the UAV in a failed state, and provide a technical basis for the position control and navigation of the UAV in a failed state. The power failure is achieved by unloading a blade, such as Figure 4 The experimental process is as shown. Figure 5 As shown, the flight trajectory is Figure 6 As shown, the flight data is as follows Figure 7 As shown in the figure, the average positioning error during hovering is 0.056m, and the average tracking accuracy error during tracking is 0.153m.

[0168] In order to improve the controllability of drones in a failed state, the present invention designs a drone navigation algorithm in a denied environment. Autonomous navigation of drones in a failed state can be achieved only through onboard sensors without relying on external positioning and computing equipment. The radar sensor used enables the drone to navigate in strong light, dark light, and outdoor environments; the navigation performance is as follows Figure 8 , Fig. 9 shown.

[0169] According to a second aspect of the present invention, there is provided an electronic device, comprising:

[0170] one or more processors;

[0171] A memory for storing 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 of UAV motion planning and fault-tolerant control for power failure.

[0173] According to a third aspect of the present invention, there is provided a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the steps of the method for UAV motion planning and fault-tolerant control for power failure faults.

[0174] Those skilled in the art will readily appreciate other embodiments of the present application after considering the specification and practicing the contents disclosed herein. The present application is intended to cover any variations, uses or adaptations of the present application, which follow the general principles of the present application and include common knowledge or customary technical means in the art that are not disclosed in the present application.

[0175] It will be appreciated that the present application is not limited to the exact construction that has been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof.

Claims

1. A UAV motion planning and fault-tolerant control method for power failure. Features include: S1: Dynamic modeling of the UAV with power failure and establishment of kinematic and dynamic models; S2: Based on the modeling results of S1, a UAV motion planning method based on acceleration adaptive optimization is designed to achieve autonomous navigation and collision avoidance of disabled UAVs; S3: By designing an observer, it is estimated whether the combined thrust and torque of the UAV match the combined thrust and torque output by the controller, and whether the UAV suffers from 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 the control of the three channels of pitch, roll and altitude. S5: By designing aerodynamic structures to deflect airflow, the torque change when the UAV power fails can be 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. According to claim 1, a method for UAV motion planning and fault-tolerant control facing power failure, It is characterized in that In step S1, the kinematic and dynamic models of the UAV are established, and the expressions are as follows: I v a B =-w B =I v w B +τ-A(w)+τ a ; In the kinematic equation, m and ξ represent the mass and center of gravity position vector of the UAV, T is the three-dimensional vector of the resultant thrust in the body coordinate system, T a is the disturbance force caused by model mismatch, g represents the gravitational acceleration vector, z B represents the last column of the rotation matrix; in rotational dynamics, represents quaternion multiplication, × represents cross multiplication, q represents quaternion, w B =[p,q,r] T represents the angular velocity, α B represents angular acceleration, I v represents the inertia matrix, τ represents the torque generated by the thrust, τ α represents the disturbance torque due to model mismatch, A(w)=[0,0,-γw,] T Represents the aerodynamic torque caused by air resistance; The combined thrust and torque caused by the rotors depends on the thrust of the four rotors and is expressed as follows: Where, ‖T‖ represents the modulus of the combined thrust vector, t = ,T 0 ,T 1 ,T 2 ,T 3 - T Represents the thrust generated by the four motors respectively, and G represents the mixing matrix where r x ,i and r y ,i are the half-axis arms r i The x and y components in the body coordinate system, k t represents the moment coefficient; Since the UAV has a motor speed response delay during actual flight, the motor dynamics model needs to be considered. The first-order model is used to represent the motor response dynamics. The expression is as follows: i=0,1,2,3; Where σ is the time constant calculated from experimental data, u i is the input command of the i-th motor.

3. According to claim 1, a method for UAV motion planning and fault-tolerant control facing power failure, It is characterized in that The step S2 is specifically as follows: MINCO is used to generate a space-time trajectory with minimum control cost, and a linear complexity space-time deformation of the plane output trajectory is realized; then the complete state is restored from the plane output; for an N-dimensional M-segment polynomial trajectory with N=2s-1, where s is the order of the optimal solution state, a linear complexity mapping is constructed, and the expression is as follows: c=M(q,T); in is the polynomial coefficient matrix, M(q,T) is the matrix from path points and time allocation to c; then the optimization variable (q,T) is transformed into *q,T+; the i-th trajectory segment p i (t) is expressed as j=0,1,…,k i -1; In trajectory optimization, the cost function of each trajectory includes three constraints: flight time, state constraint, and obstacle avoidance: Where λ is a weight vector used to weigh the various cost functions.

4. According to claim 3, a method for UAV motion planning and fault-tolerant control facing power failure, It is characterized in that The three constraints of flight time, state constraint and obstacle avoidance are specifically: (4.1) Flight time cost J t : In order to speed up the navigation process, we first minimize the total time; the expression is as follows: (4.2) State constraint cost J s :Limit the linear velocity and acceleration to ensure feasibility; the formula of the cost function is as follows: I s =J s,v +J s,a ; where v max and a max is the threshold value of linear velocity and acceleration; J s,v is the constraint cost of the speed term, J s,a is the constraint cost of the acceleration term; Furthermore, the acceleration reachability analysis is used to balance the remaining maneuverability and trajectory optimization parameters to maintain the z I The balance in the axis direction, ignoring the horizontal air resistance, the acceleration of the drone in the z-axis is 0, then we get: where a des is the horizontal acceleration of the drone, T s Represents the combined force of the four motors acting on the drone; the output thrust of each motor satisfies 0≤T≤T max , T max Indicates the maximum thrust output by a single motor; and 0≤T s ≤4T max ; If a power failure occurs, the No. 1 motor fails T 1 = 0, then in order to ensure the balance of the drone, the opposite motor, i.e. motor No. 0, needs to reduce its output thrust. Assume that T 0 ≈0, then the drone satisfies 0≤T s ≤2T max , that is, after the UAV fails to function, the following expression is derived: Further we get: (1.3) Obstacle avoidance cost J c : In order to ensure the generation of collision-free trajectories, the Euclidean signed distance field ESDF map is used to obtain the distances and corresponding gradient information of adjacent obstacles; Its expression is as follows: where E(·) is the distance to the nearest obstacle obtained from ESDF, d s is the safety distance threshold; after the final trajectory is generated, the sampling points and their high-order derivatives along the trajectory are taken, and the differential flatness is used to restore the desired complete state and input as the reference input for trajectory tracking control.

5. According to claim 1, a method for UAV motion planning and fault-tolerant control facing power failure, It is characterized in that In step S4, a fault-tolerant controller based on nonlinear model predictive control is designed, specifically: The control law is generated by solving the finite-time optimal control problem in the receding time domain using nonlinear model predictive control; that is, by expressing the state and control input u=,u 1 ,u 2 ,u 3 ,u 4 - T , modeled in discrete time form as x 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 in the finite time domain; N steps are equally spaced t N ∈[t,t+h], the step size is dt=h / N, where h represents the length of the time period; therefore, the following constrained optimization expression is obtained: s.t.x i+1 =f(x i ,u i ),i=k,k+1,…,k+N-1; where k, N, x k:k+N Represent the current time step, sampling time step and predicted state trajectory respectively; Q, Q N ,R is the weight matrix; u , are the upper and lower limits of the control law, w , are the upper and lower limits of the angular velocity; under normal flight conditions, the limits on control input are set to If the i-th power fails and is identified by the fault detection module, the i-th element of u is adjusted to The cost function includes two parts: the first part is the running cost accumulated at each time step related to u and x, and the second part is the terminal cost dependent on the terminal state; the yaw control is performed using the yaw moment, and the effectiveness of the yaw control is about one order of magnitude lower than that of the pitch and roll, making the yaw control susceptible to motor saturation; First determine the attitude error: where q r is the reference posture, q is the current posture; Decompose the attitude error into z and xy components: Construct the vector y of running cost i And the diagonal weight matrix Q: Q=diag(Q p ,Q xy ,Q z ,Q v ,Q w ,Q t ,Q u ); Q N =diag(Q p ,Q xy ,Q z ,Q v ,Q w ,Q t ); The reference value in the cost function is the hovering reference value at all time points in the predicted time domain or the sampling points in the trajectory; after the power failure diagnosis confirms the failure, the weight coefficient related to the yaw motion is set to zero, and the other weight coefficients remain unchanged; the thrust direction is forced to be aligned with the reference, and the control of the yaw motion is abandoned.

6. The method for motion planning and fault-tolerant control of a UAV facing power failure according to claim 1, It is characterized in that When the UAV power fails, 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 UAV dynamics equation: Where T f is the estimated total thrust, each thrust is obtained by combining the feedback motor speed with the thrust coefficient. represents the acceleration estimated by the filter, represents the estimated external force; represents the estimated external moment, α B,f 、w B,f , τ f They represent the angular acceleration, angular velocity, and torque obtained by the filter respectively; and then the sum of the external forces is solved, which is expressed as: According to the mixing matrix, we can get: in Indicates the abnormal value of thrust in the four motors, combined with the control law output by the controller t=,T 0 ,T 1 ,T 2 ,T 3 - T , the diagnosis is implemented according to the following rules: Therefore, when the ratio of the estimated thrust corresponding to a motor to the controller output exceeds the critical value of 0.8, it is considered that the motor has a power failure, and the UAV controller and planner will switch to the fault-tolerant control mode.

7. The method for motion planning and fault-tolerant control of a UAV facing power failure according to claim 1, It is characterized in that The step S5 is specifically as follows: when the power of the UAV fails, the yaw torque will be unbalanced and the UAV will enter autospin; in order to reduce the torque generated by the rotation of each blade, that is, to reduce the torque applied to the entire system when the blade fails, thereby reducing the autospin speed of the system; and then an aerodynamic structure is designed, whose aerodynamic characteristics are related to the rotation direction of the blades. The structure is symmetrically installed on the UAV like the propeller, and can deflect the downwash airflow of the propeller to generate a torque in the opposite direction of the propeller, which is used to offset a part of the yaw torque generated by the rotation of the propeller, and finally realize the control of the UAV system by the torque generated by the rotation of the propeller in the case of power failure.

8. An electronic device, It is characterized in that include: one or more processors; A memory for storing 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 method according to any one of claims 1 to 7.

9. A computer-readable storage medium having computer instructions stored thereon, It is characterized in that When the instruction is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Citation Information

Patent Citations

  • Lifting and pushing integrated power device and application method of lifting and pushing integrated power device on aircraft

    CN110901896A

  • Unmanned aerial vehicle anti-disturbance control method without airflow angle measurement in composite wind field environment

    CN116859752A

  • V / STOL biplane aircraft

    US20030136873A1

  • Adaptive control method for unmanned vehicle with slung load

    US9146557B1

Cited By

  • Multi-rotor unmanned aerial vehicle paddle breaking fault rapid diagnosis and fault-tolerant stable control system and method

    CN121070053A