UWB / IMU fusion positioning method based on environment judgment

By using the PSO-IAUKF algorithm to process UWB/IMU fusion positioning, the problems of large signal noise impact of UWB in non-line-of-sight environments and cumulative error of IMU are solved, achieving high-precision and stable indoor positioning.

CN118913262APending Publication Date: 2024-11-08XUZHOU NORMAL UNIVERSITY
View PDF 0 Cites 4 Cited by

Patent Information

Application Number
CN202411081410.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-07
Publication Date
2024-11-08

AI Technical Summary

Technical Problem

In indoor environments, UWB positioning technology is greatly affected by signal noise in non-line-of-sight environments, resulting in jitter in positioning results, while IMU positioning technology has a large cumulative error and a decrease in positioning accuracy.

Method used

The PSO-IAUKF algorithm based on environment judgment is adopted. It processes UWB ranging information through particle swarm optimization and combines it with the IMU motion state model to perform adaptive noise matrix update and threshold judgment, suppressing filter divergence in NLOS environment and realizing UWB/IMU fusion positioning.

Benefits of technology

It improves the accuracy and stability of indoor positioning, effectively avoids the limitations of UWB and IMU in non-line-of-sight environments, and enhances the robustness of the positioning system.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118913262A_ABST
    Figure CN118913262A_ABST
Patent Text Reader

Abstract

The invention relates to a UWB / IMU fusion positioning method based on environment judgment, and the method comprises the following steps: (1) collecting the data information of UWB and IMU, including the position information of UWB and the position information, speed information and attitude information of IMU; (2) using a particle swarm optimization algorithm to optimize the position coordinates calculated according to the UWB; and (3) fusing the optimized coordinate information with carrier state information calculated by the IMU strapdown inertial navigation system, introducing an adaptive factor to update a noise matrix in real time, and adding a threshold value to judge current environment information, so that the improvement of a traditional UKF fusion positioning algorithm is realized, and the positioning precision of the positioning system in different noise environments is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to a UWB / IMU fusion positioning method based on environment judgment. Background Art

[0002] With the development of navigation technology, indoor positioning has become a hot research topic under sensor wireless communication technology, especially the positioning technology of unmanned robots, which has been developed and applied in indoor environments such as warehouses, parking lots, and airports. At present, inertial navigation system (INS), ultra-wideband technology (UWB), visual capture technology, radio frequency identification technology (RFID), etc. Compared with other positioning technologies, UWB positioning technology has high accuracy, low cost, and general algorithm complexity, and has gradually become the preferred positioning technology in indoor scenes.

[0003] UWB technology is a wireless carrier communication technology that transmits data through non-sinusoidal narrow pulses without carrier signals, and relies on high-frequency non-sinusoidal narrow pulses to transmit data, occupying a very high frequency band and having a high communication rate. In a line-of-sight (LOS) environment, UWB can achieve high-precision positioning, but in a non-line-of-sight (NLOS) environment, various forms of noise will affect UWB signal capture, causing the positioning results to jitter.

[0004] IMU positioning technology relies on sensors such as accelerometers and gyroscopes to measure state information such as acceleration and angular velocity for positioning. IMU positioning technology has the advantage of not being affected by the environment, but it will have cumulative errors over time, and the positioning accuracy will become increasingly poor.

[0005] Therefore, the use of UWB / IMU fusion positioning system, combined with the corresponding fusion positioning algorithm, can effectively avoid the limitations of UWB and IMU subsystems in positioning. Summary of the invention

[0006] 1. Technical issues to be resolved

[0007] Aiming at the robot positioning problem in indoor scenarios, the kinematic model and observation model of the robot in planar motion were constructed, and a multi-sensor positioning system integrating UWB and IMU and the corresponding indoor positioning solution were designed based on the model.

[0008] (II) Technical solution

[0009] The basic steps of the UWB / IMU fusion positioning algorithm of PSO-IAUKF based on environmental judgment are as follows:

[0010] Step 1: Establish an observation model of four UWB base stations consisting of a main base station and three secondary base stations, and use the least squares method to obtain the position coordinates (x 0,k,y 0,k );

[0011] Step 2: Establish a particle swarm optimization model to optimize the acquired UWB position coordinates and obtain the optimized coordinates (x 1,k ,y 1,k ). After obtaining the optimized coordinates, solve the distance d between the coordinates and each UWB base station. i,k (i=4), used by the subsequent fusion positioning algorithm;

[0012] Step 3: Establish IMU motion state model X k , including the position and velocity information of the carrier in the plane coordinate system;

[0013] The model data comes from the xyz three-axis acceleration measured by the accelerometer and the xyz three-axis angular velocity measured by the gyroscope. According to the acceleration and angular velocity information output by the IMU, the output carrier state information can be converted into a coordinate system and an integral transformation to solve the position and velocity estimation results of the carrier at the next moment;

[0014] Step 4: Substitute the d obtained in step 2 i,k With step 3 X k The IAUKF algorithm fusion under tight combination is carried out. The algorithm steps include: (1) sigma point sampling; (2) weight calculation; (3) prediction value update; (4) noise matrix calculation; (5) state update. Compared with the traditional UKF algorithm, IAUKF improves steps (4) and (5), establishes an adaptive noise estimator to estimate the noise matrix, adds adaptive coefficients and thresholds, so that the positioning system can judge the impact of the current noise on the environment and make different suppression responses.

[0015] The present invention proposes a UWB / IMU fusion positioning algorithm based on environmental judgment adaptive PSO-IAUKF. The particle swarm optimization algorithm is used to process the ranging information of UWB, and the new information between the measured value and the predicted value is calculated. The adaptive factor and the forgetting function are introduced to update the noise matrix at each moment. At the same time, a threshold is introduced to judge whether there is NLOS environmental noise at present, and a secondary dynamic adjustment is performed to suppress the filtering divergence of the positioning trajectory in the NLOS environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is the structure of UWB / IMU fusion positioning system;

[0017] Figure 2 This is the flow chart of particle swarm optimization algorithm;

[0018] Figure 3 This is a schematic diagram of coordinate axis transformation;

[0019] Figure 4 This is the IAUKF algorithm flow chart;

[0020] Figure 5 This is the overall algorithm flow chart. DETAILED DESCRIPTION

[0021] The present invention is further described in detail below based on the accompanying drawings and embodiments. The described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0022] A UWB / IMU fusion positioning algorithm based on PSO-IAUKF of environmental judgment includes the following steps:

[0023] Step 1: Establish an observation model of four UWB base stations consisting of one main base station and three secondary base stations. After receiving the measured distances between each UWB base station and the tag, use the least squares method to solve the position coordinates (x 0,k ,y 0,k ). The specific solution is as follows:

[0024] According to the distance formula between the UWB tag and the base station, let the measured distance between the UWB base station and the tag be d i (i=4), then the following equations can be listed:

[0025]

[0026]

[0027]

[0028]

[0029] Where (x, y) is the set UWB tag location coordinates, (x i ,y i ) are the position coordinates of the four UWB base stations. Further, the corresponding equations can be obtained:

[0030]

[0031]

[0032]

[0033] Further, the matrix equation can be established:

[0034]

[0035] Finally, the corresponding UWB tag coordinates (x, y) can be obtained.

[0036] Step 2: Establish a particle swarm optimization model to optimize the acquired UWB position coordinates. Figure 2 The algorithm flow chart shown is divided into the following steps:

[0037] (3) Particle initialization

[0038] Assume that there are n particles in the current D-dimensional space. With the position coordinates obtained by the least squares method as the center, each particle is assigned an initial position and initial velocity, which can be expressed as:

[0039] x i =(x i1 , x i2 , ..., x i )(i=1,2...n) (9)

[0040] v i =(v i1 , v i2 , ..., v i )(i=1,2...n) (10)

[0041] At the same time, the fixed weight ω, the particle's motion range and position interval, and the learning factors c1 and c2 are calibrated.

[0042] (4) Calculate the initial fitness of the particle

[0043] According to the objective function of the optimization problem, the corresponding fitness equation is established to make the particles continuously converge to the local optimal solution. This paper selects the positioning variance as the fitness equation:

[0044]

[0045] Among them, f i =s i -s1-(u i -u1),(i=1,2,3),s i is the distance between the UWB base station and the tag obtained by the PSI algorithm, u i The distance between the base station and the tag measured by the UWB hardware.

[0046] (3) Compare with the local optimal solution Pbest.

[0047] Compare the fitness value of each particle with the local optimal solution Pbest i By comparison, if the current particle is better than the local optimal solution, it will be replaced.

[0048] (4) Compare with the global optimal solution Gbest.

[0049] The local optimal solution Gbest each time i The fitness value of the particle is compared with the global optimal solution. If the current particle is better than the global optimal solution, it will be replaced.

[0050] (5) Update the particle's position and velocity

[0051]

[0052]

[0053] Among them, v ij represents the speed of the ith particle at the jth iteration, x ij represents the position coordinates of the ith particle at the jth iteration, and the range that the particle can reach is [X min ,X max ].

[0054] (6) When the fitness meets the requirements, execute step 7. If not, return to step 2 to continue iterating.

[0055] (7) Output the final result.

[0056] Since there are many kinds of environmental noise in practice, when the UWB ranging information is greatly affected, the PSO optimization algorithm is prone to large position offset. Therefore, this paper performs threshold processing on the algorithm and only uses the PSO optimization algorithm to perform a small smoothing process on the UWB received data information without correcting the large offset error.

[0057]

[0058] Thus, the optimized coordinates can be obtained

[0059] Step 3: Establish IMU motion state model X k , including the position and velocity information of the carrier in the plane coordinate system.

[0060] When the carrier is moving, the state equation can be simplified as:

[0061] X k+1 =AX k +Bu k +W k (15)

[0062] Where A is the state transfer matrix; B is the state input matrix, u k is the input data, W k is the state noise.

[0063] For the variables in the input matrix, the IMU is used for real-time observation, and the strapdown inertial navigation system (INS) is used to update the position and velocity. The accelerometer and gyroscope in the IMU are used to record the three-axis acceleration and angular velocity at each moment to deduce the position and direction of the carrier. It is divided into the following two steps:

[0064] (1) Coordinate system rotation

[0065] Since the IMU measurement value is in the carrier coordinate system, it needs to be converted into the navigation coordinate system in practical applications, such as Figure 3 shown.

[0066] The body coordinate system is now recorded as the b coordinate system, and the navigation coordinate system is the n coordinate system. It is called the rotation matrix that transforms the carrier coordinate system into the navigation coordinate system. By rotating around the z, y, and x axes at angles ψ, θ, and γ, the rotation matrix from the carrier coordinate system to the navigation coordinate system can be obtained.

[0067] (2) Derivation of kinematic equations

[0068] By rotating the matrix in step (1) The three-axis acceleration of the mobile robot measured by IMU based on the carrier coordinate system is: Then the three-axis acceleration in the navigation coordinate system can be obtained through the rotation matrix:

[0069]

[0070] Where g is the acceleration due to gravity.

[0071] If the current time is k and the time change is ΔT, then the position and velocity at time k+1 are:

[0072]

[0073]

[0074] Therefore, after determining the initial position, INS can give the estimated position and speed results of the carrier at different times.

[0075] Step 4: Combine the IAUKF algorithm obtained in step 2 with the one obtained in step 3 in a tight combination. The algorithm steps include: (1) sigma point sampling; (2) weight calculation; (3) prediction value update; (4) noise matrix calculation; (5) state update.

[0076] according to Figure 4 As shown in the algorithm flow chart, the algorithm flow designed in this paper is as follows:

[0077] (1) Sigma point sampling

[0078]

[0079]

[0080]

[0081] in, is the state estimate at time k, Initial sampling point, X i is the i-th sampling point. P is the covariance matrix, n is the dimension of the system state quantity, and λ is the scaling ratio, which determines the discrete degree of the sampling point and can be adjusted appropriately according to the actual situation.

[0082] (2) Calculate weights

[0083] Calculate the mean and covariance weights of each sampling point in the point set:

[0084]

[0085]

[0086]

[0087] Among them, W m is the weight of the sampling point, W c is the weight of the sampling point covariance; β reflects the high-order characteristics of the state history information. For Gaussian distribution information, the value can be set to 2. α takes a very small positive number, ranging from 10 -4 ≤α≤1.

[0088] (3) Update of predicted value

[0089] Substitute the sigma point in step (1) into the prediction function:

[0090]

[0091]

[0092] At the same time, the system covariance matrix is ​​calculated:

[0093]

[0094] (4) Noise matrix calculation

[0095] In different noise environments, the pseudo-range values ​​received by UWB will be affected differently. For example, the accuracy is higher in line-of-sight environments, but extreme values ​​are more likely to appear in non-line-of-sight environments. Therefore, it is difficult to ensure that the method of changing the noise matrix alone works in all environments. Therefore, this paper uses the Suga-Husa filter to update the measurement noise at each moment to achieve accurate estimation of the fusion positioning system.

[0096] The specific steps are as follows:

[0097] 1) Based on the IMU data information, calculate the predicted estimated value of the i-th sample at the k+1th time

[0098]

[0099] 2) Calculate the measurement estimate at the k+1th time

[0100]

[0101] 3) Calculate the residual ε between the measured information and the predicted information k+1 , and establish an adaptive noise estimator

[0102]

[0103]

[0104] in, b is the forgetting factor, and its value range is generally (0.95, 0.995).

[0105] 4) Estimate the noise matrix:

[0106]

[0107] From formula (33), we can see that in the second half Used to update the noise error. When the filter shows a convergence trend, HP k+1|k H T The value of will be close to 0 and can be ignored. Therefore, this formula can be simplified to:

[0108]

[0109] (5) Status Update

[0110]

[0111]

[0112] in, is the prediction covariance matrix, is the interaction covariance matrix.

[0113] For the residual in (32) Test and calculate multiple LOS environments The average value of is set as the threshold T. For comparison. It is considered that there is an NLOS environmental error in this observation channel at this moment and dynamic adjustment is required; if It is considered that the observation channel is normal at this moment and no dynamic adjustment is required.

[0114] Construct S = diag[s1,s2...s n ], as the updated interaction covariance matrix The weight matrix of :

[0115]

[0116] Rewrite expression (36) as:

[0117]

[0118] Filter gain K k+1 for:

[0119]

[0120] Then the estimated value of the system state at time k+1 is for:

[0121]

[0122] The updated posterior estimate of the error covariance matrix is:

[0123]

[0124] The above description is only for illustrating the implementation mode of the present invention and is not intended to limit the present invention. For those skilled in the art, any modification, equivalent substitution, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A UWB / IMU fusion positioning method based on environmental judgment, characterized in that: include: Step 1: Establish an observation model of four UWB base stations consisting of a main base station and three secondary base stations, and use the least squares method to obtain the position coordinates (x 0,k ,y 0,k ); Step 2: Establish a particle swarm optimization model to optimize the acquired UWB position coordinates and obtain the optimized coordinates (x 1,k ,y 1,k ), after obtaining the optimized coordinates, solve the distance d between the coordinates and each UWB base station i,k (i=4), used by the subsequent fusion positioning algorithm; Step 3: Establish IMU motion state model X k , including the position and velocity information of the carrier in the plane coordinate system; the model data comes from the xyz three-axis acceleration measured by the accelerometer and the xyz three-axis angular velocity measured by the gyroscope. According to the acceleration and angular velocity information output by the IMU, the output carrier state information can be converted into a coordinate system and integrally transformed, so as to solve the position and velocity estimation results of the carrier at the next moment; Step 4: Substitute the d obtained in step 2 i,k With step 3 X k The IAUKF algorithm fusion under tight combination is carried out. The algorithm steps include: (1) sigma point sampling; (2) weight calculation; (3) prediction value update; (4) noise matrix calculation; (5) state update. Compared with the traditional UKF algorithm, IAUKF improves steps (4) and (5), establishes an adaptive noise estimator to estimate the noise matrix, adds adaptive coefficients and thresholds, so that the positioning system can judge the impact of the current noise on the environment and make different suppression responses.

2. The UWB / IMU fusion positioning method based on environmental judgment according to claim 1 is characterized in that: The solution method of step 1 is as follows: According to the distance formula between the UWB tag and the base station, let the measured distance between the UWB base station and the tag be d i (i=4), then the following equations can be listed: Where (x, y) is the set UWB tag location coordinates, (x i ,y i ) are the position coordinates of the four UWB base stations, and the corresponding equations can be further obtained: Further, the matrix equation can be established: Finally, the corresponding UWB tag coordinates (x, y) can be obtained.

3. The UWB / IMU fusion positioning method based on environmental judgment according to claim 1 is characterized in that: Step 3 is specifically divided into the following steps: (1) Particle initialization Assume that there are n particles in the current D-dimensional space. With the position coordinates obtained by the least squares method as the center, each particle is assigned an initial position and initial velocity, which can be expressed as: x i =(x i1 ,x i2 ,...,x i ) (i=1,2...n) (9) v i =(v i1 ,v i2 ,...,v i ) (i=1,2...n) (10) At the same time, calibrate the fixed weight ω, the particle's motion range and position interval, and the learning factors c1 and c 2, (2) Calculate the initial fitness of the particle According to the objective function of the optimization problem, the corresponding fitness equation is established to make the particles continuously converge to the local optimal solution. This paper selects the positioning variance as the fitness equation fitness: Among them, f i =s i -s1-(u i -u1),(i=1,2,3),s i is the distance between the UWB base station and the tag obtained by the PSO algorithm, u i is the distance between the base station and the tag measured by the UWB hardware, (3)Compare with the local optimal solution Pbest, Compare the fitness value of each particle with the local optimal solution Pbest i By comparison, if the current particle is better than the local optimal solution, it will be replaced. (4)Compare with the global optimal solution Gbest, The local optimal solution Gbest each time i The fitness value of the particle is compared with the global optimal solution. If the current particle is better than the global optimal solution, it will be replaced. (5) Update the particle's position and velocity Among them, v ij represents the speed of the ith particle at the jth iteration, x ij represents the position coordinates of the ith particle at the jth iteration, and the range that the particle can reach is [X min ,X max ], (6) When the fitness meets the requirements, execute step 7. If not, return to step 2 and continue iterating. (7) Output the final result, Since there are many kinds of environmental noise in practice, when the UWB ranging information is greatly affected, the PSO optimization algorithm is prone to large position offset. Therefore, this paper performs threshold processing on the algorithm and only uses the PSO optimization algorithm to perform a small amount of smoothing on the UWB received data information without correcting the large offset error. Thus, the optimized coordinates can be obtained 4. The UWB / IMU fusion positioning method based on environmental judgment according to claim 1 is characterized in that: Step 3 is divided into the following two steps: (1) Coordinate system rotation Since the IMU measurement value is in the carrier coordinate system, it needs to be converted into the navigation coordinate system in practical applications. The body coordinate system is now recorded as the b coordinate system, and the navigation coordinate system is the n coordinate system. It is called the rotation matrix that transforms the carrier coordinate system into the navigation coordinate system. By rotating around the z, y, and x axes at angles ψ, θ, and γ, the rotation matrix from the carrier coordinate system to the navigation coordinate system can be obtained. (2) Derivation of kinematic equations By rotating the matrix in step (1) The three-axis acceleration of the mobile robot measured by IMU based on the carrier coordinate system is: Then the three-axis acceleration in the navigation coordinate system can be obtained through the rotation matrix: Where g is the acceleration due to gravity, If the current time is k and the time change is ΔT, then the position and velocity at time k+1 are: Therefore, after determining the initial position, INS can give the estimated position and speed results of the carrier at different times.

5. The UWB / IMU fusion positioning method based on environmental judgment according to claim 1, characterized in that: Step 4 combines the IAUKF algorithm obtained in step 2 with the IAUKF algorithm obtained in step 3 in a tight combination. The algorithm steps include: (1) sigma point sampling; (2) weight calculation; (3) prediction value update; (4) noise matrix calculation; (5) state update.

Citation Information

Cited By

  • Base station self-calibration method combined with direct non-line-of-sight error mitigation

    CN119854722A

  • A base station self-calibration method combined with direct non-line-of-sight error mitigation

    CN119854722B

  • Unmanned aerial vehicle cluster dynamic measurement method based on cooperative positioning and multi-source fusion

    CN120871201A

  • An indoor positioning method and system based on UWB / IMU joint estimation of wireless propagation state and a storage medium

    CN122881700A