Adaptive cooperative positioning method based on UWB and IMU fusion

Abstract

The invention discloses a self-adaptive cooperative positioning method based on UWB (Ultra Wideband) and IMU (Inertial Measurement Unit) fusion. The method comprises the steps of firstly initializing an observation network and a target state; then dynamically screening and adjusting the observation base stations based on the observation quality of the base stations, and adaptively adjusting the observation noise covariance in combination with the self-positioning uncertainty of the mobile nodes; then, a double-layer particle filtering framework is constructed, an upper layer samples a base station subset and outputs observation constraints, a lower layer executes target state particle filtering under the constraints, and self-adaptive probability data association is introduced to process multi-target observation uncertainty; and finally outputting a target positioning result. According to the invention, robust and high-precision positioning under the unstable condition of the observation network is realized.

Description

Technical Field

[0001] This invention belongs to the field of positioning technology, and in particular relates to an adaptive cooperative positioning method based on the fusion of UWB and IMU. Background Technology

[0002] In complex indoor environments such as mines and underground tunnels, where global satellite navigation signals are blocked, real-time accurate positioning methods for moving targets are shifting towards local wireless positioning technologies such as ultra-wideband (UWB), Wi-Fi fingerprint positioning, and inertial navigation systems. Positioning schemes based on UWB ranging combined with filtering algorithms are widely used due to their balance between cost and accuracy.

[0003] However, in practical deployments, especially in complex scenarios involving multiple targets, multipath effects, and non-line-of-sight propagation, existing technologies face significant challenges. First, fixed base stations may experience decreased observation quality or even failure due to environmental changes, equipment malfunctions, or electromagnetic interference, leading to reduced network reliability. Second, when multiple targets are moving simultaneously, traditional filtering algorithms (such as extended Kalman filtering, unscented Kalman filtering, or standard particle filtering) and data association methods (such as probabilistic data association) are prone to state estimation divergence due to observation confusion and erroneous associations, resulting in a sharp deterioration in positioning accuracy. Existing solutions often employ static networks and single filtering structures, lacking the ability to adaptively adjust observations when the observation quality of some base stations changes dynamically. They also cannot collaboratively handle base station optimization and multi-target state estimation problems, making it difficult to maintain continuous, stable, and high-precision positioning performance in harsh environments.

[0004] Therefore, there is an urgent need for a robust localization method that can adaptively respond to dynamic changes in the observation network and effectively handle multi-target interference. Summary of the Invention

[0005] This invention proposes an adaptive cooperative localization method based on the fusion of UWB and IMU to solve the problems existing in the prior art.

[0006] To achieve the above objectives, this invention provides an adaptive cooperative localization method based on UWB and IMU fusion, comprising the following steps: Step S1: Initialize the target state vector, observation model, and observation network consisting of fixed base stations and candidate mobile base stations; Step S2: Based on the observation quality of the base stations in the observation network, dynamically select the base stations used for target observation and adaptively adjust the observation noise covariance of the selected base stations; Step S3: Construct a two-layer filtering framework, including: S31: At the upper layer, the observation density and prior weights are calculated based on the selected base station and its adjusted observation noise covariance. S32: In the lower layer, based on the constraints of observation density and prior weights, the target state is estimated by particle filtering, and during the particle filtering estimation process, adaptive probabilistic data association is performed to calculate the association probability between observation and target. Step S4: Using the fused data of UWB and IMU, perform self-localization on the mobile node acting as a base station, and output the uncertainty information of self-localization to update the observation noise covariance; Step S5: Based on the particle filter estimation results in step S32, obtain the localization results of each target.

[0007] Optionally, step S2 includes: Calculate the observation weights of base stations in the observation network; When the observation weight of a base station is lower than a set threshold, a mobile node is selected from the candidate mobile base stations to replace the base station whose observation weight is lower than the set threshold, based on the geometric relationship between the target and the base station and the target's motion state. The observation noise covariance of the base station after substitution is amplified based on the self-positioning uncertainty of the selected mobile node.

[0008] Optionally, the observation weights of the base station are calculated as a function based on the following factors: the distance between the base station and the target, the geometric configuration of the base station relative to the target, the historical measurement error of the base station, and the stability of the mobile node as the base station.

[0009] Optionally, selecting a mobile node from candidate mobile base stations includes: Calculate the distance from the target to the low-quality base station and the target's motion direction vector; Targets whose movement is directed toward low-quality base stations are filtered out. From the selected targets, the target closest to the low-quality base station is chosen as the mobile node to replace the low-quality base station.

[0010] Optionally, step S31 includes: The selection state of the base stations is sampled using the first group of particles, and the state of each particle represents that a subset of base stations is selected. Based on the observation data of the selected base station subset, the joint likelihood probability of each particle is calculated, and the particle weights are updated accordingly. Based on the subset of base stations corresponding to the particle with the highest weight, determine the effective set of base stations and their observation covariance for target state estimation.

[0011] Optionally, step S32 includes: A second set of particles is used for state estimation for each target; Based on the observation data of the effective base station set, calculate the observation likelihood of each particle in the second group of particles; The weights of the second group of particles are adjusted by combining the correlation probability calculated from the adaptive probability data. The state of the target is estimated based on the second group of particles after weight adjustment.

[0012] Optional adaptive probabilistic data association includes: Based on the predicted value of the target state and its covariance, calculate the predicted observation vector and the corresponding observation prediction covariance; Set an association threshold and filter valid observations based on the association threshold; For each observation that passes the screening, calculate its association probability from the corresponding target; Based on the association probability, the observation likelihood calculation used for weight update in particle filter estimation is adjusted.

[0013] Optionally, step S4 includes: State prediction is performed based on IMU data to obtain the prior state and prior covariance of the mobile node. Acquire UWB ranging observation data between the mobile node and multiple fixed anchor points; Adjust the corresponding ranging observation covariance based on the geometric quality of the UWB ranging observation data; Based on the adjusted ranging observation covariance, the prior state is corrected using Kalman update to obtain the self-localization result of the moving node and its uncertainty.

[0014] Optional adjustments to the ranging observation covariance include: Calculate the geometric quality score based on the azimuth distribution and distance uniformity of the fixed anchor points providing distance measurement observations; The variance of the base distance measurement is adjusted based on the geometric quality score, where the worse the geometric quality, the larger the adjusted observation covariance.

[0015] Compared with the prior art, the present invention has the following advantages and technical effects: This invention effectively overcomes the problem of degraded positioning performance caused by the degradation of observation base station performance and multi-target interference in complex environments by introducing a dynamic base station optimization mechanism, a two-layer particle filter framework, and an adaptive probabilistic data association synergistic technical solution, achieving significant technical results. Specifically, this invention achieves continuous, robust, and high-precision positioning and tracking of moving targets. In simulations and field tests, the system's average positioning error remains at a low level. In tests simulating the gradual degradation of fixed base station performance, even if multiple base stations fail sequentially, the positioning accuracy and positioning failure rate (PDR) of this method remain stable without significant deterioration, demonstrating superior environmental adaptability and robustness. Especially under extreme conditions of high noise and multi-target intersection, this method achieves a significant performance improvement in key indicators such as positioning failure rate compared to traditional observation algorithms, significantly reducing the risk of positioning failure caused by fluctuations in observation quality and data association errors. Attached Figure Description

[0016] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings: Figure 1 This is a flowchart of the DLPF-APDA implementation of an embodiment of the present invention; Figure 2 This is a flowchart of the APDA process according to an embodiment of the present invention; Figure 3 This is a diagram of the UWB+IMU self-localization framework according to an embodiment of the present invention. Detailed Implementation

[0017] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0018] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0019] Example 1 like Figures 1-3 As shown, this embodiment provides an adaptive cooperative localization method based on UWB and IMU fusion, including the following steps: Step S1: Initialize the target state vector, observation model, and observation network consisting of fixed base stations and candidate mobile base stations; Specifically, construct the target state vector of the constant velocity (CV) mode. ,in Indicates the first The goal is at a certain moment Location, The corresponding velocity components are represented; the observation model is established as a joint range-azimuth measurement; a set of fixed base stations and a set of candidate mobile base stations are defined; the IMU state prediction equation, the UWB ranging error model, and the noise covariance adaptive rule are set.

[0020] Step S2: Based on the observation quality of the base stations in the observation network, dynamically select the base stations used for target observation and adaptively adjust the observation noise covariance of the selected base stations; Specifically, during normal system operation, the observation weights of each base station are calculated in real time. When a base station's weight is detected to be consistently below a set threshold and a more observable moving target exists, a candidate dynamic base station set is constructed based on the target-base station geometric relationship, motion state, and GDOP. A mobile node carrying a UWB-IMU module replaces the low-weight base station, forming a temporary observation site. The observation noise covariance is adaptively amplified according to the mobile station's self-localization uncertainty. (1) in For the uncertainty of the mobile base station location, Based on the observation noise covariance, this adaptive adjustment mechanism ensures that the observation quality of the mobile base station matches its positioning accuracy, avoiding a decline in observation quality due to its own positioning error.

[0021] Furthermore, adaptively adjusting the observation noise covariance of the selected base station includes: calculating the observation weights of the base stations in the observation network; When the observation weight of a base station is lower than a set threshold, a mobile node is selected from the candidate mobile base stations to replace the base station whose observation weight is lower than the set threshold, based on the geometric relationship between the target and the base station and the target's motion state. The observation noise covariance of the base station after substitution is amplified based on the self-positioning uncertainty of the selected mobile node.

[0022] The observation weights of the base station are calculated as a function of the following factors: the distance between the base station and the target, the geometric configuration of the base station relative to the target, the historical measurement error of the base station, and the stability of the mobile node acting as the base station.

[0023] The selection of mobile nodes from candidate mobile base stations includes: Calculate the distance from the target to the low-quality base station and the target's motion direction vector; Targets whose movement is directed toward low-quality base stations are filtered out. From the selected targets, the target closest to the low-quality base station is chosen as the mobile node to replace the low-quality base station.

[0024] Step S3: Construct a two-layer filtering framework, including: S31: At the upper layer, based on the selected base stations and their adjusted observation noise covariance, the observation density and prior weights are calculated; specifically, a small number of particles are used to sample "base station subset selection and weighting," and the weighted observation covariance is obtained based on joint likelihood and geometric reliability assessment. The output is the observation density and prior weight that satisfy the probability threshold.

[0025] Specifically, the selection state of the base stations is sampled using the first group of particles, and the state of each particle represents a subset of base stations that are selected; Based on the observation data of the selected base station subset, the joint likelihood probability of each particle is calculated, and the particle weights are updated accordingly. Based on the subset of base stations corresponding to the particle with the highest weight, determine the effective set of base stations and their observation covariance for target state estimation.

[0026] S32: In the lower layer, based on the constraints of observation density and prior weights, particle filtering estimation is performed on the target state. During the particle filtering estimation process, adaptive probabilistic data association is performed to calculate the association probability between the observation and the target. Specifically, under the constraint of the observation block output by the selection layer, particle filtering / residual consistency check / resampling is performed on the target state. When updating the measurement, adaptive PDA (APDA) is introduced to inject three types of weights, namely dynamic, static and geometric reliability, into the association threshold and weight calculation to suppress target intersection and spurious measurements.

[0027] Specifically, a second set of particles is used for state estimation for each target; Based on the observation data of the effective base station set, calculate the observation likelihood of each particle in the second group of particles; The weights of the second group of particles are adjusted by combining the correlation probability calculated from the adaptive probability data. The state of the target is estimated based on the second group of particles after weight adjustment.

[0028] Furthermore, adaptive probabilistic data association includes: Based on the predicted value of the target state and its covariance, calculate the predicted observation vector and the corresponding observation prediction covariance; Set an association threshold and filter valid observations based on the association threshold; For each observation that passes the screening, calculate its association probability from the corresponding target; Based on the association probability, the observation likelihood calculation used for weight update in particle filter estimation is adjusted.

[0029] Step S4: Using the fused data from UWB and IMU, the mobile node, acting as a base station, performs self-localization and outputs the uncertainty information of self-localization to update the observation noise covariance; specifically: the mobile station uses IMU forward integral prediction and UWB ranging to update, outputting the position and covariance; its covariance is injected into S2. This could affect the overall reliability of the measurement.

[0030] Specifically, state prediction is performed based on IMU data to obtain the prior state and prior covariance of the mobile node; Acquire UWB ranging observation data between the mobile node and multiple fixed anchor points; Adjust the corresponding ranging observation covariance based on the geometric quality of the UWB ranging observation data; Based on the adjusted ranging observation covariance, the prior state is corrected using Kalman update to obtain the self-localization result of the moving node and its uncertainty.

[0031] Furthermore, adjusting the ranging observation covariance includes: Calculate the geometric quality score based on the azimuth distribution and distance uniformity of the fixed anchor points providing distance measurement observations; The variance of the base distance measurement is adjusted based on the geometric quality score, where the worse the geometric quality, the larger the adjusted observation covariance.

[0032] Step S5: Based on the particle filter estimation results in step S32, obtain the positioning results of each target, and output the position estimate and confidence interval of each target; dynamically adjust the active base station set based on the historical observation residuals, effective particle count and PDR index to maintain the observation network.

[0033] This embodiment also includes the following operations: The following example, using a mining environment with 4 moving targets and 5 fixed base stations, illustrates the implementation process of this invention: I. System Initialization: Deploy 5 fixed base stations and 4 targets carrying UWB tags and IMU modules; II. Base station observation quality detection: At t=37s, a sudden drop in observation quality was detected for base stations 2 and 5; III. Dynamic Base Station Optimization: 1. Calculate the observability score of each target relative to the low-weight base station; (2) 2. Based on a comprehensive evaluation of geometric distance, direction of motion, and target's own positioning accuracy, the target with the highest observability is selected as the dynamic base station; (1) Determine the direction vector of inefficient base stations; For each inefficient base station Its location is Calculate the direction vector of the inefficient base station relative to the scene center. for: (3) in The parameters represent the boundary range of the scene, followed by the direction vector. It is normalized to a unit vector.

[0034] (2) Calculate the target's motion vector; set up It is the state estimated by particle filtering for each target. Its current estimated position is The speed is The target's motion vector Defined as: (4) (3) Update the base station observation network and replace low-weight base stations with selected dynamic base stations; Calculate the target by traversing all targets. to inefficient base stations distance for: (5) Simultaneously, calculate the dot product between the target motion vector and the inefficient base station direction vector: (6) Select candidate targets that meet the following conditions: 1) Distance Minimum; 2) That is, the target's movement direction is towards the inefficient base station (ensuring the target can approach the location of the inefficient base station, enhancing the feasibility of replacement), and the candidate target that is closest and meets the direction condition is selected. As a dynamic base station, the mathematical expression for the entire process is: (7) 3) Physical conditions: The robot's battery level is greater than 60%, the signal strength is strong, and it is located on a road section without turns.

[0035] (4) Update the dynamic base station location; Select target Afterwards, inefficient base stations Location updated to target Estimated location: (8) IV. DLPF-APDA Filtering: 1. Upper-layer output base station subset and its weights; (1) Particle initialization; coarse-grained layer use There are 3 particles, each representing a possible state chosen by the base station. The state vector of a particle is defined as follows: ,in .vector The Each component Indicates base station Selected This indicates that the particle was not selected. Initially, particles are generated randomly to ensure that each particle selects a certain number of base stations. That is, at least one base station must be selected.

[0036] (2) Particle propagation; Due to base station selection status Relatively stable in time, particle propagation employs a simple random perturbation strategy: random flipping with a probability of 0.1. A certain component (i.e., 01 swap) is selected, while ensuring that at least one base station is selected.

[0037] (3) Weight update; The weight assignment for each particle is calculated based on the observation likelihood of its chosen base station. The selected set of base stations is Using the observation data from these base stations Calculate all targets The joint likelihood probability. Assuming observation noise is independent, the weights... Update as follows: (9) For target-based Current estimated state The predicted observation of d, To observe the noise variance, adjustments are made based on the dynamic base station. After weight normalization, the particle with the highest weight is selected. The corresponding set of base stations Used for subsequent target state estimation.

[0038] 2. The lower layer performs state prediction and updates, and correlates probabilistic data; (1) Particle initialization; Fine-grained layers for each target use There are 10 particles, initially, the particles start from the initial state of the target. Randomly generated from nearby areas, with Gaussian noise added. Based on the state vector defined in the uniform velocity model in the previous section, the initial state of the particle is: , .

[0039] (2) Particle propagation; The state transition equation of a particle: (10) (11) In the formula, F, G, and Q are defined as shown in section 2.1.2. To improve the accuracy of particle propagation, the true velocity of the target at the previous moment is used: (12) (13) (3) Weight update; Base stations selected based on coarse-grained layers Calculate each target If the observed likelihood probability of a particle is given, then the actual observation vector is... ,in Predict the observed vector j is based on particle states and observation model The calculation yields the following formulas for predicting the observation vector and updating the weights: (14) (15) In the formula, It is The joint noise covariance matrix of multi-base station observations represents the independence of the noise observed by each base station. yes The identity matrix, after weight normalization, yields the target state estimate as follows: (16) 3. Adaptive probabilistic data association; In mining environments, multi-target tracking faces challenges such as observation interference and erroneous associations. For example, target overlap, occlusion, or noise interference can cause deviations in the matching of observation data with targets. To address this issue, this embodiment introduces an adaptive probabilistic data association module. By calculating the association probability between observations and targets, it mitigates the uncertainty of data association and further improves the robustness and accuracy of multi-target tracking.

[0040] At any moment The set of base stations selected by the coarse-grained layer is For each target and low-quality base stations Provide an observation vector However, due to the complexity of multi-target tracking scenarios, the sources of observation information may include the following: observation information From the corresponding target (Correct association); Observation information From a non-corresponding target (erroneous association); observation information It could be clutter or noise (spurious observations). Given the uncertainty surrounding the source of these observations, the PDA module will calculate the probability of each observation using a probabilistic model. With the goal Association probability The calculated probabilities are then used to adjust the weights of the particle filter appropriately.

[0041] (1) Predict the observation vector and distance threshold; Based on fine-grained layered particle filtering for target The estimated state is The predicted observation vector is calculated. The state covariance of the target can be approximated by the particle variance as follows: (17) Observational prediction covariance matrix This reflects the uncertainty of the predicted observations: (18) In the formula, It is the observation function exist The Jacobian matrix at that location.

[0042] Set a Mahalanobis distance threshold to avoid irrelevant observations and calculate the observation vector. Mahalanobis distance, (19) in , Let be the inverse cumulative distribution function of the chi-square distribution, with 2 degrees of freedom (dimensionality of the observation vector). Only when... Observations are accepted if they are made in real time; otherwise, they are considered clutter.

[0043] (2) Calculation of association probability; Calculate the threshold With the goal Association probability PDA assumes that the observation may be a true observation of the target or clutter, and the correlation probability is calculated based on a Bayesian probability model: (20) In the formula, The detection probability represents the prior probability that the observation originates from the target; Here, clutter density represents the probability density of spurious observations; Let be a Gaussian probability density function. Furthermore, the non-correlated probability (i.e., the probability that the observation is clutter) is defined as: This ensures that the sum of all associated probabilities is 1.

[0044] (3) Particle weight adjustment; Using association probability Adjust the weights of the fine-grained layer particle filter. For each particle... The predicted observation vector is Then each base station The likelihood probability is: (twenty one) By comprehensively calculating the likelihood probabilities of all base stations, the particle weights are updated as follows: (twenty two) After weight normalization, the target state estimate is recalculated as follows: (twenty three) V. UWB-IMU Fusion Self-Localization: Dynamic base stations achieve self-localization through IMU and UWB ranging, and inject their uncertainty into the observation noise model; 1. IMU propagation model; First, define the mobile base station state vector, which includes position, velocity, Euler angles, and angular velocity, together forming the estimator: (twenty four) IMU propagation is based on discretized inertial navigation kinematics, utilizing the measured specific force and angular velocity over a time step. Extrapolation prior states: (25) (26) in It involves performing attitude integration and acceleration integration in the global coordinate system. Let be the process noise covariance.

[0045] To reflect the increase in uncertainty caused by maneuvering, a time-varying process noise scaling factor is introduced: (27) (28) in It is a scaling factor for maneuver intensity. It is the mobility strength coefficient and .

[0046] 2. UWB ranging observation and joint observation model; UWB provides absolute measurements of location. Let... For a moment Available anchor point set, for the first Given the coordinates of several anchor points, the distance measurement for a single anchor point is: (29) (30) in It is the dynamic base station location covariance. It is the variance of the basic distance measurement.

[0047] Will Measurements within the range are merged into a joint observation vector in a fixed order, resulting in: (31) (32) (33) The linearized Jacobian for the position components is: Thus, the observation matrix is ​​constructed. .

[0048] 3. Geometric and operational condition adjustments for measuring covariance; The uncertainty in distance measurement is affected by the geometry of the anchor point and the motion conditions. Let... Let represent the geometric quality score obtained from the anchor point orientation distribution and distance uniformity, and let To represent the weights under high dynamic / normal operating conditions, the joint measurement covariance is set to... The worse the geometric quality or the more dynamic the operating conditions, The corresponding increase will reduce the impact of this batch of measurements in subsequent updates: (34) 4. Consistency check and standardized residuals; To ensure the robustness of the update, a consistency check is performed on the joint residual vector and its covariance matrix. First, define: (35) (36) Reconstructing Standardized Residuals When the absolute value of a component exceeds a given threshold, its influence is weakened in the subsequent gain calculation, i.e., a larger equivalent covariance is used for that component to reduce the impact of random errors on the estimation.

[0049] 5. Kalman update for linearized ranging; Under the aforementioned prior and observation conditions, a standard Kalman update is employed: (37) (38) (39) To ensure numerical stability, for Symmetry is applied and a lower bound for the minimum eigenvalue is set under spectral decomposition to maintain positive definiteness.

[0050] 6. Boundary conditions and upper velocity bound; Considering prior knowledge of the scene, constraints are imposed on position and velocity to avoid incomprehensible behavior: (40) (41) If an out-of-bounds or numerical anomaly is detected, a gentle backoff is performed using the previous state as a reference, and the covariance is increased accordingly to reflect the increase in uncertainty. The uncertainty of the mobile base station enters DLPF-APDA in two standard forms: First, when participating in the localization of other targets as a station, the observation covariance of that station is scaled according to the position covariance, thereby reducing its impact in the joint observation density; Second, the position / velocity variance is mapped to the mobile station term in the station weights, and after normalization, it is jointly determined with distance, GDOP, historical error, etc. .

[0051] VI. Output positioning results: Real-time output of position estimates for each target and visualization of the trajectory.

[0052] In simulations and field measurements, the average RMSE is approximately 0.52 ± 0.05 m. In progressive inefficiency experiments, even with three base stations becoming progressively less efficient, the RMSE of this method remains stable at 0.50–0.60 m, and the PDR is consistently below 30%, without a significant increase. In experiments comparing noise levels at different multiples, the filtered PDR is approximately 10.3%–13.6%. This effect is directly generated by the combined technical features of "dynamic base station optimization + dual-layer particle filter hierarchical solution + adaptive PDA and UWB-IMU uncertainty injection".

[0053] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. An adaptive cooperative localization method based on UWB and IMU fusion, characterized in that, Includes the following steps: Step S1: Initialize the target state vector, observation model, and observation network consisting of fixed base stations and candidate mobile base stations; Step S2: Based on the observation quality of the base stations in the observation network, dynamically select the base stations used for target observation and adaptively adjust the observation noise covariance of the selected base stations; Step S3: Construct a two-layer filtering framework, including: S31: At the upper layer, the observation density and prior weights are calculated based on the selected base station and its adjusted observation noise covariance. S32: In the lower layer, based on the constraints of observation density and prior weights, the target state is estimated by particle filtering, and during the particle filtering estimation process, adaptive probabilistic data association is performed to calculate the association probability between observation and target. Step S4: Using the fused data of UWB and IMU, perform self-localization on the mobile node acting as a base station, and output the uncertainty information of self-localization to update the observation noise covariance; Step S5: Based on the particle filter estimation results in step S32, obtain the localization results of each target.

2. The method according to claim 1, characterized in that, Step S2 includes: Calculate the observation weights of base stations in the observation network; When the observation weight of a base station is lower than a set threshold, a mobile node is selected from the candidate mobile base stations to replace the low-quality base station whose observation weight is lower than the set threshold, based on the geometric relationship between the target and the base station and the target's motion state. The observation noise covariance of the base station after substitution is amplified based on the self-positioning uncertainty of the selected mobile node.

3. The method according to claim 2, characterized in that, The observation weights of the base station are calculated as a function of the following factors: the distance between the base station and the target, the geometric configuration of the base station relative to the target, the historical measurement error of the base station, and the stability of the mobile node acting as the base station.

4. The method according to claim 2, characterized in that, The selection of mobile nodes from candidate mobile base stations includes: Calculate the distance from the target to the low-quality base station and the target's motion direction vector; Targets whose movement is directed toward low-quality base stations are filtered out. From the selected targets, the target closest to the low-quality base station is chosen as the mobile node to replace the low-quality base station.

5. The method according to claim 1, characterized in that, Step S31 includes: The selection state of the base stations is sampled using the first group of particles, and the state of each particle represents that a subset of base stations is selected. Based on the observation data of the selected base station subset, the joint likelihood probability of each particle is calculated, and the particle weights are updated accordingly. Based on the subset of base stations corresponding to the particle with the highest weight, determine the effective set of base stations and their observation covariance for target state estimation.

6. The method according to claim 5, characterized in that, Step S32 includes: A second set of particles is used for state estimation for each target; Based on the observation data of the effective base station set, calculate the observation likelihood of each particle in the second group of particles; The weights of the second group of particles are adjusted by combining the correlation probability calculated from the adaptive probability data. The state of the target is estimated based on the second group of particles after weight adjustment.

7. The method according to claim 1, characterized in that, Adaptive probabilistic data association includes: Based on the predicted value of the target state and its covariance, calculate the predicted observation vector and the corresponding observation prediction covariance; Set an association threshold and filter valid observations based on the association threshold; For each observation that passes the screening, calculate its association probability from the corresponding target; Based on the association probability, the observation likelihood calculation used for weight update in particle filter estimation is adjusted.

8. The method according to claim 1, characterized in that, Step S4 includes: State prediction is performed based on IMU data to obtain the prior state and prior covariance of the mobile node. Acquire UWB ranging observation data between the mobile node and multiple fixed anchor points; Adjust the corresponding ranging observation covariance based on the geometric quality of the UWB ranging observation data; Based on the adjusted ranging observation covariance, the prior state is corrected using extended Kalman update to obtain the self-localization result of the moving node and its uncertainty.

9. The method according to claim 8, characterized in that, Adjusting the distance measurement observation covariance includes: Calculate the geometric quality score based on the azimuth distribution and distance uniformity of the fixed anchor points providing distance measurement observations; The variance of the base distance measurement is adjusted based on the geometric quality score, where the worse the geometric quality, the larger the adjusted observation covariance.

Images