A uav cluster cooperative positioning and obstacle avoidance control system and method

Abstract

This invention discloses a collaborative localization and obstacle avoidance control system and method for unmanned aerial vehicle (UAV) swarms. This method uses a first comprehensive cost function to issue swarm change commands only when the accuracy improvement from configuration changes significantly outweighs the cost of flight maneuvers, effectively filtering out the ping-pong effect caused by frequent swarm changes. Through coarse-to-fine hierarchical localization processing, the method utilizes FM coarse localization solutions to resolve the TDOA observation equations in the far-field high-altitude region. GDOP The technical challenge of regional divergence is addressed by using a "hot start" mechanism to map the initial value of GN to EKF in real time, thus avoiding the accuracy collapse caused by the accumulation of linearization errors in the cold start phase of traditional filtering algorithms. This invention overcomes the limitation of decoupling the logic of "obstacle avoidance" and "positioning accuracy maintenance." Within the rolling optimization framework of model predictive control, it explicitly embeds rigid geometric constraints based on the Laplace matrix, enabling the cluster to replace disordered drift with elastic compression when performing obstacle avoidance actions.

Description

Technical Field

[0001] This invention belongs to the field of unmanned system swarm cooperative control and modern signal processing technology, specifically relating to a swarm cooperative positioning and obstacle avoidance control system and method for unmanned aerial vehicles. Background Technology

[0002] Multi-UAV collaborative formations for complex and dangerous missions are an inevitable trend for future development. For multi-UAV collaborative formations, the first step is information perception and fusion of multi-source information; secondly, task allocation and decision-making; next, trajectory planning for each UAV to generate the desired trajectory; then, formation control methods are used to achieve multi-UAV formation flight missions; network communication issues need to be carefully considered during formation control design; finally, a virtual simulation platform and a physical demonstration platform for simulating multi-UAV collaborative formation flight are built to verify the feasibility and effectiveness of the algorithm. With the informatization and intelligentization of the digital environment, passive positioning technology, due to its non-signal transmission, has significant advantages such as strong concealment and wide detection range.

[0003] In the field of UAV swarm localization and tracking, the most commonly used approach is nonlinear estimation based on the TDOA observation system. Initial localization is typically achieved using snapshot algorithms such as the Chan algorithm and Taylor series expansion, followed by dynamic target tracking using Extended Kalman Filter (EKF) or Particle Filter. These methods construct a hyperbolic equation system based on the time difference of arrival of signals received from multiple observation stations, and iteratively approximate the target state using first-order Taylor linearization, given a known target motion model (e.g., uniform velocity or uniform acceleration). Regarding formation decision-making and reconfiguration, traditional techniques often employ hard-switching logic based on distance thresholds. When the distance between the UAV and the target reaches a preset value, the swarm is instructed to perform discrete formation changes. This approach typically does not consider the energy consumption during formation changes, nor does it assess the real-time impact of current environmental errors (such as station location errors and time difference measurement noise) on the localization performance of different configurations.

[0004] While traditional existing technologies can accomplish basic tasks in their respective fields, they suffer from the following significant technical problems under complex, constrained, and non-ideal observation environments: Poor far-field convergence and stability of the localization solution: Due to the strong nonlinearity and geometric dilution characteristics of the TDOA observation equation in the far field, traditional methods based on dynamic fusion or single-stage EKF suffer from rapid amplification of the linearization truncation error generated by the first-order Taylor expansion during the cold start phase, which lacks accurate initial values. This not only causes severe divergence in the solution results but also prevents the system from achieving stable target acquisition in the early stages of long-range detection.

[0005] Obstacle avoidance maneuvers are susceptible to mechanical deadlock and target unreachability traps: Traditional artificial potential field methods only model the resultant force based on radial attraction and repulsion. When the UAV, obstacle, and target point are in collinear and symmetrical positions, the force vectors cancel each other out along the same straight line, forming a potential energy saddle point, which can cause the UAV to get stuck in a local minimum deadlock or generate high-frequency oscillations. In addition, if the target point is located within the repulsive force domain of the obstacle, it can cause mission stagnation and target unreachability.

[0006] There is a serious logical decoupling between obstacle avoidance maneuvers and localization geometry performance: most existing obstacle avoidance algorithms treat flight safety as the sole criterion, ignoring the impact of maneuvers on the observation configuration. When traversing confined spaces such as narrow gates, the lack of rigid constraints on the overall swarm geometry leads to disordered offsets by individual UAVs to ensure safety, causing the optimal observation aperture to completely collapse. This topological distortion deteriorates the condition number of the observation matrix, causing the GDOP to rise sharply with obstacle avoidance maneuvers, ultimately resulting in the failure of the localization task during obstacle avoidance due to geometric degradation.

[0007] The formation reconfiguration strategy lacks a trade-off assessment of energy efficiency benefits, and formation decisions are often arbitrary: existing formation switching is mostly based on simple hard distance threshold triggers, without introducing a trade-off mechanism between positioning accuracy gains and physical maneuver energy consumption. This leads to frequent formation changes in areas where performance improvements are not significant, creating a "ping-pong effect" that not only introduces additional positioning noise but also significantly shortens the mission endurance of the UAV swarm. Summary of the Invention

[0008] To address the aforementioned problems in the existing technology, this invention provides a collaborative positioning and obstacle avoidance control system and method for unmanned aerial vehicle (UAV) swarms. The technical problem to be solved by this invention is achieved through the following technical solution: In a first aspect, the present invention provides a method for cooperative localization and obstacle avoidance control of a swarm of unmanned aerial vehicles (UAVs), the method comprising: Based on the site error, time difference measurement error, and target distance, a search and matching process is performed in a pre-built offline strategy library, and the optimal formation configuration of the UAV cluster for the current mission stage is determined using the first comprehensive cost function. Based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration at the current mission stage, and the target estimated position at the previous moment, coarse-fine level positioning processing is performed to obtain the target estimated position at the current moment. Based on the target's estimated position, a virtual center guiding force is synthesized and a rotational field correction is applied using the improved artificial potential field (I-APF) to generate a reference trajectory. Based on the optimal formation configuration and integrated dynamic constraints of the reference trajectory, the underlying control commands are obtained through rolling optimization calculation of the second comprehensive cost function, realizing MPC-based two-layer hierarchical control and formation rigidity maintenance, so as to control the UAV swarm to complete obstacle avoidance; The target's estimated position is fed back in real time to achieve closed-loop feedback.

[0009] In one embodiment of the present invention, based on the site error, time difference measurement error, and target distance, a search and matching process is performed in a pre-built offline strategy library, and the optimal formation configuration of the UAV swarm for the current mission stage is determined using a first comprehensive cost function, including: By sensing distance and error, we can obtain the current station location error, time difference measurement error, and target distance. The target formation configuration is confirmed by searching a pre-built offline strategy library based on the station site error, time difference measurement error, and target distance. Using the first synthesis cost function The difference between the accuracy improvement gained from switching to the target configuration and the maneuver energy consumption required to perform the configuration change is used to make a judgment: if the difference is greater than a preset hysteresis threshold... When the target configuration is selected as the optimal formation configuration for the current mission phase, and the difference is less than or equal to the preset hysteresis threshold, the formation configuration is considered optimal. When the formation configuration of the previous moment is taken as the optimal formation configuration for the current mission phase.

[0010] In one embodiment of the present invention, the first comprehensive cost function The expression is as follows: ; in, Indicates the precision gain weight. This represents the theoretical root mean square error of the current formation. This represents the theoretical root mean square error after the formation change. Indicates the weight of motor energy efficiency loss. Represents the normalization factor. This indicates the total number of drones in the drone swarm. Indicates the first i Ideal configuration location for a drone Indicates the first i The current location of the drone.

[0011] In one embodiment of the present invention, coarse-to-fine level positioning processing is performed based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration of the current mission phase, and the target estimated position at the previous moment, to obtain the target estimated position at the current moment, including: The system receives the current station coordinates of the UAV formation and the TDOA measurement data relative to the main station in real time, performs time alignment and gross error removal preprocessing, and obtains the relative geometric relationship between the UAV cluster and the target radiation source. Based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration for the current mission phase, and the target's estimated position from the previous moment, calculate the geometric accuracy factor for the current moment. GDOP ; Geometric precision factor GDOP With preset threshold Comparison: When geometric precision factor GDOP Greater than the preset threshold At that time, a rough position estimate for the current moment is obtained through FM coarse positioning solution; when the geometric precision factor... GDOP Less than or equal to the preset threshold At that time, the coarse position estimate of the previous moment is used as the initial state vector of EKF, and the fine position estimate of the current moment is obtained by prediction and correction through fine positioning / tracking of EKF. Use the coarse position estimate or the fine position estimate at the current moment as the target estimated position at the current moment.

[0012] In one embodiment of the present invention, geometric precision factor GDOP The expression is as follows: ; in, Represents the trace of a matrix. Let Jacobian matrix be the sum of the partial derivatives of the TDOA observation equations formed by each UAV and the main station. This represents the covariance matrix of the TDOA observation noise.

[0013] In one embodiment of the present invention, based on the estimated target position, a virtual center guiding force is synthesized and a rotational field correction is performed using an improved artificial potential field (I-APF) to generate a reference trajectory, including: The target's estimated location is used as the target's attraction point at the virtual center to construct the target's gravity; Construct a repulsive field based on the obstacle's location and radius of influence; The basic guiding force is obtained by synthesizing the target's gravitational and repulsive fields based on the relative distance adjustment factor. By superimposing the tangential force of the curl field on the basic guiding force, the corrected total resultant force is obtained; The corrected total resultant force is transformed into the reference velocity direction of the virtual center, and the reference trajectory of the virtual center is gradually generated through discrete integration.

[0014] In one embodiment of the present invention, the expression for the tangential force of the curl field is as follows: ; in, Represents the tangential force of the curl field. Indicates the curl field gain coefficient. This represents the square of the Euclidean distance between the drone and the obstacle. express of Rotation operator matrix, Indicates the first i A radial unit vector pointing from the drone to the center of the obstacle.

[0015] In one embodiment of the present invention, the expression for the dynamic constraint is as follows: ; in, Indicates the prediction time domain, Indicates the first The predicted state vector at time t. Indicates the first The ideal reference state vector at time t. Indicates the first The control input vector at time t, Indicates state tracking error. The weight matrix represents the control quantity. The constraint coefficients representing the geometric stiffness term. Represents the Laplace matrix, Represents the location matrix of the drone swarm. This represents the preset relative spacing vector of the target formation.

[0016] In one embodiment of the present invention, the expression for the second comprehensive cost function is as follows: ; in, express k The second comprehensive cost function at time t, This represents the trajectory tracking cost term. This represents the cost of maintaining the geometric formation. This represents the control smoothing cost term. This indicates a penalty for avoiding obstacles.

[0017] Secondly, this invention provides a collaborative positioning and obstacle avoidance control system for unmanned aerial vehicle (UAV) swarms, comprising: The formation confirmation layer is used to search and match in a pre-built offline policy library based on site error, time difference measurement error, and target distance, and uses the first comprehensive cost function to confirm the optimal formation configuration of the UAV cluster in the current mission phase. The state estimation layer is used to perform coarse-to-fine level positioning processing based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration of the current mission phase, and the target estimated position of the previous moment, so as to obtain the target estimated position at the current moment. The path planning layer is used to synthesize a virtual center guiding force and perform rotational field correction based on the improved artificial potential field I-APF to generate a reference trajectory. The collaborative execution layer is used to integrate dynamic constraints based on the optimal formation configuration and reference trajectory, and obtain the underlying control commands through rolling optimization calculation of the second comprehensive cost function. This enables MPC-based two-layer hierarchical control and formation rigidity maintenance to control the UAV swarm to complete obstacle avoidance; it also provides real-time feedback on the estimated target position at the current moment to achieve closed-loop feedback.

[0018] The beneficial effects of this invention are: The solution provided by this invention determines the optimal formation configuration of the UAV swarm for the current mission phase through a first comprehensive cost function. A change-of-formation command is only issued when the accuracy improvement brought by the configuration change significantly outweighs the flight maneuver cost, effectively filtering out the "ping-pong effect" caused by frequent changes. Through coarse-to-fine hierarchical positioning processing, the coarse-of-formation (FM) positioning solution, which requires no prior information and possesses strong robustness, solves the problem of TDOA observation equations in the far-field high-altitude region. GDOP The technical challenge of highly prone regional divergence is addressed by using a "hot start" mechanism to map the initial GN values ​​to the EKF in real time, avoiding the accuracy collapse caused by the accumulation of linearization errors in traditional filtering algorithms during the cold start phase. This invention overcomes the limitation of logical decoupling between "obstacle avoidance" and "positioning accuracy maintenance." Within the rolling optimization framework of model predictive control, it explicitly embeds rigid geometric constraints based on the Laplace matrix, enabling the swarm to replace disordered drift with elastic compression when performing evasive maneuvers (such as traversing confined spaces). Its core effect is to maximize the preservation of the optimal observation configuration and suppress [obstacles] while ensuring flight safety. GDOP To prevent deterioration and ensure the continued high accuracy of positioning tasks, this invention further breaks the mechanical equilibrium state of the traditional potential field method in symmetrical environments by introducing the tangential force of the curl field and the relative distance adjustment factor. This induces the UAV to achieve streamlined and smooth obstacle avoidance, completely eliminating local minimum deadlock and ensuring the reachability of the target in complex environments. Attached Figure Description

[0019] Figure 1 This is a schematic diagram illustrating the steps of a drone swarm cooperative positioning and obstacle avoidance control method provided in an embodiment of the present invention; Figure 2 This is a flowchart illustrating a method for collaborative localization and obstacle avoidance control of unmanned aerial vehicle (UAV) swarms provided in an embodiment of the present invention. Figure 3 This is a flowchart illustrating the coarse-to-fine hierarchical positioning processing in a UAV swarm cooperative positioning and obstacle avoidance control method provided in an embodiment of the present invention. Figure 4 This is a schematic diagram of the UAV formation configuration library in a UAV swarm cooperative positioning and obstacle avoidance control method provided in an embodiment of the present invention; Figure 5 The different time difference errors and site errors in the formation strategy diagram of the UAV swarm cooperative positioning and obstacle avoidance control method provided in the embodiments of the present invention; Figure 6 The static positioning result diagram and error curve diagram are provided in the UAV swarm cooperative positioning and obstacle avoidance control method provided in the embodiment of the present invention. Figure 7 This is a formation switching positioning accuracy diagram in a UAV swarm cooperative positioning and obstacle avoidance control method provided in an embodiment of the present invention; Figure 8 This is a formation cooperative traversal trajectory diagram in a multi-obstacle environment in a drone swarm cooperative positioning and obstacle avoidance control method provided in an embodiment of the present invention. Detailed Implementation

[0020] The present invention will be further described in detail below with reference to specific embodiments, but the implementation of the present invention is not limited thereto.

[0021] The embodiments of the present invention solve the following core technical problems by deeply coupling localization estimation, decision logic, and execution control: To address the issue of divergence in far-field acquisition and achieve stable target tracking throughout the entire process, this invention constructs a coarse-fine hierarchical localization architecture. It utilizes a robust initial solution algorithm to provide reliable initial values ​​for subsequent filtering, eliminating the linearization truncation error during the cold start phase and ensuring stable convergence of the system throughout the entire process from far-field approach to near-field tracking.

[0022] This invention addresses the mechanical deadlock problem during obstacle avoidance, ensuring the continuity of mission execution. By introducing a tangential curl force field and a relative distance adjustment factor, it breaks the mechanical equilibrium state of the traditional potential field method in symmetrical environments, inducing the UAV to achieve streamlined and smooth obstacle avoidance, completely eliminating local minimum deadlock, and ensuring target accessibility in complex environments.

[0023] This invention addresses the conflict between obstacle avoidance maneuvers and accuracy maintenance, achieving "obstacle avoidance without formation disintegration." By embedding rigid geometric constraints on formation at the control layer, the swarm can replace disordered drift with elastic compression when performing evasive maneuvers (such as traversing confined spaces). Its core effect is to maximize the preservation of the optimal observation configuration while ensuring flight safety and suppressing... GDOP To prevent deterioration and ensure the continued high accuracy of positioning tasks.

[0024] To address the issue of blind formation decision-making and optimize the mission energy efficiency of the swarm, this invention establishes an intelligent decision-making database based on a comprehensive cost function to quantitatively evaluate positioning accuracy gain and maneuver energy loss. By setting a hysteresis threshold judgment mechanism, invalid and frequent formation change commands are filtered out, thereby maximizing the operational radius and operating time of the UAV swarm while ensuring positioning performance.

[0025] To address the aforementioned problems, embodiments of the present invention provide a method for cooperative localization and obstacle avoidance control of unmanned aerial vehicle (UAV) swarms, such as... Figure 1 and Figure 2 As shown, it may include: S1, based on the site error, time difference measurement error, and target distance, performs a search and matching in a pre-built offline policy library, and uses the first comprehensive cost function to determine the optimal formation configuration of the UAV swarm for the current mission stage, which may include: By sensing distance and error, we can obtain the current station location error, time difference measurement error, and target distance. The target formation configuration is confirmed by searching a pre-built offline strategy library based on the station site error, time difference measurement error, and target distance. Using the first synthesis cost function The difference between the accuracy improvement gained from switching to the target configuration and the maneuver energy consumption required to perform the configuration change is used to make a judgment: if the difference is greater than a preset hysteresis threshold... When the target configuration is selected as the optimal formation configuration for the current mission phase, and the difference is less than or equal to the preset hysteresis threshold, the formation configuration is considered optimal. When the formation configuration of the previous moment is taken as the optimal formation configuration for the current mission phase.

[0026] The UAV swarm cooperative localization and obstacle avoidance control method provided in this invention is based on the construction of a closed-loop system integrating offline policy decision-making, hierarchical state estimation, and two-layer hierarchical control. By establishing a mathematical model of UAV swarm TDOA passive localization, the coupled impact of site error and time difference measurement error on positioning accuracy is deeply analyzed. Through full-parameter space traversal simulation, an offline policy library of "target distance-measurement error-optimal formation configuration" is established. Simultaneously, a first comprehensive cost function is defined. This function consists of a positioning accuracy gain term (based on...) GDOP The improvement level is weighted by the mobility energy loss term (based on the distance traveled by the UAV) and a preset hysteresis threshold is introduced. To suppress the "ping-pong effect" caused by frequent formation changes.

[0027] In task execution decision-making, the first comprehensive cost function is calculated in real time. To determine whether a change in formation is triggered, the first comprehensive cost function... The expression is as follows: ; in, Indicates the precision gain weight. This represents the theoretical root mean square error of the current formation. This represents the theoretical root mean square error after the formation change. Indicates the weight of motor energy efficiency loss. This represents the normalization factor, used to eliminate the influence of dimensions. This indicates the total number of drones in the drone swarm. Indicates the first i Ideal configuration location for a drone Indicates the first i The current position of each drone. The core of this decision-making logic lies in obtaining the theoretical root mean square error (RMSE) limit for each configuration at different distances through offline pre-calculation. Only when the accuracy improvement brought by switching to the target configuration significantly exceeds the maneuver energy consumption required to perform the configuration change, and the difference exceeds a preset hysteresis threshold, will the decision be made. Only when the decision-making layer issues a configuration switching command to the lower-level controller will the configuration switching command be executed. This mechanism effectively filters out frequent configuration changes in areas where accuracy improvement is not significant, and significantly reduces unnecessary energy consumption of the cluster.

[0028] The system compares the current positioning accuracy with the suggested configuration from the offline strategy library in real time. When the conditions are met... At that time, a formation reconfiguration command is triggered. The reconfiguration command is sent to the MPC controller, which drives the drone swarm to smoothly switch to the new configuration while maintaining continuous target lock, forming a dynamic closed loop of "perception-decision-execution".

[0029] The dynamic formation decision-making mechanism based on the game between accuracy gains and maneuver energy consumption proposed in this invention pre-constructs an offline optimal configuration strategy library with the root mean square error (RMSE) of positioning as the evaluation index, and applies a first comprehensive cost function including a hysteresis threshold during the online execution phase. The system can quantitatively assess the balance between the RMSE accuracy improvement gained from configuration switching and the maneuvering energy costs incurred by physical array changes, issuing reconfiguration commands only when the benefits are significant. This mechanism effectively suppresses frequent array changes (ping-pong effect) in areas of minimal accuracy improvement, greatly optimizing the mission energy efficiency and endurance of UAV swarms while ensuring optimal observation conditions.

[0030] S2, based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration for the current mission phase, and the target's estimated position from the previous moment, performs coarse-to-fine level positioning processing to obtain the target's estimated position at the current moment, such as... Figure 3 As shown, it may include: The system receives the current station coordinates of the UAV formation and the TDOA measurement data relative to the main station in real time, performs time alignment and gross error removal preprocessing, and obtains the relative geometric relationship between the UAV cluster and the target radiation source. Based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration for the current mission phase, and the target's estimated position from the previous moment, calculate the geometric accuracy factor for the current moment. GDOP ; Geometric precision factor GDOP With preset threshold Comparison: When geometric precision factor GDOP Greater than the preset threshold At that time, a rough position estimate for the current moment is obtained through FM coarse positioning solution; when the geometric precision factor... GDOP Less than or equal to the preset threshold At that time, the coarse position estimate of the previous moment is used as the initial state vector of EKF, and the fine position estimate of the current moment is obtained by prediction and correction through fine positioning / tracking of EKF. Use the coarse position estimate or the fine position estimate at the current moment as the target estimated position at the current moment.

[0031] Geometric Dilution of Precision GDOP () is an important indicator for measuring the layout of observation stations. It quantifies the amplification of errors by the geometric configuration of the formation and mainly evaluates the positioning performance of the selected UAV formation for radiation source targets.

[0032] ; in, Represents the trace of a matrix. The coordinates of the target radiation source, , , They are matrices The three elements on the main diagonal represent the theoretical minimum values ​​of the estimated variance of the radiation source target in the x-axis, y-axis, and z-axis directions, respectively. GDOP The smaller the value, the better the current formation configuration and the stronger the ability to suppress time difference errors and station location errors; GDOP The larger the value, the worse the configuration, and the smaller the measurement noise will be amplified by the geometric configuration.

[0033] In the passive localization scenario of UAV swarm TDOA proposed in this embodiment of the invention, GDOP This is used to measure the impact of the current formation configuration and the relative geometry of the target on the propagation of positioning errors. Let the master station be the first UAV, and the second... The TDOA observation equation formed by the UAV and the main station is as follows: ; in, For the target coordinates, For the first Taking the coordinates of the drone and applying a first-order linearization to it at the current estimated point, we get: ; in, H Let be the Jacobian matrix composed of the partial derivatives of each TDOA observation equation, and let its th be the Jacobian matrix. Behavior: ; Under the assumption of linear Gaussian error, the covariance of the target position estimation error can be approximately expressed as: ; in, R The TDOA observation noise covariance matrix and geometric precision factor are given. GDOP The expression is as follows: ; in, Represents the trace of a matrix. Let Jacobian matrix be the sum of the partial derivatives of the TDOA observation equations formed by each UAV and the main station. This represents the covariance matrix of the TDOA observation noise.

[0034] The smaller this index, the better the current formation observation geometry, and the weaker the amplification effect of time difference measurement error on target position estimation. Based on this, the embodiments of the present invention focus on far-field high... GDOP Under the given conditions, FM coarse positioning solution is used for fast coarse positioning. GDOP After descending to a preset threshold, it switches to EKF to achieve adaptive hierarchical positioning from coarse to fine.

[0035] During mission execution, based on the relative geometric relationship between the UAV swarm and the target radiation source, a tiered positioning solution is performed: First stage (far-field coarse positioning): When the system is in a cold start state or... GDOP When the target exceeds a preset threshold, a snapshot-style solution is performed using the Gauss-Newton (GN) iterative algorithm. Leveraging the algorithm's independence from prior historical information, it converges rapidly under ill-conditioned observation conditions, obtaining a preliminary estimate of the target's position. The second stage (fine-grained tracking) involves real-time monitoring of solution stability and a warm-start mechanism to map the GN algorithm's output to the initial value of the EKF. The EKF's filtering properties in the time domain are used to suppress noise, achieving smooth, high-precision, and continuous tracking of highly maneuverable targets.

[0036] This invention employs a coarse-fine hierarchical processing architecture to address the ill-conditioned characteristics of far-field observations. During the initial far-field acquisition phase of the mission, when the system is in a cold-start state or under geometric precision conditions... GDOP When extremely high values ​​cause the filter to easily diverge, the Gauss-Newton (GN) iterative algorithm is first invoked to perform a snapshot-style coarse localization. The iterative increment formula is as follows: ; in, The correction vector for the target position. Let Jacobian matrix be the observation equation with respect to the target state. To measure the noise covariance matrix, This represents the residual vector between the actual observation time difference and the theoretical observation value. Leveraging the GN algorithm's independence from historical prior information and its strong search capability across the entire space, the system can quickly lock the target into the neighborhood of the ground truth. Subsequently, through a warm-start mechanism, the coordinate solution converged by the GN algorithm is used as the initial state of the EKF. The specific flowchart is as follows: Figure 3 As shown, the embodiments of the present invention use time steps k For cyclical operation, the specific processing steps are as follows: First, data input and preprocessing are performed. The system receives the current station coordinates of the UAV formation and the TDOA measurement data relative to the master station in real time, and performs necessary time alignment and gross error removal preprocessing. Then, geometric condition evaluation is performed. Using the current formation configuration and the target estimated position at the previous moment, the current position is calculated. GDOP Compare this value with a preset threshold. When comparing, GDOP Greater than If the target is determined to be in the far field, the GN iteration branch is entered; otherwise, the target enters the mid-to-near field and the EKF branch is entered. In the far field... GDOP Under these conditions, the system employs the robust GN algorithm for nonlinear least squares solution, outputting a coarse position estimate. This result is directly used as the system output at the current moment and fed back to the data processing stage at the next moment, mainly to ensure the convergence of the system in low signal-to-noise ratio environments. When the system decides to enter the EKF branch, it first checks whether it is the first time entering. If it is the first time entering, an initialization operation is performed, and the estimated value output by the GN algorithm is used. Assigned to the EKF initial state vector This avoids transient oscillations or divergences in the EKF filter during cold starts due to large initial value deviations. After completing the initialization check, the EKF filter performs prediction and correction steps, using TDOA observations to correct the state and output a high-precision position estimate. Regardless of the time of output still The system has completed the current time. The solution is calculated and the result is fed back to the input as the next time step. The prior information is used to form a complete real-time closed-loop positioning system.

[0037] During the fine-grained tracking phase, the system utilizes the following state update equation to achieve smooth tracking: ; in, for k At each step, the posterior state estimate is fused with the current measurement. for k The time step is the prior state estimate obtained by extrapolating the state from the previous time step. For Kalman gain, For actual measurement vectors, This is the observation model. Experimental data shows that the hierarchical architecture improves the first-capture success rate by about 90% in a 20km far-field environment, solving the accuracy collapse problem caused by linearization truncation in traditional single-stage filtering when the initial error is large.

[0038] The embodiments of the present invention are based on GDOP The threshold-triggered "coarse-fine" hierarchical adaptive localization method addresses the strong nonlinearity of the TDOA observation equation in the far field by calculating [the method] in the early stages of the mission. GDOP Value discrimination task phase: in the far field high GDOP In this environment, the Gauss-Newton iterative algorithm is first used to perform a robust snapshot-style solution to capture the target; then, the core uses the convergent solution of GN as the initial state value for the extended Kalman filter for a "warm start" mapping. This hierarchical architecture effectively solves the divergence risk caused by linearization truncation error in the cold start phase of traditional filtering algorithms, ensuring the continuity and stability of target tracking throughout the entire task.

[0039] S3, based on the estimated target position, synthesize a virtual center guiding force and perform rotational field correction based on the improved artificial potential field (I-APF) to generate a reference trajectory, which may include: The target's estimated location is used as the target's attraction point at the virtual center to construct the target's gravity; Construct a repulsive field based on the obstacle's location and radius of influence; The basic guiding force is obtained by synthesizing the target's gravitational and repulsive fields based on the relative distance adjustment factor. By superimposing the tangential force of the curl field on the basic guiding force, the corrected total resultant force is obtained; The corrected total resultant force is transformed into the reference velocity direction of the virtual center, and the reference trajectory of the virtual center is gradually generated through discrete integration.

[0040] Specifically, the GN-EKF module outputs the estimated target position based on the current stage; simultaneously, the environmental perception module identifies obstacles and establishes an obstacle model. Then, at the path planning layer, the estimated target position is used as the attraction target for the virtual center, constructing a target gravity; next, a repulsive field is constructed based on the obstacle position and influence radius, and the basic guiding force is synthesized by combining a relative distance adjustment factor. Considering that traditional APF is prone to local minima and oscillations in front of obstacles, a curl field tangential force is further superimposed on the basic guiding force, so that the virtual center no longer directly collides with the obstacle when approaching it, but instead smoothly circles around the obstacle boundary. Finally, the corrected total resultant force is converted into the reference velocity direction of the virtual center, and a virtual center reference trajectory is gradually generated through discrete integration. This trajectory then serves as the reference feedforward input for subsequent MPC, driving the formation to complete obstacle avoidance and continuous approach under the conditions of satisfying dynamics and formation maintenance constraints.

[0041] In the upper-level path planning, to address obstacle avoidance requirements in complex environments, this embodiment of the invention improves the artificial potential field method with a non-conservative force field: a relative distance adjustment factor is introduced into the repulsive force function, so that when the UAV approaches the target point, even if the obstacle is in the target's neighborhood, the repulsive force can smoothly decay to zero, completely solving the target unreachability problem in the traditional potential field method; a tangential curl force field is introduced, and when the attraction and repulsion forces are detected to cancel each other out radially collinearly, a tangential induced force perpendicular to the radial vector is applied, thereby breaking the mechanical equilibrium, guiding the UAV to achieve streamlined obstacle avoidance, and eliminating local minimum deadlock. The expression for the curl field tangential force corresponding to the tangential curl force field is as follows: ; in, Represents the tangential force of the curl field. Indicates the curl field gain coefficient. This represents the square of the Euclidean distance between the drone and the obstacle. Indicates the first i A unit radial vector pointing from the drone to the center of the obstacle. express of Rotation operator matrix. This operator induces lateral slippage in the UAV by injecting tangential force into the potential field, enabling streamlined obstacle avoidance and thus eliminating local minima dwelling.

[0042] The improved artificial potential field obstacle avoidance algorithm proposed in this invention, which incorporates a curl field and a distance adjustment factor, addresses the mechanical equilibrium deadlock problem of traditional potential field methods in symmetrical environments. It injects a tangential curl force component into the potential field function and applies an induced force at the point where the radial net force is zero using a rotation operator matrix, forcibly breaking the equilibrium state to achieve streamlined, smooth obstacle avoidance. Simultaneously, by dynamically correcting the repulsive gradient of the target neighborhood using a relative distance adjustment factor, it ensures the reachability of the target point as a globally unique potential energy minimum, completely solving the steady-state error problem of "target unreachability" when obstacles approach the target.

[0043] S4 integrates dynamic constraints based on the optimal formation configuration and reference trajectory, and obtains the underlying control commands through rolling optimization calculation of the second comprehensive cost function. This enables MPC-based two-layer hierarchical control and formation rigidity maintenance, thereby controlling the UAV swarm to complete obstacle avoidance.

[0044] The underlying execution layer employs a Model Predictive Control (MPC) framework, using the path generated by the I-APF as the reference trajectory. In the rolling optimization cost function of MPC, in addition to the trajectory tracking error term and the dynamic constraint term, a formation geometry rigidity constraint term is explicitly embedded. This constraint term enforces the maintenance of the relative positional proportions among formation members, allowing the cluster to perform overall elastic compression when passing through confined spaces such as narrow gates, rather than disordered individual drift.

[0045] This invention employs a model predictive control framework with integrated geometric rigid constraints, using the improved potential field generation path as a reference trajectory, and explicitly embedding a Laplace matrix-based approach into the rolling optimization objective function of the MPC. The formation maintenance term, and the dynamic constraint expression, are as follows: ; in, Indicates the prediction time domain, Indicates the first The predicted state vector at time t. Indicates the first The ideal reference state vector at time t. Indicates the first The control input vector at time t, Indicates state tracking error. The weight matrix represents the control quantity. The constraint coefficients representing the geometric stiffness term. Represents the Laplace matrix, Represents the location matrix of the drone swarm. This represents the preset relative spacing vector of the target formation. (Laplace matrix) The topological connections between formation nodes are defined. This constraint penalizes deformation of the relative topology among formation members, forcing the maintenance of a preset geometric proportion. When traversing confined spaces such as "narrow gates" or dense obstacle zones, the swarm can pass through with overall elastic compression. That is, while ensuring obstacle avoidance safety, it maintains the geometric features required for TDOA observation by scaling proportionally, rather than generating disordered individual drift. This mechanism ensures that the condition number of the localization observation matrix remains stable during complex obstacle avoidance maneuvers, avoiding tracking failures caused by formation collapse.

[0046] Understandably, the essence of model predictive control is to achieve multi-objective coordination by solving for the optimal control sequence in a finite time domain, under the condition of satisfying system dynamics and physical constraints. In order to balance the mission execution, positioning performance and flight safety of UAV formations during dynamic obstacle avoidance, this embodiment of the invention constructs performance indicators for four dimensions: trajectory tracking, formation geometric stiffness, control smoothness and obstacle avoidance penalty, and finally synthesizes the second comprehensive cost function of the system.

[0047] To address the requirement for drone swarms to follow the planned paths from higher layers, a trajectory tracking cost term is constructed. This aims to guide the formation to conform to a collision-free reference trajectory generated by the I-APF in macroscopic motion. Let... For the first A drone in Always looking towards the future Predicted position of the step The ideal array position, calculated based on the virtual structure principle and the virtual center state, is then defined as the tracking cost in the prediction time domain. Weighted accumulation of internal position tracking error: ; in, It is a positive definite weight matrix used to adjust the proportion of tracking accuracy in the total cost.

[0048] To ensure high-precision positioning throughout the entire process, it is necessary to maintain a low altitude during flight. GDOP The configuration, because the positioning accuracy of TDOA is highly dependent on the stability of the geometric topology, therefore... GDOP The indicators are transformed into computable control constraints, and the formation geometry preservation cost term is constructed. When a UAV encounters an obstacle and is forced to maneuver, the lack of configuration constraints can easily lead to formation disintegration, which in turn deteriorates the condition number of the observation matrix. Therefore, this invention constructs a geometrically rigid penalty term based on relative position error: Let the optimal normalized shape matrix recommended by the policy library at the current target distance be... The baseline length is Then the first frame and the first The expected relative position vector between the drones is Therefore, the formation cost is defined as follows: ; in, This is the formation topology weighting coefficient, used to adjust the importance of different communication links within the formation. By minimizing this cost term, the optimizer forces all UAVs to maintain stable relative positions during dynamic motion, which is mathematically equivalent to controlling the formation as a rigid body, avoiding the problems caused by loose formation. GDOP deterioration.

[0049] Considering the physical limitations and energy efficiency of UAV maneuverability, a control smoothing cost term is constructed. This term applies to the control input in the prediction time domain. and its rate of change Penalties are imposed to prevent actuator saturation and violent fluctuations in control commands, ensuring flight stability. ; in, To control the input weight matrix, Control the incremental weight matrix.

[0050] Although the upper-level planning layer provides a collision-free reference path, an obstacle avoidance penalty term is introduced in the MPC solution layer to address model mismatch and the uncertainty of the dynamic environment. As a second line of defense, a soft constraint function in the form of a potential field is used. When the predicted position of the UAV approaches the safety boundary of the obstacle, the cost increases exponentially, forcing the optimization solution to deviate from the danger zone. ; in, The obstacle avoidance penalty weighting coefficient, For the first The center coordinates of each obstacle For the first The physical radius of the obstacle For safety margin, This is a regularization parameter that ensures the numerical stability of the optimization solution.

[0051] Based on the above four performance indicators, the final structure is obtained. The second comprehensive cost function at time 1 The expression for the second comprehensive cost function is as follows: ; in, express k The second comprehensive cost function at time t, This represents the trajectory tracking cost term. This represents the cost of maintaining the geometric formation. This represents the control smoothing cost term. This indicates a penalty for avoiding obstacles.

[0052] Based on this, the cooperative obstacle avoidance control problem of UAV formations can be transformed into the following online constraint optimization problem: ; in, The control input sequence to be optimized is the objective of the optimization problem, which is contained in the prediction time domain. The future control volume of all drones, i.e. ; This is the system's current state vector, containing the states of all UAVs at that moment. The position and velocity information at any given moment are the initial conditions for predicting the future. The trajectory tracking cost measures the deviation between the actual position of the UAV and the reference trajectory generated by the I-APF. The cost of maintaining formation is measured by the deviation between the relative positions of the drones and the desired geometric configuration, i.e., the rigid constraint term; To control the cost of smoothing, penalties are imposed on the magnitude and rate of change of the control input to prevent excessive maneuvering and ensure smooth flight. The obstacle avoidance potential field cost is a soft constraint term. This value increases sharply as the drone approaches an obstacle, guiding the optimizer to actively avoid obstacles. The constraint parameters include... It consists of three parts: a state prediction model, physical constraints, and safety hard constraints. In the state prediction model, The state transition matrix describes the natural evolution of the system in the absence of input. The control input matrix describes how the control input affects the system state; this constraint ensures that the predicted trajectory conforms to the laws of physical kinematics, such as Newton's second law. In physical constraints, For predicting control variables, it means that in The first time predicted Step control input; and Let be the physical limits of the actuator, representing the minimum and maximum speeds or accelerations that the UAV can achieve, respectively. In the context of hard safety constraints, For the first The drone in the future Predicted position of the step; For the first The center coordinates of the obstacles.

[0053] By solving this optimization problem at each sampling time, the system can obtain the optimal control sequence that balances task execution, positioning accuracy, and flight safety, and apply the first term of the sequence to the UAV swarm to achieve closed-loop control.

[0054] In a practical UAV system, the desired three-axis acceleration output by each UAV in this embodiment of the invention is essentially a high-level control command given by the collaborative control layer and cannot be directly applied to the motors. In engineering implementation, the flight control system first converts the desired acceleration into attitude and thrust commands. The planar acceleration is usually mapped to the desired roll and pitch angles, the vertical acceleration is mapped to the total thrust or throttle command, and the heading requirement corresponds to the yaw angle or yaw rate. Subsequently, these attitude / thrust commands enter the lower-level flight control closed loop, where the attitude controller and angular velocity controller further solve for the required total thrust and torque of each axis. Finally, the control distribution module decomposes the total thrust and roll, pitch, and yaw torques into the speed commands of each rotor motor, which are then executed by the ESC-driven motors, thereby realizing the layer-by-layer transmission and physical implementation of "desired acceleration - attitude / thrust - torque / total thrust - motor speed".

[0055] In this system, the state estimation layer first obtains the target's estimated coordinates through GN-EKF; the path planning layer uses this as a guide to generate a virtual center reference trajectory in conjunction with an obstacle model. Subsequently, the cooperative execution layer uses the current cluster state as an initial value and the virtual center reference trajectory and desired formation configuration as tracking targets to establish a constrained MPC optimization problem. By solving this optimization problem, the optimal control input sequence for all UAVs in the future prediction time domain can be obtained. Considering that the system adopts a rolling time-domain control mechanism, only the first term of this sequence is extracted during actual execution, and it is broken down into the current control commands for each UAV and issued for execution. The new states generated after the UAVs execute are then fed back to the next sampling period, forming a closed-loop control.

[0056] The Model Predictive Control (MPC) cooperative maintenance technique with integrated geometric rigid constraints proposed in this invention explicitly embeds a Laplace matrix-based formation maintenance constraint term within the MPC rolling optimization framework. This constraint term forces the maintenance of a preset geometric proportion by penalizing the deformation of the relative topology among formation members. This enables the swarm to replace disordered individual drift with an overall elastic compression mode when traversing confined spaces such as narrow gates, ensuring the stability of the condition number of the positioning observation matrix during complex obstacle avoidance maneuvers, and fundamentally suppressing formation collapse. GDOP The deterioration ensured the accuracy of the positioning task during obstacle avoidance.

[0057] S5 provides real-time feedback on the estimated target position at the current moment to achieve closed-loop feedback.

[0058] The vertical closed-loop system architecture proposed in this invention, encompassing state estimation, task decision-making, and collaborative execution, uses the target estimated position output by the positioning estimation layer as the core link to drive the path planning layer in real time to synthesize a virtual center guiding force and constrain the instruction generation of the underlying MPC controller. Simultaneously, an offline policy library is introduced for global decision guidance. This hierarchical closed-loop design achieves high positioning performance metrics (RMSE / GDOP The deep coupling between the UAV swarm and the underlying physical flight control commands ensures that the UAV swarm can balance flight safety, configuration optimization and high-precision continuous locking performance against radiation source targets in complex and dynamic environments.

[0059] The effectiveness of the embodiments of this invention has been fully verified in full-process simulation experiments. In a continuous task flow including approach, obstacle avoidance, and stable tracking, the system successfully achieved reliable target locking in a far-field environment and completed dynamic tracking with meter-level accuracy. Especially in obstacle crossing scenarios, the embodiments of this invention improve the positioning accuracy maintenance capability by more than 2 times compared to traditional methods without rigid constraints. Finally, the system constructs a complete dynamic closed loop from offline policy indexing and hierarchical state estimation to cooperative execution control, providing robust technical support for passive reconnaissance missions of UAV swarms in complex electromagnetic environments.

[0060] The UAV swarm cooperative localization and obstacle avoidance control method provided in this invention determines the optimal formation configuration of the UAV swarm in the current mission phase through a first comprehensive cost function. It only issues a formation change command when the accuracy improvement brought by the configuration change significantly outweighs the flight maneuver cost, effectively filtering out the "ping-pong effect" caused by frequent formation changes. Through coarse-to-fine hierarchical localization processing, it utilizes the characteristics of FM coarse localization solution—which requires no prior information and has strong robustness—to solve the problem of TDOA observation equations in the far-field high-altitude region. GDOP The technical challenge of highly prone regional divergence is addressed by using a "hot start" mechanism to map the initial GN values ​​to the EKF in real time, avoiding the accuracy collapse caused by the accumulation of linearization errors in traditional filtering algorithms during the cold start phase. This invention overcomes the limitation of logical decoupling between "obstacle avoidance" and "positioning accuracy maintenance." Within the rolling optimization framework of model predictive control, it explicitly embeds rigid geometric constraints based on the Laplace matrix, enabling the swarm to replace disordered drift with elastic compression when performing evasive maneuvers (such as traversing confined spaces). Its core effect is to maximize the preservation of the optimal observation configuration and suppress [obstacles] while ensuring flight safety. GDOP To prevent deterioration and ensure the continued high accuracy of positioning tasks, this invention further breaks the mechanical equilibrium state of the traditional potential field method in symmetrical environments by introducing the tangential force of the curl field and the relative distance adjustment factor. This induces the UAV to achieve streamlined and smooth obstacle avoidance, completely eliminating local minimum deadlock and ensuring the reachability of the target in complex environments.

[0061] Secondly, corresponding to the above method embodiments, this invention also provides a drone swarm cooperative positioning and obstacle avoidance control system, which may include: The formation confirmation layer is used to search and match in a pre-built offline policy library based on site error, time difference measurement error, and target distance, and uses the first comprehensive cost function to confirm the optimal formation configuration of the UAV cluster in the current mission phase. The state estimation layer is used to perform coarse-to-fine level positioning processing based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration of the current mission phase, and the target estimated position of the previous moment, so as to obtain the target estimated position at the current moment. The path planning layer is used to synthesize a virtual center guiding force and perform rotational field correction based on the improved artificial potential field I-APF to generate a reference trajectory. The collaborative execution layer is used to integrate dynamic constraints based on the optimal formation configuration and reference trajectory, and obtain the underlying control commands through rolling optimization calculation of the second comprehensive cost function. This enables MPC-based two-layer hierarchical control and formation rigidity maintenance to control the UAV swarm to complete obstacle avoidance; it also provides real-time feedback on the estimated target position at the current moment to achieve closed-loop feedback.

[0062] The UAV swarm cooperative localization and obstacle avoidance control system provided in this invention constructs a complete feedback loop from external environment observation to UAV control command generation through the vertical integration of the decision and estimation layer and a two-layer hierarchical control framework. The system first outputs the estimated target position through the observation modeling and error perception module and the state estimation layer, while simultaneously establishing the optimal formation configuration for the current task stage by the offline decision layer. These two core data points converge as key inputs into the control framework: the path planning layer (I-APF) synthesizes a virtual center guiding force based on the estimated coordinates and performs curl field correction to generate a reference trajectory; the cooperative execution layer (MPC), after integrating dynamic constraints, calculates the final underlying control commands through rolling optimization. This hierarchical closed-loop architecture ensures that the swarm can achieve continuous target locking and safe avoidance with optimal geometric layout in complex dynamic environments.

[0063] The following specific simulation experiments demonstrate the beneficial effects of the UAV swarm cooperative positioning and obstacle avoidance control method provided in this embodiment of the invention.

[0064] Simulation experiment: This invention embodiment uses a swarm of four UAVs to conduct a reconnaissance mission against a non-cooperative radiation source target 20km away. The embodiment employs four rotary-wing UAVs, with UAV 1 as the primary station and UAVs 2-4 as secondary stations. Each UAV is equipped with an ultra-shortwave intercept receiver and a clock synchronization module, and maintains swarm communication via a wireless broadband data link. The mission area is pre-defined to contain multiple stationary obstacles (such as mountains or no-fly zones). The hardware platform for the simulation experiment is an Intel(R) Core(TM) Ultra 5 125H processor with 32GB of memory. The software platform for the simulation experiment is Windows 10 operating system and MATLAB R2023b. The configuration of the four UAVs is as follows... Figure 4 As shown, the observation error is the time difference error. UAV site error The initial distance between the radiation source and the drone swarm is .

[0065] The system retrieves a pre-generated offline policy library from the onboard storage unit, which records different timing errors (such as...). Under these conditions, RMSE varies with distance. R The changing configuration index table, different time difference errors and station location errors in the formation strategy diagram, such as... Figure 5 As shown, when the cluster is approximately 20 km from the target, the geometric conditions deteriorate drastically as the hyperboloid observed by TDOA tends to become parallel. At this point, the main station aggregates the raw TDOA data transmitted from each secondary station and calculates the current configuration in real time. GDOP Value. Determine the current value. GDOP If the value exceeds a preset threshold, the system automatically enters the GN iteration branch. At this point, the algorithm uses an iterative increment formula for snapshot-style calculation, without requiring the previous position as prior information. After 5-10 iterations, it outputs the target position correction vector. The estimated target position is output at this time. Although there are some pulse-like jumps, it ensures that the system does not diverge during the cold start phase and successfully obtains the initial orientation of the target.

[0066] The static positioning result diagram and error curve diagram provided in the embodiments of the present invention are as follows: Figure 6 As shown, Figure 6 The left side of the image shows a scatter plot of the 3D positioning results. It's clear that while the red dots (GN algorithm results) are distributed around the real target (green pentagram), they are highly dispersed, forming a loose error cloud. This indicates that the GN algorithm, as a single-snapshot solution method, is quite sensitive to random noise from a single measurement, resulting in significant random fluctuations in the positioning results. In contrast, the EKF algorithm's superior noise suppression capability is clearly demonstrated. Figure 6The right side of the graph shows the error convergence curves of the two algorithms, further quantifying their time-domain characteristics. The blue line (GN-EKF) did not exhibit the common cold-start oscillations seen in traditional filters at the initial stage; its initial error directly inherited the accuracy level of GN (approximately 300m), achieving a seamless transition. As the number of observation steps increased, the error of the red line (GN) fluctuated wildly around the mean (dashed line, approximately 370m) without any convergence trend. In contrast, the GN-EKF algorithm recursively corrected the target position using a state equation, resulting in a significant decrease in its error curve. Statistical analysis of the steady-state data from the last 50 observations showed that the RMSE of the standard GN algorithm was 359.82m, while the RMSE of the GN-EKF algorithm was only 37.76m. By introducing joint time-domain filtering, the positioning accuracy was improved by 89.5%.

[0067] As the drone swarm approached the target to within 15km, the decision-making body... Real-time evaluation of the current configuration (e.g., linear). Compared to the optimal configuration recommended by the strategy library (such as the inverted V shape) A comparison is made. This is achieved by calculating the first comprehensive cost function. When the accuracy gain brought about by switching configurations is significantly improved at this time, it satisfies the requirement. The system issues a configuration switching command. At this time, the four drones, based on the calculated desired coordinates... The system can be reconfigured to smoothly switch to an inverted V-shaped configuration with better positioning performance while maintaining continuous observation of the target.

[0068] To verify the effectiveness of the above cost function in a dynamic process, the simulation conditions were set to time difference error. Site error The system assigns precision weights Weighting of energy consumption This indicates a greater pursuit of system positioning accuracy, and the normalization factor... Switch threshold , conducted Second from the far field A full-process flight simulation of approaching the target radiation source, taking the average experimental values, and a formation switching positioning accuracy map, as shown below. Figure 7 As shown.

[0069] In the formation switching positioning accuracy map, the image above shows a comparison of the root mean square error (RMSE) between the dynamic switching strategy (red implementation) and the fixed linear formation strategy (gray dashed line). In the far-field phase... The two curves largely overlap, indicating that within this distance range, a straight-line formation is indeed the optimal solution under the current conditions, and the decision-making system maintains the current configuration to save energy. (Mid-game phase) As the target distance decreases, the error of the linear formation shows a divergent trend and fluctuates significantly, making the dynamic strategy more effective. Near-field adjustments were made to suppress the upward trend of error and maintain RMSE at a low level. (In the near-field phase) The straight-line formation became largely ineffective, and the error increased sharply, while the red line experienced a sharp drop due to switching to an inverted V-shaped configuration that is more suitable for the near field.

[0070] When the drone swarm approaches to within an 8km radius, the geometric conditions improve, and the system... GDOP When the value falls below a set threshold, a warm-start mechanism is triggered. The system uses the optimal solution of the GN algorithm at the current moment as the initial state value and covariance of the Extended Kalman Filter (EKF). The EKF filter initiates recursive estimation using the expression for the tangential force of the curl field. Through state prediction and observation correction, a highly smooth target trajectory with meter-level accuracy is output.

[0071] During the approach, when the drone swarm encounters collinearly distributed obstacles or no-fly zones, the improved artificial potential field (I-APF) module senses the repulsive force of the obstacles and simultaneously calculates the tangential curl force. This force induces lateral velocity in the cluster, preventing it from stalling directly in front of obstacles. The MPC controller, when solving the optimization problem of the dynamic constraints, is limited by geometric rigidity constraints. When the UAV displaces to avoid obstacles, Constraints enforced maintaining the relative geometric proportions among the four drones. The swarm exhibited a proportionally compressed overall structure as it passed through the obstacle course, rather than scattering randomly. Experimental observations revealed that during obstacle avoidance, localization... GDOP The fluctuation range was suppressed to within 15%, effectively ensuring the continuity of the tracking mission during obstacle avoidance.

[0072] To verify the synergistic performance of the control method proposed in this invention between rigid maintenance and flexible obstacle avoidance, a scenario was designed to simulate a formation continuously traversing different types of obstacles during long-distance flight. The focus was on examining how the formation maintains its geometric structure through baseline compression or overall translation when facing obstacles. The simulation scenario involved four UAVs forming an inverted V-shaped formation, starting from a starting point... Fly to the destination The obstacle avoidance scenario involves narrow gates and offset obstacles. The narrow gates are located at... At this point, two symmetrical obstacles force the formation to compress laterally; the offset obstacle is located at... The center is off-center. This forces the formation to perform a combination of longitudinal climb and compression maneuvers. Figure 8 The diagram shows the collaborative trajectories of drone formations navigating multiple obstacles under MPC control.

[0073] Depend on Figure 8Therefore, based on the principle of virtual structures, two typical cooperative maneuver modes can be seen from the diagram: Elastic compression: in In the narrow passageway, facing a gate obstacle, the MPC controller did not scatter the drones to avoid it. Instead, it adjusted the baseline length of the virtual structure based on the policy library. The four drones synchronously contracted towards the virtual center. Although the formation size decreased by 70%, it still strictly maintained an inverted V-shaped topology, effectively avoiding problems caused by formation chaos. GDOP Accuracy deteriorates.

[0074] Rigid translation: in Within the designated area, facing an obstacle offset to the right, the formation executed a challenging S-shaped climb maneuver. The virtual center trajectory shifted, causing the entire rigid structure to move upwards into a safe zone, demonstrating the algorithm's strong constraint on the overall stability of the formation.

[0075] The simulation experiments above demonstrate that the MPC-based cooperative control framework proposed in this invention effectively combines the MPC prediction model with a comprehensive multi-objective cost function. The algorithm successfully maintains formation under strong obstacle avoidance constraints, and the UAV formation exhibits a balance of rigidity and flexibility: maintaining high rigidity in the safe zone to optimize... GDOP In obstacle areas, controlled elastic deformation is used to gain survival space, verifying the correctness of the theoretical model and the robustness of the control strategy.

[0076] The beneficial effects of the embodiments of the present invention are mainly reflected in the following four aspects, covering positioning stability, obstacle avoidance logic, cooperative control coupling, and system energy efficiency ratio: Significantly improved the acquisition reliability and tracking accuracy of passive positioning systems in complex far-field environments: This invention, through the construction of a coarse-fine hierarchical positioning architecture, utilizes the Gauss-Newton iterative algorithm, which requires no prior information and possesses strong robustness, to solve the problem of TDOA observation equations in the far-field high-resolution model. GDOP The technical challenge of regional divergence is addressed. A "warm start" mechanism maps the initial value of the GN to the EKF in real time, avoiding the accuracy collapse caused by the accumulation of linearization errors in the cold start phase of traditional filtering algorithms. Simulation data shows that this scheme improves the first acquisition success rate in far-field environments by approximately 90% and achieves smooth target tracking with meter-level accuracy across the entire range.

[0077] Completely eliminates mechanical deadlock and target unreachability traps during obstacle avoidance maneuvers: To address the problem of traditional artificial potential field methods easily getting trapped in local minima in symmetrical environments, this invention introduces a tangential curl force field and uses a rotation operator to apply induced components at the radial force equilibrium point, forcibly breaking the mechanical equilibrium and driving the UAV to achieve streamlined, smooth obstacle avoidance. Simultaneously, by combining a relative distance adjustment factor to correct the repulsive force function, the reachability of the target point as a globally unique potential energy minimum is ensured, greatly enhancing the survivability and mission continuity of UAV swarms in complex dynamic obstacle environments.

[0078] It achieves deep collaborative protection of obstacle avoidance maneuverability and positioning geometry performance: This invention overcomes the limitation of logical decoupling between "obstacle avoidance" and "positioning accuracy maintenance." Within the rolling optimization framework of model predictive control, it explicitly embeds rigid geometric constraints based on the Laplace matrix for formation. This mechanism allows the cluster to maintain its geometric proportions strictly while reducing its formation size through an "elastic compression" mode when passing through confined spaces such as narrow gates, thereby ensuring the stability of the observation matrix condition number and suppressing [the influence of [other factors]]. GDOP The deterioration of the situation. Experimental verification shows that during obstacle avoidance maneuvers, the embodiment of the present invention improves the ability to maintain positioning accuracy by more than 2 times compared with traditional algorithms, achieving "obstacle avoidance without disarray and no loss of accuracy".

[0079] The cluster's task energy efficiency was optimized and unnecessary maneuver losses were suppressed. By constructing a comprehensive cost function decision system integrating accuracy gains, maneuver energy consumption, and hysteresis thresholds, this invention achieves intelligent and dynamic adjustment of formation configuration. The system only issues change-of-formation commands when the accuracy improvement benefits from configuration changes significantly outweigh the flight maneuver costs, effectively filtering out the "ping-pong effect" caused by frequent changes. Simulation analysis confirms that this decision mechanism can reduce ineffective change-of-formation actions by approximately 60%, maximizing the operational endurance and combat radius of the UAV swarm while ensuring optimal observation conditions.

[0080] It should be noted that, in the description of this invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0081] The various embodiments in this specification are described in a related manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions of the method embodiments.

[0082] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A method for cooperative localization and obstacle avoidance control of unmanned aerial vehicle (UAV) swarms, characterized in that, include: Based on the site error, time difference measurement error, and target distance, a search and matching process is performed in a pre-built offline strategy library, and the optimal formation configuration of the UAV cluster for the current mission stage is determined using the first comprehensive cost function. Based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration at the current mission stage, and the target estimated position at the previous moment, coarse-to-fine level positioning processing is performed to obtain the target estimated position at the current moment. Based on the target's estimated position, a virtual center guiding force is synthesized and a rotational field correction is applied using the improved artificial potential field (I-APF) to generate a reference trajectory. Based on the optimal formation configuration and integrated dynamic constraints of the reference trajectory, the underlying control commands are obtained through rolling optimization calculation of the second comprehensive cost function, realizing MPC-based two-layer hierarchical control and formation rigidity maintenance, so as to control the UAV swarm to complete obstacle avoidance; The target's estimated position is fed back in real time to achieve closed-loop feedback.

2. The method for cooperative localization and obstacle avoidance control of a drone swarm according to claim 1, characterized in that, The process of searching and matching in a pre-built offline strategy library based on site error, time difference measurement error, and target distance, and using a first comprehensive cost function to determine the optimal formation configuration of the UAV swarm for the current mission phase, includes: By sensing distance and error, we can obtain the current station location error, time difference measurement error, and target distance. The target formation configuration is confirmed by searching a pre-built offline strategy library based on the station site error, time difference measurement error, and target distance. Using the first synthesis cost function The difference between the accuracy improvement gained from switching to the target configuration and the maneuver energy consumption required to perform the configuration change is used to make a judgment: if the difference is greater than a preset hysteresis threshold... When the target configuration is selected as the optimal formation configuration for the current mission phase, and the difference is less than or equal to the preset hysteresis threshold, the formation configuration is considered optimal. When the formation configuration of the previous moment is taken as the optimal formation configuration for the current mission phase.

3. The method for cooperative positioning and obstacle avoidance control of a drone swarm according to claim 1, characterized in that, The first comprehensive cost function The expression is as follows: ； in, Indicates the precision gain weight. This represents the theoretical root mean square error of the current formation. This represents the theoretical root mean square error after the formation change. Indicates the weight of motor energy efficiency loss. Represents the normalization factor. This indicates the total number of drones in the drone swarm. Indicates the first i Ideal configuration location for a drone Indicates the first i The current location of the drone.

4. The method for cooperative localization and obstacle avoidance control of a drone swarm according to claim 1, characterized in that, The step of performing coarse-to-fine level positioning processing based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration at the current mission stage, and the target estimated position at the previous moment, to obtain the target estimated position at the current moment, includes: The system receives the current station coordinates of the UAV formation and the TDOA measurement data relative to the main station in real time, performs time alignment and gross error removal preprocessing, and obtains the relative geometric relationship between the UAV cluster and the target radiation source. Based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration for the current mission phase, and the estimated target position from the previous moment, calculate the geometric accuracy factor for the current moment. GDOP ; Geometric precision factor GDOP With preset threshold Comparison: When geometric precision factor GDOP Greater than the preset threshold At that time, a rough position estimate for the current moment is obtained through FM coarse positioning solution; when the geometric precision factor... GDOP Less than or equal to the preset threshold At that time, the coarse position estimate of the previous moment is used as the initial state vector of EKF. Prediction and correction are performed through EKF fine positioning / tracking to obtain the fine position estimate of the current moment. Use the coarse position estimate or the fine position estimate at the current moment as the target estimated position at the current moment.

5. The method for cooperative localization and obstacle avoidance control of a drone swarm according to claim 4, characterized in that, The geometric precision factor GDOP The expression is as follows: ； in, Represents the trace of a matrix. Let Jacobian matrix be the sum of the partial derivatives of the TDOA observation equations formed by each UAV and the main station. This represents the covariance matrix of the TDOA observation noise.

6. The method for cooperative localization and obstacle avoidance control of a drone swarm according to claim 1, characterized in that, The process of estimating the target position, synthesizing a virtual center guiding force based on the improved artificial potential field (I-APF), and performing rotational field correction to generate a reference trajectory includes: The target's estimated location is used as the target's attraction point at the virtual center to construct the target's gravity; Construct a repulsive field based on the obstacle's location and radius of influence; The basic guiding force is obtained by synthesizing the target's gravitational and repulsive fields based on the relative distance adjustment factor; By superimposing the tangential force of the curl field on the basic guiding force, the corrected total resultant force is obtained; The corrected total resultant force is transformed into the reference velocity direction of the virtual center, and the reference trajectory of the virtual center is gradually generated through discrete integration.

7. The method for cooperative localization and obstacle avoidance control of a drone swarm according to claim 6, characterized in that, The expression for the tangential force of the curl field is as follows: ； in, Represents the tangential force of the curl field. Indicates the curl field gain coefficient. This represents the square of the Euclidean distance between the drone and the obstacle. express of Rotation operator matrix, Indicates the first i A radial unit vector pointing from the drone to the center of the obstacle.

8. The method for cooperative localization and obstacle avoidance control of a drone swarm according to claim 1, characterized in that, The expression for the dynamic constraint is as follows: ； in, Indicates the prediction time domain, Indicates the first The predicted state vector at time t. Indicates the first The ideal reference state vector at time t. Indicates the first The control input vector at time t, Indicates state tracking error. The weight matrix represents the control quantity. The constraint coefficients representing the geometric stiffness term. Represents the Laplace matrix, Represents the location matrix of the drone swarm. This represents the preset relative spacing vector of the target formation.

9. The method for cooperative localization and obstacle avoidance control of a drone swarm according to claim 1, characterized in that, The expression for the second comprehensive cost function is as follows: ； in, express k The second comprehensive cost function at time t, This represents the trajectory tracking cost term. This represents the cost of maintaining the geometric formation. This represents the control smoothing cost term. This indicates a penalty for avoiding obstacles.

10. A collaborative positioning and obstacle avoidance control system for unmanned aerial vehicle (UAV) swarms, characterized in that, include: The formation confirmation layer is used to search and match in a pre-built offline policy library based on site error, time difference measurement error, and target distance, and uses the first comprehensive cost function to confirm the optimal formation configuration of the UAV cluster in the current mission phase. The state estimation layer is used to perform coarse-to-fine level positioning processing based on the relative geometric relationship between the UAV swarm and the target radiation source, the optimal formation configuration of the current mission phase, and the target estimated position of the previous moment, so as to obtain the target estimated position at the current moment. The path planning layer is used to synthesize a virtual center guiding force and perform rotational field correction based on the improved artificial potential field I-APF to generate a reference trajectory. The collaborative execution layer is used to integrate dynamic constraints based on the optimal formation configuration and reference trajectory, and obtain the underlying control commands through rolling optimization calculation of the second comprehensive cost function to realize MPC-based two-layer hierarchical control and formation rigidity maintenance, so as to control the UAV swarm to complete obstacle avoidance. The target's estimated position is fed back in real time to achieve closed-loop feedback.

Images