A UWB-IMU combined indoor positioning method based on sliding window factor graph

Abstract

A UWB-IMU combined indoor positioning method based on a sliding window factor graph specifically includes the following steps: S1. Solve the initial position of the system and provide the initial value of the system; S2. Parameterize the position and error state of the IMU as the optimization variables of the factor graph; S3. Perform IMU pre-integration calculation; S4. When UWB data is detected, perform UWB non-line-of-sight discrimination, add UWB ranging factors to UWB line-of-sight observation values, and add IMU pre-integration factors to the factor graph; S5. Determine whether the sliding window of the factor graph is full. If the window is full, marginalize and delete the earliest state in the window to reduce the amount of system calculation. If the window is not full, directly perform joint optimization to solve the optimization result. The present invention provides a UWB-IMU combined indoor positioning method based on a sliding window factor graph, converting the combined positioning problem of UWB and IMU into a graph optimization problem, using UWB ranging values ​​and IMU pre-integration as factors in the factor graph, considering the non-line-of-sight discrimination and sliding window marginalization of UWB, and obtaining a positioning result with higher accuracy.

Description

Technical Field

[0001] The invention belongs to the technical field of indoor positioning, and in particular relates to a UWB-IMU combined indoor positioning method based on a sliding window factor graph. Background Art

[0002] For many indoor positioning applications, reliable and high-precision positioning is still an open research problem. At present, the fusion estimation of the global navigation satellite system (GNSS) and the inertial navigation system (INS) can meet the requirements of high precision. However, in the satellite denial environment, the GNSS signal is difficult to reach, resulting in the failure of its positioning function. Although INS has the advantages of low cost and no environmental influence, it needs to integrate the high-frequency angular velocity and acceleration signals output by the gyroscope and accelerometer twice, which is easy to cause cumulative errors that grow over time and cannot continuously provide reliable position information. The existing positioning methods in satellite denial environments are mainly based on geomagnetism, ultrasound, vision and lidar. However, geomagnetic signal positioning is extremely dependent on accurate environmental prior geomagnetic information and has poor adaptability to different scenes; ultrasonic positioning is easily affected by the environment, has a short working distance and low accuracy; visual positioning has a large amount of calculation, is seriously affected by light, and is prone to failure in dynamic and changeable scenes; lidar positioning is expensive, has a large amount of calculation, is difficult to promote and apply, and is seriously affected by homogeneous characteristics.

[0003] Wireless sensor positioning methods represented by Bluetooth and ultra wide band (UWB) are low-cost and high-precision. They measure the distance between the base station and the receiving station by using wireless signals, and estimate the receiving station position by using the known base station location information. Among them, UWB has high time resolution, strong penetration, low power consumption, good anti-multipath effect, and strong security. Its observation value can be used to suppress and correct the cumulative error caused by INS. Therefore, UWB / INS fusion positioning has become a mainstream indoor positioning solution in GNSS denial environment. However, the non-line-of-sight (NLOS) phenomenon that easily occurs during UWB measurement is an important factor affecting the positioning results. Non-line-of-sight refers to the propagation path between the base station and the receiving station being blocked by dynamic and static obstacles, which leads to a longer propagation time and a larger measured distance relative to the actual distance. The non-line-of-sight effect will cause the quality of the UWB observation signal to change frequently, affecting the positioning accuracy. Therefore, efficient non-line-of-sight detection and discrimination and adaptive fusion methods are the key to UWB / INS positioning.

[0004] Existing data fusion methods mostly use Kalman filter and its variants. However, such methods rely on an inherent and accurate mathematical model established in advance, and their adaptive ability is weak when the observed signal is variable. If the non-line-of-sight signal of UWB is received, the inherent filter model will cause serious errors. Therefore, the existing UWB / INS fusion positioning method still has difficulty in solving the problem of inaccurate positioning caused by the variable quality of UWB observation signals in indoor environments, and it is difficult to meet the high-precision positioning requirements indoors. Summary of the invention

[0005] To solve the above technical problems, the present invention proposes a UWB-IMU combined indoor positioning method based on a sliding window factor graph. In essence, the UWB and IMU data are fused by using the UWB / IMU combined positioning method optimized based on the sliding window factor graph, and the non-line-of-sight discrimination of UWB is performed at the same time to reduce the influence of non-line-of-sight measurement values ​​on the positioning results.

[0006] To achieve the above object, the technical solution adopted by the present invention is:

[0007] A UWB-IMU combined indoor positioning method based on a sliding window factor graph, characterized in that it specifically comprises the following steps:

[0008] S1. Solve the initial position of the system and provide the initial value of the system;

[0009] S2. Parameterize the position and error state of the IMU as optimization variables of the factor graph;

[0010] S3. Perform IMU pre-integration calculation;

[0011] S4. When UWB data is detected, UWB non-line-of-sight discrimination is performed, and a UWB ranging factor is added to the UWB line-of-sight observation value, and an IMU pre-integration factor is added to the factor graph;

[0012] S5. Determine whether the sliding window of the factor graph is full. If the window is full, marginalize and delete the earliest state in the window to reduce the amount of system calculation. If the window is not full, perform joint optimization directly to solve the optimization result.

[0013] As a further step of the present invention, in step S5, the steps of fusing UWB and IMU data are as follows:

[0014] The IMU state in the sliding window factor graph is X = [x0, x1, ..., x n ], n is the sliding window size, where x k is the IMU status, Status x kIncludes the position, velocity, attitude of the IMU in the navigation coordinate system, as well as the drift and bias of the gyroscope and accelerometer;

[0015] When new UWB data is detected, UWB non-line-of-sight discrimination is performed. After eliminating the non-line-of-sight measurement value, a new IMU pre-integration factor and a new UWB observation factor corresponding to the UWB observation value are added to the factor graph. When the maximum value of the window is reached, the earliest state needs to be marginalized to control the size of the optimization window, and the marginalized state is used as a priori information factor and then added to the factor graph to make full use of the information in the system state.

[0016] According to the factor graph theory, the residual of the whole system is minimized and the least squares estimate is obtained as follows:

[0017]

[0018] The r in the formula pre and r UWB They represent the pre-integration factor of IMU and the UWB observation factor, respectively. m+1 is the number of UWB observation factors. p -H p X represents the prior information brought by marginalization. The Ceres framework is used to solve the least squares estimate in equation 1 to obtain the optimal estimate of the system state.

[0019] As a further step of the present invention, in step S4, the step of performing non-line-of-sight discrimination on the UWB ranging value is as follows:

[0020] By comparing the distance from the tag to the base station obtained according to the factor graph with the actual UWB measurement value, the non-line-of-sight signal is determined. The specific method is as follows: when receiving new UWB data, the position (x k ,y k , z k ), the position covariance matrix obtained by updating the factor graph obtains the standard deviation of the position: The confidence intervals of the position coordinates are and Therefore, at time k, the theoretical maximum distance from the tag to the i-th base station is:

[0021]

[0022] in, is the possible position coordinate at the current moment, (x i ,y i , z i ) is the coordinate of the i-th base station, and the distance between the tag and the i-th base station at the current moment is The distance measurement error between the tag and the base station is σi , then if The ranging value is determined to be a non-line-of-sight ranging value, and is downgraded or directly eliminated.

[0023] The present invention provides a UWB-IMU combined indoor positioning method based on a sliding window factor graph, which mainly includes:

[0024] (1) A UWB / IMU data fusion algorithm based on factor graph optimization algorithm is proposed. The factor graph algorithm is highly flexible and can be used in combination with different sensors in a plug-and-play manner. If a sensor fails, the factor corresponding to the failed sensor can be deleted in time.

[0025] (2) The non-line-of-sight signals are judged by using the position and position confidence output by the factor graph, and the UWB non-line-of-sight measurement values ​​are downgraded or eliminated. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 It is the system workflow diagram;

[0027] Figure 2 It is a schematic diagram of the principle of factor graph;

[0028] Figure 3 It is the structure diagram of the system factor graph. DETAILED DESCRIPTION

[0029] In order to solve these problems, this method proposes a more effective method to fuse inertial and UWB measurements. In essence, it uses the UWB / IMU combined positioning method based on sliding window factor graph optimization to fuse UWB and IMU data, and at the same time performs non-line-of-sight discrimination of UWB to reduce the impact of non-line-of-sight measurements on positioning results. It mainly includes:

[0030] (1) A UWB / IMU data fusion algorithm based on factor graph optimization algorithm is proposed. The factor graph algorithm is highly flexible and can be used in combination with different sensors in a plug-and-play manner. If a sensor fails, the factor corresponding to the failed sensor can be deleted in time.

[0031] (2) The non-line-of-sight signals are judged by using the position and position confidence output by the factor graph, and the UWB non-line-of-sight measurement values ​​are downgraded or eliminated.

[0032] Factor Graph Concepts

[0033] Factoring a function into the product of multiple functions, and then connecting the two expressions with straight lines to form an undirected graph is called a factor graph. The factor graph model is a probabilistic graph model derived from the Bayesian theory.

[0034] Let the factor graph model be: H = (F, X, E), where X represents the variable node, that is, the parameter in the factor graph that needs to be optimized and estimated, F is the factor node in the factor graph, and E is the edge connecting the factor node and the variable node, which is an undirected edge. If a factor node is connected to a variable node through an undirected edge E, then the variable node is the variable node of the factor. The joint probability distribution is represented by the joint product of all factors in the data set.

[0035] Assume that the probability distribution function is g(x1,…,x n ):

[0036]

[0037] In formula 1, x i represents a variable node, f j (X j ) represents a local function. When the variable nodes and local functions are connected via undirected edges, and all nodes in the factor graph are connected, a factor graph model can be constructed, such as Figure 2 shown.

[0038] Factor graph optimization algorithm flow

[0039] like Figure 1 As shown in the figure, it is the algorithm flow of the UWB / IMU combined positioning algorithm based on the factor graph. First, the initial pre-integration of the IMU is performed to convert the system state into specific position and error parameters, which are added to the factor graph as the variable node of the factor graph. Then, the IMU pre-integration is performed. When the UWB observation data is detected, the non-line-of-sight discrimination and elimination of the UWB are performed, and the UWB ranging factor is added to the line-of-sight UWB measurement value, and the IMU pre-integration factor is added at the same time. Then, it is determined whether the number of state parameters optimized simultaneously in the factor graph reaches the set window size. If the window is full, marginalization operation is performed to marginalize the earliest historical state, and finally, joint optimization of multiple states is performed. The optimization process is the graph optimization solution process. The difference between the observed value and the predicted value calculated for each factor, that is, the residual value, and the Jacobian matrix corresponding to the residual are calculated, and the factor graph optimization solution method is used to solve it.

[0040] System Equations

[0041] Figure 3 This is the factor graph structure diagram of the UWB / IMU tight combination positioning system based on the sliding window factor graph in this chapter. The IMU state in the sliding window is X = [x0, x1, ..., x n ], n is the sliding window size, where x k is the IMU status, Status x kIt includes the position, velocity, attitude of the IMU in the navigation coordinate system, as well as the drift and bias of the gyroscope and accelerometer. When new UWB data is detected, UWB non-line-of-sight discrimination is performed. After eliminating the non-line-of-sight measurement value, a new IMU pre-integration factor and a new UWB observation factor corresponding to the UWB observation value are added to the factor graph. When the maximum value of the window is reached, the earliest state needs to be marginalized to control the size of the optimization window, and the marginalized state is used as a priori information factor and then added to the factor graph to make full use of the information in the system state, improve data utilization, and improve positioning accuracy.

[0042] According to the factor graph theory, the residual of the whole system is minimized and the least squares estimate is obtained as follows:

[0043]

[0044] The r in the formula pre and r UWB They represent the pre-integration factor of IMU and the UWB observation factor, respectively. m+1 is the number of UWB observation factors. p -H p X represents the prior information brought by marginalization, which will be introduced in the following subsections. Solving the least squares estimate in equation 2, we can obtain the optimal estimate of the system state. The process of solving the least squares estimate can be solved by the Ceres framework.

[0045] IMU pre-integration factor

[0046] In the optimization process, the introduction of IMU data for multi-sensor data fusion can improve the smoothness and accuracy of positioning. By integrating the IMU data and taking the position, velocity and quaternion of the previous moment as the initial value, the position, velocity and quaternion of the current moment can be obtained, and the position, velocity and quaternion can be updated. However, in factor graph optimization, the overall window optimization is often performed. During the optimization process, the initial value of the previous moment will change after a round of optimization, so the corresponding IMU integral must also be recalculated. In this case, the amount of calculation is large, the calculation process is complicated, and computing resources are wasted. Therefore, a pre-integration method is proposed to increase the calculation speed and avoid wasting computing power. The core idea of ​​pre-integration is to "pick out" the initial value in the integral, so that the integral result is independent of the initial value. No matter how the initial value changes during the optimization process, it has no effect on the calculation of the IMU data integral, thereby improving the calculation speed.

[0047] The residual r of the pre-integration factor pre for:

[0048]

[0049] In the formula and are the pre-integrated position, velocity and quaternion observations, respectively. and are the predicted values ​​of position, velocity and quaternion, and are the gyroscope and accelerometer biases respectively.

[0050] Non-line-of-sight signal discrimination

[0051] When the UWB distance measurement value is affected by non-line-of-sight, resulting in the distance measurement value being greater than the actual distance, if the UWB non-line-of-sight measurement value is still simply added to the factor graph to construct the UWB factor, it will have a great impact on the positioning result. Therefore, it is very important to accurately identify and eliminate the non-line-of-sight information in UWB to keep the positioning result accurate and reliable.

[0052] In this method, the non-line-of-sight signal is determined by comparing the distance from the tag to the base station obtained according to the factor graph with the actual UWB measurement value. The specific method is as follows: when receiving new UWB data, the position (x k ,y k , z k ), the position covariance matrix obtained by updating the factor graph can obtain the standard deviation of the position as The confidence intervals of the position coordinates are and Therefore, at time k, the theoretical maximum distance from the tag to the i-th base station is:

[0053]

[0054] in, is the possible position coordinate at the current moment, (x i ,y i , z i ) is the coordinate of the i-th base station. The distance between the tag and the i-th base station at the current moment is The distance measurement error between the tag and the base station is σ i , then if The ranging value can be determined to be a non-line-of-sight ranging value, and can be downgraded or directly eliminated.

[0055] UWB Observation Factor

[0056] There are two main ways to combine UWB and IMU, namely loose combination and tight combination. Loose combination is to obtain the positioning result by UWB, and then add the positioning position of UWB as the observation value to the factor graph. This method is simple to calculate and easy to implement, but the connection between the UWB and IMU systems is not close enough. When UWB positioning anomalies occur, it will have a greater impact on the combined positioning system. The tight combination method uses the UWB ranging value as the UWB observation value. When UWB non-line-of-sight measurement occurs, the UWB non-line-of-sight measurement can be identified and eliminated in time without affecting the use of other normal data. Therefore, using the tight combination mode can obtain higher positioning accuracy, and the UWB / IMU combination method in this method is also a tight combination. In the tight combination mode, the residual of the UWB observation factor can be defined as the difference between the UWB ranging value and the distance between the tag and the base station calculated by the system:

[0057] r UWB =d-||p uwb -p base ||

[0058] Where d represents the UWB ranging value, ||p uwb -p base || represents the distance from the UWB tag to the base station calculated based on the system status.

[0059] Marginalization

[0060] As the system running time increases, the number of state variables to be optimized added to the window will also increase. If the number of states to be optimized is not limited, the factor graph optimization calculation complexity will become higher and higher, and the operation efficiency will become lower and lower. Therefore, in order to reduce the complexity of system calculation, the optimization process of the factor graph adopts a method based on the sliding window idea, limits the size of the system's optimization state window, sets a fixed size, and balances the amount of calculation to a certain extent. In the process of window sliding, new UWB factors and new IMU pre-integration factors are constantly added to the sliding window, while the old system state, old UWB ranging factors and old IMU pre-integration factors need to be removed. However, there is a constraint relationship between the system state and the observation factor at the historical moment and other states in the window. If the data outside the window is directly discarded, it will lead to the loss of associated information and inaccurate state estimation, affecting the accuracy of the system. Therefore, the method of deleting the old state in the sliding window is not to directly remove the old state, but to marginalize the state to be deleted and information to play its final role. The idea of ​​marginalization is to convert the associated constraints into prior information while removing the old state, and add it to the optimization problem again. The marginalization method is Schur complement.

[0061] The overall workflow of the present invention is as follows Figure 1As shown, a UWB-IMU combined indoor positioning method based on a sliding window factor graph includes the following steps:

[0062] S1. Solve the initial position of the system and provide the initial value of the system;

[0063] S2. Parameterize the position and error state of the IMU as optimization variables of the factor graph;

[0064] S3. Perform IMU pre-integration calculation;

[0065] S4. When UWB data is detected, UWB non-line-of-sight discrimination is performed, and a UWB ranging factor is added to the UWB line-of-sight observation value, and an IMU pre-integration factor is added to the factor graph;

[0066] S5. Determine whether the sliding window of the factor graph is full. If the window is full, marginalize and delete the earliest state in the window to reduce the amount of system calculation. If the window is not full, perform joint optimization directly to solve the optimization result.

[0067] Factor graph optimization algorithm flow

[0068] like Figure 1 As shown in the figure, it is the algorithm flow of the UWB / IMU combined positioning algorithm based on the factor graph. First, the initial pre-integration of the IMU is performed to convert the system state into specific position and error parameters, which are added to the factor graph as the variable node of the factor graph. Then, the IMU pre-integration is performed. When the UWB observation data is detected, the non-line-of-sight discrimination and elimination of the UWB are performed, and the UWB ranging factor is added to the line-of-sight UWB measurement value, and the IMU pre-integration factor is added at the same time. Then, it is determined whether the number of state parameters optimized simultaneously in the factor graph reaches the set window size. If the window is full, marginalization operation is performed to marginalize the earliest historical state, and finally, joint optimization of multiple states is performed. The optimization process is the graph optimization solution process. The difference between the observed value and the predicted value calculated for each factor, that is, the residual value, and the Jacobian matrix corresponding to the residual are calculated, and the factor graph optimization solution method is used to solve it.

[0069] System Equations

[0070] Figure 3 This is the factor graph structure diagram of the UWB / IMU tight combination positioning system based on the sliding window factor graph in this chapter. The IMU state in the sliding window is X = [x0, x1, ..., x n ], n is the sliding window size, where x k is the IMU status, Status x kIt includes the position, velocity, attitude of the IMU in the navigation coordinate system, as well as the drift and bias of the gyroscope and accelerometer. When new UWB data is detected, UWB non-line-of-sight discrimination is performed. After eliminating the non-line-of-sight measurement value, a new IMU pre-integration factor and a new UWB observation factor corresponding to the UWB observation value are added to the factor graph. When the maximum value of the window is reached, the earliest state needs to be marginalized to control the size of the optimization window, and the marginalized state is used as a priori information factor and then added to the factor graph to make full use of the information in the system state, improve data utilization, and improve positioning accuracy.

[0071] According to the factor graph theory, the residual of the whole system is minimized and the least squares estimate is obtained as follows:

[0072]

[0073] The r in the formula pre and r UWB They represent the pre-integration factor of IMU and the UWB observation factor, respectively. m+1 is the number of UWB observation factors. p -H p X represents the prior information brought by marginalization, which will be introduced in the following subsections. Solve the least squares estimate in equation 6 to obtain the optimal estimate of the system state.

[0074] IMU pre-integration factor

[0075] In the optimization process, the introduction of IMU data for multi-sensor data fusion can improve the smoothness and accuracy of positioning. By integrating the IMU data and taking the position, velocity and quaternion of the previous moment as the initial value, the position, velocity and quaternion of the current moment can be obtained, and the position, velocity and quaternion can be updated. However, in factor graph optimization, the overall window optimization is often performed. During the optimization process, the initial value of the previous moment will change after a round of optimization, so the corresponding IMU integral must also be recalculated. In this case, the amount of calculation is large, the calculation process is complicated, and computing resources are wasted. Therefore, a pre-integration method is proposed to increase the calculation speed and avoid wasting computing power. The core idea of ​​pre-integration is to "pick out" the initial value in the integral, so that the integral result is independent of the initial value. No matter how the initial value changes during the optimization process, it has no effect on the calculation of the IMU data integral, thereby improving the calculation speed.

[0076] The residual r of the pre-integration factor pre for:

[0077]

[0078] In the formula and are the pre-integrated position, velocity and quaternion observations, respectively. and are the predicted values ​​of position, velocity and quaternion, and are the gyroscope and accelerometer biases respectively.

[0079] Non-line-of-sight signal discrimination

[0080] When the UWB distance measurement value is affected by non-line-of-sight, resulting in the distance measurement value being greater than the actual distance, if the UWB non-line-of-sight measurement value is still simply added to the factor graph to construct the UWB factor, it will have a great impact on the positioning result. Therefore, it is very important to accurately identify and eliminate the non-line-of-sight information in UWB to keep the positioning result accurate and reliable.

[0081] In this method, the non-line-of-sight signal is determined by comparing the distance from the tag to the base station obtained according to the factor graph with the actual UWB measurement value. The specific method is as follows: when receiving new UWB data, the position (x k ,y k , z k ), the position covariance matrix obtained by updating the factor graph can obtain the standard deviation of the position as The confidence intervals of the position coordinates are and Therefore, at time k, the theoretical maximum distance from the tag to the i-th base station is:

[0082]

[0083] in, is the possible position coordinate at the current moment, (x i ,y i , z i ) is the coordinate of the i-th base station. The distance between the tag and the i-th base station at the current moment is The distance measurement error between the tag and the base station is σ i , then if The ranging value can be determined to be a non-line-of-sight ranging value, and can be downgraded or directly eliminated.

[0084] UWB Observation Factor

[0085] There are two main ways to combine UWB and IMU, namely loose combination and tight combination. Loose combination is to obtain the positioning result by UWB, and then add the positioning position of UWB as the observation value to the factor graph. This method is simple to calculate and easy to implement, but the connection between the UWB and IMU systems is not close enough. When UWB positioning anomalies occur, it will have a greater impact on the combined positioning system. The tight combination method uses the UWB ranging value as the UWB observation value. When UWB non-line-of-sight measurement occurs, the UWB non-line-of-sight measurement can be identified and eliminated in time without affecting the use of other normal data. Therefore, using the tight combination mode can obtain higher positioning accuracy. The UWB / IMU combination method in this chapter is also a tight combination. In the tight combination mode, the residual of the UWB observation factor can be defined as the difference between the UWB ranging value and the distance between the tag and the base station calculated by the system:

[0086] r UWB =d-||p uwb -p base ||

[0087] Where d represents the UWB ranging value, ||p uwb -p base || represents the distance from the UWB tag to the base station calculated based on the system status.

[0088] Marginalization

[0089] As the system running time increases, the number of state variables to be optimized added to the window will also increase. If the number of states to be optimized is not limited, the factor graph optimization calculation complexity will become higher and higher, and the operation efficiency will become lower and lower. Therefore, in order to reduce the complexity of system calculation, the optimization process of the factor graph adopts a method based on the sliding window idea, limits the size of the system's optimization state window, sets a fixed size, and balances the amount of calculation to a certain extent. In the process of window sliding, new UWB factors and new IMU pre-integration factors are constantly added to the sliding window, while the old system state, old UWB ranging factors and old IMU pre-integration factors need to be removed. However, there is a constraint relationship between the system state and the observation factor at the historical moment and other states in the window. If the data outside the window is directly discarded, it will lead to the loss of associated information and inaccurate state estimation, affecting the accuracy of the system. Therefore, the method of deleting the old state in the sliding window is not to directly remove the old state, but to marginalize the state to be deleted and information to play its final role. The idea of ​​marginalization is to convert the associated constraints into prior information while removing the old state, and add it to the optimization problem again. The marginalization method is Schur complement.

[0090] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A UWB-IMU combined indoor positioning method based on sliding window factor graph, characterized in that: The specific steps include: S1. Solve the initial position of the system and provide the initial value of the system; S2. Parameterize the position and error state of the IMU as optimization variables of the factor graph; S3. Perform IMU pre-integration calculation; S4. When UWB data is detected, UWB non-line-of-sight discrimination is performed, and a UWB ranging factor is added to the UWB line-of-sight observation value, and an IMU pre-integration factor is added to the factor graph; In step S4, the steps of performing non-line-of-sight determination on the UWB ranging value are as follows: By comparing the distance from the tag to the base station obtained according to the factor graph with the actual UWB measurement value, the non-line-of-sight signal is determined. The specific method is as follows: when receiving new UWB data, the position (x k ,y k , z k ), the position covariance matrix obtained by updating the factor graph obtains the standard deviation of the position: The confidence intervals of the position coordinates are and Therefore, at time k, the theoretical maximum distance from the tag to the i-th base station is: in, is the possible position coordinate at the current moment, (x i ,y i , z i ) is the coordinate of the i-th base station, and the distance between the tag and the i-th base station at the current moment is The distance measurement error between the tag and the base station is σ i , then if Determine that the ranging value is a non-line-of-sight ranging value, and downgrade it or directly eliminate it; S5. Determine whether the sliding window of the factor graph is full. If the window is full, marginalize and delete the earliest state in the window to reduce the amount of system calculation. If the window is not full, directly perform joint optimization to solve the optimization result. In step S5, the steps of fusing UWB and IMU data are as follows: The IMU state in the sliding window factor graph is X = [x0, x1, ..., x n ], n is the sliding window size, where x k is the IMU status, Status x k Includes the position, velocity, attitude of the IMU in the navigation coordinate system, as well as the drift and bias of the gyroscope and accelerometer; When new UWB data is detected, UWB non-line-of-sight discrimination is performed. After eliminating the non-line-of-sight measurement value, a new IMU pre-integration factor and a new UWB observation factor corresponding to the UWB observation value are added to the factor graph. When the maximum value of the window is reached, the earliest state needs to be marginalized to control the size of the optimization window, and the marginalized state is used as a priori information factor and then added to the factor graph to make full use of the information in the system state. According to the factor graph theory, the residual of the whole system is minimized and the least squares estimate is obtained as follows: The r in the formula pre and r UWB They represent the pre-integration factor of IMU and the UWB observation factor, respectively. m+1 is the number of UWB observation factors. p -H p X represents the prior information brought by marginalization. The Ceres framework is used to solve the least squares estimate in equation 1 to obtain the optimal estimate of the system state.

Images