Large-scale unmanned aerial vehicle group tight coupling positioning method based on IMU / TDOA fusion

Abstract

The invention discloses a large-scale unmanned aerial vehicle group tight coupling positioning method based on IMU / TDOA fusion, and the method comprises the steps: 1, building an unmanned aerial vehicle system generalized model which comprises a kinematics model of an unknown unmanned aerial vehicle, a time difference of arrival (TDOA) and an observation model of inertial navigation (IMU), and preprocessing a TDOA measurement value through moving average filtering (MAF); 2, performing particle initialization, state prediction, measurement updating, resampling and state estimation on each unknown unmanned aerial vehicle by adopting an improved particle filtering algorithm according to the measurement information of each unknown unmanned aerial vehicle, so as to obtain an optimal position estimation value of each unknown unmanned aerial vehicle at the current moment; 3, each unmanned aerial vehicle moves, and the position of the unmanned aerial vehicle at the next moment continues to be estimated. According to the method, high-precision cooperative positioning of a large-scale unmanned aerial vehicle group in a GNSS denial environment can be realized, and compared with a traditional algorithm, the positioning precision and robustness are improved.

Description

Technical Field

[0001] This invention relates to the field of multi-UAV positioning system technology, specifically to a large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion. Background Technology

[0002] Unmanned aerial vehicle (UAV) swarm positioning technology is an important research direction in the field of UAVs, with wide applications in both military and daily life. In the military, accurate and real-time acquisition of the swarm's internal location information is essential for subsequent path planning, task allocation, and obstacle avoidance. In the civilian sector, UAVs are widely used in traffic monitoring, logistics, environmental monitoring, and public services. Especially in emergency rescue operations, such as earthquake zones, forest fires, and urban disaster sites, UAV swarms can be rapidly deployed to perform communication relay and search and rescue operations, greatly improving the efficiency of emergency response. UAV swarm positioning is currently a hot topic in the UAV field, and an increasing number of domestic and international scholars are dedicated to researching large-scale UAV swarm positioning technology, achieving some remarkable results.

[0003] In large-scale UAV swarm localization, common methods include GNSS-based localization or single-sensor methods. However, in complex environments, GNSS signals are easily blocked or interfered with, and single sensors struggle to guarantee long-term stable accuracy, thus failing to meet the collaborative localization requirements of large-scale swarms. To improve the robustness and accuracy of localization, multi-sensor fusion is typically necessary. This involves designing an optimal fusion strategy to achieve the best overall localization accuracy while meeting real-time and resource constraints. However, this problem also faces challenges from nonlinear measurement models and non-Gaussian measurement noise, with computational complexity increasing rapidly with the number of UAVs. Even with traditional particle filter fusion for multi-sensor data fusion, issues such as particle degradation, resampling defects, and sensitivity to measurement outliers still arise in multi-UAV collaborative scenarios. Summary of the Invention

[0004] To address the shortcomings of traditional positioning methods for large-scale UAV swarms, this invention provides a tightly coupled positioning method for large-scale UAV swarms based on IMU / TDOA fusion, which enables the UAV swarm to achieve real-time positioning of itself and ensures the robustness of UAV positioning accuracy.

[0005] The technical solution of this invention is as follows: A large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion, including anchor UAVs and unknown UAVs, where m is the number of anchor UAVs and m≥4, and n is the number of unknown UAVs, including the following steps:

[0006] Step 1: Establish a generalized model of the UAV system, including the kinematic model of the unknown UAV, the observation model of the time difference of arrival (TDOA) and the inertial navigation (IMU), and preprocess the TDOA measurements by moving average filtering (MAF).

[0007] Step 2: For each unknown UAV, an improved particle filtering algorithm is used to process the measurement information. Specifically, firstly, particles are initialized and an initial particle set is generated. The particle state is predicted using the inertial navigation integral formula. The residual values ​​of IMU and TDOA are calculated based on the predicted values. Then, the residual values ​​are mapped to a probability value based on the Huber loss likelihood function. The joint likelihood value is obtained by fusing the IMU and TDOA likelihood values. This joint likelihood value is multiplied by the particle weight of the previous time step to update the particle weights. The number of effective particles is used to determine whether to resample and update the particle set. Finally, the final optimal state estimate is obtained by weighted averaging and fusing the states of all particles in the particle set at the current time step.

[0008] Step 3: Each drone moves and continues to estimate its position at the next moment.

[0009] Preferably, the kinematic model of the dynamic target in step 1 adopts a discrete-time state-space model, described as follows: ,in Indicates time State vector, , and These represent the position, velocity, and acceleration of the UAV in three-dimensional space, respectively. State transition function. It is non-linear. The process noise is represented as zero-mean Gaussian white noise, with a covariance matrix of... ,Right now .

[0010] Preferably, the observation model for the unknown UAV in step 1 is as follows: each unknown UAV carries an IMU positioning module and a TDOA positioning module. The IMU positioning module can measure the linear acceleration of the unknown UAV, described as follows: ,in, Indicates that IMU is in Acceleration measured at any time For real acceleration, This represents the IMU measurement noise, assumed to be zero-mean Gaussian white noise, with the covariance matrix as follows. The TDOA positioning module can measure the time difference of arrival between the unknown drone and the anchor drone, described as follows: ,in, and For anchor drones, For the measured Time Difference of Arrival (TDOA). This represents the true time difference of arrival. Measurement noise. Assuming it is Gaussian white noise, Reflects the noise interference intensity in the TDOA signal, which is affected by the distance difference and signal-to-noise ratio. (Subscript) Indicates the first At that moment, .

[0011] Preferably, the TDOA measurement values ​​are preprocessed using a moving average filter (MAF), and expressed as follows: , For a moment The original TDOA measurement value, From 0 to The index is used to point to each value in the window relative to the current time. Historical measurements, unknown drones receive data from anchored drones and TDOA signal, This is the length of the sliding window.

[0012] Preferably, in step 2, the particle state prediction is performed using the inertial navigation integral formula: at the current time... According to the acceleration values ​​measured by the IMU The kinematic model predicts the particle state at the current moment, and the prediction formula for each particle state vector is as follows: In the formula, No. Time of the first Measurements from the IMU of a drone. , Process noise for velocity and position, respectively. This represents the sampling time interval, with the index k-1 indicating time k-1, and the predicted velocity at time k is obtained. and predicted location .

[0013] Preferably, in step 2, the method for mapping the residual value to a probability value is as follows: First, calculate... IMU observation residuals at time: ,in For the first The actual IMU measurements of each drone node, with superscripts i, i-1, and n representing particles i, i-1, and n respectively. Indicates the first One drone node, For particles obtained using the inertial navigation integral formula Predicted speed; calculation TDOA observation residuals at time: , in, The TDOA measurement is smoothed. For particles obtained using the inertial navigation integral formula Location estimation, , For anchor drones and The actual position of , where c is the speed of light;

[0014] Then, a loss likelihood function is constructed based on the observation residuals. The Huber loss likelihood function based on the IMU is expressed as: ,

[0015] The Huber loss likelihood function based on TDOA is expressed as:

[0016] ,

[0017] In the formula, , These are the covariance matrix of the IMU measurement noise and the covariance matrix of the smoothed TDOA measurement noise, respectively. Here is the Huber loss function. The noise variance after MAF smoothing. M represents the variance of the measurement noise of the IMU accelerometer on the three coordinate axes, where M is the number of observations and the subscript j represents the j-th measurement noise.

[0018] Preferably, in step 2, the method for obtaining the joint likelihood value is as follows: First, construct a weighting factor based on the TDOA residuals. : In the formula, To control the response sensitivity parameters, For offset parameters, Measure the average value of the residuals for TDOA;

[0019] Weighting factors for constructing IMU residuals : ;

[0020] Then, the particles are calculated through weighted combination. Joint observational likelihood :

[0021] .

[0022] Preferably, in step 2, the method for updating the particle weights is as follows: (The rest of the text is missing.) For drone nodes At any moment The The weight of each particle, then The particle weights at time step 1 are updated as follows: , For particles The joint observation likelihood was calculated, and the weights were normalized.

[0023] Preferably, in step 2, the method for determining whether to resample and update the particle set based on the effective particle count is as follows: for each UAV node Calculate its effective particle number and given a threshold ,like < Then resampling is performed; the resampling operation is as follows: the particle set is divided into two subsets: a low-weight particle set and an elite particle set. For the low-weight particle set, an elite particle set is randomly selected and crossed with it to obtain offspring particles. The offspring particles are selected by the mutation rate to perform mutation operation to obtain the final mutated particle set. The elite particle set and the mutated particle set are combined to obtain a new particle set for subsequent filtering iterations, and all particles are assigned uniform initial weights.

[0024] Preferably, in step 2, the method for obtaining the final optimal state estimate is as follows: using a weighted average method, the state estimates of each particle for the UAV are fused together to obtain the optimal estimate of the UAV state at the current moment. : In the formula, For the updated particle weights, It is the total number of particles.

[0025] As a preferred option, the genetic algorithm in step 2.4 is as follows: the particle set obtained in step 2.3 is divided into two subsets: a low-weight particle set and an elite particle set. For the low-weight particle set, an elite particle set is randomly selected and crossed with it to obtain offspring particles. The offspring particles are selected by the mutation rate to perform mutation operations to obtain the final mutated particle set. The elite particle set and the mutated particle set are combined to obtain a new particle set for subsequent filtering iterations, and all particles are assigned uniform initial weights.

[0026] Beneficial effects:

[0027] (1) This invention achieves high-precision estimation of UAV node positions in GNSS denied environments by fusing the original TDOA ranging values ​​and IMU measured acceleration information through particle filtering. Among them, the observation information of TDOA and IMU is dynamically weighted and fused, which can effectively make up for their respective deficiencies, and focuses on solving the multipath error problem of TDOA in NLOS environment and the problem of IMU error divergence over time;

[0028] (2) This invention uses a moving average filtering TDOA outlier processing mechanism, combined with a sliding window method to smooth short-term fluctuations, effectively removes outliers in TDOA ranging data, improves overall measurement stability, and provides reliable input data for subsequent fusion algorithms;

[0029] (3) This invention improves the particle filter framework by integrating Huber robust likelihood function and genetic algorithm resampling. The framework introduces a likelihood function based on Huber loss function for weight calculation and uses genetic algorithm to optimize the particle resampling process to increase particle diversity, which significantly improves the robustness and positioning accuracy of particle filtering in complex interference scenarios. Attached Figure Description

[0030] Figure 1 This is a flowchart of a large-scale UAV swarm tightly coupled localization method based on IMU / TDOA fusion according to an embodiment of the present invention;

[0031] Figure 2 This is the localization result of the algorithm proposed in one embodiment of the present invention;

[0032] Figure 3 This is a comparison of the RMSE of different localization algorithms over time in one embodiment of the present invention.

[0033] Figure 4 This is a simulation result of the trajectory of a drone according to an embodiment of the present invention. Detailed Implementation

[0034] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0035] This invention discloses a large-scale tightly coupled localization method for UAV swarms based on IMU / TDOA fusion, comprising anchor UAVs G and unknown UAVs G, wherein the number of anchor UAVs is m, m≥4, and the number of unknown UAVs is n, as shown below. Figure 1 As shown, it includes the following steps:

[0036] Step 1: Establish a generalized model of the UAV system, including the kinematic model of the unknown UAV, the observation model of the time difference of arrival (TDOA) and the inertial navigation (IMU), and preprocess the TDOA measurements by moving average filtering (MAF).

[0037] The kinematic model of a dynamic target uses a discrete-time state-space model, described as follows: ,in Indicates time State vector, , and These represent the position, velocity, and acceleration of the UAV in three-dimensional space, respectively. State transition function. It is non-linear. The process noise is represented as zero-mean Gaussian white noise, with a covariance matrix of... ,Right now .

[0038] The observation model for the unknown UAV is as follows: each unknown UAV carries an IMU positioning module and a TDOA positioning module.

[0039] The IMU positioning module can measure the linear acceleration of an unknown drone, described as follows: ,in, Indicates that IMU is in Acceleration measured at any time For real acceleration, This represents the IMU measurement noise, assumed to be zero-mean Gaussian white noise, with the covariance matrix as follows. .

[0040] The TDOA positioning module can measure the time difference of arrival between the unknown UAV and the anchor UAV, as described below: ,in, and For anchor drones, For the measured Time Difference of Arrival (TDOA). This represents the true time difference of arrival. Measurement noise. Assuming it is Gaussian white noise, Reflects the noise interference intensity in the TDOA signal, which is affected by the distance difference and signal-to-noise ratio. (Subscript) Indicates the first At that moment, ,make Indicates the location of the target drone. and These represent anchored drones. and The location of the target. The distance from the target to the anchored drone is... and The True Time Difference of Arrival (TDOA) is defined as: ,in, The speed of light. Therefore, the TDOA measurement model can be expressed as: .

[0041] The results of preprocessing TDOA measurements using moving average filtering are as follows: ,in, For a moment Unknown drone receives data from anchored drone and The TDOA signal. Let... Given a TDOA signal sequence, where the sliding window length is given. .

[0042] Step 2: Based on the measurement information of each unknown UAV, an improved particle filter algorithm is used to perform particle initialization, state prediction, measurement update, resampling, and state estimation for each unknown UAV, thereby obtaining the optimal position estimate of each unknown UAV at the current moment. Specifically:

[0043] Step 2.1: Based on the positions of the n unknown drones at the initial time, initialize the particle state for each unknown drone.

[0044] At the initial moment For each drone node Generate the initial particle set: Wherein, the state vector estimated by the particle is: The subscript 0 indicates the initial time. The superscript n indicates the nth The i-th drone node, where i represents the i-th drone node. One particle, The total number of particles, , The subscript 0 indicates time 0, p, v, and a represent position, velocity, and acceleration, respectively, the superscript ~ indicates the estimated value, the superscript i, n represents the i-th particle of the n-th node of the UAV, the superscript ' indicates transpose, and the corresponding weights... Uniformly initialized to .

[0045] Step 2.2: Based on the measurement information of each UAV IMU, the particle state at the current moment is predicted using the inertial navigation integral formula (IMU).

[0046] At the present moment According to the acceleration values ​​measured by the IMU The kinematic model predicts the particle state at the current moment, and the prediction formula for each particle state vector is as follows:

[0047] ,

[0048] in, No. Time of the first Measurements from the IMU of a drone. , Process noise for velocity and position, respectively. This represents the sampling time interval. The predicted velocity at time k is obtained through the prediction formula. and predicted location These are used for IMU residual calculation and TDOA residual calculation in step 2.3, respectively. This embodiment utilizes the short-time, high-precision characteristics of the IMU to provide time-recursive prediction of particle states.

[0049] Step 2.3: Based on the measurement information of each UAV's TDOA and IMU, the likelihood functions of TDOA and IMU are calculated separately using the Huber loss function. A dynamic weighting method is then used to fuse the likelihood functions of TDOA and IMU, updating the particle weights to address outlier sensitivity. Further, a threshold for the number of effective particles is used to determine whether resampling is necessary. Specifically,

[0050] Step 2.31, calculate the observation residuals:

[0051] At every moment , No. The actual IMU measurement value of each drone node is , No. IMU observation residuals of individual particles in three-dimensional space for: ,in, Represents particles Predicted speed.

[0052] At every moment Assuming drone nodes Collected If there are TDOA observations, then the TDOA observation residuals are: ,in, Indicates the first TDOA observation residuals. Anchored UAV. and For example, then:

[0053] ,

[0054] In the formula, The TDOA measurement is smoothed. For the first A particle-to-drone Location estimation, , For anchor drones and The actual location.

[0055] Step 2.32: Construct the Huber loss likelihood function based on the observation residuals from Step 2.31:

[0056] The Huber loss likelihood function based on IMU is defined as:

[0057] ,

[0058] in, To measure the noise covariance matrix of the IMU, The variance of noise is measured on the three coordinate axes of the IMU accelerometer.

[0059] The Huber loss likelihood function based on TDOA is defined as:

[0060] ,

[0061] in, To give the covariance matrix of the smoothed TDOA measurement noise, assume the th Measurement noise of TDOA signal ,but , where is the noise variance after MAF smoothing; The Huber loss function is expressed as: , in, To observe the residuals, The threshold parameter is used to suppress the impact of large errors on the weights.

[0062] Step 2.33, Dynamic Weighted Fusion:

[0063] For drone nodes At any moment The Sigmoid function is used to construct a weighting factor for each particle based on the TDOA residual. : ,

[0064] in, To control the response sensitivity parameters, and ; For offset parameters; The average value of the residuals measured for TDOA, i.e. M represents the number of observations.

[0065] Weighting factor of IMU residuals : .

[0066] Calculate the particle by weighted combination of the two. Joint observational likelihood , represented as:

[0067] ,

[0068] remember For drone nodes At any moment The The weight of each particle, then at time... The particle weights are updated as follows: Finally, the weights are normalized to make... .

[0069] Step 2.34, Valid particle determination:

[0070] For each drone node Calculate its effective particle number : Given a preset threshold If the number of effective particles Less than the threshold If the result is positive, proceed to step 2.4 to trigger resampling; otherwise, proceed to step 2.5. The reliability of TDOA and IMU is balanced by the effective particle count. When the TDOA residual is large, the weight is reduced to avoid outliers contaminating the localization results.

[0071] Step 2.4: When the number of effective particles is insufficient, the particle set in Step 2.3 is resampled based on the genetic algorithm to optimize particle diversity.

[0072] Step 2.41, divide the particle set:

[0073] Through weight threshold The particle set obtained in step 2.3 is divided into two subsets: the low-weight particle set. and Elite Particle Set ,Right now: Elite Particle Set All elite particles are retained for the next generation.

[0074] Step 2.42, cross operation:

[0075] For low-weight particle sets Each particle in Randomly select a corresponding elite particle set Elite particles New offspring particles are generated through linear crossover: ,in, The crossover factor is the sampled value from a uniform distribution.

[0076] Step 2.43, Mutation operation:

[0077] The mutation rate γ is used to select offspring particles for mutation. For each offspring particle... If selected, mutated particles are generated by applying a Gaussian perturbation: in, The noise is zero-mean Gaussian white noise, and the covariance matrix is... ,and Use these as design parameters; otherwise, retain offspring particles. The final set of mutated particles is denoted as... .

[0078] Step 2.44, construct a new particle set:

[0079] By combining elite particle sets and mutated particle set Constructing a new particle set This is used for subsequent filtering iterations and is expressed as: Among them, record for The total number of particles in the array, and assign uniform initial weights to all particles: .

[0080] Step 2.5, Particle set retention: If there are enough effective particles, directly retain the particle set output in step 2.3 without resampling.

[0081] Step 2.6, State Estimation Fusion:

[0082] A weighted average method is used to fuse the state estimates of the drone from each particle, resulting in the optimal estimate of the drone's state at the current moment. : The optimal estimate is the weighted sum of the states of all particles at the current time.

[0083] Step 2 first calculates the IMU residual, which reflects the difference between the acceleration actually measured by the IMU and the acceleration inferred from the velocity change predicted based on the particle state. The smaller the difference, the better the state prediction of the particle matches the IMU measurement, meaning the particle is more reliable, and its weight should be greater.

[0084] The residuals are then mapped to a probability value using the Huber loss likelihood function. The smaller the residual, the larger the calculated likelihood value. A joint likelihood is then obtained by fusing the IMU and TDOA likelihood values, and the particle weights are updated accordingly.

[0085] To overcome the problem of particle weight degradation, a resampling step is introduced. Resampling filters and optimizes the particle weights, so that the new particle set is more concentrated around the high-weight particles corresponding to more credible state hypotheses. Specifically, the particle states before resampling are directly obtained from the prediction step; after resampling, the states of the new particle set are generated by optimizing and filtering the old particle set states using a genetic algorithm.

[0086] Ultimately, regardless of whether resampling is performed, the final optimal state estimate is obtained by weighted averaging and fusing the states of all particles at the current moment. This estimate represents the most accurate position, velocity, and other information of the UAV at the current moment.

[0087] Step 3: Each drone moves and continues to estimate its position at the next moment.

[0088] The specific implementation process of the simulation experiment in this embodiment of the invention is as follows: The simulation was conducted using Matlab 2022b, with the simulation environment set as a three-dimensional space of 2200m * 2200m * 1400m. 100 UAV nodes were deployed in this area, including 20 anchor UAVs with known locations and 80 unknown UAVs to be located. All UAVs adopted a uniform linear motion model with a flight speed of 10m / s and a communication range of 600m. The total simulation time was 120s, and the sampling interval was 0.05s. The IMU measurement noise variance was set to diag{0.1,0.1,0.1}, and the TDOA measurement signal-to-noise ratio was 50 dB. A particle filtering method was used in the algorithm, with 300 particles, a mutation rate of 0.1, and an effective particle number threshold of 150. The experimental procedure is as follows: Step 1, establish a generalized model of the UAV system, including the kinematic model of the unknown UAV, the time difference of arrival (TDOA) and the observation model of the inertial navigation (IMU), and preprocess the TDOA measurements using moving average filtering (MAF); Step 2, based on the measurement information of each unknown UAV, use an improved particle filtering algorithm to perform particle initialization, state prediction, measurement update, resampling and state estimation for each unknown UAV, thereby obtaining the optimal position estimate of each unknown UAV at the current moment; Step 3, each UAV moves and continues to estimate its position at the next moment.

[0089] Figure 2 The figure shows the positioning results of the proposed algorithm at a randomly selected time step. It can be seen that the calculated position is highly consistent with the actual position, with only minor deviations in some edge areas. This verifies that the proposed algorithm has good performance in maintaining accurate positioning of large-scale UAV swarms. Figure 3The comparison of RMSE changes over time for different positioning algorithms shows that the scheme using only IMU exhibits unbounded error accumulation, with an average RMSE of 37 meters. While the scheme using only TDOA does not diverge, it exhibits significant fluctuations, with a peak error as high as 16 meters. In contrast, the algorithm proposed in this application maintains excellent performance throughout the simulation, consistently achieving an RMSE of less than 2 meters with minimal fluctuations. Figure 4 The simulation results of the UAV trajectory show that the scheme using only TDOA exhibits significant position jitter, with some estimated positions deviating severely from the actual trajectory. The scheme using only IMU produces a smoother trajectory, but suffers from drift errors that accumulate over time. In contrast, the trajectory generated by the algorithm proposed in this application almost completely overlaps with the actual trajectory, verifying that the multi-sensor fusion method proposed in this application consistently outperforms the single-sensor scheme, better adapts to different observation quality conditions, and significantly improves robustness to system noise interference.

[0090] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A large-scale UAV swarm tightly coupled localization method based on IMU / TDOA fusion, comprising m anchored UAVs and n unknown UAVs, where m ≥ 4, characterized in that... Includes the following steps: Step 1: Establish a generalized model of the UAV system, including the kinematic model of the unknown UAV, the time difference of arrival (TDOA) observation model, and the inertial navigation (IMU) observation model, and preprocess the TDOA measurements using moving average filtering (MAF). Step 2: For each unknown UAV, an improved particle filtering algorithm is used to process the measurement information. Specifically, firstly, particles are initialized and an initial particle set is generated. The particle state is predicted using the inertial navigation integral formula. The residual values ​​of IMU and TDOA are calculated based on the predicted values. Then, the residual values ​​are mapped to a probability value based on the Huber loss likelihood function. The joint likelihood value is obtained by fusing the IMU and TDOA likelihood values. This joint likelihood value is multiplied by the particle weight of the previous time step to update the particle weights. The number of effective particles is used to determine whether to resample and update the particle set. Finally, the final optimal state estimate is obtained by weighted averaging and fusing the states of all particles in the particle set at the current time step. Step 3: Each drone moves and iteratively estimates its position at the next moment.

2. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 1, characterized in that, In step 1, the kinematic model adopts a discrete-time state-space model, described as follows: ,in It is a state vector; , and Let k, k-1 represent the position, velocity, and acceleration of the UAV in three-dimensional space, respectively. The superscript ' indicates transpose, and the subscript k, k-1 represents time k, k-1. It is a nonlinear state transition function. For process noise, , Let be the covariance matrix.

3. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 2, characterized in that, In step 1, each unknown UAV carries an IMU positioning module and a TDOA positioning module, and the observation model includes: The IMU observation model, used to measure the linear acceleration of an unknown UAV, is expressed as: ,in, Indicates that IMU is in Acceleration measured at any time For real acceleration, Indicates IMU measurement noise; The TDOA observation model, used to measure the linear acceleration of an unknown UAV, is expressed as: ,in, and For anchor drones, For the measured time difference of arrival (TDOA), The index represents the actual arrival time difference. Indicates the first At that moment, This indicates the noise level measured by TDOA.

4. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 3, characterized in that, In step 1, the TDOA measurements are preprocessed using a moving average filter (MAF).

5. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 1, characterized in that, In step 2, the particle state prediction is performed using the inertial navigation integral formula: at the current time... According to the acceleration values ​​measured by the IMU The kinematic model predicts the particle state at the current moment, and the prediction formula for each particle state vector is as follows: In the formula, No. Time of the first Measurements from the IMU of a drone. , The process noises are velocity and position, respectively. The subscript k-1 represents time k-1, and the superscripts i and n represent the i-th particle at the n-th node of the UAV. This represents the sampling time interval. The predicted velocity at time k is obtained according to the prediction formula. and predicted location .

6. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to any one of claims 1-5, characterized in that, In step 2, the method for mapping the residual value to a probability value is as follows: First, calculate... IMU observation residuals at time: ,in For the first The actual IMU measurements of each drone node, where the superscript i and n represent the i-th particle of the n-th node of the drone. For particles obtained using the inertial navigation integral formula Predicted speed; calculation TDOA observation residuals at time: ,in, The TDOA measurement is smoothed. For particles obtained using the IMU formula Location estimation, , For anchor drones and The actual position of , c is the speed of light; Then, a loss likelihood function is constructed based on the observation residuals. The Huber loss likelihood function based on the IMU is expressed as: ， The Huber loss likelihood function based on TDOA is expressed as: ， In the formula, , These are the covariance matrix of the IMU measurement noise and the covariance matrix of the smoothed TDOA measurement noise, respectively. The noise variance after MAF smoothing. The variance of the measurement noise of the IMU accelerometer on the three coordinate axes is given by M, where M is the number of observations and the subscript j represents the j-th measurement noise. Here is the Huber loss function.

7. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 6, characterized in that, In step 2, the method for obtaining the joint likelihood value is as follows: First, construct a weighting factor based on the TDOA residuals. : In the formula, To control the response sensitivity parameters, For offset parameters, Measure the average value of the residuals for TDOA; Weighting factors for constructing IMU residuals : ; Then, the particles are calculated through weighted combination. Joint observational likelihood : 。 8. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 7, characterized in that, In step 2, the method for updating particle weights is as follows: remember For drone nodes At any moment The The weight of each particle, then The particle weights at time step 1 are updated as follows: And perform weight normalization processing.

9. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 1, characterized in that, In step 2, the method for determining whether to resample and update the particle set based on the effective particle count is as follows: for each drone node Calculate its effective particle number and given a threshold ,like < Then resampling is performed; the resampling operation is as follows: the particle set is divided into two subsets: a low-weight particle set and an elite particle set. For the low-weight particle set, an elite particle set is randomly selected and crossed with it to obtain offspring particles. The offspring particles are selected by the mutation rate to perform mutation operation to obtain the final mutated particle set. The elite particle set and the mutated particle set are combined to obtain a new particle set for subsequent filtering iterations, and all particles are assigned uniform initial weights.

10. The large-scale UAV swarm tightly coupled positioning method based on IMU / TDOA fusion according to claim 1, characterized in that, In step 2, the method for obtaining the final optimal state estimate is as follows: a weighted average method is used to fuse the state estimates of the UAV for each particle, which is then used as the optimal estimate of the UAV state at time k. : In the formula, For the updated particle weights, Let be the estimated value of the i-th particle in the n-th node of the UAV at time k. It is the total number of particles.

Images