Formation route following and anti-collision control method for unmanned aerial vehicle swarm

Abstract

The application discloses a kind of unmanned aerial vehicle cluster formation route follow-up and anti-collision control method, comprising: obtaining flight task waypoint, local machine and adjacent machine flight state and formation configuration, generate local machine independent flight path;According to different flight phases, generate route reference position, reference speed and reference heading;Adjacent machine relative position and speed are projected to reference heading, and the formation speed correction amount along heading is generated by proportional differential calculation;Conflict adjacent machine is screened to construct two-dimensional ORCA half-plane constraint, and collision-free horizontal velocity is solved and combined with vertical stratification to generate collision avoidance correction amount and channel occupancy state;After passing through channel control concession according to occupancy state, route reference speed, formation speed correction amount and collision avoidance correction amount are fused to form speed, height and heading control instruction.The application passes through channel control concession, only concedes occupied channel when avoiding collision, retains non-conflict control component, avoids mutual offset of multiple controllers, and gives consideration to real-time anti-collision and formation flight stability.

Description

Technical Field

[0001] This invention relates to the field of collaborative control technology for unmanned aerial vehicle (UAV) swarms, specifically to a method for formation flight path following and collision avoidance control of UAV swarms. Background Technology

[0002] As multi-rotor drones are increasingly used in tasks such as collaborative inspection, area search, emergency reconnaissance and swarm transportation, drone formations not only need to fly stably along predetermined routes, but also need to maintain predetermined formations under conditions of communication delays, individual state differences and dynamic interference from neighboring drones, and avoid potential collisions in real time.

[0003] Existing swarm flight control typically employs separate controllers for course following, formation keeping, and collision avoidance. When multiple controllers simultaneously modify the same horizontal speed channel, the lateral pullback generated by the course controller may be opposite to the collision avoidance direction, and the longitudinal speed adjustment generated by the formation spacing control may also counteract the collision avoidance speed, leading to problems such as speed oscillations and trajectory jumps. If lower-priority controllers are directly disabled through simple priority control, heading, altitude, or speed along the course direction that does not conflict with collision avoidance will be lost, slowing down the formation recovery process.

[0004] For rotary-wing UAVs that can directly control planar speed and altitude, it is still necessary to solve the following problems: First, how to simultaneously satisfy speed, acceleration, and jerk constraints under short straight sections and frequent start-stop conditions; second, how to balance geometric passability and high-order dynamic continuity when turning; third, how to solve for safe speed through ORCA half-plane constraints in multi-neighbor dynamic interactions; and fourth, how to yield only the occupied control channels when ORCA collision avoidance intervenes, while preserving non-conflicting flight paths and formation control components.

[0005] Furthermore, horizontal two-dimensional collision avoidance is prone to stalling when drones in the same group meet symmetrically. Using only a single-plane collision avoidance strategy makes it difficult to take advantage of the independent altitude adjustment capability of rotorcraft drones; while directly superimposing altitude commands may cause frequent up-and-down switching.

[0006] Therefore, a new formation flight path following and collision avoidance control scheme for UAV swarms needs to be proposed to solve the above problems. Summary of the Invention

[0007] In view of the above problems, the present invention is proposed to provide a method for formation flight path following and collision avoidance control of UAV swarms that overcomes or at least partially solves the above problems. This method generates a smooth flight path reference, formation longitudinal correction and ORCA collision avoidance correction within a unified control cycle, and performs channel-specific control transfer based on the actual movement channel occupied for collision avoidance. This avoids multiple controllers canceling each other out on the same channel while retaining non-collision control capabilities.

[0008] This invention provides a method for formation flight path following and collision avoidance control of a drone swarm, executed by each drone in the swarm, including the following steps: The system acquires flight mission waypoints, its own flight status, neighboring aircraft flight status, and formation configuration. It then converts the own offset in the formation configuration to the local navigation coordinate system to generate its own independent flight track. Based on the independent flight path and the current flight phase of the aircraft, generate the route reference position, route reference speed, and reference heading; The relative position and relative speed of the aircraft and at least one neighboring aircraft are projected onto the reference heading, and the formation speed correction along the reference heading is generated by proportional differential calculation based on the longitudinal relative parameters. Based on the relative motion between the local machine and its neighboring machines, conflicting neighboring machines are selected. Two-dimensional ORCA half-plane constraints are constructed for the selected conflicting neighboring machines. Within the feasible velocity domain defined by the constraints, the collision-free horizontal velocity close to the desired velocity is obtained. The collision avoidance correction amount and the channel occupancy status are generated by combining the vertical layering. Based on the channel occupancy status, channel control and handover are performed. The route reference speed, formation speed correction, and collision avoidance correction after channel control and handover are integrated to form the speed, altitude, and heading control commands for the UAV.

[0009] In some implementations, converting the local offset in the formation configuration to the local navigation coordinate system to generate the local independent track includes: rotating the lateral formation offset in the airframe configuration to the local navigation coordinate system around the desired heading; calculating the actual connecting waypoints at the connecting waypoints of adjacent segments based on the angle bisector of the normal vectors on the same side of the two segments and the cosine compensation of the turning angle to form mutually parallel independent tracks for the members; superimposing the altitude offset onto the independent tracks for the members; and using the formation speed correction to achieve the longitudinal formation spacing in the airframe configuration. Based on the independent flight path and the current flight phase of the aircraft, the generation of the route reference position, route reference speed, and reference heading includes: in the straight flight phase, generating the reference speed along the route direction based on speed, acceleration, and jerk constraints, and generating the lateral speed correction based on the L1 guidance method; in the turning flight phase, constructing the turning reference curve using a quadratic Bezier curve, and generating the route reference position and route reference speed using the MinimumSnap polynomial. The channel control and handover based on the channel occupancy status includes: when lateral collision avoidance correction is required, the line following module sets the lateral speed correction to zero or smoothly decays it, but still retains the reference speed along the line direction; when directional collision avoidance correction is required, the formation holding module retains the formation position offset, but smoothly reduces the formation speed correction to zero according to a preset rate of change; when the collision avoidance correction only includes the vertical layer component, the occupancy status of the horizontal lateral channel and the directional channel is not set, and the horizontal line following and formation holding continue to operate.

[0010] In some implementations, generating a reference speed along the flight path during the straight-line flight phase based on speed, acceleration, and jerk constraints includes: Determine the feasible peak speed along the route direction based on the current remaining segment length, maximum permissible speed, maximum permissible acceleration, and maximum permissible jerk. With the feasible peak velocity as the peak target, the jerk is constructed as a piecewise constant in multiple sequentially connected phases. The multiple phases sequentially complete the acceleration establishment, constant acceleration, acceleration exit, uniform velocity, deceleration establishment, constant deceleration, and deceleration exit. By integrating the jerk in each phase piecewise, a monotonically increasing parametric mapping between position and velocity along the flight path is established. Based on the mapping relationship, an S-curve is formed, where the S-curve defines the reference velocity value at any position along the flight path. Project the machine's position along the direction of the unit tangent vector of the current straight segment to obtain the actual progress along the flight path; The actual progress along the route is used as the independent variable of the S-curve mapping, and the corresponding speed value is directly output as the reference speed of the route.

[0011] In some embodiments, generating the lateral velocity correction based on the L1 guidance method includes: determining the lateral error direction and the lateral velocity projection direction based on the unit normal vector of the current flight segment; calculating the lateral velocity correction using the L1 guidance law based on the lateral track error of the aircraft position relative to the current flight segment and the projection of the aircraft's actual velocity onto the unit normal vector, wherein the lateral velocity correction is a scalar value along the unit normal vector direction; and when the horizontal lateral collision avoidance channel is occupied, the lateral velocity correction is set to zero or smoothly decays to zero at a preset rate. During the turning flight phase, a quadratic Bézier curve is used to construct the turning reference curve, and the route reference position and speed are generated using the MinimumSnap polynomial. This includes: calculating the advance entry distance based on the current speed and the maximum allowable acceleration to determine the timing for switching from the straight segment to the turning segment; constructing a quadratic Bézier curve using the turning entry point, waypoint corner control point, and turning exit point as the turning reference geometric skeleton; determining at least one intermediate connection point on the turning reference geometric skeleton to divide the trajectory from the entry to the exit into at least two segments, constructing a seventh-order polynomial for each segment, and setting continuity constraints for position, velocity, acceleration, and jerk at the endpoints and intermediate connection points of each segment; solving for the polynomial coefficients that satisfy the continuity constraints with the objective of minimizing the squared integral of the fourth derivative of the polynomial trajectory position with respect to time for each segment; establishing the mapping relationship between the trajectory arc length and time parameters, and querying the turning reference position and speed based on the actual arc length progress of the aircraft on the trajectory.

[0012] In some implementations, projecting the relative position and relative speed of the aircraft and at least one neighboring aircraft onto the reference heading, and generating a formation speed correction along the reference heading based on the proportional derivative of the longitudinal relative parameters includes: Project the relative position and relative speed of the machine and at least one neighboring machine onto the reference heading to obtain the longitudinal spacing error and longitudinal relative speed of each neighboring machine; Based on the longitudinal spacing error and longitudinal relative speed, proportional-derivative adjustment amounts are generated for each adjacent machine through proportional-derivative control. The average control is obtained by averaging the proportional-derivative controls of all effective adjacent machines. The average adjustment amount is sequentially subjected to dead zone processing, amplitude limitation, and rate of change limitation. The output after the dead zone processing, amplitude limitation, and rate of change limitation is used as the formation speed correction amount along the reference heading.

[0013] In some implementations, filtering conflicting neighboring machines based on the relative motion between the local machine and its neighboring machines includes: For each neighboring machine within the detection range, calculate the straight-line distance, closing speed, and estimated collision time between the local machine and the neighboring machine. When the closing speed is greater than the preset closing speed dead zone, the estimated collision time is estimated by dividing the difference between the straight-line distance and the safe merging radius by the closing speed. The safe merging radius is the sum of the safe radii of the two machines plus a safety margin. A strict intervention distance threshold and intervention time threshold are used to determine whether to initiate collision avoidance. When the straight-line distance is less than the intervention distance threshold or the expected collision time is less than the intervention time threshold, the neighboring machine is included in the conflicting neighboring machine set. A hysteresis mechanism is formed by adopting a relatively lenient holding distance threshold and holding time threshold. When the straight-line distance is greater than or equal to the holding distance threshold and the expected collision time is greater than or equal to the holding time threshold, and the collision avoidance correction has been smoothly decayed into the release dead zone, the neighboring machine is removed from the set of conflicting neighboring machines and the collision avoidance state is released. The release dead zone is an error tolerance interval used to determine that the collision avoidance correction has been sufficiently converged.

[0014] In some implementations, a two-dimensional ORCA half-plane constraint is constructed for each selected conflicting neighbor, and a collision-free horizontal velocity adjacent to the desired velocity is obtained within the feasible velocity domain defined by the constraint, including: For each conflicting neighbor, based on the relative position, relative speed, safe merging radius and finite time window of the local machine and the neighbor, a cutoff speed barrier is constructed in the relative speed space. If the current relative speed is within the cutoff speed barrier, the nearest point from the current relative speed to the boundary of the cutoff speed barrier is calculated, and the difference between the current relative speed and the nearest point is used as the minimum correction vector to make the relative speed leave the speed barrier boundary. At the same time, the unit normal vector pointing to the safe side is determined. Based on the principle that both parties bear part of the responsibility for avoiding collisions, the sum of the current speed of the machine and the boundary offset determined based on the minimum correction vector is used as the boundary point of the half-plane. Combined with the unit normal vector, the collision-free velocity half-plane of the machine is constructed. The feasible velocity domain is obtained by intersecting the non-collision velocity half-plane corresponding to all conflicting neighboring machines with the circle centered on the current velocity of the machine and with the maximum horizontal velocity as the radius. Within the feasible speed domain, the speed that minimizes the Euclidean distance from the desired speed of the machine is determined as the collision-free horizontal speed, where the desired speed of the machine is formed by the current route reference speed. When the intersection is empty, the candidate velocities are projected and corrected in order of constraint priority.

[0015] In some implementations, the vertical layering includes at least one of the following: In the local coordinate system, when there is a clear height difference between the machine and its neighboring machine, the machine with the higher height continues to move upward and the machine with the lower height continues to move downward. When the two drones are approximately at the same height, determine the opposite up and down directions based on the drone markings; For UAVs in the same group that exchange lateral positions, discrete altitude layers are assigned according to the member order, so that symmetrical exchanges have a definite vertical selection. The final vertical offset is output after applying amplitude and rate of change limits.

[0016] In some implementations, before constructing two-dimensional ORCA half-plane constraints for each selected conflicting neighbor, the method further includes: For UAVs that are approximately symmetrical and flying towards each other in a formation, their desired speeds are rotated in the same direction by an angle related to the distance of the UAV from the formation center to form an offset desired speed to eliminate stagnation in symmetrical flying towards each other. This offset desired speed is used as the desired speed input when constructing the ORCA half-plane constraint. The rotated speed is only used to determine the desired direction in the ORCA feasible region, and the ORCA half-plane is still responsible for safety constraints.

[0017] In some implementations, the fusion of the route reference speed, formation speed correction, and collision avoidance correction after the channel control handover includes at least one of the following: The horizontal collision avoidance position offset is converted into a horizontal collision avoidance speed difference according to a preset time domain, wherein the horizontal collision avoidance position offset is obtained by multiplying the difference between the non-collision horizontal speed and the machine's expected speed by the time domain. The vector obtained by multiplying the baseline horizontal speed, the formation speed correction along the baseline heading by the baseline heading unit vector, and the horizontal collision avoidance speed difference are vector superimposed, and the vector magnitude is limited on the composite horizontal speed instead of trimming the coordinate components separately. The vertical collision avoidance offset is superimposed on the flight path reference height; Switch to position holding mode when the synthesis speed is below the position holding threshold; When at least one of the route reference position and speed is not available, a safety instruction is generated using the current measured position and heading.

[0018] Compared with the prior art, the present invention achieves at least one of the following beneficial effects: First, compared with the simple superposition of route following, formation maintenance and collision avoidance control, the present invention controls the transfer of control channels by the occupancy status of horizontal lateral channels, along the course channels and vertical channels. This can prevent lateral pullback and collision avoidance lateral movement, formation speed adjustment and collision avoidance longitudinal speed from canceling each other out, while retaining non-conflicting control components.

[0019] Secondly, compared with the fixed-wing trajectory scheme that uses adaptive L1 and circular interpolation, this invention takes into account the characteristic that rotary-wing UAVs can directly track planar velocity, eliminates lateral position error and oscillation with PD control, and queries the jerk-limited S-curve with the actual position along the flight path, which can reduce the reference speed jump caused by calculation cycle jitter, short flight segments and pause recovery.

[0020] Furthermore, the combination of quadratic Bézier curves and two seventh-order Minimum-Snap optimizations ensures the continuity of the positions and multi-order derivatives at the turn entrance, connection point, and exit, thus balancing the geometric passability at waypoint corners with the smooth dynamic response of the rotor platform.

[0021] Furthermore, compared with artificial potential field superposition or candidate velocity discrete sampling, two-dimensional ORCA represents each neighbor machine constraint as a collision-free velocity half-plane and selects the result closest to the desired velocity within the feasible region; combined with hysteresis mechanism, velocity difference smoothing, and vertical layering, it improves the continuity and repeatability of multi-machine interaction.

[0022] Furthermore, the adjustment of adjacent aircraft in the formation adopts average, dead zone, amplitude limit, and rate of change limit, which can reduce the impact of changes in the number of adjacent aircraft and measurement disturbances on the formation speed. When exiting collision avoidance, each correction gradually decays at the same rate of change, which can reduce secondary oscillations when re-aligning with the flight path and formation.

[0023] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it in accordance with the contents of the specification, and in order to make the above and other objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention are described below. Attached Figure Description

[0024] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings: Figure 1 A flowchart illustrating a method for formation flight tracking and collision avoidance control of a drone swarm according to an embodiment of the present invention is shown. Figure 2 A general framework diagram of rotorcraft unmanned aerial vehicle (UAV) swarm formation following, formation maintenance, ORCA collision avoidance, and control fusion according to an embodiment of the present invention is shown. Figure 3 is a flowchart of a method for implementing a route following module according to an embodiment of the present invention; Figure 4 is a flowchart of a formation-keeping module implementation method according to an embodiment of the present invention; Figure 5 is a flowchart of an embodiment of the ORCA collision avoidance module implementation method of the present invention; Figure 6 This is a flowchart illustrating a control fusion module implementation method according to an embodiment of the present invention; Figure 7 is a schematic diagram of the member bias route generation principle according to an embodiment of the present invention; Figure 8 This is a schematic diagram illustrating the principle of smoothing a straight-line route using a seven-segment S-curve according to an embodiment of the present invention. Figure 9 is a schematic diagram of a quadratic Bezier reference curve and two segments of the seventh-order Minimum-Snap final turning trajectory according to an embodiment of the present invention; Figure 10 is a schematic diagram of a collision avoidance safety area according to an embodiment of the present invention; Figure 11 is a schematic diagram of ORCA (Optimal Reciprocal Collision Avoidance) safe half-plane solution according to an embodiment of the present invention. Detailed Implementation

[0025] Exemplary embodiments of the invention will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this invention will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.

[0026] The present invention will now be described with reference to Figures 1 to 11, illustrating the complete control process of a rotorcraft UAV formation performing a flight path mission.

[0027] This invention equips each rotary-wing UAV with a flight path following module, a formation maintenance module, an ORCA collision avoidance module, and a control fusion module, which respectively execute the steps in the UAV swarm formation flight path following and collision avoidance control methods. Each module shares its own state, neighboring UAV states, mission waypoints, formation configuration, and pause and turn states, and calculates the corresponding outputs within the same control cycle. Combined with... Figure 1 , 2 As shown, the above modules perform the following steps respectively: Step S1: Obtain flight mission waypoints, local flight status, neighboring aircraft flight status and formation configuration, convert the local offset in the formation configuration to the local navigation coordinate system, and generate the local independent track; Step S2: Based on the independent flight path and the current flight phase of the aircraft, generate the route reference position, route reference speed, and reference heading; Step S3: Project the relative position and relative speed of the aircraft and at least one neighboring aircraft onto the reference heading, and generate the formation speed correction along the reference heading based on the proportional differential calculation of the longitudinal relative parameters; Step S4: Based on the relative motion between the local machine and its neighboring machines, identify conflicting neighboring machines, construct two-dimensional ORCA half-plane constraints for the identified conflicting neighboring machines, obtain the collision-free horizontal speed close to the desired speed within the feasible speed range defined by the constraints, and generate collision avoidance correction and channel occupancy status by combining vertical layering. Step S5: Perform channel control handover based on the channel occupancy status, and integrate the route reference speed, formation speed correction, and collision avoidance correction after channel control handover to form the speed, altitude, and heading control commands for the UAV.

[0028] The flight path following module first constructs an independent flight path for each rotary-wing UAV based on the member bias. Let the common waypoint be... The expected course is The machine system bias is Then, through the rotation matrix about the vertical axis Transform the offset to the local navigation coordinate system. Lateral offset. Used to construct parallel flight paths, altitude offset Direct stacking, vertical offset The indicated front-to-back spacing is achieved by the formation-keeping module through speed correction.

[0029] As shown in Figure 7, at the waypoints connecting adjacent flight segments, let the unit tangent vectors of the preceding and following flight segments be respectively... and Rotate both on the same side Obtain the unit normal vector and And take the direction that bisects the normal angle. Let the minimum included angle between the two flight segments be... The actual connecting waypoints can then be calculated using the following formula: in, To prevent the lower limit of offset divergence at sharp corners, This is the vertical unit vector in the local NED coordinate system. By setting an upper limit on the offset magnitude, excessively long waypoints at large turns can be avoided.

[0030] For a straight flight segment, let the unit tangent vector of the segment be... The unit normal vector is The length of the flight segment is Set local location Projecting the current flight segment yields the actual progress along the flight path. As shown in Figure 8, the velocity along the flight path is given by the jerk-constrained S-curve. Jerk is the third derivative of position with respect to time. It is constant for any given jerk. Phase, local time The acceleration, velocity, and displacement within are respectively The seven phases sequentially complete acceleration establishment, constant acceleration, acceleration exit, uniform velocity, deceleration establishment, constant deceleration, and deceleration exit; the constant acceleration phase or uniform velocity phase is allowed to have a zero duration. The controller... Reverse lookup reference speed Instead of simply sliding forward with accumulated running time, this method allows the track reference to be unaffected by communication or computation cycle errors.

[0031] Lateral track following employs proportional-derivative (PD) control based on track error. Let the lateral error be... The actual lateral speed of this machine is Then the lateral velocity correction can be expressed as This gives the basic horizontal velocity of the straight segment. The differential term uses the measured lateral velocity, which provides damping during lateral convergence.

[0032] As you approach the waypoint corner, based on your current speed and maximum allowable acceleration Basic terms for calculating braking distance Determine the advance entry distance for the curve. (Based on the entry point) Corner control points and exit point Constructing the turning reference curve This baseline curve is used to indicate the possible turning directions and intermediate connection conditions.

[0033] like Figure 9 As shown, based on the baseline curve, at least two segments of seventh-order polynomials are constructed respectively. To minimize the snap (the fourth derivative of position with respect to time), i.e., the square integral of the fourth derivative: Position constraints are set for the inlet, connection point, and outlet, as well as continuous constraints for velocity, acceleration, and jerk. Here, `snap` is the fourth derivative of position with respect to time. This optimization is called minimum-snap trajectory optimization. The objective function is written as a quadratic form, and the equality constraints are written as... Then, the polynomial coefficients are solved using the corresponding KKT linear equations. After trajectory generation, an arc length-time lookup table is established, and the reference position and velocity are queried based on the actual flight progress during runtime.

[0034] The formation-keeping module projects the relative position and relative speed of each neighboring aircraft onto the desired heading. Let's assume the aircraft is starting from this... Pointing to neighboring machines The relative position is The relative speed is The desired vertical spacing is Then define Error control has a dead zone, meaning that control action only occurs when the error exceeds a certain range. This portion of the error is set as... The adjustment amount corresponding to a single neighboring machine can be expressed as: For effective neighboring machines By averaging instead of summing directly, and then applying amplitude and rate-of-change limits, the longitudinal speed control quantity of the formation is obtained. This avoids sudden changes in total gain caused by changes in the number of neighboring machines.

[0035] The ORCA collision avoidance module first calculates the distance to neighboring devices within the detection range. Closing speed and the expected collision time .when When the dead zone is greater than the closing velocity, the remaining amount of the current distance relative to the safe merging radius can be used. Ratio estimation The intervention criterion uses a stricter distance or time threshold, while the maintenance criterion uses a more lenient threshold, thus forming a hysteresis mechanism; the collision avoidance correction decays to within the release dead zone before the overall collision avoidance state is released.

[0036] As shown in Figures 10 and 11, for each conflicting neighbor in the horizontal plane, based on relative position, relative velocity, merging radius, and finite time window... Construct a cutoff velocity barrier If the current relative velocity is within the velocity barrier, then calculate the minimum correction vector that will move it away from the velocity barrier boundary. and the unit normal vector pointing to the safe side When both parties bear equal responsibility for avoidance, the collision-free velocity half-plane of this machine can be written as: All Intersects with the circle of maximum horizontal speed, while satisfying all obstacle avoidance constraints and speed limits, and then selects the speed that is closest to the current speed.

[0037] in, It is formed from the current route reference speed. The solution can be obtained by semi-plane linear programming; when the local intersection is empty, the candidate speed is projected and corrected according to the constraint processing order to prioritize satisfying the previously established safety constraints.

[0038] To utilize the vertical maneuverability of rotary-wing UAVs, the ORCA collision avoidance module simultaneously generates vertical layered offsets. In the NED (North-East-Down) coordinate system, a smaller vertical coordinate indicates a higher altitude; when there is a clear altitude difference between two UAVs, the higher one continues upward and the lower one continues downward; when the two UAVs are approximately at the same altitude, the opposite up and down directions are determined by the member identifier. For lateral repositioning of UAVs in the same group, discrete altitude layers can be assigned according to the member order, ensuring that symmetrical exchanges have a definite vertical selection. The final vertical offset is output after being limited by amplitude and rate of change.

[0039] When drones in the same group are flying approximately symmetrically towards each other about the formation center, ORCA may cause both to simultaneously reduce their head-on speed, resulting in the drones coming to a complete stop and being unable to continue the obstacle avoidance mission. To solve this problem, [the following can be done]: Additional rotation in the same direction. The rotation angle increases as the distance from the UAV to the formation center decreases and is limited by a maximum angle. The velocity after rotation is only used to determine the desired direction within the ORCA feasible region; the ORCA half-plane remains responsible for safety constraints.

[0040] ORCA collision avoidance module will Multiply by the time domain The horizontal collision avoidance position offset is obtained and, together with the vertical layer offset, constitutes... The offset, after being limited by amplitude and three-dimensional rate of change, is written into the shared state. The control fusion module then presses the same... By restoring the horizontal offset to the velocity difference, the intermediate offset variable can be easily reused in the position control interface and can also be easily implemented in a unified manner to achieve smooth decay.

[0041] To prevent the controllers from canceling each other out, the ORCA collision avoidance module further generates at least three states based on the projection of the collision avoidance offset relative to the flight path tangent vector and normal vector: horizontal collision avoidance state. Occupancy status along the heading Total collision avoidance status .when When effective, the route following module will Zeroing or smooth decay, but still retaining ;when When effective, the formation maintenance module retains the formation position offset, but will... Smoothly reduce to zero according to the rate of change; when only vertical stratification exists. and If not positioned, horizontal navigation and formation control continue to operate.

[0042] The horizontal speed command generated by the control fusion module can be written as: The altitude command is The heading command remains the reference heading of the route. The fused horizontal velocity is constrained by vector magnitude rather than having its coordinate components individually trimmed, in order to maintain the synthesized direction.

[0043] When the synthesized horizontal velocity falls below the position-holding threshold, the control fusion module switches to rotor position-holding mode to avoid position integral drift near zero speed. If necessary inputs such as the route reference position and velocity are incomplete, a safety command is generated using the current measured position, zero airspeed or zero horizontal velocity, and the current heading. The issuance or issuance of position-holding commands ceases when the mission is paused.

[0044] During mission pause, waypoint indices, S-curve progress, and turn progress are frozen. If recovery occurs on a straight segment, the S-curve is replanned based on the current position, measured speed along the flight path, and remaining flight segment. If recovery occurs on a turn segment, the original Minimum-Snap trajectory is abandoned; the straight segment is reconstructed from the current position, and the turn trajectory is replanned when the turn entry conditions are met. Upon reaching the mission endpoint, the endpoint position is latched, and position hold commands are continuously output. The following describes an integrated implementation where each processing stage operates collaboratively within the same airborne control system and continuous control cycle. Each threshold, gain, speed limit, and offset limit can be designed based on the rotorcraft UAV's size, dynamic performance, positioning accuracy, communication cycle, and mission safety interval.

[0045] Example: Formation following, maintaining, ORCA collision avoidance and control integration In this embodiment, the cluster consists of multiple rotary-wing UAVs, each equipped with a positioning and inertial measurement unit, inter-UAV communication device, onboard processor, and flight controller. At the start of the mission, the ground terminal or leader node sends the common waypoint sequence, cruise speed, number of cycles, member identifier, desired formation offset, and formation change configuration to each member. Each member periodically receives the position, speed, heading, and data timestamps of itself and its neighbors, and writes a state snapshot of the same control cycle to the shared data area or behavior tree blackboard.

[0046] The technical terms used in this embodiment are defined as follows: NED coordinate system is a local navigation coordinate system with North, East, and Ground as its three axes, where the positive direction of the Ground axis is downward; the direction perpendicular to the horizontal track is called the lateral (cross-track); CTE is the Cross-Track Error; TTC is the Time to Collision; jerk is the third time derivative of position, and snap is the fourth time derivative of position; Velocity Obstacle (VO) represents the set of relative velocities that would cause a collision between the two aircraft within a given time window; ORCA half-plane constraint is the set of safe speeds constructed based on the VO boundary, minimum speed correction, and ORCA liability factor.

[0047] like Figure 7 As shown, during the initialization phase, the route following module selects the local machine system offset based on the member identifier. Let the common waypoints be... The unit normal vectors on the same side of the flight segment before and after the turn are respectively and The angle bisectors of the two are in the direction of The turning point of the flight segment is The flight path following module will offset laterally. along Cosine compensation is applied to the direction to obtain the member connection points. The vertical distance from this connection point to both the original flight segments before and after the turn is... Therefore, crew tracks can connect continuously at corners and maintain the prescribed lateral spacing. Altitude offset Individual stacking, vertical offset The formation-keeping module still manages the speed correction. If a corner is too sharp or close to a U-turn, causing the connection point distance to be too far or the calculation to be unstable, the connection distance is limited, or an arc is used to complete the turn.

[0048] As shown in Figure 2, each control cycle completes a closed-loop calculation based on unified data and module dependencies. The route following module first generates the reference position, reference speed, reference heading, and straight / turning status; the formation holding module generates the formation speed correction along the heading based on the reference heading and the status of adjacent aircraft; the ORCA collision avoidance module uses the route reference speed as the desired speed and combines it with the relative motion of adjacent aircraft to generate ORCA collision avoidance correction and channel occupancy status; finally, the control fusion module reads the three types of outputs, performs channel-specific control handover, vector synthesis, and amplitude limiting, and issues only one unified control command to the flight controller.

[0049] As shown in Figures 3 and 8, when the aircraft is in a straight flight state, the route following module projects the current position onto the current flight segment of the aircraft to obtain the actual progress along the route. The longitudinal velocity planner generates seven S-curves based on velocity, acceleration, and jerk constraints. Figure 8 The jerk is shown from top to bottom. acceleration ,speed and location The correspondence is as follows: the first three stages allow acceleration to be smoothly established and exited; the fourth stage is used for constant-speed cruise; and the last three stages allow the UAV to smoothly decelerate to the end of the flight segment. After successive integration of segmented constant acceleration, continuous acceleration, velocity, and position are formed. The fourth stage is retained for long flight segments, while it is shortened or eliminated for short flight segments by searching for feasible peak velocities. The constant acceleration stage can also be zero in duration. The controller is based on... Query reference speed along the flight path Instead of advancing the reference point solely based on cumulative running time, the lateral controller simultaneously generates proportional-derivative corrections based on both CTE and measured lateral velocity. This allows us to obtain the baseline horizontal speed for the flight path.

[0050] As shown in Figures 3 and 9, when the remaining segment length enters the curve entry window determined by the current speed and maximum permissible acceleration, the route state machine first completes the straight-line output for the current control cycle, and then switches to the turning state in the next control cycle. The turning planner uses the entry point... Waypoint corners and exit points Construct a quadratic Bézier reference skeleton and determine intermediate connection points on the skeleton. ; then in to as well as to The seventh-order Minimum-Snap polynomial trajectory is solved separately. In Figure 8, the dashed line represents the quadratic Bezier reference skeleton, and the solid line represents the final Minimum-Snap trajectory; the inlet and outlet velocity directions are constrained by the pre-turn and post-turn segments, respectively, and the two trajectories are... The position and its fourth derivative remain continuous, so the final trajectory can smoothly bypass the original sharp corner without passing through the original waypoint corner. During the turn, the reference position, speed, and heading are output based on the arc length-time lookup relationship and the actual progress. After the turn is completed, the system switches to the next straight segment.

[0051] In conjunction with the route baseline calculation, the formation keeping module will record the relative positions of adjacent aircraft. and relative velocity Projected onto reference heading This results in longitudinal spacing error. and error change rate The proportional term is used to eliminate the deviation between the desired front-to-back distance and the actual distance, while the derivative term forms the relative speed damping. The adjustments from multiple adjacent machines are first averaged, then sequentially passed through the error dead zone, speed amplitude limit, and rate of change limit before output. Using averaging instead of direct summation avoids abrupt changes in the equivalent control gain when the number of effective neighboring machines changes.

[0052] The ORCA collision avoidance module then calculates the relative distance, closing speed, and TTC of each neighboring machine within the same control cycle. Closing speed indicates whether two machines are approaching; TTC estimates the time required to reach the safe merging radius while maintaining the current relative speed. The module employs a hysteresis mechanism with a tighter intervention threshold and a looser hold threshold to prevent frequent ORCA state starts and stops due to fluctuations in distance or TTC around the thresholds. Only neighboring machines that meet the detection distance, state validity, and conflict criteria are included in the ORCA constraint set for the current cycle.

[0053] As shown in Figures 10 and 11, for each conflicting neighbor, the ORCA collision avoidance module determines the collision avoidance parameters based on relative position, relative velocity, and safe merging radius. and finite time window Construct a velocity barrier. Figure 10 shows a barrier consisting of two tangents and a radius of... The shaded area enclosed by the truncated arc represents the dangerous relative speed range, and the area outside the shade is the safe zone; when the current relative speed... When located within a danger zone, take the point closest to the speed barrier boundary. And obtain the minimum velocity correction vector. Figure 11 illustrates the construction process of the ORCA half-plane: when both sides execute the same ORCA method, the local machine at the current speed On the basis of undertaking half of the correction amount u, with u is the boundary point, and the unit normal vector points towards the safe side. A half-plane is allowed, with another UAV undertaking the correction in the opposite direction. If the neighboring UAV does not participate in cooperative collision avoidance, the ORCA liability coefficient of the first UAV is increased. The intersection of all ORCA half-planes and the maximum velocity circle constitutes the feasible velocity domain. Linear programming is used to find the collision-free horizontal velocity that is closest to the optimal velocity from this domain. .

[0054] To leverage the vertical maneuverability of rotary-wing UAVs, the ORCA collision avoidance module simultaneously calculates vertical layer offsets in addition to solving for horizontal ORCA velocity. If two UAVs already have a clear height difference, the separation trend of the higher one moving upwards and the lower one downwards is maintained; if the two UAVs are approximately at the same height, opposite vertical directions are determined based on member identifiers; if members of the same group exchange positions laterally, discrete height layers are assigned according to member order. Vertical layering is an obstacle avoidance method designed for the three-dimensional maneuverability of rotary-wing UAVs, without changing the definition of the two-dimensional ORCA half-plane constraints, nor is vertical layering equivalent to the standard two-dimensional ORCA algorithm itself.

[0055] ORCA collision avoidance module will The system is decomposed into a lateral component and a longitudinal component, and combined with the vertical layering results to generate a lateral occupancy state A, a transverse occupancy state H, and a total collision avoidance state C. H is set when the transverse component exceeds the transverse dead zone; A is set when the lateral component exceeds the dead zone; C remains active as long as ORCA is still engaged or the collision avoidance offset has not decayed to within the release dead zone. The dead zone is the interval where small errors do not trigger state switching, used to suppress control jitter caused by measurement noise.

[0056] As shown in Figure 4, the channel occupancy status immediately participates in module coordination: when H is valid, the route following module corrects the lateral speed. Zero or smooth decay, but retain speed along the flight path. When A is valid, the formation holding module preserves the formation bias while simultaneously enabling... Smoothing approaches zero; when ORCA only outputs vertical layer offset, neither H nor A is set, and horizontal line following and formation continue to work.

[0057] As shown in Figure 6, the control fusion module divides the position offset of the ORCA horizontal velocity difference after conversion to the time domain Th by... The speed correction is restored and superimposed with the route reference horizontal speed, the formation speed correction along the heading, and the ORCA horizontal speed correction according to Equation (12). The altitude channel superimposes the route reference altitude with the vertical layer offset, and the heading channel uses the route reference heading. If the synthesized horizontal speed is lower than the position holding threshold, the rotor position holding mode is switched; if the necessary input is incomplete, a safety command is generated with the current measured position, zero horizontal speed, and current heading.

[0058] For example, when the formation is flying along a straight segment and an adjacent aircraft is approaching laterally, the flight path following module first provides S-curve speed and lateral speed corrections, while the formation holding module simultaneously provides spacing corrections; after detecting that the TTC has entered the intervention threshold, the ORCA collision avoidance module constructs a velocity half-plane and calculates the velocity with lateral and heading components. Then H and A are set. In the next fusion result, the route following module no longer has a control function, the formation longitudinal speed regulation is smoothly reduced, the ORCA correction obtains the corresponding channel control, while the non-conflicting components such as route reference, altitude and heading are still retained, so there will be no mutual cancellation between the three controllers on the same speed channel.

[0059] As the formation approaches a corner, the state machine generates and provides the turning state to the formation holding module and the ORCA collision avoidance module. The formation speed decays smoothly to avoid disrupting the high-curvature turning trajectory; the ORCA horizontal solver can pause, limit output, or update the desired speed using the turning reference speed based on the turning state. If only height layering is required at this time, the horizontal turning trajectory remains valid. Therefore, turning is not a separate control process independent of formation and collision avoidance, but rather a route reference state within the same control chain.

[0060] Once the colliding neighboring aircraft is far away and both TTC and distance conditions meet the release criteria, the ORCA obstacle avoidance mode is deactivated. Existing horizontal and vertical offsets are not immediately zeroed but gradually decay; C remains active until decay is complete. After the lateral and heading-side occupancy is cleared, lateral control and formation speed correction resume at the limited rate, allowing the aircraft to smoothly return to the crew track and desired formation, avoiding secondary oscillations or overshoot at the moment of collision avoidance termination.

[0061] When the mission is paused, the route module freezes the waypoint index, the actual progress of the S-curve, and the turning progress, while the control fusion module outputs the current position hold command. If recovery occurs on a straight section, the S-curve is replanned based on the current position, measured speed along the route direction, and remaining distance. If recovery occurs on a turning section, the original Minimum-Snap trajectory is abandoned, a straight-line reference is reconstructed from the current position, and a new turning trajectory is planned when the turning conditions are met again. Upon reaching the final waypoint, the final point position is latched, and the formation and ORCA status continue to maintain safe conditions, ultimately entering position hold mode.

[0062] In this embodiment, the flight path following module, formation keeping module, ORCA collision avoidance module, and control fusion module can be implemented as software nodes on the onboard processor, exchanging data via shared memory, publish-subscribe bus, or behavior tree port. A non-limiting parameter set can set a 15-meter neighbor detection range, a 6-meter safe merging radius, a 3-second ORCA time window, a maximum horizontal speed of 10 meters per second, a speed difference converted to time domain of approximately 1 second, a formation error dead zone of approximately 1 meter, and a corresponding rate of change upper limit. The above values ​​are only used to illustrate the data relationships and control timing between modules and do not constitute a limitation on the scope of protection of the claims.

[0063] Compared to existing methods that employ adaptive L1 trajectory tracking, circular turns, and a combination of artificial potential fields and speed barriers, while simultaneously increasing overall priority during collision avoidance, this invention, in a single end-to-end embodiment, combines a jerk-constrained seven-segment S-curve based on actual flight path position sampling, L1 guidance control, quadratic Bézier curves and seventh-order minimum-snap trajectories, linear programming based on two-dimensional ORCA half-plane constraints, vertical deterministic hierarchical structure, and channel-specific control handover. Those skilled in the art can make equivalent substitutions for the S-curve solution method, the number of polynomial segments, the ORCA liability coefficient, the conflict threshold, the hierarchical rules, the software framework, and the control interface. As long as each module still forms a single rotor control command within a unified control cycle according to the aforementioned data dependencies and control component priority relationships, this invention can be implemented.

[0064] It should be noted that: The algorithms and displays provided herein are not inherently related to any particular computer, virtual device, or other equipment. Various general-purpose devices can also be used in conjunction with the teachings herein. The required structure for constructing such devices is apparent from the above description. Furthermore, this invention is not directed to any particular programming language. It should be understood that the contents of the invention described herein can be implemented using various programming languages, and the above description of specific languages ​​is for the purpose of disclosing the best mode of implementation of the invention.

[0065] Numerous specific details are set forth in the specification provided herein. However, it will be understood that embodiments of the invention may be practiced without these specific details. In some instances, well-known methods, structures, and techniques have not been shown in detail so as not to obscure the understanding of this specification.

[0066] Similarly, it should be understood that, in order to streamline the invention and aid in understanding one or more of the various aspects of the invention, features of the invention are sometimes grouped together in a single embodiment, figure, or description thereof in the above description of exemplary embodiments of the invention. However, this disclosure should not be construed as reflecting an intention that the claimed invention requires more features than are expressly recited in each claim.

[0067] Those skilled in the art will understand that modules in the device of the embodiments can be adaptively changed and placed in one or more devices different from that embodiment. Modules, units, or components in the embodiments can be combined into a single module, unit, or component, and further, they can be divided into multiple sub-modules, sub-units, or sub-components. Except where at least some of such features and / or processes or units are mutually exclusive, any combination can be used to combine all features disclosed in this specification (including the accompanying claims, abstract, and drawings) and all processes or units of any method or device so disclosed. Unless expressly stated otherwise, each feature disclosed in this specification (including the accompanying claims, abstract, and drawings) may be replaced by an alternative feature that serves the same, equivalent, or similar purpose.

[0068] Furthermore, those skilled in the art will understand that although some embodiments described herein include certain features included in other embodiments but not others, combinations of features from different embodiments are meant to be within the scope of the invention and form different embodiments.

[0069] The various component embodiments of the present invention can be implemented in hardware, or as software modules running on one or more processors, or a combination thereof. Those skilled in the art will understand that microprocessors or digital signal processors (DSPs) can be used in practice to implement some or all of the functions of some or all of the components in the UAV swarm formation following and collision avoidance control device according to embodiments of the present invention. The present invention can also be implemented as a device or apparatus program (e.g., a computer program and computer program product) for performing part or all of the methods described herein. Such programs implementing the present invention can be stored on a computer-readable medium or can be in the form of one or more signals. Such signals can be downloaded from an Internet website, provided on a carrier signal, or provided in any other form.

[0070] This invention provides a non-volatile computer storage medium storing at least one executable instruction that can execute the above-described method embodiment of the UAV swarm formation flight path following and collision avoidance control method.

[0071] It should be noted that the above embodiments are illustrative of the invention and not restrictive, and that those skilled in the art can devise alternative embodiments without departing from the scope of the appended claims. In the claims, any reference signs placed between parentheses should not be construed as limiting the claims. The word "comprising" does not exclude the presence of elements or steps not listed in the claims. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The invention can be implemented by means of hardware comprising several different elements and by means of a suitably programmed computer. In the unit claims enumerating several means, several of these means may be embodied by the same item of hardware. The use of the words first, second, and third, etc., does not indicate any order. These words can be interpreted as names.

Claims

1. A method for formation flight path following and collision avoidance control of a drone swarm, executed by each drone in the swarm, characterized in that, The steps include the following: The system acquires flight mission waypoints, its own flight status, neighboring aircraft flight status, and formation configuration. It then converts the own offset in the formation configuration to the local navigation coordinate system to generate its own independent flight track. Based on the independent flight path and the current flight phase of the aircraft, generate the route reference position, route reference speed, and reference heading; The relative position and relative speed of the aircraft and at least one neighboring aircraft are projected onto the reference heading, and the formation speed correction along the reference heading is generated by proportional differential calculation based on the longitudinal relative parameters. Based on the relative motion between the local machine and its neighboring machines, conflicting neighboring machines are selected. Two-dimensional ORCA half-plane constraints are constructed for the selected conflicting neighboring machines. Within the feasible velocity domain defined by the constraints, the collision-free horizontal velocity close to the desired velocity is obtained. The collision avoidance correction amount and the channel occupancy status are generated by combining the vertical layering. Based on the channel occupancy status, channel control and handover are performed. The route reference speed, formation speed correction, and collision avoidance correction after channel control and handover are integrated to form the speed, altitude, and heading control commands for the UAV.

2. The method according to claim 1, characterized in that, Transforming the machine offset in the formation configuration to the local navigation coordinate system and generating the machine's independent track includes: rotating the lateral formation offset in the machine system around the desired heading to the local navigation coordinate system; at the connecting waypoints of adjacent segments, calculating the actual connecting waypoints based on the angle bisector of the normal vectors on the same side of the two segments and the cosine compensation of the turning angle to form mutually parallel independent tracks of the members; superimposing the altitude offset onto the independent tracks of the members; and using the formation speed correction to achieve the longitudinal formation spacing in the machine system. Based on the independent flight path and the current flight phase of the aircraft, the generation of the route reference position, route reference speed, and reference heading includes: in the straight flight phase, generating the reference speed along the route direction based on speed, acceleration, and jerk constraints, and generating the lateral speed correction based on the L1 guidance method; in the turning flight phase, constructing the turning reference curve using a quadratic Bezier curve, and generating the route reference position and route reference speed using the MinimumSnap polynomial. The channel control and handover based on the channel occupancy status includes: when lateral collision avoidance correction is required, the line following module sets the lateral speed correction to zero or smoothly decays it, but still retains the reference speed along the line direction; when directional collision avoidance correction is required, the formation holding module retains the formation position offset, but smoothly reduces the formation speed correction to zero according to a preset rate of change; when the collision avoidance correction only includes the vertical layer component, the occupancy status of the horizontal lateral channel and the directional channel is not set, and the horizontal line following and formation holding continue to operate.

3. The method according to claim 2, characterized in that, During the straight-line flight phase, a reference speed along the flight path is generated based on speed, acceleration, and jerk constraints, including: Determine the feasible peak speed along the route direction based on the current remaining segment length, maximum permissible speed, maximum permissible acceleration, and maximum permissible jerk. With the feasible peak velocity as the peak target, the jerk is constructed as a piecewise constant in multiple sequentially connected phases. The multiple phases sequentially complete the acceleration establishment, constant acceleration, acceleration exit, uniform velocity, deceleration establishment, constant deceleration, and deceleration exit. By integrating the jerk in each phase piecewise, a monotonically increasing parametric mapping between position and velocity along the flight path is established. Based on the mapping relationship, an S-curve is formed, where the S-curve defines the reference velocity value at any position along the flight path. Project the machine's position along the direction of the unit tangent vector of the current straight segment to obtain the actual progress along the flight path; The actual progress along the route is used as the independent variable of the S-curve mapping, and the corresponding speed value is directly output as the reference speed of the route.

4. The method according to claim 2, characterized in that, The generation of lateral velocity correction based on the L1 guidance method includes: determining the lateral error direction and the lateral velocity projection direction based on the unit normal vector of the current flight segment; calculating the lateral velocity correction using the L1 guidance law based on the lateral track error of the aircraft position relative to the current flight segment and the projection of the aircraft's actual velocity onto the unit normal vector, wherein the lateral velocity correction is a scalar value along the unit normal vector direction; when the horizontal lateral collision avoidance channel is occupied, the lateral velocity correction is set to zero or smoothly decays to zero at a preset rate; During the turning flight phase, a quadratic Bézier curve is used to construct the turning reference curve, and the route reference position and speed are generated using the MinimumSnap polynomial. This includes: calculating the advance entry distance based on the current speed and the maximum allowable acceleration to determine the timing for switching from the straight segment to the turning segment; constructing a quadratic Bézier curve using the turning entry point, waypoint corner control point, and turning exit point as the turning reference geometric skeleton; determining at least one intermediate connection point on the turning reference geometric skeleton to divide the trajectory from the entry to the exit into at least two segments, constructing a seventh-order polynomial for each segment, and setting continuity constraints for position, velocity, acceleration, and jerk at the endpoints and intermediate connection points of each segment; solving for the polynomial coefficients that satisfy the continuity constraints with the objective of minimizing the squared integral of the fourth derivative of the polynomial trajectory position with respect to time for each segment; establishing the mapping relationship between the trajectory arc length and time parameters, and querying the turning reference position and speed based on the actual arc length progress of the aircraft on the trajectory.

5. The method according to claim 1 or 2, characterized in that, Projecting the relative position and relative speed of this aircraft and at least one neighboring aircraft onto the reference heading, and generating the formation speed correction along the reference heading based on the proportional derivative of the longitudinal relative parameters includes: Project the relative position and relative speed of the machine and at least one neighboring machine onto the reference heading to obtain the longitudinal spacing error and longitudinal relative speed of each neighboring machine; Based on the longitudinal spacing error and longitudinal relative speed, proportional-derivative adjustment amounts are generated for each adjacent machine through proportional-derivative control. The average control is obtained by averaging the proportional-derivative controls of all effective adjacent machines. The average adjustment amount is sequentially subjected to dead zone processing, amplitude limitation, and rate of change limitation. The output after the dead zone processing, amplitude limitation, and rate of change limitation is used as the formation speed correction amount along the reference heading.

6. The method according to claim 1 or 2, characterized in that, Conflicting neighboring machines are identified based on the relative motion between the machine and its neighboring machines, including: For each neighboring machine within the detection range, calculate the straight-line distance, closing speed, and estimated collision time between the local machine and the neighboring machine. When the closing speed is greater than the preset closing speed dead zone, the estimated collision time is estimated by dividing the difference between the straight-line distance and the safe merging radius by the closing speed. The safe merging radius is the sum of the safe radii of the two machines plus a safety margin. A strict intervention distance threshold and intervention time threshold are used to determine whether to initiate collision avoidance. When the straight-line distance is less than the intervention distance threshold or the expected collision time is less than the intervention time threshold, the neighboring machine is included in the conflicting neighboring machine set. A hysteresis mechanism is formed by adopting a relatively lenient holding distance threshold and holding time threshold. When the straight-line distance is greater than or equal to the holding distance threshold and the expected collision time is greater than or equal to the holding time threshold, and the collision avoidance correction has been smoothly decayed into the release dead zone, the neighboring machine is removed from the set of conflicting neighboring machines and the collision avoidance state is released. The release dead zone is an error tolerance interval used to determine that the collision avoidance correction has been sufficiently converged.

7. The method according to claim 1, characterized in that, To construct two-dimensional ORCA half-plane constraints for each selected conflicting neighbor, and to obtain the collision-free horizontal velocity near the desired velocity within the feasible velocity domain defined by the constraints, including: For each conflicting neighbor, based on the relative position, relative speed, safe merging radius and finite time window of the local machine and the neighbor, a cutoff speed barrier is constructed in the relative speed space. If the current relative speed is within the cutoff speed barrier, the nearest point from the current relative speed to the boundary of the cutoff speed barrier is calculated, and the difference between the current relative speed and the nearest point is used as the minimum correction vector to make the relative speed leave the speed barrier boundary. At the same time, the unit normal vector pointing to the safe side is determined. Based on the principle that both parties bear part of the responsibility for avoiding collisions, the sum of the current speed of the machine and the boundary offset determined based on the minimum correction vector is used as the boundary point of the half-plane. Combined with the unit normal vector, the collision-free velocity half-plane of the machine is constructed. The feasible velocity domain is obtained by intersecting the non-collision velocity half-plane corresponding to all conflicting neighboring machines with the circle centered on the current velocity of the machine and with the maximum horizontal velocity as the radius. Within the feasible speed domain, the speed that minimizes the Euclidean distance from the desired speed of the machine is determined as the collision-free horizontal speed, where the desired speed of the machine is formed by the current route reference speed. When the intersection is empty, the candidate velocities are projected and corrected in order of constraint priority.

8. The method according to claim 1 or 2, characterized in that, The vertical layering includes at least one of the following: In the local coordinate system, when there is a clear height difference between the machine and its neighboring machine, the machine with the higher height continues to move upward and the machine with the lower height continues to move downward. When the two drones are approximately at the same height, determine the opposite up and down directions based on the drone markings; For UAVs in the same group that exchange lateral positions, discrete altitude layers are assigned according to the member order, so that symmetrical exchanges have a definite vertical selection. The final vertical offset is output after applying amplitude and rate of change limits.

9. The method according to claim 1 or 2, characterized in that, Before constructing two-dimensional ORCA half-plane constraints for each selected conflicting neighbor, the following steps are also included: For UAVs that are approximately symmetrical and flying towards each other in a formation, their desired speeds are rotated in the same direction by an angle related to the distance of the UAV from the formation center to form an offset desired speed to eliminate stagnation in symmetrical flying towards each other. This offset desired speed is used as the desired speed input when constructing the ORCA half-plane constraint. The rotated speed is only used to determine the desired direction in the ORCA feasible region, and the ORCA half-plane is still responsible for safety constraints.

10. The method according to claim 1 or 2, characterized in that, The fusion of the route reference speed, formation speed correction, and collision avoidance correction after the passage control handover includes at least one of the following: The horizontal collision avoidance position offset is converted into a horizontal collision avoidance speed difference according to a preset time domain, wherein the horizontal collision avoidance position offset is obtained by multiplying the difference between the non-collision horizontal speed and the machine's expected speed by the time domain. The vector obtained by multiplying the baseline horizontal speed, the formation speed correction along the baseline heading by the baseline heading unit vector, and the horizontal collision avoidance speed difference are vector superimposed, and the vector magnitude is limited on the composite horizontal speed instead of trimming the coordinate components separately. The vertical collision avoidance offset is superimposed on the flight path reference height; Switch to position holding mode when the synthesis speed is below the position holding threshold; When at least one of the route reference position and speed is not available, a safety instruction is generated using the current measured position and heading.

Images