A method and system for UWB and IMU tight combination positioning based on sliding window and motion constraint

Abstract

This invention relates to the field of indoor positioning technology, specifically to a UWB and IMU tightly coupled positioning method and system based on sliding window and motion constraints, comprising the following steps: S1, deploying UWB base stations in the space to be measured, installing UWB and IMU units on a ground robot, acquiring distance observation values ​​through bilateral bidirectional ranging, and establishing a set of distance equations in conjunction with the base station coordinates, and solving for the initial position using linear least squares; S2, initializing the initial position input error state Kalman filter, and predicting the position, velocity, and attitude based on IMU data; S3, constructing an innovation vector based on the UWB observation vector and the predicted state, updating the observation noise covariance through a sliding window, and fusing UWB and inertial information; S4, constructing a Z-axis pseudo-zero velocity observation model and configuring the observation noise covariance, updating the error state through the input filter, and correcting the nominal state. This invention achieves the collaborative fusion of UWB and IMU information, thereby improving the accuracy and stability of ground robot positioning.

Description

Technical Field

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

[0002] In complex indoor environments such as warehousing and logistics, and intelligent manufacturing, ground mobile robots need stable, reliable, and high-precision positioning capabilities to support path planning, task scheduling, and safe operation. Ultra-wideband (UWB) technology, with its centimeter-level ranging accuracy, is widely used in indoor positioning and is often combined with inertial measurement units (IMUs) to form integrated navigation systems. By fusing the absolute position information from UWB with the continuous motion information from the IMU, continuous estimation of the robot's position and attitude is achieved, making it one of the main technical approaches for indoor mobile robot positioning.

[0003] However, existing UWB and IMU combined positioning methods still have significant shortcomings in complex real-world environments. First, UWB signals are easily affected by non-line-of-sight propagation, multipath effects, and dynamic obstacles in indoor environments, resulting in significant time-varying and random variations in ranging errors. Traditional fusion methods typically use fixed or offline calibrated noise models, which struggle to reflect changes in observation quality in real time, leading to jumps or short-term inaccuracies in positioning results when interference occurs. Second, IMU errors accumulate during integration, especially since the zero-bias error and attitude error of low-cost IMUs exhibit a coupled propagation effect in state space, potentially mapping attitude or acceleration errors into velocity or position estimates in other directions. Existing fusion methods often rely on UWB observations to directly correct position, lacking effective kinematic constraint mechanisms to suppress error propagation, thus limiting the overall positioning accuracy of the system. Summary of the Invention

[0004] This invention provides a UWB and IMU tightly coupled localization method and system based on sliding window and motion constraints. By introducing a sliding window to perform statistical analysis on UWB observation information, online adaptive estimation of observation noise is achieved, thereby improving the system's anti-interference capability in complex environments. At the same time, considering the motion characteristics of ground mobile robots with theoretically near-zero velocity in the Z-axis direction, a pseudo-zero velocity observation model for the Z-axis is introduced to constrain vertical channel errors without adding additional hardware, thereby suppressing error coupling propagation and improving overall positioning accuracy and system stability.

[0005] A UWB and IMU tightly coupled localization method based on sliding window and motion constraints includes the following steps: S1. Multiple UWB base stations are deployed in the space to be measured. UWB measurement units and IMU units are installed on the ground robot to be located. The distance observation values ​​between each base station and the ground robot are obtained through bilateral bidirectional ranging. The distance observation values ​​and the acceleration and angular velocity collected by the IMU are uploaded to the host computer. A set of distance equations is constructed based on the known coordinates of the UWB base stations and the distance observation values. The initial position coordinates of the ground robot in the navigation coordinate system are solved by the linearized least squares algorithm. S2, initialize the state Kalman filter of the initial position coordinate input error, and predict the position, velocity and attitude of the ground robot based on the acceleration and angular velocity collected by the IMU through the motion prediction model; S3. Construct an innovation vector based on the difference between the UWB observation vector and the predicted state. Input the innovation vector into a sliding window for statistical analysis to estimate and update the observation noise covariance matrix of the error state Kalman filter online. Then, fuse and update the UWB observation information and inertial prediction information based on the updated observation noise covariance matrix. S4. Construct a pseudo-zero velocity observation model for the Z-axis of the carrier coordinate system, and adaptively configure the observation noise covariance according to the positioning operation stage. Use the pseudo-zero velocity observation input error state Kalman filter to update the error state and correct the system state, thus completing the positioning update for the current filtering cycle.

[0006] Optionally, S1 includes: S11, During the initial deployment phase, a spatial rectangular coordinate system is established as the global reference system, and multiple UWB base stations are deployed within the space to be measured. Each base station is fixedly installed at known spatial coordinates, including... , , as well as ; S12, the ground robot initiates a UWB measurement request, and obtains the distance observation values ​​between each UWB base station and the ground robot through bilateral two-way ranging. The distance observation values ​​are uploaded to the host computer via the communication link, and the acceleration and angular velocity collected by the ground robot's IMU are uploaded synchronously. S13, Let the initial position coordinates of the tag to be located be... Based on the known spatial coordinates and corresponding distance observations of each UWB base station, a set of nonlinear distance equations between the tag to be located and each base station is established using the Euclidean distance relationship between two points in space, expressed as: ; ; ; ; in, , , , These are the observed spatial distances between the first, second, third, and fourth UWB base stations and the tag to be located, respectively. S14, perform algebraic transformation and linearization on the nonlinear distance equation system. By introducing auxiliary variables and rearranging the equations into matrix form, construct a linear equation system, expressed as: ; ; ; in, Let S be the sum of the squares of the coordinates of the label to be located. For the first The sum of squares of the coordinates of each UWB base station; S15, the system of linear equations is expressed as The unknown variables are solved using a linear least squares algorithm to obtain the initial position coordinates of the tag to be located in the navigation coordinate system, expressed as: .

[0007] Optionally, S2 includes: S21, the initial position coordinates are input into the error state Kalman filter to initialize the position, velocity, attitude, and IMU zero-bias state of the ground robot. Using the acceleration and angular velocity measured by the IMU at adjacent time points, the attitude, velocity, and position of the ground robot are recursively updated through the motion prediction model. The motion prediction model is expressed as: ; ; ; ; ; in, This is the equivalent rotation vector within the current sampling period. The relevant algorithms for converting equivalent rotation vectors into quaternions, This is the rotation matrix that transforms the vector in the vehicle coordinate system to the navigation coordinate system at the current moment. for down Time's up The velocity increment at time, for Time's up The angular increment at time t, This represents the gravitational acceleration component in the navigation coordinate system. S22, during the prediction process, the angular increment, equivalent rotation vector, and velocity increment are calculated using IMU measurements, and are expressed as follows: ; ; ; ; S23, based on the nominal state prediction, updates the error state vector, as follows: ; ; ; ; ; in, The difference between the acceleration measurement value and the zero bias of the acceleration measurement in the carrier coordinate system The antisymmetric matrix; S24, Establish the error state transition matrix of the error state Kalman filter based on the error state vector. , is represented as: ; S25, Construct the noise driving matrix based on the system noise model. , is represented as: ; S26, using the error state transition matrix and the noise driving matrix to propagate and update the error state covariance matrix, expressed as: ; 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.

[0008] Optionally, S3 includes: S31, construct the innovation vector based on the difference between the UWB observation vector and the predicted observation vector calculated from the predicted state, and simultaneously calculate the observation matrix based on the current predicted position, as follows: ; ; S32, continuously inject the innovation vector into the sliding window, and calculate the innovation covariance moment using the statistical information of the innovation sequence within the window, expressed as: ; S33, Calculate the observation noise covariance matrix based on the information covariance matrix and the prediction error covariance matrix, expressed as: ; S34. The observation noise covariance matrix is ​​symmetricized, and upper and lower bounds are imposed on its values ​​to generate an adaptive observation noise covariance matrix. , is represented as: ; ; S35, the optimal Kalman gain is calculated using the prediction error state covariance matrix and the adaptive observation noise covariance matrix, and the error state is updated according to the innovation vector, as shown below: ; ; ; S36, Correct the nominal state according to the updated error state, and update the posterior error covariance matrix, as shown below: ; ; in, To convert the rotation vector components in the error state vector into quaternions.

[0009] Optionally, S4 includes: S41, extract the velocity estimate in the navigation coordinate system from the nominal state of the error state Kalman filter, and calculate the rotation matrix from the navigation coordinate system to the vehicle coordinate system in combination with the current attitude, transform the velocity vector to the vehicle coordinate system, and obtain the three-axis velocity components in the vehicle coordinate system. S42, determine the current positioning stage based on the cumulative number of valid observations of the UWB positioning results, and adjust the observation noise covariance of the Z-axis pseudo-zero velocity observation. When the number of valid UWB observations is lower than the observation number threshold, configure the first observation noise covariance. When the number of valid UWB observations reaches or exceeds the observation number threshold, configure the second observation noise covariance. S43, a pseudo-zero velocity observation model is constructed with the velocity component of the Z-axis of the carrier coordinate system as the observation object, the corresponding observation matrix is ​​established, and the Kalman gain is calculated by combining the error state covariance matrix and the observation noise covariance matrix. S44. Based on the difference between the estimated Z-axis velocity and the zero value, the observation residual is constructed, and the error state vector is updated using Kalman gain. The updated error state is then injected into the nominal state to complete the state correction, while the error covariance matrix is ​​updated.

[0010] Optionally, S41 includes: S411, extract the velocity estimate of the ground robot in the navigation coordinate system at the current moment from the nominal state of the error state Kalman filter, and obtain the velocity vector in the navigation coordinate system, expressed as: ; in, For the first Ground robot in navigation coordinate system The three axial components of the velocity vector: X, Y, and Z. S412, calculate the direction cosine matrix from the navigation coordinate system to the vehicle coordinate system based on the current attitude quaternion, expressed as: ; in, Let be the attitude quaternion at the current moment. This is the transformation function for calculating the direction cosine matrix from quaternions; S413, using the rotation matrix, the velocity vector in the navigation coordinate system is transformed to the vehicle coordinate system, obtaining the three-axis velocity components in the vehicle coordinate system, expressed as: ; in, These are the three-axis velocity components.

[0011] Optionally, S42 includes: S421, the effective observation results obtained during the UWB positioning process are cumulatively counted to obtain the number of effective UWB observations at the current moment, and compared with the observation count threshold, as follows: ; ; in, As of the date The cumulative number of valid UWB observations obtained at each time point For the first Indicator variables for the validity of the UWB observations; S422, when the number of effective UWB observations is lower than the observation count threshold, the first observation noise covariance is used; when the number of effective UWB observations reaches or exceeds the observation count threshold, the second observation noise covariance is used, expressed as: ; in, For the first The observation noise covariance corresponding to the pseudo-zero velocity observation along the Z-axis at time 1. The first observation noise covariance, For the second observation noise covariance, The threshold for the number of observations; S423, construct the observation noise covariance matrix of the pseudo-zero velocity observation based on the Z-axis observation noise covariance, and use it as the observation noise parameter in the error state Kalman filter update process, expressed as: ; in, The Z-axis pseudo-zero velocity observation noise covariance matrix is ​​given.

[0012] Optionally, S43 includes: S431, a pseudo-zero velocity observation model is established with the velocity component of the Z-axis of the carrier coordinate system as the observation object. Its expected observation value is set to zero, which is used to constrain the velocity in the Z-axis direction of the carrier coordinate system. S432, construct the observation matrix based on the dimension of the error state vector, which describes the mapping relationship between the Z-axis velocity of the carrier coordinate system and the error state, expressed as: ; ; in, This is the observation matrix corresponding to the pseudo-zero velocity observation model along the Z-axis. For dimension The zero vector, For dimension The zero vector, Choose the transpose of the vector for the Z-axis; S433, after obtaining the observation matrix Then, combining the current error state covariance matrix and the Z-axis pseudo-zero velocity observation noise covariance matrix, the corresponding Kalman gain is calculated, expressed as: .

[0013] Optionally, S44 includes: S441, using zero as the expected observed value of the Z-axis velocity in the carrier coordinate system, compares it with the estimated value of the Z-axis velocity in the carrier coordinate system, and calculates the observation residual, which is used to characterize the degree to which the current Z-axis velocity deviates from zero, expressed as: ; in, Let Z be the expected observed value of the velocity along the Z-axis of the carrier coordinate system (set to 0). This is the estimated value of the Z-axis velocity in the carrier coordinate system. The residual observed at pseudo-zero velocity along the Z-axis; S442, combined with Kalman gain With observation residuals Update the error state vector estimate and, based on the observation matrix... The residual information is mapped to error states such as position, velocity, attitude, and sensor bias. The updated error states are then injected into the nominal state vector, and the error covariance matrix is ​​updated simultaneously, as shown below: ; ; ; ; ; in, Let be the error state covariance matrix. This is the error state update amount. These are the position, velocity, and sensor bias components in the nominal state vector. The attitude quaternion state, is a quaternion generated from the rotation vector in the error state.

[0014] A UWB and IMU tightly coupled positioning system based on sliding window and motion constraints, used to implement the aforementioned UWB and IMU tightly coupled positioning method based on sliding window and motion constraints, includes the following modules: UWB base station deployment and ranging module: Deploy multiple UWB base stations in the space to be measured, and obtain the distance observation values ​​between each base station and the ground robot through bilateral bidirectional ranging; Inertial data acquisition module: Acquires acceleration and angular velocity information from the ground robot's IMU; Initial positioning calculation module: Constructs a set of distance equations based on the known coordinates and distance observations of the UWB base station, and solves for the initial position coordinates of the ground robot in the navigation coordinate system using a linearized least squares algorithm; Inertial prediction module: Based on the acceleration and angular velocity collected by the IMU, it predicts the position, velocity and attitude of the ground robot through a motion prediction model; Adaptive observation noise estimation module: Constructs an innovation vector based on the difference between the UWB observation vector and the predicted state, and estimates and updates the observation noise covariance matrix online through sliding window statistical analysis; Motion constraint fusion and update module: Constructs a pseudo-zero velocity observation model of the Z-axis of the carrier coordinate system, and adaptively configures the observation noise covariance according to the positioning operation stage. It then fuses and updates the UWB observation information, inertial prediction information, and pseudo-zero velocity observation information into the error state Kalman filter to complete the positioning update of the ground robot.

[0015] The beneficial effects of this invention are: This invention introduces a sliding window-based information statistics mechanism into the UWB and IMU tightly coupled positioning framework to perform online estimation and adaptive updating of the UWB observation noise covariance. This enables the filter to dynamically adjust the observation weights according to environmental changes, thereby effectively mitigating the impact of UWB ranging errors, non-line-of-sight interference, and observation instability on positioning results and improving the robustness and stability of the UWB and IMU fusion positioning process.

[0016] This invention constructs a pseudo-zero velocity observation model along the Z-axis of the carrier coordinate system and adaptively adjusts the observation noise covariance by combining the effective number of UWB observations. This achieves continuous constraint on vertical velocity drift and suppresses the accumulation of errors in the vertical direction of the inertial navigation system without relying on additional external sensors, thereby improving the stability and reliability of the positioning system during long-term operation.

[0017] This invention combines a sliding window adaptive observation noise estimation mechanism with a motion-constrained pseudo-zero velocity observation model and achieves unified fusion within the error state Kalman filter framework. This enables UWB observation information, inertial prediction information, and motion constraint information to collaboratively participate in state updates, thereby improving the accuracy and continuity of the ground robot's positioning results and enhancing the system's autonomous navigation capability in complex environments. Attached Figure Description

[0018] 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.

[0019] Figure 1 This is a schematic diagram of the positioning method according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the system functional modules according to an embodiment of the present invention. Detailed Implementation

[0020] 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.

[0021] like Figure 1 As shown, a UWB and IMU tightly coupled localization method based on sliding window and motion constraints includes the following steps: S1. Multiple UWB base stations are deployed in the space to be measured. UWB measurement units and IMU units are installed on the ground robot to be located. The distance observation values ​​between each base station and the ground robot are obtained through bilateral bidirectional ranging. The distance observation values ​​and the acceleration and angular velocity collected by the IMU are uploaded to the host computer. A set of distance equations is constructed based on the known coordinates of the UWB base stations and the distance observation values. The initial position coordinates of the ground robot in the navigation coordinate system are solved by linearized least squares algorithm. S2, initialize the state of the Kalman filter by inputting the initial position coordinates into the error state, and predict the position, velocity and attitude of the ground robot by using the motion prediction model based on the acceleration and angular velocity collected by the IMU; S3. Construct an innovation vector based on the difference between the UWB observation vector and the predicted state. Input the innovation vector into a sliding window for statistical analysis to estimate and update the observation noise covariance matrix of the error state Kalman filter online. Then, fuse and update the UWB observation information and inertial prediction information based on the updated observation noise covariance matrix. S4. Construct a pseudo-zero velocity observation model for the Z-axis of the carrier coordinate system, and adaptively configure the observation noise covariance according to the positioning operation stage. Use the pseudo-zero velocity observation input error state Kalman filter to update the error state and correct the system state, thus completing the positioning update for the current filtering cycle.

[0022] S1 includes: S11, During the initial deployment phase, a spatial rectangular coordinate system is established as the global reference system, and multiple UWB base stations are deployed within the space to be measured. Each base station is fixedly installed at known spatial coordinates, including... , , as well as ; S12, the ground robot initiates a UWB measurement request, and obtains the distance observation values ​​between each UWB base station and the ground robot through bilateral two-way ranging. The distance observation values ​​are uploaded to the host computer via the communication link, and the acceleration and angular velocity collected by the ground robot's IMU are uploaded synchronously. S13, Let the initial position coordinates of the tag to be located be... Based on the known spatial coordinates and corresponding distance observations of each UWB base station, a set of nonlinear distance equations between the tag to be located and each base station is established using the Euclidean distance relationship between two points in space, expressed as: ; ; ; ; in, , , , These are the observed spatial distances between the first, second, third, and fourth UWB base stations and the tag to be located, respectively. S14. The nonlinear distance equations are algebraically transformed and linearized. By introducing auxiliary variables and rearranging the equations into matrix form, a linear system of equations is constructed, expressed as: ; ; ; in, Let S be the sum of the squares of the coordinates of the label to be located. For the first The sum of squares of the coordinates of each UWB base station; S15, representing the system of linear equations as The unknown variables are solved using a linear least squares algorithm to obtain the initial position coordinates of the tag to be located in the navigation coordinate system, expressed as: .

[0023] S2 includes: S21, the initial position coordinates are input into the error state Kalman filter to initialize the position, velocity, attitude, and IMU zero-bias state of the ground robot. Using the acceleration and angular velocity measured by the IMU at adjacent time points, the attitude, velocity, and position of the ground robot are recursively updated through the motion prediction model. The motion prediction model is expressed as: ; ; ; ; ; in, This is the equivalent rotation vector within the current sampling period. The relevant algorithms for converting equivalent rotation vectors into quaternions, This is the rotation matrix that transforms the vector in the vehicle coordinate system to the navigation coordinate system at the current moment. for down Time's up The velocity increment at time, for Time's up The angular increment at time t, This represents the gravitational acceleration component in the navigation coordinate system. S22, during the prediction process, the angular increment, equivalent rotation vector, and velocity increment are calculated using IMU measurements, and are expressed as follows: ; ; ; ; S23, based on the nominal state prediction, updates the error state vector, as follows: ; ; ; ; ; in, The difference between the acceleration measurement value and the zero bias of the acceleration measurement in the carrier coordinate system The antisymmetric matrix; S24, Establish the error state transition matrix of the error state Kalman filter based on the error state vector. , is represented as: ; S25, Construct the noise driving matrix based on the system noise model. , is represented as: ; S26, using the error state transition matrix and the noise driving matrix to propagate and update the error state covariance matrix, expressed as: ; 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.

[0024] S3 includes: S31, construct the innovation vector based on the difference between the UWB observation vector and the predicted observation vector calculated from the predicted state, and simultaneously calculate the observation matrix based on the current predicted position, as follows: ; ; S32, continuously inject the innovation vector into the sliding window, and calculate the innovation covariance moment using the statistical information of the innovation sequence within the window, expressed as: ; S33, Calculate the observation noise covariance matrix based on the information covariance matrix and the prediction error covariance matrix, expressed as: ; S34. The observation noise covariance matrix is ​​symmetricized, and upper and lower bounds are imposed on its values ​​to generate an adaptive observation noise covariance matrix. , is represented as: ; ; S35, the optimal Kalman gain is calculated using the prediction error state covariance matrix and the adaptive observation noise covariance matrix, and the error state is updated according to the innovation vector, as shown below: ; ; ; S36, Correct the nominal state according to the updated error state, and update the posterior error covariance matrix, as shown below: ; ; in, To convert the rotation vector components in the error state vector into quaternions.

[0025] S4 includes: S41, extract the velocity estimate in the navigation coordinate system from the nominal state of the error state Kalman filter, and calculate the rotation matrix from the navigation coordinate system to the vehicle coordinate system in combination with the current attitude, transform the velocity vector to the vehicle coordinate system, and obtain the three-axis velocity components in the vehicle coordinate system. S42, determine the current positioning stage based on the cumulative number of valid observations of the UWB positioning results, and adjust the observation noise covariance of the Z-axis pseudo-zero velocity observation. When the number of valid UWB observations is lower than the observation number threshold, configure the first observation noise covariance. When the number of valid UWB observations reaches or exceeds the observation number threshold, configure the second observation noise covariance. S43, a pseudo-zero velocity observation model is constructed with the velocity component of the Z-axis of the carrier coordinate system as the observation object, the corresponding observation matrix is ​​established, and the Kalman gain is calculated by combining the error state covariance matrix and the observation noise covariance matrix. S44. Based on the difference between the estimated Z-axis velocity and the zero value, the observation residual is constructed, and the error state vector is updated using Kalman gain. The updated error state is then injected into the nominal state to complete the state correction, while the error covariance matrix is ​​updated.

[0026] S41 includes: S411, extract the velocity estimate of the ground robot in the navigation coordinate system at the current moment from the nominal state of the error state Kalman filter, and obtain the velocity vector in the navigation coordinate system, expressed as: ; in, For the first Ground robot in navigation coordinate system The three axial components of the velocity vector: X, Y, and Z. S412, calculate the direction cosine matrix from the navigation coordinate system to the vehicle coordinate system based on the current attitude quaternion, which describes the attitude transformation relationship between the two coordinate systems, expressed as: ; in, Let be the attitude quaternion at the current moment. This is the transformation function for calculating the direction cosine matrix from quaternions; S413, using a rotation matrix, the velocity vector in the navigation coordinate system is transformed to the vehicle coordinate system, resulting in the three-axis velocity components in the vehicle coordinate system, expressed as: ; in, These are the three-axis velocity components.

[0027] S42 includes: S421, the effective observation results obtained during the UWB positioning process are accumulated and statistically analyzed to obtain the number of effective UWB observations at the current moment, and compared with the observation count threshold to determine whether the current positioning is in the initialization phase or the stable operation phase, as shown below: ; ; in, As of the date The cumulative number of valid UWB observations obtained at each time point For the first Indicator variables for the validity of the UWB observations; S422, when the number of effective UWB observations is lower than the observation count threshold, the first observation noise covariance is used; when the number of effective UWB observations reaches or exceeds the observation count threshold, the second observation noise covariance is used, expressed as: ; in, For the first The observation noise covariance corresponding to the pseudo-zero velocity observation along the Z-axis at time 1. The first observation noise covariance, For the second observation noise covariance, The threshold for the number of observations; observation number threshold Represented as: ; ; in, To ensure the effective observation ratio, To calculate the window length, For UWB ranging / positioning update frequency, This is the preset initialization convergence time window; S423, construct the observation noise covariance matrix of the pseudo-zero velocity observation based on the Z-axis observation noise covariance, and use it as the observation noise parameter in the error state Kalman filter update process, expressed as: ; in, The Z-axis pseudo-zero velocity observation noise covariance matrix is ​​given.

[0028] S43 includes: S431, a pseudo-zero velocity observation model is established with the velocity component of the Z-axis of the carrier coordinate system as the observation object. Its expected observation value is set to zero, which is used to constrain the velocity in the Z-axis direction of the carrier coordinate system. S432, construct the observation matrix based on the dimension of the error state vector, which describes the mapping relationship between the Z-axis velocity of the carrier coordinate system and the error state, expressed as: ; ; in, This is the observation matrix corresponding to the pseudo-zero velocity observation model along the Z-axis. For dimension The zero vector, For dimension The zero vector, Choose the transpose of the vector for the Z-axis; S433, after obtaining the observation matrix Then, combining the current error state covariance matrix and the Z-axis pseudo-zero velocity observation noise covariance matrix, the corresponding Kalman gain is calculated, expressed as: .

[0029] S44 includes: S441, using zero as the expected observed value of the Z-axis velocity in the carrier coordinate system, compares it with the estimated value of the Z-axis velocity in the carrier coordinate system, and calculates the observation residual, which is used to characterize the degree to which the current Z-axis velocity deviates from zero, expressed as: ; in, Let Z be the expected observed value of the velocity along the Z-axis of the carrier coordinate system (set to 0). This is the estimated value of the Z-axis velocity in the carrier coordinate system. The residual observed at pseudo-zero velocity along the Z-axis; S442, combined with Kalman gain With observation residuals Update the error state vector estimate and, based on the observation matrix... The residual information is mapped to error states such as position, velocity, attitude, and sensor bias. The updated error states are then injected into the nominal state vector, and the error covariance matrix is ​​updated simultaneously, as shown below: ; ; ; ; ; in, Let be the error state covariance matrix. This is the error state update amount. These are the position, velocity, and sensor bias components in the nominal state vector. The attitude quaternion state, is a quaternion generated from the rotation vector in the error state.

[0030] like Figure 2 As shown, a UWB and IMU tightly coupled positioning system based on sliding window and motion constraints is used to implement the aforementioned UWB and IMU tightly coupled positioning method based on sliding window and motion constraints, and includes the following modules: UWB base station deployment and ranging module: Deploy multiple UWB base stations in the space to be measured, and obtain the distance observation values ​​between each base station and the ground robot through bilateral bidirectional ranging; Inertial data acquisition module: Acquires acceleration and angular velocity information from the ground robot's IMU; Initial positioning calculation module: Constructs a set of distance equations based on the known coordinates and distance observations of the UWB base station, and solves for the initial position coordinates of the ground robot in the navigation coordinate system using a linearized least squares algorithm; Inertial prediction module: Based on the acceleration and angular velocity collected by the IMU, it predicts the position, velocity and attitude of the ground robot through a motion prediction model; Adaptive observation noise estimation module: Constructs an innovation vector based on the difference between the UWB observation vector and the predicted state, and estimates and updates the observation noise covariance matrix online through sliding window statistical analysis; Motion constraint fusion and update module: Constructs a pseudo-zero velocity observation model of the Z-axis of the carrier coordinate system, and adaptively configures the observation noise covariance according to the positioning operation stage. It then fuses and updates the UWB observation information, inertial prediction information, and pseudo-zero velocity observation information into the error state Kalman filter to complete the positioning update of the ground robot.

[0031] 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.

[0032] 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 tightly coupled positioning method based on sliding window and motion constraints, characterized in that, Includes the following steps: S1. Multiple UWB base stations are deployed in the space to be measured. UWB measurement units and IMU units are installed on the ground robot to be located. The distance observation values ​​between each base station and the ground robot are obtained through bilateral bidirectional ranging. The distance observation values ​​and the acceleration and angular velocity collected by the IMU are uploaded to the host computer. A set of distance equations is constructed based on the known coordinates of the UWB base stations and the distance observation values. The initial position coordinates of the ground robot in the navigation coordinate system are solved by the linearized least squares algorithm. S2, initialize the state Kalman filter of the initial position coordinate input error, and predict the position, velocity and attitude of the ground robot based on the acceleration and angular velocity collected by the IMU through the motion prediction model; S3. Construct an innovation vector based on the difference between the UWB observation vector and the predicted state. Input the innovation vector into a sliding window for statistical analysis to estimate and update the observation noise covariance matrix of the error state Kalman filter online. Then, fuse and update the UWB observation information and inertial prediction information based on the updated observation noise covariance matrix. S4. Construct a pseudo-zero velocity observation model for the Z-axis of the carrier coordinate system, and adaptively configure the observation noise covariance according to the positioning operation stage. Use the pseudo-zero velocity observation input error state Kalman filter to update the error state and correct the system state, thus completing the positioning update for the current filtering cycle.

2. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 1, characterized in that, S1 includes: S11, During the initial deployment phase, a spatial rectangular coordinate system is established as the global reference system, and multiple UWB base stations are deployed within the space to be measured. Each base station is fixedly installed at known spatial coordinates, including... , , as well as ; S12, the ground robot initiates a UWB measurement request, and obtains the distance observation values ​​between each UWB base station and the ground robot through bilateral two-way ranging. The distance observation values ​​are uploaded to the host computer via the communication link, and the acceleration and angular velocity collected by the ground robot's IMU are uploaded synchronously. S13, Let the initial position coordinates of the tag to be located be... Based on the known spatial coordinates and corresponding distance observations of each UWB base station, a set of nonlinear distance equations between the tag to be located and each base station is established using the Euclidean distance relationship between two points in space, expressed as: ； ； ； ； in, , , , These are the observed spatial distances between the first, second, third, and fourth UWB base stations and the tag to be located, respectively. S14, perform algebraic transformation and linearization on the nonlinear distance equation system. By introducing auxiliary variables and rearranging the equations into matrix form, construct a linear equation system, expressed as: ； ； ； in, Let S be the sum of the squares of the coordinates of the label to be located. For the first The sum of squares of the coordinates of each UWB base station; S15, the system of linear equations is expressed as The unknown variables are solved using a linear least squares algorithm to obtain the initial position coordinates of the tag to be located in the navigation coordinate system, expressed as: 。 3. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 2, characterized in that, S2 includes: S21, the initial position coordinates are input into the error state Kalman filter to initialize the position, velocity, attitude, and IMU zero-bias state of the ground robot. Using the acceleration and angular velocity measured by the IMU at adjacent time points, the attitude, velocity, and position of the ground robot are recursively updated through the motion prediction model. The motion prediction model is expressed as: ； ； ； ； ； in, This is the equivalent rotation vector within the current sampling period. The relevant algorithms for converting equivalent rotation vectors into quaternions, This is the rotation matrix that transforms the vector in the vehicle coordinate system to the navigation coordinate system at the current moment. for down Time's up The velocity increment at time, for Time's up The angular increment at time t, This represents the gravitational acceleration component in the navigation coordinate system. S22, during the prediction process, the angular increment, equivalent rotation vector, and velocity increment are calculated using IMU measurements, and are expressed as follows: ； ； ； ； S23, based on the nominal state prediction, updates the error state vector, as follows: ； ； ； ； ； in, The difference between the acceleration measurement value and the zero bias of the acceleration measurement in the carrier coordinate system The antisymmetric matrix; S24, Establish the error state transition matrix of the error state Kalman filter based on the error state vector. , is represented as: ； S25, Construct the noise driving matrix based on the system noise model. , is represented as: ； S26, using the error state transition matrix and the noise driving matrix to propagate and update the error state covariance matrix, expressed as: ； 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.

4. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 3, characterized in that, S3 includes: S31, construct the innovation vector based on the difference between the UWB observation vector and the predicted observation vector calculated from the predicted state, and simultaneously calculate the observation matrix based on the current predicted position, as follows: ； ； S32, continuously inject the innovation vector into the sliding window, and calculate the innovation covariance moment using the statistical information of the innovation sequence within the window, expressed as: ； S33, Calculate the observation noise covariance matrix based on the information covariance matrix and the prediction error covariance matrix, expressed as: ； S34. The observation noise covariance matrix is ​​symmetricized, and upper and lower bounds are imposed on its values ​​to generate an adaptive observation noise covariance matrix. , is represented as: ； ； S35, the optimal Kalman gain is calculated using the prediction error state covariance matrix and the adaptive observation noise covariance matrix, and the error state is updated according to the innovation vector, as shown below: ； ； ； S36, Correct the nominal state according to the updated error state, and update the posterior error covariance matrix, as shown below: ； ； in, To convert the rotation vector components in the error state vector into quaternions.

5. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 4, characterized in that, S4 includes: S41, extract the velocity estimate in the navigation coordinate system from the nominal state of the error state Kalman filter, and calculate the rotation matrix from the navigation coordinate system to the vehicle coordinate system in combination with the current attitude, transform the velocity vector to the vehicle coordinate system, and obtain the three-axis velocity components in the vehicle coordinate system. S42, determine the current positioning stage based on the cumulative number of valid observations of the UWB positioning results, and adjust the observation noise covariance of the Z-axis pseudo-zero velocity observation. When the number of valid UWB observations is lower than the observation number threshold, configure the first observation noise covariance. When the number of valid UWB observations reaches or exceeds the observation number threshold, configure the second observation noise covariance. S43, a pseudo-zero velocity observation model is constructed with the velocity component of the Z-axis of the carrier coordinate system as the observation object, the corresponding observation matrix is ​​established, and the Kalman gain is calculated by combining the error state covariance matrix and the observation noise covariance matrix. S44. Based on the difference between the estimated Z-axis velocity and the zero value, the observation residual is constructed, and the error state vector is updated using Kalman gain. The updated error state is then injected into the nominal state to complete the state correction, while the error covariance matrix is ​​updated.

6. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 5, characterized in that, S41 includes: S411, extract the velocity estimate of the ground robot in the navigation coordinate system at the current moment from the nominal state of the error state Kalman filter, and obtain the velocity vector in the navigation coordinate system, expressed as: ； in, For the first Ground robot in navigation coordinate system The three axial components of the velocity vector: X, Y, and Z. S412, calculate the direction cosine matrix from the navigation coordinate system to the vehicle coordinate system based on the current attitude quaternion, expressed as: ； in, Let be the attitude quaternion at the current moment. This is the transformation function for calculating the direction cosine matrix from quaternions; S413, using the rotation matrix, the velocity vector in the navigation coordinate system is transformed to the vehicle coordinate system, obtaining the three-axis velocity components in the vehicle coordinate system, expressed as: ； in, These are the three-axis velocity components.

7. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 6, characterized in that, S42 includes: S421, the effective observation results obtained during the UWB positioning process are cumulatively counted to obtain the number of effective UWB observations at the current moment, and compared with the observation count threshold, as follows: ； ； in, As of the date The cumulative number of valid UWB observations obtained at each time point For the first Indicator variables for the validity of the UWB observations; S422, when the number of effective UWB observations is lower than the observation count threshold, the first observation noise covariance is used; when the number of effective UWB observations reaches or exceeds the observation count threshold, the second observation noise covariance is used, expressed as: ； in, For the first The observation noise covariance corresponding to the pseudo-zero velocity observation along the Z-axis at time 1. The first observation noise covariance, For the second observation noise covariance, The threshold for the number of observations; S423, construct the observation noise covariance matrix of the pseudo-zero velocity observation based on the Z-axis observation noise covariance, and use it as the observation noise parameter in the error state Kalman filter update process, expressed as: ； in, The Z-axis pseudo-zero velocity observation noise covariance matrix is ​​given.

8. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 7, characterized in that, S43 includes: S431, a pseudo-zero velocity observation model is established with the velocity component of the Z-axis of the carrier coordinate system as the observation object. Its expected observation value is set to zero, which is used to constrain the velocity in the Z-axis direction of the carrier coordinate system. S432, construct the observation matrix based on the dimension of the error state vector, which describes the mapping relationship between the Z-axis velocity of the carrier coordinate system and the error state, expressed as: ； ； in, This is the observation matrix corresponding to the pseudo-zero velocity observation model along the Z-axis. For dimension The zero vector, For dimension The zero vector, Choose the transpose of the vector for the Z-axis; S433, after obtaining the observation matrix Then, combining the current error state covariance matrix and the Z-axis pseudo-zero velocity observation noise covariance matrix, the corresponding Kalman gain is calculated, expressed as: 。 9. The UWB and IMU tightly coupled positioning method based on sliding window and motion constraints according to claim 8, characterized in that, S44 includes: S441, using zero as the expected observed value of the Z-axis velocity in the carrier coordinate system, compares it with the estimated value of the Z-axis velocity in the carrier coordinate system, and calculates the observation residual, which is used to characterize the degree to which the current Z-axis velocity deviates from zero, expressed as: ； in, This represents the expected observed value of the velocity along the Z-axis of the carrier coordinate system. This is the estimated value of the Z-axis velocity in the carrier coordinate system. The residual observed at pseudo-zero velocity along the Z-axis; S442, combined with Kalman gain With observation residuals Update the error state vector estimate and, based on the observation matrix... The residual information is mapped to error states such as position, velocity, attitude, and sensor bias. The updated error states are then injected into the nominal state vector, and the error covariance matrix is ​​updated simultaneously, as shown below: ； ； ； ； ； in, Let be the error state covariance matrix. This is the error state update amount. These are the position, velocity, and sensor bias components in the nominal state vector. The attitude quaternion state, is a quaternion generated from the rotation vector in the error state.

10. A UWB and IMU tightly coupled positioning system based on sliding window and motion constraints, used to implement the UWB and IMU tightly coupled positioning method based on sliding window and motion constraints as described in any one of claims 1-9, characterized in that, Includes the following modules: UWB base station deployment and ranging module: Deploy multiple UWB base stations in the space to be measured, and obtain the distance observation values ​​between each base station and the ground robot through bilateral bidirectional ranging; Inertial data acquisition module: Acquires acceleration and angular velocity information from the ground robot's IMU; Initial positioning calculation module: Constructs a set of distance equations based on the known coordinates and distance observations of the UWB base station, and solves for the initial position coordinates of the ground robot in the navigation coordinate system using a linearized least squares algorithm; Inertial prediction module: Based on the acceleration and angular velocity collected by the IMU, it predicts the position, velocity and attitude of the ground robot through a motion prediction model; Adaptive observation noise estimation module: Constructs an innovation vector based on the difference between the UWB observation vector and the predicted state, and estimates and updates the observation noise covariance matrix online through sliding window statistical analysis; Motion constraint fusion and update module: Constructs a pseudo-zero velocity observation model of the Z-axis of the carrier coordinate system, and adaptively configures the observation noise covariance according to the positioning operation stage. It then fuses and updates the UWB observation information, inertial prediction information, and pseudo-zero velocity observation information into the error state Kalman filter to complete the positioning update of the ground robot.

Images