A flight trajectory planning and attitude control system of a forest rat-eliminating unmanned aerial vehicle

Abstract

The present application relates to the technical field of unmanned aerial vehicle flight control, in particular to a flight trajectory planning and attitude control system of a forest rat-eliminating unmanned aerial vehicle, comprising: a state sensing step, obtaining flight state data and environmental disturbance characteristics; a margin quantization step, constructing a nonlinear dynamics envelope for the flight state data to generate a real-time attitude recovery margin; an instruction pre-rehearsal step, performing future state deduction on the captured trajectory instruction for the body dynamics model to generate a predicted margin consumption rate; a fuse decision step, combining the real-time margin and consumption rate to predict the remaining margin, and comparing with the dynamic safety lower limit to determine whether to trigger the fuse; a dual-mode execution module, used for executing the escape maneuver trajectory generated to recover the aerodynamic control right if the control fuse instruction is not triggered; the present application solves the game between task greed and physical survival robustness, prevents the unmanned aerial vehicle from entering the irreversible aerodynamic stall area, and significantly reduces the crash risk in the complex forest environment.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) flight control technology, specifically to a flight trajectory planning and attitude control system for a forest rodent control UAV. Background Technology

[0002] In the unstructured operating environment beneath the forest canopy, drones performing biological control tasks need to cope with high-density obstacles and unsteady strong aerodynamic interference such as the Venturi effect between trees; existing flight control schemes usually focus on the precise response to upper-level capture commands in order to pursue high-efficiency moving target tracking.

[0003] This strategy often lacks real-time perception of the physical and dynamic boundaries of the aircraft. Blindly executing aggressive pursuit maneuvers under extreme disturbances can lead to an imbalance between mission greed and physical survivability robustness. Due to the lack of a mechanism to predict the risk of losing control in the future, the drone is very likely to fall into an irreversible stall or rollover state due to overextension of control capabilities, which can lead to a crash.

[0004] Therefore, how to quantify the attitude recovery capability of the aircraft in real time under strong interference environment, and establish a command circuit breaker and adaptive survival control mechanism based on dynamic safety boundary, so as to significantly reduce the risk of crash while ensuring operational efficiency, is an urgent technical problem to be solved. Summary of the Invention

[0005] To solve the above-mentioned technical problems, the present invention provides a flight trajectory planning and attitude control system for a forest rodent control drone. Specifically, the technical solution of the present invention includes:

[0006] The state perception module is used to monitor the UAV's airframe dynamics and external environmental disturbances in real time, and to acquire flight state data and environmental disturbance characteristics.

[0007] The margin quantization module is used to construct a nonlinear dynamic envelope based on flight state data, calculate the distance of the current state relative to the dynamic runaway boundary, and generate real-time attitude recovery margin.

[0008] The instruction pre-simulation module is used to receive the capture trajectory instruction generated by the upper-level planning algorithm, and based on the body dynamics model, to perform state extrapolation on the future time window after the execution of the capture trajectory instruction and generate the expected margin consumption rate.

[0009] The circuit breaker decision module is used to combine the real-time attitude recovery margin with the expected margin consumption rate, predict the remaining attitude recovery margin at the end of the instruction execution, and compare the remaining attitude recovery margin with the dynamic safety lower limit to determine whether to trigger the control circuit breaker instruction.

[0010] The dual-mode execution module is used to execute the capture trajectory command through the underlying controller if the control circuit breaker command is not triggered; if the control circuit breaker command is triggered, the capture trajectory command is forcibly rejected, and an escape maneuver trajectory aimed at restoring aerodynamic control is generated and executed.

[0011] Preferably, methods for acquiring flight status data and environmental disturbance characteristics include:

[0012] The airborne inertial measurement unit collects high-frequency angular velocity and acceleration data of the aircraft.

[0013] Collect spatial distribution data of surrounding obstacles using visual sensors;

[0014] Input the angular velocity, acceleration data and spatial distribution data into the preset state observer to perform residual estimation of the airflow disturbance term;

[0015] Based on the residual estimation results, the unsteady aerodynamic components caused by the environmental wind field are separated, and the unsteady aerodynamic components are used as environmental disturbance characteristics. The motion parameters after removing the disturbance are used as flight state data.

[0016] Preferably, the method for generating real-time attitude recovery margin includes:

[0017] Based on the physical constraint parameters of the UAV, a multidimensional reachable set boundary is constructed in phase space, and the reachable set boundary is defined as the dynamic runaway boundary.

[0018] The flight status data is mapped to phase space to determine the current state point;

[0019] Calculate the minimum geodesic distance from the current state point to the dynamic runaway boundary on the state-space manifold;

[0020] The minimum geodesic distance is normalized to obtain a value that characterizes the current disturbance rejection capability, which is used as the real-time attitude recovery margin.

[0021] Preferably, the method for generating the projected margin consumption rate includes:

[0022] Obtain the desired acceleration sequence and desired angular velocity sequence contained in the captured trajectory command;

[0023] The desired acceleration sequence and desired angular velocity sequence are used as inputs and substituted into a preset predictive control model to perform rolling time-domain simulation, generating the predicted state trajectory within a preset future time period.

[0024] For each discrete time step in the predicted state trajectory, the corresponding process attitude recovery margin is calculated sequentially.

[0025] Differential calculations are performed on the attitude recovery margin for all processes, and the slope of change during the period of fastest margin decrease is extracted as the expected margin consumption rate.

[0026] Preferably, the method for determining whether a control circuit breaker command has been triggered includes:

[0027] Based on the real-time attitude recovery margin, the remaining attitude recovery margin at the end of the command execution is calculated by subtracting the product of the expected margin consumption rate and the preset prediction time.

[0028] Calculate the amplitude of the current environmental interference characteristics, determine the interference intensity level based on the amplitude, and match the corresponding dynamic safety lower limit from the preset threshold mapping table according to the interference intensity level;

[0029] Compare the remaining attitude recovery margin with the dynamic safety lower bound:

[0030] If the remaining attitude recovery margin is greater than or equal to the dynamic safety lower limit, the system is determined to be in the controllable region and no control circuit breaker command is generated.

[0031] If the remaining attitude recovery margin is less than the dynamic safety lower limit, the system is determined to be about to enter the irreversible stall zone, and a control circuit breaker command is generated.

[0032] Preferably, the method for generating the escape maneuver trajectory includes:

[0033] When a control circuit breaker command is received, the access rights of the capture trajectory command to the underlying actuator are immediately blocked.

[0034] With the goal of maximizing the real-time attitude recovery margin, and with the maximum motor torque and environmental obstacles as constraints, a nonlinear programming problem is constructed.

[0035] A real-time iterative algorithm is used to solve the nonlinear programming problem, generating a sequence of control variables that enables the UAV to quickly move away from the dynamic runaway boundary within a preset time, serving as the escape maneuver trajectory.

[0036] Preferably, it also includes an induced risk assessment module, used for:

[0037] Identify the movement trend of the target tracked by the capture trajectory command;

[0038] The number of obstacle feature points per unit volume within the area pointed to by the target's movement trend is used as the obstacle density.

[0039] Calculate the variance of the airflow velocity vector within this region as the airflow turbulence degree;

[0040] If the density of obstacles or the degree of airflow turbulence exceeds the preset risk threshold, an induced risk coefficient is generated based on the proportion exceeding the threshold.

[0041] The dynamic safety lower limit is positively corrected based on the induced risk coefficient to improve the sensitivity of the trigger control circuit breaker command.

[0042] Preferably, it also includes an evolution trend analysis module, used for:

[0043] Within the same flight mission cycle, record the environmental interference characteristics and flight status data at all times when control circuit breaker commands are triggered to form a high-risk sample set;

[0044] Temporal correlation analysis was performed on high-risk sample sets to capture the evolution trend of airflow patterns that lead to a sharp drop in attitude recovery margin;

[0045] The disturbance prediction parameters in the state observer are updated based on the evolution trend in order to identify airflow patterns that cause a sharp drop in attitude recovery margin in advance during subsequent flights.

[0046] Preferably, the dual-mode execution module is also used for:

[0047] During the execution of the escape maneuver trajectory, the recovery of the real-time attitude recovery margin is monitored in real time;

[0048] When the real-time attitude recovery margin rises to above the sum of the dynamic safety lower limit and the preset safety hysteresis, the shielding of the trajectory capture command is lifted, the response to the upper-level planning algorithm is restored, and the automatic reset from survival mode to task mode is completed.

[0049] Compared with the prior art, the present invention has the following beneficial effects:

[0050] 1. This invention achieves a balance between the greediness of flight missions and the robustness of physical survival by establishing a nonlinear dynamic envelope and a command pre-simulation circuit breaker mechanism. It achieves the effect of being able to pre-determine the margin consumption rate based on the aircraft model within a virtual time window when receiving an aggressive capture command from the upper layer, and forcibly triggering the circuit breaker when the predicted remaining margin is insufficient. Compared with the problem in the prior art that UAVs are prone to entering the irreversible aerodynamic stall zone due to blindly executing pursuit commands, this invention effectively prevents the risk of aircraft crashes in complex forest environments by establishing a dynamic safety red line, and ensures the survivability of the aircraft in extreme operating environments.

[0051] 2. This invention achieves real-time separation and precise quantification of unsteady aerodynamic disturbances in forest environments through a model-based nonlinear disturbance observer and multi-source data fusion technology. It achieves the effect of accurately separating the force and torque components caused by the environmental wind field and obtaining pure flight state data even under conditions of lack of airflow visibility and weak GPS signals. Compared with the shortcomings of existing technologies that are difficult to distinguish between the aircraft's maneuvering inertial forces and external airflow disturbances, this invention uses visual sensors and inertial measurement units to construct full-dimensional perception, providing an accurate disturbance benchmark for calculating attitude recovery margin and solving the problem of perception distortion in traditional control systems under strong aerodynamic disturbances.

[0052] 3. This invention achieves scalar characterization and adaptive risk control of the current anti-disturbance capability of UAVs through phase space geometric analysis and dynamic safety lower limit correction strategy; it transforms the abstract concept of stability into a calculable normalized index and dynamically adjusts the circuit breaker threshold according to the intensity of environmental interference and the risk of target induction; compared with the limitations of existing technologies that use fixed safety thresholds and cannot adapt to changes in forest microclimate, this invention can allow aggressive actions to improve efficiency when the wind is calm, while automatically tightening the safety boundary when there is turbulent airflow or induced risk, thus solving the problem that a single control strategy is difficult to cope with sudden changes in dynamic environment;

[0053] 4. This invention achieves a leap from passive loss of control defense to active survival evolution through a dual-mode execution architecture and evolution trend analysis based on historical data; it achieves the effect of automatically generating an escape trajectory with the goal of maximizing attitude recovery margin when the circuit breaker is triggered, and using long short-term memory networks to analyze high-risk sample sets to update observer parameters; compared with the existing technology, which lacks autonomous rescue planning when encountering danger and cannot learn from historical errors, this invention can not only take over control and restore aerodynamic stability in critical moments, but also self-optimize disturbance prediction parameters by identifying airflow patterns, significantly improving the system's sensitivity in identifying potential aerodynamic instability conditions in subsequent flights. Attached Figure Description

[0054] The present invention will be further explained below with reference to the accompanying drawings and embodiments:

[0055] Figure 1 This is a structural diagram of the system of the present invention. Detailed Implementation

[0056] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments.

[0057] Example 1:

[0058] Please see Figure 1A flight trajectory planning and attitude control system for a forest rodent control drone includes: a state perception module, used to monitor the drone's body dynamics and external environmental disturbances in real time, and acquire flight state data and environmental disturbance characteristics;

[0059] The margin quantization module is used to construct a nonlinear dynamic envelope based on flight state data, calculate the distance of the current state relative to the dynamic runaway boundary, and generate real-time attitude recovery margin.

[0060] The instruction pre-simulation module is used to receive the capture trajectory instruction generated by the upper-level planning algorithm, and based on the body dynamics model, to perform state extrapolation on the future time window after the execution of the capture trajectory instruction and generate the expected margin consumption rate.

[0061] The circuit breaker decision module is used to combine the real-time attitude recovery margin with the expected margin consumption rate, predict the remaining attitude recovery margin at the end of the instruction execution, and compare the remaining attitude recovery margin with the dynamic safety lower limit to determine whether to trigger the control circuit breaker instruction.

[0062] The dual-mode execution module is used to execute the capture trajectory command through the underlying controller if the control circuit breaker command is not triggered; if the control circuit breaker command is triggered, the capture trajectory command is forcibly rejected, and an escape maneuver trajectory aimed at restoring aerodynamic control is generated and executed.

[0063] This embodiment proposes an adaptive survival control system based on attitude recovery margin perception for the extreme operating environment of unstructured and strongly aerodynamically disturbed environments below the forest canopy. The core of this embodiment is to solve the trade-off between task greed and physical survival robustness. By quantifying the distance of the UAV from the runaway boundary in the current dynamic state, it realizes the pre-rehearsal and circuit breaking of high-risk commands.

[0064] The system builds the UAV’s full-dimensional perception capability through the state perception module. It not only monitors the physical motion parameters of the UAV, but also separates the unsteady aerodynamic disturbances in the environment in real time through observer technology, such as the Venturi effect airflow in the gaps between trees, thereby providing an accurate interference benchmark for subsequent margin calculation.

[0065] The margin quantification module transforms the abstract concept of stability into a computable scalar index. Based on the nonlinear dynamic envelope, it calculates the real-time attitude recovery margin in real time. This margin characterizes the UAV's ability to resist the maximum external disturbance without flipping or stalling in the current state using its remaining control capabilities. The command pre-simulation module gives the system the ability to predict future risks. After receiving the capture trajectory command generated by the upper-level planning algorithm, it does not execute it directly. Instead, it performs a simulation based on the body model within a virtual time window to calculate the expected margin consumption rate that will result from executing the command.

[0066] Based on this, the circuit breaker decision module performs a survival-first decision. According to the current margin and the expected consumption rate, it predicts the remaining margin when the instruction ends. Once the predicted value is lower than the dynamic safety lower limit, the system will determine that although the instruction can complete the rat extermination task, it will cause a survival crisis, thereby triggering the control circuit breaker instruction.

[0067] The dual-mode execution module enables seamless switching between task mode and survival mode. In normal mode, it executes capture trajectory commands to pursue rodent extermination efficiency. In emergency mode, which triggers the circuit breaker, it forcibly rejects upper-level commands and executes an escape maneuver trajectory with the sole objective of regaining aerodynamic control and maximizing attitude recovery margin. This embodiment effectively prevents forest rodent extermination drones from entering irreversible aerodynamic stall zones due to blindly executing aggressive pursuit commands by establishing an insurmountable dynamic safety red line. While ensuring mission execution efficiency, it significantly reduces the risk of drone crashes in complex forest obstacle environments, achieving a leapfrog transformation from passive crash to active survival.

[0068] Example 2:

[0069] The method for acquiring flight status data and environmental disturbance characteristics includes: collecting high-frequency angular velocity and acceleration data of the aircraft through an airborne inertial measurement unit; collecting spatial distribution data of surrounding obstacles through a visual sensor; inputting the angular velocity, acceleration data and spatial distribution data into a preset state observer to perform residual estimation of the airflow disturbance term; separating the unsteady aerodynamic components caused by the environmental wind field based on the residual estimation results; using the unsteady aerodynamic components as environmental disturbance characteristics; and using the motion parameters after removing the disturbance as flight status data.

[0070] This embodiment details the specific methods for acquiring flight status data and environmental interference characteristics. Addressing the challenges of weak GPS signals and invisible airflow in forest environments, a model-based observer technique is employed. The system performs multi-source data acquisition, using an onboard inertial measurement unit to collect angular velocities in the body coordinate system at a frequency of 1000Hz. With acceleration Simultaneously, a local voxel map is constructed using binocular vision sensors or LiDAR to obtain spatial distribution data of surrounding obstacles;

[0071] The system introduces a nonlinear disturbance observer (NDOB), into which collected angular velocity, acceleration, and spatial distribution data are input to perform residual estimation of the airflow disturbance term. To meet the code-level reproducibility requirements, the specific mathematical structure and parameter matrix definitions of the observer are defined here. The observer is constructed based on the momentum error dynamics equation, and its calculation formula is as follows:

[0072]

[0073] in, The estimated perturbation vector includes 3D force perturbation and 3D torque perturbation; The observer gain matrix is ​​a diagonal positive definite matrix that determines the observer's bandwidth and convergence speed. In the initial configuration of this embodiment, to match the aforementioned 0.5Hz cutoff frequency characteristics, this matrix is ​​specifically set as follows:

[0074]

[0075] Specifically, the force disturbance observation bandwidth is set to The torque disturbance observation bandwidth is set to a high value (rad / s) to ensure a rapid response to changes in aerodynamic torque; among which... The numerical derivative of the angular velocity is used. To ensure real-time performance and causality, this embodiment specifically employs a first-order backward difference formula for calculation, as follows:

[0076]

[0077] Among them, sampling period Set as ; in the formula To measure the rate of change of momentum, its specific composition is defined as follows:

[0078]

[0079] in, The mass of the drone is expressed in units of [missing information]. The specific force of the airframe measured by the airborne accelerometer, in units of ; The moment of inertia matrix of the UAV relative to the body coordinate system, in units of ; This is an estimate of the angular acceleration.

[0080] To control the input vector, it is defined as follows: ,in Indicates the first No. 1 motor ( The rotational speed, in units of ; This is an inherent nonlinear dynamic function of the organism;

[0081] The function represents the inherent nonlinear dynamics of the organism, characterizing the Coriolis force, centrifugal force, and gravitational torque effects generated during motion in a non-inertial frame; based on the organism's momentum. ,in The specific formula for calculating the body velocity is as follows:

[0082]

[0083] in, It is the acceleration due to gravity. Let be the perpendicular unit vector of the inertial frame. Let be the rotation matrix of the organism from the inertial frame. This indicates the vector cross product operation; this term is used to eliminate the inertial forces generated by the machine's own large maneuvers in the observer, ensuring... It only reflects external aerodynamic disturbances; The control performance matrix is ​​given; for a typical X-shaped quadcopter configuration, the wheelbase is... The specific expansion form of this matrix is:

[0084]

[0085] in, ; The thrust coefficient row vector is defined as follows:

[0086]

[0087] in This is the propeller thrust coefficient, in units of... , here through Extend the scalar thrust into a three-dimensional force vector;

[0088] for The torque distribution matrix, in its physical sense, maps the square of the rotational speed of each motor to the three-axis torque of the machine body; the torque distribution submatrix for:

[0089]

[0090] in, This refers to the drone's motor wheelbase and arm length, in units of... , This is the propeller thrust coefficient. This is the propeller anti-torque coefficient, in units of... It needs to be obtained through bench testing and calibration; the system is based on the observer output. Perform feature separation, This refers to a mixed disturbance term that includes both low-frequency persistent winds and high-frequency gusts. This disturbance is filtered by the observer's low-pass filter characteristics. The decision naturally smoothed out the measurement noise;

[0091] The system will It is directly used as a characteristic of environmental disturbance and subtracted from the original acceleration measurement. The corresponding force disturbance component is divided by the mass to obtain the pure motion parameters after removing the disturbance, which are used as flight state data.

[0092] This embodiment defines the observer gain matrix. Nonlinear dynamics term The numerical structure of the allocation matrix ensures the feasibility of the algorithm on the embedded controller and provides specific adjustment variables for adaptive parameter adjustment.

[0093] Example 3:

[0094] The method for generating real-time attitude recovery margin includes: constructing a multi-dimensional reachable set boundary in phase space based on the physical constraint parameters of the UAV, defining the reachable set boundary as the dynamic runaway boundary; mapping flight state data to phase space to determine the current state point; calculating the minimum geodesic distance from the current state point to the dynamic runaway boundary on the state space manifold; normalizing the minimum geodesic distance to obtain a value characterizing the current disturbance resistance capability, which is used as the real-time attitude recovery margin.

[0095] This embodiment details the method for generating real-time attitude recovery margin. To accurately describe the degree of UAV distance runaway, a phase space geometric analysis method is employed. The system constructs reachability set boundaries in a multi-dimensional phase space based on the UAV's physical constraint parameters. To ensure the reproducibility of the generated dynamic runaway boundary, this embodiment specifies the concrete constraint parameters and divergence criteria.

[0096] Physical constraint parameters: Define the set of input constraints, and their calculation formula is as follows:

[0097]

[0098] in, Representing the Control input of each motor This represents the maximum permissible speed of the motor, a typical value. ;

[0099] Out-of-control divergence criterion: Define the set of safe states Excessive tilt angle: or ,in For the roll angle of the aircraft, The pitch angle of the aircraft; altitude anomaly is... Define the NED coordinate system in the northeast direction, with the Z-axis pointing downwards as positive. Indicates falling below the ground, or This means that the vertical downward velocity exceeds the threshold, which means stalling and falling.

[0100] Boundary construction process: In the offline stage, inverse Monte Carlo simulation is used; from the runaway critical state, for example... Apply random limit control inputs in reverse time. Integrate and record all trajectory endpoints that can return to a stable hovering state; train these point sets using the Support Vector Data Description (SVDD) algorithm to obtain the equation of the closed hypersurface, the calculation formula of which is:

[0101]

[0102] in, The normalized state vector contains . Let be the radius of the SVDD hypersphere. For support vectors, For the corresponding Lagrange multipliers, the parameters are all determined by offline SVDD algorithm training; this hypersurface is the dynamic runaway boundary.

[0103] The system maps the current flight status data to phase space. In the process, determine the current state point. On the phase space manifold, calculate the minimum geodesic distance from the current state point to the dynamic runaway boundary; addressing the issue that the direct calculation of Euclidean distance lacks clear physical meaning due to the different physical dimensions of angles and angular velocities in the state space, this embodiment uses the metric tensor... A rigorous dimensional normalization design was implemented; definitions were defined. The characteristic time constant of the system attitude loop is used in this embodiment, and the weighting factor is taken as the weighting factor. That is, characteristic time constant Set as:

[0104]

[0105] By introducing the square of the time dimension Weighting factors The angular velocity term is weighted, and a velocity weighting factor is introduced. By weighting the linear velocity term, a dimensionless fusion of different physical quantities is achieved.

[0106] At this point, the calculated It is no longer a simple numerical distance, but a normalized state deviation norm with a clear physical meaning; the calculation formula is as follows:

[0107]

[0108] in, For points on the dynamic runaway boundary, This is the current state point. For connection With boundary points The parameterization path Path pair parameters The derivative; regarding the implicit boundary in the above optimization problem. For problems that are difficult to solve analytically directly, this embodiment uses a gradient-based fast projection iteration method for real-time calculation. The specific steps are as follows:

[0109] Initialization: Set the initial boundary search point Current state point The extension point along the gradient direction of the SVDD decision function;

[0110] Gradient projection iteration: utilizing the gradient information of the SVDD decision function Iterative updates are performed to quickly approximate the boundary of the zero level set; it is hereby clarified that the SVDD algorithm uses the Gaussian radial basis kernel function, the calculation formula of which is:

[0111]

[0112] in, Then the gradient The analytical expression is defined as follows:

[0113]

[0114] in, For the support vector index set, These are Lagrange multipliers; the iterative formula is:

[0115]

[0116] in, This refers to the step size factor in gradient descent. To avoid confusion, it specifically refers to the iteration step size, with a typical value... ; To prevent tiny quantities with a denominator of zero Iterate until Alternatively, the maximum number of iterations can be reached, let's say 5, to obtain the convergence boundary point. Distance calculation: Due to the metric tensor set in this embodiment For a constant diagonal matrix, the geodesic paths on a Riemannian manifold Degenerate into a straight line, the above integral formula can be simplified to the direct calculation of the weighted Euclidean distance:

[0117]

[0118] To obtain the real-time attitude recovery margin in the interval [0,1], the system performs normalization processing; to avoid normalization traps, i.e., denominators of 0 or scale failures, this embodiment defines a feature scale factor. Let be the equivalent radius of the volume enclosed by the SVDD hypersurface, and let its value be the value of all sample points in the offline training data. The statistical average value is obtained from offline training data and the typical value is The normalization formula uses a soft-limiting function, and its calculation formula is as follows:

[0119]

[0120] This formula ensures that when That is, when the critical runaway occurs ,when That is, when it is extremely safe Furthermore, it exhibits good linearity within the safe zone, thereby quantifying the geometric safety margin of the current state from the runaway boundary.

[0121] Example 4:

[0122] The method for generating the expected margin consumption rate includes: acquiring the expected acceleration sequence and expected angular velocity sequence contained in the capture trajectory command; using the expected acceleration sequence and expected angular velocity sequence as input, substituting them into a preset predictive control model to perform rolling time-domain simulation, generating a predicted state trajectory within a preset future time period; calculating the corresponding process attitude recovery margin for each discrete time step in the predicted state trajectory; performing differential calculation on all process attitude recovery margins, extracting the slope of change during the period with the fastest margin decrease, as the expected margin consumption rate.

[0123] This embodiment details a method for generating the expected margin consumption rate, which aims to quantify the cost of executing the current instruction; the system parses the instruction sequence and extracts the expected acceleration sequence for a future period from the captured trajectory instructions. With the desired angular velocity sequence The above sequence is input into a preset predictive control model for rolling time-domain simulation. To ensure that the predictive model can correctly respond to kinematic commands and reflect dynamic constraints, inverse dynamics calculation is required before inputting the data into the model: based on the current body model parameters, the nominal thrust required to maintain the desired trajectory is solved. With nominal torque Defined here For the body coordinate system The magnitude scalar of the thrust acting in the negative direction of the axis is always non-negative, and its calculation formula is as follows:

[0124]

[0125] in, The acceleration due to gravity is constant. The Z-axis unit vector in the inertial coordinate system , Represents the cross product of vectors;

[0126] The solution obtained and As a feedforward input, the six-degree-of-freedom rigid body dynamics equations, including environmental disturbance terms, are substituted into the equations for state derivation. The specific form and calculation formula are as follows:

[0127]

[0128]

[0129]

[0130]

[0131] in, Let gravitational acceleration be constant. The Z-axis unit vector in the inertial coordinate system. For the overall quality of the drone, Position and velocity in inertial coordinate system Let be the rotation matrix from the body coordinate system to the inertial coordinate system. angular velocity The antisymmetric matrix, i.e. The matrix representation, For the input thrust and torque commands, Here is the moment of inertia matrix of the machine body. External environmental disturbance torque;

[0132] In particular, to satisfy the requirement of variable traceability, the equations in That is, to predict environmental acceleration, and That is, the estimated environmental disturbance moment is not an arbitrarily set constant, but is strictly coupled with the real-time output of the state perception module in Example 2: the system extracts the 6-dimensional disturbance vector output by the state observer at the current moment. It is decomposed into unsteady aerodynamic components in the first three dimensions. With the disturbance torque components in the latter three dimensions The formula is:

[0133]

[0134] The force disturbance is converted into an equivalent acceleration in the inertial coordinate system, and the disturbance torque is directly assigned to... Assuming that the disturbance remains constant relative to the body coordinate system within the short-term prediction, both are assigned to each time step in the future prediction time domain to reflect the continuous impact of the current environmental wind field in a short period of time. The system uses the fourth-order Runge-Kutta method RK4 to numerically integrate the above equations with a step size of 0.01s and a prediction time domain of 2.0s, thereby obtaining an accurate future state sequence.

[0135] For each discrete time step in the predicted state trajectory, the system uses the aforementioned phase space distance calculation method to sequentially calculate the process attitude recovery margin corresponding to that moment. It then performs differential calculations on all process attitude recovery margins, extracting the slope of the time period with the fastest margin decrease as the expected margin consumption rate. The calculation formula is as follows:

[0136]

[0137] Among them, inner functions The maximum value between zero and the margin decline rate is used to ensure that only the trend of margin deterioration is counted. For the index of the predicted time step, Characterizes the margin loss per unit time step; The estimated margin consumption rate is derived from the differential maximum extraction, and the unit is... ; For the first The attitude recovery margin for each predicted time step; The time step is derived from the system sampling settings;

[0138] This embodiment can identify trap-like trajectories that appear safe at the entrance but are extremely dangerous at the exit in advance. By calculating the expected margin consumption rate, the system can predict the severe loss of stability caused by large-scale maneuvering capture actions, thus intervening before the action begins and avoiding greed. Figure 1 Dynamic overdraft caused by time capture.

[0139] Example 5:

[0140] The method for determining whether a control circuit breaker command has been triggered includes: calculating the remaining attitude recovery margin at the command execution endpoint by subtracting the product of the expected margin consumption rate and the preset prediction duration from the real-time attitude recovery margin; calculating the amplitude of the current environmental disturbance characteristics, determining the disturbance intensity level based on the amplitude, and matching the corresponding dynamic safety lower limit from a preset threshold mapping table based on the disturbance intensity level; comparing the remaining attitude recovery margin with the dynamic safety lower limit: if the remaining attitude recovery margin is greater than or equal to the dynamic safety lower limit, the system is determined to be in a controllable region, and no control circuit breaker command is generated; if the remaining attitude recovery margin is less than the dynamic safety lower limit, the system is determined to be about to enter an irreversible stall region, and a control circuit breaker command is generated.

[0141] This embodiment details the specific logic for determining whether a control circuit breaker command is triggered, which is the core of the system's decision-making. Based on the current state and consumption rate, the system estimates the remaining capacity upon completion of the command execution and calculates the remaining attitude recovery margin at the end of the command execution. The calculation formula is as follows:

[0142]

[0143] in, The remaining attitude recovery margin is derived from linear extrapolation calculation; The real-time attitude recovery margin is derived from the current state solution; The projected margin consumption rate is derived from the maximum instantaneous rate of change in the instruction rehearsal; The preset prediction duration is used to address potential ambiguities in the definition of this parameter. In this embodiment, it is explicitly defined as a fixed constant pre-calibrated based on the system's dynamic characteristics, rather than a runtime variable. The calibration logic is as follows:

[0144]

[0145] in, The physical prediction window is fixed at 2.0 seconds. This is a dimensionless kinematic compaction factor, which differs from the iteration step size in Example 3. ; The logic for determining the value is based on: This is the normalized reciprocal of the ratio of the motor's response time to 90% of its maximum thrust to the prediction window. Under the specific hardware configuration of this embodiment, this value was calibrated through offline testing. It is 0.4; therefore, The system parameter table explicitly defines it as 0.8s; this definition eliminates parameter uncertainty, ensures the uniqueness and reproducibility of key decision parameters, and also conforms to the physical nature of the equivalent peak duration, i.e., preventing damage caused by localized instantaneous high consumption rates. The overly conservative estimate is caused by directly multiplying by the total physical duration;

[0146] The system determines the dynamic safety lower limit and calculates the amplitude of environmental disturbance characteristics; to ensure consistency with the dimensions of the acceleration threshold, the system extracts the environmental disturbance characteristic vector. The first three dimensions of the component, i.e., the force disturbance component. And calculate its equivalent perturbation acceleration:

[0147]

[0148] in, For the quality of drones, The disturbance vector estimated by the observer in Example 2 The first three dimensions, i.e., the force disturbance component, are expressed in units of . According to the equivalent disturbance acceleration Determine the disturbance intensity level and match the corresponding dynamic safety lower limit by looking up the table accordingly; the specific threshold mapping table configuration is as follows: Level I wind: when the equivalent disturbance acceleration... At that time, the dynamic safety lower limit L = 0.20 is matched;

[0149] Level II: When the equivalent disturbance acceleration At that time, the dynamic safety lower limit L = 0.45;

[0150] Level III: When the equivalent disturbance acceleration At that time, the dynamic safety lower limit L = 0.70 is matched;

[0151] The system compares the remaining attitude recovery margin with the dynamic safety lower limit: if the remaining attitude recovery margin is greater than or equal to the dynamic safety lower limit, the system is determined to be in a controllable region and the capture command is allowed to be executed; if the remaining attitude recovery margin is less than the dynamic safety lower limit, the system is determined to be about to enter the irreversible stall region and a control circuit breaker command is immediately generated.

[0152] This embodiment achieves environmentally adaptive risk control. By introducing equivalent duration correction logic, it allows the drone to perform more aggressive capture actions when the forest is calm, while automatically tightening the safety boundary when the airflow is turbulent, ensuring that the drone always retains enough energy to cope with unknown environmental changes, thereby maintaining a survival baseline in the dynamically changing forest microclimate environment.

[0153] Example 6:

[0154] The method for generating the escape maneuver trajectory includes: when a control circuit breaker command is received, immediately blocking the access permissions of the capture trajectory command to the underlying actuator; constructing a nonlinear programming problem with the goal of maximizing the real-time attitude recovery margin and the constraints of the maximum motor torque and environmental obstacles; and solving the nonlinear programming problem using a real-time iterative algorithm to generate a sequence of control quantities that enables the UAV to quickly move away from the dynamic runaway boundary within a preset time, which serves as the escape maneuver trajectory.

[0155] This embodiment details the method for generating escape maneuver trajectories. When the circuit breaker is triggered, the system's target switches from capture to survival the instantaneously.

[0156] The system execution permission is blocked, and the underlying controller immediately cuts off the response channel to the upper layer's trajectory capture command;

[0157] The system constructs a nonlinear programming problem with the optimization objective of maximizing the real-time attitude recovery margin, while taking physical limitations on the maximum motor torque and environmental obstacle avoidance as constraints. The optimization objective function is as follows:

[0158]

[0159] in, The initial moment when the circuit breaker is triggered; To optimize the target, it is derived from the margin integral, which physically means the cumulative safety during the escape process. That is, the sum of the attitude recovery margin within the escape window. The dimension is margin × time. The larger the value, the stronger the drone's ability to maintain a high safety margin throughout the entire escape period.

[0160] The real-time attitude recovery margin function is derived from the dynamic state mapping;

[0161] This is a sequence of control variables, derived from optimization variables, and its physical meaning is the motor speed command;

[0162] The duration of the escape maneuver is determined by a preset emergency window, typically set to 1.5 to 3.0 seconds. The typical value for the safe radius of the drone is set to... ;

[0163] The state variables are derived from the system state equations; in the constraints, environmental obstacles are represented by the Euclidean symbolic distance field (ESDF), and inequality constraints are constructed. ,in The safe radius of the UAV is determined; a real-time iterative algorithm is used to solve the above problem in milliseconds. Specifically, the Sequence Quadratic Programming (SQP) algorithm is used, and the control solution of the previous moment is used as the initial value for hot start to accelerate convergence and generate the optimal control quantity sequence, i.e., the escape maneuver trajectory. This trajectory is usually manifested as the UAV rapidly leveling off its attitude, increasing the throttle to climb to an open airspace or hovering.

[0164] This embodiment ensures that when faced with the risk of losing control, the drone can autonomously plan a life-saving path that conforms to physical limits, without relying on human intervention. It makes optimal decisions based entirely on the drone's current dynamic capabilities, greatly improving its survival rate in extreme situations in densely forested areas.

[0165] Example 7:

[0166] It also includes an induced risk assessment module, which is used to: identify the target movement trend tracked by the capture trajectory command; count the number of obstacle feature points per unit volume in the area pointed to by the target movement trend as obstacle density; calculate the variance of the airflow velocity vector in the area as airflow turbulence; if the obstacle density or airflow turbulence exceeds a preset risk threshold, generate an induced risk coefficient based on the proportion exceeding the threshold; and positively correct the dynamic safety lower limit based on the induced risk coefficient to improve the sensitivity of triggering the control circuit breaker command.

[0167] This embodiment further introduces an induced risk assessment module to address the induced escape strategies of the rat swarm; the system identifies the target movement trend tracked by the capture trajectory command and specifically calculates the target's current velocity vector. Using this vector as the axis, a conical space with a vertex angle of 30 degrees and a length of 10 meters is constructed as the pointing area; the system counts the number of obstacle feature points per unit volume within this area and queries it through a point cloud map to determine the obstacle density. ;

[0168] For the step of calculating the variance of the airflow velocity vector within the region, since the UAV has not yet reached the target area and cannot be directly measured, this embodiment adopts a remote inversion method based on visual optical flow: the system uses a visual sensor to capture the vegetation video stream of the target area, and uses the Lucas-Kanade optical flow algorithm to track vegetation feature points to obtain the original optical flow field; in order to eliminate the background optical flow caused by the UAV's own motion, the system performs strict self-motion compensation, and calculates the velocity of the UAV body based on the pinhole camera model and the optical flow constraint equation. and angular velocity The theoretical self-moving optical flow caused by it The calculation formula is as follows:

[0169]

[0170] in, Focal length These are the pixel coordinates of the feature point. The depth values ​​of the corresponding feature points are obtained through lidar or binocular parallax; the residual optical flow vector is calculated. The calculation formula is as follows:

[0171]

[0172] Calculate the residual optical flow modulus The calculation formula is as follows:

[0173]

[0174] The residual optical flow retains only the motion component generated by the airflow blowing the vegetation; the statistical variance of this residual velocity sequence within a preset sampling window, such as 0.5 seconds, is calculated as the visual jitter variance. The formula is:

[0175]

[0176] in, The total number of data frames within the sampling window, for example, the window size. Frame rate ,but ; This is the arithmetic mean of the residual optical flow modulus within the window;

[0177] Using a pre-calibrated vegetation optical flow jitter-wind field turbulence mapping function, the variance of visual observations is transformed into the variance of the airflow velocity vector in the region, i.e., the airflow turbulence degree. Considering the physical property that the optical flow modulus is inversely proportional to the observation depth, the average depth value of the feature points in the target region is introduced. The correction is made, and the specific mapping function is a quadratic multinomial regression model that includes depth weights, and its calculation formula is as follows:

[0178]

[0179] in, This is the square of the average depth of feature points within the target region, used to compensate for the geometric properties of optical flow field attenuation with distance, in units of... Regression coefficient and It was obtained through offline wind tunnel experiments and calibration, for example, by simulating a turbulent environment with known variance in a wind tunnel and measuring the variance of vegetation optical flow, and then obtaining it through least squares fitting;

[0180] In the specific experimental setup of this embodiment, for a typical mixed coniferous and broadleaf forest environment, the specific values ​​obtained by calibration are as follows: ; In response to the above indicators exceeding a preset risk threshold, such as an obstacle density threshold indivual airflow turbulence threshold The system calculates the excess ratio and generates an induced risk coefficient. The calculation formula is as follows:

[0181]

[0182] in, As a preset risk weighting factor, this embodiment sets , The system uses this coefficient to positively correct the dynamic safety lower limit, as shown in the formula:

[0183]

[0184] in, The dynamic safety lower bound is obtained from the original table lookup. The sensitivity adjustment factor is set to 0.3; this calculation significantly raises the dynamic safety lower limit when mice escape into dangerous areas, making the system more sensitive and conservative.

[0185] Example 8:

[0186] It also includes an evolution trend analysis module, which is used to: record the environmental disturbance characteristics and flight status data of all times when the control circuit breaker command is triggered within the same flight mission cycle, forming a high-risk sample set; perform time-series correlation analysis on the high-risk sample set to capture the evolution trend of airflow patterns that cause a sharp drop in attitude recovery margin; and update the disturbance prediction parameters in the state observer based on the evolution trend in order to identify airflow patterns that cause a sharp drop in attitude recovery margin in advance in subsequent flights.

[0187] This embodiment introduces an evolution trend analysis module for the system's self-evolution; within the task cycle, whenever a control circuit breaker command is triggered, the system automatically records the environmental interference characteristics within the 2.0 seconds preceding that moment. Together with flight status data, they form a high-risk sample set;

[0188] The system performs temporal correlation analysis on a high-risk sample set, specifically employing a pattern matching algorithm based on a Long Short-Term Memory (LSTM) network. The input layer of this LSTM network has a dimension of 6, corresponding to 3D force perturbations and 3D torque perturbations, while the output layer corresponds to a predefined... Typical forest turbulence patterns include: 1-canopy shear wind, 2-trunk vortex street, and 3-surface thermal upwelling; the network output is a pattern classification probability vector. The perturbation prediction parameters in the state observer are updated based on the evolution trend; here, the perturbation prediction parameters specifically refer to the observer gain matrix defined in Example 2. The system has a pre-stored airflow mode-gain correction table, in which each mode... Corresponding to a gain correction matrix For example, for high-frequency vortex street patterns around tree trunks, The diagonal elements are positive, which is intended to increase the observer bandwidth to reduce phase lag to high-frequency disturbances;

[0189] To meet the feasibility requirements, a matrix example under a typical pattern is given here: For The mode, and its corresponding gain correction matrix is ​​set as follows: This setting significantly enhances the observer's sensitivity to high-frequency aerodynamic forces in the horizontal plane; the update logic is as follows:

[0190]

[0191] in, The initial gain matrix is ​​defined in Example 2. for The network output belongs to the first Probability value of airflow-like patterns; For the first The gain correction matrix corresponding to the class mode is used to directionally increase the observation bandwidth of the corresponding channel based on the frequency characteristics of the specific mode. The learning rate is set to 0.05. This update mechanism automatically adjusts the observer's differential equation when the system identifies a specific airflow pattern in the current environment that causes a sharp drop in margin. In The parameters are modified to change the frequency response characteristics of the observer, enabling it to more sensitively detect such dangerous disturbances in subsequent flights, thereby making the calculation of real-time attitude recovery margin more conservative and safe.

[0192] Example 9:

[0193] The dual-mode execution module is also used to: monitor the recovery of the real-time attitude recovery margin in real time during the execution of the escape maneuver trajectory; when the real-time attitude recovery margin recovers to a level higher than the sum of the dynamic safety lower limit and the preset safety hysteresis, the shielding of the capture trajectory command is removed, the response to the upper-level planning algorithm is restored, and the automatic reset from survival mode to mission mode is completed.

[0194] This embodiment details the reset mechanism of the dual-mode execution module, designed to prevent system oscillation. During the execution of the escape maneuver trajectory, the system continuously monitors the recovery of the real-time attitude recovery margin. To prevent frequent transitions between mission mode and survival mode, this embodiment introduces hysteresis comparison logic. The reset determination conditions are as follows:

[0195]

[0196] in, The real-time attitude recovery margin is derived from real-time calculation; This is the current dynamic safety lower limit, derived from environment matching; The safety hysteresis is set as a dynamic safety lower limit. of ,Right now The parameters are derived from preset parameters, namely the buffer to prevent jitter; in response to the above conditions being met, the system removes the shielding of the trajectory capture command, resumes the response to the upper-level planning algorithm, and completes the automatic reset from survival mode to task mode.

[0197] This embodiment ensures that the drone will only resume the capture mission after its attitude is completely stable and there is sufficient safety margin, avoiding repeated triggering of the fuse in the critical state, ensuring the smoothness of the control system and the continuity of the rodent extermination mission, and achieving a dynamic balance between safety and efficiency.

[0198] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A flight trajectory planning and attitude control system for a forest rodent control drone, characterized in that, include: The state perception module is used to monitor the UAV's airframe dynamics and external environmental disturbances in real time, and to acquire flight state data and environmental disturbance characteristics. The margin quantization module is used to construct a nonlinear dynamic envelope based on flight state data, calculate the distance of the current state relative to the dynamic runaway boundary, and generate real-time attitude recovery margin. The instruction pre-simulation module is used to receive the capture trajectory instruction generated by the upper-level planning algorithm, and based on the body dynamics model, to perform state extrapolation on the future time window after the execution of the capture trajectory instruction and generate the expected margin consumption rate. The circuit breaker decision module is used to combine the real-time attitude recovery margin with the expected margin consumption rate, predict the remaining attitude recovery margin at the end of the instruction execution, and compare the remaining attitude recovery margin with the dynamic safety lower limit to determine whether to trigger the control circuit breaker instruction. The dual-mode execution module is used to execute the capture trajectory command through the underlying controller if the control circuit breaker command is not triggered; if the control circuit breaker command is triggered, the capture trajectory command is forcibly rejected, and an escape maneuver trajectory aimed at restoring aerodynamic control is generated and executed.

2. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 1, characterized in that, Methods for acquiring flight status data and environmental disturbance characteristics include: The airborne inertial measurement unit collects high-frequency angular velocity and acceleration data of the aircraft. Collect spatial distribution data of surrounding obstacles using visual sensors; Input the angular velocity, acceleration data and spatial distribution data into the preset state observer to perform residual estimation of the airflow disturbance term; Based on the residual estimation results, the unsteady aerodynamic components caused by the environmental wind field are separated, and the unsteady aerodynamic components are used as environmental disturbance characteristics. The motion parameters after removing the disturbance are used as flight state data.

3. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 2, characterized in that, Methods for generating real-time attitude recovery margin include: Based on the physical constraint parameters of the UAV, a multidimensional reachable set boundary is constructed in phase space, and the reachable set boundary is defined as the dynamic runaway boundary. The flight status data is mapped to phase space to determine the current state point; Calculate the minimum geodesic distance from the current state point to the dynamic runaway boundary on the state-space manifold; The minimum geodesic distance is normalized to obtain a value that characterizes the current disturbance rejection capability, which is used as the real-time attitude recovery margin.

4. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 3, characterized in that, Methods for generating the projected margin consumption rate include: Obtain the desired acceleration sequence and desired angular velocity sequence contained in the captured trajectory command; The desired acceleration sequence and desired angular velocity sequence are used as inputs and substituted into a preset predictive control model to perform rolling time-domain simulation, generating the predicted state trajectory within a preset future time period. For each discrete time step in the predicted state trajectory, the corresponding process attitude recovery margin is calculated sequentially. Differential calculations are performed on the attitude recovery margin for all processes, and the slope of change during the period of fastest margin decrease is extracted as the expected margin consumption rate.

5. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 4, characterized in that, Methods for determining whether a circuit breaker command has been triggered include: Based on the real-time attitude recovery margin, the remaining attitude recovery margin at the end of the command execution is calculated by subtracting the product of the expected margin consumption rate and the preset prediction time. Calculate the amplitude of the current environmental interference characteristics, determine the interference intensity level based on the amplitude, and match the corresponding dynamic safety lower limit from the preset threshold mapping table according to the interference intensity level; Compare the remaining attitude recovery margin with the dynamic safety lower bound: If the remaining attitude recovery margin is greater than or equal to the dynamic safety lower limit, the system is determined to be in the controllable region and no control circuit breaker command is generated. If the remaining attitude recovery margin is less than the dynamic safety lower limit, the system is determined to be about to enter the irreversible stall zone, and a control circuit breaker command is generated.

6. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 5, characterized in that, Methods for generating escape maneuver trajectories include: When a control circuit breaker command is received, the access rights of the capture trajectory command to the underlying actuator are immediately blocked. With the goal of maximizing the real-time attitude recovery margin, and with the maximum motor torque and environmental obstacles as constraints, a nonlinear programming problem is constructed. A real-time iterative algorithm is used to solve the nonlinear programming problem, generating a sequence of control variables that enables the UAV to quickly move away from the dynamic runaway boundary within a preset time, serving as the escape maneuver trajectory.

7. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 6, characterized in that, It also includes an induced risk assessment module, used for: Identify the movement trend of the target tracked by the capture trajectory command; The number of obstacle feature points per unit volume within the area pointed to by the target's movement trend is used as the obstacle density. Calculate the variance of the airflow velocity vector within this region as the airflow turbulence degree; If the density of obstacles or the degree of airflow turbulence exceeds the preset risk threshold, an induced risk coefficient is generated based on the proportion exceeding the threshold. The dynamic safety lower limit is positively corrected based on the induced risk coefficient to improve the sensitivity of the trigger control circuit breaker command.

8. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 7, characterized in that, It also includes an evolution trend analysis module, used for: Within the same flight mission cycle, record the environmental interference characteristics and flight status data at all times when control circuit breaker commands are triggered to form a high-risk sample set; Temporal correlation analysis was performed on high-risk sample sets to capture the evolution trend of airflow patterns that lead to a sharp drop in attitude recovery margin; The disturbance prediction parameters in the state observer are updated based on the evolution trend in order to identify airflow patterns that cause a sharp drop in attitude recovery margin in advance during subsequent flights.

9. The flight trajectory planning and attitude control system of a forest rodent control drone according to claim 8, characterized in that, The dual-mode execution module is also used for: During the execution of the escape maneuver trajectory, the recovery of the real-time attitude recovery margin is monitored in real time; When the real-time attitude recovery margin rises to above the sum of the dynamic safety lower limit and the preset safety hysteresis, the shielding of the trajectory capture command is lifted, the response to the upper-level planning algorithm is restored, and the automatic reset from survival mode to task mode is completed.

Images