SLAM probability grid registration method and device

Abstract

The invention discloses an SLAM probability grid registration method and device, and the method comprises the steps: detecting whether the local matching of current laser scanning data and a prior probability grid map fails in a positioning process, and judging whether a map updating condition is satisfied or not when the matching fails; if yes, a dynamic map updating process is started, and if matching succeeds, the pose is updated, and positioning succeeds; based on the updated dynamic map, executing multi-stage positioning matching, which comprises the following steps of: firstly, performing global matching by using a priori map; if the global matching fails, performing global matching by using a dynamic map; and if the dynamic map matching fails, repositioning is carried out. The method has the beneficial effects that by introducing a dynamic map updating strategy, the system can respond to the environment change in real time when detecting that the local matching fails. According to the method, the perception and fusion capability of newly added semi-static objects is increased, and map information redundancy and accumulative drift are avoided by deleting outdated grids.

Description

Technical Field

[0001] This invention relates to the field of localization and mapping, and more particularly to a SLAM probabilistic grid registration method and apparatus. Background Technology

[0002] With the widespread application of mobile robots across various industries, Simultaneous Localization and Mapping (SLAM) technology has become a core support for enabling robots to navigate autonomously in unknown environments. Among these, 2D SLAM (2DSLAM) systems based on LiDAR often employ probabilistic grid maps for environmental modeling and real-time localization. Currently, typical 2DSLAM methods (such as Gmapping, Cartographer, and HectorSLAM) primarily rely on the following matching strategies to achieve localization and mapping:

[0003] Scan matching: Align the current frame laser point cloud with the existing raster map using methods such as Iterative Closest Point (ICP), Correlative Scan Matching (CSM), or Gauss-Newton optimization.

[0004] Probability-based grid matching: The matching likelihood is calculated based on the probability of a laser point falling into the grid, and the log-likelihood function is commonly used for evaluation.

[0005] Combining global and local matching: For example, Cartographer uses a submap strategy, which first constructs and matches submaps locally, and then performs global optimization through loop closure detection.

[0006] However, the above methods have significant limitations in real-world dynamic environments or long-term operating scenarios:

[0007] Fixed probability thresholds are difficult to adapt to environmental changes: Traditional methods usually use static probability update rules and matching thresholds, which cannot effectively deal with dynamic objects that temporarily appear in the environment (such as moving vehicles, temporary stacks of materials, etc.), resulting in a decrease in matching scores, positioning drift, or even failure.

[0008] Imperfect map update mechanism: During the continuous mapping process, new observation data may directly overwrite old map information. In particular, the existence of semi-static objects (static for a short time but moving in the long term) can easily cause cumulative map errors and structural distortion, thus affecting the long-term consistency of positioning. Summary of the Invention

[0009] Purpose of the invention: The purpose of this invention is to solve the technical problems in the prior art and provide a SLAM probabilistic grid registration method and apparatus.

[0010] Technical solution:

[0011] The first part of this application proposes a SLAM probabilistic raster registration method, including:

[0012] During the localization process, it is checked whether the local matching between the current laser scan data and the prior probability grid map fails. If the matching fails, it is determined whether the map update conditions are met. If they are met, the dynamic map update process is started. If the matching is successful, the pose is updated and the localization is successful.

[0013] Based on the updated dynamic map, multi-stage localization and matching is performed, including:

[0014] First, global matching is performed using a priori map;

[0015] If global matching fails, a dynamic map will be used for global matching.

[0016] If the dynamic map matching fails, relocation will be performed;

[0017] In the dynamic map update process, the newly inserted point cloud data is rasterized to control the number of newly added point clouds, and different weights are assigned to the new and old rasteres, and the raster occupancy probability is updated.

[0018] During the global matching process, the pose optimization method is adopted by combining the weights of the old and new grids, and the matching success is determined based on the matching rate.

[0019] Preferably, in step S1, the map update conditions include at least one of the following:

[0020] The pose movement distance exceeds a preset threshold;

[0021] The pose rotation angle exceeds a preset threshold;

[0022] The time interval since the last update exceeds the preset duration.

[0023] Preferably, controlling the number of newly added point clouds includes:

[0024] If the total number of inserted grid cells exceeds the preset limit, the earliest inserted grid cell and its corresponding point cloud data and probability information will be deleted.

[0025] Preferably, the newly inserted point cloud data is managed in a rasterized manner, including:

[0026] Iterate through the currently acquired point cloud data;

[0027] Based on the currently estimated relative pose, each point cloud data is projected onto the grid map coordinate system to obtain its coordinates in the grid map;

[0028] Calculate the grid index corresponding to the point based on the coordinates, and determine whether the grid with the index already exists in the current map;

[0029] If the grid does not exist, a new grid is created for the point, and the point cloud data is added to the newly created grid.

[0030] If the grid already exists, update the data in the existing grid;

[0031] Specifically, when adding point cloud data to a raster, if the number of point clouds already contained in the raster reaches or exceeds a preset sparsity threshold, the addition of new point cloud data to the raster will be stopped.

[0032] Preferably, the dynamic map update step further includes a map expansion judgment step before performing rasterization management, specifically including:

[0033] Transform the point cloud data in the lidar coordinate system to the map coordinate system according to the current pose.

[0034] Based on the coordinates of all points after transformation, determine their coordinate boundaries in the map coordinate system;

[0035] Determine if at least one point is outside the preset range of the current map. If there are points within the range, then expand the map.

[0036] If points are found to be outside the allowed range, the decision to expand the map depends on the system's operating mode: if expansion is allowed, the map expansion operation is performed; if expansion is restricted, the points outside the allowed range are discarded.

[0037] Preferably, the map expansion operation includes:

[0038] Create a new map configuration with a larger size based on the coordinate boundaries;

[0039] Copy the data from the existing map to the corresponding area configured in the new map;

[0040] Set the new map configuration as the current map.

[0041] Preferably, assigning different weights to old and new rasters includes assigning higher weights to old rasters in the dynamic map and lower weights to new rasters.

[0042] Preferably, during the global matching process, the pose optimization method is adopted by combining the weights of the old and new grids, and the matching success is determined based on the matching rate, including:

[0043] The matching score of the candidate pose is calculated based on the weights of the old and new grids.

[0044] If the matching score exceeds the preset threshold, the match is considered successful;

[0045] Otherwise, generate multiple sub-candidate poses based on the current candidate pose and continue matching and calculation.

[0046] Preferably, in the probabilistic grid registration process, bicubic interpolation is used to smooth the grid probability distribution, wherein the bicubic interpolation is calculated based on the grid probability values ​​in a 4×4 neighborhood.

[0047] Preferred methods for updating grid probabilities include:

[0048] Calculate the distance uncertainty factor based on the measured distance of the laser scanning laser point;

[0049] The hit probability is corrected using the distance uncertainty factor to obtain a probability value that takes uncertainty into account;

[0050] Based on the probability value that takes uncertainty into account, the occupancy probability of the raster is updated using a Bayesian update rule.

[0051] In the second part, this application proposes a SLAM probabilistic grid registration device and a matching update device, which are used to detect whether the local matching between the current laser scanning data and the prior probabilistic grid map fails during the positioning process. When the matching fails, it is determined whether the map update condition is met. If it is met, the dynamic map update process is started. If the matching is successful, the pose is updated and the positioning is successful.

[0052] A multi-stage localization and matching device is used to perform multi-stage localization and matching based on an updated dynamic map, including:

[0053] First, global matching is performed using a priori map;

[0054] If global matching fails, a dynamic map will be used for global matching.

[0055] If the dynamic map matching fails, relocation will be performed;

[0056] In the dynamic map update process, the newly inserted point cloud data is rasterized to control the number of newly added point clouds, and different weights are assigned to the new and old rasteres, and the raster occupancy probability is updated.

[0057] During the global matching process, the pose optimization method is adopted by combining the weights of the old and new grids, and the matching success is determined based on the matching rate. If the matching is successful, the pose is updated and the localization is successful.

[0058] Beneficial effects:

[0059] By introducing a dynamic map update strategy, the system can respond to environmental changes in real time when a local matching failure is detected. This method not only enhances the perception and fusion capabilities for newly added semi-static objects (such as temporarily stacked materials), but also avoids map information redundancy and cumulative drift by deleting outdated grids. This allows the robot to maintain a map representation highly consistent with the real environment even when the environment changes, thus providing a reliable foundation for subsequent registration and directly improving the absolute accuracy of localization. In the matching stage, the classification and weight allocation logic of old and new grids is innovatively integrated into the branch and bound optimization framework. By assigning higher weights to stable "old" grids in the dynamic map and lower weights to newly added "new" grids, the matching process can rely more on long-term stable environmental structures while prudently utilizing the latest observation information. This mechanism effectively suppresses matching interference caused by transient and unstable environmental changes (such as moving people), greatly enhancing the adaptability and robustness of the localization system in dynamic scenarios and reducing the occurrence of localization failures and jumps.

[0060] An improved probabilistic raster update and registration logic, particularly combining Bayesian probabilistic updates with bicubic interpolation techniques to address distance uncertainty, significantly smooths the raster probability field. This effectively reduces jagged noise and discrete outliers in the map (noise count reduced by approximately 73.1%). This not only improves the map's visual quality and interpretability but, more importantly, provides a continuous and smooth objective function for nonlinear optimization, promoting rapid and stable convergence of the optimization algorithm. The dynamic map's capacity management mechanisms (such as setting an upper limit on the number of raster cells and removing the oldest data) and the sparsity control of point cloud addition (such as a single raster cell point threshold) jointly ensure the map's memory efficiency and information quality during long-term operation, avoiding memory bloat and information obsolescence issues caused by unlimited data growth, thus ensuring the map's long-term availability.

[0061] The multi-stage localization process proposed in this invention (global matching → matching after dynamic map update → relocalization) forms an efficient fault recovery mechanism. While ensuring matching accuracy, this process intelligently triggers updates and rematches through conditional judgments (pose change, time interval), avoiding unnecessary computational overhead and achieving a good balance between localization accuracy and computational efficiency. Experimental data shows that the improved matching time is reduced by approximately 15.6%.

[0062] This comprehensive solution closely addresses the challenges faced by AGVs and other robots in real-world applications, providing a complete solution from map building and dynamic updates to robust localization. It is independent of specific sensor configurations (compatible with LiDAR, IMU, and wheel speed sensor fusion) and offers more refined and proactive handling of dynamic objects, thus possessing broad applicability and high engineering practical value. (See attached diagram.)

[0063] Figure 1 A schematic diagram of the method framework for this invention is provided;

[0064] Figure 2 A schematic diagram of the positioning method flowchart is provided for this invention;

[0065] Figure 3 This invention provides a schematic diagram of the map update method flowchart.

[0066] Figure 4 A schematic diagram illustrating the improved matching strategy for this invention;

[0067] Figure 5 A schematic diagram illustrating the improved matching strategy provided by this invention;

[0068] Figure 6 This is a block diagram of a device structure provided in one embodiment of this application;

[0069] Figure 7 This is a block diagram of an electronic device structure provided in one embodiment of this application. Detailed Implementation

[0070] To make the technical solution of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0071] Example 1

[0072] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions in the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without inventive effort are within the scope of protection of this invention. Unless otherwise defined, the technical or scientific terms used herein should have the ordinary meaning understood by those skilled in the art. The terms "comprising" and similar expressions used herein mean that the element or object preceding the word covers the element or object listed following the word and its equivalents, but do not exclude other elements or objects.

[0073] In response to the problems existing in the current technology, such as Figure 1-3 As shown, a SLAM probabilistic raster registration method is proposed, including:

[0074] Step S101: During the positioning process, check whether the local matching between the current laser scanning data and the prior probability grid map fails. If the matching fails, determine whether the map update conditions are met. If they are met, start the dynamic map update process. If the matching is successful, update the pose and the positioning is successful.

[0075] Step S102: Based on the updated dynamic map, perform multi-stage localization and matching, including:

[0076] First, global matching is performed using a priori map;

[0077] If global matching fails, a dynamic map will be used for global matching.

[0078] If the dynamic map matching fails, relocation will be performed;

[0079] In the dynamic map update process, the newly inserted point cloud data is rasterized to control the number of newly added point clouds, and different weights are assigned to the new and old rasteres, and the raster occupancy probability is updated.

[0080] Step S103: During the global matching process, the pose optimization method is adopted by combining the weights of the old and new grids, and the matching success is judged based on the matching rate. If the matching is successful, the pose is updated and the localization is successful. Through dynamic map updates and adjustments to the weights of the old and new grids, the system can maintain high localization accuracy and robustness when facing environmental changes (such as dynamic objects or temporary obstacles). As the environment changes, the system can update the map in real time and prioritize the latest environmental information to avoid localization drift caused by static assumptions. Through multi-stage matching strategies and optimization methods, especially through the weight allocation of the old and new grids and branch-and-bound optimization, accurate localization can be achieved in more complex environments.

[0081] In some specific embodiments, the map update conditions in step S1 include at least one of the following:

[0082] The pose movement distance exceeds a preset threshold;

[0083] The pose rotation angle exceeds a preset threshold;

[0084] The time interval since the last update exceeds a preset duration. When local matching fails, further determination is made as to whether at least one of the following map update conditions is met: For example:

[0085] The robot's pose movement distance exceeds a first preset threshold (e.g., 1 cm).

[0086] The robot's pose rotation angle exceeds the second preset threshold (e.g., 1 degree).

[0087] The time interval since the last map update exceeds the preset duration (e.g., 2 seconds).

[0088] In some specific embodiments, controlling the number of newly added point clouds includes:

[0089] If the total number of inserted rasters exceeds a preset limit, the oldest inserted raster, along with its corresponding point cloud data and probability information, is deleted. As the environment changes, especially in dynamic environments, new point cloud data is constantly added to the map. Without controlling the number of rasters, the amount of raster data and corresponding point cloud information will continuously increase, leading to excessive computational and storage burdens. By setting a preset limit and periodically deleting outdated rasters, it is ensured that the system will not experience memory overflow or decreased processing power due to an excessive number of rasters. By deleting the oldest inserted raster data, the system can always maintain the latest information about the current environment. This approach avoids the impact of outdated data on positioning accuracy, especially in dynamic environments where earlier data may no longer represent current environmental changes. Deleting this no longer valid data helps the system adapt to environmental changes and maintain map timeliness. In SLAM systems, map updates and registration processes require significant computational resources. If the number of rasters is too large, map calculation and matching become more complex and time-consuming.

[0090] By limiting the number of grid cells, the system maintains high computational efficiency during updates and matching, ensuring that real-time positioning performance is not affected. In dynamic environments, new point cloud data typically represents the latest state of environmental changes. By controlling the number of grid cells, the system prioritizes retaining the most up-to-date point cloud information, making positioning and map construction more accurate and able to reflect environmental changes in real time. Deleting the oldest inserted grid cell avoids old, inaccurate information from affecting positioning accuracy, thereby improving the robustness of the SLAM system. For example, when the number of inserted grid cells exceeds 10,000, the system will delete the oldest inserted grid cell, remove outdated data, and keep the number of grid cells within a reasonable range. When deleting grid data, the system looks up the corresponding point cloud data using the grid cell's index, removes the relevant information from the pointsMap and probMap (grid occupancy probability), and removes the corresponding grid cell data from the dynamic map stack.

[0091] In some specific embodiments, the newly inserted point cloud data is rasterized, including:

[0092] Traversing the currently acquired point cloud data; traversing all newly acquired point cloud data is to process each point cloud individually and map it onto the raster map. This is a fundamental step in managing point cloud data, ensuring that each new point is correctly incorporated into the current map.

[0093] Based on the currently estimated relative pose, each point cloud data is projected onto the grid map coordinate system to obtain its coordinates in the grid map;

[0094] Calculate the grid index corresponding to the point based on the coordinates, and determine whether the grid with the index already exists in the current map; based on the currently estimated relative pose (the robot's relative position and attitude relative to the environment or previous pose), transform each point cloud data from its original coordinate system to the grid map coordinate system.

[0095] The formula for converting physical coordinates to raster indexes is as follows:

[0096] ;

[0097] ;

[0098] Actual grid position:

[0099] ;

[0100]

[0101] The physical coordinates (x, y) are converted to raster coordinates (dx, dy), and then the raster coordinates are calculated based on the map resolution. Here, columns and rows are the number of columns and rows of the map, respectively, and dx and dy are the offsets of the physical coordinates (x, y) relative to the raster resolution.

[0102] Because the SLAM system needs to compare all acquired data with previously created locations... Figure 1 To ensure map accuracy and consistency, the raster index for each point is calculated to map point cloud data to specific raster locations. The system checks if the raster already exists to avoid creating duplicates. If the raster doesn't exist, it needs to be created; if it already exists, the data can be updated directly within that raster.

[0103] The one-dimensional raster index is calculated using the transformed (cellX, cellY) coordinates. The calculation method is as follows:

[0104] ;

[0105] One-dimensional raster indexes can conveniently store and retrieve raster data within a data structure. In a two-dimensional map, the two-dimensional coordinates (cellX, cellY) of a raster are converted into a one-dimensional index, which facilitates quick searching and updating of the raster's status (such as occupancy probability, point cloud data, etc.).

[0106] If the grid does not exist, a new grid is created for the point, and the point cloud data is added to the newly created grid.

[0107] The presence of data in a raster is determined by checking whether the hash table (or other data structure) contains the calculated raster index.

[0108] If the grid already exists, update the data in the existing grid;

[0109] If the grid does not exist, a new grid is created, and the current point cloud data is added to that grid; if the grid already exists, the point cloud data is updated in the existing grid. This step ensures that the point cloud information in the map is always up-to-date and that the grid map can be continuously updated based on real-time data.

[0110] Specifically, when adding point cloud data to a raster, if the number of point clouds already contained in the raster reaches or exceeds a preset sparsity threshold (e.g., 3 points), then adding new point cloud data to the raster will stop.

[0111] When the number of point cloud data contained in a raster reaches or exceeds a preset sparsity threshold, adding new point cloud data to that raster stops. This serves several purposes:

[0112] An excessive number of point clouds in a raster can lead to overly dense information within that raster, preventing it from effectively reflecting changes in the local environment and resulting in decreased accuracy. Setting a sparsity threshold can prevent this. Too much data in each raster can increase the computational load during subsequent localization and matching, especially in the matching stage, where overly dense data may negatively impact optimization efficiency. Controlling the amount of data in each raster can improve the overall computational efficiency of the system. Appropriate sparsity helps reduce the influence of noisy points, making the map clearer. Excessive point cloud data can introduce errors, especially when the accuracy of new point cloud data is low; sparsity helps maintain map smoothness.

[0113] In some specific embodiments, the dynamic map update step, before performing rasterization management, further includes a map expansion judgment step, specifically including:

[0114] The point cloud data in the LiDAR coordinate system is transformed to the map coordinate system based on the current pose. Since LiDAR data is acquired based on its own coordinate system, while the map is constructed based on a global coordinate system, the point cloud data in the LiDAR coordinate system needs to be transformed to the map coordinate system using the currently estimated pose. This is to ensure that the point cloud data can be correctly integrated into the existing map.

[0115] Based on the coordinates of all points after transformation, determine their coordinate boundaries in the map coordinate system; based on the transformed point cloud coordinates, calculate their coordinate boundaries in the map. This step ensures that the system knows the spatial distribution of the current point cloud data, helping to determine whether it has exceeded the current map's boundaries. In the transformed raster coordinates (cellX, cellY), check whether they are within the valid range of the current map. The valid range is determined by the number of rows and columns of the map, i.e.:

[0116] ;

[0117] ;

[0118] Ensure that the converted raster coordinates are within the valid area of ​​the map, avoiding them from exceeding the map's boundaries. If the raster coordinates exceed the boundaries, map expansion or adjustment is necessary to ensure that the new data can be correctly mapped.

[0119] The system determines whether at least one point exceeds the preset range of the current map. If points exist within the preset range, the map is expanded. The system uses this determination to decide whether map expansion is necessary. If some points in the point cloud data extend beyond the map boundaries, the current map needs to be expanded to accommodate this new environmental information.

[0120] If points are found to be outside the allowed range, the decision to expand the map depends on the system's operating mode: if expansion is allowed, the map expansion operation is performed; if expansion is restricted, the points outside the allowed range are discarded.

[0121] If there are no out-of-range points: If the point cloud data does not exceed the current map's range, the system does not need to expand it and can directly perform rasterization management.

[0122] If points exceed the current map's range: If point cloud data exceeds the current map's range, the system will decide whether to perform an expansion based on the current operating mode.

[0123] Allow Expansion Mode: In this mode, the system expands the map to include all new out-of-range points. The map expansion process typically involves adding new raster regions, adding point cloud data that has exceeded the original range to these new areas. This allows the map to adapt to environmental changes, ensuring that positioning is not hindered by map boundary limitations.

[0124] Limited Expansion Mode: In this mode, the system discards point cloud data that exceeds the allowed area and does not expand the map. This mode maintains a relatively stable map extent by limiting map expansion, avoiding excessive map changes. It is suitable for application scenarios that require keeping the map size within a fixed range.

[0125] For example: if a point is not within the range and m_isExpandGrid is true, then expand the map; if a point is not within the range and m_isExpandGrid is false, then discard the point.

[0126] In some specific embodiments, the map expansion operation includes:

[0127] A new map configuration with a larger size is created based on the coordinate boundaries; a new map configuration with a larger size is also created based on the coordinate boundaries of point cloud data that exceeds the current map's preset range. This is to ensure that the new map can contain all new point cloud data, while avoiding excessively frequent map expansion needs due to future environmental changes. By reserving sufficient space, the system can handle potential changes in the future environment, avoiding frequent map expansion operations and ensuring the map's adaptability and continuity.

[0128] Data from the existing map is copied to the corresponding area of ​​the new map configuration; existing map data (i.e., raster data from the current map) is copied to its corresponding area in the newly created, larger map configuration. This ensures that information from the old map is preserved in the new map, thus avoiding the loss of previous environmental data. This step ensures map continuity and consistency. By retaining data from the original map and migrating it to the expanded area, the loss of environmental data is avoided. Regardless of the size of the newly expanded map, the original environmental data remains intact, guaranteeing positioning accuracy and map integrity.

[0129] The newly created map is then set as the current map. After the map expansion operation is complete, the newly created map is set as the current map. This means that in subsequent localization and mapping processes, the new, larger map will be used as the basis for processing and use. By setting the newly expanded map as the current map, the system can immediately begin using the new map for localization and updates. This ensures a seamless transition in the expansion process, does not affect the current SLAM process, and ensures that subsequent point cloud data can be correctly mapped to the new map.

[0130] In some specific embodiments, during the global matching process, the pose optimization is performed using a branch and bound method (see "Branch and Cut: An Empirical Study"), taking into account the weights of the old and new grids, and the matching success is determined based on the matching rate, including:

[0131] Based on the weights of the old and new grids, the matching score of the candidate pose is calculated, and the old grids in the dynamic map are assigned higher weights and the new grids are assigned lower weights; based on the weights of the old and new grids, the matching score of the current candidate pose is calculated.

[0132] Traditional branch-and-bound methods typically find the optimal solution by exploring the solution space and progressively eliminating impossible solutions. However, traditional methods face challenges in handling dynamic environments, especially with map updates and dynamic changes, struggling to effectively cope with environmental shifts. To address this issue, this paper combines dynamically updated map techniques with matching rate logic and a weighting strategy for new and old rasters to improve matching accuracy.

[0133] Specifically, older rasters in the dynamic map are assigned higher weights, while newer rasters are assigned lower weights.

[0134] Older rasters have higher weights: because older rasters represent previously stable environmental information, higher weights ensure that the system relies more on this known and reliable data when matching.

[0135] New grids have lower weights: New grids represent the most recently updated environmental data, which may contain noisy or unstable information. Therefore, they are given lower weights to prevent unreliable new data from having too much influence on the matching results.

[0136] By assigning different weights to old and new grids, the system can more robustly handle dynamically changing environmental information, avoiding a decrease in positioning accuracy due to sudden dynamic objects or changes. Weight allocation ensures the system's robustness by prioritizing more reliable environmental data.

[0137] If the matching score exceeds a preset threshold, the match is considered successful. Based on the calculated matching score, it is determined whether the current candidate pose has matched successfully. The score is obtained by evaluating the matching between the candidate pose and the grid in the map. A higher matching score means that the pose has a higher degree of fit with the map data and better positioning accuracy.

[0138] This threshold is set according to the actual needs of the system. If the matching score is greater than this threshold, the pose is considered to be accurate enough and can be used as the current valid pose.

[0139] By comparing the matching score with a threshold, the success of the current match can be effectively determined. A successful match means that the system can determine the robot's current pose and update the map; if the match fails, the system continues to search for a suitable pose.

[0140] Otherwise, multiple sub-candidate poses are generated based on the current candidate pose, and matching calculations continue. If the current matching score fails to exceed a preset threshold, multiple sub-candidate poses are generated based on the current candidate pose, and matching calculations continue. By making small incremental changes (such as small changes in position or orientation) based on the current candidate pose, a new set of sub-candidate poses is generated. This allows for the exploration of more possible matching results in the pose space, finding a more suitable pose. Matching calculations are performed on the sub-candidate poses, and the matching scores are used to further evaluate which pose is closer to the true pose.

[0141] The purpose of generating sub-candidate poses is to increase the flexibility of pose matching, especially in complex or dynamic environments where robots may exhibit slight localization errors. By incrementally adjusting the pose, the search range can be refined to find more accurate matching results, thereby avoiding matching failures caused by local optima. During the search process, candidate poses are continuously generated and filtered based on matching scores, gradually eliminating poses that do not meet the conditions until a valid pose with a sufficiently high matching score is found.

[0142] In some specific embodiments, bicubic interpolation is used to smooth the raster probability distribution during probabilistic raster registration. This bicubic interpolation is calculated based on the raster probability values ​​within a 4×4 neighborhood. The bicubic interpolation method calculates the interpolation result based on the raster probability values ​​within a 4×4 neighborhood. By using 16 surrounding points (i.e., raster data within the 4×4 neighborhood), bicubic interpolation calculates a smooth intermediate value. Compared to traditional linear interpolation methods, bicubic interpolation ensures the continuity of function values ​​and first derivatives at the interpolation points, thereby reducing the "step-like" effect and avoiding overly coarse transitions in the rasterized data. Bicubic interpolation effectively smooths the raster probability distribution, avoiding the "jagged" effect caused by rasterization, resulting in a smoother map surface. Especially for structures such as walls that should exhibit straight-line characteristics, the smoothed probability distribution after interpolation better represents the continuity and accuracy of the environment. This helps subsequent optimization algorithms (such as the Ceres solver) converge quickly, thereby improving the overall accuracy of localization and mapping.

[0143] In some specific embodiments, the grid probability update method includes:

[0144] The distance uncertainty factor is calculated based on the measured distance of the laser scanning laser point. Laser ranging typically involves a certain degree of error, especially at longer distances where the error usually increases. The distance uncertainty factor reflects the impact of ranging uncertainty on the grid occupancy probability. The uncertainty factor is usually a function based on the measured distance, and can be calculated, for example, using the square root of the distance or a linear relationship. By introducing the uncertainty factor, the system can more accurately reflect the actual error of the laser measurement data. This correction avoids unreliable occupancy probabilities caused by inaccurate measurement data, particularly for data from long distances or in complex environments, thus improving the accuracy and robustness of the SLAM system.

[0145] The hit probability is corrected using the distance uncertainty factor to obtain a probability value that accounts for uncertainty. The hit probability is calculated based on whether a laser point hits a grid cell, typically with an initial value, such as 50% (meaning the grid cell may be either empty or occupied). However, due to measurement errors, the corrected probability value better reflects the actual situation. By combining the uncertainty factor with the initial hit probability, a corrected probability value is obtained, taking into account measurement uncertainty. For example, a larger uncertainty factor might cause the corrected probability value to be biased towards a lower value, indicating a lower probability of grid cell occupancy. The corrected probability value updates the grid cell occupancy probability using a Bayesian update rule. The probability value corrected to account for uncertainty more accurately reflects the actual occupancy of the grid cells. In dynamic environments, where measurement uncertainty is high, the corrected probability value better reflects dynamic environmental changes. This method improves map accuracy and avoids positioning errors caused by uncertainty.

[0146] Based on the probability values ​​considering uncertainty, the occupancy probability of the raster is updated using a Bayesian update rule. Bayesian update is a common method for updating probabilities based on prior knowledge and new observation data. In SLAM systems, the occupancy probability of a raster can be recursively updated using Bayesian rules:

[0147] Bayesian updates combine new observation data with probabilistic information from prior maps to progressively optimize the occupancy probability of grid cells. This method enables the system to continuously correct the map based on new sensor data, providing an efficient and dynamically updated process that adapts to environmental changes and improves the system's robustness and positioning accuracy.

[0148] Exemplary: Combination Figure 4 and Figure 5 ;

[0149] 1. Raster conversion process

[0150] Raster conversion before improvement:

[0151] For the point (2.341, 0.123), transform it to a raster coordinate system:

[0152] ;

[0153] ;

[0154] Calculate cellX=174 and cellY=130, then calculate using a one-dimensional index:

[0155] ;

[0156] The probability update uses the basic hit probability update formula:

[0157] ;

[0158] ;

[0159] Improved raster conversion:

[0160] For the same physical point (2.341, 0.123), the raster coordinate transformation remains consistent:

[0161] ;

[0162] Calculate cellX=174 and cellY=130, then calculate the one-dimensional index:

[0163] ;

[0164] Considering distance uncertainty:

[0165] Calculate the distance from the point to the origin:

[0166] ;

[0167] Calculate the uncertainty factor:

[0168] ;

[0169] The improved hit probability uses a higher value (hit_prob=0.75), and is then adjusted based on uncertainty:

[0170] ;

[0171] Using the Bayesian update formula:

[0172] ;

[0173]

[0174] 2. Aggregation processing of multiple points

[0175] For multiple consecutive points in the corner region (e.g., (174, 130) to (174, 134)), the probability changes before and after the improvement are as follows:

[0176] Before the improvement: the probability value remained at a low level, approximately 0.545 to 0.548.

[0177] After improvement: Through uncertainty and updated hit probabilities, the probability values ​​are significantly improved. For example, the probability of (174,130) increases from 0.545 to 0.716, and the probability of (174,134) also increases from 0.548 to 0.719.

[0178] 3. Matching optimization process

[0179] Matching optimization before improvement:

[0180] Using simple ICP residual calculation:

[0181] ;

[0182] This method relies solely on the distance to the nearest point when calculating the matching error, which may not be sensitive enough to local features of the environment.

[0183] Improved matching optimization:

[0184] Using probability field residuals, the grid probability values ​​are first interpolated using bicubic interpolation:

[0185] ;

[0186] Then, considering the uncertainty of distance, adjust the probability value:

[0187] ;

[0188] Finally, calculate the matching residuals:

[0189] ;

[0190] 4. The data quantification comparison is shown in the table below:

[0191] .

[0192] This application proposes a SLAM probabilistic grid registration device, combined with Figure 6The matching update device 201 is used to detect whether the local matching between the current laser scanning data and the prior probability grid map fails during the positioning process. When the matching fails, it determines whether the map update conditions are met. If they are met, the dynamic map update process is started. If the matching is successful, the pose is updated and the positioning is successful.

[0193] Multi-stage localization matching device 202, used to perform multi-stage localization matching based on an updated dynamic map, including:

[0194] First, global matching is performed using a priori map;

[0195] If global matching fails, a dynamic map will be used for global matching.

[0196] If the dynamic map matching fails, relocation will be performed;

[0197] In the dynamic map update process, the newly inserted point cloud data is rasterized to control the number of newly added point clouds, and different weights are assigned to the new and old rasteres, and the raster occupancy probability is updated.

[0198] During the global matching process, the pose optimization method is adopted by combining the weights of the old and new grids, and the matching success is determined based on the matching rate. If the matching is successful, the pose is updated and the localization is successful.

[0199] In other embodiments of the present invention, an electronic device 400 is disclosed, such as... Figure 7 As shown, the electronic device may include: one or more processors 401; a memory 402; a display 403; one or more application programs (not shown); and one or more computer programs 404. These devices can be connected via one or more communication buses 405. The one or more computer programs 404 are stored in the memory 402 and configured to be executed by the one or more processors 401. The one or more computer programs 404 include instructions that can be used to perform actions such as... Figure 1 And the steps in the corresponding embodiments.

[0200] Through the above description of the embodiments, those skilled in the art will clearly understand that, for the sake of convenience and brevity, only the division of the above functional modules is used as an example. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. The specific working process of the system, device, and unit described above can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0201] In the various embodiments of this invention, the functional units can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0202] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiments of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as flash memory, portable hard disk, read-only memory, random access memory, magnetic disk, or optical disk.

[0203] The above description is merely a specific implementation of the embodiments of the present invention, but the protection scope of the embodiments of the present invention is not limited thereto. Any changes or substitutions within the technical scope disclosed in the embodiments of the present invention should be covered within the protection scope of the embodiments of the present invention. Therefore, the protection scope of the embodiments of the present invention should be determined by the protection scope of the claims.

Claims

1. A SLAM probability grid registration method, characterized by, Comprise: In the positioning process, it is detected whether the local matching of the current laser scanning data and the prior probability grid map fails, and when the matching fails, it is judged whether the map updating condition is met; If met, the dynamic map updating process is started, and if the matching is successful, the pose is updated and the positioning is successful; Based on the updated dynamic map, multi-stage positioning matching is performed, including: First, use the prior map for global matching; If the global matching fails, use the dynamic map for global matching; If the dynamic map matching fails, repositioning is performed; In the dynamic map updating process, the newly inserted point cloud data is rasterized and managed, the number of newly added point clouds is controlled, different weights are assigned to new and old grids, and the grid occupancy probability is updated; In the global matching process, the weights of new and old grids are combined, the branch and bound method is used for pose optimization, and whether the matching is successful is judged based on the matching rate, and if the matching is successful, the pose is updated and the positioning is successful.

2. The SLAM probabilistic grid registration method of claim 1, wherein, The map updating condition in step S1 includes at least one of the following: The pose moving distance exceeds the preset threshold; The pose rotation angle exceeds the preset threshold; The time interval from the last update exceeds the preset time length.

3. The SLAM probabilistic grid registration method of claim 1, wherein, Controlling the number of newly added point clouds includes: If the total number of inserted grids exceeds the preset upper limit, delete the earliest inserted grid and its corresponding point cloud data and probability information.

4. The SLAM probabilistic grid registration method of claim 1, wherein, The rasterization management of the newly inserted point cloud data includes: Traverse the currently obtained point cloud data; According to the currently estimated relative pose, project each point cloud data into the grid map coordinate system to obtain its coordinates in the grid map; Calculate the grid index corresponding to the point according to the coordinates, and judge whether the grid with the index already exists in the current map; If the grid does not exist, create a new grid for the point and add the point cloud data to the newly created grid; If the grid already exists, update the data in the existing grid; Wherein, when adding point cloud data to the grid, if the number of point clouds already contained in the grid reaches or exceeds the preset sparsity threshold, stop adding new point cloud data to the grid.

5. The SLAM probabilistic grid registration method of claim 1, wherein, In the dynamic map updating step, before performing the rasterization management, it further includes a map expansion judgment step, specifically including: Transform the point cloud data in the laser radar coordinate system to the map coordinate system according to the current pose; Determine the coordinate boundary of all points in the map coordinate system according to the coordinates of the transformed points; Judge whether at least one point exceeds the preset range of the current map, if there is a point that does not exceed the range, perform the expansion map; If there is an out-of-range point, decide whether to expand the map according to the system running mode: in the expansion allowed mode, perform the map expansion operation; in the expansion limited mode, discard the out-of-range point.

6. The SLAM probabilistic grid registration method of claim 5, wherein, The map expansion operation includes: Create a new map configuration with larger size according to the coordinate boundary; Copy the data in the existing map to the corresponding area of the new map configuration; Set the new map configuration as the current map.

7. The SLAM probabilistic grid registration method of claim 1, wherein, In the global matching process, the weights of new and old grids are combined, the branch and bound method is used for pose optimization, and whether the matching is successful is judged based on the matching rate, including: According to the weights of the old and new grids, a matching score of the candidate pose is calculated, wherein a higher weight is assigned to the old grid in the dynamic map and a lower weight is assigned to the new grid; If the matching score exceeds a preset threshold, it is considered that the matching is successful; Otherwise, a plurality of sub candidate poses are generated based on the current candidate pose, and the matching calculation is continued.

8. The SLAM probabilistic grid registration method of claim 1, wherein, In the process of probability grid registration, a bicubic interpolation method is used to smooth the grid probability distribution, and the bicubic interpolation is calculated based on the grid probability values in a 4*4 neighborhood.

9. The SLAM probabilistic grid registration method of claim 8, wherein, The updating method of the grid probability comprises: According to the measured distance of the laser scanning laser point, a distance uncertainty factor is calculated; Using the distance uncertainty factor, the hit probability is corrected to obtain a probability value considering uncertainty; Based on the probability value considering uncertainty, the occupancy probability of the grid is updated using the Bayes updating rule.

10. A SLAM probability grid registration apparatus, characterized by, It comprises: A matching updating device is used to detect whether the local matching of the current laser scanning data and the prior probability grid map fails in the positioning process, and when the matching fails, it is judged whether the map updating condition is met; if met, the dynamic map updating process is started, and if the matching is successful, the pose is updated and the positioning is successful; A multi-stage positioning matching device is used to perform multi-stage positioning matching based on the updated dynamic map, comprising: Firstly, global matching is performed using the prior map; If the global matching fails, the dynamic map is used for global matching; If the dynamic map matching fails, repositioning is performed; In the dynamic map updating process, the newly inserted point cloud data is rasterized and managed, the number of newly added point clouds is controlled, different weights are assigned to the new and old grids, and the grid occupancy probability is updated; In the global matching process, the weights of the new and old grids are combined, the branch and bound method is used for pose optimization, and it is judged whether the matching is successful based on the matching rate; if the matching is successful, the pose is updated and the positioning is successful.

Images