Robot long-period slam method and system based on graph sparsity maintenance

Abstract

The application discloses a robot long-period SLAM method and system based on graph sparsity maintenance, and comprises the following steps: S1, performing SLAM mapping, generating a submap according to environment information measured continuously when a robot moves, and splicing all the submaps to form an initial meta-map; S2, rasterizing all the submaps contained in the meta-map, and generating a grid value centroid of each submap in the meta-map based on a probability of the submap grid being occupied; S3, detecting a centroid position of each updated submap, obtaining a centroid distance between the updated submap and all the submaps contained in the meta-map, and merging or deleting the updated submap when the centroid distance is less than a threshold value; and S4, repeating step S3 in a robot moving and measuring process until a navigation task is completed. The application solves the problem of picture data update redundancy in the process of establishing a map and positioning by SLAM, reduces a hardware memory occupancy rate, and has good robustness.

Description

Technical Field

[0001] This invention relates to a long-cycle SLAM method and system for robots based on graph sparsity maintenance, belonging to the field of robot localization and mapping technology. Background Technology

[0002] SLAM (Simultaneous Localization and Mapping) is a technique for simultaneously building a map and locating the robot, commonly used in autonomous vehicles, robotics, and augmented reality. Graph optimization algorithms play a crucial role in SLAM. These algorithms utilize sensor data and constraints to solve the map building and localization tasks in SLAM, improving accuracy and robustness. In SLAM, graph optimization algorithms typically use graph models to represent the map and the robot's state, optimizing state estimation by minimizing an error function. This error function includes error terms from sensor observations, motion models, and environmental constraints. Through an iterative optimization process, graph optimization algorithms adjust the values ​​of state variables to minimize the error function, resulting in more accurate maps and localization results.

[0003] Recent methods have demonstrated the superior performance of graph optimization algorithms in graph construction. However, because they require accumulating data for each observation, updating the map is not as straightforward as with filtering algorithms. Furthermore, in long-lived tasks, where completed parts of the graph are revisited multiple times, graph optimization algorithms add each observation to the graph model. To maintain consistency and global optimization, the algorithm needs to retain historical observations and constraints, not just the current state. This results in the need to store historical data of past observations and constraints in machine memory, which accumulates continuously as the task progresses, leading to a linear increase in memory usage.

[0004] Therefore, using graph optimization algorithms in SLAM and maintaining graph sparsity over long-term use is extremely important. Traditional methods typically maintain sparsity by compressing and simplifying historical data. For earlier observations and constraint data, compression or simplification is used to reduce memory usage. This step requires detecting overlapping graphs to determine which data to compress and simplify. The number of overlapping rasters in submaps is often used to determine whether to delete a submap. The threshold for the number of rasters needs to be manually tuned. Setting it too low will result in a large number of overlapping submaps, while setting it too high will cause necessary submaps to be deleted too quickly when the environment changes, resulting in missing map information and difficulties in practical applications. Summary of the Invention

[0005] The technical problem solved by this invention is to address the memory consumption problem caused by the excessive retention of historical data in existing graph optimization algorithms used in SLAM. This invention provides a long-term SLAM method and system for robots based on graph sparsity maintenance, which updates the map with new environmental changes to ensure the accuracy of navigation and positioning while maintaining the sparsity of the graph.

[0006] This invention is achieved using the following technical solution:

[0007] This invention discloses a long-cycle SLAM method for robots based on graph sparsity maintenance, which specifically includes the following steps:

[0008] S1. Perform SLAM mapping. Generate sub-maps based on the environmental information continuously measured during robot movement. All sub-maps are stitched together to form the initial meta-map.

[0009] S2. Rasterize all submaps contained in the metamap, obtain the submap grid value based on the probability that the submap grid is occupied, replace the quality of the submap centroid with the submap grid value, and generate the grid value centroid of each submap in the metamap.

[0010] S3. During the map update phase, detect the centroid position of the raster value of each updated sub-map, and obtain the centroid distance between the updated sub-map and all sub-maps contained in the metamap. Merge or delete updated sub-maps whose centroid distance is less than the threshold, thereby updating the metamap.

[0011] S4. Repeat step S3 during the robot's movement and measurement process until the navigation task ends.

[0012] In the long-period SLAM method for robots based on graph sparsity maintenance of the present invention, in step S1, Cartographer is used as the SLAM algorithm, and the environmental information collected by the robot is input into the SLAM algorithm. Let the robot pose ξ at the initial node obtained by the lidar sensor at the first moment be... S0 for:

[0013]

[0014] in, and These represent the robot's two-dimensional translation coordinates and rotation angles within the map coordinate system at the first moment;

[0015] The robot continuously scans, matches, and aligns the environmental information it collects, and then compares the environmental information collected after each subsequent movement at a set distance with ξ. S0After the Cartographer front-end filtering algorithm processes the environmental information data collected at each time step, and then optimizes the scan through Ceres, the robot pose at the corresponding environmental information collection time n is obtained through matching: and These represent the robot's two-dimensional translation coordinates and rotation angle within the map coordinate system at time n, respectively.

[0016] After accumulating n nodes, the grid map obtained by superimposing the grid values ​​converted from the LiDAR point cloud information carried by each node is the sub-map. The initial node robot pose ξ S0 This refers to the pose of the origin of the sub-map's coordinate system in the map's coordinate system.

[0017] Each submap is based on its own ξ S0 Connected with other submaps to form the initial metamap, the optimized submap coordinate system pose ξ is then output by the backend in Cartographer. s .

[0018] In the long-cycle SLAM method for robots based on graph sparsity maintenance of the present invention, the step of obtaining the sub-map grid value in step S2 is as follows:

[0019] The metamap, constructed by stitching together submaps, is modeled as a standard occupied grid map. Each contained submap is one of the occupied grid maps, and the probability of one grid cell in a submap being occupied is expressed by the following formula:

[0020]

[0021] Among them, c ij Submap m i The j-th grid cell on the grid, P(c ij ) represents the raster c ij The probability of being occupied. Represents grid c ij The probability of being idle;

[0022] P(c ij )and The initial value is set to 0.5, and P(c) is taken. ij )and The ratio is denoted as odds(c ij ), for odds(c ij Taking the logarithm is denoted as l(c ij ), used to represent grid c ij The raster values ​​are:

[0023] l(c ij ) = log(odds(c ij )),

[0024] Suppose that at time t-1, the grid c ij The grid value is l t-1 (c ij At time t, the robot's new measurement data is z. t , grid c ij The raster values ​​are updated to:

[0025]

[0026]

[0027] in, The value is a fixed value set according to the parameters of the lidar sensor, l occupied For the updated value of the occupied grid, l free To update the value of a grid that is idle, l occupied and l free The value range is 0 to 1, excluding the endpoints 0 and 1;

[0028] When the light from the LiDAR sensor passes through the grid map, the probability of each grid cell being occupied is updated: if the light from the LiDAR sensor hits a grid cell, the grid cell value increases by l. occupied If the light from the lidar sensor passes through the grid but does not hit the grid, the grid value decreases by 1. free ;

[0029] After updating the raster values ​​of all submaps, the submaps store the final state of all raster values. Based on the position and value of all raster cells, the raster values ​​are replaced with the mass using the centroid formula to obtain the raster centroid of the entire submap.

[0030] Furthermore, based on the position and grid value of each grid cell in the updated sub-map, the grid value is replaced by the mass using the centroid formula to calculate the centroid of each sub-map in the map coordinate system. The centroid calculation formula for the sub-map is as follows:

[0031] B i (x,y)=(x b ,y b ),

[0032]

[0033]

[0034] q ij =l(c ij ),

[0035] Among them, B iLet x be the centroid of the submap. b ,y b B is the centroid of the submap. i The position coordinates q in the map coordinate system ij Let x be the mass of each particle in a discrete particle system. ij y ij ) is the position of each particle;

[0036] The pose ξ of the submap coordinate system corresponding to the submap obtained in step S1 s The center of mass B i Transform to the map coordinate system to obtain the raster centroids of the submap in the map coordinate system:

[0037]

[0038] in, Cen is the rotation matrix for pose transformation based on the submap coordinate system of the submap. i This represents the centroid position of the submap grid value in the map coordinate system.

[0039] In the long-cycle SLAM method for robots based on graph sparsity maintenance of the present invention, the characteristic is that in step S3, navigation and map updating are performed based on the metamap. In the map update stage, a new submap is constructed through step S1 and the centroid of the newly constructed submap is calculated through step S2. The centroid distance between the newly constructed submap and the submap of the metamap is calculated by the following formula:

[0040]

[0041]

[0042] Among them, Cen i For a sub-map in a map coordinate system, m i The centroid position of the raster value, Cen k+1 For the newly constructed submap m k+1 The centroid position of the raster value. For Cen k+1 and Cen i The coordinate difference between them This is the distance from the centroid of the newly constructed submap to the centroid of the submap in the previous metamap.

[0043] Furthermore, if the centroid distance between the newly constructed sub-map and the sub-map of the metamap is less than the threshold d, coverage detection is performed on the newly constructed sub-map and the corresponding sub-map in the metamap. The specific steps are as follows:

[0044] First, transfer all grid cells in the newly constructed submap to the coordinate system of the corresponding submap in the metamap using the following formula:

[0045]

[0046] in, For submap m in the metamap i Newly constructed submap m in coordinate system k+1 The j-th grid cell, To determine the submap m in the metamap i coordinate system pose The rotation matrix of the transformation, To construct a new submap m k+1 coordinate system pose The inverse of the rotation matrix of the transformation, c (k+1)j For sub-map m k+1 The j-th grid cell;

[0047] In the transformed submap m k+1 Find all submaps on m i Does the grid position contain a grid value?

[0048] If all submaps m i For each grid position, a corresponding submap m can be found. k+1 The raster value indicates the submap m in the metamap. i The newly constructed submap m k+1 Complete coverage, including submap m in the metamap. i Delete, before deletion, add to the metamap and submap m in the metamap. i Constraints between adjacent submaps;

[0049] If all submaps m i The grid position could not be completely found to correspond to the submap m k+1 If the raster value is given, the two submaps will be merged, and the result will be converted to submap m according to the following formula. i Submap m in coordinate system k+1 All raster values ​​are overlaid onto the submap m based on their location. i superior:

[0050]

[0051] l1(c ij ) represents the sub-map m in the overlaid metamap. i The raster value of the j-th cell on the grid. To construct a new submap m k+1 Transform to a submap m in a metamap i Let be the raster value of the j-th cell in the coordinate system.

[0052] Furthermore, looking up raster values ​​is implemented using hash tables.

[0053] Furthermore, the constraints between submaps are the coordinate transformations between the deleted submap and its adjacent submaps.

[0054] The present invention also discloses a long-cycle SLAM system for robots based on graph sparsity maintenance, including a memory and a processor. The memory stores a long-cycle SLAM program that can run on the processor. When the long-cycle SLAM program is executed by the processor, it implements the steps of the long-cycle SLAM method for robots based on graph sparsity maintenance as described in any one of claims 1-8.

[0055] The present invention also discloses a storage medium storing a long-cycle SLAM program, wherein when the long-cycle SLAM program is executed by a processor, it implements the steps of the robot long-cycle SLAM method based on graph sparsity maintenance as described in any one of claims 1-8.

[0056] The present invention, by adopting the above technical solution, has the following beneficial effects:

[0057] (1) This invention uses Cartographer as the main algorithm for SLAM. Cartographer is a graph-based backend optimization method that can greatly reduce the impact of cumulative error on map construction.

[0058] (2) This invention proposes a sub-map coverage detection method based on the centroid distance of grid values, which searches for redundant map information and trims it to maintain the sparsity of the map.

[0059] In this field, the number of overlapping grid cells in submaps is typically used to determine whether to delete a submap. The threshold for the number of grid cells needs to be manually adjusted; setting it too low will result in a large number of overlapping submaps, while setting it too high will cause the deletion of necessary submaps too quickly when the environment changes, leading to missing map information and difficulties in practical applications. Furthermore, the origin of the Cartographer submap coordinate system is the pose of the first frame scan in the map coordinate system. If the radar scan range is not 360 degrees or the environment changes, relying on the submap coordinate system to determine overlap only indicates that the two submaps have the same starting point; their subsequent trajectories may be completely different, and the areas covered by the submaps will also be completely different, thus leading to the deletion of too much information and missing map data. This invention uses the distance between the centroids of grid values ​​as the criterion for judging submap overlap. It first uses the distance between the centroids of grid values ​​for judgment, and then performs coverage detection on the two "overlapping" submaps, which is not only more rigorous but also saves computational resources.

[0060] (3) The present invention introduces a map update strategy that incorporates changes in subsequent mapping stages into the original map, thereby enhancing the loop robustness of the system.

[0061] In summary, the long-cycle SLAM method for robots based on graph sparsity maintenance provided by this invention compresses and simplifies image data by detecting and judging overlapping sub-maps, effectively maintaining the sparsity of the generated map images. This solves the problem of redundant image data updates in conventional SLAM during map building and localization, reduces the system hardware memory usage, helps to further improve computational efficiency, and has good robustness.

[0062] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. Attached Figure Description

[0063] Figure 1 This is a flowchart illustrating the long-cycle SLAM method for robots based on graph sparsity maintenance provided by the present invention.

[0064] Figure 2 This is a schematic diagram of the overall framework of the long-cycle SLAM method for robots based on graph sparsity maintenance provided by the present invention.

[0065] Figure 3 This is a schematic diagram of the graph optimization relationships during the map building phase.

[0066] Figure 4 This is a schematic diagram of the probability update of a raster map.

[0067] Figure 5 This is a schematic diagram of the centroid obtained from the raster map.

[0068] Figure 6 A diagram illustrating how a submap in a metamap is completely covered by a newly created submap.

[0069] Figure 7 This is a schematic diagram of a long-cycle SLAM system for robots based on graph sparsity maintenance, provided by the present invention. Detailed Implementation

[0070] Example

[0071] like Figure 1 As shown, the long-cycle SLAM method for robots based on graph sparsity maintenance of the present invention includes the following steps:

[0072] S1. Perform SLAM mapping. Generate sub-maps based on the environmental information continuously measured during robot movement. All sub-maps are stitched together to form the initial meta-map.

[0073] S2. Rasterize all submaps contained in the metamap, obtain the submap grid value based on the probability that the submap grid is occupied, replace the quality of the submap centroid with the submap grid value, and generate the grid value centroid of each submap in the metamap.

[0074] S3. During the map update phase, detect the centroid position of the raster value of each updated sub-map, and obtain the centroid distance between the updated sub-map and all sub-maps contained in the metamap. Merge or delete updated sub-maps whose centroid distance is less than the threshold, thereby updating the metamap.

[0075] S4. Repeat step S3 during the robot's movement and measurement process until the navigation task ends.

[0076] The following provides a detailed explanation of the above steps.

[0077] In step S1, SLAM mapping is performed to generate a sub-map. LiDAR is used as a sensor to collect environmental information input, and other sensors such as inertial measurement units or encoders can also be used as auxiliary sensors.

[0078] The mapping uses Cartographer as the SLAM algorithm. The environmental information collected by the LiDAR sensor is input into the algorithm. The algorithm will continuously generate sub-maps based on the environmental information obtained by the laser measurement as the robot moves. The construction of sub-maps is an iterative process of repeated alignment scans and sub-map coordinate systems.

[0079] To further explain the sub-map generation process, assume that the robot pose at the initial node obtained by the LiDAR sensor at the first time step is ξ. S0 It is called a scan.

[0080]

[0081] in, These are the robot's two-dimensional translation coordinates within the map coordinate system at the first moment. This represents the robot's rotation angle within the map coordinate system at the first moment.

[0082] As the SLAM mapping process continuously scans, matches, and aligns, the environmental information collected by laser measurement at each subsequent robot movement over a set distance is compared with ξ. S0The matching and scan matching methods are provided by the Ceres library. Scanning refers to the point data acquired by the LiDAR sensor, and matching involves comparing subsequent measurements with the first frame and calculating an odometry based on the flight path. This is derived from the well-known Ceres scan matching method in the Ceres library. Users can set the number of nodes, n, as needed. Each node represents the pose obtained by optimizing the scan matching using the LiDAR measurement data from the Cartographer front-end filtering algorithm at each sampling time. and These represent the robot's two-dimensional translation coordinates and rotation angle within the map coordinate system at time n, respectively. After accumulating n nodes, the raster map obtained by superimposing the raster values ​​converted from the LiDAR point cloud information carried by each node is the sub-map m. The initial node ξ... S0 The pose of the origin of the submap's coordinate system in the map coordinate system is referred to here as the submap coordinate system pose.

[0083] During the map generation process, several sub-maps m will be generated. i Each submap contains n nodes, and all submaps are pieced together to form the initial metamap M.

[0084] Specifically, each submap is positioned according to its own submap coordinate system pose ξ. S0 Connecting to other submaps allows for mutual pose confirmation and the acquisition of relative pose relationships between them. The coordinate transformation between two submap coordinate systems is initially provided by the connecting nodes, and then optimized by the backend in Cartographer, outputting the optimized submap coordinate system pose ξ. S .

[0085] like Figure 3 As shown, the metamap M ultimately contains k submaps, k*n nodes, and all transformation relationships between nodes and submaps. These transformation relationships are treated as constraints in graph optimization.

[0086] Step S2 generates raster centroids based on the submaps. The metamap M is the initial map in this method. Based on all the submaps contained in the metamap, the raster centroids of each submap are generated.

[0087] Furthermore, the description of grid values ​​is based on an occupied grid map. Assuming that laser measurements and individual laser beams are independent, map M is modeled as a standard occupied grid map, and each contained submap is also an occupied grid map. An occupied grid map is a grid composed of independent discrete binary random variables, with values ​​categorized as "occupied" and "free". The probability of a grid cell on a submap being occupied is expressed by the following formula:

[0088]

[0089] Among them, c ij Submap m i The j-th grid cell on the grid, P(c ij ) represents the raster c ij The probability of being occupied. Represents grid c ij The probability of being idle;

[0090] The initial values ​​of all graticles, i.e., P(c) before the upper-level data is observed. ij )and Set all values ​​to 0.5. Take P(c) ij )and The ratio is denoted as odds(c ij ), for odds(c ij Taking the logarithm is denoted as l(c ij ), used to represent grid c ij The raster values ​​are:

[0091]

[0092] l(c ij ) = log(odds(c ij )),

[0093] raster value l(c) ij ) can be used to represent a raster c ij The probability of being occupied, and the log operation transforms multiplication into addition, speeding up computation in the computer. Assume that at time t-1, submap m... i medium grid c ij The grid value is l t-1 (c ij At time t, the robot's new measurement data is z. t The raster value was updated to

[0094]

[0095] l0(c ij ) is a sub-map m i medium grid c ij The initial raster value, due to the previous setting of P(c) ij )and Both were set to 0.5, so L0(c ij The value of ) is 0, and the above expression becomes

[0096]

[0097] The value is a fixed value set according to the parameters of the lidar, provided or calibrated by the lidar manufacturer.

[0098]

[0099] Data z t This includes point cloud data acquired by the lidar sensor at time t, which contains information on whether each grid cell was hit, and c. ij One-to-one correspondence, and This value is a fixed value, P(c) ij |z t When it is determined whether the point cloud hits the grid, it is also determined whether the grid is occupied. The subsequent step is to update the corresponding grid value. In this way, every time the grid state is updated, it is added.

[0100] like Figure 4 As shown, when the laser beam emitted by the lidar sensor passes through the grid map, the probability of each grid cell is updated, and the value of the corresponding grid cell that hits the target cell increases by l. occupied The value of a raster that passes through but does not hit the raster is reduced by l. free l occupied For the updated value of the occupied grid, l free To update the value of a grid that is idle, l occupied and l free The value range is 0 to 1, preferably 0.4 to 0.9.

[0101] like Figure 5 As shown, after updating the raster values ​​of all submaps, submap m i The final state of the raster values ​​contained in all rasters is stored. Based on the location of all rasters and their raster values, the raster values ​​are replaced with the quality. Using the centroid formula, the centroid of the raster values ​​of the entire submap is calculated.

[0102] Specifically, in a discrete particle system, where each particle has a mass q ij and position (x) ij ,y ij For rasters in the "free" state, their submaps m i center of mass B i The calculation formula is:

[0103] B i (x,y)=(x b ,y b ),

[0104]

[0105]

[0106] q ij = l(c ij ),

[0107] where B i is the centroid of the sub - map, (x b , y b ) is the position coordinates of the centroid B i of the sub - map in the map coordinate system, l(c ij ) is the j - th grid on the sub - map m i , q ij is the mass of each particle in the discrete particle system, (x ij , y ij ) is the position of each particle. After calculating the centroid B i of each sub - map m i in the meta - map, through the pose ξ i of the sub - map coordinate system of the sub - map m s obtained in step S1, the centroid B i is transformed to the following map coordinate system to obtain the centroid of the grid values of the sub - map in the map coordinate system;

[0108]

[0109] where, is the rotation matrix converted according to the pose i of the sub - map coordinate system of the sub - map m , Cen i is the centroid position of the sub - map m i in the map coordinate system.

[0110] Step S3 detects the distance between centroids, repeats step S2 to obtain the centroid positions {Cen i |0 < i < k} of all sub - maps, and then the robot proceeds to the next stage of mapping, that is, navigation and continuous update of the map based on the meta - map, which is called the map update stage.

[0111] In the map update stage, the robot performs the sub - map construction link in step S1, and then calculates the centroid of the grid values Cen k+1 of the newly constructed sub - map m k+1 . Calculate the centroid distance between the newly constructed sub - map and the sub - maps of the meta - map through the following formula, and judge the position of Cen k+1 and the centroids of the grid values of all sub - maps {Cen i |0 < i < k} included in the previous meta - map through the following formula,

[0112]

[0113]

[0114] Cen i For a sub-map in a map coordinate system, m i The centroid position of the raster value, Cen k+1 For the newly constructed submap m k+1 The centroid position of the raster value. For Cen k+1 and Cen i The coordinate difference between them This is the distance from the centroid of the newly constructed submap to the centroid of the submap in the previous metamap.

[0115] If the distance from the centroid of the newly constructed submap to the centroid of the submap in the previous metamap If the value exceeds the threshold d, no further processing will be performed, and the new sub-map will be directly stored and used for normal map building. If the distance between the centroid of the newly constructed sub-map and the centroid of the sub-map in the previous meta-map is... If the value is less than the threshold d, proceed to the subsequent steps of this invention. Here, the threshold d parameter is set by the user, and the threshold d is usually related to node n. The threshold d is selected using the following empirical formula:

[0116]

[0117] In step S3, submaps with close centroid distances are merged or deleted. The newly constructed submap m is then obtained. k+1 raster value centroid Cen k+1 With a submap m in the metamap i Cen i If the distance is less than the threshold d, then for sub-map m k+1 And the map m i The specific steps for performing coverage detection are as follows:

[0118] First, transfer all grid cells in the newly constructed submap to the coordinate system of the corresponding submap in the metamap using the following formula:

[0119]

[0120] in, For submap m in the metamap i Newly constructed submap m in coordinate system k+1 The j-th grid cell, To determine the submap m in the metamap i coordinate system pose The rotation matrix of the transformation, To construct a new submap m k+1 coordinate system pose The inverse of the rotation matrix of the transformation, c(k+1)j For sub-map m k+1 The j-th grid cell.

[0121] like Figure 6 As shown, in the transformed submap m k+1 Find all submaps on m i Does the raster position contain a raster value? If all m i m can be found at any grid position k+1 The raster value indicates the submap m in the metamap. i The newly constructed submap m k+1 Complete coverage, at this point the submap m in the metamap will be... i Delete, before deletion, add to the metamap and submap m in the metamap. i Constraints between adjacent submaps.

[0122] To further explain, the lookup of raster values ​​is implemented using a hash table, supplemented by m. i-1 and m i+1 The constraint is the deleted submap m. i Coordinate transformation between two adjacent subgraphs.

[0123] If all submaps m i The grid position could not be completely found to correspond to the submap m k+1 If the raster value is given, the two submaps will be merged, and the result will be converted to submap m according to the following formula. i Submap m in coordinate system k+1 All raster values ​​are overlaid onto the submap m based on their location. i superior:

[0124]

[0125] l1(c ij ) represents the sub-map m in the overlaid metamap. i The raster value of the j-th cell on the grid. To construct a new submap m k+1 Transform to a submap m in a metamap i Let be the raster value of the j-th cell in the coordinate system.

[0126] In step S4, during the map update phase, the robot continuously repeats step S3 until this navigation task ends, and saves the last updated metamap locally. When the next task begins, it uses the saved metamap to start repeating steps S1 to S4.

[0127] like Figure 7As shown, the present invention also provides a long-cycle SLAM system for robots based on graph sparsity maintenance. This system is used for real-time navigation, localization and map building in fields such as unmanned vehicles, robots and augmented reality. It includes a processor 101 and a memory 102. The memory 102 stores a long-cycle SLAM program that can run on the processor 101. When the long-cycle SLAM program is executed by the processor, it implements the steps of the long-cycle SLAM method for robots based on graph sparsity maintenance described in the above embodiments.

[0128] The present invention also provides a computer-readable storage medium storing a SLAM map update program, which, when executed by a processor, implements the steps of the long-cycle SLAM method for robots based on graph sparsity maintenance as described in the above embodiments.

[0129] The specific embodiments of the long-cycle SLAM system based on graph sparsity maintenance and the readable storage medium of the present invention are basically the same as the long-cycle SLAM method for robots based on graph sparsity maintenance described above, and will not be repeated here.

[0130] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or system. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or system that includes that element.

[0131] The sequence numbers of the above embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0132] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device (which may be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in the various embodiments of the present invention.

[0133] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A long-cycle SLAM method for robots based on graph sparsity maintenance, characterized in that... Includes the following steps: S1. Perform SLAM mapping. Generate sub-maps based on the environmental information continuously measured during robot movement. All sub-maps are stitched together to form the initial meta-map. S2. Rasterize all submaps contained in the metamap, obtain the submap grid value based on the probability that the submap grid is occupied, replace the quality of the submap centroid with the submap grid value, and generate the grid value centroid of each submap in the metamap. The steps for obtaining the sub-map grid values ​​are as follows: The metamap, constructed by stitching together submaps, is modeled as a standard occupied grid map. Each contained submap is one of the occupied grid maps, and the probability of one grid cell in a submap being occupied is expressed by the following formula: ， Among them, c ij Submap m i The j-th grid cell on the grid, P(c ij ) represents the raster c ij The probability of being occupied, P(¬c) ij ) represents the raster c ij The probability of being idle; P(c ij ) and P(¬c ij The initial value is set to 0.5, and P(c) is taken as... ij ) and P(¬c ij The ratio of (c) is denoted as odds(c) ij ), for odds(c ij Taking the logarithm is denoted as l(c ij ), used to represent grid c ij The raster values ​​are: , Let the grid c be at time t-1. ij The grid value is l t-1 (c ij At time t, the robot's new measurement data is: , grid c ij The raster values ​​were updated to 、 ， in, The value is a fixed value set according to the parameters of the lidar sensor, l occupied For the updated value of the occupied grid, l free The update increments the value for an empty grid cell; when the LiDAR sensor's light passes through the grid map, the probability of each grid cell being occupied is updated: if the LiDAR sensor's light hits a grid cell, the grid cell's value is increased by l. occupied If the light from the lidar sensor passes through the grid but does not hit the grid, the grid value decreases by 1. free Complete the update of all sub-map grid values. The sub-map stores the final state of all grid values. Based on the position and grid value of all grids, use the centroid formula to replace the mass with the grid value to obtain the grid centroid of the entire sub-map. Based on the position and grid value of each grid cell in the updated sub-map, the grid value is replaced by the mass using the centroid formula to calculate the centroid of each sub-map in the map coordinate system. The centroid calculation formula for the sub-map is as follows: 、 、 、 ， Among them, B i Let the centroid of the submap be... The centroid B of the submap i The position coordinates q in the map coordinate system ij Let x be the mass of each particle in a discrete particle system. ij y ij The position of each mass point; the pose of the sub-map coordinate system corresponding to the sub-map obtained in step S1. The center of mass B i Transform to the map coordinate system to obtain the raster centroids of the submap in the map coordinate system: ， in, Cen is the rotation matrix for pose transformation based on the submap coordinate system of the submap. i This refers to the centroid position of the submap grid value in the map coordinate system. S3. During the map update phase, detect the centroid position of the raster value of each updated sub-map, and obtain the centroid distance between the updated sub-map and all sub-maps contained in the metamap. Merge or delete updated sub-maps whose centroid distance is less than the threshold, thereby updating the metamap. S4. Repeat step S3 during the robot's movement and measurement process until the navigation task ends.

2. The long-cycle SLAM method for robots based on graph sparsity maintenance according to claim 1, characterized in that, In step S1, Cartographer is used as the SLAM algorithm, and the environmental information collected by the robot is input into the SLAM algorithm. Let the robot pose ξ at the initial node be obtained by the LiDAR sensor at the first moment. S0 for: ， in, and These represent the robot's two-dimensional translation coordinates and rotation angles within the map coordinate system at the first moment; The robot continuously scans, matches, and aligns the environmental information it collects, and then compares the environmental information collected after each subsequent movement at a set distance with ξ. S0 After the Cartographer front-end filtering algorithm processes the environmental information data collected at each time step, and then optimizes the scan through Ceres, the robot pose at the corresponding environmental information collection time n is obtained through matching: , and These represent the robot's two-dimensional translation coordinates and rotation angle in the map coordinate system at time n. After accumulating n nodes, the grid map obtained by superimposing the grid values ​​converted from the LiDAR point cloud information carried by each node is the sub-map. The initial node robot pose ξ S0 This refers to the pose of the origin of the sub-map's coordinate system in the map's coordinate system. Each submap is based on its own ξ S0 Connected with other submaps to form the initial metamap, the optimized submap coordinate system pose is then output by the backend in Cartographer. .

3. The long-cycle SLAM method for robots based on graph sparsity maintenance according to claim 1, characterized in that, In step S3, navigation and map updates are performed based on the metamap. During the map update phase, a new submap is constructed through step S1, and the centroid of the newly constructed submap is calculated through step S2. The centroid distance between the newly constructed submap and the submap of the metamap is calculated using the following formula: 、 ， in, For a sub-map in a map coordinate system, m i The centroid position of the raster value, Cen k+1 For the newly constructed submap m k+1 The centroid position of the raster value. For Cen k+1 and The coordinate difference between them This is the distance from the centroid of the newly constructed submap to the centroid of the submap in the previous metamap.

4. The long-cycle SLAM method for robots based on graph sparsity maintenance according to claim 3, characterized in that, If the centroid distance between the newly constructed sub-map and the sub-map of the metamap is less than the threshold d, then perform coverage detection on the newly constructed sub-map and the corresponding sub-map in the metamap. The specific steps are as follows: First, transfer all grid cells in the newly constructed submap to the coordinate system of the corresponding submap in the metamap using the following formula: ， in, For submap m in the metamap i Newly constructed submap m in coordinate system k+1 The j-th grid cell, To determine the submap m in the metamap i coordinate system pose The rotation matrix of the transformation, To construct a new submap m k+1 coordinate system pose The inverse of the rotation matrix of the transformation, c (k+1)j For sub-map m k+1 The j-th grid cell; In the transformed submap m k+1 Find all submaps on m i Does the grid position contain a grid value? If all submaps m i For each grid position, a corresponding submap m can be found. k+1 The raster value indicates the submap m in the metamap. i The newly constructed submap m k+1 Complete coverage, including submap m in the metamap. i Delete, before deletion, add to the metamap and submap m in the metamap. i Constraints between adjacent submaps; If all submaps m i The grid position could not be completely found to correspond to the submap m k+1 If the raster value is given, the two submaps will be merged, and the result will be converted to submap m according to the following formula. i Submap m in coordinate system k+1 All raster values ​​are overlaid onto the submap m based on their location. i superior: ， l1(c ij ) represents the sub-map m in the overlaid metamap. i The raster value of the j-th cell on the grid. To construct a new submap m k+1 Transform to a submap m in a metamap i Let be the raster value of the j-th cell in the coordinate system.

5. The long-cycle SLAM method for robots based on graph sparsity maintenance according to claim 4, characterized in that, A hash table is used to look up raster values.

6. The long-cycle SLAM method for robots based on graph sparsity maintenance according to claim 4, characterized in that, The constraints between submaps are the coordinate transformations between the deleted submap and its adjacent submaps.

7. A long-cycle SLAM system for robots based on graph sparsity maintenance, characterized in that: The system includes a memory and a processor, wherein the memory stores a long-cycle SLAM program that can run on the processor, and the long-cycle SLAM program, when executed by the processor, implements the steps of the long-cycle SLAM method for robots based on graph sparsity maintenance as described in any one of claims 1-6.

8. A storage medium, characterized in that, The storage medium stores a long-cycle SLAM program, which, when executed by a processor, implements the steps of the robot long-cycle SLAM method based on graph sparsity maintenance as described in any one of claims 1-6.

Images