Unmanned aerial vehicle AGV full-automatic precise recovery control method in harsh field environment

Abstract

The present application relates to the technical field of unmanned aerial vehicle control, in particular to a kind of unmanned aerial vehicle AGV full-automatic precision recovery control method under field harsh environment, comprising the following steps: obtaining unmanned aerial vehicle body rotor angular acceleration parameter and fuselage control moment residual error parameter under field harsh environment, operation is carried out, and aggregate disturbance moment parameter is generated;The aggregate disturbance moment parameter is input into frequency domain filter and is filtered frequency band separation calculation, and low-frequency disturbance moment component and high-frequency disturbance moment component are extracted.The present application generates after landing position tracking deviation, expected fuselage control moment and environmental wind disturbance moment component synchronous participation algebraic accumulation, anti-interference control moment is mapped into unmanned aerial vehicle rotor drive parameter again, so that rotor drive output simultaneously responds trajectory deviation, attitude demand and wind disturbance compensation, and then improve trajectory adhesion degree and fuselage attitude stability in the process of mobile platform recovery under field harsh environment.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) control technology, and in particular to a fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh outdoor environments. Background Technology

[0002] The field of UAV control technology mainly involves the state perception, attitude adjustment, trajectory planning, motion control, and anti-disturbance execution of UAVs during flight, hovering, cruise, obstacle avoidance, take-off and landing, formation, and recovery.

[0003] Current UAV control technology typically employs closed-loop adjustments based on position, attitude, speed, and a predetermined trajectory during flight, hovering, cruise, obstacle avoidance, takeoff and landing, formation flying, and recovery. In actual operation, the expression of disturbances to the aircraft tends to focus on position or attitude deviations. The inherent correlation between rotor angular acceleration changes and the residual control torque of the fuselage is difficult to translate into a clear disturbance torque in a timely manner. This leads to compensatory lag in control output when external wind disturbances, load eccentricity, and rotor response differences all work together. In harsh field environments, wind direction changes frequently, and the ground-based AGV is in motion. The aircraft's center of gravity may shift due to load distribution and attitude changes. If recovery control is based solely on a fixed landing point or a conventional trajectory, the landing reference path may deviate from the center of the moving load-bearing surface. This can result in lateral drift, uneven vertical sinking, and increased attitude swaying when the aircraft approaches the AGV. Therefore, improvements are needed. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and propose a fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh outdoor environments.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments, comprising the following steps: The rotor angular acceleration parameters and control torque residual parameters of the UAV are obtained in harsh field environments. The calculations are performed to generate lumped disturbance torque parameters. The lumped disturbance moment parameters are input into a frequency domain filter for filtering frequency band separation calculation. The low-frequency disturbance moment components and high-frequency disturbance moment components are extracted. The low-frequency disturbance moment components are calculated with the UAV body weight parameters to generate two-dimensional centroid offset coordinates. The high-frequency disturbance moment components are extracted as environmental wind disturbance moment components. The current position coordinates and velocity parameters of the AGV are obtained. The current position coordinates and velocity parameters of the AGV are used as terminal constraints. Combined with the two-dimensional offset coordinates of the centroid and the current position coordinates of the UAV, variable mapping operation is performed to generate a reference flat state space variable. The elements of the reference flat state space variable are extracted and multi-order time differential solution operation and matrix splicing calculation are performed to generate a landing and recovery reference trajectory.

[0006] Preferably, the method further includes: The landing and recovery reference trajectory is spatially subtracted from the current position coordinates of the UAV to generate a landing position tracking deviation variable. The landing position tracking deviation variable is substituted into the position and attitude cascade control law equation for differentiation and gain calculation to generate the desired fuselage control torque. The desired fuselage control torque is algebraically accumulated with the environmental wind disturbance torque component to generate the disturbance rejection control torque. The disturbance rejection control torque is substituted into the UAV control allocation matrix for linear mapping calculation to generate the UAV rotor drive parameters.

[0007] Preferably, the steps for obtaining the lumped disturbance moment parameters are as follows: To obtain the angular acceleration parameters of the UAV rotor in harsh field environments, read the angular acceleration change sequence of each UAV rotor during attitude adjustment, remove records with missing acquisition time, missing rotor number, and missing angular acceleration direction marking, arrange the angular acceleration change sequence according to rotor number and acquisition time, obtain the fuselage control torque residual parameter, extract the difference between the expected fuselage control torque and the actual fuselage response torque, and obtain the UAV rotor angular acceleration parameters and fuselage control torque residual parameters; The inertia parameters of the UAV body, the rotor installation orientation parameters, and the fuselage attitude angle change parameters are called. The angular acceleration parameters of the UAV rotor body are mapped to the fuselage coordinate direction according to the rotor number. The fuselage control torque residual parameters are decomposed into components according to the pitch direction, roll direction, and yaw direction. The angular acceleration mapping and control torque residual components at the same acquisition time are arranged accordingly to form the momentum observer input parameters. The momentum observer input parameters are substituted into the momentum observer constructed based on the Lagrange dynamics equations. The angular acceleration mapping, control torque residual components, and body inertia parameters are extracted item by item according to the fuselage coordinate direction. The equivalent inertial torque and control torque residual components generated by the angular acceleration mapping are algebraically superimposed. The algebraically superimposed torque components are continuously arranged according to the acquisition time to generate the lumped disturbance torque parameters.

[0008] Preferably, the step of obtaining the two-dimensional offset coordinates of the centroid is as follows: The lumped disturbance moment parameters are input into a frequency domain filter. The roll moment component, pitch moment component, and yaw moment component in the lumped disturbance moment parameters are read according to the acquisition time. The moment components in each direction are marked with frequency bands. The slowly varying moment components and the fast wave moment components are distinguished according to the frequency band markings. The slowly varying moment components are sorted into low-frequency disturbance moment components, and the fast wave moment components are sorted into high-frequency disturbance moment components, thus obtaining low-frequency disturbance moment components and high-frequency disturbance moment components. Extract the low-frequency disturbance moment values ​​in the roll and pitch directions from the low-frequency disturbance moment components. Using the UAV's body weight parameters, perform a scalar division operation on the roll direction low-frequency disturbance moment value according to the UAV's body weight parameters to generate a lateral centroid offset value. Perform a scalar division operation on the pitch direction low-frequency disturbance moment value according to the UAV's body weight parameters to generate a longitudinal centroid offset value. Combine the lateral and longitudinal centroid offset values ​​according to their coordinate order to generate a two-dimensional centroid offset coordinate system.

[0009] Preferably, the step of obtaining the environmental wind disturbance torque component is as follows: Extract the high-frequency disturbance moment values ​​in the roll direction, pitch direction, and yaw direction from the high-frequency disturbance moment components. Verify the direction and time markers of the high-frequency disturbance moment components at each acquisition time. Remove the high-frequency disturbance moment components with missing direction or time markers. Arrange the remaining high-frequency disturbance moment components continuously according to the acquisition time and aggregate them according to the fuselage coordinate direction to generate the environmental wind disturbance moment components.

[0010] Preferably, the step of obtaining the reference flat state space variables is as follows: Obtain the current position coordinates and speed parameters of the AGV, read the horizontal coordinate values, vertical coordinate values ​​and vertical coordinate values ​​of the AGV in the coordinate system of the recycling area, extract the horizontal speed values, vertical speed values ​​and vertical speed values ​​of the AGV at the corresponding acquisition time, check the consistency between the coordinate acquisition time and the speed acquisition time, remove records with missing coordinate axis labels and missing speed direction labels, arrange the retained current position coordinates and speed parameters of the AGV according to the acquisition time, and generate terminal constraint conditions; Read the horizontal coordinate values, vertical coordinate values, horizontal velocity values, vertical velocity values, and vertical velocity values ​​from the terminal constraints. Superimpose the two-dimensional offset coordinates of the centroid onto the horizontal and vertical coordinate values ​​of the current position coordinates of the UAV. Perform nonlinear variable mapping calculations on the superimposed UAV position coordinate values ​​to generate reference flat state space variables.

[0011] Preferably, the step of obtaining the landing and recovery reference trajectory is as follows: The position, velocity, attitude, and time elements of the reference flat state space variables are extracted one by one. The first-order time derivative of the position element is solved according to the order of the time elements, the second-order time derivative of the velocity element is solved, and the attitude change rate of the attitude element is solved. The solved time change elements are then matrix-concatenated according to the order of position, velocity, attitude, and time to generate the landing and recovery reference trajectory.

[0012] Preferably, the step of obtaining the landing position tracking deviation variable is as follows: The landing and recovery reference trajectory is spatially subtracted from the current position coordinates of the UAV. The horizontal, vertical, and longitudinal reference position values ​​in the landing and recovery reference trajectory are read at the same acquisition time. The horizontal, vertical, and longitudinal current position values ​​in the current position coordinates of the UAV are also read. The spatial difference between the reference position values ​​and the current position values ​​is calculated axis by axis. The spatial difference is arranged in the order of horizontal spatial difference, vertical spatial difference, and longitudinal spatial difference to generate the landing position tracking deviation variable.

[0013] Preferably, the steps for obtaining the UAV rotor drive parameters are as follows: Substitute the landing position tracking deviation variable into the position and attitude cascade control law equation, extract the lateral spatial difference, longitudinal spatial difference and vertical spatial difference according to the acquisition time, perform derivative processing on the change of spatial difference between adjacent acquisition times, match the derivative processing results with lateral position gain, longitudinal position gain and vertical position gain respectively, and then convert the matched gain solution into roll direction moment component, pitch direction moment component and yaw direction moment component to generate the desired fuselage control torque; The roll, pitch, and yaw torque components of the desired fuselage control torque are read at the same acquisition time. The roll, pitch, and yaw wind disturbance torque values ​​of the environmental wind disturbance torque components are also read. The torque values ​​in the same fuselage coordinate direction are algebraically accumulated to form the disturbance rejection control torque. The disturbance rejection control torque is substituted into the UAV control allocation matrix and mapped to the corresponding drive quantity of each rotor according to the rotor number to generate the UAV rotor drive parameters.

[0014] Compared with the prior art, the advantages and positive effects of the present invention are as follows: This invention, after obtaining the rotor angular acceleration parameters and fuselage control torque residual parameters of the UAV, directly performs algebraic calculations around the rotor rotation changes and fuselage torque deviations. The lumped disturbance torque parameters can reflect the internal response deviations and the superposition state of external disturbances, avoiding the need to rely solely on position deviations to lag and correct flight attitude. Frequency band separation calculations are performed on the lumped disturbance torque parameters; the low-frequency disturbance torque components are used to convert the two-dimensional offset coordinates of the centroid, and the high-frequency disturbance torque components are used to form the environmental wind disturbance torque components, separating the gradual effects caused by load offsets from the rapid disturbances caused by wind field impacts for subsequent control. The current position coordinates and speed parameters of the AGV are then used to... The data serves as the terminal constraint condition, and is combined with the two-dimensional offset coordinates of the centroid and the current position coordinates of the UAV for nonlinear variable mapping. The landing and recovery reference trajectory is no longer only oriented towards the fixed landing point, but is formed by the constraints of the mobile recovery platform, the eccentric state of the body, and the current spatial position. After the landing position tracking deviation is generated, it is expected that the components of the fuselage control torque and the environmental wind disturbance torque will participate in the algebraic accumulation simultaneously. The disturbance rejection control torque is then mapped into the UAV rotor drive parameters, so that the rotor drive output can simultaneously respond to the trajectory deviation, attitude requirements, and wind disturbance compensation, thereby improving the trajectory fit and fuselage attitude stability during the mobile platform recovery process in harsh field environments. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the steps of the present invention; Figure 2 A frequency band separation curve of the lumped disturbance moment parameters in the roll direction; Figure 3 This is a reference trajectory diagram for landing and recovery. Detailed Implementation

[0016] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0017] Please see Figure 1-3 This invention provides a technical solution: a fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh outdoor environments, comprising the following steps: The rotor angular acceleration parameters and control torque residual parameters of the UAV are obtained in harsh field environments. The calculations are performed to generate lumped disturbance torque parameters. The lumped disturbance moment parameters are input into the frequency domain filter for filtering frequency band separation calculation. The low-frequency disturbance moment components and high-frequency disturbance moment components are extracted. The low-frequency disturbance moment components are calculated with the UAV body weight parameters to generate the two-dimensional offset coordinates of the centroid. The high-frequency disturbance moment components are extracted as the environmental wind disturbance moment components. The current position coordinates and speed parameters of the AGV are obtained. The current position coordinates and speed parameters of the AGV are used as terminal constraints. Combined with the two-dimensional offset coordinates of the centroid and the current position coordinates of the UAV, variable mapping calculation is performed to generate reference flat state space variables. The elements of each item of the reference flat state space variables are extracted and multi-order time differential solution calculation and matrix splicing calculation are performed to generate landing and recovery reference trajectory. Spatial subtraction is performed between the landing and recovery reference trajectory and the current position coordinates of the UAV to generate the landing position tracking deviation variable. The landing position tracking deviation variable is substituted into the position and attitude cascade control law equation for differentiation and gain calculation to generate the desired fuselage control torque. The desired fuselage control torque is algebraically accumulated with the environmental wind disturbance torque component to generate the disturbance rejection control torque. The disturbance rejection control torque is substituted into the UAV control allocation matrix for linear mapping calculation to generate the UAV rotor drive parameters.

[0018] The steps for obtaining the lumped disturbance moment parameters are as follows: To obtain the angular acceleration parameters of the UAV rotor in harsh field environments, read the angular acceleration change sequence of each UAV rotor during attitude adjustment, remove records with missing acquisition time, missing rotor number, and missing angular acceleration direction marking, arrange the angular acceleration change sequence according to rotor number and acquisition time, obtain the fuselage control torque residual parameter, extract the difference between the expected fuselage control torque and the actual fuselage response torque, and obtain the UAV rotor angular acceleration parameters and fuselage control torque residual parameters; By calling the UAV's inertia parameters, rotor installation orientation parameters, and fuselage attitude angle change parameters, the UAV's rotor angular acceleration parameters are mapped to the fuselage coordinate direction according to the rotor number. The fuselage control torque residual parameters are decomposed into components according to the pitch, roll, and yaw directions. The angular acceleration mapping and control torque residual components at the same acquisition time are arranged accordingly to form the momentum observer input parameters. The momentum observer input parameters are substituted into the momentum observer constructed based on the Lagrange dynamics equations. The angular acceleration mapping, control torque residual components, and body inertia parameters are extracted item by item according to the fuselage coordinate direction. The equivalent inertial torque generated by the angular acceleration mapping and the control torque residual components are algebraically superimposed. The algebraically superimposed torque components are continuously arranged according to the acquisition time to generate the lumped disturbance torque parameters.

[0019] Specifically, the angular acceleration parameters of the UAV's rotors are acquired in harsh outdoor environments. This is achieved through the UAV's built-in inertial measurement unit (IMU) and the encoders of each rotor motor. The time series of the aircraft's angular velocity and the time series of each rotor's rotational speed (RPM) during attitude adjustment are continuously acquired at a frequency of 100Hz. The aircraft's angular acceleration is obtained by performing first-order time-difference processing on the angular velocity data, and the rotor's angular acceleration is obtained by performing first-order time-difference processing on the rotor speed data, forming the original angular acceleration change sequence. Next, the acquired original sequence undergoes data cleaning. First, the integrity of each record is checked. Records with invalid or empty values ​​in any of the three key fields—acquisition time, rotor number, and angular acceleration direction marker—are marked and removed. For example, if the rotor number of a quadcopter UAV is not a value between 1 and 4, or if the angular acceleration direction marker is not the preset +, the record will be removed. If the value is 1 (clockwise) or -1 (counterclockwise), the record is considered invalid. Then, the rotors are grouped according to their unique numbers, and within each group, they are sorted in ascending order based on the acquisition time. This completes the processing of the UAV rotor angular acceleration parameters and simultaneously acquires the fuselage control torque residual parameters. This process first reads the desired fuselage control torque vector sequence generated by the position and attitude cascade control law from the flight control log. At the same time, based on the processed rotor speed data, the actual speed of each rotor is converted into the actual output response torque using the torque coefficient provided by the motor manufacturer. Then, combined with the rotor installation orientation parameters, the response torques of each rotor are synthesized into the actual fuselage response torque vector. Finally, at the same acquisition time, the desired fuselage control torque vector and the actual fuselage response torque vector are subtracted to calculate the difference between the two, thus obtaining the UAV rotor angular acceleration parameters and fuselage control torque residual parameters.

[0020] The system calls the pre-calibrated UAV body inertia parameters stored in the flight control non-volatile memory. These parameters are a 3x3 inertia tensor matrix, where the diagonal elements represent the moments of inertia about the x, y, and z axes of the fuselage, and the off-diagonal elements represent the inertia product. Simultaneously, it calls the rotor mounting orientation parameters, which define the three-dimensional coordinate position and thrust vector direction of each rotor relative to the fuselage's center of gravity. The system also obtains the fuselage attitude angle change parameters in real time from the inertial measurement unit, i.e., the three-axis angular velocities and three-axis angles in the body coordinate system. Then, the UAV rotor angular acceleration parameters obtained in the previous step are converted into gyroscopic torques on the fuselage based on their corresponding rotor numbers and the lever arm vectors defined in the rotor mounting orientation parameters, and mapped. Angular acceleration mappings are generated in the pitch, roll, and yaw directions of the fuselage coordinate system. Simultaneously, the three-dimensional vector of the fuselage control torque residual parameter is directly decomposed into three torque components along the x-axis (roll), y-axis (pitch), and z-axis (yaw) of the fuselage coordinate system, resulting in three components of the control torque residual. After completing the conversion and decomposition of the two core parameters, the angular acceleration mapping (a three-dimensional vector) at the same time is aligned and merged with the three control torque residual components based on the acquisition time as a unique identifier, constructing a structured time series dataset. Each row of data contains a timestamp and the corresponding angular acceleration mapping and control torque residual components, forming the momentum observer input parameters.

[0021] The momentum observer input parameters generated in the previous step are substituted one by one into a momentum observer model constructed based on the simplified Lagrange dynamics equations for calculation. This observer aims to statically estimate the lumped disturbance torque acting on the UAV fuselage. Its core calculation logic is a single algebraic superposition operation. In practice, for each data point at each acquisition time, the angular acceleration mapping and control torque residual components are extracted from the momentum observer input parameters according to the fuselage coordinate direction (roll, pitch, yaw), and the fuselage inertia parameters are called. Then, the observer performs the first step of calculation, calculating the angular acceleration mapping and the fuselage inertia parameters to estimate the equivalent inertial torque generated by the change in fuselage angular acceleration. This process involves left-multiplying the fuselage angular acceleration vector. The inertial tensor matrix of the organism is used to obtain the equivalent inertial torque vector. Then, the observer performs the core algebraic superposition step, adding the calculated equivalent inertial torque vector to the control torque residual component vector at the corresponding moment in the momentum observer input parameters. This superposition operation is defined as a compensation process, where the control torque residual component is regarded as a correction term for the ideal control model. By superimposing it with the dynamic inertial torque, the combined disturbance effect caused by external environment and internal uncertainty on the organism is characterized. Finally, the resultant torque vector obtained after algebraic superposition at each acquisition moment is arranged continuously according to the order of acquisition time to form a time series representing the change of disturbance torque over time, generating the lumped disturbance torque parameter.

[0022] The steps to obtain the two-dimensional offset coordinates of the centroid are as follows: The lumped disturbance moment parameters are input into the frequency domain filter. The roll, pitch, and yaw moment components in the lumped disturbance moment parameters are read according to the acquisition time. The moment components in each direction are marked with frequency bands. The slowly varying moment components and the fast wave moment components are distinguished according to the frequency band markings. The slowly varying moment components are sorted into low-frequency disturbance moment components, and the fast wave moment components are sorted into high-frequency disturbance moment components, thus obtaining the low-frequency disturbance moment components and the high-frequency disturbance moment components. Extract the low-frequency disturbance moment values ​​in the roll and pitch directions from the low-frequency disturbance moment components. Using the UAV's weight parameters, perform a scalar division operation on the roll direction low-frequency disturbance moment value according to the UAV's weight parameters to generate a lateral centroid offset value. Perform a scalar division operation on the pitch direction low-frequency disturbance moment value according to the UAV's weight parameters to generate a longitudinal centroid offset value. Combine the lateral and longitudinal centroid offset values ​​according to their coordinate order to generate a two-dimensional centroid offset coordinate system.

[0023] Specifically, the lumped disturbance moment parameters are input into a digital frequency domain filter for processing. In practice, for the previously generated time series of lumped disturbance moment parameters, which contains moment components in the roll, pitch, and yaw directions at each acquisition moment, a fourth-order Butterworth filter bank is applied to the time series of these three components for frequency band separation. This filter bank consists of a low-pass filter and a high-pass filter, with its cutoff frequency set to a preset frequency band division threshold. This threshold is based on historical statistical analysis of UAV flight data in harsh field environments. Through power spectral density estimation of the lumped disturbance moment parameters, it was found that the energy of the slowly varying moment components caused by factors such as mass imbalance and slight swaying of the payload is mainly concentrated in the frequency band below 0.5Hz, while the energy of the fast wave moment components caused by high-frequency airflow disturbances such as gusts is mainly distributed... In the frequency band above 0.5Hz, 0.5Hz is taken as the reference value to distinguish between the two components. For example, in a certain flight test, if 95% of the energy spectrum of the slowly varying torque component is distributed within 0.4Hz, while 95% of the energy spectrum of the fast wave torque component is distributed above 0.6Hz, then 0.5Hz is a reasonable dividing point. Applying this cutoff frequency to a Butterworth low-pass filter to filter the time series of torque components in each direction, the result is the slowly varying torque component. This is then processed into low-frequency disturbance torque components. Subsequently, the original time series of torque components in each direction is subtracted from their corresponding slowly varying torque components, and the difference is the fast wave torque component. This process is equivalent to applying a high-pass filter with the same cutoff frequency to process the obtained fast wave torque component into high-frequency disturbance torque components, thus obtaining low-frequency disturbance torque components and high-frequency disturbance torque components.

[0024] Extract the low-frequency disturbance moment values ​​in the roll and pitch directions from the low-frequency disturbance moment components generated in the previous step, and call the UAV body weight parameter stored in the flight control parameter table. This parameter is the total weight of the UAV including all mounted equipment, measured during ground calibration. For example, for a UAV with a total mass of 5 kg, its body weight parameter is 49 Newtons. This calculation is based on a static moment balance, that is, the observed low-frequency disturbance moment is mainly generated by a quasi-static offset of the UAV's actual center of mass relative to its geometric center. This offset lever arm multiplied by the body weight forms the disturbance moment. Therefore, this offset can be estimated by reverse calculation. Specifically, for each acquisition time, the roll and pitch directions of the UAV body weight at that time are calculated. The low-frequency disturbance moment value in the roll direction is calculated by scalar division according to the UAV's body weight parameters. For example, if the low-frequency disturbance moment in the roll direction is -0.98 Nm at a certain moment, then -0.98 Nm is divided by 49 Nm to get -0.02 m. This result is the lateral centroid offset value. Similarly, the low-frequency disturbance moment value in the pitch direction at the same moment, for example, 0.49 Nm, is calculated by scalar division according to the UAV's body weight parameters to get 0.01 m, forming the longitudinal centroid offset value. After completing the calculation of the offsets in the two directions, according to the definition of longitudinal (x-axis) and lateral (y-axis) in the body coordinate system, the longitudinal centroid offset value and the lateral centroid offset value are combined in coordinate order to form a two-dimensional vector (0.01, -0.02), generating the two-dimensional centroid offset coordinates.

[0025] The steps for obtaining the environmental wind disturbance moment components are as follows: The high-frequency disturbance moment values ​​in the roll, pitch, and yaw directions are extracted from the high-frequency disturbance moment components. The direction and time markers of the high-frequency disturbance moment components at each acquisition time are checked. High-frequency disturbance moment components with missing direction or time markers are removed. The remaining high-frequency disturbance moment components are arranged continuously according to the acquisition time and aggregated according to the fuselage coordinate direction to generate the environmental wind disturbance moment components.

[0026] Specifically, the high-frequency disturbance torque component separated in the previous step is extracted. This component is considered to be the main component of the environmental wind disturbance torque. Before officially using it as the environmental wind disturbance torque component, a data integrity check is performed. First, the timestamps are checked. The time series of the high-frequency disturbance torque component is traversed, and the difference between the timestamps of two adjacent acquisition times is checked to see if it is within the preset valid range. This range is set according to the system's 100Hz sampling frequency, with a lower limit of 8 milliseconds and an upper limit of 12 milliseconds. If the timestamp difference exceeds this range, or if the timestamps are not monotonically increasing, the data records at abnormal time points are marked as invalid and discarded. Second, the direction markings are checked to see if the high-frequency disturbance torque component is within the specified range at each retained acquisition time. The roll, pitch, and yaw torque values ​​of the frequency disturbance torque components are checked to ensure they are all valid finite values. If the value in any direction is infinite or non-numerical (NaN), the entire torque record at that acquisition time is considered invalid and discarded. After completing the above verification and discarding operations, all the remaining valid high-frequency disturbance torque component records are arranged continuously according to the order of acquisition time, and the components are aggregated according to the fuselage coordinate direction (roll, pitch, yaw). That is, the roll torque values ​​at all times are organized into a sequence, and the pitch and yaw directions are handled similarly, ultimately forming three time-synchronized torque sequences. This set of verified and organized data is finally determined, generating the environmental wind disturbance torque components.

[0027] The steps for obtaining the reference flat state-space variables are as follows: Obtain the current position coordinates and speed parameters of the AGV, read the horizontal coordinate values, vertical coordinate values ​​and vertical coordinate values ​​of the AGV in the coordinate system of the recycling area, extract the horizontal speed values, vertical speed values ​​and vertical speed values ​​of the AGV at the corresponding acquisition time, check the consistency between the coordinate acquisition time and the speed acquisition time, remove records with missing coordinate axis labels and missing speed direction labels, arrange the retained current position coordinates and speed parameters of the AGV according to the acquisition time, and generate terminal constraint conditions; Read the horizontal, vertical, and longitudinal coordinate values, as well as the horizontal, longitudinal, and vertical velocity values ​​from the terminal constraints. Superimpose the two-dimensional offset coordinates of the centroid onto the horizontal and longitudinal coordinate values ​​of the UAV's current position coordinates. Perform nonlinear variable mapping operations on the superimposed UAV position coordinate values ​​to generate reference flat state space variables.

[0028] Specifically, the current position coordinates and speed parameters of the AGV are obtained. Data is collected at a frequency of 20Hz using the differential GPS (RTK-GPS) and inertial measurement unit (IMU) on the AGV. The lateral, longitudinal, and vertical coordinates of the AGV in a Cartesian coordinate system established with the center of the recovery area as the origin are read. Simultaneously, the lateral, longitudinal, and vertical speed values ​​in the same coordinate system are extracted from the fusion calculation results of the AGV's wheel odometer and IMU. Data synchronization and cleaning are then performed. Due to the potential slight time delay between the position and speed data sources, each position data point... A preset synchronization window is searched along the time axis. The width of this window is set to 1.5 times the sampling period, i.e., 75 milliseconds. Within this window, the speed data point with the closest timestamp is searched for pairing. If no pairing is found, the data point is discarded. Next, the validity of the successfully paired data is checked, and abnormal records with coordinate axis labels other than x, y, z or speed direction labels other than vx, vy, vz are removed. After cleaning, the retained, time-synchronized AGV current position coordinates and speed parameters are arranged in ascending order according to the acquisition time to form a state sequence containing timestamps, three-dimensional position vectors, and three-dimensional speed vectors, generating terminal constraints.

[0029] The system reads the lateral, longitudinal, and vertical coordinates, as well as the lateral, longitudinal, and vertical velocities from the terminal constraints generated in the previous step. These values ​​constitute the final state of the UAV's landing trajectory. Simultaneously, it obtains the UAV's current position coordinates from its own navigation system and vector-superimposes the previously calculated centroid two-dimensional offset coordinates (e.g., (0.01, -0.02) meters) onto the UAV's current position coordinates to obtain a compensated initial position coordinate. Then, it uses the compensated initial position coordinate, the UAV's current velocity, and the AGV's position and velocity (as the final state) as boundary conditions for a nonlinear variable mapping operation. This operation is based on differential flatness theory, selecting the UAV's position (x, y, ...) as the boundary conditions. The yaw angle (ψ) and z) are used as flat outputs. Through these flat outputs and their time derivatives, the complete state of the UAV (including attitude angles) and control inputs can be algebraically determined. The mapping process transforms the complex nonlinear dynamic model of the UAV into an equivalent linear system. Specifically, the compensated initial position coordinates and current velocity of the UAV are used as the flat outputs at the initial time t=0, and the position and velocity of the AGV in the terminal constraint are used as the flat outputs at the target time t=T, where T is the preset total landing time, for example, set to 10 seconds. This process transforms the UAV landing problem from a complex nonlinear control problem into a problem of solving boundary values ​​in a flat space, generating reference flat state space variables.

[0030] The steps for obtaining the landing and recovery reference trajectory are as follows: The position, velocity, attitude, and time elements are extracted from the reference flat state space variables one by one. The first-order time derivative of the position element is solved in the order of the time elements, the second-order time derivative of the velocity element is solved, and the attitude change rate of the attitude element is solved. The solved time change elements are then matrix-concatenated in the order of position, velocity, attitude, and time to generate the landing and recovery reference trajectory.

[0031] Specifically, the reference flat state-space variables generated in the previous step are extracted item by item. These variables include the four-dimensional flat outputs (x, y, z, ψ) of the trajectory's starting point (t=0) and ending point (t=T), along with their first derivative (velocity) and second derivative (acceleration, usually set to 0) boundary conditions. Subsequently, multi-order time differential calculations are performed independently on each flat output component. This calculation uses a seventh-order polynomial interpolation method to generate a smooth trajectory. For the trajectory in the x-direction... Its form is Using the eight boundary conditions—position, velocity, acceleration, and jerk (usually set to 0) at the starting and ending points—a system of linear equations can be constructed to uniquely determine the position of the starting and ending points. arrive These eight polynomial coefficients undergo the same solution process for the y, z, and ψ components, yielding their respective polynomial expressions that vary with time. Then, at a preset time step, such as 0.02 seconds, these four polynomials are evaluated within the interval from 0 to T, and their first to fourth time derivatives are calculated, yielding the position, velocity, acceleration, jerk, and jerk-jerk elements for each moment. Finally, a matrix concatenation calculation is performed. All time-varying elements obtained at each time step are horizontally concatenated in the order of [time column, position column (x, y, z), velocity column (vx, vy, vz), attitude column (ψ), attitude change rate column (ψ_dot), ...] to form a large state matrix. Each row of this matrix represents the complete reference state at a specific moment. All rows are combined to generate the landing and recovery reference trajectory.

[0032] The steps for obtaining the landing position tracking deviation variable are as follows: Spatial subtraction is performed between the landing and recovery reference trajectory and the current position coordinates of the UAV. The horizontal, vertical, and longitudinal reference position values ​​in the landing and recovery reference trajectory are read at the same acquisition time. The horizontal, vertical, and longitudinal current position values ​​in the current position coordinates of the UAV are also read. The spatial difference between the reference position values ​​and the current position values ​​is calculated axis by axis. The spatial difference is arranged in the order of horizontal, vertical, and longitudinal spatial difference to generate the landing position tracking deviation variable.

[0033] Specifically, a spatial subtraction operation is performed between the landing and recovery reference trajectory and the current position coordinates of the UAV. This process is carried out in real time within each control cycle. First, based on the current system timestamp, the reference position at the current moment is obtained from the landing and recovery reference trajectory data through linear interpolation. Specifically, two reference points that are temporally adjacent to the current moment are found in the reference trajectory, and their lateral, longitudinal, and vertical reference position values ​​are read. Based on the relative position of the current moment in the time interval between these two reference points, the interpolated reference position at the current moment is calculated proportionally. At the same time, the lateral, longitudinal, and vertical current position values ​​at the same moment are obtained from the UAV's own navigation system. Then, a subtraction operation is performed on each coordinate axis, subtracting the corresponding current position value from the interpolated reference position value. For example, the lateral spatial difference is the interpolated lateral reference position minus the lateral current position. The longitudinal and vertical spatial differences are calculated in the same way. The three differences obtained from this calculation represent the position deviations that the UAV needs to correct in the three axes. Finally, the calculated lateral, longitudinal, and vertical spatial differences are combined into a three-dimensional vector to generate the landing position tracking deviation variable.

[0034] The steps for obtaining the rotor drive parameters of a UAV are as follows: Substitute the landing position tracking deviation variable into the position and attitude cascade control law equation, extract the lateral spatial difference, longitudinal spatial difference, and vertical spatial difference according to the acquisition time, differentiate the changes in spatial difference between adjacent acquisition times, match the derivative results with the lateral position gain, longitudinal position gain, and vertical position gain respectively, and then convert the matched gain solution into roll moment components, pitch moment components, and yaw moment components to generate the desired fuselage control torque; At the same acquisition time, the roll, pitch, and yaw torque components of the desired fuselage control torque are read, and the roll, pitch, and yaw wind disturbance torque values ​​of the environmental wind disturbance torque components are read. The torque values ​​in the same fuselage coordinate direction are algebraically accumulated to form the disturbance rejection control torque. The disturbance rejection control torque is substituted into the UAV control allocation matrix and mapped to the corresponding drive quantity of each rotor according to the rotor number to generate the UAV rotor drive parameters.

[0035] Specifically, the landing position tracking deviation variable is substituted into a position and attitude cascade control law equation. The outer loop of this control law is the position controller. The lateral, longitudinal, and vertical spatial differences of the landing position tracking deviation variable are extracted according to the acquisition time. These differences and their rates of change over time are input into three independent PID controllers to calculate the desired acceleration that the UAV needs to achieve. For example, the desired vertical acceleration is calculated using the formula... Perform calculations, where, It is the calculated expected vertical acceleration. It is the vertical spatial difference. It is the cumulative integral of the vertical spatial difference over time. It is the derivative of the vertical spatial difference with respect to time, i.e., the rate of change. , and These are the pre-tuned vertical position ratio, integral, and differential gain coefficients, for example, set to 3.5, 0.8, and 2.0 respectively based on flight test data. The desired lateral and longitudinal accelerations are calculated in a similar way. After obtaining the desired acceleration vector, it is converted into the desired total thrust and desired attitude angle. The desired total thrust is calculated from the desired vertical acceleration and gravity. The desired roll and pitch angles are calculated from the desired lateral and longitudinal accelerations through coordinate system rotation transformation. These desired attitude angles will be used as inputs to the inner-loop attitude controller. The attitude controller also uses a PID control law to compare the desired attitude angles with the actual attitude angles fed back by the UAV's inertial measurement unit, calculate the roll torque component, pitch torque component, and yaw torque component used to correct attitude deviations, and generate the desired fuselage control torque.

[0036] At the same acquisition time, the roll, pitch, and yaw moment components of the desired fuselage control torque generated in the previous step are read, along with the corresponding wind disturbance moment values ​​from the previously separated environmental wind disturbance moment components. Feedforward compensation calculations are then performed. Specifically, each component of the desired fuselage control torque is algebraically summed with its corresponding component of the environmental wind disturbance moment. For example, the final roll control torque equals the desired roll torque plus the estimated roll wind disturbance moment. The pitch and yaw moments are also summed in the same way to form the disturbance rejection control torque. Subsequently, this disturbance rejection control torque, along with the desired total thrust calculated by the outer loop controller, is substituted into the UAV's control allocation matrix for solution. The centralized control commands are then distributed to each rotor. This process is achieved by solving a system of linear equations. ,in, It is the expected total thrust. , , These are the three components of the disturbance rejection control torque. It is the target rotational speed of the i-th rotor. It is the rotor thrust coefficient. It is the rotor's anti-torque coefficient. These are the distances from the rotor center to the fuselage center of gravity. All three parameters are known constants obtained through experimental calibration. For example... The value is 0.22 meters. By solving the system of equations, the square value of the target rotational speed of each rotor is obtained. Then, the square root of the square root is used to obtain the target rotational speed of each rotor. Finally, the target rotational speed is converted into a PWM signal that can be executed by the motor driver to generate the rotor drive parameters of the UAV.

[0037] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments that can be applied to other fields. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A fully automated and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments, characterized in that, Includes the following steps: The rotor angular acceleration parameters and control torque residual parameters of the UAV in harsh field environments are obtained, and calculations are performed to generate lumped disturbance torque parameters. The lumped disturbance moment parameters are input into a frequency domain filter for filtering frequency band separation calculation. The low-frequency disturbance moment components and high-frequency disturbance moment components are extracted. The low-frequency disturbance moment components are calculated with the UAV body weight parameters to generate two-dimensional centroid offset coordinates. The high-frequency disturbance moment components are extracted as environmental wind disturbance moment components. The current position coordinates and velocity parameters of the AGV are obtained. The current position coordinates and velocity parameters of the AGV are used as terminal constraints. Combined with the two-dimensional offset coordinates of the centroid and the current position coordinates of the UAV, variable mapping operation is performed to generate a reference flat state space variable. The elements of the reference flat state space variable are extracted and multi-order time differential solution operation and matrix splicing calculation are performed to generate a landing and recovery reference trajectory.

2. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 1, characterized in that, The method further includes: The landing and recovery reference trajectory is spatially subtracted from the current position coordinates of the UAV to generate a landing position tracking deviation variable. The landing position tracking deviation variable is substituted into the position and attitude cascade control law equation for differentiation and gain calculation to generate the desired fuselage control torque. The desired fuselage control torque is algebraically accumulated with the environmental wind disturbance torque component to generate the disturbance rejection control torque. The disturbance rejection control torque is substituted into the UAV control allocation matrix for linear mapping calculation to generate the UAV rotor drive parameters.

3. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 1, characterized in that, The steps for obtaining the lumped disturbance moment parameters are as follows: To obtain the angular acceleration parameters of the UAV rotor in harsh field environments, read the angular acceleration change sequence of each UAV rotor during attitude adjustment, remove records with missing acquisition time, missing rotor number, and missing angular acceleration direction marking, arrange the angular acceleration change sequence according to rotor number and acquisition time, obtain the fuselage control torque residual parameter, extract the difference between the expected fuselage control torque and the actual fuselage response torque, and obtain the UAV rotor angular acceleration parameters and fuselage control torque residual parameters; The inertia parameters of the UAV body, the rotor installation orientation parameters, and the fuselage attitude angle change parameters are called. The angular acceleration parameters of the UAV rotor body are mapped to the fuselage coordinate direction according to the rotor number. The fuselage control torque residual parameters are decomposed into components according to the pitch direction, roll direction, and yaw direction. The angular acceleration mapping and control torque residual components at the same acquisition time are arranged accordingly to form the momentum observer input parameters. The momentum observer input parameters are substituted into the momentum observer constructed based on the Lagrange dynamics equations. The angular acceleration mapping, control torque residual components, and body inertia parameters are extracted item by item according to the fuselage coordinate direction. The equivalent inertial torque and control torque residual components generated by the angular acceleration mapping are algebraically superimposed. The algebraically superimposed torque components are continuously arranged according to the acquisition time to generate the lumped disturbance torque parameters.

4. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 1, characterized in that, The steps for obtaining the two-dimensional offset coordinates of the centroid are as follows: The lumped disturbance moment parameters are input into a frequency domain filter. The roll moment component, pitch moment component, and yaw moment component in the lumped disturbance moment parameters are read according to the acquisition time. The moment components in each direction are marked with frequency bands. The slowly varying moment components and the fast wave moment components are distinguished according to the frequency band markings. The slowly varying moment components are sorted into low-frequency disturbance moment components, and the fast wave moment components are sorted into high-frequency disturbance moment components, thus obtaining low-frequency disturbance moment components and high-frequency disturbance moment components. Extract the low-frequency disturbance moment values ​​in the roll and pitch directions from the low-frequency disturbance moment components. Using the UAV's body weight parameters, perform a scalar division operation on the roll direction low-frequency disturbance moment value according to the UAV's body weight parameters to generate a lateral centroid offset value. Perform a scalar division operation on the pitch direction low-frequency disturbance moment value according to the UAV's body weight parameters to generate a longitudinal centroid offset value. Combine the lateral and longitudinal centroid offset values ​​according to their coordinate order to generate a two-dimensional centroid offset coordinate system.

5. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 1, characterized in that, The steps for obtaining the environmental wind disturbance moment components are as follows: Extract the high-frequency disturbance moment values ​​in the roll direction, pitch direction, and yaw direction from the high-frequency disturbance moment components. Verify the direction and time markers of the high-frequency disturbance moment components at each acquisition time. Remove the high-frequency disturbance moment components with missing direction or time markers. Arrange the remaining high-frequency disturbance moment components continuously according to the acquisition time and aggregate them according to the fuselage coordinate direction to generate the environmental wind disturbance moment components.

6. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 1, characterized in that, The steps for obtaining the reference flat state space variables are as follows: Obtain the current position coordinates and speed parameters of the AGV, read the horizontal coordinate values, vertical coordinate values ​​and vertical coordinate values ​​of the AGV in the coordinate system of the recycling area, extract the horizontal speed values, vertical speed values ​​and vertical speed values ​​of the AGV at the corresponding acquisition time, check the consistency between the coordinate acquisition time and the speed acquisition time, remove records with missing coordinate axis labels and missing speed direction labels, arrange the retained current position coordinates and speed parameters of the AGV according to the acquisition time, and generate terminal constraint conditions; Read the horizontal coordinate values, vertical coordinate values, horizontal velocity values, vertical velocity values, and vertical velocity values ​​from the terminal constraints. Superimpose the two-dimensional offset coordinates of the centroid onto the horizontal and vertical coordinate values ​​of the current position coordinates of the UAV. Perform nonlinear variable mapping calculations on the superimposed UAV position coordinate values ​​to generate reference flat state space variables.

7. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 1, characterized in that, The steps for obtaining the landing and recovery reference trajectory are as follows: The position, velocity, attitude, and time elements of the reference flat state space variables are extracted one by one. The first-order time derivative of the position element is solved according to the order of the time elements, the second-order time derivative of the velocity element is solved, and the attitude change rate of the attitude element is solved. The solved time change elements are then matrix-concatenated according to the order of position, velocity, attitude, and time to generate the landing and recovery reference trajectory.

8. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 2, characterized in that, The steps for obtaining the landing position tracking deviation variable are as follows: The landing and recovery reference trajectory is spatially subtracted from the current position coordinates of the UAV. The horizontal, vertical, and longitudinal reference position values ​​in the landing and recovery reference trajectory are read at the same acquisition time. The horizontal, vertical, and longitudinal current position values ​​in the current position coordinates of the UAV are also read. The spatial difference between the reference position values ​​and the current position values ​​is calculated axis by axis. The spatial difference is arranged in the order of horizontal spatial difference, vertical spatial difference, and longitudinal spatial difference to generate the landing position tracking deviation variable.

9. The fully automatic and precise recovery control method for unmanned aerial vehicles (AGVs) in harsh field environments according to claim 2, characterized in that, The steps for obtaining the UAV rotor drive parameters are as follows: Substitute the landing position tracking deviation variable into the position and attitude cascade control law equation, extract the lateral spatial difference, longitudinal spatial difference and vertical spatial difference according to the acquisition time, perform derivative processing on the change of spatial difference between adjacent acquisition times, match the derivative processing results with lateral position gain, longitudinal position gain and vertical position gain respectively, and then convert the matched gain solution into roll direction moment component, pitch direction moment component and yaw direction moment component to generate the desired fuselage control torque; The roll, pitch, and yaw torque components of the desired fuselage control torque are read at the same acquisition time. The roll, pitch, and yaw wind disturbance torque values ​​of the environmental wind disturbance torque components are also read. The torque values ​​in the same fuselage coordinate direction are algebraically accumulated to form the disturbance rejection control torque. The disturbance rejection control torque is substituted into the UAV control allocation matrix and mapped to the corresponding drive quantity of each rotor according to the rotor number to generate the UAV rotor drive parameters.

Images