A manned-unmanned aerial vehicle cooperative formation control and adaptive obstacle avoidance method

Abstract

This invention discloses a manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method, relating to the field of aircraft cooperative control technology. The method includes acquiring the state information of the lead manned aircraft and following UAVs within the formation; filtering and estimating the state information of the lead manned aircraft to obtain a posterior state estimate, which includes posterior position, posterior velocity, and posterior yaw angle; calculating the turn rate based on the difference in posterior yaw angles between adjacent periods; and calculating the predicted position and predicted velocity of the lead manned aircraft. Based on the posterior and predicted positions, a forward-shifting envelope center is determined through linear interpolation; an envelope direction reference is determined based on the predicted velocity; and a lateral offset is determined based on the turn rate, constructing a dynamic asymmetric safety envelope. This eliminates the inner collision blind spot during maneuvering and following, improving the safety and stability of formation flight.

Description

Technical Field

[0001] This invention relates to the field of aircraft cooperative control technology, and in particular to a manned-unmanned aircraft cooperative formation control and adaptive obstacle avoidance method. Background Technology

[0002] With the rapid development of modern aviation technology, manned and unmanned aircraft coordinated formation has become the core development direction in modern aviation operations and autonomous swarms. In a typical lead-follow configuration, the lead manned aircraft is responsible for mission decision-making and global path planning, while the follower unmanned aircraft needs to autonomously maintain the desired relative spatial formation and have the ability to autonomously avoid unknown environmental obstacles.

[0003] Existing technical solutions still have certain shortcomings. Existing cooperative formation methods based on fixed path planning or simplified dynamic models are usually difficult to cope with real-time changing environments, especially in the presence of many obstacles or sudden interference. They are difficult to guarantee safe distances between aircraft and flight stability. Traditional control methods often focus on simple following behavior, while neglecting the ability to adaptively adjust to various flight states. Summary of the Invention

[0004] This invention provides a manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method, comprising: The state information of the lead manned aircraft and the follower unmanned aircraft in the formation is obtained. The state information of the lead manned aircraft is filtered and estimated to obtain the posterior state estimate. The posterior state estimate includes the posterior position, posterior velocity and posterior yaw angle. The turning rate is calculated based on the difference of the posterior yaw angle in adjacent periods. The predicted position and predicted velocity of the lead manned aircraft are also calculated. Based on the posterior position and the predicted position, the forward envelope center is determined by linear interpolation, the envelope direction reference is determined based on the predicted velocity, and the lateral offset is determined based on the turning rate, thus constructing a dynamic asymmetric safety envelope. The difference between the predicted position and the posterior position is used as the intention feedforward correction term. Combined with the expected relative position of the formation, the intention correction formation error of the following UAV is calculated. The nominal control command is generated by combining the posterior velocity, the predicted velocity and the state information of the following UAV. Based on the dynamic asymmetric safety envelope, a navigation safety barrier function is constructed, and an obstacle safety barrier function is constructed based on the measurement information of external obstacles. For the second-order integrator model, the navigation safety barrier function and the obstacle safety barrier function are transformed into a set of linear safety constraints. Using nominal control commands as the optimization objective, a quadratic programming problem with slack variables is constructed based on a set of linear safety constraints. The solution generates safety filtering control commands. During the execution of safety filtering control commands, if the preset obstacle avoidance completion criterion is met, the terminal sliding mode control law is activated, and a recovery control command is generated under the constraints of the set of linear safety constraints.

[0005] As a preferred embodiment of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method of the present invention, wherein: the step of filtering and estimating the state information of the lead manned aircraft to obtain the posterior state estimate includes: The position and velocity of the follower drone in the inertial coordinate system are obtained as the state information of the follower drone; The position and geometric envelope dimensions of external obstacles relative to the following drone are obtained as external obstacle measurement information; Receive the position, speed and yaw angle broadcast by the navigating manned aircraft, construct the navigating state vector as the state information of the navigating manned aircraft; The posterior state estimate is obtained by recursively estimating the pilot state vector using a Kalman filter based on a piecewise constant velocity state transition model.

[0006] As a preferred embodiment of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method of the present invention, wherein: the calculation of the predicted position and predicted velocity of the lead manned aircraft within a short-term prediction window includes: Extract posterior position, posterior velocity, and posterior yaw angle from the posterior state estimate; The predicted position is obtained by adding the posterior position and the displacement increment of the posterior velocity within the short-time prediction window; and the posterior velocity is used as the predicted velocity. The difference between the posterior yaw angle of the current cycle and the posterior yaw angle of the previous cycle is calculated. The difference is then wrapped with the principal angle value and divided by the control cycle to obtain the turning rate.

[0007] As a preferred embodiment of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method of the present invention, wherein: the construction of the dynamic asymmetric safety envelope includes: Linear interpolation is performed between the posterior position and the predicted position to obtain the nominal envelope center that moves forward along the predicted motion direction; When the magnitude of the predicted velocity is greater than the preset velocity threshold, the normalized vector of the predicted velocity is used as the envelope direction reference. When the magnitude of the predicted velocity is not greater than the preset velocity threshold, the envelope direction reference is determined based on the posterior yaw angle. Construct the lateral unit direction and vertical unit direction based on the envelope direction reference, and calculate the lateral offset along the lateral unit direction according to the turning rate. The nominal envelope center is offset by a lateral offset in the lateral unit direction to obtain the offset envelope center. Based on the offset envelope center, the envelope direction reference, the lateral unit direction, the vertical unit direction, and the preset semi-axis length, an ellipsoidal dynamic asymmetric safety envelope is constructed.

[0008] As a preferred embodiment of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method of the present invention, wherein: the generation of nominal control commands includes: The intention feedforward displacement is obtained by multiplying the difference between the predicted position and the posterior position by a preset intention feedforward gain. The difference between the posterior position and the expected relative position of the formation is calculated and extracted from the current position of the following drone from the state information of the following drone. This difference is then added to the intention feedforward displacement to obtain the intention-corrected formation error. The posterior velocity of the current cycle is used to calculate the difference between the posterior velocity of the previous cycle to obtain the feedforward acceleration of the navigation maneuver. Based on the intention to correct formation error, the velocity error between the posterior velocity and the current velocity of the following drone extracted from the state information of the following drone, and the feedforward acceleration of the lead maneuver, a nominal control command representing the desired acceleration is generated through a proportional-derivative control law.

[0009] As a preferred embodiment of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method of the present invention, wherein: the transformation of the navigation safety barrier function and the obstacle safety barrier function into a linear set of safety constraints containing control inputs includes: Calculate the first-order Lie derivative, second-order Lie derivative, and control-related Lie derivative of the navigation safety barrier function and the obstacle safety barrier function with respect to the state variables of the second-order integrator model, respectively. By combining the first-order Lie derivative, the second-order Lie derivative, and the control-related Lie derivative with the preset first-order decay rate parameter and zero-order decay rate parameter, a higher-order control barrier constraint inequality is constructed. The higher-order control barrier constraint inequalities are expanded and rearranged into linear inequalities, and then combined to obtain a set of linear safety constraints.

[0010] As a preferred embodiment of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method of the present invention, wherein: the step of constructing a quadratic programming problem with slack variables based on a linear safety constraint set, and solving it to generate safety filtering control commands, includes: The optimization objective is to minimize the deviation between the control input to be sought and the nominal control command, and a squared penalty term for the slack variable is added to the optimization objective. The constraints are the linear safety constraint set, the control input amplitude constraint, and the non-negativity constraint of the slack variable, and slack variables are added to the right-hand side of the linear safety constraint set. The quadratic programming problem is solved by convex optimization to obtain the safety filter control command.

[0011] As a preferred embodiment of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method of the present invention, wherein: the activation of the terminal sliding mode control law and the generation of recovery control commands under the constraints of a linear safety constraint set include: When the slack variable is zero, the values ​​of all obstacle safety barrier functions are greater than the preset recovery trigger threshold, and this state continues to reach the preset confirmation time window, the obstacle avoidance completion criterion is satisfied. Based on the position error between the current position of the following drone and the desired formation position extracted from the state information of the following drone, and the speed error between the current speed of the following drone and the desired formation speed extracted from the state information of the following drone, a linear sliding surface and a comprehensive error vector are constructed. The nominal control command in the quadratic programming problem is replaced with the nominal recovery command, and the recovery control command is obtained by solving the problem until the preset steady-state tolerance is met, and then the nominal formation control mode is switched back.

[0012] The beneficial effects of this invention are: This invention introduces a short-term intention prediction and turn rate lateral bias mechanism to construct a dynamic asymmetric safety envelope that deforms in real time with the navigator's movement intention, eliminating the inner collision blind zone during maneuvering and ensuring a safe distance between aircraft. Targeting the second-order dynamics characteristics of UAVs, it innovatively employs a high-order control barrier function to rigorously transform position-level constraints into acceleration control constraints. A quadratic programming solver with relaxation variables is used for safety filtering, ensuring absolute flight stability under multiple obstacles and sudden disturbances. After obstacle avoidance is removed, a fractional-order terminal sliding mode control law with finite-time convergence is re-embedded into the quadratic programming safety constraint framework, enabling the UAV to quickly and agilely recover the desired formation while strictly adhering to any safety boundaries, eliminating the risk of secondary collisions in complex environments. Attached Figure Description

[0013] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0014] Figure 1 This is a flowchart of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method in Example 1; Figure 2 This is a schematic diagram of the manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method in Example 1. Detailed Implementation

[0015] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0016] Example 1, referring to Figure 1 and Figure 2 This embodiment of the invention provides a manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method, comprising the following steps: S1. Obtain the state information of the lead manned aircraft and the follower unmanned aircraft in the formation, filter and estimate the state information of the lead manned aircraft to obtain the posterior state estimate, the posterior state estimate includes the posterior position, posterior velocity and posterior yaw angle, calculate the turning rate based on the difference of the posterior yaw angle of adjacent periods, and calculate the predicted position and predicted velocity of the lead manned aircraft. Specifically, the follower drone uses an onboard inertial measurement unit and a global navigation satellite system receiver to measure the drone's three-dimensional position vector and three-dimensional velocity vector in the inertial coordinate system as the drone's state information. It also uses an onboard lidar to measure the three-dimensional position vector and geometric envelope size of nearby environmental obstacles relative to the drone as external obstacle measurement information. Through a directed communication link, which allows for a transmission delay of no more than 200 milliseconds and a packet loss rate of no more than 10%, the system receives the current status message of the navigating manned aircraft. This message is generated by the navigating aircraft's onboard navigation system, encoded via a data link, and sent out. Its content includes the following seven scalar data items: the three coordinate components of the navigating aircraft's current position in the inertial coordinate system, the three components of the navigating aircraft's current velocity, and the navigating aircraft's current yaw angle. The above seven data items are arranged in a fixed order to form the navigator's state vector; According to Newton's laws of motion, the lead manned aircraft is considered as a point mass driven by an unknown acceleration; Between adjacent control cycles, it is assumed that the navigator undergoes uniformly accelerated motion. Its position change is equal to the initial velocity multiplied by the time interval plus the quadratic term caused by the acceleration, and its velocity change is equal to the acceleration multiplied by the time interval. Since the acceleration is unknown, it is treated as a random disturbance and included in the process noise term. Under this assumption, the discrete-time state transition equation of the navigator is: ; in, For the navigator The state vector at time t, Let be the process noise vector, used to represent the model error caused by the unknown acceleration. It is assumed to follow a pattern with zero mean and a covariance matrix of . The Gaussian distribution is used, and Q is set as a diagonal matrix, whose diagonal elements are determined based on empirical values ​​of the typical maneuverability of the navigator: the position noise variance is taken as... The variance of velocity noise is taken Yaw angle noise variance is taken , The state transition matrix is ​​constructed based on a constant velocity motion model. Under this model, the position update equals the current velocity multiplied by the control period, and the velocity and yaw angle remain unchanged when there is no input. The specific block form of A is as follows: ; in, It is the identity matrix. It is a zero matrix. To control the cycle duration, it is determined by the system clock of the onboard mission computer, with a preferred value of 0.1 seconds; Kalman filtering is a standard method in the field of target tracking. Under the assumption of a linear Gaussian system, it provides the optimal linear unbiased estimate of the state by minimizing the covariance matrix of the posterior estimation error. In this step, time update and observation update are executed sequentially in each control cycle, and the Kalman filter is used to recursively estimate the navigation state observation vector. The above control cycle The posterior state estimate and posterior error covariance matrix are used as inputs to calculate the prior state estimate and prior error covariance matrix for the current period. This operation performs a forward prediction of the navigation state based on the motion model before obtaining new measurements for the current period. The formula for calculating the prior state estimate is: ; in, In order to be in The prior state estimation vector at time t represents the predicted value of the navigation state before the current measurement is used; The formula for calculating the prior error covariance matrix is: ; in, Let be the prior error covariance matrix, and let represent the confidence interval of the prior estimate. The larger the diagonal elements, the more uncertain the estimate of the corresponding state component. This is the transpose of the state transition matrix; The two formulas above are as follows: the prior state estimate is obtained by directly forwarding the motion model, and the prior covariance matrix is ​​obtained by superimposing the process noise covariance after the posterior covariance of the previous cycle is propagated through the state transition matrix. This propagation process is part of the standard framework of Kalman filtering and is the covariance propagation law of linear transformation of random vectors. Using the navigation state received in the current cycle as the measurement value, combined with the prior estimate and its covariance, the Kalman gain matrix is ​​calculated, and the posterior state estimate and posterior error covariance matrix are obtained accordingly. This operation uses the new measurement information to correct the bias of the model prediction. The formula for calculating the Kalman gain matrix is: ; in, The Kalman gain matrix determines the weight of the measurement correction. Its value is determined by the relative magnitudes of the prior uncertainty and the measurement uncertainty: when the prior uncertainty is large, the gain increases, and the correction effect is enhanced; when the measurement uncertainty is large, the gain decreases, and the correction effect is weakened. To observe the noise covariance matrix, it was calibrated using the airborne sensor accuracy manual. Specifically, the position measurement variance was taken as... Square meters, speed measurement variance taken square meters per square second, yaw angle measurement variance is taken as Squared radians, R is set as a diagonal matrix, and the diagonal elements correspond to the variance values ​​mentioned above; The formula for calculating the posterior state estimate is: ; in, This is the posterior state estimation vector, which is the optimal estimate of the current period's navigation state after measurement correction; The formula for calculating the posterior error covariance matrix is: ; in, It is a 7×7 identity matrix. Let be the posterior error covariance matrix, and let represent the confidence interval of the posterior estimate. The above Kalman gain calculation formula: In the standard Kalman filtering framework, the derivation of the gain matrix is ​​based on minimizing the trace of the posterior estimation error covariance matrix; When packet loss occurs in the communication link and no valid data is received in the current period, the observation update operation is skipped, and the prior state estimate is directly used as the posterior state estimate output. The posterior position, posterior velocity and posterior yaw angle are extracted from the posterior state estimate. Furthermore, the difference between the posterior yaw angle of the current cycle and the posterior yaw angle of the previous cycle is calculated, and the difference is processed by the main angle value and then divided by the control cycle to obtain the turn rate of the navigator. Using the current period's posterior state estimate as the starting condition, a future short-term window is generated through open-loop recursion. The predicted state within the time domain serves as the basis for subsequent asymmetric secure envelope time-domain feedforward; Specifically, the short-time prediction time domain length is preset. The value is 2.0 seconds. This value is set offline by the onboard mission computer based on the navigator's maximum maneuverability and safe advance response time. When extrapolating in the time domain, a state transition matrix is ​​used. It is assumed that the navigator's acceleration will be zero in the short time domain in the future (i.e., the current velocity vector will remain unchanged). Because the aircraft is constrained by physical inertia, its speed will not change significantly in such a short time. Under the above constant velocity assumption, the extrapolation process can be completed directly through analytical methods without the need for cycle-by-cycle iteration; Specifically, the navigator predicts the endpoint in the time domain. The position, velocity, and yaw angle at any given time can be directly calculated using the following formula: Predicted location It equals the current position plus the product of the current speed and the prediction duration: ; Predicting speed Equal to the current speed, and remain unchanged: ; Predicted yaw angle Equal to the current yaw angle, and remains unchanged: ; in, , as well as The posterior estimates at the current time are respectively Position, velocity, and yaw angle components; Combining the above three equations forms a complete predicted state vector. The formula is: ; This predicted state is the position and speed that the lead manned aircraft is expected to reach in the future, assuming that its current velocity vector remains unchanged. Together with the previously calculated turn rate, it is used in subsequent steps to construct a dynamic asymmetric safety envelope, thereby enabling the protection domain to predict the movement trend of the lead aircraft.

[0017] S2. Based on the posterior position and the predicted position, determine the forward envelope center through linear interpolation, determine the envelope direction reference based on the predicted velocity, determine the lateral offset based on the turning rate, and construct a dynamic asymmetric safety envelope. Specifically, the current position component of the navigator is extracted from the posterior state estimate, and the predicted position component is extracted from the predicted state. Linear interpolation is performed between the posterior position and the predicted position to obtain the nominal envelope center shifted forward along the predicted motion direction, as shown in the formula: ; in, For the current control cycle The safe envelope center location vector, The forward interpolation weighting coefficient is set to 0.3. The physical meaning of this coefficient is to push the envelope center forward by 30% of the predicted total displacement from the current position along the expected displacement direction. A coefficient of 0 indicates no forward movement (degenerates into a symmetrical envelope), and a coefficient of 1 indicates a direct jump to the predicted position. A value of 0.3 preserves the prediction information while avoiding excessive center shift due to prediction errors, thus providing ample protection space for the envelope in the direction of the navigator's movement. The formula treats the difference between the current position and the predicted position as the expected displacement vector of the navigator in the predicted time domain. By multiplying it by a forward shift coefficient less than 1, the center of the envelope is moved forward a certain distance along this displacement direction. The center of the safety envelope no longer coincides with the current position of the navigator, but is located at a point ahead of the navigator's expected trajectory, making the protection space of the envelope in the direction of the navigator's movement more abundant. When the magnitude of the predicted speed is greater than the preset speed threshold (e.g., 0.1 m / s), the normalized vector of the predicted speed is used as the envelope direction reference (i.e., the longitudinal unit direction). When the magnitude is not greater than the threshold (considered as hovering or extremely low speed), the envelope direction reference is determined based on the posterior yaw angle, with its horizontal component being the cosine and sine values ​​of the yaw angle and its vertical component being 0. Construct a lateral unit vector orthogonal to the above longitudinal direction. Based on the right-hand orthogonal coordinate system criterion, in order to make the lateral unit vector point to the right side of the navigator's forward movement, perform a cross product operation between the longitudinal unit vector and the vertical axis unit vector of the global coordinate system, and then normalize the result to obtain the lateral unit vector. When the longitudinal unit vector is parallel to the vertical axis (i.e., the navigator is flying vertically), the cross product result is a zero vector. In this case, the due east or due north direction in the global coordinate system is taken as the replacement value of the lateral unit vector. This degradation process only applies to the case where the longitudinal unit vector is obtained by normalizing the velocity vector. When the longitudinal vector is generated by yaw angle degradation, since its vertical component is always zero, it will not be parallel to the vertical axis. The vertical unit vector is obtained by the cross product of the longitudinal unit vector and the lateral unit vector to ensure that the three directions form a right-handed orthogonal coordinate system. Set the semi-axis length of the envelope along three orthogonal directions, for example, the longitudinal semi-axis is 15.0 meters, the lateral semi-axis is 8.0 meters, and the vertical semi-axis is 5.0 meters; The aforementioned half-shaft lengths are set offline by designers based on the typical dimensions of manned aircraft (wingspan of approximately 15 meters) and safety margins: approximately 1.0 times the wingspan is used as the protection distance in the longitudinal direction, approximately 0.5 times the wingspan is used as the protection distance in the lateral direction, and approximately 0.3 times the wingspan is used as the protection distance in the vertical direction. The above values ​​can be obtained from publicly available standards for unmanned aerial vehicle (UAV) formation flight safety (such as ASTM F3322-22 Standard Specification for Small Unmanned Aerial Vehicle Systems). The above parameters collectively define a rotational ellipsoid, namely an ellipsoid with three unequal semi-axis lengths, whose center is located at... The principal axis direction is determined by the three orthogonal directions mentioned above, and the semi-axis lengths are 15.0 meters, 8.0 meters, and 5.0 meters, respectively. This ellipsoid is the initial geometry of the constructed safety envelope. The turn rate is extracted from the posterior state estimate. This value represents the rate of change of the yaw angle of the navigator at the current moment, expressed in radians per second. The turn rate is calculated by subtracting the posterior yaw angle estimates from those of adjacent control cycles, taking into account the yaw angle changes over time. or Numerical jumps can occur at boundaries, requiring the difference to be wrapped with the principal angle value to map it to... Within the range, divide by the control cycle duration; The posterior yaw angle estimate used in the above turning rate calculation is taken from the last component of the posterior state estimate vector; The offset amplitude coefficient is set to 3.0 m·s / radian. This coefficient is determined offline by the system designers based on the typical turning radius and protection margin. When the navigator performs a standard turn at a typical speed of 20 m / s and a turning rate of 0.25 radians / s (approximately 14 degrees / s), its turning radius is approximately 80 meters. A coefficient of 3.0 can produce a lateral offset of approximately 0.75 meters. This offset accounts for approximately 9% of the 8.0-meter lateral half-shaft, which is within a moderate adjustment range. Lateral offset is equal to the product of offset magnitude coefficient and turn rate. This calculation method is based on the following notation: assuming that a positive yaw angle change corresponds to a right turn of the navigator, then when the turn rate is positive, the lateral offset is positive, indicating that the envelope center is offset in the direction of the lateral unit vector (i.e., the right side of the navigator's forward movement). When the turn rate is negative, the lateral offset is negative, indicating that the envelope center is offset in the opposite direction of the lateral unit vector (i.e., the left side of the navigator's forward movement). This design ensures that the envelope always gains additional extension in the direction of the turn. The above lateral offset is applied to the center position of the envelope: starting from the initial envelope center, move along the lateral unit vector direction by an offset proportional to the turning rate to obtain the envelope center position after asymmetric offset. This operation causes the entire envelope to shift in the turning direction, thereby obtaining a more adequate protection space on the inside of the turn. The parameters mentioned above collectively define a three-dimensional ellipsoidal safety envelope with lateral asymmetric bias, which is mathematically defined as: all spatial points satisfying the following inequality The set formed is represented by the formula: ; Where p is the coordinate of any point in space. For the current control cycle The location of the envelope center after offset. , as well as For the current control cycle The three orthogonal unit vectors correspond to the longitudinal, lateral, and vertical directions, respectively. , as well as These are the lengths of the three semi-axis, with values ​​of 15.0, 8.0, and 5.0 respectively. This inequality defines the interior region of the ellipsoid, that is, the spatial extent covered by the safety envelope.

[0018] S3. The difference between the predicted position and the posterior position is used as the intention feedforward correction term. Combined with the expected relative position of the formation, the intention correction formation error of the following UAV is calculated. Combined with the posterior velocity, the predicted velocity and the state information of the following UAV, the nominal control command is generated. Specifically, the current position vector and current velocity vector of the navigator are extracted from the posterior state estimate, and the navigator's position vector and velocity vector are extracted from the predicted state. The predicted position vector at time, and the expected relative position vector of the formation are given in advance by the formation configuration planning, with priority values ​​of 1 and 2. The value is measured in meters and is loaded by the onboard mission computer before the formation mission is executed. The difference between the predicted position and the posterior position is used as the intention feedforward correction term. Combined with the current position of the following UAV, the intention correction formation error is calculated. The formula is as follows: ; in, For the current control cycle The formation error vector intended for correction. To follow drone i, in the current control cycle The three-dimensional position vector, For the navigator in the current control cycle The three-dimensional position vector, The relative position vector of the formation is determined offline by the formation configuration planning. For the navigator in future control cycles The predicted position vector, To determine the intentional feedforward gain, the calibration method for this value is as follows: increase β from 0 in steps of 0.05 to 0.5, and select the value that minimizes the formation error overshoot; The formation error to be corrected is calculated by an additive expression, which is constructed by two terms: the first term is the tracking deviation between the target position and the actual position, which is the current position of the navigator plus the expected offset minus the position of the follower. This term ensures that negative feedback is generated in subsequent control to drive the follower to approach the desired formation position. The second term is the intentional feedforward correction term, which is the predicted displacement of the navigator (the difference between the predicted position and the current position) multiplied by a feedforward gain coefficient, so that the follower responds to the short-term motion trend of the navigator in advance. The correction error means that the target position of the following aircraft is no longer the static offset point of the navigator, but is dynamically moved forward based on the navigator's predicted position, which reflects the intention-driven formation control concept. Extract the current cycle velocity vector and the previous control cycle velocity vector of the navigator from the posterior state estimate, and extract the current velocity vector of the follower UAV from the local state. The calculation involves the feedforward acceleration term, which is obtained by dividing the change in velocity of the navigator over adjacent control cycles by the duration of the control cycle. Essentially, it is a discretized estimate of the navigator's current acceleration, as shown in the formula: ; in, For the current control cycle The intention is to feed forward the acceleration vector to compensate for the navigator's acceleration in advance. This is the feedforward acceleration gain, with a value range of [value range missing]. The preferred value is 0.8. The calibration basis for this gain is to ensure that the feedforward acceleration does not exceed 80% of the maximum execution capability of the drone (typically 10 m / s²). The nominal control command is generated by combining the aforementioned feedforward acceleration with a standard proportional-derivative (PD) controller. The proportional term of this PD controller is used to drive formation error convergence, while the derivative term is used to suppress velocity oscillations and increase system damping. ; in, For the current control cycle The nominal control command vector represents the desired acceleration for following the drone. The scaling gain matrix is ​​set as a diagonal matrix. This value is determined by the expected natural frequency of the formation system. For the current control cycle The formation error vector intended for correction. The differential gain matrix is ​​set as a diagonal matrix. This should be taken and Both conditions must be met to satisfy the critical damping condition (damping ratio of 1.0) in order to eliminate overshoot during the tracking process; The above control law combines standard PD control with intentional feedforward acceleration, enabling the follower to make stable corrections based on the current tracking error (negative feedback) and to respond in advance to the acceleration changes of the navigator (feedforward compensation), significantly reducing the formation tracking delay caused by the navigator's maneuvers; Since the drone uses a second-order integrator model with acceleration as the control input, and the barrier function is only related to position, a higher-order control barrier function is required. For the navigation safety envelope and the obstacle safety barrier function constructed based on external measurements, the formula is: ; in, This is the obstacle barrier function, which defines the safe distance constraint between the following drone and the obstacle: when the function value is greater than or equal to 0, it indicates that the drone is in a safe area. Let the radius be the geometric envelope radius of the obstacle. =15 meters is a fixed safety margin The geometric center of the obstacle; Since the above barrier function depends only on the position variable, its first Lie derivative with respect to the control input is always zero. In order to make the constraint conditions explicitly include the control input, a framework consistent with the higher-order control barrier function is adopted. This framework requires two Lie derivative operations on the barrier function with respect to the system dynamics model. The barrier function applies to the position vector. The gradient is a row vector, and its result is twice the transpose of the relative position vector. Based on this gradient value, the Lie derivatives of each order can be calculated sequentially: The first-order Lie derivative is the dot product of the gradient row vector and the current velocity vector of the following UAV. Its calculation result is the dot product of the relative position vector and the velocity vector, which is negative twice. The second-order Lie derivative is the first-order Lie derivative further directionally along the drift field. Based on the above assumption of instantaneous static nature of the obstacle, the calculated result of this derivative is twice the negative velocity vector dot product; The control-dependent Lie derivative represents the influence coefficient of the acceleration control input on the first-order Lie derivative. For the second-order integrator dynamics model, this coefficient is equal to the gradient of the barrier function with respect to position. The above items are combined with the preset first-order decay rate and zero-order decay rate to transform the input to be determined acceleration control. The linear security constraint inequality is given by the following formula: ; For the second-order integrator dynamic model used in this method , The Lie derivatives of the above terms have the following definite analytical form: in, For the barrier function itself, only related to position and Reference Center (Navigation Envelope Center) (or the geometric center of the obstacle) Let be the first-order Lie derivative of the barrier function along the system's drift field. It is calculated as the dot product of the row vector of the barrier function's gradient with respect to the position vector and the current velocity vector of the following UAV. The second Lie derivative of the barrier function along the system's drift field is calculated by taking the derivative of the first Lie derivative again along the drift field. Its analytical result is a combination of the position gradient and the quadratic velocity form in this step. Let be the row vector of control coefficients of the barrier function with respect to the control input. For a second-order system, these control coefficients are equal to the gradient of the barrier function with respect to the position. and These are the decay rate parameters for the first-order Lie derivative term and the zeroth-order barrier function term, respectively. This indicates that since the original barrier function does not contain a velocity term, its zeroth-order Lie derivative with respect to the control input is a zero row vector and does not participate in the final inequality construction; By merging all navigation constraints and obstacle constraints, we obtain the final set of linear safety constraints; Under the above constraints, with the optimization objective of minimizing the deviation between the desired control input and the nominal control command, and by adding non-negative relaxation variables to ensure a feasible solution under extreme conditions, a quadratic programming problem is constructed: Solving for an acceleration command, the formula is: ; in, This is the optimal control command vector after safety correction. For nominal control command vector, Let be the acceleration control input vector to be solved, which is the decision variable of this quadratic programming problem. These are slack variables used to handle situations where safety constraints have no feasible solutions under extreme operating conditions. The relaxation penalty coefficient is determined based on the principle of ensuring that the optimization problem always has a feasible solution while minimizing the relaxation amount. It satisfies three types of constraint inequalities for the navigation envelope and each obstacle, subject to the addition of slack variables to the right-hand side of the linear safety constraint set.

[0019] S4. Based on the dynamic asymmetric safety envelope, construct the navigation safety barrier function and the obstacle safety barrier function according to the external obstacle measurement information. For the second-order integrator model, transform the navigation safety barrier function and the obstacle safety barrier function into a set of linear safety constraints. Specifically, the solution status of the quadratic programming problem is monitored in each control cycle. When the following three conditions are met simultaneously, the obstacle avoidance completion criterion is satisfied, and an obstacle avoidance completion flag is obtained: A slack variable of zero indicates that the current system is not subject to any extreme physical limitations, and that the nominal control instructions no longer attempt to violate the safety boundary. The values ​​of all obstacle safety barrier functions are greater than the preset recovery trigger threshold: that is, the follower drone has substantially moved away from the influence area of ​​the obstacle. Preferably, the recovery trigger threshold is set to 5.0 square meters. The above state continues until the preset confirmation time window is reached: the time window is set to 1.0 second (i.e., 10 consecutive control cycles). This time delay mechanism is used to filter out the barrier function value jitter caused by sensor noise or irregular obstacle edges, and to prevent frequent switching of control mode, i.e., the ping-pong effect. Once obstacle avoidance is determined to be successful, the system suspends the nominal control law and enters recovery mode. Based on the position error between the current position and the desired formation position extracted from the state information of the following UAV, and the speed error between the current speed and the desired formation speed, the two error vectors are concatenated in sequence to form a six-dimensional comprehensive error vector. This concatenation operation is a standard state-space representation method, and its mathematical essence is to combine the position error and speed error into a unified error state. Construct a three-dimensional linear sliding surface, which is defined as follows: the position error is scaled by a positive definite diagonal matrix and then linearly superimposed with the velocity error. The diagonal elements of the positive definite matrix determine the asymptotic decay rate on the sliding surface, preferably 0.8. The construction method of this linear sliding surface belongs to the standard design method in sliding mode control. To overcome the slow convergence speed of traditional linear feedback, this invention employs a nonlinear terminal sliding mode control law to ensure that the error converges to zero within a finite time. The terminal sliding mode control law is constructed to generate the nominal recovery command, and the formula is as follows: ; in, To restore the nominal instruction vector, The feedback gain matrix, used to provide the basic linear traction force, is designed offline using the linear quadratic regulator (LQR) method, and its design weight matrix is ​​taken as... , This is obtained by solving the standard Riccati equation, where e is the composite error vector. The switching gain is set to 1.5 to overcome unmodeled dynamics and disturbances in the system. The value of the fractional power term is in the range of (0,1), preferably set to 0.6. This fractional power term allows the control gain to be nonlinearly amplified as the system approaches the equilibrium point (i.e., the error approaches zero), thereby achieving finite-time convergence. This is an element-wise sign function that returns the sign of each element in the input vector (1 for positive, -1 for negative, and 0 for zero). The Hadamard product is the element-wise multiplication of vectors. To calculate the absolute value of each element of the sliding surface vector before performing the calculation Power; In the above control law, the first term Provides a basic linear traction force to ensure that the error quickly approaches the sliding surface when far from the equilibrium point. The second item... A nonlinear terminal attractor is provided to ensure that the error converges in finite time rather than asymptotically as it approaches the equilibrium point.

[0020] S5. Taking the nominal control command as the optimization objective, construct a quadratic programming problem with slack variables based on the linear safety constraint set, solve it to generate a safety filter control command. During the execution of the safety filter control command, if the preset obstacle avoidance completion criterion is met, activate the terminal sliding mode control law and generate a recovery control command under the constraints of the linear safety constraint set. Specifically, during the strong sliding mode pull-back process, the UAV may enter the asymmetric envelope of the navigator or brush past the obstacle edge again due to the rapid convergence of its trajectory. The nominal recovery command is then used as the optimization objective, replacing the original nominal command, and the quadratic programming problem is reconstructed. The formula is: ; ; in, and These are the constraint coefficient matrix and right-hand side vector extracted from the linear safety constraint set, respectively, containing navigation safety constraints and all obstacle avoidance constraints, with 1 representing an all-1 vector; The construction logic of this optimization problem is as follows: under the constraint of retaining the set of linear safety constraints (i.e. all HOCBF inequality constraints), the final recovery control command is obtained. This mechanism keeps the strong convergence of sliding mode control within the cage of the safety barrier to ensure that the constraint boundary defined by the obstacle safety barrier function is not violated during the formation recovery process. The system continues to output the recovery control command until the preset steady-state tolerance is met (for example, the position error magnitude is less than 0.5 meters and the velocity error magnitude is less than 0.2 meters / second). It is determined that the system has completely and smoothly returned to the desired node, and then it switches back to the nominal formation control mode. At the end of each control cycle, the onboard mission computer uses the recovery control command output in recovery mode as the final desired acceleration signal. Specifically, the underlying flight controller of the drone receives the desired acceleration signal, combines it with the drone's current attitude angle and thrust model, and maps the three-dimensional acceleration vector into the desired thrust scalar and desired attitude angle vector (roll angle, pitch angle and yaw angle) of the drone rotor through inverse dynamics calculation. The underlying flight controller converts the aforementioned attitude and thrust commands into pulse width modulation (PWM) duty cycle signals for each motor, driving the UAV's physical actuators to generate corresponding aerodynamics, thereby strictly following the commands in the physical world, maintaining the predetermined formation and achieving absolute safe avoidance of dynamic / static obstacles.

[0021] 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, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method, characterized in that, include: The state information of the lead manned aircraft and the follower unmanned aircraft in the formation is obtained. The state information of the lead manned aircraft is filtered and estimated to obtain the posterior state estimate. The posterior state estimate includes the posterior position, posterior velocity and posterior yaw angle. The turning rate is calculated based on the difference of the posterior yaw angle in adjacent periods. The predicted position and predicted velocity of the lead manned aircraft are also calculated. Based on the posterior position and the predicted position, the forward envelope center is determined by linear interpolation, the envelope direction reference is determined based on the predicted velocity, and the lateral offset is determined based on the turning rate, thus constructing a dynamic asymmetric safety envelope. The difference between the predicted position and the posterior position is used as the intention feedforward correction term. Combined with the expected relative position of the formation, the intention correction formation error of the following UAV is calculated. The nominal control command is generated by combining the posterior velocity, the predicted velocity and the state information of the following UAV. Based on the dynamic asymmetric safety envelope, a navigation safety barrier function is constructed, and an obstacle safety barrier function is constructed based on the measurement information of external obstacles. For the second-order integrator model, the navigation safety barrier function and the obstacle safety barrier function are transformed into a set of linear safety constraints. Using nominal control commands as the optimization objective, a quadratic programming problem with slack variables is constructed based on a set of linear safety constraints. The solution generates safety filtering control commands. During the execution of safety filtering control commands, if the preset obstacle avoidance completion criterion is met, the terminal sliding mode control law is activated, and a recovery control command is generated under the constraints of the set of linear safety constraints.

2. The manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method as described in claim 1, characterized in that, The step of filtering and estimating the state information of the navigating manned aircraft to obtain the posterior state estimate includes: The position and velocity of the follower drone in the inertial coordinate system are obtained as the state information of the follower drone; The position and geometric envelope dimensions of external obstacles relative to the following drone are obtained as external obstacle measurement information; Receive the position, speed and yaw angle broadcast by the navigating manned aircraft, construct the navigating state vector as the state information of the navigating manned aircraft; The posterior state estimate is obtained by recursively estimating the pilot state vector using a Kalman filter based on a piecewise constant velocity state transition model.

3. The manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method as described in claim 2, characterized in that, The calculation of the predicted position and predicted velocity of the leading manned aircraft within the short-term prediction window includes: Extract posterior position, posterior velocity, and posterior yaw angle from the posterior state estimate; The predicted position is obtained by adding the posterior position and the displacement increment of the posterior velocity within the short-time prediction window; and the posterior velocity is used as the predicted velocity. The difference between the posterior yaw angle of the current cycle and the posterior yaw angle of the previous cycle is calculated. The difference is then wrapped with the principal angle value and divided by the control cycle to obtain the turning rate.

4. The manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method as described in claim 3, characterized in that, The construction of the dynamic asymmetric security envelope includes: Linear interpolation is performed between the posterior position and the predicted position to obtain the nominal envelope center that moves forward along the predicted motion direction; When the magnitude of the predicted velocity is greater than the preset velocity threshold, the normalized vector of the predicted velocity is used as the envelope direction reference. When the magnitude of the predicted velocity is not greater than the preset velocity threshold, the envelope direction reference is determined based on the posterior yaw angle. Construct the lateral unit direction and vertical unit direction based on the envelope direction reference, and calculate the lateral offset along the lateral unit direction according to the turning rate. The nominal envelope center is offset by a lateral offset in the lateral unit direction to obtain the offset envelope center. Based on the offset envelope center, the envelope direction reference, the lateral unit direction, the vertical unit direction, and the preset semi-axis length, an ellipsoidal dynamic asymmetric safety envelope is constructed.

5. The manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method as described in claim 4, characterized in that, The generation of nominal control instructions includes: The intention feedforward displacement is obtained by multiplying the difference between the predicted position and the posterior position by a preset intention feedforward gain. The difference between the posterior position and the expected relative position of the formation is calculated and extracted from the current position of the following drone from the state information of the following drone. This difference is then added to the intention feedforward displacement to obtain the intention-corrected formation error. The posterior velocity of the current cycle is used to calculate the difference between the posterior velocity of the previous cycle to obtain the feedforward acceleration of the navigation maneuver. Based on the intention to correct formation error, the velocity error between the posterior velocity and the current velocity of the following drone extracted from the state information of the following drone, and the feedforward acceleration of the lead maneuver, a nominal control command representing the desired acceleration is generated through a proportional-derivative control law.

6. The manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method as described in claim 5, characterized in that, The process of transforming the navigation safety barrier function and the obstacle safety barrier function into a linear set of safety constraints containing control inputs includes: Calculate the first-order Lie derivative, second-order Lie derivative, and control-related Lie derivative of the navigation safety barrier function and the obstacle safety barrier function with respect to the state variables of the second-order integrator model, respectively. By combining the first-order Lie derivative, the second-order Lie derivative, and the control-related Lie derivative with the preset first-order decay rate parameter and zero-order decay rate parameter, a higher-order control barrier constraint inequality is constructed. The higher-order control barrier constraint inequalities are expanded and rearranged into linear inequalities, and then combined to obtain a set of linear safety constraints.

7. The manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method as described in claim 6, characterized in that, The method for constructing a quadratic programming problem with slack variables based on a set of linear safety constraints, and solving it to generate safety filtering control commands, includes: The optimization objective is to minimize the deviation between the control input to be sought and the nominal control command, and a squared penalty term for the slack variable is added to the optimization objective. The constraints are the linear safety constraint set, the control input amplitude constraint, and the non-negativity constraint of the slack variable, and slack variables are added to the right-hand side of the linear safety constraint set. The quadratic programming problem is solved by convex optimization to obtain the safety filter control command.

8. The manned-unmanned aerial vehicle (UAV) cooperative formation control and adaptive obstacle avoidance method as described in claim 7, characterized in that, The activation of the terminal sliding mode control law, and the generation of recovery control commands under the constraints of the linear safety constraint set, includes: When the slack variable is zero, the values ​​of all obstacle safety barrier functions are greater than the preset recovery trigger threshold, and this state continues to reach the preset confirmation time window, the obstacle avoidance completion criterion is satisfied. Based on the position error between the current position of the following drone and the desired formation position extracted from the state information of the following drone, and the speed error between the current speed of the following drone and the desired formation speed extracted from the state information of the following drone, a linear sliding surface and a comprehensive error vector are constructed. The nominal control command in the quadratic programming problem is replaced with the nominal recovery command, and the recovery control command is obtained by solving the problem until the preset steady-state tolerance is met, and then the nominal formation control mode is switched back.

Images