Robot localization and mapping system and method based on particle filtering and graph optimization

Abstract

This invention discloses a robot localization and mapping system and method based on particle filtering and graph optimization. The resampling strategy is optimized based on a SIR filter, using a minimum sampling variance resampling method instead of a simple weight-based resampling method, effectively improving particle diversity. A subgraph system is introduced, maintaining two active subgraphs simultaneously. Constraints are constructed between a frame of laser data and the two subgraphs, and closure constraints are built through correlation scanning matching for backend optimization. The nonlinear least squares method is used to minimize the error, optimizing all historical poses and subgraph poses, and new poses are added to the optimized pose set. This invention uses a particle filter as a front-end odometry to estimate good initial pose values, generate local subgraphs, and combines graph optimization to minimize errors using information from all time points. Under the premise of meeting real-time requirements, high-precision poses and grid maps are obtained, providing a reference for subsequent substation inspection work.

Description

Technical Field

[0001] This invention belongs to the field of autonomous mobile robot positioning technology, specifically relating to a substation inspection robot positioning and mapping method based on particle filtering and graph optimization. Background Technology

[0002] Against the backdrop of the vigorous development of the power industry, substations, as an indispensable link in the power production and transportation process, are constantly being expanded and upgraded to adapt to the increasing voltage levels and the expansion of input lines. This has led to a growing difficulty for manual inspections, making the replacement of manual inspections by substation inspection robots an urgent necessity. Substation inspection robots are comprehensive systems integrating environmental perception, dynamic decision-making, motion control, and equipment detection. They can complete inspection tasks through map building, autonomous positioning, path planning, and equipment detection, improving inspection efficiency while effectively avoiding a series of problems faced by manual inspections.

[0003] Map building and autonomous localization are the foundation of robot navigation and movement, and also the core of robot inspection operations. Therefore, Simultaneous Localization and Mapping (SLAM) is one of the key technologies for intelligent substation inspection robots.

[0004] The Rao-Blackwellized Particle Filter (RBPF) method, based on Monte Carlo sampling theory, is an important method in filtered SLAM algorithms. Its core idea is to represent the distribution of states using random state particles drawn from the posterior probability; the more particles, the higher the accuracy of the RBPF method. However, since each particle carries a complete map, too many particles can put enormous pressure on memory and affect the real-time performance of the entire system. Therefore, improving the accuracy of the RBPF method with fewer particles is a current research direction for RBPF improvement.

[0005] Currently, graph optimization methods are a hot topic in academic research. They use vertices as variables to be optimized and observation equations as constraints, constructing a nonlinear least squares problem to minimize the error, and then iteratively optimizing it using methods such as gradient descent, Gauss-Newton's method, and Levenberg-Marquardt (LM) algorithm. Graph-optimized SLAM incorporates states from all past time steps, resulting in slower error divergence and stronger stability, but significantly increases computational cost. However, with the discovery of sparsity in the optimization matrix and continuous leaps in computer performance, real-time optimization of SLAM has become possible.

[0006] In the complex outdoor inspection environment of substations, the site is relatively open with few structured scenes, and the environmental information obtained by single-line lidar is limited. The matching error of the optimized SLAM front end is large, which greatly limits the general optimization methods. Although the particle filter algorithm can achieve relatively ideal results in the short term, its error will continue to diverge over time. Summary of the Invention

[0007] To address the shortcomings of existing technologies, this invention proposes a robot localization and mapping system and method based on particle filtering and graph optimization. The system uses a particle filter as a front-end odometry to estimate a good initial pose value and generate a local sub-map. By combining graph optimization with the advantage of minimizing errors using information from all time points, high-precision pose and grid map can be obtained while meeting real-time requirements.

[0008] The SLAM problem outputs the best estimate of robot pose and current map under the condition of only observation input. The RBPF method decomposes this problem into an independent robot localization problem and an environmental map construction problem based on the known robot pose, as shown in Equation (1).

[0009] P(x 1:t ,m|u 1:t-1 ,z 1:t )=P(x 1:t |u 1:t-1 ,z 1:t )P(m|x 1:t ,z 1:t (1)

[0010] In the formula u 1:t This represents all wheel speed measurement information of the robot from time 1 to time t, z 1:t Let x represent the laser observations of the robot from time 1 to time t. 1:t and m represent the robot pose and map estimated by the robot localization and mapping system from time 1 to t, respectively.

[0011] The robot localization problem is decomposed and simplified by using the conditional Bayesian formula and the conditional joint probability formula, and the pose estimation problem is transformed into an incremental estimation problem, as shown in Equation (2).

[0012] P(x 1:t |u 1:t-1 ,z 1:t )=P(z t |x t )P(x t |x t-1 ,u t-1 )P(x 1:t-1 |u 1:t-2 ,z 1:t-1 (2)

[0013] Wherein, P(x 1:t-1 |u 1:t-2 ,z 1:t-1 P(x) represents the robot pose carried by the particle swarm at the previous moment. t |x t-1 ,u t-1 For each particle, the pose at the previous moment and the predicted pose obtained from odometry are accumulated according to the robot's kinematics model, and the observation model P(z) is used. t |x t Weights are calculated.

[0014] This invention optimizes the resampling strategy based on the SIR (sampling importance resampling) filter, using a minimum sampling variance resampling method instead of a simple weight-based resampling method, effectively improving particle diversity. A subgraph system is introduced, maintaining two active subgraphs simultaneously. Constraints are constructed using a frame of laser data and the two subgraphs, and closure constraints are built through correlation scanning matching for backend optimization. The nonlinear least squares method is used to minimize the error, optimizing all historical poses and subgraph poses, and new poses are added to the optimized pose set.

[0015] A robot localization and mapping system based on particle filtering and graph optimization, including a local SLAM module and a global SLAM module;

[0016] The local SLAM module includes a particle ensemble generation module, a resampling module, and a subgraph system; the global SLAM module includes a constraint construction module and a pose optimization module.

[0017] The particle set generation module is used to perform scanning matching and particle sampling to generate a particle set, in which each particle will obtain an odometry pose and generate and carry a current sub-image.

[0018] The resampling module employs an improved resampling method. The particle set generation module continuously resamples the particle set until the weight dispersion of the particle set exceeds a set threshold, thereby obtaining an optimized particle set.

[0019] The subgraph system is used to maintain two active subgraphs simultaneously, preserving a sufficient number of laser inter-frame pose constraints.

[0020] The constraint construction module uses CSM matching and branch-bound to accelerate the acquisition of closure constraints;

[0021] The pose optimization module uses least squares to solve the optimal pose and map based on the odometry constraints calculated by the local SLAM module and the loop closure constraints constructed by the constraint construction module to construct the optimization problem.

[0022] When a new frame of laser data enters the system (after one observation), the local SLAM module receives the data and integrates all wheel velocity data between the current moment and the previous laser reception moment. The particle assemblies generation module performs scan matching and particle sampling to generate a particle assemblies, where each particle receives an odometry pose, generating and carrying a current sub-map. Using an improved resampling method, the resampling module continuously resamples until the weight dispersion of the particle assemblies exceeds a set threshold, resulting in an optimized particle assemblies. The sub-map system simultaneously maintains two active sub-maps, preserving a sufficient number of inter-laser frame pose constraints. The system then enters the global SLAM module, where the constraint construction module uses CSM matching and branch-and-bound to accelerate the acquisition of loop closure constraints. Finally, the pose optimization module solves the optimization problem by using least squares to find the optimal pose and map based on the odometry constraints calculated by the local SLAM module and the loop closure constraints obtained by the constraint construction module.

[0023] The robot localization and mapping method based on particle filtering and graph optimization has the following steps:

[0024] Step 1: Perform scanning matching and particle sampling through the particle set generation module to generate a particle set;

[0025] Step 2: Improve resampling, continuously resample until the weight dispersion of the particle set is higher than the set threshold, and obtain the optimized particle set;

[0026] Step 3: Complete subgraph maintenance through the subgraph system;

[0027] Step 4: Loop closure detection, using CSM matching and branch and bound to accelerate the acquisition of loop closure constraints;

[0028] Step 5: Based on the calculated odometer constraints and the lapsing constraints obtained from the constraint construction module, construct the graph optimization problem and solve the optimal pose and map using least squares.

[0029] The beneficial effects of this invention are as follows:

[0030] This invention proposes a SLAM method that combines particle filtering and graph optimization. The particle filter is used as a front-end odometry to estimate a good initial pose value and generate a local sub-map. The graph optimization method takes advantage of minimizing the error by utilizing information from all time points. Under the premise of meeting real-time requirements, a high-precision pose and grid map are obtained, which provides a reference for subsequent substation inspection work. Attached Figure Description

[0031] Figure 1 This is a system processing flowchart of an embodiment of the present invention;

[0032] Figure 2This is a flowchart of the subgraph generation algorithm according to an embodiment of the present invention;

[0033] Figure 3 This is a schematic diagram illustrating the global optimization of an embodiment of the present invention;

[0034] Figure 4 For the observation reliability of the embodiment of the present invention, the observation distribution L (i) The impact;

[0035] Figure 5 This is a schematic diagram illustrating sub-graph maintenance in an embodiment of the present invention;

[0036] Figure 6 This is a schematic diagram illustrating branch delimitation acceleration according to an embodiment of the present invention; Detailed Implementation

[0037] The method of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0038] Figure 1 This is a system processing flowchart of an embodiment of the present invention;

[0039] The specific process of the local SLAM subgraph generation algorithm based on the SIR (sampling importance resampling) filter is as follows: Figure 2 As shown.

[0040] Global optimization employs a graph optimization method, using the laser frame pose and subgraph pose as nodes, and pose constraints between poses as edges. A least-squares problem of error is constructed and solved to obtain the optimal solution for each pose, such as... Figure 3 As shown. The pose constraints include front-end constraints constructed using local SLAM and loop closure constraints constructed using loop closure detection.

[0041] The robot localization and mapping method based on particle filtering and graph optimization has the following steps:

[0042] Step 1: Perform scanning matching and particle sampling through the particle set generation module to generate a particle set;

[0043] After a new frame of laser data enters the robot's localization and mapping system, the system initializes the particle swarm S at the current moment. t Circularly traverse the particle swarm S from the previous time step t-1 Get the particle from the previous time step (taking the i-th particle as an example). position Weight and carrying subgraph The motion model of the wheel velocity meter is used to predict the pose at time t through state propagation. Where u t-1is the wheel speedometer data at time t-1. Since the particles are not unique, several trajectories will be estimated at the same time, and the weight of each trajectory is crucial. is used to represent the trajectory of the particle, and its importance weight can be defined as Equation (3):

[0044]

[0045] Among them, the numerator is the target distribution, and the denominator is the proposal distribution. Each time an observation is made, such as calculating the weight of the complete trajectory, the computational amount of the importance weight of the trajectory will increase with time. Given the particle trajectory under the condition, by calculate the sub-graph corresponding to the trajectory and expand and simplify it, the importance weight recurrence formula (4) can be obtained.

[0046]

[0047] LiDAR matching is more accurate than odometer measurement. In terms of distribution, its variance is also more advantageous than that of the odometer. As Figure 4 shown, when the LiDAR frame has a high matching degree with the existing map, the reliability of the observation is high, the observation distribution L (i) interval is small, and the number of particles generated by LiDAR matching sampling falling in the interval L (i) is larger, and the probability distribution of the pose can be covered with fewer particles.

[0048] When the LiDAR matching degree is low, the observation reliability is low, and the motion model is used to sample and generate particles.

[0049] The observation reliability is judged by the maximum likelihood estimation of the map matching degree. First, search around the predicted pose and calculate the matching degree between the observation z t of the current frame and the sub-graph . If the matching degree exceeds the set threshold, it is considered that the observation reliability is high, as shown in Equation (5):

[0050]

[0051] If an extreme point with a matching degree exceeding the threshold is found around the predicted pose extreme point represents the probability peak region in the observation interval L (i) . Centered on , Δ is the radius, and k fixed points {x1,…,x k} are randomly sampled, and the sampling is completed.

[0052] Because the variance of the laser matching model is small, the sampled particle set is assumed to follow a Gaussian distribution, and its vector mean is calculated by combining laser observation and wheel speed measurement information. Covariance As shown in equation (6):

[0053]

[0054]

[0055] Where the normalization parameter T is the matrix transpose operator.

[0056] New particle point set From The probability calculation is performed using the multivariate normal distribution formula, and the proposed distribution π in the weights is improved as follows: The weight of the new particle is calculated as shown in equation (7).

[0057]

[0058] If, according to equation (5), there are no maps with a matching degree exceeding the threshold, ... Then use the motion model When sampling new particles, it is recommended that the distribution π use a motion model, and its importance weight is calculated as shown in equation (8).

[0059]

[0060] After sampling is completed, given the map from the previous time step... Current position } and observation z t Under the conditions, through Update the subgraph carried by each particle. Then, the subgraphs corresponding to each trajectory are integrated according to their weight ratios to obtain subgraph m. t .

[0061] Step 2: Improve resampling, continuously resample until the weight dispersion of the particle set is higher than the set threshold, and obtain the optimized particle set;

[0062] Resampling is the process of deleting low-weight particles and generating new particles while maintaining the total number of particles. Continuous resampling leads to particle dissipation, meaning all particles are copied from a single particle, resulting in a loss of particle diversity. Equation (9) is used to determine the degree of dispersion N of the weights. eff The decision on whether to resample is made, among which For particle normalization weights, when N eff If the weights of the particles are less than the set discrete threshold, it is considered that the weights of the particles differ significantly, and resampling is performed; otherwise, resampling is not performed.

[0063]

[0064] An improved resampling method is proposed, using sampling variance to measure the difference in particle distribution before and after resampling, thus assessing the extent to which resampling disrupts particle diversity. Sampling variance is calculated as the sum of squares of the differences between the actual number of times a particle is replicated and the expected number of times it is replicated. The smaller the sampling variance, the smaller the difference in particle distribution before and after resampling, and the higher the relative particle diversity. The formula for calculating sampling variance (SV) is shown in Equation (10).

[0065]

[0066] Where M represents the total number of particles before resampling, and N represents the total number of particles after resampling. Let be the number of times the particle actually replicates, and let the particle's weight at time t be... use This indicates the number of times the particle is expected to replicate.

[0067] The minimum sampling variance resampling method is used instead of the simple resampling method based on weight, so that the particle distribution before and after resampling tends to be consistent.

[0068] Use the floor function to round down and assign the expected number of replications to the particles.

[0069]

[0070] Calculate the residual weight of the particles And the total number of times all particles are replicated, L.

[0071]

[0072]

[0073] By copying the ML particles with the highest remaining weights and performing resampling, the deletion of high-quality particles with lower weights can be effectively reduced.

[0074] Step 3: Complete subgraph maintenance through the subgraph system;

[0075] To ensure sufficient constraints during global optimization, the robot localization and mapping system employs a subgraph system, using dual threads to maintain two active subgraphs simultaneously. A single laser scan frame is matched against both active subgraphs using a particle filter. When the number of laser frames in an active subgraph reaches a set threshold ρ... maxThe subgraph is no longer maintained, is converted from an active subgraph to a historical subgraph, and the pose of the last laser frame is used as the starting pose of the newly generated active subgraph. During system initialization, only one active subgraph is maintained. When the number of laser frames in this subgraph reaches ρ... max At / 2, a new thread is started to generate and maintain the second active subgraph. This process is as follows: Figure 5 As shown.

[0076] Step 4: Loop closure detection, using CSM matching and branch and bound to accelerate the acquisition of loop closure constraints;

[0077] Loop closure detection uses a scan-to-map matching method, performing a search and matching process for each laser frame. When a historical subgraph m exists... his With current frame node x now If the relative distance is below a set threshold, a scan and matching process is initiated. To avoid wasting computational resources by searching the entire map, the search window W is limited to the current frame node x. now The region is a square with a side length of d centered at the pose ξ.

[0078] The scanning matching algorithm uses the CSM frame matching algorithm, assuming the current frame node x now The loop-bound pose ξ' must exist within the search window W. A brute-force search is performed within W with a step size λ = (Δx, Δy, Δθ), traversing all poses within W. Specifically, for each pose, the radar pose of the current frame is represented by the transition matrix T. ξ =(R ξ ,t ξ This indicates that the radar observation data h of the current frame will be used. k Projected onto local subgraph m his The above can be represented by equation (14).

[0079] T ξ ·h k =R ξ ·h k +t ξ (14)

[0080] The score for a pose is calculated by summing the probability values ​​of all grid cells hit by the laser using the `score()` function. If the score exceeds a set threshold, a loop closure is considered to have occurred, and the pose with the highest score is the one in the current frame x. now In the historical subgraph m his The observed pose (i.e., the loop constraint pose) ξ' in the equation is shown in equation (15).

[0081] in

[0082] While brute-force search matching avoids getting trapped in local optima, its massive computational cost can impact the system's real-time performance. For example, with a search window W of 10m × 10m, a step size λ = (0.01m, 0.01m, 1°), and a search range of 30°, 10... 3 ×10 3 ×30=3×10 7 The calculation is repeated several times. To reduce the computational load, the branch and bound method is used to accelerate the calculation.

[0083] The branch and bound method smooths and blurs the map within the search area, reducing its resolution from r to 2. 1 r、2 2 r、2 3 r to 2 d r. Calculate using Equation (15) from the map with the lowest resolution, retaining the region with the highest score and discarding the low-scoring regions. Use the region with the highest score as a new search window and increase the resolution by one time to continue searching for matches. Repeat this process until the search resolution reaches the highest resolution, such as... Figure 6 As shown.

[0084] Step 5: Based on the calculated odometer constraints and the lapsing constraints obtained from the constraint construction module, construct the graph optimization problem and solve the optimal pose and map using least squares.

[0085] Use a laser frame pose and a subgraph pose as vertex x β and m α The pose constraints between vertices are used as edges for global optimization. Specifically, the error function is constructed as follows: vertex x is obtained through loop closure detection and matching calculation in step 4. β and m α The loop-relative pose, i.e., the observation z' αβ =(t αβ ,θ αβ Transformation matrix Z' αβ =V2T(z') αβ ), where t is the translation vector and θ is the rotation amount. The vertex x is obtained through steps 1 and 2. β and m α The pose is ξ β and ξ α Using pose ξ β and ξ α The calculated odometry relative pose, i.e., the predicted value Z, is obtained. αβ =V2T(ξ α ) T V2T(ξ β ).

[0086] V2T represents the transformation matrix of the point in the global map coordinate system. Therefore, the error function is set as Equation (16).

[0087] e αβ =T2V(Z) αβ -1 Z' αβ (16)

[0088] This error represents vertex m α and x β The relative pose of the loop closure obtained by loop closure detection and the relative pose of the odometry obtained by the local SLAM module.

[0089] in R is the rotation matrix obtained by the rotation amount θ, and t is the translation vector.

[0090] Calculated

[0091] Therefore, the matrix expression of the error function is equation (17).

[0092]

[0093] From a global perspective, the global pose of all laser frames is denoted as X = {ξ}. β}, β=1,2,…,n, let the global pose of all subgraphs be denoted as M={ξ α}, α=1,2,…,s, and the local pose ξ generated by the particle filter algorithm. αβ Constraints are applied. The nonlinear least squares expression for global optimization is then given by equation (18).

[0094]

[0095] Among them, E 2 (ξ α ,ξ β Ω αβ ,ξ αβ )=e(ξ α ,ξ β ξ αβ ) T Ω αβ e(ξ α ,ξ β ξ αβ ), e is the error function constructed using equation (17), Ω αβ The information matrix is ​​a Gaussian distribution, representing the degree of importance given to each term of the error. The function ρ is the Huber loss function, used to penalize terms with excessively large errors.

[0096] By inputting all the vertices and edges obtained through laser inter-frame matching into the Google open-source Ceres library, the optimization problem can be solved to obtain the optimal pose of each vertex.

Claims

1. A robot localization and mapping system based on particle filtering and graph optimization, characterized in that, Includes local SLAM modules and global SLAM modules; The local SLAM module includes a particle ensemble generation module, a resampling module, and a subgraph system; the global SLAM module includes a constraint construction module and a pose optimization module. The particle set generation module is used to perform scanning matching and particle sampling to generate a particle set, in which each particle will obtain an odometry pose and generate and carry a current sub-image. The resampling module employs an improved resampling method. The particle set generation module continuously resamples the particle set until the weight dispersion of the particle set exceeds a set threshold, thereby obtaining an optimized particle set. The subgraph system is used to maintain two active subgraphs simultaneously, preserving a sufficient number of laser inter-frame pose constraints. The constraint construction module uses CSM matching and branch-bound to accelerate the acquisition of closure constraints; The pose optimization module uses least squares to solve the optimal pose and map based on the odometry constraints calculated by the local SLAM module and the loop closure constraints constructed by the constraint construction module to construct the optimization problem.

2. A robot localization and mapping method based on particle filtering and graph optimization, characterized in that, Here are the steps: Step 1: Perform scanning matching and particle sampling through the particle set generation module to generate a particle set, in which each particle will obtain an odometry pose, generating and carrying a current sub-image; Step 2: Improve resampling, continuously resample until the weight dispersion of the particle set is higher than the set threshold, and obtain the optimized particle set; Step 3: Subgraph maintenance is completed through a subgraph system, which is used to maintain two active subgraphs simultaneously and maintain a sufficient number of laser inter-frame pose constraints. Step 4: Loop closure detection, using CSM matching and branch and bound to accelerate the acquisition of loop closure constraints; Step 5: Based on the calculated odometer constraints and the lapsing constraints obtained from the constraint construction module, construct the graph optimization problem and solve the optimal pose and map using least squares.

3. The robot localization and mapping method based on particle filtering and graph optimization according to claim 2, characterized in that, The specific method for step 1 is as follows; After a new frame of laser data enters the robot's localization and mapping system, the system initializes the particle swarm S at the current moment. t Circularly traverse the particle swarm S from the previous time step t-1 Obtain the particles from the previous moment. position Weight and carrying subgraph The motion model of the wheel velocity meter is used to predict the pose at time t through state propagation. Where u t-1 The data is based on wheel velocity measurements at time t-1. Since the particles are not unique, several trajectories will be estimated at the same time. The weight of each trajectory is crucial, and therefore, the following approach is used. The trajectory of the particle can be represented by its importance weight, which can be defined by equation (3): The numerator represents the target distribution, and the denominator represents the proposed distribution. With each observation, if the complete trajectory weight is calculated, the computational complexity of the trajectory importance weight increases over time. (This is in the context of known particle trajectories.) Under the conditions, through Calculate the corresponding subgraph of the trajectory By expanding and simplifying it, we can obtain the importance weight recursive formula (4); When the laser frame matches the existing map well, the reliability of the observation is high, and the observation distribution L (i) The interval is small, and the particles generated by laser matching sampling fall within the interval L. (i) With a larger number of particles, the probability distribution of the pose can be covered with fewer particles; When the laser matching degree is low, the observation reliability is low, so a motion model is used. Sample generation of particles; The observation reliability is judged by the maximum likelihood estimated map matching degree. First, search around the predicted pose and calculate the observation z t of the current frame with the sub-map If the matching degree exceeds the set threshold, it is considered that the observation reliability is high, as shown in Equation (5): If the pose is predicted The search found extreme points in the surrounding area with a matching degree exceeding the threshold. extreme points That is, representing the observation interval L (i) In the medium probability peak region, with Centered on a point with radius Δ, a fixed number of k points {x1,…,x2} are randomly sampled. k }, that is, sampling is complete; Because the variance of the laser matching model is small, the sampled particle set is assumed to follow a Gaussian distribution, and its vector mean is calculated by combining laser observation and wheel speed measurement information. Covariance As shown in equation (6): Where the normalization parameter T is the matrix transpose operator; New particle point set Will be from The probability calculation is performed using the multivariate normal distribution formula, and the proposed distribution π in the weights is improved as follows: The weight of the new particle is calculated as shown in equation (7); If, according to equation (5), there are no maps with a matching degree exceeding the threshold, ... Then use the motion model When sampling new particles, it is recommended that the distribution π use a motion model, and its importance weight is calculated as shown in equation (8). After sampling is completed, given the map from the previous time step... Current position } and observation z t Under the conditions, through Update the subgraph carried by each particle. Then, the subgraphs corresponding to each trajectory are integrated according to their weight ratios to obtain subgraph m. t .

4. The robot localization and mapping method based on particle filtering and graph optimization according to claim 3, characterized in that, The specific method for step 2 is as follows; Resampling is the process of deleting low-weight particles and generating new particles while keeping the total number of particles constant. However, continuous resampling leads to particle dissipation, where all particles are copied from a single particle, resulting in a loss of particle diversity. Equation (9) is used to determine the degree of dispersion N of the weights. eff The decision on whether to resample is made, among which For particle normalization weights, when N eff If the weights of the particles are less than the set discrete threshold, it is considered that the weights of the particles differ significantly, and resampling is performed; otherwise, resampling is not performed. The improved resampling method uses sampling variance to measure the difference in particle distribution before and after resampling, and measures the degree of destruction of particle diversity by resampling. Sampling variance is calculated as the sum of squares of the difference between the actual number of times a particle is replicated and the expected number of times it is replicated. The smaller the sampling variance, the smaller the difference in particle distribution before and after resampling, and the higher the particle diversity. The formula for calculating sampling variance (SV) is as shown in Equation (10). Where M represents the total number of particles before resampling, and N represents the total number of particles after resampling. Let be the number of times the particle actually replicates, and let the particle's weight at time t be... use This indicates the expected number of times the particle will replicate. The minimum sampling variance resampling method is used instead of the simple resampling method based on weight, so that the particle distribution before and after resampling tends to be consistent; Use the floor function to round down and assign the expected number of replications to the particles. Calculate the residual weight of the particles and the total number of times all particles are replicated, L; By copying the ML particles with the highest remaining weights and performing resampling, the deletion of high-quality particles with lower weights can be effectively reduced.

5. The robot localization and mapping method based on particle filtering and graph optimization according to claim 4, characterized in that, Step 3: Complete subgraph maintenance through the subgraph system; To ensure sufficient constraints during global optimization, the robot localization and mapping system employs a subgraph system, using dual threads to maintain two active subgraphs simultaneously. A single laser scan frame is matched against both active subgraphs using a particle filter. When the number of laser frames in an active subgraph reaches a set threshold ρ... max The subgraph is no longer maintained, and it is converted from an active subgraph to a historical subgraph. The pose of the last laser frame is used as the starting pose of the newly generated active subgraph. During system initialization, only one active subgraph is maintained. When the number of laser frames in this subgraph reaches ρ... max When / 2, start a new thread to generate and maintain the second active subgraph.

6. The robot localization and mapping method based on particle filtering and graph optimization according to claim 5, characterized in that, The specific method for step 4 is as follows; Loop closure detection uses a scan-to-map matching method, performing a search and matching process for each laser frame; when a historical submap m exists... his With current frame node x now If the relative distance is below a set threshold, a scan and matching process is performed; to avoid wasting computational resources by searching the entire map, the search window W is limited to the current frame node x. now A square region with a side length of d centered at the pose ξ; The scanning matching algorithm uses the CSM frame matching algorithm, assuming the current frame node x now The loop-bound pose ξ' must exist within the search window W; a brute-force search is performed within W with a step size λ = (Δx, Δy, Δθ), traversing all poses within W; specifically for each pose, the radar pose of the current frame is represented by the transition matrix T. ξ =(R ξ ,t ξ This indicates that the radar observation data h of the current frame will be used. k Projected onto local subgraph m his The above can be represented by equation (14); T ξ ·h k =R ξ ·h k +t ξ (14) The score for a pose is calculated by summing the probability values ​​of all grid cells hit by the laser using the score() function. If the score exceeds a set threshold, a loop closure is considered to have occurred, and the pose with the highest score is the one in the current frame x. now In the historical subgraph m his The observed pose ξ' in the equation is shown in equation (15); Although brute-force search matching avoids getting trapped in local optima, its huge computational cost can affect the real-time performance of the system; in order to reduce the computational cost, the branch and bound method is used to accelerate the calculation. The branch and bound method smooths and blurs the map within the search area, reducing its resolution from r to 2. 1 r、2 2 r、2 3 r to 2 d r; calculate using formula (15) from the map with the lowest resolution, retain the area with the highest score, and cut off the low-scoring areas; take the area with the highest score as the new search window, and increase the resolution by one time to continue searching for matching; repeat this process until the search resolution reaches the highest resolution.

7. The robot localization and mapping method based on particle filtering and graph optimization according to claim 6, characterized in that, Step 5 is explained in detail below; Use a laser frame pose and a subgraph pose as vertex x β and m α The pose constraints between vertices are used as edges for global optimization; specifically, the error function is constructed by calculating the vertex x through loop closure detection in step 4. β and m α The loop-relative pose, i.e., the observation z' αβ =(t αβ ,θ αβ Transformation matrix Z' αβ =V2T(z') αβ ), where t is the translation vector and θ is the rotation amount; the vertex x is obtained through steps 1 and 2. β and m α The pose is ξ β and ξ α Using pose ξ β and ξ α The calculated odometry relative pose, i.e., the predicted value Z, is obtained. αβ =V2T(ξ α ) T V2T(ξ β ); V2T represents the transformation matrix of the point in the global map coordinate system; therefore, the error function is set as equation (16); e αβ =T2V(Z αβ -1 Z' αβ ) (16) This error represents vertex m α and x β The relative pose of the loop closure obtained by loop closure detection and the relative pose of the odometry obtained by the local SLAM module; in R is the rotation matrix obtained by the rotation amount θ, and t is the translation vector; Calculated Therefore, the matrix expression of the error function is equation (17); From a global perspective, the global pose of all laser frames is denoted as X = {ξ}. β },β=1,2,…,n, denote the global pose of all subgraphs as M'={ξ α }, α=1,2,…,s, and the local pose ξ generated by the particle filter algorithm. αβ If constraints are applied, the nonlinear least squares expression for global optimization is given by equation (18). Among them, E 2 (ξ α ,ξ β Ω αβ ,ξ αβ )=e(ξ α ,ξ β ξ αβ ) T Ω αβ e(ξ α ,ξ β ξ αβ ), e is the error function constructed using equation (17), Ω αβ The information matrix is ​​a Gaussian distribution, representing the degree of importance given to each term of the error; the function ρ is the Huber loss function, used to penalize terms with excessively large errors. By inputting all the vertices and edges obtained through laser inter-frame matching into the Google open-source Ceres library, the optimization problem can be solved to obtain the optimal pose of each vertex.

Images