Unmanned aerial vehicle formation anti-collision control method and system

Abstract

The present application relates to the technical field of unmanned aerial vehicle formation flight control, and particularly relates to an unmanned aerial vehicle formation anti-disturbance collision avoidance control method and system. The method obtains the motion state of a leading unmanned aerial vehicle, a following unmanned aerial vehicle and neighboring unmanned aerial vehicles, relative position parameters of the formation, and unmanned aerial vehicle execution constraints, generates expected motion state of the formation, trajectory tracking deviation, formation geometry deviation, and control available margin, and forms a formation control state. The formation control state is matched with a state interval in a feedback gain set, a feedback gain is selected, and a formation tracking control amount is generated. A collision risk direction and level are calculated according to relative position and relative velocity, the formation tracking control amount is directionally projected and component limited, and a collision avoidance control amount is generated. The collision avoidance control amount and the remaining tracking control components are sequentially configured within the control available margin, a target flight control instruction is formed, and the instruction is fed back and updated. The present application reduces directional conflicts of the two types of control amounts, and enables orderly distribution of limited execution capabilities.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) formation flight control technology, and in particular to a UAV formation anti-interference and collision avoidance control method and system. Background Technology

[0002] In the field of UAV formation cooperative flight control technology, a lead UAV and multiple follower UAVs typically fly in formation according to preset relative position parameters. Existing solutions generally generate the desired position, desired velocity, or desired acceleration of the follower UAVs based on the motion state of the lead UAV. Then, based on the deviation between the actual motion state of the follower UAVs and the desired motion state of the formation, they use consistency control, adaptive control, linear quadratic regulators, or active disturbance rejection controllers to generate formation tracking control quantities. When the distance between the UAVs enters the safe distance range, collision avoidance control quantities are generated based on relative position, relative velocity, or artificial potential field. The formation tracking control quantities and collision avoidance control quantities are then superimposed, weighted, or limited.

[0003] Such schemes can maintain the relative positional relationship between the following drone and the lead drone under conditions of smooth formation trajectories, small changes in inter-drone distances, and sufficient drone execution capabilities. However, when external disturbances, formation turns, load changes, or multiple drones simultaneously approach each other, the formation tracking control and collision avoidance control variables may point in opposite directions. Existing direct superposition or uniform weighting methods typically do not distinguish between the control components pointing towards and away from neighboring drones in the formation tracking control variable, which can easily lead to the two types of control variables canceling each other out, or weakening the collision avoidance control effect while limiting the total control input.

[0004] Existing solutions often employ fixed control parameters, single error-driven parameter adjustments, or uniform limiting of the synthesized total control quantity. When trajectory tracking deviations, formation geometry deviations, and UAV execution constraints change with the control cycle, insufficient matching easily occurs between the feedback control parameters and the current formation state. Furthermore, when maximum acceleration, maximum tilt angle, or maximum thrust approaches the boundaries, there is a lack of a clear order of control capability allocation between collision avoidance control and formation tracking control. Consequently, as the risk of inter-UAV collisions continuously changes, it is difficult to form a continuous and coordinated control link between formation tracking control, collision avoidance control, and execution constraints. Therefore, it is necessary to address the directional conflicts and control capability configuration issues between formation tracking control and collision avoidance control when UAV formation execution capabilities are limited and inter-UAV collision risks change. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention provides a method for anti-interference and collision avoidance control of unmanned aerial vehicle (UAV) formations, comprising: S100: Obtain the motion state of the lead drone, the motion state of the follow drone, the formation relative position parameters, and the drone execution constraints; generate the desired formation motion state based on the motion state of the lead drone and the formation relative position parameters, calculate the trajectory tracking deviation and formation geometric deviation, generate the available control margin based on the difference between the drone execution constraints and the current control input, and form the formation control state; S200: Match the trajectory tracking deviation, formation geometry deviation, and available control margin in the formation control state with the state intervals associated with each feedback gain item in the feedback gain set, and select the current feedback gain; generate the formation tracking control quantity based on the current feedback gain and the trajectory tracking deviation; S300: Calculate the collision risk direction and collision risk level based on the relative position and relative speed between the following UAV and the neighboring UAV; project the formation tracking control quantity onto the collision risk direction; limit the tracking control component pointing to the neighboring UAV according to the collision risk level; generate collision avoidance control quantity; and form a control component coordination result. S400. Based on the coordination result of the control components, first configure the collision avoidance control quantity within the available control margin, then configure the reserved tracking control component with the remaining available control margin, and generate the target flight control command; execute the target flight control command, and update the formation control state and the collision risk level according to the motion state of the following UAV after execution.

[0006] Furthermore, the motion state of the lead drone includes its position, speed, and yaw angle, and the motion state of the follower drone includes its position and speed. Time alignment and validity verification are performed on the motion states of the lead drone and the follower drone. Based on the yaw angle, the relative position parameters of the formation are transformed from the lead drone coordinate system to the world coordinate system. The transformation result is superimposed with the position of the lead drone to generate the desired position of the follower drone, and the desired speed and desired acceleration are generated based on the time variation of the desired position.

[0007] Furthermore, the trajectory tracking deviation includes the difference between the desired position and the position of the following UAV, and the difference between the desired speed and the speed of the following UAV; the formation geometry deviation is generated based on the difference between the actual relative positions between the UAVs and the formation relative position parameters; the UAV execution constraints include maximum acceleration, maximum tilt angle, and maximum thrust; the available control margin is generated based on the difference between the control capability corresponding to the maximum acceleration, the maximum tilt angle, and the maximum thrust and the current control input.

[0008] Furthermore, the generation of the feedback gain set includes: setting multiple trajectory tracking deviation intervals, multiple formation geometry deviation intervals, and multiple control margin intervals, and combining them to form multiple formation control conditions; using the state weight matrix and control weight matrix of the linear quadratic regulator as particle positions, calculating the fitness of each particle based on the trajectory tracking deviation, the formation geometry deviation, and the control input, and updating the particle position and particle velocity; solving for the feedback gain corresponding to each formation control condition, and associating each feedback gain with the corresponding trajectory tracking deviation interval, formation geometry deviation interval, and control margin interval to form the feedback gain set.

[0009] Furthermore, the selection of the current feedback gain includes: matching the current trajectory tracking deviation, the current formation geometry deviation, and the current available control margin with the trajectory tracking deviation interval, formation geometry deviation interval, and available control margin interval associated with each feedback gain term, respectively; when a matching feedback gain term exists, calling the feedback gain corresponding to the feedback gain term; when the current formation control state is between adjacent state intervals, performing feedback gain interpolation on adjacent feedback gains; performing feedback gain validity verification on the feedback gain obtained by feedback gain interpolation, and calling the previous valid feedback gain if the feedback gain validity verification fails.

[0010] Furthermore, the calculation of the collision risk direction and the collision risk level includes: calculating the relative closing velocity, the expected nearest contact distance, and the expected collision time based on the relative position and the relative velocity; using the direction of the relative position as the collision risk direction; comparing the expected nearest contact distance and the expected collision time with a warning threshold, a danger threshold, and an exit threshold, and generating a collision risk level corresponding to a normal state, a warning state, or a danger state based on the comparison results.

[0011] Further, the formation tracking control quantity is projected onto the collision risk direction to obtain the risk direction tracking control component; the difference between the formation tracking control quantity and the risk direction tracking control component is taken as the orthogonal tracking control component; when the risk direction tracking control component points to the neighboring aircraft, the risk direction tracking control component is attenuated or set to zero according to the control component allocation ratio corresponding to the collision risk level; the orthogonal tracking control component and the risk direction tracking control component that does not point to the neighboring aircraft are taken as the reserved tracking control components.

[0012] Furthermore, when the following drone corresponds to multiple neighboring drones, multiple collision risk items are generated based on the relative position and relative speed of each neighboring drone. The multiple collision risk items are sorted in ascending order of expected collision time, and if the expected collision times are the same, they are sorted in ascending order of expected nearest distance. Based on the sorting results, the collision avoidance control quantity corresponding to each collision risk item is configured sequentially within the available control margin, and the remaining available control margin after configuring each collision avoidance control quantity is configured to the reserved tracking control component.

[0013] Furthermore, when the collision risk level remains below the exit threshold for a preset duration, a formation recovery state is generated; in the formation recovery state, the formation tracking control quantity is configured according to the control component allocation ratio corresponding to the formation geometric deviation; when the formation geometric deviation is less than the formation recovery deviation threshold, the formation recovery state ends; when the collision risk level in the formation recovery state reaches the warning threshold, the formation recovery state is updated to a collision avoidance state.

[0014] The present invention also provides a drone formation anti-interference and collision avoidance control system, comprising: The formation control state generation module is used to acquire the motion state of the lead drone, the motion state of the follow drone, the relative position parameters of the formation, and the drone execution constraints; generate the desired motion state of the formation based on the motion state of the lead drone and the relative position parameters of the formation, calculate the trajectory tracking deviation and the formation geometric deviation, and generate the available control margin based on the difference between the drone execution constraints and the current control input, thus forming the formation control state; The formation tracking control module, connected to the formation control state generation module, is used to match the trajectory tracking deviation, formation geometry deviation, and available control margin in the formation control state with the state intervals associated with each feedback gain item in the preset feedback gain set, and select the current feedback gain; and generate the formation tracking control quantity based on the current feedback gain and the trajectory tracking deviation. The collision risk coordination module, connected to the formation tracking control module, is used to calculate the collision risk direction and collision risk level based on the relative position and relative speed between the following UAV and the neighboring UAV; project the formation tracking control quantity onto the collision risk direction; limit the tracking control component pointing to the neighboring UAV according to the collision risk level; and generate a collision avoidance control quantity, forming a control component coordination result including the retained tracking control component and the collision avoidance control quantity. The flight command execution and feedback module is connected to the collision risk coordination module and the formation control state generation module, respectively. It is used to configure the collision avoidance control quantity first within the available control margin based on the control component coordination result, and then configure the reserved tracking control component with the remaining available control margin to generate and execute the target flight control command; and update the formation control state and the collision risk level according to the motion state of the following UAV after execution.

[0015] The key innovations of this invention include: (1) The trajectory tracking deviation, formation geometry deviation and control margin are organized together into the formation control state, and the three types of states are matched with the state intervals associated with each feedback gain item in the feedback gain set, so that the current feedback gain is simultaneously constrained by the formation error state and the UAV execution capability; the current feedback gain and the trajectory tracking deviation together form the formation tracking control quantity.

[0016] (2) Based on the relative position and relative speed between the following UAV and the neighboring UAV, the collision risk direction and collision risk level are formed. The formation tracking control quantity is projected onto the collision risk direction, and the tracking control component pointing to the neighboring UAV and the orthogonal tracking control component are distinguished. The tracking control component pointing to the neighboring UAV is restricted according to the collision risk level. The retained tracking control component and the collision avoidance control quantity together constitute the control component coordination result.

[0017] (3) The available control margin is used as the common configuration boundary for collision avoidance control quantity and reserved tracking control component. First, the collision avoidance control quantity is configured within the available control margin, and then the reserved tracking control component is configured with the remaining available control margin. The calculation process of the motion state of the following UAV after the target flight control command is executed, the return to the formation control state, and the collision risk level forms a control feedback relationship across control cycles.

[0018] The following are its main beneficial effects: (1) By having the trajectory tracking deviation, formation geometry deviation and available control margin participate in feedback gain matching, the formation tracking control quantity changes with the formation deviation state and current execution capability, thereby reducing the control input exceeding the limit and the accumulation of formation tracking deviation caused by the mismatch between fixed feedback gain or single error adjustment and the current formation operating condition.

[0019] (2) By projecting the formation tracking control quantity into the direction of collision risk and limiting the tracking control component pointing to the neighboring aircraft, the mutual cancellation between the formation tracking control quantity and the collision avoidance control quantity in opposite directions is reduced; the tracking control component orthogonal to the direction of collision risk or far away from the neighboring aircraft continues to participate in the control, so that the formation tracking function that does not conflict with the collision risk is still retained during the collision avoidance period.

[0020] (3) By sequentially configuring collision avoidance control quantity and reserved tracking control component within the available control margin, when the maximum acceleration, maximum tilt angle or maximum thrust approaches the boundary, the collision avoidance control quantity first occupies the currently available control capability, and the remaining control capability is then used for formation tracking; the motion state after execution participates in the gain matching and collision risk calculation of the next control cycle, reducing the weakening of collision avoidance control caused by uniform amplitude limiting and the disconnection of state connection between adjacent control cycles. Attached Figure Description

[0021] Figure 1 A flowchart illustrating a method for anti-interference and collision avoidance control of unmanned aerial vehicle (UAV) formations provided in this application embodiment; Figure 2 This is a structural block diagram of a drone formation anti-interference and collision avoidance control system provided in an embodiment of this application. Detailed Implementation

[0022] Example 1: Refer to Figure 1 This is a flowchart illustrating a method for anti-interference and collision avoidance control of unmanned aerial vehicle (UAV) formations provided in an embodiment of the present invention. The flowchart may include at least steps S100-S400: S100: Obtain the motion state of the lead drone, the motion state of the follow drone, the formation relative position parameters, and the drone execution constraints; generate the desired formation motion state based on the motion state of the lead drone and the formation relative position parameters, calculate the trajectory tracking deviation and formation geometric deviation, generate the available control margin based on the difference between the drone execution constraints and the current control input, and form the formation control state; S200: Match the trajectory tracking deviation, formation geometry deviation, and available control margin in the formation control state with the state intervals associated with each feedback gain item in the feedback gain set, and select the current feedback gain; generate the formation tracking control quantity based on the current feedback gain and the trajectory tracking deviation; S300: Calculate the collision risk direction and collision risk level based on the relative position and relative speed between the following UAV and the neighboring UAV; project the formation tracking control quantity onto the collision risk direction; limit the tracking control component pointing to the neighboring UAV according to the collision risk level; generate collision avoidance control quantity; and form a control component coordination result. S400. Based on the coordination result of the control components, first configure the collision avoidance control quantity within the available control margin, then configure the reserved tracking control component with the remaining available control margin, and generate the target flight control command; execute the target flight control command, and update the formation control state and the collision risk level according to the motion state of the following UAV after execution.

[0023] S100: Obtain the motion state of the lead drone, the motion state of the follow drone, the formation relative position parameters, and the drone execution constraints; generate the desired formation motion state based on the motion state of the lead drone and the formation relative position parameters, calculate the trajectory tracking deviation and formation geometric deviation, generate the available control margin based on the difference between the drone execution constraints and the current control input, and form the formation control state; After the UAV formation enters the lead-follow flight phase, the onboard controllers of each follower UAV initiate this step according to a preset control cycle. The UAV formation includes one lead UAV and multiple follower UAVs. Each follower UAV is equipped with a formation state acquisition unit, a desired state generation unit, and a formation control state generation unit. The motion state of the lead UAV is transmitted to each follower UAV via the formation communication link; the motion state of the follower UAVs is output by the onboard state estimator; the formation relative position parameters are configured in the onboard controller before the mission begins; the UAV execution constraints are provided by the flight controller based on the aircraft's dynamic parameters and current execution state. The onboard state estimator can receive measurement results from the Global Navigation Satellite System, the Visual-Inertial Navigation System, and the Inertial Measurement Unit.

[0024] After receiving the motion states of the lead UAV and the follower UAVs, the formation state acquisition unit first performs time alignment based on the sampling times carried by each state. When the sampling time of the lead UAV's state is inconsistent with the current control cycle, linear interpolation is performed based on two adjacent valid states; if only the previous valid state exists, the current control time is calculated based on the previous valid position and velocity. The lead UAV's motion state whose calculation time exceeds the valid duration of the state data does not directly enter the expected state calculation. The airborne controller maintains the expected formation motion state of the previous control cycle and limits the current feedback gain to the previous valid feedback gain. When the follower UAV's own state is missing, the flight controller maintains the attitude reference values ​​in the previous target flight control command, the formation control process is paused and updated, and the formation control state is recalculated after the own state is restored.

[0025] The motion state of the lead drone includes its position, velocity, and yaw angle, while the motion state of the follower drones includes their position and velocity. In one embodiment, the motion state further includes acceleration, roll angle, pitch angle, and yaw rate. The formation relative position parameters represent the preset position offset of each follower drone in the lead drone's coordinate system. This position offset rotates with the lead drone's coordinate system during mission execution, but does not remain fixed with the world coordinate system orientation. Therefore, when the lead drone turns, the desired position of the follower drones changes synchronously with the lead drone's yaw angle.

[0026] The desired state generation unit constructs a rotation matrix around the vertical axis of the world coordinate system based on the yaw angle of the lead drone, transforming the formation relative position parameters of the following drones from the lead drone coordinate system to the world coordinate system. The resulting position offset is added to the position of the lead drone at the current control moment to form the desired position of the following drones. For the i-th following drone, its desired position can be expressed as the sum of the lead drone's position and the rotated position offset. When the lead drone's yaw angle undergoes a cross-cycle jump, the yaw angle is processed to be continuous, avoiding abrupt changes in the desired position caused by the angle representation jumping from a positive boundary to a negative boundary.

[0027] The desired velocity and desired acceleration are calculated based on the change in desired position within adjacent control cycles. In one implementation, the desired velocity is obtained by dividing the difference between the current desired position and the desired position of the previous control cycle by the control cycle, and the desired acceleration is obtained by dividing the difference between the current desired velocity and the desired velocity of the previous control cycle by the control cycle. When the motion state of the lead drone includes effective velocity and acceleration, the desired velocity and desired acceleration can also be calculated jointly from the lead drone's velocity, lead drone acceleration, and the rotational component of the position offset after rotation. The calculated desired velocity and desired acceleration are limited according to the drone's maximum velocity and maximum acceleration, respectively. The limited desired position, desired velocity, and desired acceleration constitute the formation's desired motion state.

[0028] The formation control state generation unit compares the desired formation motion state with the motion state of the following UAV. The difference between the desired position and the actual position forms the position tracking deviation, and the difference between the desired velocity and the actual velocity forms the velocity tracking deviation. The position tracking deviation and the velocity tracking deviation together constitute the trajectory tracking deviation. The trajectory tracking deviation retains the three axes of the world coordinate system and is not converted into a single error magnitude beforehand, so that subsequent feedback gain calculations can still distinguish deviations in different directions. When the confidence level of the state estimate in a certain direction is lower than the preset effective state condition, the deviation in that direction uses the effective value of the previous control cycle, and the rate of change of the control quantity in that direction is limited.

[0029] The formation geometry deviation is calculated based on the actual relative positions of the drones and the formation relative position parameters. For the lead drone and the i-th follower drone, their actual positions are first subtracted to obtain the actual relative position; then, the actual relative position is transformed to the lead drone's coordinate system and subtracted from the formation relative position parameters of the i-th follower drone to obtain the lead-follower relative position deviation. When configuring adjacency relationships between follower drones, the neighbor drone relative position deviation is also calculated based on the actual relative positions of adjacent follower drones and preset relative positions. Multiple relative position deviations can be combined according to the connection weights in the formation communication topology to form the formation geometry deviation corresponding to the current follower drone. When a neighbor drone in the communication topology fails, the connection weight of that neighbor drone in the current control cycle is canceled, and the remaining connection weights are normalized.

[0030] The constraints for UAV execution include maximum acceleration, maximum tilt angle, and maximum thrust. Maximum acceleration is configured by the flight controller; maximum tilt angle limits the control capability that the airframe thrust can achieve in the horizontal direction; maximum thrust is obtained based on the motor, rotor, and power supply status, as well as the current flight mass. The formation control state generation unit converts the maximum tilt angle and maximum thrust into usable acceleration boundaries in the world coordinate system, and then takes a common allowable range with the maximum acceleration boundary to form the upper limit of control capability for the current control cycle. The current control input uses the acceleration control quantity actually sent to the flight controller in the previous control cycle; when the flight controller returns to the actual thrust or attitude execution state, it replaces the previous command quantity with the actual executed quantity.

[0031] The available control margin is calculated as the difference between the upper limit of control capability in each direction and the current control input. Since positive and negative control capabilities are affected by airframe attitude and thrust direction, the available control margin can be recorded separately for the three axes of positive and negative directions, rather than just a single scalar value. When the current thrust approaches the maximum thrust, the remaining control capability in the vertical direction decreases accordingly; when the current tilt angle approaches the maximum tilt angle, the margin for further increasing acceleration in the horizontal direction decreases accordingly. When the flight controller returns to actuator saturation, the available control margin in the corresponding direction is set to zero, and subsequent control component coordination will no longer increase control input in that direction.

[0032] The trajectory tracking deviation, formation geometry deviation, and available control margin are combined into a formation control state at the same control moment. This formation control state retains the directional components, error magnitude, error change direction, and available control margin for each direction of each deviation. The error magnitude is used by S200 to match the state interval of the feedback gain term, and the directional components are used by S200 to calculate the formation tracking control quantity. The feedback gain effective state, collision risk level, and formation recovery state from the previous control cycle are transmitted along with the formation control state, but the values ​​of the trajectory tracking deviation, formation geometry deviation, and available control margin remain unchanged. The resulting formation control state is processed by the feedback gain scheduling unit.

[0033] S200: Match the trajectory tracking deviation, formation geometry deviation, and available control margin in the formation control state with the state intervals associated with each feedback gain term in the feedback gain set, and select the current feedback gain; generate the formation tracking control quantity based on the current feedback gain and the trajectory tracking deviation: After receiving the formation control state generated by S100, the feedback gain scheduling unit reads the trajectory tracking deviation, formation geometry deviation, and available control margin, and accesses the pre-configured feedback gain set. The feedback gain set is generated and stored in the onboard controller before the UAV performs the formation task. During the online control cycle, complete particle swarm iteration and algebraic Riccati equation solving are not performed; only state interval matching, feedback gain invocation, or feedback gain interpolation are performed to adapt the feedback gain determination process to the flight control cycle.

[0034] The offline generation of the feedback gain set is based on a linear quadratic regulator. The error state is composed of the position and velocity deviations of the following UAV, and the desired three-axis acceleration correction is used as the control input to establish an error state space model. For low-speed flight or quasi-hovering conditions, the three-axis position channels can be established as second-order integral elements; for formation turning conditions, the UAV dynamics model can be linearized at the corresponding operating point to obtain the state matrix and control matrix corresponding to that operating point.

[0035] When generating the feedback gain set offline, multiple trajectory tracking deviation intervals, multiple formation geometry deviation intervals, and multiple control margin intervals are set, and these three types of intervals are combined into multiple formation control conditions. For example, trajectory tracking deviations can be divided into intervals of small deviation, medium deviation, and large deviation; formation geometry deviations can be divided in the same way; and control margins can be divided into sufficient, limited, and near-saturation. The boundaries of each interval are configured based on the UAV size, formation spacing, maximum acceleration, and mission-allowed deviation; the number of intervals is not limited to the above numbers.

[0036] Under each formation control condition, the diagonal elements of the state weight matrix and control weight matrix of the linear quadratic regulator are used as particle positions, and particle swarm optimization (PSO) is employed for iterative processing. Each particle corresponds to a set of state weights and control weights, with the state weight matrix maintaining positive semi-definiteness and the control weight matrix maintaining positive definiteness. When a particle goes out of bounds, the corresponding matrix elements are restricted to preset upper and lower bounds; if the matrix does not meet the positive definiteness condition, the feedback gain corresponding to that particle is not calculated, and the fitness of that particle is set to an invalid value.

[0037] The fitness of each particle is calculated based on the trajectory tracking deviation, formation geometry deviation, and control input under the formation control condition. In one implementation, the fitness is formed by adding the time-weighted integrals of position deviation, velocity deviation, formation geometry deviation, and control input according to preset weights. If, during simulation, the control input exceeds the corresponding available control margin, the closed-loop state diverges, or the formation spacing is lower than the preset minimum spacing, a constraint term is added to the fitness. After completing the iteration of particle position and particle velocity, the state weight matrix and control weight matrix with the minimum effective fitness are selected, and the corresponding feedback gain is solved.

[0038] Each feedback gain is associated with the trajectory tracking deviation range, formation geometry deviation range, and available control margin range used when generating that feedback gain, forming a feedback gain term. The feedback gain term also records the dimension of the corresponding error state-space model and the dimension of the control input; the number of rows and columns of the feedback gain matrix is ​​checked before online invocation. When different state-space models are used for different operating points, the feedback gain term is also associated with the corresponding operating point range; if the current flight speed or yaw rate exceeds this range, the feedback gain term is not invoked.

[0039] During online matching, the feedback gain scheduling unit calculates the current trajectory tracking deviation magnitude, the current formation geometric deviation magnitude, and the current control available margin ratio. The control available margin ratio is calculated as the ratio of the current remaining control capability to the upper limit of the control capability. Subsequently, the three current values ​​are compared with the three types of state intervals associated with each feedback gain term. Only when all three current values ​​fall into the interval corresponding to the same feedback gain term is that feedback gain term used as a matching term.

[0040] When a unique match exists, the feedback gain corresponding to that match is directly invoked. When multiple matches with overlapping boundaries exist, the feedback gain with the smaller distance is selected based on the normalized distance from the current value to the center of each state interval. When the current formation control state is between adjacent state intervals and there is no complete match, the feedback gain corresponding to the adjacent state interval is read, and feedback gain interpolation is performed based on the distance between the current state value and the boundaries of the two intervals. When all three types of states simultaneously cross intervals, multi-dimensional interpolation is performed based on the interval positions of trajectory tracking deviation, formation geometry deviation, and available control margin.

[0041] After the feedback gain interpolation is completed, the feedback gain scheduling unit performs a validity check. The check includes the matrix dimension, the finiteness of matrix elements, and the closed-loop stable state. The interpolated feedback gain is substituted into the current error state space model to calculate the closed-loop state matrix; if the real parts of all eigenvalues ​​of the closed-loop state matrix are less than zero, the feedback gain is marked as valid. If the predicted control quantity calculated based on the feedback gain exceeds the current available control margin, the feedback gain is not directly judged as invalid, but rather the predicted control quantity is handed over to S400 for constrained configuration; if the feedback gain matrix contains non-numerical elements, has inconsistent dimensions, or the closed-loop state matrix does not meet the stability conditions, the previous valid feedback gain is invoked.

[0042] If the previous effective feedback gain is unavailable, a safety feedback gain associated with small trajectory tracking deviation, small formation geometric deviation, and limited control margin is invoked from the feedback gain set. If no effective feedback gain is obtained for several consecutive control cycles, the feedback gain scheduling unit maintains the safety feedback gain and limits the rate of change of the formation's desired motion state. After the feedback gain passes the validity check again, it switches to a new feedback gain according to the feedback gain change limit of the adjacent control cycle, avoiding sudden changes in the feedback gain that directly cause sudden changes in the control input.

[0043] The formation tracking control unit multiplies the current feedback gain by the trajectory tracking deviation to obtain the state feedback correction. This state feedback correction is then combined with the desired acceleration in the desired formation motion state to form the formation tracking control quantity. When the trajectory tracking deviation is defined as "desired state minus actual state," the state feedback correction is configured along the direction of deviation reduction. Currently in the formation recovery state, the control component allocation ratio corresponding to the formation geometric deviation still affects the formation tracking control quantity, but the collision risk direction has not yet participated in the control quantity decomposition in this step.

[0044] The S200 outputs the current feedback gain, the effective state of the feedback gain, and the formation tracking control quantity. The current feedback gain and the effective state of the feedback gain are retained until the next control cycle, while the formation tracking control quantity is passed to the collision risk handling unit and the control component coordination unit. Subsequent steps do not directly change the current feedback gain, but instead limit a portion of the directional component in the formation tracking control quantity based on the direction of the collision risk.

[0045] S300: Calculate the collision risk direction and collision risk level based on the relative position and relative speed between the following UAV and the neighboring UAV; project the formation tracking control quantity onto the collision risk direction; limit the tracking control component pointing to the neighboring UAV according to the collision risk level; generate collision avoidance control quantity; and form a control component coordination result. After receiving the formation tracking control input from the S200, the collision risk handling unit reads the motion status of the following UAV and the motion status of neighboring UAVs. Neighboring UAVs are those with a communication connection to the currently following UAV or those within the airborne sensing range, including the lead UAV and other following UAVs. When the motion status of neighboring UAVs is obtained through the formation communication link, it is first time-aligned with the motion status of the current following UAV according to the state timestamp; when obtained through airborne sensing, the sensing coordinates are transformed to the world coordinate system. When both sources exist simultaneously and the positional difference exceeds a preset consistency range, the state source with the more recent sampling time and higher state validity is used.

[0046] For the currently following drone i and its neighbor j, the relative position is obtained by subtracting the current drone's position from the neighbor's position; the relative velocity is obtained by subtracting the current drone's velocity from the neighbor's velocity. The magnitude of the relative position is the current distance between the drones, and the unit vector pointing from the current drone to the neighbor is used as the direction of collision risk. When the dot product of the relative position and relative velocity is less than zero, the two drones are in a close proximity state; when the dot product of the relative position and relative velocity is greater than or equal to zero, the two drones are in a separated or parallel motion state.

[0047] In the approach phase, the relative closing velocity is calculated based on the relative position and relative velocity. The relative closing velocity is the projection of the relative velocity in the opposite direction to the collision risk direction; a positive projection value indicates that the distance between the vehicles is decreasing. When the relative velocity magnitude is greater than the preset minimum velocity, a short-time linear prediction is performed based on the current relative motion to calculate the expected closest approach time. If the expected closest approach time is less than zero, the vehicle is treated as not approaching. If the expected closest approach time exceeds the risk prediction time domain, it is limited to the end of the risk prediction time domain. Substituting the expected closest approach time into the relative motion equations yields the expected closest approach distance.

[0048] The estimated collision time is calculated based on the safe distance between the two drones. This safe distance comprises the drone's envelope size, positioning error range, and the movement distance corresponding to communication delay. The inter-drone distance in the relative motion equations is set as this safe distance, and the moment of entering the safe distance boundary is obtained by solving for it. If multiple positive solutions exist, the smaller positive moment is taken as the estimated collision time. If the relative velocity is too small to form a valid solution, the estimated collision time is calculated based on the current inter-drone distance and the relative closing velocity. If the relative closing velocity is not positive, the estimated collision time is set outside the risk prediction time domain.

[0049] The collision risk level is determined by both the expected nearest approach distance and the expected collision time. A normal state is defined as the expected nearest approach distance being greater than the warning distance and the expected collision time being greater than the warning time. A warning state is defined as the expected nearest approach distance entering the warning distance range or the expected collision time being less than the warning time. A dangerous state is defined as the expected nearest approach distance entering the danger distance range and the expected collision time being less than the danger time. The exit threshold and warning threshold are configured separately. When the current state returns to normal from a warning state or dangerous state, the expected nearest approach distance must be greater than the exit distance, and the expected collision time must be greater than the exit time. Therefore, when the inter-machine state fluctuates briefly near the warning boundary, the collision risk level does not immediately and repeatedly switch.

[0050] If a neighboring drone's status exceeds the effective duration but not the short-term prediction duration, the collision risk processing unit calculates the current status based on the neighboring drone's previous effective position and speed, and maintains the collision risk at a level no lower than that of the previous control cycle. After the neighboring drone's status exceeds the short-term prediction duration, the precise collision avoidance control quantity is no longer calculated based on its position; the currently following drone retains the collision avoidance direction corresponding to the previous control cycle, and the component in the formation tracking control quantity pointing in the opposite direction of that collision avoidance direction is limited, until the neighboring drone's status recovers or the neighboring drone leaves the perception range.

[0051] The control component coordination unit projects the formation tracking control quantity based on the collision risk direction. Assuming the formation tracking control quantity is a three-dimensional acceleration vector, the inner product of the formation tracking control quantity and the collision risk direction, multiplied by the collision risk direction, yields the risk direction tracking control component. Subtracting the risk direction tracking control component from the formation tracking control quantity yields the orthogonal tracking control component. When the risk direction tracking control component is in the same direction as the collision risk direction, this component points towards the neighboring aircraft; when they are in opposite directions, this component moves away from the neighboring aircraft.

[0052] Under normal conditions, the risk direction tracking control component pointing towards the adjacent aircraft remains unchanged, and the collision avoidance control component is zero. Under warning conditions, the risk direction tracking control component pointing towards the adjacent aircraft is attenuated according to the control component allocation ratio corresponding to the expected nearest approach distance and the expected collision time. The closer the expected nearest approach distance is to the danger distance, or the closer the expected collision time is to the danger time, the smaller the retention ratio. Under dangerous conditions, the risk direction tracking control component pointing towards the adjacent aircraft is set to zero. Risk direction tracking control components far from the adjacent aircraft are not subject to these restrictions, and the orthogonal tracking control component is retained.

[0053] The collision avoidance control quantity is generated in the opposite direction to the collision risk direction. In one implementation, an artificial potential field with a finite range of action is used to calculate the collision avoidance control quantity; when the current inter-machine distance is greater than the warning distance, the collision avoidance control quantity is zero; after the current inter-machine distance enters the warning distance range, the repulsive force amplitude is calculated based on the difference between the current inter-machine distance and the warning distance, and the repulsive force amplitude is adjusted according to the collision risk level. The direction of the collision avoidance control quantity is opposite to the collision risk direction. When the relative closing velocity is large, the relative closing velocity term is added to the repulsive force amplitude, so that the collision avoidance control quantity increases in advance when the expected collision time is shortened.

[0054] When adjacent aircraft exist simultaneously in both directions, the direct addition of multiple collision avoidance control quantities may cause them to cancel each other out. To address this, the collision risk processing unit establishes a collision risk item for each adjacent aircraft. Each collision risk item includes the adjacent aircraft identifier, estimated collision time, estimated nearest-neighbor distance, collision risk direction, collision risk level, and collision avoidance control quantity. Multiple collision risk items are first sorted in ascending order of estimated collision time; if the estimated collision times are the same, they are then sorted in ascending order of estimated nearest-neighbor distance. The sorting result is passed to S400, where S300 does not directly sum all collision avoidance control quantities without considering available control margin.

[0055] When the highest-risk neighboring aircraft is located in the direction orthogonal to the formation tracking control variable and the artificial potential field exhibits directional equilibrium, in one implementation, a tangential collision avoidance direction is selected based on the collision risk direction. The tangential collision avoidance direction is orthogonal to the collision risk direction and maintains the same rotation direction as the collision avoidance direction from the previous control cycle. This tangential component serves as part of the collision avoidance control variable for the corresponding collision risk item; if directional equilibrium is not achieved, only the normal collision avoidance control variable furthest from the neighboring aircraft is used.

[0056] S300 generates a control component coordination result. This result includes the retained tracking control components, the collision avoidance control quantities corresponding to each collision risk item, the collision risk direction, the collision risk level, and the ranking of multiple collision risk items. The retained tracking control components consist of orthogonal tracking control components, risk direction tracking control components far from neighboring aircraft, and neighboring aircraft-oriented components retained after risk level restrictions. These results are then passed to S400, which performs the actual configuration based on available control margins.

[0057] S400. Based on the control component coordination result, first configure the collision avoidance control quantity within the available control margin, then configure the reserved tracking control component with the remaining available control margin, and generate the target flight control command; execute the target flight control command, and update the formation control state and the collision risk level according to the motion state of the following UAV after execution; The control component coordination unit receives the control component coordination result generated in S300 and reads the available control margin calculated in S100. In this step, the available control margin is treated as a configurable control range, rather than a uniformly limited range applied to all synthesized control quantities. The configuration order is executed according to the collision risk level and the order of collision risk items; the collision avoidance control quantity corresponding to a dangerous state takes precedence over the collision avoidance control quantity corresponding to a warning state, and the collision avoidance control quantity takes precedence over the reserved tracking control component.

[0058] When only one collision risk item exists, the collision avoidance control quantity corresponding to that risk item is first projected onto the current available control margin range for the three-axis control. If the collision avoidance control quantity exceeds the corresponding available control margin in a certain direction, the collision avoidance control quantity in that direction is limited to the available control margin for that direction; directions that do not exceed the margin retain their original values. The configured collision avoidance control quantity is subtracted from the available control margin to obtain the remaining available control margin. Subsequently, the retained tracking control component is projected onto the remaining available control margin range to form the actually configured tracking control component.

[0059] When multiple collision risk items exist, they are configured sequentially according to the sorting result formed by S300. After allocating the available control margin to the collision risk item ranked first, the first remaining available control margin is calculated; then, the first remaining available control margin is used to configure the collision risk item ranked second. Subsequent collision risk items are processed in the same way. When the collision avoidance direction of a collision risk item is opposite to the direction of the configured collision avoidance control quantity, the impact of both on their respective expected nearest-neighbor distances is calculated first, and the result of vector addition is not directly used as the configuration value; if it is not possible to allocate a complete collision avoidance control quantity at the same time, the component corresponding to the collision risk item with the shorter expected collision time is retained, and the remaining directional control capability is used for the other collision risk item.

[0060] After all collision avoidance control parameters are configured, the remaining control margin is allocated to the reserved tracking control components. In hazardous conditions, when the remaining control margin is zero, no additional tracking control components are added in the current control cycle. In warning conditions, the tracking control components are limited according to the proportion allocated to control components corresponding to the collision risk level. In normal conditions, the collision avoidance control parameters are zero, and the formation tracking control parameters are configured according to the constraints imposed by the UAV. In formation recovery conditions, the reserved tracking control components are also limited by the proportion allocated to formation recovery control components, which varies with the formation geometry deviation and the duration of risk clearance.

[0061] The configured collision avoidance control component is added to the configured tracking control component to form the desired acceleration command. The flight controller, based on the UAV's mass, gravitational acceleration, and current attitude, converts the desired acceleration into the desired total thrust direction and magnitude. If the desired total thrust magnitude exceeds the maximum thrust, it is limited to the maximum thrust; if the roll or pitch angle corresponding to the desired thrust direction exceeds the maximum tilt angle, the horizontal acceleration component is limited to ensure the converted desired attitude falls within the maximum tilt angle range.

[0062] In one implementation, the flight command execution unit generates a desired rotation matrix based on the desired acceleration and a preset yaw angle, obtains the desired roll angle, desired pitch angle, and desired yaw angle from the desired rotation matrix, and sends the desired total thrust and desired attitude to the flight controller. In another implementation, the flight controller directly receives the desired acceleration and desired yaw angle, and the control quantity conversion is completed by the position outer loop and attitude inner loop within the flight controller. The actual objects among the desired acceleration, desired total thrust, and desired attitude constitute the target flight control command.

[0063] Before issuing target flight control commands, the current command is compared with the command from the previous control cycle according to the control change rate constraint. When the change in the tracking control component exceeds the control change rate, a slope limit is applied. The collision avoidance control quantity corresponding to a dangerous state does not use a uniform low-pass filter, but is limited only according to the maximum change rate allowed by the actuator. As a result, non-emergency tracking corrections will not produce excessive cycle jumps, and emergency collision avoidance components will not incur additional delays due to full low-pass processing.

[0064] After sending the target flight control command to the flight controller, the flight command execution unit reads the command reception status, actual thrust status, actual attitude status, and actuator saturation status returned by the flight controller. If no command reception status is received within the current control cycle, the previous target flight control command is maintained, and the current collision risk level is kept at a level no lower than that of the previous control cycle. When the flight controller returns the actuator saturation status, the actual control capability in the corresponding direction is used as the upper limit of the control capability for calculating the available control margin in the next control cycle S100, and the rated upper limit unaffected by saturation is no longer used.

[0065] After the next state estimation result arrives, the feedback update unit replaces the follower UAV's motion state from the previous control cycle with the executed follower UAV's position and velocity. The executed motion state is re-involved in the calculation of trajectory tracking deviation and formation geometry deviation; the actual control input and actual thrust state are re-involved in the calculation of available control margin; the updated relative position and relative velocity are re-involved in the calculation of expected nearest distance, expected collision time, and collision risk level. The previous effective feedback gain, control saturation state, and the ranking results of collision risk items for each neighboring UAV are retained to the next control cycle for use in feedback gain anomaly rollback and risk continuity processing.

[0066] When the collision risk level changes from a dangerous or warning state to a normal state, full formation tracking control is not immediately restored. The feedback update unit accumulates the duration for which the collision risk level remains below the exit threshold; after the accumulated duration reaches a preset duration, a formation recovery state is generated. In the formation recovery state, the control component allocation ratio of the formation tracking control quantity is set according to the formation geometric deviation. When the formation geometric deviation is large, a limited ratio is used, and then gradually increased as the formation geometric deviation decreases, to avoid the following UAVs rapidly returning to their original formation position immediately after the risk is eliminated.

[0067] Once the formation geometric deviation is less than the formation recovery deviation threshold and remains below it for a preset control period, the formation recovery state ends, and the next control cycle configures the formation tracking control variables according to the normal state. If the collision risk level of any neighboring aircraft reaches the warning state again during the formation recovery period, the formation recovery state is immediately terminated, the corresponding collision risk item is re-added to the sorting results, and the available control margin is allocated according to the priority of collision avoidance control variables. Thus, the execution results of the target flight control commands continuously affect the subsequent formation control state, feedback gain selection, and collision risk calculation, forming a cross-control cycle formation anti-interference and collision avoidance control process.

[0068] Example 2: Figure 2 A structural block diagram of a drone formation anti-interference and collision avoidance control system according to an embodiment of the present invention is shown. Figure 2 As shown, the structure may include: The formation control state generation module 01 is used to acquire the motion state of the lead drone, the motion state of the follower drone, the relative position parameters of the formation, and the drone execution constraints. Based on the motion state of the lead drone and the relative position parameters, it generates the desired formation motion state, calculates the trajectory tracking deviation and formation geometry deviation, and generates a control margin based on the difference between the drone execution constraints and the current control input, thus forming the formation control state. The lead drone's motion state is transmitted by the lead drone via the formation communication link, the follower drone's motion state is provided by the follower drone's onboard state estimator, the relative position parameters are pre-configured in the follower drone's onboard controller, and the drone execution constraints are output by the flight controller based on maximum acceleration, maximum tilt angle, and maximum thrust. The formation control state generation module reads the above inputs according to the control cycle and performs time alignment based on the sampling time of each motion state. When the sampling time of the lead drone's motion state and the follower drone's motion state is inconsistent, it interpolates to the current control time based on adjacent valid motion states. If the state duration exceeds a preset valid duration, it retains the desired formation motion state of the previous control cycle and transmits the valid state result to the formation tracking control module.

[0069] The motion state of the lead drone includes its position, velocity, and yaw angle, while the motion state of the follower drone includes its position and velocity. The formation control state generation module transforms the relative position parameters of the formation from the lead drone's coordinate system to the world coordinate system based on the yaw angle. The transformed position offset is then superimposed with the lead drone's position to obtain the desired position of the follower drone. The desired velocity is calculated based on the change in desired position between adjacent control cycles, and the desired acceleration is calculated based on the change in desired velocity between adjacent control cycles. The desired position, desired velocity, and desired acceleration constitute the desired motion state of the formation. When the yaw angle crosses an angle representation boundary, adjacent yaw angles are processed to ensure continuity, avoiding abrupt changes in the coordinate transformation results caused by non-flight motion.

[0070] The formation control state generation module uses the difference between the desired position and the actual position of the following UAV as the position tracking deviation, and the difference between the desired speed and the actual speed of the following UAV as the speed tracking deviation. The position tracking deviation and the speed tracking deviation together constitute the trajectory tracking deviation. The formation geometric deviation is obtained by comparing the actual relative position between the lead UAV and the following UAV, or the actual relative position between adjacent following UAVs, with the formation relative position parameters. When the motion state of a neighboring UAV is missing or exceeds its effective duration, the relative position deviation corresponding to that neighboring UAV is not used in the current control cycle, and the relative position deviations corresponding to the remaining valid neighboring UAVs are recombined according to the connection relationship of the formation communication topology.

[0071] The UAV execution constraints are converted into the upper limit of the allowable control capability in each motion direction within the current control cycle. The current control input is the acceleration control quantity actually issued in the previous control cycle, or the control quantity corresponding to the actual thrust and attitude fed back by the flight controller. The formation control state generation module calculates the difference between the upper limit of the control capability and the current control input according to the motion direction to obtain the available control margin. When the flight controller reports that the actuator in a certain direction is in a saturated state, the available control margin in that direction is updated to zero. The trajectory tracking deviation, the formation geometry deviation, and the available control margin constitute the formation control state at the same control moment. The formation control state is sent to the formation tracking control module and updated in subsequent control cycles upon receiving the execution state returned by the flight command execution and feedback module.

[0072] The formation tracking control module 02, connected to the formation control state generation module, is used to match the trajectory tracking deviation, formation geometry deviation, and available control margin in the formation control state with the state intervals associated with each feedback gain item in a preset feedback gain set, and select the current feedback gain; generate formation tracking control quantities based on the current feedback gain and the trajectory tracking deviation; the preset feedback gain set is stored in the onboard controller of the following UAV, and each feedback gain item is associated with the trajectory tracking deviation interval, the formation geometry deviation interval, and the available control margin interval, respectively. The feedback gain set can be generated before mission execution based on multiple formation control conditions, wherein each formation control condition is formed by a combination of different trajectory tracking deviation intervals, formation geometry deviation intervals, and available control margin intervals.

[0073] After receiving the formation control status, the formation tracking control module calculates the positions of the current trajectory tracking deviation, current formation geometric deviation, and current available control margin within the corresponding state intervals. When all three states fall within the state interval associated with the same feedback gain term, the feedback gain corresponding to that term is invoked. If multiple matching terms exist, the feedback gain term with the smaller distance is selected based on the normalized distance between the three states and the center of each state interval. When the current formation control status is located between adjacent state intervals, the adjacent feedback gains are interpolated based on the distance between the current status and the boundary of the adjacent state interval.

[0074] For the interpolated feedback gain, the formation tracking control module verifies the dimension of the feedback gain matrix and the valid states of its elements, and substitutes the feedback gain into the current error state space model for closed-loop state verification. If the dimension of the feedback gain matrix is ​​incorrect, invalid values ​​appear in the matrix elements, or the closed-loop state does not meet preset conditions, the previous valid feedback gain is invoked; if no previous valid feedback gain exists, a preset feedback gain associated with the available margin interval of the restricted control is invoked. If no valid feedback gain is matched for several consecutive control cycles, the preset feedback gain is maintained, and the change in the desired formation motion state between adjacent control cycles is limited.

[0075] After selecting the current feedback gain, the formation tracking control module applies the current feedback gain to the trajectory tracking deviation to obtain a state feedback correction. This state feedback correction is then combined with the desired acceleration in the desired formation motion state to form the formation tracking control quantity. The formation tracking control quantity retains control components in the three motion directions of the world coordinate system and is not summed with the collision avoidance control quantity within this module. The current feedback gain is stored as the previous effective feedback gain for the next control cycle, and the formation tracking control quantity is transmitted to the collision risk coordination module.

[0076] The collision risk coordination module 03, connected to the formation tracking control module, is used to calculate the collision risk direction and collision risk level based on the relative position and relative speed between the following UAV and the neighboring UAV; project the formation tracking control quantity onto the collision risk direction; limit the tracking control component pointing to the neighboring UAV according to the collision risk level; and generate a collision avoidance control quantity, forming a control component coordination result including the retained tracking control component and the collision avoidance control quantity; the neighboring UAV's motion state comes from the formation communication link or airborne perception results, and is aligned with the following UAV's motion state at the sampling time before entering the collision risk calculation. When the formation communication link and airborne perception simultaneously provide the same neighboring UAV state, the neighboring UAV position and speed used in the current control cycle are selected according to the sampling time and the valid state; when the difference between the two sources exceeds a preset range, the collision risk level of the previous control cycle is retained, and the current neighboring UAV state is resampled.

[0077] The collision risk coordination module obtains the relative position by subtracting the position of the following drone from the position of the neighboring drone, and obtains the relative speed by subtracting the speed of the following drone from the speed of the neighboring drone. The direction of the relative position serves as the collision risk direction. Based on the relative position and relative speed, the relative closing velocity, the estimated nearest-neighbor distance, and the estimated collision time are calculated. The estimated nearest-neighbor distance and the estimated collision time are compared with a warning threshold, a danger threshold, and an exit threshold, respectively. The comparison results correspond to a normal state, a warning state, or a danger state. The normal state, the warning state, and the danger state constitute the collision risk level. When the neighboring drone's state is temporarily missing within a preset short-term prediction period, the current relative motion state is calculated based on the neighboring drone's previous valid position and speed, and the collision risk level is maintained at a level not lower than that of the previous control cycle.

[0078] After obtaining the collision risk direction, the collision risk coordination module projects the formation tracking control quantity onto the collision risk direction to obtain a risk direction tracking control component. The risk direction tracking control component is then subtracted from the formation tracking control quantity to obtain an orthogonal tracking control component. When the risk direction tracking control component is in the same direction as the collision risk direction, it points towards the neighboring aircraft. In a warning state, the control component is attenuated according to the control component allocation ratio corresponding to the expected closest approach distance and expected collision time. In a dangerous state, the control component is set to zero. The risk direction tracking control component retains its original value when it is far from the neighboring aircraft, and the orthogonal tracking control component is not deleted due to risk direction restrictions.

[0079] The collision avoidance control quantity is generated in the opposite direction to the collision risk direction, and its amplitude is configured based on the current inter-aircraft distance, the expected nearest-neighbor distance, the expected collision time, and the collision risk level. When the relative motion between the aircraft corresponds to a normal state, the collision avoidance control quantity is zero; when corresponding to a warning state or a dangerous state, the collision avoidance control quantity changes according to the shortening of the inter-aircraft distance or the reduction of the expected collision time. When the following UAV corresponds to multiple neighboring aircraft, multiple collision risk items are established and sorted in ascending order of expected collision time; if the expected collision times are the same, they are sorted in ascending order of expected nearest-neighbor distance. The collision risk coordination module does not directly sum the multiple collision avoidance control quantities, but instead combines the sorting result, each collision avoidance control quantity, and the retained tracking control component to form the control component coordination result, which is then sent to the flight command execution and feedback module.

[0080] The flight command execution and feedback module 04 is connected to both the collision risk coordination module and the formation control state generation module. Based on the control component coordination results, it first configures the collision avoidance control quantity within the available control margin, then configures the reserved tracking control component using the remaining available control margin, generating and executing the target flight control command. It updates the formation control state and the collision risk level based on the motion state of the following UAV after execution. After receiving the control component coordination results, the flight command execution and feedback module reads the collision risk level, collision avoidance control quantity, reserved tracking control component, and the sorting result of multiple collision risk items. Simultaneously, it reads the current available control margin from the formation control state generation module. The available control margin is represented in three motion directions of the world coordinate system, with the positive and negative directions configured with available ranges respectively.

[0081] When only one collision risk item exists, the flight command execution and feedback module first limits the corresponding collision avoidance control quantity to within the current available control margin, and then subtracts the configured collision avoidance control quantity from the current available control margin to form the remaining available control margin; subsequently, the reserved tracking control component is limited to the remaining available control margin. When multiple collision risk items exist, each collision avoidance control quantity is configured sequentially according to the sorting result, and the remaining available control margin is updated once after the configuration of each collision avoidance control quantity is completed. When the collision avoidance control quantity corresponding to a certain collision risk item exceeds the available control margin in the current direction, the collision avoidance control quantity in that direction is limited to the available control margin in the current direction, and subsequent risk items will no longer occupy the control capability in that direction.

[0082] After configuring the collision avoidance control component and the retained tracking control component, the two are combined into the desired acceleration. The flight command execution and feedback module converts the desired acceleration into desired total thrust and desired attitude based on the UAV's mass, gravitational acceleration, and current attitude. Thrust limiting is applied when the desired total thrust exceeds the maximum thrust, and horizontal acceleration in the corresponding direction is limited when the desired attitude exceeds the maximum tilt angle. The flight controller receives the desired acceleration, desired total thrust, desired roll angle, desired pitch angle, and desired yaw angle, which together form the target flight control command, and sends it to the flight controller of the following UAV through the flight control interface.

[0083] After executing the target flight control command, the flight controller returns the command reception status, actual thrust status, actual attitude status, and actuator saturation status to the flight command execution and feedback module. If no command reception status is received in the current control cycle, the previous target flight control command is maintained, and the current collision risk level does not transition to a lower level. When the flight controller returns the actuator saturation status, it feeds back the actual control capability in the corresponding direction to the formation control status generation module, which serves as the upper limit of control capability for calculating the available control margin in the next control cycle.

[0084] After execution, the position and velocity of the following UAV are used in the calculation of trajectory tracking deviation, formation geometry deviation, relative position, and relative velocity in the next control cycle. The actual thrust state and the saturation state of the actuator are used in the calculation of the available control margin in the next control cycle. When the collision risk level remains below the exit threshold for a preset duration, the flight command execution and feedback module configures the formation tracking control quantity according to the control component allocation ratio corresponding to the formation geometry deviation. When the formation geometry deviation is less than the formation recovery deviation threshold, normal formation control is restored. If the collision risk level reaches the warning state again during the recovery period, the current recovery process is terminated, and the available control margin is reconfigured according to the priority order of collision avoidance control quantity.

Claims

1. A method for anti-interference and collision avoidance control of unmanned aerial vehicle (UAV) formations, characterized in that, include: S100: Obtain the motion state of the lead drone, the motion state of the follow drone, the formation relative position parameters, and the drone execution constraints; generate the desired formation motion state based on the motion state of the lead drone and the formation relative position parameters, calculate the trajectory tracking deviation and formation geometric deviation, generate the available control margin based on the difference between the drone execution constraints and the current control input, and form the formation control state; S200: Match the trajectory tracking deviation, formation geometry deviation, and available control margin in the formation control state with the state intervals associated with each feedback gain item in the feedback gain set, and select the current feedback gain; generate the formation tracking control quantity based on the current feedback gain and the trajectory tracking deviation; S300: Calculate the collision risk direction and collision risk level based on the relative position and relative speed between the following UAV and the neighboring UAV; project the formation tracking control quantity onto the collision risk direction; limit the tracking control component pointing to the neighboring UAV according to the collision risk level; generate collision avoidance control quantity; and form a control component coordination result. S400. Based on the coordination result of the control components, first configure the collision avoidance control quantity within the available control margin, then configure the reserved tracking control component with the remaining available control margin, and generate the target flight control command; execute the target flight control command, and update the formation control state and the collision risk level according to the motion state of the following UAV after execution.

2. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 1, characterized in that, The motion state of the lead drone includes its position, speed, and yaw angle; the motion state of the follower drone includes its position and speed; time alignment and validity verification are performed on the motion states of the lead drone and the follower drone. The relative position parameters of the formation are transformed from the coordinate system of the lead drone to the world coordinate system based on the yaw angle. The transformation result is superimposed with the position of the lead drone to generate the expected position of the follower drone. The expected speed and expected acceleration are generated based on the time change of the expected position.

3. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 2, characterized in that, The trajectory tracking deviation includes the difference between the desired position and the position of the following UAV, and the difference between the desired speed and the speed of the following UAV; the formation geometry deviation is generated based on the difference between the actual relative positions between the UAVs and the formation relative position parameters; the UAV execution constraints include maximum acceleration, maximum tilt angle, and maximum thrust; the available control margin is generated based on the difference between the control capability corresponding to the maximum acceleration, the maximum tilt angle, and the maximum thrust and the current control input.

4. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 1, characterized in that, The generation of the feedback gain set includes: setting multiple trajectory tracking deviation intervals, multiple formation geometry deviation intervals, and multiple control margin intervals, and combining them to form multiple formation control conditions; using the state weight matrix and control weight matrix of the linear quadratic regulator as particle positions, calculating the fitness of each particle based on the trajectory tracking deviation, the formation geometry deviation, and the control input, and updating the particle position and particle velocity; solving for the feedback gain corresponding to each formation control condition, and associating each feedback gain with the corresponding trajectory tracking deviation interval, formation geometry deviation interval, and control margin interval to form the feedback gain set.

5. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 4, characterized in that, The selection of the current feedback gain includes: matching the current trajectory tracking deviation, current formation geometry deviation, and current available control margin with the trajectory tracking deviation interval, formation geometry deviation interval, and available control margin interval associated with each feedback gain term, respectively; when a matching feedback gain term exists, calling the feedback gain corresponding to the feedback gain term; when the current formation control state is between adjacent state intervals, performing feedback gain interpolation on adjacent feedback gains; performing feedback gain validity verification on the feedback gain obtained by feedback gain interpolation, and calling the previous valid feedback gain if the feedback gain validity verification fails.

6. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 1, characterized in that, The calculation of the collision risk direction and the collision risk level includes: calculating the relative closing velocity, the expected nearest contact distance, and the expected collision time based on the relative position and the relative speed; using the direction of the relative position as the collision risk direction; comparing the expected nearest contact distance and the expected collision time with a warning threshold, a danger threshold, and an exit threshold, and generating a collision risk level corresponding to a normal state, a warning state, or a danger state based on the comparison results.

7. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 6, characterized in that, The formation tracking control quantity is projected onto the collision risk direction to obtain the risk direction tracking control component; The difference between the formation tracking control quantity and the risk direction tracking control component is taken as the orthogonal tracking control component; When the risk direction tracking control component points to the neighboring machine, the risk direction tracking control component is attenuated or set to zero according to the control component allocation ratio corresponding to the collision risk level; the orthogonal tracking control component and the risk direction tracking control component that does not point to the neighboring machine are used as the reserved tracking control components.

8. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 6, characterized in that, When the following drone corresponds to multiple neighboring drones, multiple collision risk items are generated based on the relative position and relative speed of each neighboring drone. The multiple collision risk items are sorted in ascending order of expected collision time, and if the expected collision time is the same, they are sorted in ascending order of expected nearest distance. According to the sorting result, the collision avoidance control quantity corresponding to each collision risk item is configured in the available control margin, and the remaining available control margin after configuring each collision avoidance control quantity is configured to the reserved tracking control component.

9. The unmanned aerial vehicle (UAV) formation anti-interference and collision avoidance control method according to claim 6, characterized in that, When the collision risk level remains below the exit threshold for a preset duration, a formation recovery state is generated. In the formation recovery state, the formation tracking control quantity is configured according to the control component allocation ratio corresponding to the formation geometric deviation. When the formation geometric deviation is less than the formation recovery deviation threshold, the formation recovery state ends. When the collision risk level in the formation recovery state reaches the warning threshold, the formation recovery state is updated to a collision avoidance state.

10. A drone formation anti-interference and collision avoidance control system, applied to the method of any one of claims 1 to 9, characterized in that, include: The formation control state generation module is used to obtain the motion state of the lead drone, the motion state of the follow drone, the relative position parameters of the formation, and the execution constraints of the drones. The desired formation motion state is generated based on the motion state of the lead drone and the relative position parameters of the formation. The trajectory tracking deviation and formation geometric deviation are calculated. The available control margin is generated by the difference between the drone's execution constraints and the current control input, thus forming the formation control state. The formation tracking control module, connected to the formation control state generation module, is used to match the trajectory tracking deviation, formation geometry deviation, and available control margin in the formation control state with the state intervals associated with each feedback gain item in the preset feedback gain set, and select the current feedback gain; and generate the formation tracking control quantity based on the current feedback gain and the trajectory tracking deviation. The collision risk coordination module, connected to the formation tracking control module, is used to calculate the collision risk direction and collision risk level based on the relative position and relative speed between the following UAV and the neighboring UAV; project the formation tracking control quantity onto the collision risk direction; limit the tracking control component pointing to the neighboring UAV according to the collision risk level; and generate a collision avoidance control quantity, forming a control component coordination result including the retained tracking control component and the collision avoidance control quantity. The flight command execution and feedback module is connected to the collision risk coordination module and the formation control state generation module, respectively. It is used to configure the collision avoidance control quantity first within the available control margin based on the control component coordination result, and then configure the reserved tracking control component with the remaining available control margin to generate and execute the target flight control command; and update the formation control state and the collision risk level according to the motion state of the following UAV after execution.

Images