An IMU / UWB tight coupling method based on constraint and geometric enhancement

Abstract

This invention discloses a tightly coupled IMU / UWB method based on constraints and geometric enhancement, comprising the following steps: S1, robustly smoothing the original ranging vector of multi-channel UWB into a smoothed ranging vector of multi-channel UWB; S2, constructing a tightly coupled state model; S3, based on IMU inertial propagation state prediction, constructing the state prediction of all UWB anchor points at time [time value missing]. k The UWB measurement residual vector and UWB linear observation matrix were obtained; and by introducing different kinematic constraints, lateral position constraints, and geometric enhancement operations to the mobile platform, the mobile platform at time t was constructed respectively. k The corresponding measurement vectors and linear observation matrix; S4, using an extended Kalman filter to measure the mobile platform at time... k The state vector is corrected and updated, and then fed back to the main state of the inertial navigation for correction to obtain the final positioning result. This method has high reliability, strong versatility, high algorithm efficiency, simple operation, high accuracy, and good practicality.

Description

Technical Field

[0001] This invention relates to the field of IMU / UWB data fusion technology, and in particular to a tight coupling method for IMU / UWB based on constraints and geometric enhancement. Background Technology

[0002] Ultra-wideband (UWB) positioning is a wireless technology that uses nanosecond-level narrow pulse signals to achieve centimeter-level high-precision positioning. However, due to its inherent characteristics and environmental factors, UWB positioning is prone to fluctuations in ranging results in complex scenarios such as non-line-of-sight obstruction. Therefore, directly using UWB ranging or its calculated position information for positioning can easily lead to trajectory jumps and discontinuous calculations. Inertial Measurement Units (IMUs) have the advantages of high-frequency, continuous output, providing stable motion information for short periods. However, their zero bias and noise accumulate and drift after integration, making it difficult to achieve high-precision positioning independently over long periods. Therefore, existing technologies often combine UWB positioning with IMU positioning to achieve accurate positioning using IMU / UWB fusion methods. However, in existing IMU / UWB fusion methods, loose coupling is highly dependent on the quality of UWB positioning and lacks robustness in the face of ranging anomalies or geometric degradation. In contrast, tightly coupled methods directly utilize UWB ranging information in state estimation, which can more fully leverage the complementary advantages of multi-source information. Therefore, how to achieve continuous, stable and high-precision positioning based on a tightly coupled framework in GNSS-denied environments such as underground parking garages and indoor corridors remains an urgent problem to be solved in the field of integrated navigation.

[0003] Currently, IMU / UWB tightly coupled methods are mainly divided into two categories: filtering-based tightly coupled methods and optimization-based tightly coupled methods. For example, the published patent CN120651247B provides an IMU / UWB integrated navigation method based on nonlinear error definition. This method constructs an observation model of nonlinear error variables based on the UWB observation equation, but lacks effective utilization of IMU motion constraint information. Similarly, the published patent CN113324544B provides a graph optimization-based UWB / IMU indoor mobile robot cooperative localization method. When UWB ranging is interfered with, it uses the LM algorithm to optimize the robot's current pose, but it also does not further utilize IMU constraint information or UWB geometric information.

[0004] Therefore, it is necessary to design an IMU / UWB tightly coupled method that introduces motion constraints and combines them with geometric enhancement mechanisms in a tightly coupled framework, so that the system can still maintain continuous and high-precision positioning performance under conditions of ranging anomalies or geometric degradation. Summary of the Invention

[0005] The purpose of this invention is to provide a constraint- and geometry-enhanced IMU / UWB tight coupling method that solves the problem of insufficient horizontal positioning accuracy in existing IMU / UWB-based tight coupling methods.

[0006] Therefore, the technical solution of the present invention is as follows:

[0007] A tightly coupled IMU / UWB method based on constraints and geometry enhancement, comprising the following steps:

[0008] S1. Obtain the original ranging vector of the multi-channel UWB and perform robust smoothing to obtain the smoothed ranging vector of the multi-channel UWB.

[0009] S2. Based on the IMU data of the mobile platform, construct a tightly coupled state model to obtain the state estimation vector of the mobile platform at time k.

[0010] S3. Based on IMU inertial propagation state prediction, construct the UWB measurement residual vector and UWB linear observation matrix of all UWB anchor points at time k; and by introducing different kinematic constraints to the mobile platform, construct the zero-velocity pseudo-measurement vector and zero-velocity update linear observation matrix, or the NHC pseudo-measurement vector and nonholonomic constraint linear observation matrix of the mobile platform at time k; by introducing lateral position constraints, construct the lateral position scalar pseudo-measurement vector and lateral position constraint linear observation matrix of the mobile platform at time k; by introducing geometric augmentation operations, construct the geometric pseudo-position measurement vector and geometric pseudo-position linear observation matrix of the mobile platform at time k.

[0011] S4. Construct the complete observation vector, complete observation matrix, and complete measurement noise covariance matrix at time k in sequence, and use the extended Kalman filter to correct and update the state vector of the mobile platform at time k; feed back the updated posterior state estimation vector of the mobile platform at time k to the inertial navigation master state for correction, and obtain the final positioning result.

[0012] Furthermore, in step S1, the robust smoothing process for the raw ranging values ​​collected at consecutive times of the i-th UWB anchor point is as follows:

[0013] 1) Based on the original ranging vectors of the acquired multi-channel UWB, determine the original ranging values ​​acquired by the i-th UWB anchor point at continuous time intervals.

[0014] 2) For any acquisition time k, set a sliding window centered on k with an odd length L, and construct a regression model expression for the original ranging value of the i-th UWB anchor point at any time j within the sliding window:

[0015] ,

[0016] In the formula, Let be the original ranging value of the i-th UWB anchor point at time j. This is the set of time indices within a sliding window centered at time k. The relative time offset of time j within the sliding window with respect to the window center time k is determined by the sampling time and is a known quantity. relative time offset The nth power, Let q be the coefficient of the q-th term in the regression model for the i-th UWB anchor point, where q is the polynomial order and its value ranges from 0 to 1. ;

[0017] 3) Definition The expression is: ,in, The coefficient of the constant term, The coefficients of the first-order term, For the first Order term coefficients;

[0018] 4) Stack the original distance measurements at each time point within the sliding window into a vector form, and use the weighted least squares method to estimate the coefficients of the q-th order term of the regression model, thereby obtaining the solution. Its constant term coefficient That is, the smoothed ranging value of the i-th UWB anchor point at time k. .

[0019] Furthermore, in step 4, the estimation expression for the coefficient of the q-th term of the regression model using the weighted least squares method is as follows:

[0020] ,

[0021] In the formula, Let be the polynomial coefficient vector of the i-th UWB anchor point within a sliding window centered at time k. The distance vector is formed by stacking all the original distance measurement values ​​of the i-th UWB anchor point in chronological order within a sliding window centered at time k. Let Vandermonde be the Vandermonde matrix of the i-th UWB anchor point within a sliding window centered at time k, consisting of the time offsets of all sampling times relative to the window center time k. The diagonal weighted matrix is ​​composed of IRLS weights. The diagonal elements of the diagonal weighted matrix are the iterative reweighted least squares weights of all the original ranging values ​​of the i-th UWB anchor point within the sliding window centered at time k, and the remaining elements are 0.

[0022] The diagonal elements of the diagonal weighted matrix are obtained iteratively, and the specific steps are as follows:

[0023] 1) Set the initial weight of each original ranging value in the sliding window to 1 to construct an initial diagonal weighted matrix. Based on the initial diagonal weighted matrix, calculate the initial estimate of the polynomial coefficient vector of the i-th UWB anchor point in the sliding window centered at time k.

[0024] 2) Calculate the residuals of each original distance measurement value within the sliding window. And update the IRLS weights corresponding to each original ranging value based on the Huber truncation function;

[0025] Among them, residual The expression is:

[0026] ,

[0027] In the formula, For the first The estimated coefficient of the q-th term in the nth iteration. The relative time offset of time 𝑗 within the sliding window relative to the time 𝑘 at the center of the window;

[0028] The Huber truncation function is used to update the IRLS adaptive weights of each original ranging value. Its expression is as follows:

[0029] ,

[0030] In the formula, For the first The Huber truncation parameter corresponding to the next iteration. std(⋅) represents the standard deviation operator;

[0031] Furthermore, construct the first The expression for the diagonal weighted matrix corresponding to the next iteration is:

[0032] ,

[0033] 3) Using the updated IRLS weights, reconstruct the diagonal weighted matrix and calculate the first polynomial coefficient vector of the i-th UWB anchor point within a sliding window centered at time k. The estimated value of the next iteration;

[0034] 4) Repeat steps 2)-3) above until the i-th UWB anchor point is in the polynomial coefficient vector of the sliding window centered at time k. The optimal IRLS weights are obtained when the estimated values ​​converge in the next iteration.

[0035] Furthermore, in step S2, the expression for the tightly coupled state model is:

[0036] ,

[0037] In the formula, This is the state estimation vector of the mobile platform at time k. Let k be the state transition matrix of the mobile platform from time k−1 to time k. This is the state estimation vector of the mobile platform at time k−1; This represents the process noise vector of the mobile platform at time k.

[0038] Wherein, the state estimation vector of the mobile platform at time k , , , These are the position error vector, velocity error vector, and attitude error vector in the navigation coordinate system, respectively. Let k be the gyroscope zero bias error at time k. Let k be the accelerometer zero bias error at time k; each element can be calculated using the dynamic equation of the state error vector of the moving platform under the assumption of small attitude error:

[0039] ,

[0040] In the formula, The angular velocity of Earth's rotation. The angular velocity of the navigation coordinate system relative to the Earth coordinate system. Let this be the force vector in the navigation coordinate system. Let the direction cosine matrix be the distance from the body coordinate system to the navigation coordinate system. The zero-bias first-order Markov correlation time constant of the gyroscope. The zero-bias first-order Markov correlation time constant of the accelerometer To achieve zero bias noise for the gyroscope, To achieve zero bias noise in the accelerometer; , , , , They are respectively , , , , The derivative with respect to time.

[0041] Furthermore, in step S3, the steps for constructing the UWB measurement residual vector and the UWB linear observation matrix for all UWB anchor points at time k are as follows:

[0042] For the nth UWB anchor point, its distance measurement model in the navigation coordinate system is as follows:

[0043] ,

[0044] In the formula, Let k be the distance measurement observation value of the i-th UWB anchor point at time k. Let k be the smoothed ranging value of the i-th UWB anchor point at time k. Let be the position vector of the vehicle tag at time k in the navigation coordinate system. Let be the position vector of the i-th UWB anchor point in the navigation coordinate system. To calculate the Euclidean distance between position vectors, The ranging noise of the i-th UWB anchor point at time k;

[0045] Then, the UWB measurement residual vector of all UWB anchor points at time k The constructed expression is:

[0046] ,

[0047] In the formula, Let UWB measurement residual vector be at time t. The predicted position vector is obtained based on the state vector of the mobile platform at time k in step S2;

[0048] Furthermore, the linear observation matrix of the i-th UWB anchor point at time k The construct expression is:

[0049] ,

[0050] In the formula, It is a zero matrix with 1 row and 12 columns.

[0051] Furthermore, the linear observation matrix of all UWB anchor points at time k The UWB linear observation matrix at time k is stacked row by row. Its expression is:

[0052] .

[0053] Furthermore, in step S3, the step of introducing different kinematic constraints to the mobile platform is as follows:

[0054] 1) When the mobile platform is stationary, a zero-velocity update is introduced to obtain the zero-velocity pseudo-measurement vector of the mobile platform at time k. Its expression is:

[0055] ,

[0056] In the formula, Let K be the true velocity vector of the mobile platform at time k in the navigation coordinate system. The measurement noise of ZUPT for the mobile platform at time k;

[0057] Furthermore, a zero-rate update linear observation matrix for the mobile platform at time k is constructed. Its expression is:

[0058] ,

[0059] In the formula, It is a 3×3 identity matrix. It is a 3×3 zero matrix;

[0060] 2) When the mobile platform is in motion and its absolute longitudinal velocity in the carrier coordinate system is greater than a preset velocity threshold, a nonholonomic constraint is introduced to obtain the NHC pseudo-measurement vector of the mobile platform at time k. Its expression is:

[0061] ,

[0062] In the formula, and These represent the lateral and vertical velocity components of the mobile platform at time k in the navigation coordinate system. The NHC measurement noise vector of the mobile platform at time k;

[0063] Furthermore, a nonholonomic constrained linear observation matrix of the mobile platform at time k is constructed. Its expression is:

[0064] ,

[0065] In the formula, Let be the direction cosine matrix from the navigation coordinate system to the vehicle coordinate system at time k. This is the selection matrix used to extract the lateral and vertical velocity components in the carrier coordinate system. It is a 2×3 zero matrix.

[0066] Furthermore, in step S3, when introducing lateral position constraints for the mobile platform,

[0067] 1) Determine whether the mobile platform is in a straight section. The determination criteria are: the yaw rate amplitude of the mobile platform is lower than the preset threshold and its speed is higher than the preset minimum speed.

[0068] 2) By introducing a method to suppress lateral drift in the straight section, a scalar pseudo-measurement vector of the lateral position of the moving platform at time k is obtained. Its expression is:

[0069] ,

[0070] In the formula, This is the transpose of the unit normal vector in the horizontal plane corresponding to the heading direction. Let k be the position vector of the mobile platform in the navigation coordinate system at time k. To detect the reference position vector of the mobile platform at the start of the straight-line segment, Measurement noise constrained for the position of the mobile platform in the straight segment at time k;

[0071] 3) Furthermore, based on the scalar pseudo-measurement vector of the lateral position of the mobile platform at time k, a linear observation matrix constrained by lateral position is constructed. Its expression is:

[0072] ,

[0073] In the formula, This is the transpose of the unit normal vector in the horizontal plane corresponding to the heading direction. It is a 1×12 zero matrix.

[0074] Furthermore, in step S3, when introducing the geometric enhancement operation,

[0075] 1) At each time k where effective UWB ranging exists, perform geometric augmentation on the ranging observation value of the corresponding UWB anchor point at time k to obtain the geometrically augmented position estimation vector of the mobile platform at time k. Its expression is:

[0076] ,

[0077] In the formula, This is the geometrically augmented position estimation vector of the mobile platform at time k. To optimize variables, Let k be the set of anchor point indices where valid distance measurement exists. Let k be the distance measurement observation value of the i-th UWB anchor point at time k. Let be the position vector of the i-th UWB anchor point in the navigation coordinate system. To extend the Kalman filter to predict the inertial position vector of the mobile platform at time k, The variance of UWB ranging noise. The variance of the uncertainty corresponding to the prior inertial position;

[0078] 2) Calculate the equivalent Fisher information matrix at the current time k. The expression is:

[0079] ,

[0080] In the formula, Let k be the line-of-sight unit vector pointing from the i-th UWB anchor point to the geometrically augmented position estimate at time k. It is a 3×3 identity matrix;

[0081] 3) Estimate the geometrically augmented position of the mobile platform at the i-th UWB anchor point. The pseudo-position measurement is injected into the extended Kalman filter and represented as the geometric pseudo-position measurement vector of the mobile platform at time k. Its expression is:

[0082] ,

[0083] In the formula, Let k be the position vector of the mobile platform in the navigation coordinate system at time k. Let K be the geometric pseudo-measurement noise vector at time k, and its covariance satisfies: .

[0084] Furthermore, a linear observation matrix of the geometric pseudo-position of the mobile platform at time k is constructed. Its expression is:

[0085] .

[0086] Furthermore, the specific implementation steps of step S4 are as follows:

[0087] S401. Construct the complete observation vector at time k. Complete observation matrix and complete measurement noise covariance matrix ;

[0088] S402. Using an extended Kalman filter, the state vector of the mobile platform at time k is corrected and updated to obtain the posterior state estimate vector of the mobile platform at time k. Its expression is:

[0089] ,

[0090] In the formula, Let Kalman gain be at time k. Let be the prior state estimation vector of the mobile platform at time k, which is the state vector estimate of the mobile platform at time k obtained by substituting the posterior state estimation vector of the mobile platform at time k−1 into the tightly coupled state model. Let be the posterior state estimation matrix of the mobile platform at time k; Let be the prior state error covariance matrix at time k. Let be the posterior state error covariance matrix at time k; The complete observation matrix at time k Let k be the measurement noise covariance matrix corresponding to all valid measurement vectors at time k. This is the complete observation vector at time k. It is a 15×15 identity matrix;

[0091] S403. The posterior state estimation vector of the mobile platform at time k. Feedback is sent to the inertial navigation master state to correct position, velocity, attitude, and sensor bias, resulting in the final positioning result; among which,

[0092] Based on inertial propagation, the navigation master state is used to obtain the predicted position at time k. Prediction speed Predicted pose matrix Predicting gyroscope zero bias and predicting accelerometer zero bias ; and combine it with the posterior state estimation vector of the mobile platform at time k obtained from step S402. Substitute into the correction expression:

[0093] ,

[0094] In the formula, These are the posterior state estimation vectors of the moving platform at time k. The components of position error, velocity error, attitude error, gyroscope bias error, and accelerometer bias error; This represents an antisymmetric matrix constructed from small-angle error vectors;

[0095] Furthermore, the output is obtained from the position ,speed Attitude matrix gyroscope zero bias The final positioning result is determined by the time k formed by the zero bias of the accelerometer.

[0096] Compared with existing technologies, this constraint- and geometry-enhanced IMU / UWB tightly coupled method solves the problem of insufficient positioning accuracy in existing IMU / UWB combined positioning methods. This method first performs robust smoothing on the raw UWB ranging data to suppress abnormal ranging errors. Then, it constructs a tightly coupled state model based on the smoothed UWB ranging results and IMU data. Subsequently, it predicts the state based on IMU inertial propagation and introduces motion and geometric constraints. Finally, it uses enhanced observations for correction and update, outputting a stable positioning result. This method has high reliability, strong versatility, high algorithm efficiency, simple operation, high accuracy, and good practicality. Attached Figure Description

[0097] Figure 1The flowchart shows the IMU / UWB tight coupling method based on constraint and geometry enhancement of the present invention.

[0098] Figure 2 The image shows the effect of a positioning experiment using the constraint- and geometry-enhanced IMU / UWB tight coupling method of this invention.

[0099] Figure 3 This is a comparison chart of errors in positioning experiments conducted using the constraint- and geometry-enhanced IMU / UWB tight coupling method of this invention. Detailed Implementation

[0100] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the following embodiments are by no means intended to limit the present invention.

[0101] See Figure 1 The specific implementation steps of this constraint- and geometry-enhanced IMU / UWB tightly coupled method are described below.

[0102] In this embodiment, the specific application scenario of this method is a mobile platform equipped with an IMU and an onboard UWB tag, specifically an unmanned vehicle. Accordingly, four UWB anchor points are set around the perimeter of the unmanned vehicle's movement area. These four UWB anchor points form a quadrilateral layout in the horizontal plane, preferably located at the four boundaries of the area to be located. The position coordinates of each UWB anchor point are obtained through pre-measurement and are known quantities. When the unmanned vehicle travels within the quadrilateral area enclosed by the four UWB anchor points, a multi-channel ranging constraint is formed between the onboard UWB tag and each anchor point, thereby providing basic observation information for subsequent processing.

[0103] S1. Obtain the original ranging vector of the multi-channel UWB, and after robust smoothing, obtain the smoothed ranging vector of the multi-channel UWB to suppress abnormal ranging errors.

[0104] The specific implementation steps of step S1 are described below.

[0105] S101. Obtain the multi-channel UWB raw ranging vector at time k, the expression of which is:

[0106] ,

[0107] In the formula, Let be the original ranging value of the i-th UWB anchor point at time k. ; k is the time, k=1,…,N.

[0108] S102. Robustly smooth the original multi-channel UWB ranging vector at time k to obtain the smoothed multi-channel UWB ranging vector at time k, the expression of which is:

[0109] ,

[0110] In the formula, Let k be the smoothed ranging value of the i-th UWB anchor point at time k. ; k is the time, k=1,…,N.

[0111] In this step, taking the i-th UWB anchor point as an example, the robust smoothing of the raw ranging values ​​collected at continuous time points is performed by calculating the raw ranging values ​​of the i-th UWB anchor point using a locally weighted multinomial regression method within a sliding window centered on time k.

[0112] Specifically, the specific operational steps of the robust smoothing process described above are as follows.

[0113] S1021. Based on the original ranging vectors of the acquired multi-channel UWB, determine the original ranging values ​​acquired by the i-th UWB anchor point at consecutive time intervals.

[0114] S1022. Set a sliding window with time k as the center and length L; where L is an odd number to ensure that the center of the window is unique; in a preferred embodiment, 𝐿 is set to 11, then the sliding window contains 11 consecutive sampling points, that is, it contains the original ranging values ​​collected at 11 consecutive time points.

[0115] Let 𝐿=2ℎ+1, then the set of time indices within the sliding window centered at time 𝑘 can be represented as:

[0116] ,

[0117] Within the sliding window, the regression model expression for the original ranging value of the i-th UWB anchor point at any time j is:

[0118] ,

[0119] In the formula, Let be the original ranging value of the i-th UWB anchor point at time j. This is the set of time indices within a sliding window centered at time k. The relative time offset of time j within the sliding window with respect to the window center time k is determined by the sampling time and is a known quantity. relative time offset The nth power, Let q be the coefficient of the q-th term in the regression model for the i-th UWB anchor point, where q is the polynomial order and its value ranges from 0 to 1. ;

[0120] S1023. Define the polynomial coefficient vector of the i-th UWB anchor point within a sliding window centered at time k. The expression is:

[0121] ,

[0122] In the formula, The coefficient of the constant term, The coefficients of the first-order term, For the first Order term coefficients.

[0123] S1024. Stack the original distance measurements at each time point within the sliding window into a vector form, and use the weighted least squares method to estimate the coefficient of the q-th term of the regression model. The estimation expression is as follows:

[0124] ,

[0125] In the formula, Let be the polynomial coefficient vector of the i-th UWB anchor point within a sliding window centered at time k. The distance vector is formed by stacking all the original distance measurement values ​​of the i-th UWB anchor point in chronological order within a sliding window centered at time k. Let Vandermonde be the Vandermonde matrix of the i-th UWB anchor point within a sliding window centered at time k, consisting of the time offsets of all sampling times relative to the window center time k. It is a diagonal weighted matrix composed of IRLS weights. The diagonal elements of the diagonal weighted matrix are the iterative reweighted least squares weights of all the original ranging values ​​of the i-th UWB anchor point within the sliding window centered at time k, and the remaining elements are 0.

[0126] Since the polynomial uses a local time coordinate system with the center k of the sliding window as the origin, the smoothing distance measurement vector of the i-th UWB anchor point at time k can be directly given by the polynomial constant term, and its expression is:

[0127] ;

[0128] Based on this, we first obtain the solution by estimating the expression. , and then take In As That is, the smoothed distance measurement value of the i-th UWB anchor point at time k.

[0129] In the above diagonal weighted matrix, the specific iterative method for obtaining the diagonal elements is as follows:

[0130] 1) Set the initial weight of each original ranging value in the sliding window to 1 to construct an initial diagonal weighted matrix. Based on the initial diagonal weighted matrix, calculate the initial estimate of the polynomial coefficient vector of the i-th UWB anchor point in the sliding window centered at time k.

[0131] 2) Calculate the residuals of each original distance measurement value within the sliding window. And update the IRLS weights corresponding to each original ranging value based on the Huber truncation function;

[0132] Among them, residual The expression is:

[0133] ,

[0134] In the formula, Let be the original ranging value of the i-th UWB anchor point at time j. For the first The estimated coefficient of the q-th term in the nth iteration. The relative time offset of time 𝑗 within the sliding window with respect to the window center time 𝑘 is determined by the sampling time and is a known quantity;

[0135] The Huber truncation function is used to update the IRLS adaptive weights corresponding to each original ranging value. Its expression is as follows:

[0136] ,

[0137] In the formula, For the first The Huber truncation parameter corresponding to the next iteration can be adaptively determined based on the residual statistics within the current sliding window. In this embodiment, The calculation expression is:

[0138] ,

[0139] In the formula, std(⋅) represents the standard deviation operator.

[0140] Based on this, the first can be constructed The expression for the diagonal weighted matrix corresponding to the next iteration is:

[0141] ,

[0142] 3) Using the updated IRLS weights, reconstruct the diagonal weighting matrix, and based on the reconstructed diagonal weighting matrix, calculate the first polynomial coefficient vector of the i-th UWB anchor point within a sliding window centered at time k. The estimated value of the next iteration;

[0143] 4) Repeat steps 2)-3) above until the i-th UWB anchor point is in the polynomial coefficient vector of the sliding window centered at time k. The estimated values ​​converge after each iteration, and the final updated IRLS weights are the optimal IRLS weights. The corresponding i-th UWB anchor point is in the polynomial coefficient vector of the sliding window centered at time k. The constant term coefficient extracted from the next iteration estimate is also the smoothed ranging value of the i-th UWB anchor point at time k obtained in the final solution. .

[0144] Using the method described above, robust smoothing is performed on the raw ranging values ​​collected at continuous time points for all UWB anchor points (i=1,⋯,c) to obtain the smoothed ranging vector of multi-channel UWB.

[0145] S2. Based on the IMU data of the mobile platform, construct a tightly coupled state model to obtain the state estimation vector of the mobile platform at time k.

[0146] Specifically, the expression for the tightly coupled state model is:

[0147] ,

[0148] In the formula, This is the state estimation vector of the mobile platform at time k. Let k be the state transition matrix of the mobile platform from time k−1 to time k. This is the state estimation vector of the mobile platform at time k−1; Let be the process noise vector of the mobile platform at time k. Generally, the posterior state estimation vector of the mobile platform at time k−1 is substituted.

[0149] In the tightly coupled state model, the state estimation vector of the mobile platform at time k It can be represented as:

[0150] ,

[0151] In the formula, , , These are the position error vector, velocity error vector, and attitude error vector in the navigation coordinate system (n-frame), respectively. Let k be the gyroscope zero bias error at time k. Let be the accelerometer zero bias error at time k.

[0152] in, Each element can be calculated using the state error vector dynamics equation of the mobile platform under the assumption of small attitude error, and its expression is:

[0153] ,

[0154] In the formula, The angular velocity of Earth's rotation. The angular velocity of the navigation coordinate system relative to the Earth coordinate system. Let this be the force vector in the navigation coordinate system. Let the direction cosine matrix be the coordinate matrix from the body coordinate system (b-frame) to the navigation coordinate system (n-frame). The zero-bias first-order Markov correlation time constant of the gyroscope. The zero-bias first-order Markov correlation time constant of the accelerometer To achieve zero bias noise for the gyroscope, For accelerometer zero bias noise; the operation symbols in the formula This represents the vector cross product operation.

[0155] in, , , , and These represent the position error vectors respectively. Velocity error vector Attitude error vector Gyroscope zero bias error and accelerometer zero bias error The derivative with respect to time has a continuous-time form.

[0156] S3. Based on IMU inertial propagation state prediction, construct the UWB measurement residual vector and UWB linear observation matrix of all UWB anchor points at time k; and by introducing different kinematic constraints to the mobile platform, construct the zero-velocity pseudo-measurement vector and zero-velocity update linear observation matrix, or the NHC pseudo-measurement vector and nonholonomic constraint linear observation matrix of the mobile platform at time k; by introducing lateral position constraints, construct the lateral position scalar pseudo-measurement vector and lateral position constraint linear observation matrix of the mobile platform at time k; by introducing geometric enhancement operations, construct the geometric pseudo-position measurement vector and geometric pseudo-position linear observation matrix of the mobile platform at time k.

[0157] The specific implementation steps of step S3 are as follows:

[0158] S301. Based on the distance measurement model of each UWB anchor point in the navigation coordinate system, the UWB measurement residual vector and UWB linear observation matrix of all UWB anchor points at time k.

[0159] In UWB / IMU tight coupling, according to the measurement model expression:

[0160] ,

[0161] In the formula, Let be the measurement vector at time t. Let be the observation matrix at time t. Let k be the state vector of the mobile platform at time k. Let k be the measurement noise vector at time k;

[0162] For the nth UWB anchor point, its distance measurement model in the navigation coordinate system (n-frame) is as follows:

[0163] ,

[0164] In the formula, Let k be the distance measurement observation value of the i-th UWB anchor point at time k. Let k be the smoothed ranging value of the i-th UWB anchor point at time k. Let be the position vector of the vehicle tag at time k in the navigation coordinate system. Let be the known position vector of the i-th UWB anchor point in the navigation coordinate system. To calculate the Euclidean distance between position vectors, Let be the ranging noise of the i-th UWB anchor point at time k.

[0165] Furthermore, the UWB measurement residual vector of all UWB anchor points at time k is constructed. The expression is:

[0166] ,

[0167] In the formula, Let UWB measurement residual vector be at time t. The predicted position vector is obtained based on the state vector of the mobile platform at time k in step S2, and is a known quantity; the value obtained in this step... This refers to the UWB measurement residual components contained in the complete observation vector in step S4.

[0168] Construct the linear observation matrix of the i-th UWB anchor point at time k. Its expression is:

[0169] ,

[0170] In the formula, It is a zero matrix with 1 row and 12 columns.

[0171] Furthermore, the linear observation matrix of each UWB anchor point at time k is... Stacking them row by row yields the UWB linear observation matrix at time k. Its expression is:

[0172] .

[0173] S302. Based on the motion state of the mobile platform, different kinematic constraints are introduced to the velocity state of the mobile platform to construct the zero-velocity pseudo-measurement vector and zero-velocity update linear observation matrix, or the NHC pseudo-measurement vector and nonholonomic constraint linear observation matrix of the mobile platform at time k.

[0174] In this step, depending on the different velocity states of the motion platform, Zero-Velocity Update (ZUPT) pseudo-measures and Nonholonomic Constraints (NHC) pseudo-measures are introduced and used as kinematic constraints for the velocity state of the mobile platform.

[0175] Case 1: When the mobile platform is stationary, Zero-rate Update (ZUPT) is introduced to obtain the zero-rate pseudo-measurement vector of the mobile platform at time k. Its expression is:

[0176] ,

[0177] In the formula, Let K be the true velocity vector of the mobile platform at time k in the navigation coordinate system. The measurement noise of ZUPT for the mobile platform at time k.

[0178] Furthermore, based on the zero-velocity pseudo-measurement vector of the mobile platform at time k, a zero-velocity update linear observation matrix of the mobile platform at time k is constructed. Its expression is:

[0179] ,

[0180] In the formula, It is a 3×3 identity matrix. It is a 3×3 zero matrix.

[0181] Case 2: When the mobile platform is in motion and its absolute longitudinal velocity in the carrier coordinate system (b-frame) is greater than a preset velocity threshold, the mobile platform is deemed to meet the applicable conditions of the nonholonomic constraint (NHC). While approximating that the lateral and vertical velocities of the mobile platform in the carrier coordinate system (b-frame) are zero, a nonholonomic constraint is introduced to obtain the NHC pseudo-measurement vector of the mobile platform at time k. Its expression is:

[0182] ,

[0183] In the formula, and These represent the lateral and vertical velocity components of the mobile platform at time k in the navigation coordinate system. Let NHC be the noise vector of the mobile platform at time k.

[0184] Furthermore, based on the NHC pseudo-measurement vector of the mobile platform at time k, a nonholonomic constrained linear observation matrix of the mobile platform at time k is constructed. Its expression is:

[0185] ,

[0186] In the formula, Let be the direction cosine matrix from the navigation coordinate system to the vehicle coordinate system at time k, and let be a known quantity. This is the selection matrix used to extract the lateral and vertical velocity components in the carrier coordinate system. It is a 2×3 zero matrix.

[0187] In scenario 2 above, when the mobile platform remains in contact with the ground and does not experience significant sideslip, jumping, or vertical instability, i.e., when it has a certain longitudinal speed (a speed higher than the preset speed threshold), it can be more reliably considered to meet the basic motion characteristics of NHC. This is unrelated to the forward or backward state of the mobile platform, or its acceleration or deceleration state. For example, when the platform is stationary, crawling at extremely low speed, or at a very low speed, the constraint that the lateral / vertical speed is approximately zero may still hold in form, but the effectiveness and stability of NHC for filter updates will significantly decrease because the influence of measurement noise, attitude disturbances, and local slippage is relatively more significant at this time.

[0188] S303. By introducing lateral position constraints, lateral drift suppression is performed on the mobile platform in the straight section to obtain the lateral position scalar pseudo-measurement vector of the mobile platform at time k and the linear observation matrix of the lateral position constraints.

[0189] The specific implementation steps of step S303 are as follows:

[0190] 1) Determine whether the mobile platform is in a straight section. The determination criteria are: the yaw rate amplitude of the mobile platform is lower than the preset threshold and its speed is higher than the preset minimum speed. Generally, the preset threshold is set to 5° / s and the minimum speed is set to 1m / s.

[0191] 2) By introducing a method to suppress lateral drift in the straight section, a scalar pseudo-measurement vector of the lateral position of the moving platform at time k is obtained. Its expression is:

[0192] ,

[0193] In the formula, This is the transpose of the unit normal vector in the horizontal plane corresponding to the heading direction. Let k be the position vector of the mobile platform in the navigation coordinate system at time k. To detect the reference position vector of the mobile platform at the start of the straight-line segment, Measurement noise constrained for the position of the mobile platform in the straight segment at time k.

[0194] 3) Furthermore, based on the scalar pseudo-measurement vector of the lateral position of the mobile platform at time k, a linear observation matrix constrained by lateral position is constructed. Its expression is:

[0195] ,

[0196] In the formula, This is the transpose of the unit normal vector in the horizontal plane corresponding to the heading direction. It is a 1×12 zero matrix.

[0197] S304. At each time k where effective UWB ranging exists, the ranging observation value of the corresponding UWB anchor point at time k is geometrically augmented to obtain the geometrically augmented position estimation vector of the mobile platform at time k.

[0198] The moment k in which effective UWB ranging exists is defined as when at least one UWB anchor point outputs a smooth ranging value at that moment k.

[0199] Furthermore, at each time k where effective UWB ranging exists, the inertial predicted position of the mobile platform at time k is geometrically augmented to obtain the geometrically augmented position estimation vector of the mobile platform at time k. Its expression is:

[0200] ,

[0201] In the formula, This is the geometrically augmented position estimation vector of the mobile platform at time k. To optimize variables, Let k be the set of anchor point indices where valid distance measurement exists. Let k be the distance measurement observation value of the i-th UWB anchor point at time k. Let be the known position vector of the i-th UWB anchor point in the navigation coordinate system. To extend the Kalman filter to predict the inertial position vector of the mobile platform at time k, The variance of UWB ranging noise. Let V be the variance of the uncertainty corresponding to the prior inertial position. Among these, the optimization variables... The corresponding calculation was obtained through iterative solution. That is, the final calculated position estimation vector of the mobile platform after geometric enhancement at time k.

[0202] S305. To quantitatively characterize the geometric configuration of the UWB anchor point and the constraint strength of inertial prior on local positioning, construct the equivalent Fisher information matrix at time k.

[0203] Specifically, the equivalent Fisher information matrix The expression is:

[0204] ,

[0205] In the formula, Let k be the line-of-sight unit vector pointing from the i-th UWB anchor point to the geometrically augmented position estimate at time k. It is a 3×3 identity matrix.

[0206] The equivalent Fisher information matrix It is used to characterize the constraint strength of the geometrically enhanced position estimate at the current time k, and its inverse matrix can be used to approximate the uncertainty of the geometrically enhanced position estimate.

[0207] S306. Construct the geometric pseudo-position measurement vector and geometric pseudo-position linear observation matrix of the mobile platform at time k.

[0208] Geometrically augmented position estimation of the mobile platform at the i-th UWB anchor point The pseudo-position measurement is injected into the extended Kalman filter and represented as the geometric pseudo-position measurement vector of the mobile platform at time k. Its expression is:

[0209] ,

[0210] In the formula, This is the geometrically augmented position estimation vector of the mobile platform at time k. Let k be the position vector of the mobile platform in the navigation coordinate system at time k. Let K be the geometric pseudo-measurement noise vector at time k, and its covariance satisfies: .

[0211] Furthermore, based on the geometric pseudo-position measurement vector of the mobile platform at time k, a linear observation matrix of the geometric pseudo-position of the mobile platform at time k is constructed. Its expression is:

[0212] .

[0213] S4. Construct the complete observation vector, complete observation matrix, and complete measurement noise covariance matrix at time k in sequence, and use the extended Kalman filter to correct and update the state vector of the mobile platform at time k; feed back the updated posterior state estimation vector of the mobile platform at time k to the inertial navigation master state for correction, and obtain the final positioning result.

[0214] The specific implementation steps of step S4 are described below.

[0215] S401. Construct the complete observation vector, complete observation matrix, and complete measurement noise covariance matrix at time k.

[0216] (1) The complete observation vector at time k The row-wise stack of all valid measurement vectors at time k is expressed as:

[0217] ,

[0218] In the formula, For all UWB anchor points constructed in step S301, the UWB measurement residual vector at time k. The zero-velocity pseudo-measurement vector of the mobile platform constructed in step S302 at time k. The NHC pseudo-measurement vector of the mobile platform constructed in step S302 at time k. This is the scalar pseudo-measurement vector of the lateral position of the mobile platform at time k in step S303. This is the geometric pseudo-position measurement vector of the mobile platform at time k in step S306.

[0219] (2) Complete observation matrix at time k The expression for the linear observation matrices corresponding to all valid measurement vectors at time k, stacked row-wise, is as follows:

[0220] ,

[0221] In the formula, Let k be the UWB linear observation matrix at time k. To update the linear observation matrix at zero speed for the mobile platform at time k, Let k be the nonholonomic constrained linear observation matrix of the mobile platform at time k. For a linear observation matrix with lateral position constraints, Let be the linear observation matrix of the geometric pseudo-position of the mobile platform at time k.

[0222] (3) Complete measurement noise covariance matrix at time k Let k be the measurement noise covariance matrix corresponding to all valid measurement vectors at time k. Its specific combination in a block-diagonal manner is expressed as follows:

[0223] ,

[0224] In the formula, The measurement noise covariance matrix is ​​the UWB measurement residual vector corresponding to all UWB anchor points at time k. Let be the measurement noise covariance matrix corresponding to the zero-velocity pseudo-measurement vector of the mobile platform at time k; Let be the measurement noise covariance matrix corresponding to the NHC pseudo-measurement vector of the mobile platform at time k; Let be the measurement noise covariance matrix corresponding to the scalar pseudo-measurement vector of the lateral position of the mobile platform at time k; Let be the measurement noise covariance matrix corresponding to the geometric pseudo-position measurement vector of the mobile platform at time k;

[0225] in,

[0226] ,

[0227] In the formula, The standard deviation of the ranging noise of the nth UWB anchor point at time t is determined by the parameters of the UWB ranging device. The noise standard deviation of the velocity constraint for zero-rate updates; and These are the noise standard deviations of the lateral velocity constraint and the vertical velocity constraint, respectively, in nonholonomic constraints. The noise standard deviation for lateral position constraints; Let be the equivalent Fisher information matrix at time k.

[0228] S402. Using an extended Kalman filter, the state vector of the mobile platform at time k is corrected and updated to obtain the posterior state estimate vector of the mobile platform at time k. Its expression is:

[0229] ,

[0230] In the formula, Let Kalman gain be at time k. Let be the prior state estimation vector of the mobile platform at time k, which is the state vector estimate of the mobile platform at time k obtained by substituting the posterior state estimation vector of the mobile platform at time k−1 into the tightly coupled state model. Let be the posterior state estimation matrix of the mobile platform at time k; Let be the prior state error covariance matrix at time k. Let be the posterior state error covariance matrix at time k; The complete observation matrix at time k Let k be the measurement noise covariance matrix corresponding to all valid measurement vectors at time k. This is the complete observation vector at time k. It is a 15×15 identity matrix.

[0231] S403. The posterior state estimation vector of the mobile platform at time k. Feedback is sent to the main state of inertial navigation to correct the position, velocity, attitude, and sensor bias, thereby obtaining the final stable positioning result.

[0232] Specifically, the implementation steps of step S403 are as follows:

[0233] (1) Obtain the navigation master state based on inertial propagation at time k, including: predicted position Prediction speed Predicted pose matrix Predicting gyroscope zero bias and predicting accelerometer zero bias ;

[0234] (2) The posterior state estimation vector of the mobile platform at time k obtained from step S402 And the navigation master state based on inertial propagation at time k obtained in step (1) above is substituted into the correction expression:

[0235] ,

[0236] In the formula, These are the posterior state estimation vectors of the mobile platform at time k. The components of position error, velocity error, attitude error, gyroscope bias error, and accelerometer bias error; This represents an antisymmetric matrix constructed from small-angle error vectors;

[0237] Then, the final stable positioning result at time k can be output, including the position. ,speed Attitude matrix gyroscope zero bias And zero bias of the accelerometer.

[0238] Furthermore, in order to verify the correctness of the present invention, actual experimental verification was carried out; the performance parameters of the IMU in the experiment are listed in Table 1, and the performance parameters of the UWB in the experiment are listed in Table 2.

[0239] Table 1:

[0240]

[0241] Table 2:

[0242] index range frequency band Nonlinear Sampling rate parameter 500 m 3.5-6.5 GHz 3 Mbps 10 Hz

[0243] Based on the IMU data with performance parameters shown in Table 1 and the UWB data with performance parameters shown in Table 2, five sets of localization experiments were repeated. The localization method of the present invention was used for each experiment, and the localization method using the traditional tightly coupled method was used as a comparative example.

[0244] like Figure 2 The figure shows a comparison of the planar positioning performance obtained by the method of this invention and the traditional IMU / UWB tightly coupled method under the same set of experimental data. As can be seen from the figure, the positioning trajectory obtained by the method of this invention is more consistent with the actual trajectory overall, especially showing better smoothness and fit in straight sections and turning areas. In contrast, the traditional IMU / UWB tightly coupled method exhibits significant lateral drift and trajectory fluctuations in the vertical road segment on the right side of the figure. A magnified view further shows that the method of this invention basically overlaps with the actual trajectory in this local area, while the traditional method deviates significantly. These results demonstrate that the present invention can effectively reduce the lateral offset caused by ranging fluctuations and accumulated inertial errors, thereby improving horizontal positioning accuracy and trajectory stability.

[0245] like Figure 3 The figure shows a schematic diagram of the horizontal error distribution obtained from a set of positioning experiment results; it can be clearly seen from the figure that the 75th percentile of the horizontal positioning error has decreased from 0.32 m (traditional IMU / UWB tight coupling) to 0.14 m (the method of this invention).

[0246] In the five sets of experiments, the results of the horizontal error test and the percentage improvement in positioning accuracy compared with the traditional tightly coupled method using the method of this application are shown in Table 3 below.

[0247] Table 3:

[0248] Experiment No. Horizontal error (m) of traditional tightly coupled method The horizontal error (m) of the method of this invention Increase in percentage (%) 1 0.32 0.14 56.25 2 0.38 0.19 50.00 3 0.32 0.16 50.00 4 0.35 0.18 48.57 5 0.31 0.15 51.61

[0249] from Figure 3 A comparison with the horizontal positioning accuracy in Table 3 shows that the method of this invention can achieve higher precision IMU / UWB tightly coupled positioning. Compared with the traditional IMU / UWB tightly coupled method, the horizontal positioning accuracy of this invention is improved by 51.29%. Therefore, the effectiveness and correctness of the method provided by this invention are verified.

[0250] The parts of this invention not disclosed in detail are well-known in the art. Although illustrative specific embodiments of the invention have been described above to help those skilled in the art understand the invention, it should be understood that the invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the invention as defined and determined by the appended claims, and all inventions utilizing the concept of this invention are protected.

Claims

1. A tightly coupled IMU / UWB method based on constraints and geometric enhancement, characterized in that, The steps are as follows: S1. Obtain the original ranging vector of the multi-channel UWB and perform robust smoothing to obtain the smoothed ranging vector of the multi-channel UWB. S2. Based on the IMU data of the mobile platform, construct a tightly coupled state model to obtain the state estimation vector of the mobile platform at time k. S3. Based on IMU inertial propagation state prediction, construct the UWB measurement residual vector and UWB linear observation matrix of all UWB anchor points at time k; and by introducing different kinematic constraints to the mobile platform, construct the zero-velocity pseudo-measurement vector and zero-velocity update linear observation matrix, or the NHC pseudo-measurement vector and nonholonomic constraint linear observation matrix of the mobile platform at time k; by introducing lateral position constraints, construct the lateral position scalar pseudo-measurement vector and lateral position constraint linear observation matrix of the mobile platform at time k. By introducing a geometric enhancement operation, the geometric pseudo-position measurement vector and the geometric pseudo-position linear observation matrix of the mobile platform at time k are constructed. S4. Construct the complete observation vector, complete observation matrix, and complete measurement noise covariance matrix at time k in sequence, and use the extended Kalman filter to correct and update the state vector of the mobile platform at time k; feed back the updated posterior state estimation vector of the mobile platform at time k to the inertial navigation master state for correction, and obtain the final positioning result.

2. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 1, characterized in that, In step S1, the robust smoothing process for the raw ranging values ​​collected at consecutive times of the i-th UWB anchor point is as follows: 1) Based on the original ranging vectors of the acquired multi-channel UWB, determine the original ranging values ​​acquired by the i-th UWB anchor point at continuous time intervals. 2) For any acquisition time k, set a sliding window centered on k with an odd length L, and construct a regression model expression for the original ranging value of the i-th UWB anchor point at any time j within the sliding window: ， In the formula, Let be the original ranging value of the i-th UWB anchor point at time j. This is the set of time indices within a sliding window centered at time k. The relative time offset of time j within the sliding window with respect to the window center time k is determined by the sampling time and is a known quantity. relative time offset The nth power, Let q be the coefficient of the q-th term in the regression model for the i-th UWB anchor point, where q is the polynomial order and its value ranges from 0 to 1. ; 3) Definition The expression is: ,in, The coefficient of the constant term, The coefficients of the first-order term, For the first Order term coefficients; 4) Stack the original distance measurements at each time point within the sliding window into a vector form, and use the weighted least squares method to estimate the coefficients of the q-th order term of the regression model, thereby obtaining the solution. Its constant term coefficient That is, the smoothed ranging value of the i-th UWB anchor point at time k. .

3. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 2, characterized in that, In step 4, the estimation expression for the coefficient of the q-th term in the regression model using the weighted least squares method is as follows: ， In the formula, Let be the polynomial coefficient vector of the i-th UWB anchor point within a sliding window centered at time k. The distance vector is formed by stacking all the original distance measurement values ​​of the i-th UWB anchor point in chronological order within a sliding window centered at time k. Let Vandermonde be the Vandermonde matrix of the i-th UWB anchor point within a sliding window centered at time k, consisting of the time offsets of all sampling times relative to the window center time k. The diagonal weighted matrix is ​​composed of IRLS weights. The diagonal elements of the diagonal weighted matrix are the iterative reweighted least squares weights of all the original ranging values ​​of the i-th UWB anchor point within the sliding window centered at time k, and the remaining elements are 0. The diagonal elements of the diagonal weighted matrix are obtained iteratively, and the specific steps are as follows: 1) Set the initial weight of each original ranging value in the sliding window to 1 to construct an initial diagonal weighted matrix. Based on the initial diagonal weighted matrix, calculate the initial estimate of the polynomial coefficient vector of the i-th UWB anchor point in the sliding window centered at time k. 2) Calculate the residuals of each original distance measurement value within the sliding window. And update the IRLS weights corresponding to each original ranging value based on the Huber truncation function; Among them, residual The expression is: ， In the formula, For the first The estimated coefficient of the q-th term in the nth iteration. The relative time offset of time 𝑗 within the sliding window relative to the time 𝑘 at the center of the window; The Huber truncation function is used to update the IRLS adaptive weights of each original ranging value. Its expression is as follows: ， In the formula, For the first The Huber truncation parameter corresponding to the next iteration. std(⋅) represents the standard deviation operator; Furthermore, construct the first The expression for the diagonal weighted matrix corresponding to the next iteration is: ， 3) Using the updated IRLS weights, reconstruct the diagonal weighted matrix and calculate the first polynomial coefficient vector of the i-th UWB anchor point within a sliding window centered at time k. The estimated value of the next iteration; 4) Repeat steps 2)-3) above until the i-th UWB anchor point is in the polynomial coefficient vector of the sliding window centered at time k. The optimal IRLS weights are obtained when the estimated values ​​converge in the next iteration.

4. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 1, characterized in that, In step S2, the expression for the tightly coupled state model is: ， In the formula, This is the state estimation vector of the mobile platform at time k. Let k be the state transition matrix of the mobile platform from time k−1 to time k. This is the state estimation vector of the mobile platform at time k−1; This represents the process noise vector of the mobile platform at time k. Wherein, the state estimation vector of the mobile platform at time k , , , These are the position error vector, velocity error vector, and attitude error vector in the navigation coordinate system, respectively. Let k be the gyroscope zero bias error at time k. Let k be the accelerometer zero bias error at time k; each element can be calculated using the dynamic equation of the state error vector of the moving platform under the assumption of small attitude error: ， In the formula, The angular velocity of Earth's rotation. The angular velocity of the navigation coordinate system relative to the Earth coordinate system. Let this be the force vector in the navigation coordinate system. Let the direction cosine matrix be the distance from the body coordinate system to the navigation coordinate system. The zero-bias first-order Markov correlation time constant of the gyroscope. The zero-bias first-order Markov correlation time constant of the accelerometer To achieve zero bias noise for the gyroscope, To achieve zero bias noise in the accelerometer; , , , , They are respectively , , , , The derivative with respect to time.

5. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 1, characterized in that, In step S3, the steps for constructing the UWB measurement residual vector and the UWB linear observation matrix for all UWB anchor points at time k are as follows: For the nth UWB anchor point, its distance measurement model in the navigation coordinate system is as follows: ， In the formula, Let k be the distance measurement observation value of the i-th UWB anchor point at time k. Let k be the smoothed ranging value of the i-th UWB anchor point at time k. Let be the position vector of the vehicle tag at time k in the navigation coordinate system. Let be the position vector of the i-th UWB anchor point in the navigation coordinate system. To calculate the Euclidean distance between position vectors, The ranging noise of the i-th UWB anchor point at time k; Then, the UWB measurement residual vector of all UWB anchor points at time k The constructed expression is: ， In the formula, Let UWB measurement residual vector be at time t. The predicted position vector is obtained based on the state vector of the mobile platform at time k in step S2; Furthermore, the linear observation matrix of the i-th UWB anchor point at time k The construct expression is: ， In the formula, It is a zero matrix with 1 row and 12 columns; Furthermore, the linear observation matrix of all UWB anchor points at time k The UWB linear observation matrix at time k is stacked row by row. Its expression is: 。 6. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 1, characterized in that, In step S3, the step of introducing different kinematic constraints to the mobile platform is when... 1) When the mobile platform is stationary, a zero-velocity update is introduced to obtain the zero-velocity pseudo-measurement vector of the mobile platform at time k. Its expression is: ， In the formula, Let K be the true velocity vector of the mobile platform at time k in the navigation coordinate system. The measurement noise of ZUPT for the mobile platform at time k; Furthermore, a zero-rate update linear observation matrix for the mobile platform at time k is constructed. Its expression is: ， In the formula, It is a 3×3 identity matrix. It is a 3×3 zero matrix; 2) When the mobile platform is in motion and its absolute longitudinal velocity in the carrier coordinate system is greater than a preset velocity threshold, a nonholonomic constraint is introduced to obtain the NHC pseudo-measurement vector of the mobile platform at time k. Its expression is: ， In the formula, and These represent the lateral and vertical velocity components of the mobile platform at time k in the navigation coordinate system. The NHC measurement noise vector of the mobile platform at time k; Furthermore, a nonholonomic constrained linear observation matrix of the mobile platform at time k is constructed. Its expression is: ， In the formula, Let be the direction cosine matrix from the navigation coordinate system to the vehicle coordinate system at time k. This is the selection matrix used to extract the lateral and vertical velocity components in the carrier coordinate system. It is a 2×3 zero matrix.

7. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 1, characterized in that, In step S3, when introducing lateral position constraints for the mobile platform, 1) Determine whether the mobile platform is in a straight section. The determination criteria are: the yaw rate amplitude of the mobile platform is lower than the preset threshold and its speed is higher than the preset minimum speed. 2) By introducing a method to suppress lateral drift in the straight section, a scalar pseudo-measurement vector of the lateral position of the moving platform at time k is obtained. Its expression is: ， In the formula, This is the transpose of the unit normal vector in the horizontal plane corresponding to the heading direction. Let k be the position vector of the mobile platform in the navigation coordinate system at time k. To detect the reference position vector of the mobile platform at the start of the straight-line segment, Measurement noise constrained for the position of the mobile platform in the straight segment at time k; 3) Furthermore, based on the scalar pseudo-measurement vector of the lateral position of the mobile platform at time k, a linear observation matrix constrained by lateral position is constructed. Its expression is: ， In the formula, This is the transpose of the unit normal vector in the horizontal plane corresponding to the heading direction. It is a 1×12 zero matrix.

8. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 1, characterized in that, In step S3, when introducing the geometric enhancement operation, 1) At each time k where effective UWB ranging exists, perform geometric augmentation on the ranging observation value of the corresponding UWB anchor point at time k to obtain the geometrically augmented position estimation vector of the mobile platform at time k. Its expression is: ， In the formula, This is the geometrically augmented position estimation vector of the mobile platform at time k. To optimize variables, Let k be the set of anchor point indices where valid distance measurement exists. Let k be the distance measurement observation value of the i-th UWB anchor point at time k. Let be the position vector of the i-th UWB anchor point in the navigation coordinate system. To extend the Kalman filter to predict the inertial position vector of the mobile platform at time k, The variance of UWB ranging noise. The variance of the uncertainty corresponding to the prior inertial position; 2) Calculate the equivalent Fisher information matrix at the current time k. The expression is: ， In the formula, Let k be the line-of-sight unit vector pointing from the i-th UWB anchor point to the geometrically augmented position estimate at time k. It is a 3×3 identity matrix; 3) Estimate the geometrically augmented position of the mobile platform at the i-th UWB anchor point. The pseudo-position measurement is injected into the extended Kalman filter and represented as the geometric pseudo-position measurement vector of the mobile platform at time k. Its expression is: ， In the formula, Let k be the position vector of the mobile platform in the navigation coordinate system at time k. Let K be the geometric pseudo-measurement noise vector at time k, and its covariance satisfies: ; Furthermore, a linear observation matrix of the geometric pseudo-position of the mobile platform at time k is constructed. Its expression is: 。 9. The constraint- and geometry-enhanced IMU / UWB tight coupling method according to claim 1, characterized in that, The specific implementation steps of step S4 are as follows: S401. Construct the complete observation vector at time k. Complete observation matrix and complete measurement noise covariance matrix ; S402. Using an extended Kalman filter, the state vector of the mobile platform at time k is corrected and updated to obtain the posterior state estimate vector of the mobile platform at time k. Its expression is: ， In the formula, Let Kalman gain be at time k. Let be the prior state estimation vector of the mobile platform at time k, which is the state vector estimate of the mobile platform at time k obtained by substituting the posterior state estimation vector of the mobile platform at time k−1 into the tightly coupled state model. Let be the posterior state estimation matrix of the mobile platform at time k; Let be the prior state error covariance matrix at time k. Let be the posterior state error covariance matrix at time k; The complete observation matrix at time k Let k be the measurement noise covariance matrix corresponding to all valid measurement vectors at time k. This is the complete observation vector at time k. It is a 15×15 identity matrix; S403. The posterior state estimation vector of the mobile platform at time k. Feedback is sent to the inertial navigation master state to correct position, velocity, attitude, and sensor bias, resulting in the final positioning result; among which, Based on inertial propagation, the navigation master state is used to obtain the predicted position at time k. Prediction speed Predicted pose matrix Predicting gyroscope zero bias and predicting accelerometer zero bias ; and combine it with the posterior state estimation vector of the mobile platform at time k obtained from step S402. Substitute into the correction expression: ， In the formula, These are the posterior state estimation vectors of the moving platform at time k. The components of position error, velocity error, attitude error, gyroscope bias error, and accelerometer bias error; This represents an antisymmetric matrix constructed from small-angle error vectors; Furthermore, the output is obtained from the position ,speed Attitude matrix gyroscope zero bias The final positioning result is determined by the time k formed by the zero bias of the accelerometer.

Images