UWB / MEMS IMU four-wheel robot indoor positioning method based on XGBoost

Abstract

The invention provides a UWB / MEMS IMU four-wheel robot indoor positioning method based on XGBoost, and the method comprises the following steps: 1, collecting UWB signal features and MEMS IMU data, taking the UWB signal features and MEMS IMU data as training samples, employing XGBoost to carry out the training of UWB and MEMS IMU features, and obtaining a UWB NLOS classification model and a straight movement state classification model; step 2, classifying UWB and MEMS IMU data by using the model obtained in the step 1; step 3, after MEMS IMU data of which the category is straight is obtained, restraining divergence of an MEMS IMU error by adopting an NHC algorithm; and a fourth step, for the UWB data without the NLOS and a result after MEMS IMU constraint, performing tight combination resolving by using improved Kalman filtering to obtain a dynamic positioning result. According to the method, the NLOS data of the UWB is identified and rejected through the XGBoost, the error divergence of the MEMS IMU is inhibited by adding the NHC combined with the motion state, the indoor positioning of the four-wheel robot is realized by adopting the improved UWB / MEMS IMU tight combination algorithm, and the precision and stability of the indoor dynamic positioning of the four-wheel robot are improved.

Description

Technical Field

[0001] The present invention belongs to the technical fields of UWB / MEMS IMU positioning systems and indoor positioning technologies, and particularly relates to an indoor positioning method for a four-wheel robot based on XGBoost using UWB / MEMS IMU. Background Art

[0002] With the continuous development of artificial intelligence, smart cities, and wireless networks, more and more robots have emerged in the public eye. Among them, robots based on four-wheel differential steering are widely used due to their simple structure and high reliability. In an outdoor open environment, robots can use satellite systems for positioning. However, in a satellite-denied environment, such as when satellite signals are severely blocked by buildings or when the number of satellites is insufficient when the robot is indoors, the required positioning information cannot be provided. Currently, there are various indoor solutions. Among them, Ultra-Wide Band (UWB) technology is widely adopted in indoor environments due to its advantages such as large signal bandwidth, low power spectral density, high time resolution, and strong obstacle penetration. However, when the UWB pulse signal is blocked, a Non Line Of Sight (NLOS) will be formed, which will greatly reduce the accuracy and stability of the UWB system. In severe cases, it will cause the UWB to be unable to position. Compared with UWB, a Micro Electro Mechanical System (MEMS) Inertial Measurement Unit (IMU) has the characteristics of strong autonomy, high information output rate, high short-term accuracy, and low cost. However, due to the constraints of the device material itself and the influence of its own positioning principle, the navigation and positioning error of the MEMS IMU accumulates rapidly over time, resulting in its inability to perform long-term independent navigation and positioning, and it is necessary to seek auxiliary information to suppress the divergence of system errors. Therefore, UWB and MEMS IMU have complementary advantages. Combining the two sensors can form a combined positioning system with better stability, richer positioning information, and higher positioning accuracy, thereby better realizing indoor positioning of four-wheel robots.

[0003] Currently, many scholars and teams are conducting research on UWB / MEMS IMU systems. In response to UWB NLOS, Li Wenfeng's team proposed a non-line-of-sight recognition method that uses UWB signal features, is simple to calculate, and has high recognition accuracy. It distinguishes NLOS by using a threshold, which is not easy to determine; Meng Q's team established a factor graph optimization (FGO) method based on elastic random model enhancement, introducing conventional neural net-works (CNNs) in random modeling to improve the integrated resistance and reliability, and using neural networks to deal with NLOS, which has high computational complexity and long training time. In response to the IMU error accumulation, Zou Qiang's team used zero-speed update, zero-angular rate update and heuristic drift reduction to preliminarily eliminate the accumulated error of the Inertial Navigation System (INS). Based on the improved square root unscented Kalman filter (ISRUKF), they deeply integrated UWB and INS in a tightly coupled manner, and used zero-speed and zero-angular rate information at the moment of rest to add motion constraints, sacrificing the maneuverability of the carrier. Niu Xiaoji's team used the fact that wheeled robots generally satisfy non-holonomic constraints (NHC), that is, when there is no slipping or vacancy, the lateral and vertical speeds of the carrier are zero. They proposed a method to project the IMU center position and speed to the vehicle NHC establishment point in the positioning of a single vehicle, thereby realizing the motion constraints on the single vehicle. This method is not suitable for the positioning of a four-wheel differential steering robot for the single vehicle positioning problem. Liu The Z team proposed an online slip parameter calibration and compensation method to solve the problem that when the vehicle turns or makes a U-turn, the side slip of the tire will have a negative impact on the positioning system measured by the NHC / wheel speed sensor. The location of the NHC establishment point is set in advance. In actual applications, the NHC establishment point may change. In response to the above problems, this patent uses extreme gradient boosting (XGBoost), which has better classification performance than other commonly used machine learning methods (SVM and RF) and is not easily affected by the quality of training data, to identify and classify the UWB ranging values ​​and eliminate NLOS data. At the same time, XGBoost is used to detect the straight state of the four-wheel differential steering robot. The NHC algorithm is used to suppress the divergence of MEMS IMU errors when the UWB data quality decreases in complex environments. Finally, the improved UWB / MEMS IMU tight combination algorithm is used to solve and obtain the positioning result.

[0004] Extreme Gradient Boosting (XGBoost) is a machine learning system based on boosting trees proposed by Chen et al. on the basis of a large amount of previous research work on gradient boosting algorithms. It adopts the ensemble idea, continuously generates new trees to fit the residuals of the previous tree model, and reduces the loss. Compared with other machine learning classification algorithms commonly used in indoor positioning, XGBoost has higher accuracy and better generalization ability.

[0005] Non-holonomic constraint (NHC) means that if the constraint equation contains the derivative of coordinates with respect to time (such as motion constraints) and the equation cannot be integrated into a finite form, such constraints are called non-holonomic constraints. For general wheeled carriers, such as cars or wheeled robots, under ideal conditions, the point where NHC holds is only when the forward speed is non-zero, and the lateral speed and elevation speed are constrained to zero. Summary of the Invention

[0006] Aiming at the deficiencies of the existing technology, the present invention proposes an indoor positioning method for a UWB / MEMS IMU four-wheel robot based on XGBoost. It identifies and eliminates NLOS data in UWB ranging through XGBoost, constructs a feature set for detecting the straight-line motion state using MEMS IMU data, identifies the straight-line motion state of the four-wheel differential steering robot through XGBoost, adopts an NHC algorithm combined with the motion state, and finally solves through an improved UWB / MEMS IMU tightly coupled algorithm that adjusts the measurement noise matrix in real time to obtain the positioning result.

[0007] An indoor positioning method for a UWB / MEMS IMU four-wheel robot based on XGBoost includes the following steps:

[0008] Step 1: Collect the direct path amplitude (DP), strongest path amplitude (SP), and signal-to-noise ratio (SNR) in multiple groups of UWB signal features respectively in LOS / NLOS environments, collect multiple groups of MEMS IMU data in straight-line and turning scenarios to extract time-domain and frequency-domain features, use the above signal features as training samples, and use XGBoost to train the UWB and MEMS IMU features respectively to obtain a UWB NLOS classification model and a straight-line state classification model;

[0009] Step 2: After collecting the data for dynamic positioning, use the UWB NLOS classification model obtained in Step 1 to classify the UWB signal features of each epoch, eliminate the ranging values corresponding to the signal features with the category of NLOS, retain the ranging values corresponding to the signal features with the category of LOS, and use the straight-line state classification model obtained in Step 1 to classify the MEMS IMU features of each epoch;

[0010] Step 3: After obtaining all the MEMS IMU data with the category of straight running, the NHC algorithm is used to constrain the divergence of the MEMS IMU error;

[0011] Step 4: For the UWB data with NLOS removed and the MEMS IMU results after combining with the running state and NHC constraint, an improved Kalman filter is used for tightly coupled solution to obtain the solution result of dynamic positioning.

[0012] For the direct path amplitude (DP), the strongest path amplitude (SP), and the signal-to-noise ratio (SNR) in the multi-group UWB signal features collected respectively in the LOS / NLOS environment in Step 1, and for the multi-group MEMS IMU data collected in the straight running and turning scenarios to extract time-domain and frequency-domain features, the above signal features are used as training samples, and XGBoost is used to train the UWB and MEMS IMU features respectively to obtain the UWB NLOS classification model and the straight running state classification model;

[0013] The specific steps are as follows:

[0014] Step 1-1: Represent the feature dataset of UWB as Φ, Φ = {(x i , y i ): x i ∈ R m , y i ∈ R} with n samples and m features. The label corresponding to the dataset x i is y i , 1 represents NLOS, 0 represents LOS, and the final NLOS prediction result integrated by t classification and regression trees (CART) is:

[0015]

[0016] Among them, each function f k is an independent regression tree, F is the set composed of CART, and fk(xi) is the predicted value of the data sample set i on the kth tree CART, is the final prediction result. The objective function of XGBoost is defined as:

[0017]

[0018]

[0019] Among them, represents the loss function, Ω(f k ) is the regularization term to prevent the model from overfitting. T and ω respectively represent the number and weight of leaf nodes, and γ and λ are penalty terms to suppress the increase in model complexity caused by the increase in leaf nodes. Let The predicted value in the t-th iteration, add the new function f t Optimize the objective function to improve the prediction accuracy. Then the objective function of the learning model in the t-th iteration is:

[0020]

[0021] Step 1-2: For the processing of the first cost function, use the Taylor expansion method to expand the cost function into the second derivative form:

[0022]

[0023] Among them, Is the first derivative of the loss function, Is the second derivative of the loss function. Define f in formula (5) t (x i ) = ω q(x) , ω ∈ R T , q: R d →{1, 2, …, T}, q represents a mapping relationship, that is, the leaf node corresponding to each data sample. At the same time, define I j ={i|q(x i ) = j}, I j Represents the sample set in the j-th leaf. Because the weights of the samples mapped to the same leaf node are the same, then there is:

[0024]

[0025] Substitute formulas (3)(6) into formula (5) to get:

[0026]

[0027] Among them, ω j Is a quadratic function. The optimal leaf node weight ω j Is:

[0028]

[0029] Substitute into formula (7) to get the final objective function as:

[0030]

[0031] Similarly, substituting the MEMS IMU feature set into XGBoost training can obtain the classification model for identifying the straight-ahead state.

[0032] After obtaining all the MEMS IMU data with the category of straight-ahead described in Step 3, use the NHC algorithm to constrain the divergence of the MEMS IMU error;

[0033] The specific steps are as follows:

[0034] Step 3-1: In the IMU coordinate system (b system), when the motion state is going straight, the velocity observation vector is expressed as:

[0035]

[0036] where, v b represents the velocity of the robot along the forward direction, and ε v represents the observation noise, and its variance parameter can be initially given according to the motion state of the carrier and adjusted through the residual. The velocity deduced from the MEMS IMU can be expressed as:

[0037]

[0038] Step 3-2: Performing perturbation analysis on Equation (11) gives:

[0039]

[0040] After expanding Equation (12) and neglecting the second-order small quantities, we get:

[0041]

[0042] Step 3-3: Combining Equations (10) and (13) gives the observation vector and the corresponding observation equation for the inertial navigation center velocity constraint:

[0043]

[0044] Step 3-4: Since NHC only constrains the lateral and vertical velocities of the MEMS IMU, only the lateral and vertical components of the above observation equation are taken.

[0045] For the UWB data after removing NLOS and the MEMS IMU results after combining with the NHC constraint in step 4, an improved Kalman filter is used for tight integration calculation to obtain the calculation result of dynamic positioning;

[0046] The specific formula is as follows:

[0047] Using the Kalman filter model for calculation, its state vector is 15-dimensional, and the dimension of the measurement vector depends on the number of available UWB ranging values. The state vector is as follows:

[0048] X = [(δr n ) T (δv n ) T φ T b g T ba T T (15)

[0049] where δr n is the position error of the MEMS IMU, δv n is the velocity error, φ is the misalignment angle error, b g is the gyro bias, and b a is the accelerometer bias.

[0050] Due to the low accuracy of the gyroscopes in the MEMS IMU, with a gyro bias repeatability of the order of 0.1° / s, the Earth's rotation information cannot be sensed. Simplifying its error differential equation, the simplified result is as follows:

[0051]

[0052] where is the attitude matrix between the IMU coordinate system and the navigation coordinate system (n - system), f b is the specific force, w a and w g are the accelerometer measurement white noise and the gyro measurement white noise respectively. The gyro and accelerometer biases are modeled as first - order Gaussian - Markov processes:

[0053]

[0054] where T gb , T ab are the correlation times of the gyro bias and the accelerometer bias respectively.

[0055] Using the position calculated by the MEMS IMU to estimate the position of the UWB rover and the distance d inversely calculated from the position of the j - th available UWB base station I,j , the measured distance from the rover to this base station is d U,j . Assuming the true position of the rover and the true distance d true,j between the rover and the base station, then there is:

[0056]

[0057] According to the position error model and the lever - arm information l b we can get:

[0058]

[0059] where is the calculated attitude matrix, is the true attitude matrix. Combining Equation (18) and Equation (19) we can get:​

[0060]

[0061] Since the true position of the UWB rover is unknown and the true distance d from the rover to the base station is also unknown true,j , the position calculated by the MEMS IMU at the previous moment and the back-calculated distance are used as the approximate coordinates of the UWB rover for approximate substitution, i.e.:

[0062]

[0063] Thus, the approximate expression of Equation (20) is obtained:

[0064]

[0065] The distance information from the UWB-measured rover to the base station is expressed as:

[0066] d U,j = d true,j + ε d,U (23)

[0067] where ε d,U is the ranging error of UWB.

[0068] Through Equations (22) and (23), in the case of assuming there are M available UWB base stations, the measurement equation of UWB / MEMS IMU tight integration is:

[0069] Z k = H k X k + V k (24)

[0070] where H k is the measurement matrix, V k is the measurement noise vector, assumed to be zero-mean Gaussian white noise, and its covariance matrix is R k , and there is:

[0071]

[0072]

[0073] Assume that during the solution process, the innovation vector at time k is υ k , then the normalized residual is:

[0074]

[0075] where is the i-th element of the covariance matrix P υk ​ is the absolute value of the i-th element of υ k . Using the IGG-3 equivalent weight function, the equivalent weight p i is calculated. The three-segment weight function of the measurement is as follows:

[0076]

[0077] where S d1 and S d2 are constants, usually selected as S d1 = 1 to 1.5 and S d2 = 2.5 to 8.

[0078] Define as the equivalent weight matrix, and there is:

[0079]

[0080] where and are respectively and the i-th diagonal element of R k . Then, the new filter gain matrix can be written as:

[0081]

[0082] According to the adjustment of the weight function, each dimension of the measurement will be given different weights according to the quality of the measurement, which will affect their weights in the filtering estimation. The better the measurement quality, the greater its weight.

[0083] Advantages of the present invention: (XGBoost detects UWB NLOS, XGBoost detects straight + NHC, improved tight integration)

[0084] 1. The present invention uses the UWB signal characteristics and the eigenvalue extracted from the MEMS IMU raw data as features, and adopts the XGBoost classification algorithm to establish the UWB NLOS recognition model and the straight-line state detection model respectively. The recognition accuracy is high, the flexibility is strong, and the dynamic plane positioning error is within 0.2 m.

[0085] 2. The present invention selects the direct path amplitude (DP), the strongest path amplitude (SP) and the signal-to-noise ratio (SNR) detected by the UWB receiver as the feature vectors of the XGBoost algorithm. These three parameters can be collected for most UWB devices and have better applicability.

[0086] 3. The present invention uses NHC combined with the motion state to directly constrain the lateral and longitudinal speeds of the MEMS IMU in the straight-line state, does not involve the lever arm with the NHC establishment point, and is not affected by the lever arm error.

[0087] 4. The present invention uses an improved UWB / MEMS IMU tightly coupled algorithm with real-time adjustment of the measurement noise matrix to calculate, further improving the accuracy and stability of positioning. Description of the Drawings

[0088] Figure 1 It is a flowchart of an indoor positioning method for a four-wheel robot based on UWB / MEMS IMU of the present invention;

[0089] Figure 2 It is a specific flowchart of step 3 of an embodiment of the present invention;

[0090] Figure 3 It is a summary flowchart of an embodiment of the present invention;

[0091] Figure 4 It is a comparison chart of the error between the indoor positioning calculation results of a four-wheel robot based on UWB / MEMS IMU with XGBoost and the indoor positioning calculation results of a four-wheel robot based on UWB / MEMS IMU that identifies NLOS with XGBoost in an embodiment of the present invention;

[0092] Figure 5 It is a comparison chart of the error between the indoor positioning calculation results of a four-wheel robot based on UWB / MEMS IMU with XGBoost and the indoor positioning calculation results of a four-wheel robot based on UWB / MEMS IMU combined with the motion state and NHC in an embodiment of the present invention. Detailed Embodiment

[0093] The following further describes an embodiment of the present invention with reference to the drawings.

[0094] In the embodiment of the present invention, the indoor positioning method for a four-wheel robot based on UWB / MEMS IMU is as Figure 1 shown, and includes the following steps:

[0095] Step 1. Respectively collect the direct path amplitude (DP), the strongest path amplitude (SP), and the signal-to-noise ratio (SNR) in multiple groups of UWB signal features in LOS / NLOS environments, and collect multiple groups of MEMS IMU data in straight-line and turning scenarios to extract time-domain and frequency-domain features. Use the above signal features as training samples, and use XGBoost to train the UWB and MEMS IMU features respectively to obtain a UWB NLOS classification model and a straight-line state classification model;

[0096] Step 1-1. Represent the feature dataset of UWB as Φ, Φ = {(x i , y i ): x i ∈ R m , yi There are n samples and m features in {∈R}. The dataset is x i The corresponding label is y i , 1 represents NLOS, 0 represents LOS, and the final NLOS prediction result of the integration of t classification and regression trees (CART) is:

[0097]

[0098] In the formula: Each function f k is an independent regression tree; F is the set composed of CART; fk(xi) is the predicted value of the data sample set i on the kth tree CART; is the final prediction result. The objective function of XGBoost is defined as:

[0099]

[0100]

[0101] In the formula: represents the loss function; Ω(f k ) is the regularization term to prevent the model from overfitting; T and ω represent the number and weight of leaf nodes respectively; γ and λ are penalty terms to suppress the model from becoming more complex as the number of leaf nodes increases. Let be the predicted value in the t-th iteration, and add the new function f t to optimize the objective function to improve the prediction accuracy. Then the objective function of the learning model in the t-th iteration is:

[0102]

[0103] Step 1-2: For the processing of the first cost function, use the Taylor expansion method to expand the cost function into the second derivative form:

[0104]

[0105] In the formula: is the first derivative of the loss function; is the second derivative of the loss function. Define f t (x i ) = ω q(x) , ω ∈ R T , q: R d →{1, 2, …, T}, q represents a mapping relationship, that is, the leaf node corresponding to each data sample. At the same time, define I j ={i|q(x i ) = j}, I jrepresents the sample set in the jth leaf. Since the weights of samples mapped to the same leaf node are the same, we have:

[0106]

[0107] Substituting equation (3) (6) into equation (5), we get:

[0108]

[0109] Where: j is a quadratic function. The optimal leaf node weight ω j for:

[0110]

[0111] Substituting into formula (7), the final objective function is:

[0112]

[0113] Similarly, by substituting the MEMS IMU feature set into XGBoost training, a classification model for identifying the straight driving state can be obtained.

[0114] Step 2: After the dynamic positioning data is collected, the UWB NLOS classification model obtained in step 1 is used to classify the UWB signal features of each epoch, the ranging values corresponding to the signal features classified as NLOS are removed, and the ranging values corresponding to the signal features classified as LOS are retained, and the straight-ahead state classification model obtained in step 1 is used to classify the MEMS IMU features of each epoch;

[0115] In the example of the present invention, the UWB training set data and test set data, MEMS IMU training set data and test set data are all collected in the lobby of an office building; UWB uses a LinkTrack PB communication and ranging module with a frequency of 10Hz, and the MEMS IMU uses an ELLIPSE-N inertial sensor with a frequency of 200Hz; the indoor positioning coordinate system uses an independent coordinate system constructed by a LeicaTS50 high-precision total station, and the high-precision fiber-optic inertial navigation with a gyroscope accuracy of 0.1deg / h is used to improve the accuracy of the combined positioning result with UWB through reverse smoothing as a reference trajectory; 4 UWB base stations are deployed in the experimental site, with a deployment area of 16m*16m, and the UWB base station is fixed on a tripod; UWB, MEMS IMU and high-precision fiber-optic inertial navigation are all connected to the mobile carrier, and the acquisition system is timed by a professional GNSS timing system in the outdoor scene to ensure the time synchronization of the data measured by each sensor. The mobile carrier is installed on the chassis of a four-wheel differential robot, and is remotely controlled by personnel to travel within the range of the base station deployment.

[0116] Step 3: After obtaining all MEMS IMU data with the category of straight movement, the NHC algorithm is used to constrain the divergence of MEMS IMU errors;

[0117] The specific steps are as follows:

[0118] Step 3-1: In the IMU coordinate system (b system), when the motion state is straight movement, the velocity observation vector is expressed as:

[0119]

[0120] where, v b represents the velocity of the robot along the forward direction, and ε v represents the observation noise, and its variance parameter can be initially given according to the motion state of the carrier and adjusted through residuals. The velocity deduced from the MEMS IMU can be expressed as:

[0121]

[0122] Step 3-2: Performing perturbation analysis on Equation (11) gives:

[0123]

[0124] After expanding Equation (12) and neglecting second-order small quantities, we get:

[0125]

[0126] Step 3-3: Combining Equations (10) and (13) gives the observation vector and the corresponding observation equation for the inertial navigation center velocity constraint:

[0127]

[0128] Step 3-4: Since NHC only constrains the lateral and vertical velocities of the MEMS IMU, only the lateral and vertical components are taken from the above observation equation.

[0129] Step 4: For the UWB data after removing NLOS and the MEMS IMU results after combining with the NHC constraint for the running state, an improved Kalman filter is used for tight integration calculation to obtain the calculation result of dynamic positioning.

[0130] The specific formula is as follows:

[0131] Using the Kalman filter model for calculation, its state vector is 15-dimensional, and the dimension of the measurement vector depends on the number of available UWB ranging values. The state vector is as follows:

[0132] X = [(δr n ) T (δvn ) T φ T b g T b a T T (15)

[0133] Among them, δr n is the position error of the MEMS IMU, δv n is the velocity error, φ is the misalignment angle error, b g is the gyroscope zero bias, b a is the accelerometer zero bias.

[0134] Due to the low accuracy of the gyroscope of the MEMS IMU, the zero bias repeatability is on the order of 0.1° / s, and the Earth's rotation information cannot be sensitively detected. The error differential equation is simplified, and the simplified result is as follows:

[0135]

[0136] Among them, is the attitude matrix of the IMU coordinate system and the navigation coordinate system (n system), f b is the specific force, w a and w g are the measurement white noise of the accelerometer and the measurement white noise of the gyroscope respectively. The zero biases of the gyroscope and the accelerometer are modeled as a first-order Gaussian - Markov process:

[0137]

[0138] Among them, T gb ,T ab are the correlation times of the gyroscope zero bias and the accelerometer zero bias respectively.

[0139] Using the position calculated by the MEMS IMU to infer the position of the UWB rover and the distance d inversely calculated from the position of the j-th available UWB base station I,j ,the measured distance from the rover to this base station is d U,j ,assuming the true position of the rover and the true distance d true,j between the base station, then there is:

[0140]

[0141] According to the position error model and the lever arm information l b it can be obtained:

[0142] ​

[0143] Among them, is the calculated attitude matrix, is the true attitude matrix. Combining equations (18) and (19), we can obtain:

[0144]

[0145] Since the true position of the UWB rover cannot be known nor can the true distance d from the rover to the base station be known true,j , therefore, the position solved by the MEMS IMU at the previous moment and the back-calculated distance are used as the approximate coordinates of the UWB rover for approximate substitution, that is:

[0146]

[0147] Thus, the approximate expression of equation (20) is obtained:

[0148]

[0149] The distance information of the UWB-measured rover to the base station is expressed as:

[0150] d U,j = d true,j + ε d,U (23)

[0151] Among them, ε d,U is the ranging error of UWB.

[0152] Through equations (22) and (23), in the case of assuming there are M available UWB base stations, the measurement equation of the UWB / MEMS IMU tight integration is:

[0153] Z k = H k X k + V k (24)

[0154] Among them, H k is the measurement matrix, V k is the measurement noise vector, assumed to be Gaussian white noise with zero mean, and its covariance matrix is R k , and there is:

[0155]

[0156]

[0157] Assume that during the solution process, the innovation vector at time k is υ k , then the normalized residual is:

[0158]

[0159] Among them, is the i-th element of the covariance matrix P υk and is the absolute value of the i-th element of υ k Using the IGG-3 equivalent weight function, calculate the equivalent weight p i , and the three-segment weight function of the measurement is:

[0160]

[0161] Among them, S d1 and S d2 are constants, usually selected as S d1 = 1 to 1.5 and S d2 = 2.5 to 8.

[0162] Define as the equivalent weight matrix, and there is:

[0163]

[0164] Among them, and are respectively and the i-th diagonal element of R k . Then, the new filter gain matrix can be written as:

[0165]

[0166] According to the adjustment of the weight function, each dimension of the measurement will be given different weights according to the quality of the measurement, which will affect their weights in the filtering estimation. The better the measurement quality, the greater its weight.

[0167] In the example of the present invention, the method described in the present invention (Scheme 3) is compared with the improved tight combination positioning method of UWB / MEMS IMU based on XGBoost for NLOS identification (Scheme 1) and the improved tight combination positioning method of UWB / MEMS IMU based on motion state combined with NHC (Scheme 2);

[0168] Such as Figure 4As shown, in order to verify the effect of the indoor positioning algorithm for the four-wheel robot based on UWB / MEMS IMU using XGBoost, the solution results of the algorithm of the present invention are compared with those of the improved tightly coupled positioning method of UWB / MEMS IMU based on XGBoost for NLOS identification. The results show that the root mean square error in the X direction of the solution results of the method of the present invention is reduced from 0.147 m to 0.091 m; the maximum error is reduced from 0.39 m to 0.331 m. The root mean square error in the Y direction is reduced from 0.222 m to 0.135 m; the maximum error is reduced from 0.817 m to 0.312 m, and the planar positioning accuracy is improved by 38.8%, which can effectively reduce the influence caused by the cumulative error of MEMS IMU.

[0169] As Figure 5 shown, compared with the solution results of the improved tightly coupled positioning method of UWB / MEMS IMU based on the combination of motion state and NHC, the results show that the root mean square error in the X direction of the solution results of the method of the present invention is reduced from 0.121 m to 0.091 m; the maximum error is reduced from 0.377 m to 0.331 m. The root mean square error in the Y direction is reduced from 0.189 m to 0.135 m; the maximum error is reduced from 0.367 m to 0.312 m, and the planar positioning accuracy is improved by 27.4%, which can effectively reduce the error caused by UWB NLOS.

[0170] As mentioned above, this is only the most basic specific implementation manner in the present invention, but the protection scope of the present invention is not limited thereto. Any substitution that can be understood by those skilled in the art within the technical scope disclosed by the present invention should be covered within the scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. An indoor positioning method for a four-wheel robot based on XGBoost and UWB / MEMS IMU, characterized in that, The following steps are involved: Step 1, respectively collect the direct path amplitude (DP), the strongest path amplitude (SP), and the signal-to-noise ratio (SNR) of multiple sets of UWB signal features in the LOS / NLOS environment, collect multiple sets of MEMS IMU data in the straight and turning scenarios, use the above signal features as training samples, use XGBoost to train the UWB and MEMS IMU features respectively, and obtain the UWB NLOS classification model and the straight state classification model; Step 2: After the dynamic positioning data is collected, the UWB NLOS classification model obtained in step 1 is used to classify the UWB signal features of each epoch, the ranging values ​​corresponding to the signal features classified as NLOS are removed, and the ranging values ​​corresponding to the signal features classified as LOS are retained, and the MEMSIMU features of each epoch are classified using the straight state classification model obtained in step 1; Step 3: After obtaining all the MEMS IMU data of the straight-line category, the NHC algorithm is used to constrain the divergence of the MEMS IMU error; Step 4: For the UWB data with NLOS removed and the MEMS IMU results with NHC constraints combined with the operating status, an improved Kalman filter is used for tight combination solution to obtain the dynamic positioning solution result.

2. The indoor positioning method of a UWB / MEMS IMU four-wheel robot based on XGBoost according to claim 1, wherein In step 1, the direct path amplitude (DP), the strongest path amplitude (SP), and the signal-to-noise ratio (SNR) of multiple sets of UWB signal features are collected in the LOS / NLOS environment, and multiple sets of MEMS IMU data are collected in the straight and turning scenarios. The above signal features are used as training samples, and XGBoost is used to train the UWB and MEMS IMU features respectively to obtain the UWBNLOS classification model and the straight state classification model; The specific steps are as follows: Step 1-1: Represent the feature dataset of UWB as Φ, Φ = {(x i , y i ): x i ∈ R m , y i ∈ R} There are n samples and m features. The label corresponding to the dataset x i is y i , where 1 represents NLOS and 0 represents LOS. The final NLOS prediction result of the integration of t classification and regression trees (CART) is: Among them, each function f k is an independent regression tree, F is a set composed of CART, and f k (x i ) is the predicted value of the data sample set i on the k-th tree CART. is the final prediction result, and the objective function of XGBoost is defined as: Among them, represents the loss function, and Ω(f k ) is the regularization term to prevent the model from overfitting. T and ω represent the number and weight of leaf nodes respectively, and γ and λ are penalty terms to inhibit the model from becoming more complex as the number of leaf nodes increases. Let be the predicted value in the t-th iteration. By adding the new function f t to optimize the objective function to improve the prediction accuracy, the objective function of the learning model in the t-th iteration is: Step 1-2: For the processing of the first cost function, use Taylor expansion to expand the cost function into a second-order derivative form: Among them, is the first derivative of the loss function, is the second derivative of the loss function. In the definition formula (5), f t (x i ) = ω q(x) , ω ∈ R T , q: R d → {1, 2, …, T}, q represents a mapping relationship, that is, the leaf node corresponding to each data sample. At the same time, define I j = {i | q(x i ) = j}, I j represents the sample set in the j-th leaf. Since the weights of the samples mapped to the same leaf node are the same, there is: Substituting equation (3) (6) into equation (5), we get: where ω j is a quadratic function, and the optimal leaf node weight ω j is as follows: Substituting into formula (7), the final objective function is: Similarly, by substituting the MEMSIMU feature set into XGBoost training, a classification model for identifying straight-ahead status can be obtained.

3. A UWB / MEMS IMU four-wheel robot indoor positioning method based on XGBoost according to claim 1, characterized in that, After all the MEMSIMU data of the straight line category are obtained in step 3, the NHC algorithm is used to constrain the divergence of the MEMSIMU error; The specific steps are as follows: Step 3-1: In the IMU coordinate system (b system), when the motion state is straight, the velocity observation vector is expressed as: Among them, v b represents the speed of the robot in the forward direction, and ε v represents the observation noise, whose variance parameter can be initially given according to the motion state of the carrier and adjusted through residuals. The speed deduced by the MEMS IMU can be expressed as: Step 3-2: Perform perturbation analysis on equation (11) to obtain: After expanding equation (12), ignoring the second-order small quantity, we can obtain: Step 3-3: Combining equations (10) and (13), we can obtain the observation vector and corresponding observation equation of the inertial navigation center velocity constraint: Step 3-4: Since NHC only constrains the lateral and vertical velocities of the MEMS IMU, the above observation equation only takes the lateral and vertical components.

4. A UWB / MEMS IMU four-wheel robot indoor positioning method based on XGBoost according to claim 1, characterized in that, For the UWB data with NLOS removed and the MEMS IMU results after combining the running state with the NHC constraint in step 4, an improved Kalman filter is used for tightly coupled solution to obtain the solution results of dynamic positioning; The specific formula is as follows: The Kalman filter model is used for solution. Its state vector is 15-dimensional, and the dimension of the measurement vector depends on the number of available UWB ranging values. The state vector is as follows: where δr n is the position error of the MEMS IMU, δv n is the velocity error, φ is the misalignment angle error, b g is the gyro bias, b a is the accelerometer bias; Since the gyroscope of the MEMS IMU has low accuracy and the zero-bias repeatability is on the order of 0.1° / s, the earth rotation information cannot be sensed, and its error differential equation is simplified. The simplified result is as follows: Among them, is the attitude matrix between the IMU coordinate system and the navigation coordinate system (n - system), f b is the specific force, w a and w g are the accelerometer measurement white noise and the gyro measurement white noise respectively. The biases of the gyro and the accelerometer are modeled as first - order Gaussian - Markov processes: Among them, T gb , T ab are the correlation times of the gyroscope zero bias and the accelerometer zero bias, respectively; Position calculated using MEMS IMU Deduce the position of the UWB rover And the position of the j-th available UWB base station The back-calculated distance is d I,j , and the measured distance from the rover to this base station is d U,j , assuming the true position of the rover And the true distance between the base station is d true,j , then there is: According to the position error model and lever arm information l b It can be obtained that: wherein, is the calculated attitude matrix, is the true attitude matrix. Combining equations (18) and (19) gives: Since the true position of the UWB rover cannot be known nor can the true distance d from the rover to the base station be known true,j , the position calculated by the MEMS IMU and the back-calculated distance at the previous moment are used as the approximate coordinates of the UWB rover for approximate substitution, that is: Thus, the approximate expression of equation (20) is obtained: The distance information from the rover to the base station measured by UWB is expressed as: d U,j = d true,j + ε d,U (23) Among them, ε d,U is the ranging error of UWB; Through equations (22) and (23), in the case of assuming there are M available UWB base stations, the measurement equation of UWB / MEMS IMU tight coupling is: Z k = H k X k + V k (24) Among them, H k is the measurement matrix, V k is the measurement noise vector, assumed to be zero-mean Gaussian white noise with covariance matrix R k , and there is: Assume that during the solution process, the innovation vector at time k is υ k , then the standardized residual is: Among them, is the i-th element of the covariance matrix P υk and is the absolute value of the i-th element of υ k Using the IGG-3 equivalent weight function, the equivalent weight p i is calculated. The three-segment weight function of the measurement is as follows: wherein, S d1 and S d2 are constants, and are usually selected such that S d1 = 1 to 1.5 and S d2 = 2.5 to 8; Define as the equivalent weight matrix, and we have: Among them, and are respectively and the i-th diagonal element of R k Then, the new filter gain matrix can be written as: According to the adjustment of the weight function, each dimension of the measurement will be given different weights according to the quality of the measurement, which will affect their weights in the filtering estimation. The better the measurement quality, the greater its weight.

Images