UWB/IMU integrated inspection robot positioning in underground substation

The task of the underground inspection robot is to detect the environment of the underground substation and the operating parameters of the electrical equipment in it, and precise positioning is the guarantee for the inspection robot to complete the above tasks. In order to solve the problems of large drift of inertial sensors based on micro-electro-mechanical systems, and susceptibility to NLOS errors in UWB positioning, a fusion positioning algorithm with extended Kalman filtering as the core was designed. The algorithm suppresses the drift of position information by estimating the deviation of the inertial device, and integrates the UWB ranging information to improve the long-term positioning stability of the carrier. Finally, a parallel window smoother is designed to smooth the fusion result. The simulation results show that the fusion algorithm has a strong ability to resist drift and non-line-of-sight errors. Compared with the traditional fusion algorithm, the error is reduced by 38%.


Introduction
As one of the six safety avoidance systems, the mine personnel positioning system provides guarantee for operation monitoring, attendance supervision, emergency evacuation, and accident rescue. However, the underground tunnel space is narrow, and there are a large number of mobile equipment and metal structure interference, and the positioning method using only UWB and other equipment still has a large error.
The multipath effect and NLOS phenomenon faced by UWB positioning systems have been studied by predecessors. For example, Zhang Yuan compared and analyzed several major UWB positioning algorithms [1] ; Chen Chen and others established a wireless signal penetration model, and This further proposed a method to reduce the NLOS error [2] ; Ouyang Wen et al. obtained the position under NLOS conditions by solving the positioning model with the distance of adjacent tags [3] ; Xu Zhechao et al. Time and angle of arrival, identify the NLOS signal of the positioning system [4] . The Inertial Measurement Unit (IMU) has a high dynamic response and can reflect the motion state of objects in real time. Compared with a single sensor, the fusion of multiple sensors can provide redundant information. For positioning systems, high-precision positioning results can ensure the safety of personnel and equipment and improve production efficiency [5,6]. In this paper, based on UWB positioning, a positioning algorithm is designed and integrated with Micro-Electro-Mechanical System (MEMS) IMU. While supplementing the dynamic response, it detects the abnormal value caused by NLOS and weakens its response. The impact of positioning results. In order to further improve the accuracy, this paper designs a parallel window smoothing algorithm to use more information to obtain more accurate positioning results without affecting the real-time positioning results.

IMU/UWB fusion algorithm
The world coordinate system uses northeast sky coordinates, that is, the axis x and axis y point to east and north respectively, and the vertical axis z points to the sky. In this coordinate system, the UWB fixed base station is represented by Base, and its position is known and constant. Define the origin of the body coordinate system at the origin of the IMU three-axis accelerometer.

IMU status update
Define the system state vector as , , is introduced to represent the current attitude of the system. To solve the system pose, first determine the rotation increment q δ : Here t δ is the time period of IMU sampling. In the formula, ω is used to obtain the gyroscope measurement value to filter the deviation.
The system rotation from time t to time 1 t + is: We use the output of the gyroscope to obtain the current attitude of the system, and now use the output of the accelerometer to obtain the speed and displacement of the system. Between two adjacent moments, we use Euler's method to integrate, which means that the system acceleration remains unchanged, and the acceleration ˆb t a is expressed as: (4) where g is expressed as the acceleration of gravity. Then the speed and displacement of the system can be expressed as: p v a (6) For now we have obtained the prior values of the displacement, velocity and attitude of the system, but it is obvious that this system is a nonlinear system. In order to obtain the system covariance, we linearize the system and obtain:

UWB observation updata
Suppose some UWB fixed base station in the plane is denoted as Here 3 ∈ 0  represents a three-dimensional zero vector. Let the ranging residual of the UWB system be: Its variance is: Then there is Mahalanobis distance: However, when the UWB system is in the NLOS environment, the interference of its signal often produces an incremental deviation [7] , which causes the observation noise of the system to no longer obey the Gaussian distribution, which makes the the Mahalanobis distance of residual error increases. Therefore, the threshold T can be set as the judgment condition: The threshold is calculated from the standard deviation of i Γ of the historical LOS point. At this time, make the following adjustments to the measurement noise: Finally, the estimated mobile station positions at different times are smoothed to further reduce the impact of NLOS

Siulation
The simulation environment is set as a 30 15 3 m m m × × rectangular chamber with 1 fixed UWB base station arranged at the four corners of the chamber. The position coordinates of each base station are shown in Table 1. The inspection robot moves in the chamber, inspects the high explosive switches in the chamber one by one, and finally returns to the starting position. Fig.1 shows the results of the three positioning methods and the smoothing results. The simple fusion positioning system is affected by the NLOS signal generated by UWB, the position output has too many glitches, and the accuracy of real-time dynamic results is not high, which will cause misjudgment in application scenarios such as electronic fences. The fusion positioning system with NLOS detection can significantly improve positioning accuracy.    Fig.3 shows the error in the entire trajectory before and after smoothing. The results show that the variance of the fusion positioning error is significantly lower than the robust optimization algorithm, and the stability is better. The error amplitude is between before smoothing, and between [ 0.3, 0.25] − after smoothing.  Figure 3. Error before and after smoothing.

Conclusion
This paper designs a fusion positioning algorithm based on IMU and UWB signals, uses Mahalanobis distance as the basis to distinguish the NLOS signals of UWB positioning, and designs a parallel window smoother to smooth the fusion result. This paper uses the fusion algorithm to obtain the positioning results of IMU and UWB signals. In particular, when UWB is in the NLOS working state, its output value is extremely unstable, and direct use will have a great impact on the fusion accuracyThe simulation results show that the algorithm shown in this paper reduces the position error in the x and y directions by about 50%, and reduces the mean square error of the estimated trajectory by 38%, and has a smaller variance.