CN-121978620-A - Self-adaptive cooperative positioning method based on UWB and IMU fusion

CN121978620ACN 121978620 ACN121978620 ACN 121978620ACN-121978620-A

Abstract

The invention discloses a self-adaptive co-location method based on UWB and IMU fusion. The method comprises the steps of initializing an observation network and a target state, dynamically screening and adjusting an observation base station based on the observation quality of the base station, adaptively adjusting the covariance of observation noise by combining the self-positioning uncertainty of a mobile node, constructing a double-layer particle filtering frame, sampling a subset of the base station by an upper layer, outputting observation constraint, performing particle filtering of the target state by a lower layer under the constraint, introducing self-adaptive probability data association to process multi-target observation uncertainty, and finally outputting a target positioning result. The invention realizes the robust and high-precision positioning under the unstable condition of the observation network.

Inventors

CHEN GONGMING
QU YUANYUAN
WANG ZIYI
LV JIAYI

Assignees

中国矿业大学(北京)

Dates

Publication Date: 20260505
Application Date: 20260206

Claims (9)

1. The self-adaptive co-location method based on the fusion of UWB and IMU is characterized by comprising the following steps: step S1, initializing a target state vector, an observation model and an observation network consisting of a fixed base station and a candidate mobile base station; Step S2, dynamically selecting a base station for target observation based on the observation quality of the base station in the observation network, and adaptively adjusting the observation noise covariance of the selected base station; step S3, constructing a double-layer filtering frame, which comprises the following steps: S31, calculating the observation density and the prior weight based on the selected base station and the adjusted observation noise covariance of the base station at the upper layer; S32, in the lower layer, carrying out particle filter estimation on the target state based on the constraint of the observation density and the prior weight, and executing self-adaptive probability data association in the particle filter estimation process to calculate the association probability between the observation and the target; S4, utilizing the fusion data of UWB and IMU to self-locate the mobile node as base station, and outputting self-locating uncertainty information for updating the observed noise covariance; and S5, acquiring a positioning result of each target according to the result of the particle filtering estimation in the step S32.
2. The method according to claim 1, wherein step S2 comprises: Calculating the observation weight of a base station in an observation network; When the observation weight of the base station is lower than a set threshold value, selecting a mobile node from candidate mobile base stations based on the geometric relation between the target and the base station and the motion state of the target to replace a low-quality base station with the observation weight lower than the set threshold value; and amplifying the observed noise covariance of the base station after substitution according to the self-positioning uncertainty of the selected mobile node.
3. The method of claim 2, wherein calculating the observation weight of the base station is a function of a distance between the base station and the target, a geometry of the base station relative to the target, a historical measurement error of the base station, and stability of the mobile node as the base station.
4. The method of claim 2, wherein selecting the mobile node from the candidate mobile base stations comprises: calculating the distance from the target to the low-quality base station and the motion direction vector of the target; Screening targets with movement directions towards low-quality base stations; from the screened targets, the target closest to the low-quality base station is selected as the mobile node to replace the low-quality base station.
5. The method according to claim 1, wherein step S31 comprises: Sampling the selection state of the base station by adopting a first group of particles, wherein the state of each particle represents that one base station subset is selected; Based on the observation data of the selected base station subset, calculating the joint likelihood probability of each particle, and updating the particle weight according to the joint likelihood probability; And determining an effective base station set for target state estimation and an observation covariance thereof according to the base station subset corresponding to the particles with the highest weight.
6. The method of claim 5, wherein step S32 includes: Performing state estimation for each target using a second set of particles; calculating the observation likelihood of each particle in the second group of particles based on the observation data of the effective base station set; the association probability calculated by associating the self-adaptive probability data is combined, and the weight of the second group of particles is adjusted; And estimating the state of the target according to the second group of particles after the weight adjustment.
7. The method of claim 1, wherein the adaptive probability data association comprises: calculating a predicted observation vector and a corresponding observed predicted covariance based on the predicted value and covariance of the target state; Setting an association threshold, and screening effective observation based on the association threshold; for each observation passing the screening, calculating the associated probability that it originates from the corresponding target; And adjusting the observation likelihood calculation for weight updating in the particle filter estimation based on the associated probability.
8. The method according to claim 1, wherein step S4 comprises: Performing state prediction based on IMU data to obtain a priori state and a priori covariance of the mobile node; acquiring UWB ranging observation data between a mobile node and a plurality of fixed anchor points; according to the geometric quality of UWB ranging observation data, adjusting the corresponding ranging observation covariance; Based on the adjusted range finding covariance, the prior state is corrected by using extended Kalman updating, and a self-positioning result and uncertainty of the mobile node are obtained.
9. The method of claim 8, wherein adjusting a ranging observation covariance comprises: Calculating a geometric quality score according to the azimuth distribution and the distance balance of the fixed anchor points for providing ranging observation; and adjusting the basic ranging variance according to the geometric quality score, wherein the worse the geometric quality is, the larger the adjusted observation covariance is.

Description

Self-adaptive cooperative positioning method based on UWB and IMU fusion Technical Field The invention belongs to the technical field of positioning, and particularly relates to a self-adaptive cooperative positioning method based on UWB and IMU fusion. Background In complex indoor environments such as mines, underground tunnels and the like, global satellite navigation signals are refused, and a real-time accurate positioning method of a mobile target is transferred to local wireless positioning technologies such as Ultra Wideband (UWB), WIFI fingerprint positioning, an inertial navigation system and the like. Positioning schemes based on UWB ranging combined with filtering algorithms are widely used due to the balance between cost and accuracy. However, in practical deployments, especially in complex scenarios where there are multiple objectives, multipath effects, and non-direct view propagation, the prior art faces significant challenges. First, a fixedly deployed base station may experience degradation or even failure in the quality of observation due to environmental changes, equipment failure or electromagnetic interference, resulting in reduced reliability of the observed network. Secondly, during multi-target cross motion, the traditional filtering algorithm (such as extended Kalman filtering, unscented Kalman filtering or standard particle filtering) and the data association method (such as probability data association) are easy to generate state estimation divergence due to observation confusion and error association, and the positioning precision is rapidly deteriorated. The existing scheme mostly adopts a static network and a single filtering structure, lacks the capability of self-adaptive observation adjustment when the observation quality of part of base stations changes dynamically, cannot cooperatively process the problems of base station optimization and multi-target state estimation, and is difficult to maintain continuous, stable and high-precision positioning performance in severe environments. Therefore, a robust positioning method capable of adaptively coping with dynamic changes of an observation network and effectively handling multi-target interference is needed. Disclosure of Invention The invention provides a self-adaptive cooperative positioning method based on UWB and IMU fusion, which aims to solve the problems existing in the prior art. In order to achieve the above purpose, the invention provides a self-adaptive co-location method based on UWB and IMU fusion, comprising the following steps: step S1, initializing a target state vector, an observation model and an observation network consisting of a fixed base station and a candidate mobile base station; Step S2, dynamically selecting a base station for target observation based on the observation quality of the base station in the observation network, and adaptively adjusting the observation noise covariance of the selected base station; step S3, constructing a double-layer filtering frame, which comprises the following steps: S31, calculating the observation density and the prior weight based on the selected base station and the adjusted observation noise covariance of the base station at the upper layer; S32, in the lower layer, carrying out particle filter estimation on the target state based on the constraint of the observation density and the prior weight, and executing self-adaptive probability data association in the particle filter estimation process to calculate the association probability between the observation and the target; S4, utilizing the fusion data of UWB and IMU to self-locate the mobile node as base station, and outputting self-locating uncertainty information for updating the observed noise covariance; and S5, acquiring a positioning result of each target according to the result of the particle filtering estimation in the step S32. Optionally, step S2 includes: Calculating the observation weight of a base station in an observation network; When the observation weight of the base station is lower than a set threshold value, selecting a mobile node from the candidate mobile base stations based on the geometric relation between the target and the base station and the motion state of the target so as to replace the base station with the observation weight lower than the set threshold value; and amplifying the observed noise covariance of the base station after substitution according to the self-positioning uncertainty of the selected mobile node. Optionally, the observation weights for the base stations are calculated as a function of the distance between the base station and the target, the geometry of the base station relative to the target, the historical measurement errors of the base station, the stability of the mobile node as a base station. Optionally, selecting the mobile node from the candidate mobile base stations includes: calculating the distance from the target to the low-quality base station a