An improved sliding window-based UWB and IMU combined positioning system and method

Abstract

The application relates to the technical field of indoor positioning, in particular to a UWB and IMU combined positioning system and method based on an improved sliding window, which comprises the following steps: S1, a plurality of UWB base stations and a label integrated with an IMU are deployed, time of arrival data is acquired and is solved into UWB distance observation values, and three-axis angular velocity and acceleration are collected at the same time; S2, based on the observation values of the first epoch, an initial position is solved through a linearized least square method; S3, IMU data is input to update the attitude, velocity and position, and an error state Kalman filter prediction covariance is constructed; S4, new information is calculated and is injected into a sliding window, and an observation noise covariance is updated based on weighted estimation; and S5, Kalman gain is calculated, and state feedback correction is completed. According to the application, UWB ranging and IMU inertial navigation information are fused, an adaptive sliding window filtering model is constructed, and high-precision, strong robustness and dynamic stability of three-dimensional combined positioning of a moving target in a complex environment are realized.

Description

Technical Field

[0001] This invention relates to the field of indoor positioning technology, and in particular to a UWB and IMU combined positioning system and method based on an improved sliding window. Background Technology

[0002] With the rapid development of IoT technology and the continuous improvement of industrial automation, precise indoor positioning has become a core requirement for many application scenarios, such as smart manufacturing, warehousing and logistics, robot navigation, and personnel monitoring. Among various positioning technologies, ultra-wideband (UWB) technology is widely regarded as the ideal choice for achieving high-precision indoor positioning due to its high temporal resolution, strong penetration capability, and good anti-multipath interference performance.

[0003] However, in complex environments, UWB ranging is susceptible to interference from non-line-of-sight (NLOS) factors, leading to significant deviations or even drastic jumps in positioning results, severely impacting the stability and availability of the system. To address this issue, some methods attempt to identify NLOS states through channel feature extraction combined with classification models, but these suffer from strong scene dependence and poor generalization ability. Other methods integrate inertial measurement units (IMUs) to improve robustness and employ Kalman filtering for state estimation; however, standard filters struggle to effectively model the non-Gaussian burst errors caused by NLOS, resulting in long-term drift or false convergence. Summary of the Invention

[0004] This invention provides a UWB and IMU combined positioning system and method based on an improved sliding window, which is a high-precision positioning method capable of dynamically identifying and suppressing the influence of NLOS errors. This method should possess good adaptive filtering capabilities and robust performance to achieve continuous, stable, and high-precision three-dimensional positioning of targets in complex indoor environments.

[0005] A UWB and IMU combined localization method based on an improved sliding window includes the following steps: S1. By using multiple UWB base stations deployed at fixed locations and UWB tags on the moving target, the original time of arrival data is obtained using a bilateral two-way ranging method. The original time of arrival data is then converted into UWB distance observation values ​​between the UWB base stations and the UWB tags. At the same time, IMU data output by the inertial measurement unit in the coordinate system of the moving target body is collected, including three-axis angular velocity and three-axis acceleration. S2. Based on the UWB distance observation value obtained in the first valid epoch, the linearized least squares algorithm is used to solve the geometric positioning equation system composed of multiple UWB base stations, and the initial position coordinates of the moving target in the global coordinate system are calculated. S3 inputs the acquired IMU data into the strapdown inertial navigation solution unit, and recursively outputs the nominal state of the moving target in real time through attitude update, velocity update and position update, including position, velocity and attitude, and constructs an error state Kalman filter to model the error state in the IMU solution process and predict its error state covariance matrix. S4. When the UWB distance observation value arrives, calculate the expected observation vector of the UWB tag and the UWB base station according to the nominal state, and calculate the innovation vector by subtracting it from the UWB distance observation value. Inject the innovation vector into the sliding window, and use geometric series weights to weight the historical innovation vectors in the sliding window to dynamically estimate and update the UWB observation noise covariance matrix in the error state Kalman filter. S5 calculates the Kalman gain based on the error state covariance matrix and the observation noise covariance matrix, performs optimal weighting on the innovation vector, estimates the current error state vector, and injects the current error state vector into the nominal state to complete the feedback correction.

[0006] Optionally, S1 includes: S11, Establish a spatial rectangular coordinate system in space, and deploy multiple UWB base stations at known coordinate points. , , as well as superior; S12, using a two-sided bidirectional ranging method, the ranging query signal Poll is sent by the tag under test. After receiving it, the UWB base station delays the signal and then sends a response signal Resp. The tag under test receives the response signal Resp and delays the signal again before sending the final signal Final. S13, the instantaneous time axis of the transmitted and received signals of the tag under test and the UWB base station is recorded, and the reception interval between the tag under test and the UWB base station is calculated, including the time interval between the tag under test sending Poll and receiving Final. The response delay of the base station from receiving the Poll to sending the Resp. The time interval between the base station sending the Resp and receiving the Final. And the response delay of the tag under test from receiving the Resp to issuing the Final. ; S14, Calculate the UWB distance observation between the UWB base station and the UWB tag based on the reception interval between the tag under test and the UWB base station. .

[0007] Optionally, the UWB distance observation value Represented as: ; ; in, This refers to the time required for a single transmission of the UWB signal between the tag under test and the base station. This represents the propagation speed of the UWB signal in the air medium.

[0008] Optionally, S2 includes: S21, using UWB distance observations between four UWB base stations and the tag under test. Establish a three-dimensional Euclidean distance constraint relationship. Let the coordinates of the label to be measured be T(x,y,z). Construct a system of equations, expressed as: ; ; ; ; S22, the squares of the constructed system of equations are calculated, as follows: ; ; ; ; S23, order , Establish a matrix equation, expressed as: ; S24, construct the matrix equation as Form, among which, , ; S25, according to the least squares method Calculate the initial position coordinates of the label to be tested. .

[0009] Optionally, S3 includes: S31, based on the three-axis angular velocity output by the IMU carried by the tag under test, and combining the Bortz equation, the differential equation of the attitude rotation vector is approximated as follows: Based on the twin-sample assumption, the equivalent rotation vector is approximated as follows: ,in, As an angle increment, the pose of the label under test is represented by a quaternion and updated accordingly, as follows: ; ; in, This represents the equivalent rotation vector from the initial attitude of the carrier to its current attitude. The scalar magnitude representing the equivalent rotation vector of the carrier. express The attitude quaternion from the b-system to the n-system at time b. express Time B system The equivalent rotation quaternion of the time-b system. Represents n-series Time's up The equivalent rotation quaternion of the n-series at time n; S32, using the trapezoidal integral method to measure the force at two consecutive moments. Integrating, we obtain the velocity increment in the carrier coordinate system. A second-order propeller effect compensation term is introduced to correct the specific force error caused by attitude rotation. A direction cosine matrix is ​​constructed using the current attitude quaternion. The corrected velocity increment is then transformed from the carrier coordinate system to the navigation coordinate system. The velocity in the navigation coordinate system is updated by integrating the previous velocity, the transformed velocity increment, and the gravity integral term, as follows: ; ; ; ; ; in, Let be the attitude matrix at the current moment; S33, using the trapezoidal integration method for position update, is expressed as: ; S34, Construct the error-state Kalman filter and define the nominal state. With error state vector , is represented as: ; ; Where p is the three-dimensional coordinate of the tag under test in the navigation frame, v is the three-dimensional velocity of the target under test in the navigation frame, and q is the unit quaternion of the rotation from the carrier frame to the navigation frame. , These are zero-bias estimations for accelerometers and gyroscopes, respectively. For speed error, For speed error, This is the three-dimensional attitude error rotation vector. , These are the zero-bias estimation errors for the accelerometer and gyroscope, respectively. S35. Based on the error propagation relationship between position, velocity, attitude, and IMU zero bias, the state update equation is derived, and the error state transition matrix is ​​constructed. and noise driving matrix , is represented as: ; ; ; in, The acceleration measurement value is compensated in the carrier coordinate system. The antisymmetric matrix; S36, combined with the error state transition matrix Noise driving matrix The accelerometer white noise variance, gyroscope white noise variance, accelerometer zero-bias drive noise variance, and gyroscope zero-bias drive noise variance are used to update the error state covariance matrix, as shown below: ; in, For the accelerometer white noise variance, The variance of the gyroscope's white noise. For the zero-bias drive noise variance of the accelerometer, The variance of the gyroscope's zero-bias drive noise; S37, based on the nominal state at the current time, predict the distance between the label and each base station to obtain the desired observation vector. The innovation vector is calculated by subtracting the actual observed vector from the innovation vector. And construct the measurement matrix , is represented as: .

[0010] Optionally, the The angular velocity output from the IMU is integrated and calculated, and expressed as follows: ; in, The label represents the value measured by the IMU. The instantaneous angular velocity at a given moment.

[0011] Optionally, S4 includes: S41, introducing a size of The sliding window uses a first-in-first-out (FIFO) approach to count the information vectors at historical moments; S42, Introducing Geometric Series Weighting Factors ,in, Determined by the position of the innovation vector within the sliding window, it can be represented as: ; S43, calculate the covariance matrix of the innovation vector, expressed as: ; S44, calculate the estimated value of the observation noise covariance matrix, expressed as: ; S45, regarding the observation noise covariance matrix By performing symmetry processing and introducing upper and lower bound constraints, it can be represented as follows: ; .

[0012] Optionally, S5 includes: S51, combined with the predicted error state covariance matrix and observation noise covariance matrix Calculate the Kalman gain, expressed as: ; S52, using Kalman gain to adjust the innovation vector The weighted average yields the current error state estimate, expressed as: ; S53, update the error state covariance matrix, expressed as: ; S54, inject the error state vector into the nominal state for state correction, expressed as: ; ; in, This means converting the rotation vector component in the error state vector into a quaternion.

[0013] A UWB and IMU combined positioning system based on an improved sliding window, used to implement the aforementioned UWB and IMU combined positioning method based on an improved sliding window, includes the following modules: UWB-IMU Cooperative Sensing Module: By using multiple UWB base stations deployed at fixed locations and UWB tags on the moving target, it acquires raw time-of-arrival (TOA) data using a bilateral two-way ranging method, and calculates the TOA data into UWB distance observations between the UWB base stations and UWB tags. At the same time, it collects IMU data output by the inertial measurement unit in the coordinate system of the moving target, including three-axis angular velocity and three-axis acceleration. Solution module: Calculates the initial position coordinates of the moving target using linearized least squares method based on UWB distance observations; Inertial navigation calculation module: Based on IMU data, it performs attitude, velocity and position recursion, outputs nominal state, and constructs error state Kalman filter to predict and estimate error state. Sliding window processing module: Receives the expected observation difference between the UWB distance observation and the nominal state at each epoch, calculates the innovation vector and injects it into the sliding window, performs weighted statistics by combining geometric series weights, and dynamically estimates the UWB observation noise covariance matrix. State correction module: Calculates Kalman gain and weights the innovation vector to complete the error state estimation, and injects it into the nominal state to achieve feedback correction.

[0014] The beneficial effects of this invention are: This invention, by fusing UWB bilateral bidirectional ranging and IMU inertial navigation data, enables high-precision three-dimensional positioning of moving targets in the absence of GPS. It utilizes linearized least squares method to complete the initial position calculation, providing a high-quality initial state for subsequent filtering and recursion, effectively improving the reliability and accuracy of the system initialization phase.

[0015] This invention constructs a strapdown inertial navigation model based on equivalent rotation vectors and attitude quaternions, and combines it with an error state Kalman filter to model and correct the IMU integral error. The system can achieve high-frequency updates and dynamic compensation of position, velocity, and attitude throughout the entire operating cycle, enhancing its adaptability to complex motion states and significantly suppressing inertial drift error.

[0016] This invention introduces a sliding window and a geometric series weighting mechanism to weight the estimation of historical information, thereby achieving dynamic adaptive adjustment of the observation noise covariance matrix. This further improves the robustness and convergence performance of the filtering system in non-line-of-sight (NLOS) interference scenarios, ensuring that the final state estimation has higher stability and positioning accuracy. Attached Figure Description

[0017] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only for this invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 This is a schematic diagram of the overall framework of the positioning system according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the operation process of the error state Kalman filter based on the improved sliding window according to an embodiment of the present invention; Figure 3 This is a schematic diagram of the bilateral two-way ranging method according to an embodiment of the present invention; Figure 4 This is a schematic diagram of simulated base station measurement data according to an embodiment of the present invention; Figure 5 This is a schematic diagram of the simulated actual motion trajectory of the tag under test in an embodiment of the present invention; Figure 6 This is a schematic diagram illustrating the position calculation effect of the method of this invention and the unmodified sliding window error state Kalman filter according to an embodiment of the invention; Figure 7 This is a schematic diagram comparing the error curves of the method of this invention and the unmodified sliding window error state Kalman filter. Figure 8 This is a schematic diagram comparing the method of this invention with the cumulative distribution function of the three-dimensional positioning error of the unmodified sliding window error state Kalman filter; Figure 9 This is a schematic diagram of the system functional modules according to an embodiment of the present invention. Detailed Implementation

[0019] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. Those skilled in the art may employ other alternative methods to implement some well-known technologies; moreover, the accompanying drawings are only for more specific description of the embodiments and are not intended to specifically limit the present invention.

[0020] like Figures 1-8 As shown, a UWB and IMU combined positioning method based on an improved sliding window includes the following steps: S1, using multiple UWB base stations deployed at fixed locations and UWB tags on a moving target, a set of raw time-of-arrival data with high temporal resolution is acquired through a bilateral, bidirectional ranging method, and then calculated into a geometric distance observation sequence between the UWB base stations and the tags. Simultaneously, using an inertial measurement unit integrated on the moving target, raw data of its three-axis angular velocity and three-axis acceleration in its body coordinate system are acquired at high frequency.

[0021] S2, based on the UWB distance observation value of the first valid epoch obtained in the above operations, uses the linearized least squares algorithm to solve the geometric positioning equations composed of multiple UWB base stations, and calculates the initial position coordinate estimate of the moving target in the global coordinate system. This step provides the necessary iterative initialization state for subsequent filtering algorithms.

[0022] In step S3, the raw IMU data acquired in step S1 is input to the strapdown inertial navigation solution unit. Through attitude, velocity, and position updates, the nominal state of the moving target (including position, velocity, and attitude) is recursively output in real time. Simultaneously, to suppress the inherent cumulative error of inertial solution, this invention employs an Error-State Kalman Filter (ESKF) as the core fusion framework. ESKF models the cumulative error of inertial solution as an error state and predicts its evolution over time.

[0023] S4. After the UWB distance observations arrive at the system, the expected observation vectors of the tag and the base station are first calculated based on the nominal state of the system. The difference between these vectors and the observed values ​​is then calculated to obtain the innovation vector, which is subsequently injected into a sliding window. This sliding window performs real-time analysis on the statistical characteristics of the historical innovation sequence within the window and estimates the innovation covariance using a geometric series weighting method. Based on this analysis, the UWB observation noise covariance matrix in the ESKF is dynamically estimated and updated. This mechanism enables the filter to adaptively reflect the measurement uncertainties of the current environment.

[0024] S5 executes the ESKF measurement update step. This utilizes the observation noise covariance matrix obtained in S4. The system performs an optimal estimate of the error state predicted in S3. Then, the estimated error state is fed back to correct the nominal state calculated by the strapdown inertial navigation system, thereby obtaining a high-precision estimate of the moving target's position, velocity, and attitude. Subsequently, the error state is reset, and the system enters the recursive and update loop for the next epoch.

[0025] In summary, the overall framework of the positioning system is as follows: Figure 1 As shown in the figure. The flowchart of the error state Kalman filter based on the improved sliding window is as follows. Figure 2 As shown.

[0026] In S1, a spatial rectangular coordinate system is established in space, and multiple base stations are deployed at known coordinate points A1(x1,y1,z1), A2(x2,y2,z2), A3(x3,y3,z3), and A4(x4,y4,z4). Ranging is performed using a bilateral, bidirectional method. The ranging query signal Poll is emitted by the tag under test. After receiving the signal, the base station delays the response and then emits a response signal Resp. The tag under test receives the signal and delays the response again before emitting a final signal Final. The tag under test and the base station record the instantaneous timelines of the emitted and received signals, and calculate the... , , , ,like Figure 3 As shown. The calculation method for signal flight time and ranging value is as follows: ; In the formula, , , , This represents the reception interval between the tag under test and the base station; d represents the time required for a single transmission of the UWB signal between the tag under test and the base station; c represents the ranging value; and c represents the propagation speed of the UWB signal in the air medium.

[0027] The distances d between the four base stations and the tag under test obtained based on S1 i (i=4), S2 calculates the initial position of the tag under test using the linearized least squares method. Let the coordinates of the tag under test be T(x,y,z). Based on the distance formula between the tag under test and the base station, the following system of equations can be constructed: ; Squaring the above system of equations respectively yields: ; make , Then the following matrix equation can be established: ; Construct the above matrix as follows Form, in which: ; According to the least squares method: ; The initial coordinates (x, y, z) of the label to be tested can then be calculated.

[0028] In S3, the attitude, velocity, and position are updated based on the acceleration and angular velocity information output by the IMU carried by the tag under test. The attitude update method is as follows: According to the Bortz equation, neglecting higher-order terms, the equivalent rotating vector differential equation can be approximated by the following expression: ; Based on the twin-sample assumption, the equivalent rotation vector can be approximated as: ; Obtained by integrating the angular velocity output from the IMU: ; in, The label represents the value measured by the IMU. The instantaneous angular velocity at any given moment. Since this patent is designed for positioning in complex indoor environments, where the target motion is relatively smooth and the IMU sampling frequency is high, the above approximation is used.

[0029] In this system, the pose of the tag under test is represented by quaternions and updated using the following formula: ; ; in, This represents the equivalent rotation vector from the initial attitude of the carrier to its current attitude. The scalar magnitude representing the equivalent rotation vector of the carrier. express The attitude quaternion from the b-system to the n-system at time b. express Time B system The equivalent rotation quaternion of the time-b system. Represents n-series Time's up The equivalent rotation quaternion of the n-series at time n.

[0030] The velocity update uses the trapezoidal integral method to measure the specific force in the carrier coordinate system at two consecutive time points. Integrating, we obtain the original velocity increment in frame b. : ; To correct for the specific force measurement error caused by the carrier rotation, a second-order paddling effect compensation term for the specific force integral is added: ; Using the pose matrix at the current time (Obtained from the current attitude quaternion), transform the compensated velocity increment to the navigation coordinate system: ; The gravity integral term within the update period is calculated using the following formula: ; Finally, the initial velocity, the force integral term, and the Goethe-Stokes integral term are integrated to complete the velocity update: ; The position update equation is as follows: ; Construct an error-state Kalman filter and define the nominal state vector. With error state vector : ; ; Where p is the three-dimensional coordinate of the tag to be tested in the navigation system; v is the three-dimensional velocity of the target to be tested in the navigation system; q represents the unit quaternion of the rotation from the carrier system to the navigation system; and The accelerometer and gyroscope zero-bias estimations are used respectively. Before updating the IMU, both will correct the IMU's original acceleration and angular velocity information. For speed error, For speed error, For the three-dimensional attitude error rotation vector, and These represent the zero-bias estimation errors of the accelerometer and gyroscope, respectively. Since the attitude quaternions are constrained, directly using them as the Kalman filter states leads to singular covariance matrices. Therefore, a three-dimensional unconstrained rotation vector is used. As an error state.

[0031] The nominal state vector prediction is completed by combining the above attitude update, velocity update, and position update with the IMU output acceleration and angular velocity. The error state vector prediction update satisfies the following equation: ; in, This represents the compensated acceleration measurement in the carrier coordinate system. The antisymmetric matrix. From the above equation, the error state transition matrix F of the error state Kalman filter can be obtained: ; Simultaneously constructing the noise driving matrix: ; Then, the error state covariance matrix is ​​updated: ; in, For the accelerometer white noise variance, The variance of the gyroscope's white noise. For the zero-bias drive noise variance of the accelerometer, This represents the variance of the gyroscope's zero-bias drive noise.

[0032] After UWB achieves ranging and successfully reports the distance, the system predicts the distance between the tag and each base station based on the nominal state vector at the current time, thus obtaining the desired observation vector. The actual UWB observation vector With the expected observation vector The difference is used to obtain the new information vector. .

[0033] Based on the observation model, calculate the measurement matrix H: ; To overcome the limitations of traditional fixed observation noise covariance matrix in S4, this invention uses an improved sliding window method to realize the observation noise matrix. Real-time updates and optimizations are achieved. By using a sliding window method to statistically analyze historical innovation vectors and then applying geometric weights based on the injection order of innovation data within the window, this method ensures that the innovation vector of the latest injected window is relevant in estimating the observation noise covariance. The [partially assigned] weight has a higher weight. This effectively improves the system's positioning accuracy in NLOS environments. The implementation method is as follows: (1) Use a sliding window of size N to count the information vectors of historical moments in a first-in-first-out manner.

[0034] (2) Introducing geometric series weighting factors The value of 'i' is determined by the position of the innovation vector within the window; the innovation vector that enters the window first has a larger value, and its corresponding value is higher. Smaller.

[0035] ; (3) Obtain the covariance matrix of the new sample: ; (4) The estimated observation noise covariance matrix is: ; (5) To ensure the observation noise covariance matrix Positive definite, it is treated symmetrically, and to avoid over-reliance on observations and prevent system divergence, the observation noise covariance matrix is ​​adjusted. Apply upper and lower limit constraints: ; ; S5 combines the predicted error state covariance matrix Covariance matrix of observation noise Calculate Kalman gain Then, the Kalman gain is used to weight the innovation vector to obtain the current error state vector estimate. Finally, the error state covariance matrix is ​​updated and the error state vector is injected into the nominal state vector. The specific method is as follows: ; The attitude error is expressed using a rotation vector in the error state vector, so it needs to be converted into a quaternion before injection. This means converting the rotation vector component in the error state vector into a quaternion.

[0036] A simulation environment was built using a virtual platform. Four base stations were placed at known locations A1(0,0,1), A2(10,0,3), A2(0,10,3.5), and A4(10,10,4). The tag under test requested ranging from the UWB base station at 50Hz and simultaneously reported its own acceleration and gyroscope information measured by its IMU at 200Hz. The initial coordinates of the tag under test were T(1,4,3). It moved clockwise around a circle with a radius of 3m and a center of (4,4) in the XY plane at a speed of 1m / s, while simultaneously moving at a speed of -0.0398m / s in the Z-axis direction. The actual motion trajectory of the tag under test is shown in the figure below. Figure 4 As shown; where the base station measurement error distribution satisfies IMU parameter reference for mid-range consumer to industrial grade sensors: accelerometer noise density is The zero bias at startup is 5mg, and the gyroscope noise density is... Zero bias upon startup To simulate a complex indoor environment, base station measurements randomly exhibited NLOS (Normally In-Service) errors. Base station 4 consistently remained in NLOS for over 30% of the total measurements. Base station measurement data are as follows: Figure 5 As shown.

[0037] The position calculation effect diagram of the method of this invention is shown below. Figure 6 As shown in the figure, the error curves of the method of the present invention are compared with those of other methods. Figure 7 As shown. (Through) Figure 6 Position calculation results diagram and Figure 7 As can be seen from the error curve comparison chart, the Kalman filter method of the present invention and the unimproved sliding window method have relatively large NLOS errors in UWB base station measurements under complex environments, resulting in some initial positioning errors. After a period of filtering, the errors gradually decrease, and new base station measurement information with NLOS errors arrives after the Kalman filter stabilizes. The present invention method has smaller error fluctuations compared to the unimproved sliding window method. Under three severe NLOS conditions, the maximum errors of the present invention method are: 0.3837m, 0.5174m, and 0.2524m; the corresponding maximum errors of the unimproved sliding window method are 0.5433m, 0.7010m, and 0.5788m. Furthermore, Figure 8The results show a comparison of the cumulative distribution function of 3D positioning errors between the method of this invention and the unmodified sliding window error-state Kalman filter method. The method of this invention exhibits a higher cumulative probability at almost any error threshold. At a 90% cumulative probability, the positioning error of the method of this invention is 0.2451m, while that of the unmodified sliding window error-state Kalman filter method is 0.4403m. This indicates that the improved sliding window error-state Kalman filter has stronger resistance to NLOS compared to the unmodified version, and can better adjust the observation noise covariance matrix when NLOS errors occur in base station measurements. To adapt it to environmental changes and enable correct adjustment of the Kalman gain .

[0038] The positioning results of the method of the present invention, error state Kalman filtering, and unmodified sliding window error state Kalman filtering are shown in Table 1: algorithm 3D positioning mean absolute error (MAE) 3D positioning root mean square error RMSE Error-state Kalman filtering 1.5537m 2.0864m Unimproved sliding window error state Kalman filter 0.1391m 0.2277m Error-state Kalman filter based on improved sliding window 0.1006m 0.1657m Table 1 Error table of localization results between the method of the present invention and other algorithms like Figure 9 As shown, a UWB and IMU combined positioning system based on an improved sliding window is used to implement the aforementioned UWB and IMU combined positioning method based on an improved sliding window, and includes the following modules: UWB-IMU Cooperative Sensing Module: By using multiple UWB base stations deployed at fixed locations and UWB tags on the moving target, it acquires raw time-of-arrival (TOA) data using a bilateral two-way ranging method, and calculates the TOA data into UWB distance observations between the UWB base stations and UWB tags. At the same time, it collects IMU data output by the inertial measurement unit in the coordinate system of the moving target, including three-axis angular velocity and three-axis acceleration. Solution module: Calculates the initial position coordinates of the moving target using linearized least squares method based on UWB distance observations; Inertial navigation calculation module: Based on IMU data, it performs attitude, velocity and position recursion, outputs nominal state, and constructs error state Kalman filter to predict and estimate error state. Sliding window processing module: Receives the expected observation difference between the UWB distance observation value and the nominal state at each epoch, calculates the innovation vector and injects it into the sliding window, performs weighted statistics by combining geometric series weights, and dynamically estimates the UWB observation noise covariance matrix. State correction module: Calculates Kalman gain and weights the innovation vector to complete the error state estimation, and injects it into the nominal state to achieve feedback correction.

[0039] This invention encompasses any substitutions, modifications, equivalent methods, and solutions made within the spirit and scope of this invention. To provide the public with a thorough understanding of this invention, specific details are described in detail in the following preferred embodiments; however, those skilled in the art will fully understand the invention even without these details. Furthermore, to avoid unnecessary misunderstanding of the essence of this invention, well-known methods, processes, procedures, components, and circuits are not described in detail.

[0040] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A UWB and IMU combined positioning method based on an improved sliding window, characterized in that, Includes the following steps: S1. By using multiple UWB base stations deployed at fixed locations and UWB tags on the moving target, the original time of arrival data is obtained using a bilateral two-way ranging method. The original time of arrival data is then converted into UWB distance observation values ​​between the UWB base stations and the UWB tags. At the same time, IMU data output by the inertial measurement unit in the coordinate system of the moving target body is collected, including three-axis angular velocity and three-axis acceleration. S2. Based on the UWB distance observation value obtained in the first valid epoch, the linearized least squares algorithm is used to solve the geometric positioning equation system composed of multiple UWB base stations, and the initial position coordinates of the moving target in the global coordinate system are calculated. S3 inputs the acquired IMU data into the strapdown inertial navigation solution unit, and recursively outputs the nominal state of the moving target in real time through attitude update, velocity update and position update, including position, velocity and attitude, and constructs an error state Kalman filter to model the error state in the IMU solution process and predict its error state covariance matrix. S4. When the UWB distance observation value arrives, calculate the expected observation vector of the UWB tag and the UWB base station according to the nominal state, and calculate the innovation vector by subtracting it from the UWB distance observation value. Inject the innovation vector into the sliding window, and use geometric series weights to weight the historical innovation vectors in the sliding window to dynamically estimate and update the UWB observation noise covariance matrix in the error state Kalman filter. S5 calculates the Kalman gain based on the error state covariance matrix and the observation noise covariance matrix, performs optimal weighting on the innovation vector, estimates the current error state vector, and injects the current error state vector into the nominal state to complete the feedback correction.

2. The UWB and IMU combined positioning method based on an improved sliding window according to claim 1, characterized in that, S1 includes: S11, Establish a spatial rectangular coordinate system in space, and deploy multiple UWB base stations at known coordinate points. , , as well as superior; S12, using a two-sided bidirectional ranging method, the ranging query signal Poll is sent by the tag under test. After receiving it, the UWB base station delays the signal and then sends a response signal Resp. The tag under test receives the response signal Resp and delays the signal again before sending the final signal Final. S13, the instantaneous time axis of the transmitted and received signals of the tag under test and the UWB base station is recorded, and the reception interval between the tag under test and the UWB base station is calculated, including the time interval between the tag under test sending Poll and receiving Final. The response delay of the base station from receiving the Poll to sending the Resp. The time interval between the base station sending the Resp and receiving the Final. And the response delay of the tag under test from receiving the Resp to issuing the Final. ; S14, Calculate the UWB distance observation between the UWB base station and the UWB tag based on the reception interval between the tag under test and the UWB base station. .

3. The UWB and IMU combined positioning method based on an improved sliding window according to claim 2, characterized in that, The UWB distance observation value Represented as: ； ； in, This refers to the time required for a single transmission of the UWB signal between the tag under test and the base station. This represents the propagation speed of the UWB signal in the air medium.

4. The UWB and IMU combined positioning method based on an improved sliding window according to claim 3, characterized in that, S2 includes: S21, using UWB distance observations between four UWB base stations and the tag under test. Establish a three-dimensional Euclidean distance constraint relationship. Let the coordinates of the label to be measured be T(x,y,z). Construct a system of equations, expressed as: ； ； ； ； S22, the squares of the constructed system of equations are calculated, as follows: ； ； ； ； S23, order , Establish a matrix equation, expressed as: ； S24, construct the matrix equation as Form, among which, , ; S25, according to the least squares method Calculate the initial position coordinates of the label to be tested. .

5. The UWB and IMU combined positioning method based on an improved sliding window according to claim 4, characterized in that, S3 includes: S31, based on the three-axis angular velocity output by the IMU carried by the tag under test, and combining the Bortz equation, the differential equation of the attitude rotation vector is approximated as follows: Based on the twin-sample assumption, the equivalent rotation vector is approximated as follows: ,in, As an angle increment, the pose of the label under test is represented by a quaternion and updated accordingly, as follows: ； ； in, This represents the equivalent rotation vector from the initial attitude of the carrier to its current attitude. The scalar magnitude representing the equivalent rotation vector of the carrier. express The attitude quaternion from the b-system to the n-system at time b. express Time B system The equivalent rotation quaternion of the time series b. Represents n-series Time's up The equivalent rotation quaternion of the n-series at time n; S32, using the trapezoidal integral method to measure the force at two consecutive moments. Integrating, we obtain the velocity increment in the carrier coordinate system. A second-order propeller effect compensation term is introduced to correct the specific force error caused by attitude rotation. A direction cosine matrix is ​​constructed using the current attitude quaternion. The corrected velocity increment is then transformed from the carrier coordinate system to the navigation coordinate system. The velocity in the navigation coordinate system is updated by integrating the previous velocity, the transformed velocity increment, and the gravity integral term, as follows: ； ； ； ； ； in, Let be the attitude matrix at the current moment; S33, using the trapezoidal integration method for position update, is expressed as: ； S34, Construct the error-state Kalman filter and define the nominal state. With error state vector , is represented as: ； ； Where p is the three-dimensional coordinate of the tag under test in the navigation frame, v is the three-dimensional velocity of the target under test in the navigation frame, and q is the unit quaternion of the rotation from the carrier frame to the navigation frame. , These are zero-bias estimations for accelerometers and gyroscopes, respectively. For speed error, For speed error, This is the three-dimensional attitude error rotation vector. , These are the zero-bias estimation errors for the accelerometer and gyroscope, respectively. S35. Based on the error propagation relationship between position, velocity, attitude, and IMU zero bias, the state update equation is derived, and the error state transition matrix is ​​constructed. and noise driving matrix , is represented as: ； ； ； in, The acceleration measurement value is compensated in the carrier coordinate system. The antisymmetric matrix; S36, combined with the error state transition matrix Noise driving matrix The accelerometer white noise variance, gyroscope white noise variance, accelerometer zero-bias drive noise variance, and gyroscope zero-bias drive noise variance are used to update the error state covariance matrix, as shown below: ； in, For the accelerometer white noise variance, The variance of the gyroscope's white noise. For the zero-bias drive noise variance of the accelerometer, The variance of the gyroscope's zero-bias drive noise; S37, based on the nominal state at the current time, predict the distance between the label and each base station to obtain the expected observation vector. The innovation vector is calculated by subtracting the actual observed vector from the innovation vector. And construct the measurement matrix , is represented as: 。 6. The UWB and IMU combined positioning method based on an improved sliding window according to claim 5, characterized in that, The The angular velocity output from the IMU is integrated and calculated, and expressed as follows: ； in, The label represents the value measured by the IMU. The instantaneous angular velocity at a given moment.

7. The UWB and IMU combined positioning method based on an improved sliding window according to claim 6, characterized in that, S4 includes: S41, introducing a size of The sliding window uses a first-in-first-out (FIFO) approach to count the information vectors at historical moments; S42, introducing geometric series weighting factors ,in, Determined by the position of the innovation vector within the sliding window, it can be represented as: ； S43, calculate the covariance matrix of the innovation vector, expressed as: ； S44, calculate the estimated value of the observation noise covariance matrix, expressed as: ； S45, regarding the observation noise covariance matrix By performing symmetry processing and introducing upper and lower bound constraints, it can be represented as follows: ； 。 8. The UWB and IMU combined positioning method based on an improved sliding window according to claim 7, characterized in that, S5 includes: S51, combined with the predicted error state covariance matrix and observation noise covariance matrix Calculate the Kalman gain, expressed as: ； S52, using Kalman gain to adjust the innovation vector The weighted average yields the current error state estimate, expressed as: ； S53, update the error state covariance matrix, expressed as: ； S54, inject the error state vector into the nominal state for state correction, expressed as: ； ； in, This means converting the rotation vector component in the error state vector into a quaternion.

9. A UWB and IMU combined positioning system based on an improved sliding window, used to implement the UWB and IMU combined positioning method based on an improved sliding window as described in any one of claims 1-8, characterized in that, Includes the following modules: UWB-IMU Cooperative Sensing Module: By using multiple UWB base stations deployed at fixed locations and UWB tags on the moving target, it acquires raw time-of-arrival (TOA) data using a bilateral two-way ranging method, and calculates the TOA data into UWB distance observations between the UWB base stations and UWB tags. At the same time, it collects IMU data output by the inertial measurement unit in the coordinate system of the moving target, including three-axis angular velocity and three-axis acceleration. Solution module: Calculates the initial position coordinates of the moving target using linearized least squares method based on UWB distance observations; Inertial navigation calculation module: Based on IMU data, it performs attitude, velocity and position recursion, outputs nominal state, and constructs error state Kalman filter to predict and estimate error state. Sliding window processing module: Receives the expected observation difference between the UWB distance observation and the nominal state at each epoch, calculates the innovation vector and injects it into the sliding window, performs weighted statistics by combining geometric series weights, and dynamically estimates the UWB observation noise covariance matrix. State correction module: Calculates the Kalman gain and weights the innovation vector to complete the error state estimation, and injects it into the nominal state to achieve feedback correction.

Images