Lidar SLAM Based on Particle Filter and Graph Optimization for Substation Inspection

Simultaneous Localization and Mapping (SLAM) is the core technology of intelligent substation inspection robot. Because of lightweight computation, Rao-Blackwellized Particle Filter (RBPF) is widely used in two-dimensional SLAM. However, it suffers from poor positioning accuracy, low robustness and rapid cumulative errors despite recent improvement. This paper presents a lidar SLAM system based on RBPF and graph optimization that can adapt to unstructured operating environment of substation. Firstly, the diversity of particles is increased by rebuilding the resample algorithm to improve the robustness of the system, and high-quality poses are estimated in submaps. Secondly, the multi-submap system is established to construct odometry constraints (one pose corresponds to two submaps). Furthermore, loop detector is an important part of optimization algorithm, and the branch-bound method is used to reduce computation burden and accelerate the loop detection. Finally, global poses of robot are optimized by the whole odometry and loop constraints in real time. Experiment results show that the proposed method is more accurate than other methods, and can maintain and produce high-precision positioning and mapping in complex substation operation and maintenance environment. It provides a new idea for intelligent substation inspection and positioning method.


I. INTRODUCTION
Smart grid plays a vital role in decarbonizing electrical energy and accommodating high penetration of renewable generation whilst maintaining a high level of supply reliability. Substation is an integral part of the smart grid in maintaining quality and standards of supply and has seen rapid expansion over recent decades in both complexity, voltage level and size in response to fast development of renewable generations. This has led to increasing difficulty in substation inspection which is usually conducted by field staff. Recent years, with the development of artificial intelligence, robots have been The associate editor coordinating the review of this manuscript and approving it for publication was Heng Wang . developed and deployed to replace manual inspection [1]. Substation inspection robot is a comprehensive system integrating environmental perception, dynamic decision-making, motion control and equipment detection. It can complete the inspection task through map construction, autonomous positioning, path planning and equipment detection [2], which not only improves the inspection efficiency, but also effectively avoids a series of problems that need to be faced by manual inspection.
Map construction and autonomous positioning are integral part of robot navigation, and is the core of robot inspection. Therefore, SLAM (Simultaneous Localization and Mapping) is one of the key technologies of intelligent substation inspection robot [3].
RBPF method based on Monte Carlo sampling theory is an important method in SLAM filter algorithm [4]. Its core idea is to randomly extract state particles to express their distribution through posterior probability. The accuracy of RBPF method increases with the increase in number of particles, however, as each particle carries a complete map, too many particles can cause greater demand on memory and affect the real-time performance of the system. Researches have been focusing on how to use fewer particles whilst improving the accuracy of RBPF method. By improving the particle sampling method, the linear feature can be used for state estimation in the sampling process to reduce the sampling area, and the weight can be updated by similarity comparison to reduce the computational complexity [5]. In addition, [6] proposed an improved importance weight by combining the proposed distribution with the Kullback-Leibler divergence, which can effectively control the number of adaptive samples. The traditional RBPF algorithm lacks diversity of particles, which can cause particle dissipation easily, [7] developed a sampling weight accumulation algorithm that can better reflect the posterior probability density function in the real state to improve the accuracy of the algorithm estimation. The Minimum Sampling Variance (MSV) resampling method [8] is used to improve the distribution to better reflect the target distribution, which can effectively improve the positioning effect and mapping quality. However, these methods cannot effectively solve the continuous divergence problem of cumulative error.
At present, the graph optimization method is one of the academic hot topics [9], which uses the vertex as the optimization variable, and the observation equation as the constraint edge to construct the nonlinear least squares problem. Then, the error is iteratively minimized by gradient descending, Gauss-Newton, Levenberg-Marquart or other methods to calculate the global optimal poses. Graph-optimized SLAM has the advantage of slow cumulation of errors and strong robustness by utilizing all the past states, but the cost of calculation is greatly increased. With the sparsity of the optimization matrix and the continuous improvement in computer performance, it is possible to optimize SLAM in real time. [10] improved SLAM sparse algorithm based on nonfactor descending, using sparse network to approximate the original dense factor to accelerate the operation. Reference [11] proposed a graph neural network based on a novel implementation of a graph convolutional-like layer to convey messages that facilitate the optimality verification of a 2D pose-graph, which can learn to classify candidate solutions of 2D pose-graphs as optimal or sub-optimal. Although this method sacrifices a slight accuracy, it is rewarded with a significant speed increase. Reference [12] uses a semi-dense map and a sparse feature map to construct a dual-window structure, and optimizes the state through a multi-layer optimization scheme with reprojection constraint and relative pose constraint to improve the tracking accuracy.
In the complex outdoor inspection environment of a substation, the field of non-equipment area is relatively empty and the equipment area lacks structured features, which limits the environmental information obtained by single-line lidar and increases the difficulty of front-end calculation in optimized SLAM. On the contrary, although the particle filter algorithm can obtain high-quality front-end results in the short term, the error will continue to grow with time due to the lack of back-end optimization [13]. In view of this, this paper proposes a SLAM method based on particle filter and graph optimization. The particle filter is used as the front-end odometry to estimate the initial poses and generate local submaps. Then, combined with graph optimization, the system minimizes the error by using the information of the past time, and obtain high-precision pose and grid map under the premise of real-time, which provides reference for subsequent substation inspection work.

II. SYSTEM OVERVIEW
The SLAM problem can be defined as outputting the optimal estimation of the robot pose and the current map under the condition of only given observation input. RBPF method decomposes the problem into an independent robot localization problem and a map construction problem based on the known robot pose, as shown in Equation (1).
where u 1:t is the odometry data of the robot at time 1∼t, z 1:t is all the observation data of laser at time 1∼t, x 1:t and m represent the poses and map of the robot estimated by the system at time 1∼t, respectively. The robot localization problem is decomposed and simplified by conditional Bayesian formula and the conditional joint probability formula, and the pose estimation problem is transformed into the incremental estimation problem, as shown in Equation (2).
is the probabilistic model of robot pose. The system predict the current pose by motion model P ( x t | x t−1 , u t−1 ),and observation model P ( z t | x t ) which is treated as weights.
In this paper, the resampling strategy is optimized based on SIR (sampling importance resampling) filter, and the minimum sampling variance resampling method is used to replace the resampling method that simply judges the weight, which effectively improves the particle diversity.
The concept of submap is widely used in SLAM. It is a subset of the global map, and the addition of submap enables a lighter global pose estimation when the map is updated. The submap system is introduced, which maintain two active submaps at the same time. When a frame of laser scan is loaded into the system, it constructs odometry constraints with these two submaps, and constructs loop constraints by correlation scan matching to optimize pose in back-end. The system architecture. When the lidar data enters the Local SLAM, the pose predictor uses the odometry data to predict the current pose, and then matches it with the map to calculate the matching degree. The particle filter calculates the current pose according to the matching degree adjustment strategy and determines whether to resampling. Output pose and generate submap until resampling is not required. The system maintains two active submaps simultaneously. Local SLAM outputs time, pose, laser scan and submaps to Global SLAM and builds constraints (odometry constraints and loopback constraints). Finally, all poses and submaps are adjusted by sparse pose adjustment. The nonlinear least square method is used to minimize the error, optimizing historical robot poses and submap poses. New pose is added to the optimized pose set. The system architecture is shown in Fig. 1.

III. LOCAL SLAM
The specific process of local submap generation algorithm based on SIR filter is shown in Fig. 2.
When the new particle set is initialized, the pose of each particle is predicted by the motion model and the filter calculates the matching degree of scan and map near the predict pose. If the scan matches the map well and exceeds the matching threshold, the filter samples particles randomly to assumes that they follow a Gaussian distribution, and the new particles are generated by this Gaussian parameter. If there is no satisfied result of match, the new particles are generated by the odometry model. Then, the filter updates the weight of each particle and resamples them by variance if satisfying the resampling condition.

A. PARTICLE SAMPLER
When a new frame of laser data enters the sampler, the system initializes the particle swarm S t at the current moment, and traverses the particle swarm S t−1 at the previous moment to obtain the pose x t−1 at the previous moment. Then the motion model of the odometry is used for state propagation to predict the pose x Where ⊕ represents that the linear and angular velocities of the odometer u t−1 over a period of time are converted to pose increments, and then added to the initial pose x (i) t−1 . Since particles are not unique, several trajectories will be estimated at the same time, and the weight of each trajectory is crucial. x (i) 1:t represents the trajectory, and its importance weight can be defined as formula (3): The numerator is the target distribution and the denominator is the proposal distribution. For each observation, the calculation amount of trajectory weight will increase with time when the weight of complete trajectory is calculated. Under the condition of known particle trajectory x Because lidar matching is more accurate than odometry measurement, its variance is more advantageous than odometry in distribution. As shown in Fig. 3, when the laser frame matches the existing map well, the observation is more reliable and the interval of observation distribution L (i) is small. So, there are larger probability of particles generated by laser matching sampling fall on the interval L (i) , and the probability distribution of pose can be covered with less particles.
When the laser matching degree is low and the observation reliability is low, the motion model x Observation reliability is judged by maximum likelihood estimation of map matching degree. Firstly, the matching degree between the observation z t of the current frame and the previous submap m t . If the matching degree exceeds the set threshold, it is considered that the observation reliability is high, as shown in Equation (5): If the extreme pointx (i) t whose matching degree exceeds the threshold is searched around the predicted pose x t is taken as the center and is taken as the radius to randomly sample point {x k } with a fixed number of K.
Because the variance of the laser matching model is small, the sampling particle set is subject to Gaussian distribution. Combined with laser observation and odometry information, the vector mean µ t and the normalization factor η (i) are calculated by Equation (6) -(8): The new particle point set {x (i) t } uses multivariate normal distribution formula for probability calculation from x , and the proposal distribution π in its weight is improved to P x t−1 , z t , u t−1 , so the weight calculation method corresponding to the new particle is Equation (9).
According to formula (5), if there is nox (i) t whose map matching degree exceeds the threshold, the motion model x t−1 , u t−1 is used to sample the new particles, and the proposal distribution π uses the motion model. The importance weight calculation method is formula (10).
After sampling, the system has the map m (i) t−1 of the previous moment, the pose set {x t } of the present moment and the environmental observation z t . The system can also update the map carried by each particle through P(m and integrate the map corresponding to each trajectory with weight ratio to obtain the submap m.

B. SV RESAMPLING
Resampling is the process of deleting low weight particles and generating new ones to keep the total number of particles unchanged. Continuous resampling brings the problem of particle dissipation, which means that all particles copy from one particle and lose the diversity of particles. The formula (11) is used to determine the discrete degree of weight and decide whether or not to resample, wherew is the normalized weight of particles. When N eff is less than the threshold, the weights of each particle are quite different, and resampling is allowed.
We improve the resampling method, and use the sampling variance to measure the difference of particle distribution before and after resampling, so as to determine the degree of resampling damage to particle diversity [14]. Sampling variance calculates the sum of squares of the difference between the actual number of copies and the expected number of copies of particles. When the variance of sampling becomes smaller, the difference between the distribution of particles before and after resampling is less, and the diversity of particles is higher. The calculation formula of sampling variance (SV) is as follows (12): M is the total number of particles before resampling, N (i) t is the number of actual replication of particles, the weight of particles is ω (i) t at t time. In this paper, the minimum sampling variance resampling method is introduced so that the particle distribution before and after resampling is consistent.
We use the floor function to take an integer downward, giving the particle an expected number of replication N Equations (14) and (15) calculate the residual weight ω (i) t of particles and the total number of replications of all particles L.ω The expected replication number N    uses graph optimization method. The laser frame pose and submap pose are used as nodes, and the constraints between pose are used as edges. Based on this, we construct the error least squares problem and solve it, and obtain the optimal solution of each pose, as shown in Fig. 4. Among them, the pose constraint includes the front-end constraint constructed by local SLAM and the loopback constraint constructed by loopback detector.

A. SUBMAP MAINTENANC
In order to ensure that there are enough constraints in global optimization, the system uses double threads to maintain two active submaps at the same time, and a frame of laser data will be matched in two active submaps at the same time through particle filter. When the number of laser frames in an active submap reaches the set threshold ρ max , the submap stops maintenance, transforms from an active submap to a historical submap, and takes the pose of the last laser frame as the initial pose of the newly generated active submap. Only one active submap is maintained when the system is initialized. When the number of laser frames in the submap reaches ρ max /2, a new thread will be opened to generate and maintains the second active submap. The process is shown in Fig. 5.

B. LOOP DETECTOR
Loopback detection corresponds to constructing loopback constraints in Fig. 1. Loopback detection uses scan-to-map matching and searches for each laser frame [15]. When  the relative distance between the historical submap m i and the current frame node x j is lower than the set threshold, the scanning matching is performed. In order to avoid the waste of computing resources caused by searching all the maps, the search window W is limited to a square area with the pose ξ of the current frame node x j as the center and the edge length d.
The scan matching algorithm uses correlative scan matching (CSM) algorithm [16], assuming that the observed value ξ of the current frame node x j must exist in the search window W . In W , the step length λ = ( x, y, θ) is used for violent search, and all postures in W are traversed. Specifically, for each pose, the laser pose of the current frame is represented by the transfer matrix T ξ =(R ξ , t ξ ), and the laser observation data h k of the current frame is projected onto the submap m i , which can be represented by Equation (16).
The score function is used to accumulate the probability value of all the grids hit by the laser, which is the score of the pose. When the score exceeds the set threshold, it is considered to constitute a loop, and the position with the highest score is the observation position ξ of the current frame x j in the historical submap m i , as shown in formula (17). Although violent search matching does not fall into local extremum, its huge computation will affect the real-time performance of the system. Taking the search window W of 10m × 10m, step length λ = (0.01m, 0.01m, 1 • ), direction angle search range of 30 • as an example, we need to calculate 10 3 × 10 3 × 30 = 3 × 10 7 times. In order to reduce the amount of calculation, the branch-bound method is used to accelerate the calculation [17].
The branch-bound method is used to smooth and blur the map within the search range, so that the map resolution γ is smoothed and blurred to 2  (17) calculation, retain the largest score area, cut off the low score area. Use the highest score area as a new search window and double the resolution to continue the search match. Repeat this process until the search resolution reaches the highest resolution, as shown in Fig. 6.

C. POSE OPTIMIZATION
This paper uses SPA to optimize the global poses [18], which is the sparse pose adjustment in Fig. 1 V 2T is the transformation matrix of this point in the global map coordinate system and the error function can be set as (18).
If there is no error, the odometer pose and the loop pose should be equal. But this is impossible, so we calculate the relative pose of these two poses as error.
So, the matrix expression of error function is formula (19).
From the global view, the global poses of all laser frames are denoted as X = ξ j , j= 1, 2, . . . ,n, and the global poses of all sub-images are denoted as M = {ξ i } , i= 1, 2, . . . ,s. The pose ξ ij generated by loop detection is constrained. Then the nonlinear least squares expression of global optimization is (20).
, ij is the information matrix of Gaussian distribution, which indicates the importance of each item of error. The function ρ is Huber loss function, which is used to punish items with large error. Finally, the system iterates over the optimization problem (20) to get the global optimal pose estimation.

A. SIMULATION EXPERIMENT 1) ENVIRONMENT
In order to simulate the outdoor complex operation and maintenance environment of substation, and to quantitatively analyze the error, this experiment uses gazebo simulation VOLUME 10, 2022 software to simulate the scene of 1650 square meters of medium-sized substation, as shown in Fig. 7.
In gazebo, we controlled a differential drive robot with a laser and a wheel speedometer, simulated the inspection and recorded the data bags according to the predetermined route. The laser we used is a single-line laser, and 1440 laser data points are sampled in a frame. The angular resolution is 0.25 degrees, the working frequency is 5.5 Hz, and the effective distance of the laser point is 8 meters.
The simulation platform is a laptop with intel i7-11800H processor and 16GB DDR4 3200MHz memory. By replaying the data bag, we compared and analyzed the accuracy of positioning and mapping between our method, Gmapping [19] and Cartographer [17].

2) RESULTS AND ANALYSIS
In the recorded simulation data bag, the robot motion time is 32 minutes, the fastest speed is 1.6 m/s, and the fastest angular velocity is 1 rad/s. Through the inspection route, the whole substation is inspected for two rounds, and the robot is controlled to return to the initial position at the end of the inspection.
We replayed the data bag and used our method, Gmapping and Cartographer to do the experiments. The results of these three methods are shown in Fig. 8.
Due to the resampling improvement on the basis of RBPF method, the algorithm further improves the high accuracy of RBPF local map. The back-end optimization and loopback detection are added to improve the continuous divergence of the error, so that the global map is not distorted. The Gmapping based on traditional RBPF method has obvious deformation of the map, and the matching loss occurs when the simulation rotation speed is too fast, which leads to the map cannot be overlapped and corrected because there is no loopback mechanism. Cartographer and our algorithm showed excellent mapping effect from the global map.
As it's showing in Fig. 9, due to the incorrect position estimation of Gmapping in the second round of inspection, the map was distorted, which makes a great distance between   the two images of the same object, so the overlapping of images is not obvious enough. The overlapping of images in Cartographer are obvious in a small range, and the local map effect is less realistic than Gmapping. On the basis of inheriting the local high precision of RBPF, our method further improves the performance of high resolution and has satisfactory results.
Because the true value of the robot path can be recorded in the simulation environment, the relative pose between the odometry coordinate system and the world coordinate system at the end of each algorithm is calculated as the positioning error.
As shown in Table 1, because Gmapping cannot detect loopback, the map is extremely distorted and has nearly 27 meters of error, so it cannot meet the substation's inspection requirements. In this paper, our method and Cartographer can meet the requirements of global mapping, but our method is optimized on the local high-precision map, and the positioning accuracy is improved by 73.8% compared with Cartographer.
The maps established by each algorithm are compared with the standard map, and seven representative devices in the local range are selected as the control to calculate and analyze the global and local mapping errors of each method. Results are shown in Table 2 and Fig. 10. It should be noted that the global error by Gmapping could not be calculated due to the distortion of the global map built by Gmapping.
The construction of map details is very important in substation inspection. Because the RBPF is optimized in this paper, the pose of each submap and node is further optimized by graph optimization method, which constructs a complete map whilst slowing down the growth of cumulative error. The average error of the global mapping is reduced by 119.24 % compared with Cartographer, and the local error is reduced by 10.84 %.
Experiments show that the improved RBPF method in this paper is superior to Cartographer and Gmapping in the unstructured outdoor substation environment, which can meet the requirements of robot inspection in substations.

B. DATASET EXPERIMENT
In order to verify the performance of the system proposed in this paper under real world, and to facilitate comparison with existing method. We use the latest open-source dataset M2DGR from Shanghai Jiaotong University [20], all the data is collected from real sensors. The dataset uses Velodyne VLP-32C lidar and provides imu data. We convert the point cloud into 2D scan to support this experiment. It is worth mentioning that the dataset has the ground truth of the trajectory provided by the Leica MS60 laser scanner, which provides strong support for our experiments.
What's more, there are many SLAM performance evaluation softwares that are open source [21]. Our experiment uses evo evaluation software to evaluate the detailed performance of the proposed method and cartographer.
We used the hall sequence with five ROS bags in this data set. This sequence provides a lot of rotation, which is a great challenge for two-dimensional laser slam. We saved the trajectory calculated by our method and Cartographer, calculate and analyze the errors between them and ground truth respectively. The experimental results show the absolute pose errors (APE) of each method in hall_01 to hall_05 ROS bags change over time, as shown in the Fig. 11. Because these errors combine translation error and rotation error, there is no unit. Fig. 11. shows that our method has a significantly lower APE in hall_01 to hall_04 than Cartographer, exhibiting superior performance, this is particularly true in hall_01 to hall_03 sequences. In hall_05, our method performed comparably to Cartographer. The reason why Cartographer has a larger error in the hall_03 sequence is that it loses positioning in too much rotation and cannot be corrected by loopback. Our method performs better and more robust in over-rotation scenarios. With the help of the improved RBPF front-end odometer, our method does not lose positioning in such a challenge situation, and the cumulative error is smaller. Table 3 shows the maximum, mean and minimum errors as well as standard deviation for each method in each ROS bag. It can be seen from the table that the mean and minimum errors of our method in hall_05 are slightly higher than those of Cartographer but the standard deviation is smaller, which is consistent with Fig. 11. (e). In hall_01 to hall_04, our method has much smaller errors and standard deviation, confirming superior performance, even in the scenes with too much rotation.

VI. CONCLUSION
At present, there are many slam schemes based on particle filter or graph optimization. In this paper, a two-dimensional laser SLAM system based on particle filter and graph optimization is proposed and experimentally verified. The system improves the resampling strategy on the basis of RBPF method, which ensures the diversity of particles while constructing local high-precision map and outputting poses, with acceleration of loop detection through branch and bound. Finally, the graph structure is established on the initial pose constructed by RBPF for back-end optimization.
Compared with the classical graph optimization scheme, Cartographer, which uses laser matching pose directly to optimize, the local and global mean errors of the map constructed by the proposed method are significantly improved in the unstructured substation scene. The map is clear and has no overlap. In addition, in the latest test of M2DGR data sets, the proposed method has a better trajectory accuracy than Cartographer. In the scene with a high level of rotation, our method performed well and does not lose positioning, which would meet the requirements of unattended substation inspection, and can also provide new ideas for future robot inspection schemes.