A method and system for cooperative positioning of unmanned aerial vehicles in a satellite-denied environment

Abstract

The application belongs to the technical field of navigation and positioning, and specifically discloses a method and system for cooperative positioning of unmanned aerial vehicles (UAVs) in a satellite denial environment. The application uses the relative distance information between cluster UAVs to design a cluster UAV cooperative positioning system architecture based on relative information assistance. Through the architecture, the communication between the inertial navigation system and the UAV cluster can provide more reliable positioning services, and the selective correction adaptive particle filtering algorithm is combined to optimize the positioning accuracy. The method of the application uses the inertial navigation system to obtain the self-position of each auxiliary UAV, uses the exogenous relative distance sensor to measure the relative distance with respect to the UAV to be assisted, and then obtains the observation position of the UAV to be assisted. The result is brought into the improved particle filter for filtering to correct the drift error of the inertial navigation system, thereby improving the positioning accuracy of cooperative positioning in the denial environment.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning technology, and relates to a method and system for cooperative positioning of unmanned aerial vehicles in a satellite-denied environment. Background Technology

[0002] In recent years, UAV cooperative navigation technology has been applied in various fields. As a small, low-cost, and self-organizing intelligent agent capable of interacting with surrounding individuals, UAVs are widely used in agriculture, rescue, military, and other fields to perform tasks such as detection, tracking, and commercial performances. Cooperative navigation is a navigation technology in multi-agent systems, aiming to improve the overall system's navigation accuracy, robustness, and task execution efficiency through information sharing, data fusion, and collaboration among multiple intelligent agents (such as UAVs and robots). Compared to single-agent navigation, cooperative navigation can achieve higher-precision positioning, path planning, and collaborative task execution in complex environments (such as those with limited GPS signals, interference, or dynamic obstacles) through inter-agent communication, distributed computing, and environmental perception.

[0003] Cooperative localization is the core of cooperative navigation, enabling multiple agents to accurately determine their relative positions, providing a crucial foundation for cooperative navigation. Cooperative localization algorithms are the key technologies for achieving cooperative localization and completing subsequent navigation tasks. Current algorithms include Kalman filtering, particle filtering, factor graphs, machine learning algorithms, and more.

[0004] Kalman filtering is an optimal estimation method based on the state-space model of a linear system. It continuously estimates and corrects state variables through two steps: prediction and update, using the system's dynamic and observation equations. However, this algorithm requires a high degree of accuracy from the system model; its filtering performance degrades when there is a significant deviation between the actual system and the model.

[0005] Particle filtering is a nonlinear filtering algorithm based on Monte Carlo simulation. It represents the probability distribution by randomly sampling a large number of particles in the state space, updating the particle weights based on sensor measurements, and then obtaining an estimate of the state through resampling and other operations. However, this algorithm requires a large number of particles to ensure the accuracy of the estimation, has relatively poor real-time performance, and may suffer from particle degradation problems when applied to high-dimensional state spaces.

[0006] In an environment where global navigation satellite systems are denied, relying solely on inertial navigation systems is insufficient to meet the demand for high-precision positioning. Summary of the Invention

[0007] The purpose of this invention is to propose a cooperative positioning method for unmanned aerial vehicles (UAVs) in a satellite-denied environment. This method obtains the predicted and observed information of the UAV to be assisted by the onboard intrinsic inertial and extrinsic relative distance sensors of the UAV swarm, respectively, and then performs filtering processing through a selectively modified adaptive particle filter algorithm, thereby improving the cooperative positioning accuracy.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] A method for cooperative localization of unmanned aerial vehicles (UAVs) in a satellite-denied environment includes the following steps:

[0010] Step 1. For each drone in the drone swarm to be assisted, obtain its initial position, as follows:

[0011] First, select three auxiliary drones and obtain the position of each auxiliary drone; then measure the distance between each auxiliary drone and the drone to be assisted, and use the least squares method to calculate the position of the drone to be assisted.

[0012] The drone swarm includes at least three auxiliary drones with a first-precision inertial navigation system and at least one drone to be assisted with a second-precision inertial navigation system, with distance sensors installed on the auxiliary drones.

[0013] Step 2. After obtaining the position of the UAV to be assisted in Step 1, its flight trajectory is obtained. The flight trajectory of the UAV to be assisted is used as observation information. Based on the selective correction adaptive particle filter algorithm, its own inertial navigation prediction trajectory is filtered to correct the inertial navigation error and finally achieve cooperative positioning of the UAV to be assisted.

[0014] Among them, the accuracy of the first-precision inertial navigation system is higher than that of the second-precision inertial navigation system.

[0015] In step 2, the selectively modified adaptive particle filter algorithm improves estimation accuracy, alleviates particle degeneration and depletion problems, and optimizes computational efficiency by introducing a high-weight particle fine-tuning strategy between the weight calculation and resampling steps.

[0016] The process of the high-weighted particle fine-tuning strategy is as follows:

[0017] I. First, define the selection threshold R and adjustment factor for high-weight particles. ;

[0018] II. After updating the particle state weights through the weight calculation step, perform the following operations:

[0019] II.1. Screening for high-weight particles: Particles that meet the following conditions are considered high-weight particles: Set a high-weight particle index set for: ;in, For particle weights;

[0020] II.2. Calculate the center position of the high-weighted particles The formula is as follows:

[0021] ;

[0022] in, Indicates the particle state. The number of high-weighted particles;

[0023] II.3. Define the low-weight particle index set for: ;

[0024] The low-weight particles then move closer to the high-weight center position, and the formula for updating the particle state is as follows:

[0025] , ;

[0026] in, The updated particle state;

[0027] When the algorithm is repeated next time, the updated particle state will be used as the initial value for state propagation.

[0028] Preferably, in the high-weight particle fine-tuning strategy processing step 2, an adaptive screening threshold is calculated based on the adaptive screening threshold feedback algorithm. , to replace the given screening threshold R for high-weight particles;

[0029] in It can adaptively adjust based on the rate of change of the residuals of the observed and filtered estimated locations, as shown in the following formula:

[0030] ;

[0031] ;

[0032] ;

[0033] in , They are respectively , The residual is the difference between the estimated position obtained by particle filtering and the position obtained by least squares method. The residual rate of change The learning rate is used to control the step size of the adjustment. Indicates the filtered estimated location. Indicates the observation location. , They represent , The threshold of time;

[0034] The first formula is used to obtain the Euclidean distance between the observed position and the filtered estimated position, which is used as the residual; the second formula is used to calculate the residual from the previous time step. The residual at that moment The difference between the residuals is used as the rate of change of the residuals; the third formula is used to calculate the rate of change of the residuals from the previous time step. The threshold is adjusted by adding a product of the learning rate and the residual rate of change. The threshold.

[0035] Preferably, in the high-weighted particle fine-tuning strategy processing step 2, a proportional distance fine-tuning method based on radial weighted contraction is further proposed for updating the particle state, specifically for the particle state update formula in II.3. The formula is as follows:

[0036] , ;

[0037] , ;

[0038] ;

[0039] in, This represents the Euclidean distance between the centers of the i-th low-weight particle and the high-weight particle at time t. This represents the state of the low-weighted particle at time t, i.e., the particle state before modification. This represents the largest Euclidean distance among the center positions of all low-weight particles and high-weight particles at time t. For the weight function, To adjust the factor, This represents the state of the low-weighted particle at time t+1, i.e., the modified state of the low-weighted particle. It is a very small positive number to prevent the denominator from being 0;

[0040] The first formula is used to calculate the Euclidean distance between the center positions of all low-weight particles and high-weight particles, as a weighting reference; the second formula is the weighting function, which means that different weights are given by different calculated Euclidean distances, and are used for subsequent weighting; the third formula means that the adjusted low-weight particle state is the particle state before adjustment, plus the product of the adjustment coefficient, weight, and distance difference.

[0041] Furthermore, based on the aforementioned UAV cooperative positioning method under satellite denial conditions, this invention also proposes a corresponding UAV cooperative positioning system under satellite denial conditions, which adopts the following technical solution:

[0042] A collaborative positioning system for unmanned aerial vehicles (UAVs) in a satellite-denied environment includes a ground control center and a swarm of UAVs.

[0043] The drone swarm includes at least three auxiliary drones with a first-precision inertial navigation system and at least one drone to be assisted with a second-precision inertial navigation system, with distance sensors installed on the auxiliary drones.

[0044] The ground control center is equipped with computer equipment; the position information of the assisted UAV and the distance information between it and the UAV to be assisted are transmitted to the computer equipment at the ground control center.

[0045] The computer device includes a memory and one or more processors; the memory stores executable code, and when the processor executes the executable code, it implements the steps of the UAV cooperative positioning method in a satellite-denied environment as described above.

[0046] The present invention has the following advantages:

[0047] As described above, this invention relates to a method and system for cooperative positioning of unmanned aerial vehicles (UAVs) in a satellite-denied environment. This cooperative positioning method utilizes the relative distance information among swarm UAVs to design a swarm UAV cooperative positioning system architecture based on relative information assistance. Through this architecture, communication between the inertial navigation system and the UAV swarm can provide more reliable positioning services, and the positioning accuracy is optimized by combining a selectively modified adaptive particle filter (SCA-PF) algorithm. The method of this invention uses the inertial navigation system to obtain the position of each UAV and uses an external relative distance sensor to measure the relative distance to the UAV to be assisted, thereby obtaining the observed position of the UAV to be assisted. The observation results are then fed into an improved particle filter for filtering to correct the drift error of the inertial navigation system, thereby improving the positioning accuracy of cooperative positioning in a satellite-denied environment. The selectively corrected adaptive particle filtering algorithm proposed in this invention can adaptively adjust the particle weights after weight updates to ensure that the adjusted particles accurately reflect the current state, calculate their center positions, and correct low-weight particles, guiding them towards the center to improve the filtering effect. This avoids erroneous guidance due to the influence of a single abnormal particle. Resampling is then performed, allowing low-weight particles to converge towards high-weight regions without completely discarding them, improving the stability of state estimation while maintaining a certain level of exploration capability. The algorithm preserves high-weight particles while adjusting only low-weight particles, enhancing positioning accuracy while maintaining particle diversity. Building upon this, this invention further proposes an adaptive screening threshold feedback algorithm, obtaining an adaptive screening threshold that can adaptively adjust based on the rate of change of the residuals between the observed and filtered estimated positions, replacing the fixed screening threshold. The adaptive screening threshold proposed in this invention can dynamically increase or decrease the screening threshold based on the current residuals. Especially when facing noise or other uncertainties, it can automatically adjust parameters, thereby enhancing the robustness of the system. Furthermore, this invention proposes a novel particle state update method: a proportional distance fine-tuning method based on radially weighted contraction. Different contraction distances are assigned to low-weight particles based on the Euclidean distance between their center positions and those of high-weight particles. A larger contraction distance indicates that the particle's weight is too low, and a larger adjustment is made to allow it to play its role and move closer to its true position. Particles with closer distances receive only minor adjustments. Radially weighted contraction explicitly utilizes the geometric positional relationships of particles in the state space, making the particle update process more consistent with spatial clustering laws. This allows the improved particle filtering algorithm to better support the high-reliability positioning requirements in UAV cooperative navigation. Attached Figure Description

[0048] Figure 1 This is a schematic diagram of the drone's position in Embodiment 1 of the present invention;

[0049] Figure 2This is a flowchart illustrating the UAV cooperative positioning method under satellite denial conditions in Embodiment 1 of the present invention.

[0050] Figure 3 This is a diagram showing the actual flight trajectories of the three auxiliary drones and the drone to be assisted, as illustrated in Embodiment 1 of the present invention.

[0051] Figure 4 This is a three-dimensional distance error analysis diagram of the observed trajectory after SCA-PF filtering to improve positioning accuracy in Embodiment 1 of the present invention;

[0052] Figure 5 This is a comparison chart of the root mean square total error of the observed trajectory after SCA-PF filtering to improve positioning accuracy in Embodiment 1 of the present invention;

[0053] Figure 6 This is a flowchart of the high-weight particle fine-tuning strategy in Embodiment 2 of the present invention. Detailed Implementation

[0054] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0055] Example 1

[0056] This embodiment 1 describes a cooperative localization method for unmanned aerial vehicles (UAVs) in a satellite-denied environment. It acquires predictive and observational information based on the perception mechanism of onboard inertial and relative distance sensors within a UAV swarm, and combines this with an improved particle filter for cooperative localization. The improved particle filter is based on the Selectively Corrected Adaptive-Particle Filter (SCA-PF) algorithm, which significantly improves estimation accuracy, alleviates particle degradation and depletion problems, and optimizes computational efficiency, making the filtering process more robust and accurate.

[0057] like Figure 2 As shown in this embodiment, the UAV cooperative positioning method under satellite denial environment includes the following steps:

[0058] Step 1. For each drone in the drone swarm to be assisted, obtain its initial position, as follows:

[0059] First, select three auxiliary drones and obtain the position of each auxiliary drone; then measure the distance between each auxiliary drone and the drone to be assisted, and use the least squares method to calculate the position of the drone to be assisted.

[0060] The drone swarm includes at least three auxiliary drones with a first-precision inertial navigation system and at least one drone to be assisted with a second-precision inertial navigation system, with distance sensors installed on the auxiliary drones.

[0061] The accuracy of the first-precision inertial navigation system is higher than that of the second-precision inertial navigation system. In this embodiment, the first-precision inertial navigation system is a high-precision inertial navigation system, and the second-precision inertial navigation system is a low-precision inertial navigation system.

[0062] by Figure 1 The example shown is a UAV assisted by three high-precision inertial navigation systems.

[0063] To improve the positioning accuracy of the low-precision inertial navigation system-assisted UAV, the assisted UAV obtains its own position through high-precision autonomous positioning, and the distance information between itself and the UAV under assisted is measured using a distance sensor. .

[0064] Taking the auxiliary drone 1 as an example, its position is known. and its distance from the drone to be assisted The positions of auxiliary drones 2 and 3, as well as their distances from the drone to be assisted, can be obtained similarly.

[0065] By incorporating this information into a three-dimensional coordinate system, the position coordinates of the drone to be assisted can be calculated using a mathematical model. Subsequently, SCA-PF is used for filtering feedback to correct inertial navigation errors. The overall steps are as follows: Figure 2 As shown.

[0066] In three-dimensional space, the actual position of the drone to be assisted is However, the location is unknown.

[0067] To estimate the position of the drone to be assisted, three high-precision positioning auxiliary drones were used for ranging, and their positions were calculated using the least squares method. The positions of the three auxiliary drones are known as follows:

[0068] , , .

[0069] The steps for a distance sensor to calculate distance based on the time of arrival (TOA) of the signal are as follows:

[0070] Step a1. Record the signal transmission and reception times. The signal transmission time is... The signal reception time is .

[0071] Step a2. Calculate the time difference: .

[0072] Step a3. Determine the signal propagation speed, defining the signal speed as c, usually the speed of light, i.e., 3 × 10⁻⁶. 8 meters per second.

[0073] Step a4. Calculate the distance .

[0074] By using steps a1 to a4 above, the relative distance between the assisting drone and the drone to be assisted can be calculated. .

[0075] Next, according to the Euclidean distance formula, the distance from the auxiliary drone to the three auxiliary drones is:

[0076] .

[0077] in This is the actual measured distance, specifically the distance from the drone to be assisted to the i-th assisting drone, where i=1,2,3. It is ranging noise, which can be obtained by solving the least squares method: .

[0078] Finally, the position of the UAV to be assisted is calculated. After obtaining the flight trajectory of the UAV to be assisted, it is used as observation information for filtering to improve positioning accuracy and correct the error information of the inertial navigation system.

[0079] Step 2. After obtaining the location of the drone to be assisted in Step 1, its flight trajectory is obtained.

[0080] Using the flight trajectory of the UAV to be assisted as observation information, the inertial navigation prediction trajectory is filtered based on the selective correction adaptive particle filter algorithm, and the inertial navigation error is corrected by feedback, so as to achieve cooperative positioning of the UAV to be assisted.

[0081] Particle filtering (PF) is a nonlinear filtering algorithm based on the Monte Carlo method, suitable for state estimation problems in non-Gaussian noise environments. The core idea of ​​the PF algorithm is to approximate the posterior probability distribution of the system using a set of weighted particles. However, during the update process, the number of high-weight particles gradually decreases, eventually concentrating most of the weight on a few particles, reducing particle swarm diversity and leading to distorted estimation results.

[0082] The improved particle filter algorithm (i.e., selectively modified adaptive particle filter algorithm) in this embodiment significantly improves the estimation accuracy by introducing a high-weight particle fine-tuning strategy between the weight calculation and resampling steps, effectively alleviating the particle degeneration and poverty problems, while optimizing computational efficiency, making the filtering process more robust and accurate.

[0083] Specifically, the processing procedure of the selectively modified adaptive particle filter algorithm in this embodiment is as follows:

[0084] Step 2.1. Initialization.

[0085] At the initial time t=0, N particles are randomly selected from the prior distribution and each particle is assigned the same weight to represent the various position states that the UAV may appear in, where N is a natural number.

[0086] The actual location of the drone to be assisted is known. for: .

[0087] The initial state of each particle is generated by adding random variables near its real location.

[0088] For the i-th particle, its initial position has three components along the x, y, and z directions:

[0089] .

[0090] in For random variables, , , This indicates the position of the drone to be assisted at time 1. This represents the position of the i-th particle in three dimensions at time 1.

[0091] Step 2.2. Prediction (state propagation).

[0092] For each time step Based on the system's state, state propagation is performed on each particle, that is, the next flight positioning is inferred based on past position information and current motion state. The process is as follows:

[0093] .

[0094] in, Let be the position of the i-th particle in three dimensions at time t; , , Let i be the position of the i-th particle in three dimensions at time t-1; , , This refers to the velocity information at time t-1.

[0095] Step 2.3. Update (weight calculation).

[0096] Step 2.3. Based on the obtained observations of the UAV to be assisted and the state of each particle, update the weight of each particle, and then normalize the updated weight of each particle.

[0097] Let the observations obtained by the auxiliary UAV be... The particles are .

[0098] Update the weight formula for each particle. The calculation formula is as follows:

[0099] .

[0100] in, As weight, To represent the degree of similarity between the observed values ​​and the particles, here, Calculated using Euclidean distance and The distance between them To measure the standard deviation of noise.

[0101] Then the weights are normalized using the following formula:

[0102] .

[0103] Step 2.4. Particle fine-tuning.

[0104] A high-weighted particle fine-tuning strategy is introduced to fine-tune the particle state.

[0105] During the update process, the number of high-weight particles gradually decreases, eventually concentrating most of the weight on a few particles, reducing particle swarm diversity and leading to distorted estimation results. An innovative improvement is introduced after weight updates to mitigate this problem. The weight update step updates the weight of each particle based on the current measurement value, reflecting the degree of matching between the particle and the current measurement. If optimization is performed before weight updates, the optimization operation may not accurately guide particles closer to the true state because the weights do not yet reflect the information of the current measurement value. Resampling operations copy and delete particles based on their weights, altering the particle distribution. If low-weight particle adjustments are performed after resampling, the particle diversity has already decreased, potentially leading to insignificant fine-tuning effects or even triggering more concentration effects, reducing exploration capability. The improved particle filtering algorithm significantly improves estimation accuracy and alleviates particle degeneration and poverty problems by introducing a high-weight particle fine-tuning strategy after weight calculation, while also optimizing computational efficiency, making the filtering process more robust and accurate.

[0106] The process of the high-weighted particle fine-tuning strategy is as follows:

[0107] I. First, define the selection threshold R and adjustment factor for high-weight particles. .

[0108] II. After updating the particle state weights through the weight calculation step, perform the following operations:

[0109] II.1. Screening for high-weight particles: Particles that meet the following conditions are considered high-weight particles: Set a high-weight particle index set for: ;in, For particle weights.

[0110] II.2. Calculate the center position of the high-weighted particles The formula is as follows:

[0111] .

[0112] in, Indicates the particle state. This represents the number of high-weighted particles.

[0113] II.3. Define the low-weight particle index set for: Then, low-weight particles move closer to the center position of high-weight particles, and the formula for calculating the updated particle state is as follows:

[0114] , .

[0115] in, This represents the updated particle state.

[0116] The next time the algorithm is repeated, the updated particle state will be used as the initial value for state propagation.

[0117] Step 2.5. Resampling.

[0118] After fine-tuning the state in step 2.4, the particles are resampled. Based on the weight of the particles, N particles are resampled, so that particles with large weights are copied multiple times and particles with small weights are eliminated.

[0119] To avoid particle degradation (i.e., most particle weights approaching zero), resampling is required. N particles are resampled based on their weights, so that particles with higher weights are replicated multiple times, while particles with lower weights are discarded.

[0120] Step 2.6. State estimation.

[0121] Based on the particle set after the update (i.e., after resampling in step 2.5), the system state estimate is calculated using a weighted average method to deduce the most likely location of the UAV. The formula obtained using the weighted average method is as follows:

[0122] ;

[0123] in, This represents the weight of the i-th particle at time t. This is the state estimate of the system.

[0124] Step 2.7. Increment t by 1 and repeat steps 2.2 to 2.6 until all time steps are completed, obtaining the precise position after filtering at each time step, and thus obtaining the flight trajectory of the UAV to be assisted.

[0125] The improved SCA-PF in this embodiment has the following advantages:

[0126] 1. Effectively overcomes the problem of estimation distortion caused by reduced particle swarm diversity, avoids misleading due to anomalies in a single particle when calculating the center position, and enhances convergence and estimation accuracy based on the classic PF, making it more robust.

[0127] 2. Unlike conventional improvement steps, this invention performs fine-tuning after weight update and before resampling. This avoids the problem that the optimization operation may not be able to accurately guide the particles toward the true state because the weights have not yet reflected the information of the current measurement value. It also prevents the fine-tuning effect from being insignificant because the diversity of particles has been reduced after resampling.

[0128] 3. Conventional weighted proximity strategies typically involve moving low-weight particles closer to high-weight particles. This inevitably leads to over-concentration of particles, resulting in a loss of diversity, reduced particle exploration capabilities, and an inability to adapt to trajectory changes promptly. In contrast, the SCA-PF proposed in this invention retains high-weight particles above a certain threshold while only adjusting low-weight particles. This optimizes particle distribution, maintains particle diversity, improves estimation accuracy, and alleviates the problem of insufficient particle diversity.

[0129] In addition, to verify the effectiveness of the method of the present invention, the following simulated flight trajectory experiment is also presented.

[0130] In the simulated flight trajectory experiment, the UAV to be assisted was equipped with a low-precision inertial navigation system, resulting in low positioning accuracy; while the assisted UAV was equipped with a high-precision inertial navigation system, achieving higher positioning accuracy. Figure 3 shows the actual trajectories of the three assisted UAVs and the UAV to be assisted. The observed trajectories were obtained by measuring distances and using the least squares method. After SCA-PF filtering to improve positioning accuracy, the three-dimensional distance error and the total mean square error are shown in Figure 4 and Figure 5, respectively. Figure 5 As shown.

[0131] Table 1 Comparison of Root Mean Square Filtering Error

[0132]

[0133] Table 1 shows the observations of the UAV to be assisted and the results after filtering using the SCA-PF method of this invention. A schematic diagram comparing the root mean square filtering errors is also included. As can be seen from Table 1, the improved SCA-PF method of this invention can reduce the observation error by about 70%, exhibiting high overall filtering accuracy, effectively reducing errors, and demonstrating good filtering performance.

[0134] The selectively corrected adaptive particle filtering algorithm proposed in this invention can adaptively adjust the particle weights after weight updates to ensure that the adjusted particles accurately reflect the current state. It calculates the center position of low-weight particles, correcting them to move closer to the center, thus improving the filtering effect and avoiding erroneous guidance due to the influence of a single abnormal particle. Resampling is then performed, allowing low-weight particles to converge towards high-weight regions without completely discarding them, improving the stability of state estimation while maintaining a certain level of exploration capability. The algorithm preserves high-weight particles while adjusting only low-weight particles, enhancing positioning accuracy while maintaining particle diversity.

[0135] The method of this invention has a wide range of applications and is convenient, making it suitable for UAV cooperative navigation under satellite navigation denial.

[0136] Example 2

[0137] This embodiment 2 describes a method for cooperative positioning of unmanned aerial vehicles in a satellite-denied environment. Except for the following technical features, which are different from those in embodiment 1, the other technical features of this method can be referred to in embodiment 1.

[0138] like Figure 6 As shown, in the process of the high-weight particle fine-tuning strategy, this embodiment no longer uses a fixed screening threshold R.

[0139] Since different filtering thresholds produce different filtering effects under different conditions, the fixed filtering threshold R used in Example 1 above may not be able to respond to these changes in a timely manner, resulting in a decrease in system performance.

[0140] Specifically, when the magnitude and direction of acceleration change, as well as when the sensor accuracy changes, the distribution of particles may change. In this case, if a pre-set fixed screening threshold R is used, it will not be able to better distinguish between high-weight particles and low-weight particles. Although this can improve the accuracy of the algorithm, it is not the optimal accuracy.

[0141] Here, the different situations specifically refer to the state of the UAV, including acceleration and angular velocity in various directions and their changes, as well as noise, including measurement errors of the inertial navigation system and distance sensor measurement errors.

[0142] This embodiment proposes an adaptive screening threshold feedback algorithm to obtain an adaptive screening threshold. It can adaptively adjust based on the rate of change of the residuals of the observed location and the filtered estimated location, thus replacing the fixed screening threshold R.

[0143] The specific formula for the adaptive screening threshold feedback algorithm is shown below:

[0144] ;

[0145] ;

[0146] ;

[0147] in , They are respectively , The residual is the difference between the estimated position obtained by particle filtering and the position obtained by least squares method. The residual rate of change The learning rate is used to control the step size of the adjustment. Indicates the filtered estimated location. Indicates the observation location. , They represent , The threshold of time.

[0148] The first formula represents the Euclidean distance between the observed position and the filtered estimated position, which is used as the residual. The second formula represents the difference between the residual from the previous time step and the residual from the current time step, which is used as the residual rate of change. The third formula represents adjusting the threshold at the current time step by adding the product of the residual learning rate and the residual rate of change to the threshold from the previous time step.

[0149] Adaptive filtering threshold It can be based on the current residual It can dynamically raise or lower the screening threshold, especially when facing noise or other uncertainties, and automatically adjust parameters to enhance the robustness of the system.

[0150] Specifically, when residual When the threshold is smaller, the system is more accurate, so it's necessary to increase the filtering threshold. This is used to filter out particles with higher weights and greater reliability, thereby improving the accuracy and efficiency of the filtering algorithm.

[0151] Conversely, when the residual When the value increases, the filtering threshold needs to be lowered to avoid erroneous local optima. This increases the particle's exploration space, thereby increasing the likelihood of reconverging to the correct solution.

[0152] Furthermore, this embodiment also improves the particle state update formula in II.3 of Embodiment 1 above.

[0153] Specifically, the unchanging particle fine-tuning method in Example 1 is not conducive to complex situations (i.e., changes in acceleration and error). Therefore, this example proposes a radially weighted contraction proportional distance fine-tuning method, as shown in the following formula:

[0154] , ;

[0155] , ;

[0156] ;

[0157] in, This represents the Euclidean distance between the centers of the i-th low-weight particle and the high-weight particle at time t. This represents the state of the low-weighted particle at time t, i.e., the particle state before modification. This represents the largest Euclidean distance among the center positions of all low-weight particles and high-weight particles at time t. For the weight function, To adjust the factor, This represents the state of the low-weighted particle at time t+1, i.e., the modified state of the low-weighted particle. It represents a very small positive number to prevent the denominator from being 0.

[0158] The first formula calculates the Euclidean distance between the centers of all low-weight particles and high-weight particles, using this distance as a weighting reference. The second formula is the weighting function, which assigns different weights based on the calculated Euclidean distances, used for subsequent weighting. The third formula means that the adjusted state of a low-weight particle is the product of its original state, the adjustment coefficient, the weight, and the distance difference.

[0159] Different shrinkage distances are assigned to the centers of low-weight particles and high-weight particles based on their Euclidean distance. The larger the shrinkage distance, the lower the weight of the particle. Through larger adjustments, the particle can play its role and be closer to its actual position.

[0160] Specifically, particles that are farther away experience a stronger contraction effect, enabling them to quickly move towards the central region, thus avoiding long-term interference from outliers in the estimation results; particles that are closer only undergo slight adjustments. The radially weighted contraction proposed in this embodiment explicitly utilizes the geometric positional relationships of particles in the state space, making the particle update process more consistent with the spatial clustering law.

[0161] The radial weighted contraction method in this embodiment differs from the traditional weight-based resampling mechanism. Its core lies in explicitly utilizing the geometric distribution relationship of particles in the state space to construct a "spatial clustering law".

[0162] Specifically, particles are no longer simply copied or discarded; instead, they are weighted and contracted based on their radial distance from the center: particles farther from the center are significantly clustered to suppress drift errors and outlier effects; particles closer to the center are slightly adjusted to maintain diversity. Thus, the entire particle set gradually shrinks towards the real-world region while maintaining coverage, achieving higher positioning accuracy and stronger robustness in complex environments. This spatial clustering pattern allows the improved particle filtering algorithm to better support the high-reliability positioning requirements of UAV cooperative navigation.

[0163] The collaborative localization method for UAVs in satellite-denied environments needs to meet the requirements of autonomy, real-time performance, accuracy, and robustness so that the method can better meet the needs of UAV collaborative localization scenarios in satellite-denied environments.

[0164] This application, designed using the above method, clearly meets these requirements well, specifically in the following ways:

[0165] 1. Regarding autonomy:

[0166] The SCA-PF algorithm proposed in this embodiment can dynamically adjust the threshold and weight parameters in a denied environment. Even without external information, the algorithm can achieve self-correction using information within the cluster, and it hardly depends on external navigation sources.

[0167] 2. Regarding real-time performance:

[0168] UAV cooperative navigation requires algorithms to maintain a fast response under conditions of limited communication and computational resources. This algorithm automatically adjusts the threshold Rt based on the rate of change of the observation and filtering residuals, rather than using a fixed threshold. This avoids manual parameter tuning under different noise levels and improves the efficiency of automated processing. High-weighted particles remain unchanged, while low-weighted particles move towards the high-weighted center; weighting is adjusted based on geometric position to reduce computational waste from invalid particles.

[0169] 3. Regarding accuracy:

[0170] Unmanned aerial vehicles (UAVs) need more precise positional accuracy to complete tasks flawlessly. This algorithm's adaptive threshold allows for optimal parameter adjustments under different conditions. The radially weighted contraction dynamically determines the contraction magnitude based on the distance between the particle and the center, avoiding a one-size-fits-all approach for all weighted particles. This makes the particle swarm distribution more consistent with spatial geometry, improving estimation accuracy.

[0171] 4. Regarding robustness:

[0172] In denied environments, UAVs often face problems such as accumulated inertial navigation drift, high ranging noise, and particle degradation, making robustness particularly crucial. This embodiment can dynamically adjust the threshold based on the observation residual. When noise fluctuates drastically or sudden interference occurs, the algorithm can tighten or relax the screening conditions to ensure that the state estimation does not diverge. Low-weight particles far from the center are adjusted with larger amplitudes, while particles nearby are adjusted with smaller amplitudes, avoiding a one-size-fits-all approach and making the particle distribution more consistent with spatial geometry.

[0173] In summary, the novel particle filter resampling improvement algorithm proposed in Example 2 is more suitable for UAV cooperative navigation in denied environments compared to conventional resampling improvements.

[0174] Example 3

[0175] Embodiment 3 of the present invention describes a UAV cooperative positioning system under satellite denial environment, which includes a ground control center and a UAV swarm.

[0176] The drone swarm includes at least three auxiliary drones with a first-precision inertial navigation system and at least one drone to be assisted with a second-precision inertial navigation system, with distance sensors installed on the auxiliary drones.

[0177] The ground control center is equipped with computer equipment; the position information of the assisted UAV and the distance information between it and the UAV to be assisted are transmitted to the computer equipment at the ground control center.

[0178] The computer device includes a memory and one or more processors; the memory stores executable code, and when the processor executes the executable code, it is used to implement the UAV cooperative positioning method in the satellite denial environment described in Embodiment 1 or 2 above.

[0179] Of course, the above description is only a preferred embodiment of the present invention. The present invention is not limited to the above-described embodiments. It should be noted that any equivalent substitutions or obvious modifications made by those skilled in the art under the guidance of this specification fall within the scope of this specification and should be protected by the present invention.

Claims

1. A method for cooperative positioning of unmanned aerial vehicles (UAVs) in a satellite-denied environment, characterized in that, Includes the following steps: Step 1. For each drone in the drone swarm to be assisted, obtain its initial position, as follows: First, select three auxiliary drones and obtain the position of each auxiliary drone; then measure the distance between each auxiliary drone and the drone to be assisted, and use the least squares method to calculate the position of the drone to be assisted. The drone swarm includes at least three auxiliary drones with a first-precision inertial navigation system and at least one drone to be assisted with a second-precision inertial navigation system, with distance sensors installed on the auxiliary drones. Step 2. After obtaining the position of the UAV to be assisted in Step 1, its flight trajectory is obtained. The flight trajectory of the UAV to be assisted is used as observation information. Based on the selective correction adaptive particle filter algorithm, its own inertial navigation prediction trajectory is filtered to correct the inertial navigation error and finally achieve cooperative positioning of the UAV to be assisted. Among them, the accuracy of the first-precision inertial navigation system is higher than that of the second-precision inertial navigation system; The selectively modified adaptive particle filter algorithm improves estimation accuracy, alleviates particle degeneration and depletion problems, and optimizes computational efficiency by introducing a high-weight particle fine-tuning strategy between weight calculation and resampling steps. The process of the high-weighted particle fine-tuning strategy is as follows: I. First, define the selection threshold R and adjustment factor for high-weight particles. ; II. After updating the particle state weights through the weight calculation step, perform the following operations: II.

1. Screening for high-weight particles: Particles that meet the following conditions are considered high-weight particles: Set a high-weight particle index set for: ;in, For particle weights; II.

2. Calculate the center position of the high-weighted particles The formula is as follows: ； in, Indicates the particle state. The number of high-weighted particles; II.

3. Define the low-weight particle index set for: ; The low-weight particles then move closer to the high-weight center position, and the formula for updating the particle state is as follows: ， ； in, The updated particle state; When the algorithm is repeated next time, the particle state after this update will be used as the initial value for state propagation; In the high-weighted particle fine-tuning strategy processing, an adaptive screening threshold is calculated based on the adaptive screening threshold feedback algorithm. , to replace the given screening threshold R for high-weight particles; in It can adaptively adjust based on the rate of change of the residuals of the observed and filtered estimated locations, as shown in the following formula: ； ； ； in , They are respectively , The residual is the difference between the estimated position obtained by particle filtering and the position obtained by least squares method. The residual rate of change The learning rate is used to control the step size of the adjustment. Indicates the filtered estimated location. Indicates the observation location. , They represent , The threshold of time; The first formula is used to obtain the Euclidean distance between the observed position and the filtered estimated position, which is used as the residual; the second formula is used to calculate the residual from the previous time step. The residual at that moment The difference between the residuals is used as the rate of change of the residuals; the third formula is used to calculate the rate of change of the residuals from the previous time step. The threshold is adjusted by adding a product of the learning rate and the residual rate of change. The threshold.

2. The UAV cooperative positioning method under satellite denial environment according to claim 1, characterized in that, In step 2, during the high-weighted particle fine-tuning strategy processing, a proportional distance fine-tuning method based on radial weighted contraction is further proposed for updating the particle state, specifically addressing the particle state update formula in II.

3. The formula is as follows: ， ； ， ； ； in, This represents the Euclidean distance between the centers of the i-th low-weight particle and the high-weight particle at time t. This represents the state of the low-weight particle at time t, i.e., the particle state before modification. This represents the largest Euclidean distance among the center positions of all low-weight particles and high-weight particles at time t. For the weight function, To adjust the factor, This represents the state of the low-weighted particle at time t+1, i.e., the modified state of the low-weighted particle. It is a very small positive number to prevent the denominator from being 0; The first formula is used to calculate the Euclidean distance between the center positions of all low-weight particles and high-weight particles, as a weighting reference; the second formula is the weighting function, which means that different weights are given by different calculated Euclidean distances, and are used for subsequent weighting; the third formula means that the adjusted low-weight particle state is the particle state before adjustment, plus the product of the adjustment coefficient, weight, and distance difference.

3. The UAV cooperative positioning method under satellite denial environment according to claim 1, characterized in that, In step 2, the selectively modified adaptive particle filter algorithm is processed as follows: Step 2.

1. At the initial time t=0, randomly select N particles from the prior distribution and assign each particle the same weight to represent the various position states that the UAV may appear in, where N is a natural number; Step 2.

2. For each time step Based on the system's state, state propagation is performed on each particle, that is, the next flight positioning is inferred based on past position information and current motion state; Step 2.

3. Based on the obtained observations of the UAV to be assisted and the state of each particle, update the weight of each particle, and then normalize the updated weight of each particle. Step 2.

4. Introduce a high-weighted particle fine-tuning strategy to fine-tune the particle state; Step 2.

5. Resample the particles after state fine-tuning. Based on the particle weights, resample N particles so that particles with high weights are copied multiple times and particles with low weights are eliminated. Step 2.

6. State estimation; Based on the particle set after the update (i.e., after resampling in step 2.5), the state estimate of the system is calculated using a weighted average method to deduce the most likely location of the UAV. Step 2.

7. Increment t by 1 and repeat steps 2.2 to 2.6 until all time steps are completed, obtaining the precise position after filtering at each time step, and thus obtaining the flight trajectory of the UAV to be assisted.

4. The UAV cooperative positioning method under satellite denial environment according to claim 3, characterized in that, In step 2.1, the actual position of the UAV to be assisted is known. for: The initial state of each particle is generated by adding random variables near its actual position. For the i-th particle, its initial position has three components along the x, y, and z directions: ； in For random variables, , , This indicates the position of the drone to be assisted at time 1. This represents the position of the i-th particle in three dimensions at time 1. In step 2.2, the process of state propagation for each particle is as follows: ； in Let i be the position of the i-th particle in three dimensions at time t; , , Let i be the position of the i-th particle in three dimensions at time t-1; , , The velocity information at time t-1; In step 2.3, let the observation value obtained by the assisted UAV be... The particles are The weight formula for each particle is updated as follows: ； in, As weight, To represent the degree of similarity between the observed values ​​and the particles, here, Calculated using Euclidean distance and The distance between them To measure the standard deviation of noise.

5. The UAV cooperative positioning method under satellite denial environment according to claim 4, characterized in that, In step 2.6, the formula for calculating the system state estimate using a weighted average method is as follows: ； in, This represents the weight of the i-th particle at time t. This is the state estimate of the system.

6. The UAV cooperative positioning method under satellite denial environment according to claim 1, characterized in that, In step 1, the distance sensor calculates the distance between each auxiliary drone and the drone to be assisted based on the signal arrival time; and obtains the distance from the drone to be assisted to the three auxiliary drones according to the Euclidean distance formula. The least squares method was used to solve the problem, and the position of the UAV to be assisted was finally calculated.

7. A collaborative positioning system for unmanned aerial vehicles (UAVs) in a satellite-denied environment, comprising a ground control center and a swarm of UAVs; The drone swarm includes at least three auxiliary drones with a first-precision inertial navigation system and at least one drone to be assisted with a second-precision inertial navigation system, with distance sensors installed on the auxiliary drones; in, The accuracy of a first-precision inertial navigation system is higher than that of a second-precision inertial navigation system. The ground control center is equipped with computer equipment; the position information of the assisted UAV and the distance information between it and the UAV to be assisted are transmitted to the computer equipment at the ground control center. The computer device includes memory and one or more processors; The memory stores executable code, characterized in that when the processor executes the executable code, it is used to implement the steps of the UAV cooperative positioning method under satellite denial environment as described in any one of claims 1 to 6.

Images