Local map matching method and device, electronic equipment and storage medium

Abstract

The invention provides a local map matching method and apparatus, an electronic device and a storage medium. The method comprises the steps of determining a voxel grid to which each map point in a local map belongs; local structure feature fitting is carried out according to map points in the voxel grids, structure parameters corresponding to the voxel grids are obtained, and the structure parameters are stored in a preset storage area; when the laser point cloud currently collected by the laser radar is matched with the local map, the structure parameters are obtained, and the laser point cloud is matched with the local map based on the structure parameters. According to the method, the structure parameters stored in the preset storage area can be directly used when the laser point cloud is matched with the local map, the situation that feature fitting calculation is carried out again in each time of matching is avoided, the calculation amount related to feature fitting is reduced, and the matching efficiency is improved.

Description

Technical Field

[0001] This application relates to the field of simultaneous localization and mapping (SLAM), and more specifically, to a method, apparatus, electronic device, and storage medium for matching local maps within the field of SLAM. Background Technology

[0002] Currently, Simultaneous Localization and Mapping (SLAM) algorithms based on LiDAR all maintain a local point cloud map near the current pose and then use a matching algorithm to match the current LiDAR point cloud with the local point cloud map to solve for the pose. Therefore, maintaining the local point cloud map is crucial for subsequent matching between the point cloud map and the LiDAR point cloud.

[0003] Most existing SLAM algorithms use static kdtrees or incremental ikdtrees to maintain point cloud maps, but the efficiency of using static kdtrees or incremental ikdtrees for point cloud map maintenance is relatively low. Summary of the Invention

[0004] This application provides a method, apparatus, electronic device, and storage medium for matching local maps. The method can reduce the computational load related to feature fitting and improve matching efficiency.

[0005] Firstly, a local map matching method is provided, which includes: determining the voxel grid to which each map point in the local map belongs; fitting local structural features based on the map points inside the voxel grid to obtain the structural parameters corresponding to the voxel grid, and storing the structural parameters in a preset storage area; when matching the laser point cloud currently acquired by the lidar with the local map, obtaining the structural parameters, and matching the laser point cloud and the local map based on the structural parameters.

[0006] In the above technical solution, the voxel grid to which each map point belongs in the local map is determined. By voxelizing the local map, the voxel grid to which each map point belongs can be quickly determined, facilitating subsequent processing of the point cloud data. Local structural features are pre-fitted based on the map points within the voxel grid, and the corresponding structural parameters of the voxel grid are saved. These stored structural parameters can be directly used when matching the laser point cloud and the local map, avoiding the need to recalculate feature fitting for each match, thus reducing computational load and improving matching efficiency. Furthermore, the structural parameters corresponding to the voxel grid are stored in a preset storage area. Subsequent maintenance of the structural parameters corresponding to the voxel grid is done directly, without needing to maintain the point cloud data. Since structural parameters typically occupy less storage space than point cloud data, directly maintaining the local structural parameters of the point cloud further reduces memory usage.

[0007] In conjunction with the first aspect, in some implementations of the first aspect, the structural parameters corresponding to the voxel grid are obtained by fitting local structural features of map points inside the voxel grid, including: determining whether the number of map points inside the voxel grid is greater than or equal to a preset number; if the number of map points is greater than or equal to the preset number, fitting local structural features of map points inside the voxel grid to obtain the structural parameters corresponding to the voxel grid; if the number of map points is less than the preset number, searching for map points in neighboring voxel grids adjacent to the voxel grid with the voxel grid as the center, and fitting local structural features of map points inside the voxel grid and map points in neighboring voxel grids to obtain the structural parameters corresponding to the voxel grid.

[0008] In the above technical solution, when the number of points within the voxel grid reaches a preset number, it indicates that there is sufficient information in the region to accurately describe the local structural features. Fitting at this point yields relatively accurate structural parameters, which is crucial for subsequent scene understanding and localization. If the number of points within the voxel grid is small, fitting based solely on these points may lead to deviations in the structural parameters. By introducing points from adjacent voxel grids, the number of points participating in the fitting is increased, thereby improving the reliability of the fitting results. This solution improves the accuracy of the fitting results and enhances the robustness of the algorithm.

[0009] Combining the first aspect and the above implementation methods, in some implementation methods of the first aspect, after performing local structural feature fitting on the map points inside the voxel grid and the map points in the nearest voxel grid to obtain the structural parameters corresponding to the voxel grid, the method further includes: if there are newly added map point clouds in the local map, and if the number of map points inside the voxel grid increases and the number of map points after the increase is greater than or equal to a preset number, then perform local structural feature fitting on the map points inside the voxel grid after the increase to obtain the updated structural parameters corresponding to the voxel grid; and update the structural parameters stored in the preset storage area to the updated structural parameters.

[0010] In the above technical solution, when new map point clouds appear in the local map, the structural parameters can be updated based on the new point cloud data, ensuring that the local map always reflects the latest environmental conditions. As the number of map points increases, the number of map points within the voxel grid also increases, providing more map points for fitting local structural features. By fitting local structural features to the added map points, the fitting accuracy can be improved, and the accuracy of the structural parameters can be enhanced.

[0011] In combination with the first aspect and the above implementation methods, in some implementation methods of the first aspect, the method further includes: when there is a newly added map point cloud in the local map, determining the voxel grid to which each map point to be added in the map point cloud belongs, and adding each map point to be added to the voxel grid to which it belongs.

[0012] Combining the first aspect and the above implementation methods, in some implementation methods of the first aspect, adding each map point to be added to its corresponding voxel grid includes: determining whether the sum of the number of map points to be added and the number of map points in the voxel grid to which the map point to be added belongs is less than or equal to a preset number; if the sum of the numbers is less than or equal to the preset number, then the map point to be added is added to its corresponding voxel grid.

[0013] In the above technical solution, by setting a preset number, the addition operation is only performed if adding map points does not cause the number of points in the voxel grid to exceed the preset number. This avoids complex structural feature fitting calculations when there are too many points, saving computational resources. Furthermore, limiting the number of map points in each voxel grid also helps reduce memory usage and improve storage efficiency.

[0014] In combination with the first aspect and the above implementation methods, in some implementation methods of the first aspect, after determining the voxel grid to which each map point to be added belongs in the map point cloud, the method further includes: moving the voxel grid to which the map point to be added belongs to the head of the voxel linked list; wherein, the voxel linked list is used to store the voxel grids in the local map according to the observation order of each voxel grid; if the data capacity in the voxel linked list is greater than the preset capacity, then the voxel grid at the tail of the voxel linked list is deleted.

[0015] In the above technical solution, moving the voxel grid to the head of the voxel list to which the map point to be added belongs ensures that newly observed voxel grids are always at the front of the voxel list, while voxel grids that have not been observed for a long time are at the back. If the data capacity in the voxel list exceeds a preset capacity, deleting voxel grids at the tail of the list ensures that the data in the local map is up-to-date, facilitating effective storage space management and improving storage efficiency.

[0016] Combining the first aspect and the above implementation methods, in some implementation methods of the first aspect, matching the laser point cloud and the local map based on structural parameters includes: determining the target observation probability of each voxel grid to which each map point in the local map belongs; wherein the target observation probability is used to characterize the probability that the voxel grid is observed; and matching the laser point cloud and the local map based on structural parameters and target observation probabilities.

[0017] Combining the first aspect and the above implementation methods, in some implementation methods of the first aspect, determining the target observation probability of each voxel grid to which each map point belongs in the local map includes: when there is a newly added map point cloud in the local map, determining the voxel grid to which each map point to be added belongs in the map point cloud, and determining whether the voxel grid to which each map point to be added belongs belongs to the local map; when it is determined that the voxel grid to which the map point to be added belongs belongs to the local map, adding the map point to be added to its voxel grid, and increasing the initial observation probability corresponding to the voxel grid based on a preset growth gradient to obtain the target observation probability corresponding to the voxel grid; for other voxel grids in the local map other than the voxel grid to which the map point to be added belongs, decreasing the initial observation probability of other voxel grids based on a preset decay gradient to obtain the target observation probability corresponding to other voxel grids; when it is determined that the voxel grid to which the map point to be added belongs does not belong to the local map, creating a new voxel grid in the local map, and determining the target observation probability corresponding to the new voxel grid as the initial observation probability.

[0018] In the above technical solution, newly added map point clouds can be detected in a timely manner, and the observation probabilities of voxel grids in the local map can be updated accordingly, ensuring that the local map can reflect changes in the environment in a timely manner. For voxel grids containing new map points, the initial observation probability is increased based on a preset growth gradient, which helps to increase the importance of these voxel grids and ensure that their weights in the local map are more accurate. For voxel grids without new map points, the initial observation probability is decreased based on a preset decay gradient, which helps to reduce the weight of these voxel grids and make them more consistent with the actual situation. Adding a probability dimension, namely the target observation probability, to the voxel grids and automatically updating the target observation probability of the voxel grids can provide voxel grid probability information to the matching algorithm, thereby improving the accuracy of the matching algorithm.

[0019] In combination with the first aspect and the above implementation methods, in some implementation methods of the first aspect, after determining the voxel grid to which each map point in the local map belongs, the method further includes: determining the first voxel coordinates of the voxel grid to which each map point belongs; mapping the first voxel coordinates to a first hash key value based on a hash function; wherein the first hash key value is used as an index to access a preset storage area and to look up the structural parameters corresponding to the voxel grid in the preset storage area.

[0020] In combination with the first aspect and the above implementation methods, in some implementation methods of the first aspect, when there are multiple map points in the voxel grid, determining the voxel grid to which each map point in the local map belongs includes: dividing the voxel grid into multiple voxel sub-grids, and determining the voxel sub-grid to which each map point in the multiple map points belongs; wherein, the resolution of the divided voxel sub-grid is higher than the resolution of the voxel grid before division; and one map point is retained in each voxel sub-grid.

[0021] In the above technical solution, when multiple map points are stored in a voxel grid, dividing the voxel grid into multiple voxel subgrids can make the points in the voxel grid evenly distributed, and each voxel subgrid retains only one point. This avoids the need to perform floating-point calculations of the distance between points when storing multiple points, and only requires an insertion operation, thus improving the efficiency of point cloud insertion.

[0022] In combination with the first aspect and the above implementation methods, in some implementation methods of the first aspect, after determining the voxel subgrid to which each map point belongs among multiple map points, the method further includes: determining the second voxel coordinates of the voxel subgrid to which each map point belongs; mapping the second voxel coordinates to a second hash key value based on a hash function; wherein the second hash key value is used as an index to access a preset storage area and to look up the structural parameters corresponding to the voxel subgrid within the preset storage area.

[0023] Secondly, a local map matching device is provided, comprising: a determining module for determining the voxel grid to which each map point in the local map belongs; a fitting module for fitting local structural features based on the map points within the voxel grid to obtain structural parameters corresponding to the voxel grid, and storing the structural parameters in a preset storage area; and a matching module for obtaining the structural parameters when matching the laser point cloud currently acquired by the lidar with the local map, and matching the laser point cloud and the local map based on the structural parameters.

[0024] In conjunction with the second aspect, in some implementations of the second aspect, the fitting module is specifically used to determine whether the number of map points inside the voxel grid is greater than or equal to a preset number; if the number of map points is determined to be greater than or equal to the preset number, local structural feature fitting is performed on the map points inside the voxel grid to obtain the structural parameters corresponding to the voxel grid; if the number of map points is determined to be less than the preset number, map points in the nearest neighbor voxel grids adjacent to the voxel grid are searched with the voxel grid as the center, and local structural feature fitting is performed on the map points inside the voxel grid and the map points in the nearest neighbor voxel grids to obtain the structural parameters corresponding to the voxel grid.

[0025] In conjunction with the second aspect and the above implementation methods, in some implementation methods of the second aspect, the device further includes: an update module, used to perform local structural feature fitting on the map points inside the voxel grid when there are newly added map point clouds in the local map, if the number of map points inside the voxel grid increases and the number of map points after the increase is greater than or equal to a preset number, to obtain the updated structural parameters corresponding to the voxel grid; and update the structural parameters stored in the preset storage area to the updated structural parameters.

[0026] In conjunction with the second aspect and the above implementation methods, in some implementation methods of the second aspect, the device further includes an adding module, which is used to determine the voxel grid to which each map point to be added in the map point cloud belongs when there is a newly added map point cloud in the local map, and add each map point to be added to the voxel grid to which it belongs.

[0027] Combining the second aspect and the above implementation methods, in some implementation methods of the second aspect, the addition module is specifically used to determine whether the sum of the number of map points to be added and the number of map points in the voxel grid to which the map points to be added belong is less than or equal to a preset number; if the sum of the numbers is less than or equal to the preset number, then the map points to be added are added to the voxel grid to which they belong.

[0028] In conjunction with the second aspect and the above implementation methods, in some implementation methods of the second aspect, the adding module is further used to move the voxel grid to which the map point to be added belongs to the head of the voxel linked list; wherein, the voxel linked list is used to store the voxel grids in the local map according to the observation order of each voxel grid; if the data capacity in the voxel linked list is greater than the preset capacity, the voxel grid at the tail of the voxel linked list is deleted.

[0029] Combining the second aspect and the above implementation methods, in some implementation methods of the second aspect, the matching module is specifically used to determine the target observation probability of each voxel grid to which each map point in the local map belongs; wherein, the target observation probability is used to characterize the probability that the voxel grid is observed; and the laser point cloud and the local map are matched based on the structural parameters and the target observation probability.

[0030] In conjunction with the second aspect and the above implementation methods, in some implementation methods of the second aspect, the matching module is further configured to: determine the voxel grid to which each map point to be added belongs in the map point cloud when there is a newly added map point cloud in the local map, and determine whether the voxel grid to which each map point to be added belongs belongs to the local map; if it is determined that the voxel grid to which the map point to be added belongs belongs to the local map, add the map point to be added to the voxel grid to which it belongs, and increment the initial observation probability corresponding to the voxel grid based on a preset growth gradient to obtain the target observation probability corresponding to the voxel grid; for other voxel grids in the local map other than the voxel grid to which the map point to be added belongs, decrement the initial observation probability of other voxel grids based on a preset decay gradient to obtain the target observation probability corresponding to other voxel grids; if it is determined that the voxel grid to which the map point to be added belongs does not belong to the local map, create a new voxel grid in the local map, and determine the target observation probability corresponding to the new voxel grid as the initial observation probability.

[0031] In conjunction with the second aspect and the above implementation methods, in some implementation methods of the second aspect, the device further includes a first mapping module, used to determine the first voxel coordinates of the voxel grid to which each map point belongs; and to map the first voxel coordinates to a first hash key value based on a hash function; wherein the first hash key value is used as an index to access a preset storage area and to look up the structural parameters corresponding to the voxel grid in the preset storage area.

[0032] Combining the second aspect and the above implementation methods, in some implementation methods of the second aspect, when there are multiple map points in the voxel grid, the determination module is specifically used to divide the voxel grid into multiple voxel sub-grids and determine the voxel sub-grid to which each map point belongs; wherein, the resolution of the divided voxel sub-grid is higher than the resolution of the voxel grid before division; and one map point is retained in each voxel sub-grid.

[0033] In conjunction with the second aspect and the above implementation methods, in some implementation methods of the second aspect, the device further includes a second mapping module, which is used to determine the second voxel coordinates of the voxel subgrid to which each map point belongs; and to map the second voxel coordinates to a second hash key value based on a hash function; wherein the second hash key value is used as an index to access a preset storage area and to look up the structural parameters corresponding to the voxel subgrid within the preset storage area.

[0034] Thirdly, an electronic device is provided, including a memory and a processor. The memory is used to store executable program code, and the processor is used to call and run the executable program code from the memory, causing the electronic device to perform the methods of the first aspect or any possible implementation thereof.

[0035] Fourthly, a computer program product is provided, comprising: computer program code, which, when run on a computer, causes the computer to perform the methods described in the first aspect or any possible implementation thereof.

[0036] Fifthly, a computer-readable storage medium is provided that stores computer program code, which, when executed on a computer, causes the computer to perform the methods described in the first aspect or any possible implementation thereof. Attached Figure Description

[0037] Figure 1 This is a schematic flowchart illustrating a local map matching method provided in an embodiment of this application.

[0038] Figure 2 This is a schematic diagram of a voxel data structure provided in an embodiment of this application.

[0039] Figure 3 This is a schematic diagram of a voxel mesh update process provided in an embodiment of this application.

[0040] Figure 4 This is a schematic diagram of the structure of a local map matching device provided in an embodiment of this application.

[0041] Figure 5 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0042] The technical solutions in this application will be clearly and thoroughly described below with reference to the accompanying drawings. In the description of the embodiments of this application, unless otherwise stated, " / " means "or," for example, A / B can mean A or B. "And / or" in the text is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Furthermore, in the description of the embodiments of this application, "multiple" refers to two or more than two.

[0043] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature.

[0044] Currently, Simultaneous Localization and Mapping (SLAM) algorithms based on LiDAR all maintain a local point cloud map near the current pose and then use a matching algorithm to match the current LiDAR point cloud with the local point cloud map to solve for the pose. Therefore, maintaining the local point cloud map is crucial for subsequent matching between the point cloud map and the LiDAR point cloud.

[0045] Most existing SLAM algorithms use static kd-trees or incremental ikd-trees to maintain point cloud maps. However, the efficiency of using static kd-trees or incremental ikd-trees to maintain point cloud maps is low, which makes it difficult to match the point cloud map with the laser point cloud.

[0046] Specifically, in the SLAM algorithm, a kd-tree (k-dimensional tree) is a commonly used data structure for efficiently storing and querying points in a multidimensional space. Static kd-trees and incremental kd-trees are two different implementations of kd-trees.

[0047] A static kd-tree is a pre-built kd-tree that remains unchanged during its construction. During construction, all points are added to the kd-tree at once. However, when the point cloud data changes, the entire kd-tree needs to be rebuilt, a process that can be very time-consuming.

[0048] An incremental ikd-tree is a type of kd-tree that allows nodes to be dynamically added or removed during its construction. It allows nodes to be added incrementally during the construction process, rather than building the entire tree all at once. However, inserting or deleting nodes in an incremental ikd-tree can affect the tree's balance, leading to decreased query efficiency.

[0049] Therefore, when the point cloud dataset is very large, rebuilding a static kd-tree or frequently updating an incremental kd-tree will lead to inefficiency. The decrease in query efficiency directly affects the real-time performance and accuracy of the SLAM algorithm, hindering the maintenance of the point cloud map and subsequent matching with laser point clouds.

[0050] Based on this, this application proposes a local map matching method, which pre-fits local structural features based on map points inside the voxel grid and saves the structural parameters corresponding to the voxel grid. The stored structural parameters can be directly used when matching laser point clouds and local maps, avoiding the need to recalculate feature fitting for each matching, which helps to reduce the amount of computation and improve matching efficiency.

[0051] This method is applied to electronic devices, such as robots or vehicles, equipped with lidar. The electronic devices can be in a mobile state, and the lidar collects real-time data on the environment around the electronic devices as they move. Based on the changes in the environment around the electronic devices, the lidar collects different laser point clouds at different times.

[0052] A local map refers to a map of a local area created by an electronic device within its current environment. This local map can be in the form of a point cloud. It is typically constructed using point cloud data acquired by LiDAR (LiDAR radar); that is, LiDAR point clouds can be used to build the local map. As the electronic device moves, new LiDAR point cloud data is collected and used to update the local map.

[0053] Laser point clouds are generally used for simultaneous localization and mapping (SLAM). For example, an electronic device can use a lidar system to collect laser point clouds of its surroundings in real time while moving, and build a map based on these real-time collected laser point clouds, or add newly collected laser point clouds to the map to update the existing map. Alternatively, the electronic device can match the laser point clouds currently collected by the lidar with stored map point clouds to determine its current location. This embodiment uses a robot as an example for illustration.

[0054] Figure 1 This is a schematic flowchart illustrating a local map matching method provided in an embodiment of this application.

[0055] For example, such as Figure 1 As shown, the method 100 includes:

[0056] Step 101: Determine the voxel grid to which each map point in the local map belongs;

[0057] Step 102: Fit local structural features based on map points inside the voxel grid to obtain the structural parameters corresponding to the voxel grid, and store the structural parameters in a preset storage area.

[0058] Step 103: When matching the laser point cloud currently acquired by the lidar with the local map, obtain the structural parameters, and match the laser point cloud with the local map based on the structural parameters.

[0059] exist Figure 1 In the illustrated embodiment, the voxel grid to which each map point belongs in the local map is determined. By voxelizing the local map, the voxel grid to which each map point belongs can be quickly determined, facilitating subsequent processing of the point cloud data. Local structural features are pre-fitted based on the map points within the voxel grid, and the corresponding structural parameters of the voxel grid are saved. These stored structural parameters can be directly used when matching the laser point cloud and the local map, avoiding recalculation of feature fitting for each match, thus reducing computational load and improving matching efficiency. When matching the laser point cloud and the local map, the nearest structural parameters can be directly searched for and residuals constructed, avoiding repeated fitting of structural parameters and quickly finding the optimal matching position for the point cloud, improving matching speed. Furthermore, the structural parameters corresponding to the voxel grid are stored in a preset storage area, allowing for direct maintenance of the local structural parameters of the point cloud without maintaining the point cloud data itself. Since structural parameters typically occupy less storage space than point cloud data, directly maintaining the local structural parameters of the point cloud further reduces memory usage.

[0060] The following is about Figure 1 The specific implementation methods of each step in the illustrated embodiments will be explained below:

[0061] In step 101, the local map can be divided into a series of small cubic units by a method of discretizing the three-dimensional space, each unit being referred to as a voxel or a voxel grid.

[0062] Furthermore, the size (usually the side length) of the voxel mesh determines its resolution. Larger voxel meshes have lower resolution, while smaller voxel meshes have higher resolution.

[0063] In some embodiments, for each map point in a local map, the voxel grid to which each map point belongs can be determined. Specifically, the coordinates of the voxel grid can be determined based on the coordinates of the map point and the resolution of the voxel grid.

[0064] For example, the coordinates of the voxel mesh can be calculated based on the following formula (1):

[0065] p = [p x p y p z ] T v = 1 / s * [p x p y p z ] T Formula (1)

[0066] Where p represents the 3D coordinates of the map point, v represents the coordinates of the voxel grid, and s represents the resolution, or size, of the voxel grid.

[0067] For example, if the preset resolution of the voxel grid is 3 meters and the three-dimensional coordinates of the map point are (6, 6, 9), substituting them into the above formula (1), we can obtain the coordinates of the voxel grid as (2, 2, 3).

[0068] Since spatial voxels divide three-dimensional space into a series of small cubic units, each unit called a voxel grid, spatial voxels are typically uniformly and continuously distributed. However, LiDAR usually only measures within the robot's environment or on its surface. Therefore, the map points in the resulting local map are usually sparse, and much of the space in the local map may be empty. If a traditional mesh structure is still used to store the voxel grid, only some voxel grids may contain map points, while others may be empty. These empty voxel grids also consume storage space, resulting in significant storage waste. Therefore, to avoid this storage waste, a hash table can be used to store spatial voxels, and voxel grids can be created only at locations where map points are present.

[0069] As is understandable, a hash table is a data structure used to store key-value pairs and allows for fast lookup, insertion, and deletion of data. Hash tables primarily access data by mapping keys to a location within the table using a hash function.

[0070] In one possible implementation, after determining the voxel grid to which each map point in the local map belongs, the method further includes: determining the first voxel coordinates of the voxel grid to which each map point belongs; mapping the first voxel coordinates to a first hash key value based on a hash function; wherein the first hash key value is used as an index to access the preset storage area and to look up the structural parameters corresponding to the voxel grid in the preset storage area.

[0071] As can be understood, as mentioned above, the first voxel coordinates of the voxel grid are determined based on the coordinates of the map points and the resolution of the voxel grid. Then, a hash function is used to map the first voxel coordinates of each voxel grid to a hash key value, forming a hash table.

[0072] The hash function is a function that takes an input (i.e., the first voxel coordinates of the voxel grid mentioned above) and returns an integer value, which is called the hash value or hash key value.

[0073] Furthermore, the first hash key value obtained from the above mapping can be used as an index of the hash table to access related data in the hash table.

[0074] The aforementioned preset storage area can store relevant data for each voxel mesh, such as structural parameters. This preset storage area can exist in the form of an independent linked list.

[0075] The aforementioned structural parameters can be obtained by fitting local structural features based on map points inside the voxel mesh. Specifically, they can be linear parameters obtained by fitting a straight line, such as direction vectors and point information on the line, or planar parameters obtained by fitting a plane, such as plane normal vectors and point information on the plane.

[0076] For example, the hash key value corresponding to the first voxel coordinate can be calculated using the following formula (2):

[0077] id v =hash(v) =(v) x n x xor(v) y n y xor(v) z n z )mod N formula (2)

[0078] Among them, id v For hash keys; hash(v) represents the hash function; v x v y v z represents the 3D coordinates of the voxel grid; N is the number of elements in the hash table; n x n y n z `xor` represents a very large prime number; `xor` stands for Exclusive OR, used to compare each bit of two binary numbers, returning 0 if they are the same and 1 if they are different; `mod` is a mathematical operator representing the modulo operation, also known as the modulo operation or remainder operation. It is used to calculate the remainder when one number is divided by another, for example, `a mod b = c` means that the remainder when `a` is divided by `b` is `c`. Here, the modulo operation is used to map the result to the effective index range of the hash table.

[0079] In one possible scenario, although there is a very small probability that the first voxel coordinates corresponding to different voxel grids are mapped to the same hash key value through the hash function, thus generating a hash collision, a hash collision may result in the inability to determine whether to obtain accurate data.

[0080] However, if there are a large number of laser points in each frame of the laser point cloud, it means that a large number of voxel grids need to be stored. In this case, even if there are hash collisions, the impact of mismatches caused by individual laser points on the overall matching result is relatively small because there are a large number of laser points. Therefore, the impact of hash collisions can be ignored.

[0081] In some embodiments, in order to improve the processing efficiency of data in local maps, only pointers to access voxel grid-related data can be stored in the hash table, and the voxel grid-related data can be stored in another independent linked list, namely the aforementioned preset storage area.

[0082] It is understandable that when accessing data, the first hash key value can be determined based on the first voxel coordinates corresponding to the voxel grid. Then, based on the first hash key value, the pointer to access the voxel grid related data can be found in the hash table, and the relevant data of the voxel grid can be accessed through the linked list storing the voxel grid related data.

[0083] In one possible scenario, when a voxel grid contains multiple map points, it may be necessary to calculate the distances between the points to determine which points belong to the same voxel. This distance calculation typically involves floating-point operations, which are computationally intensive and result in low efficiency for inserting new map points.

[0084] To improve the efficiency of map point maintenance and ensure a uniform distribution of points within the voxel grid, embodiments of this application can provide a multi-level voxel structure, which further subdivides the aforementioned voxel grid into high-resolution small voxel grids.

[0085] In one possible implementation, when there are multiple map points in a voxel grid, determining the voxel grid to which each map point in the local map belongs includes: dividing the voxel grid into multiple voxel subgrids, and determining the voxel subgrid to which each map point belongs among the multiple map points; wherein the resolution of the divided voxel subgrid is higher than the resolution of the voxel grid before division; and one map point is retained in each voxel subgrid.

[0086] It is understandable that if a voxel grid contains multiple map points, the voxel grid can be divided into multiple voxel subgrids, which are high-resolution small voxel grids.

[0087] As mentioned earlier, larger voxel meshes have lower resolution, while smaller voxel meshes have higher resolution. The original voxel mesh was larger and had lower resolution, while the resulting voxel sub-mesh was smaller and had higher resolution. Therefore, the resolution of the resulting voxel sub-mesh is higher than that of the original voxel mesh.

[0088] For example, the side length of the voxel grid can be set to 3 in advance, and the resolution of the voxel subgrid can be set to one-third of that of the voxel grid, that is, the side length of the voxel subgrid is 1.

[0089] Then, the voxel subgrid to which each map point belongs can be further determined. Similar to determining the voxel subgrid to which each map point belongs, the coordinates of the voxel subgrid to which the map point belongs can be determined based on the coordinates of the map point and the resolution of the voxel subgrid.

[0090] Furthermore, to improve storage efficiency, only one map point can be retained within each voxel subgrid. For example, the map point retained within a voxel subgrid can be either the map point closest to the center point within that voxel subgrid or the map point located at the center point of the voxel subgrid.

[0091] To reduce the number of points stored in each voxel grid, a maximum of 5 points can be retained per voxel grid. For example, the map point closest to the center point or the map point located at the center point of the voxel subgrid within each voxel grid can be retained, thus ensuring that at least one point is retained in each voxel subgrid.

[0092] Similarly, for each voxel subgrid, the structural parameters corresponding to the voxel subgrid can also be stored using a hash table data structure.

[0093] In one possible implementation, after determining the voxel subgrid to which each of the plurality of map points belongs, the method further includes: determining the second voxel coordinates of the voxel subgrid to which each map point belongs; mapping the second voxel coordinates to a second hash key value based on a hash function; wherein the second hash key value is used as an index to access the preset storage area and to look up the structural parameters corresponding to the voxel subgrid within the preset storage area.

[0094] Understandably, similar to the previous example, the second voxel coordinates of the voxel subgrid can be determined based on the coordinates of the map points and the resolution of the voxel subgrid. Then, through a hash function, the second voxel coordinates of each voxel subgrid can be mapped to a second hash key value to form a hash table corresponding to the voxel subgrid.

[0095] The second hash key value obtained from the above mapping can be used as an index to the hash table corresponding to the voxel subgrid, and access the relevant data of the voxel subgrid in the hash table, such as the structural parameters corresponding to the voxel subgrid.

[0096] The structure of dividing the above voxel grid into multiple voxel subgrids can be called a multi-level voxel structure. This multi-level voxel structure is an optimized spatial structure based on the sparse voxel spatial structure iVox, and can also be called the iPVox voxel data structure.

[0097] For example, such as Figure 2As shown, the iPVox voxel data structure can contain a voxel grid and its internal voxel subgrids, as illustrated in the figure. The figure shows the voxel grid and the voxel grids within its nearest neighbor search range (k-NN search range). The coordinates (h(x, y, z)) of each voxel grid and each voxel subgrid can be mapped to hash keys using a spatial hashing function to form a hash table, i.e. Figure 2 The "Hash map" in the code can store the hash key values ​​corresponding to multiple voxel grids. In addition to storing the hash table, it can also store the coordinates of each map point, which can be stored in another linked list or data structure (underlying structure).

[0098] Current SLAM algorithms based on LiDAR all maintain a local map of the vicinity of the current pose and then use a matching algorithm to match the current LiDAR point cloud with the local map to solve for the pose. Most mainstream algorithms use point-to-line or point-to-plane geometric models for map matching. Therefore, it is necessary to search for several nearest-neighbor map points near the current point and then use these nearest-neighbor map points to fit local lines or planes to construct residuals. Since the overlap rate between adjacent LiDAR point clouds is relatively high during robot movement, for repeatedly observed map points, local line or plane fitting is required each time, a process that is computationally intensive and time-consuming.

[0099] In step 102, as described above, after determining the voxel grid to which each map point in the local map belongs, the relevant parameters of that voxel grid can be stored. To improve matching efficiency, local structural features can be actively fitted within the voxel grid, and the fitted structural parameters can be saved. This way, when matching the local map with the laser point cloud, the nearest structural parameters can be directly searched to construct the residual, without having to repeatedly fit the map points.

[0100] The aforementioned local structural feature fitting refers to extracting specific geometric features, such as lines or planes, from map points within a voxel grid and calculating the parameters of these features.

[0101] For example, for each map point within a voxel grid, a straight line can be fitted using methods such as least squares, and the corresponding line parameters can be obtained. These line parameters can be a direction vector and a point on the line. Here, the direction vector represents the direction of the line, and the point on the line is used to determine the position of the line.

[0102] Similarly, a plane can be fitted using methods such as least squares, and the corresponding plane parameters can be obtained. These parameters can be a normal vector and a point on the plane. Here, the normal vector represents the direction of the plane's normal, and a point on the plane is used to determine the plane's position.

[0103] Understandably, when performing line or plane fitting, a minimum sample size requirement is usually met. For example, fitting a line requires at least two points, and fitting a plane requires at least three non-collinear points. Based on this, a preset number can be set, ensuring that the number of map points used for fitting local structural features is greater than or equal to the preset number.

[0104] In one possible implementation, the structural parameters of the voxel grid are obtained by fitting local structural features to the map points inside the voxel grid, including: determining whether the number of map points inside the voxel grid is greater than or equal to a preset number; if the number of map points is greater than or equal to the preset number, fitting local structural features to the map points inside the voxel grid to obtain the structural parameters of the voxel grid; if the number of map points is less than the preset number, searching for map points in neighboring voxel grids adjacent to the voxel grid, and fitting local structural features to the map points inside the voxel grid and the map points in the neighboring voxel grids to obtain the structural parameters of the voxel grid.

[0105] Understandably, as mentioned above, this preset number can be set based on the minimum number of samples required for fitting a straight line or plane, and can be set higher than that minimum number of samples, for example, the preset number can be set to 5.

[0106] Before fitting local structural features based on map points inside the voxel grid, it can be determined whether the number of map points inside the voxel grid is greater than or equal to a preset number.

[0107] If the number of map points inside the voxel grid is greater than or equal to the preset number, it means that local structural feature fitting can be performed. Then, the least squares method can be used to fit a straight line or a plane. The fitted structural parameters (such as the direction vector and points of the straight line, or the normal vector and points of the plane) can be stored in the preset storage area.

[0108] If the number of map points inside the voxel grid is less than the preset number, it means that the number of map points inside the current voxel grid is insufficient for fitting local structural features. In this case, map points in the nearest neighbor voxel grids adjacent to the voxel grid can be searched with the voxel grid as the center, thereby increasing the number of map points used for fitting local structural features.

[0109] In some embodiments, searching for map points within neighboring voxel grids adjacent to the voxel grid can specifically be done by taking the current voxel grid as the center and considering the positive and negative directions of each axis to search for the nearest neighboring voxel grid. Specifically, it can be done by searching for the nearest neighboring voxel grids in the six directions of front, back, left, right, up, and down of the voxel grid.

[0110] For example, if the coordinates of the current voxel grid are (2, 4, 1), the nearest voxel grids along the x-axis can be searched with the voxel grid as the center. The coordinates of the nearest voxel grids found are (1, 4, 1) and (3, 4, 1). The nearest voxel grids along the y-axis can be searched. The coordinates of the nearest voxel grids found are (2, 3, 1) and (2, 5, 1). The nearest voxel grids along the z-axis can also be searched. The coordinates of the nearest voxel grids found are (2, 4, 0) and (2, 4, 2).

[0111] Furthermore, after finding the nearest voxel grid, map points in the nearest voxel grid can be obtained, and local structural features can be fitted together based on the map points in the nearest voxel grid and the map points inside the voxel grid.

[0112] For example, the number of map points in the nearest voxel grid to be acquired can be determined based on the number of map points inside the voxel and the aforementioned preset number. For instance, if the preset number is 5 and the number of map points inside the voxel is 3, then the number of map points in the nearest voxel grid to be acquired can be determined to be 2.

[0113] In matching laser point clouds and local maps, it's typically not necessary to find the closest point in the local map to every point in the laser point cloud. Instead, it's about finding several local map points near each point in the laser point cloud, usually within a pre-defined search range. For each point in the laser point cloud, several nearby local map points are found. Then, residuals are calculated based on a defined geometric model (such as the distance from a point to a line or the distance from a point to a plane). These residuals reflect the distance or deviation between the point cloud points and the local map points.

[0114] To ensure the validity of the match, the range of neighboring map points needs to be limited. If the distance is too far, these points may not belong to the same local structure, potentially leading to incorrect matches. Typically, a search radius or search range is set, and points outside this range are not considered. Based on this, when fitting local structural features using map points within a voxel grid, if it's necessary to search for neighboring voxel grids, a search range can also be set to avoid searching for voxel grids that are too far away from the current voxel grid, thus preventing incorrect matches.

[0115] In other embodiments, a nearest neighbor model can be pre-defined, and when performing a nearest neighbor search, the nearest voxel grid can be determined within the range of the nearest neighbor model, centered on the current voxel grid.

[0116] Understandably, the nearest neighbor model defines a rule or range for determining which voxel grids around the current voxel grid will be considered as nearest neighbor voxel grids. This nearest neighbor model can be a fixed search range, a specific voxel grid layout, or a dynamically adjusted range based on the characteristics of the point cloud distribution.

[0117] For example, three nearest neighbor models can be set, such as the Nearby18 nearest neighbor model, the Nearby26 nearest neighbor model, and the Nearby124 nearest neighbor model. The Nearby18 nearest neighbor model consists of voxel meshes that are directly adjacent to the current voxel mesh via a face or edge. The Nearby26 model includes all voxel meshes within a 3x3x3 area centered on the current voxel mesh. The Nearby124 model includes all voxel meshes within a 5x5x5 area centered on the current voxel mesh.

[0118] Similarly, after finding the nearest voxel grid, map points in the nearest voxel grid can be obtained, and local structural features can be fitted together based on the map points in the nearest voxel grid and the map points inside the voxel grid.

[0119] Furthermore, as the robot continues to move, the map expands. To improve the efficiency of local map maintenance and reduce memory usage, historical map data exceeding the measurement range of the robot's sensors needs to be deleted promptly. Therefore, incremental maintenance of the local map near the current pose is required, with continuous updates to the local map.

[0120] Understandably, incremental maintenance means continuously updating the local map as the robot continues to move. When new point cloud data is available, the new point cloud is added to the local map, and the structural parameters of the voxel mesh in the local map are updated as needed to ensure that the local map is always up-to-date.

[0121] In one possible implementation, the method further includes: if there is a newly added map point cloud in the local map, determining the voxel grid to which each map point to be added in the map point cloud belongs, and adding each map point to be added to the voxel grid to which it belongs.

[0122] It is understandable that the newly added map point cloud is usually obtained by processing and converting the laser point cloud newly captured by the LiDAR. The voxel grid to which the map points to be added belong can be an existing voxel grid or a newly determined voxel grid.

[0123] When receiving a new map point cloud, the voxel grid to which each map point to be added belongs can be determined first. The specific calculation method is as described above and will not be repeated here.

[0124] Furthermore, after determining the voxel grid to which each map point to be added belongs, each map point to be added can be added to its respective voxel grid.

[0125] As mentioned earlier, to reduce the number of points stored in each voxel grid, a maximum of 5 map points are retained within each voxel grid. Therefore, when adding map points to their respective voxel grids, it is necessary to ensure that the number of map points within the voxel grid after adding the map points does not exceed 5.

[0126] In one possible implementation, adding each map point to be added to its corresponding voxel grid includes: determining whether the sum of the number of map points to be added and the number of map points in the voxel grid to which the map point to be added belongs is less than or equal to a preset number; if the sum of the numbers is less than or equal to the preset number, then adding the map point to be added to its corresponding voxel grid.

[0127] Understandably, before adding a map point to its voxel grid, it is necessary to determine whether the sum of the number of map points to be added and the number of current map points in its voxel grid exceeds a preset number.

[0128] If the number of map points in the voxel grid exceeds the preset number after adding the map point to be added, it may occupy too much storage space and is not beneficial to the maintenance of the local map. Therefore, if the sum of the number of map points to be added and the number of current map points in the voxel grid exceeds the preset number, the map point to be added may not be added to the voxel grid.

[0129] If the number of map points in the voxel grid to which the map point to be added belongs does not exceed the preset number after the map point to be added is added to the voxel grid to which it belongs, then the map point to be added can be added to the voxel grid to which it belongs.

[0130] Furthermore, if a new map point cloud appears in a local area of ​​the map, it indicates that the lidar has newly observed a laser point, and that the laser point has been processed and converted to obtain the map point to be added. After determining the voxel grid to which the map point to be added belongs, it can be confirmed that the voxel grid to which the map point to be added belongs is the most recently observed voxel grid.

[0131] To more intuitively understand the observation order of each voxel grid, a voxel linked list can be constructed. This list stores voxel grids and arranges them according to the order in which they were observed. The head of the voxel linked list contains the most recently observed voxel grids, while the tail contains voxel grids observed much earlier.

[0132] In one possible implementation, after determining the voxel grid to which each map point to be added belongs in the map point cloud, the method further includes: moving the voxel grid to which the map point to be added belongs to the head of the voxel linked list; wherein the voxel linked list is used to store the voxel grids in the local map according to the observation order of each voxel grid; if the data capacity of the voxel linked list is greater than a preset capacity, then deleting the voxel grid at the tail of the voxel linked list.

[0133] Understandably, as mentioned earlier, the voxel list stores voxel grids in the order they were observed. After determining the voxel grid to which the map point to be added belongs, that voxel grid can be moved to the head of the voxel list to indicate that it is the most recently observed voxel grid.

[0134] Regardless of whether the map point to be added is added to its corresponding voxel grid, the voxel grid to which the map point to be added belongs can be moved to the head of the voxel list first.

[0135] For example, if there are multiple map points to be added, the voxel grid to which each map point belongs can be determined sequentially. After determining the voxel grid to which each map point belongs, the voxel grid to which the map point belongs is moved to the head of the voxel list, until all voxel grids to which each map point belongs have been moved.

[0136] To avoid wasting storage space and facilitate effective storage management, a maximum capacity for the local map can be set, i.e., the preset capacity mentioned above. If the data capacity in the voxel list exceeds this maximum capacity, the voxel grid at the end of the voxel list is deleted.

[0137] It is understandable that the voxel grids deleted above can specifically be the voxel grids that are at the end of the voxel chain and exceed the maximum capacity.

[0138] For example, if the maximum capacity of the local map, i.e. the preset capacity, is 100 voxel grids, after determining the voxel grid to which the map point to be added belongs and moving the voxel grid to which the map point to be added belongs to the head of the voxel list, if the number of voxel grids in the current voxel list is 102, then the two voxel grids at the tail of the voxel list can be deleted.

[0139] Understandably, the above scheme actually uses the Least Recently Used (LRU) caching approach to update the voxel grid. That is, when the cache space is limited, if a new voxel grid needs to be inserted after the cache is full, the least recently observed voxel grid is removed.

[0140] For example, such as Figure 3 As shown, the left side represents the scenario where no new map points are added to the local map. Specifically, within the current LiDAR observation range (the dashed ellipse), only 6 voxel grids exist. The voxel list can store these 6 voxel grids (1, 2, 3, 4, 5, and 6), and a hash table can be used to retrieve the relevant data for each voxel grid in the list. However, when the observed target (Vehicle) moves, the LiDAR's acquired laser points change; new laser points appear, and old laser points may leave the observation range. The right side represents the scenario where new map points are added to the local map. Within the current LiDAR observation range (the dashed ellipse), there are new map points and new voxel grids (7 and 8). The current voxel list stores 8 voxel grids (1, 2, 3, 4, 5, 6, 7, and 8). After determining the voxel grid to which the new map point belongs (either a newly added voxel grid or an existing one), its corresponding voxel grids (7, 8) can be moved to the head of the voxel list. The positions of the voxel grids in the hash table remain unaffected. This results in the other voxel grids in the voxel list—those not recently used (1, 2)—being at the tail of the list. If a maximum storage capacity for the voxel list is set, when the data capacity in the voxel list exceeds the maximum capacity, the voxel grids at the tail are removed; for example, the two voxel grids at the tail (1, 2) can be removed.

[0141] The above method moves the voxel grid to the head of the voxel list to ensure that newly observed voxel grids are always at the front of the list, while voxel grids that have not been observed for a long time are at the back. If the data capacity in the voxel list exceeds a preset capacity, deleting voxel grids at the tail of the list ensures that the data in the local map is up-to-date, facilitating effective storage space management and improving storage efficiency.

[0142] As mentioned earlier, if the number of map points within a voxel grid is less than a preset number, it is necessary to obtain map points from the nearest voxel grids. Based on the map points in the nearest voxel grids and the map points within the voxel grid, local structural features are fitted to obtain the structural parameters corresponding to the voxel grid. However, if new map point clouds are added to the local map, the number of map points within the voxel grid may increase to a number greater than or equal to the preset number. After the increase in map points within the voxel grid, local structural features can be refitted based on the increased map points.

[0143] In one possible implementation, after fitting local structural features to map points within the voxel grid and map points within neighboring voxel grids to obtain the structural parameters corresponding to the voxel grid, the method further includes: if there are newly added map point clouds in the local map, and if the number of map points within the voxel grid increases and the number of added map points is greater than or equal to a preset number, then fitting local structural features to the added map points within the voxel grid to obtain the updated structural parameters corresponding to the voxel grid; and updating the structural parameters stored in the preset storage area to the updated structural parameters.

[0144] Understandably, when new map point clouds are added to a local map, it is necessary to check whether the number of map points within the voxel grid containing these new point clouds has increased. If the number of map points within the voxel grid increases, and the increased number of map points is greater than or equal to a preset number, it indicates that the map points within that voxel grid can be fitted with local structural features.

[0145] Furthermore, local structural features are fitted to the map points added within the voxel grid to obtain the updated structural parameters corresponding to the voxel grid. The structural parameters stored in the preset storage area can be replaced with the updated structural parameters to reflect the latest information corresponding to the voxel grid.

[0146] In some embodiments, after updating the structure parameters stored in the preset storage area to the updated structure parameters, all map points in the voxel grid can be deleted, and only the updated structure parameters corresponding to the voxel grid can be retained.

[0147] As can be understood, as mentioned above, structural parameters can be either linear parameters or planar parameters. Taking planar parameters as an example, since a plane can be determined by a normal vector and a point on the plane, when saving planar parameters, only the normal vector and a point on the plane need to be saved.

[0148] A normal vector can be represented as three floating-point numbers (float parameters), and a point on a plane can be represented as one floating-point number. Therefore, maintaining a local planar structural feature only requires storing four floating-point numbers of the plane, meaning the memory occupied is the size of four floating-point numbers.

[0149] However, when maintaining points, each 3D point requires at least 3 floating-point numbers. If there are 5 map points within a voxel mesh, it would require 15 floating-point numbers. Therefore, maintaining points uses 3.75 times the memory required to maintain a local plane. Thus, retaining only the structural parameters corresponding to the voxel mesh can significantly reduce memory usage.

[0150] In step 103, after storing the structural parameters corresponding to the voxel grid into a preset storage area, the structural parameters can be obtained when matching the laser point cloud and local map currently acquired by the lidar, and the laser point cloud and local map can be matched based on the structural parameters.

[0151] As can be understood, as mentioned above, structural parameters are feature parameters extracted from map points within the voxel grid. These structural parameters are obtained by fitting local structural features when the number of map points within the voxel grid reaches a certain level.

[0152] Current SLAM algorithms based on LiDAR maintain a local map of the vicinity of the current pose and then use a matching algorithm to match the current LiDAR point cloud with the local map point cloud to solve for the pose. To determine the current pose, a matching algorithm is typically used to match the point cloud data obtained from the latest LiDAR scan (the current LiDAR point cloud) with map points in the maintained local map. The matching process aims to find the best correspondence between the two sets of point cloud data.

[0153] In this embodiment, the structural parameters are pre-stored in a preset storage area. When matching the laser point cloud currently acquired by the LiDAR with the local map, the relationship between the laser points in the laser point cloud and the map points can be determined by comparing the points in the laser point cloud with the structural parameters stored in the local map. This eliminates the need for feature extraction and fitting of the map points in the local map again, significantly reducing the computational load in the online matching process and improving matching speed and efficiency.

[0154] For example, if the stored structural parameters are line parameters obtained by fitting a straight line, then after the lidar acquires a new laser point cloud, the direction vector in the line parameters and the point information on the line can be used to find a line in the local map that matches each laser point in the laser point cloud. Furthermore, the quality of the match can be evaluated by calculating the distance from the point to the line.

[0155] If the stored structural parameters are planar parameters obtained by fitting a plane, then after the lidar acquires a new laser point cloud, the plane normal vector and point information on the plane in the planar parameters can be used to find a plane in the local map that matches each laser point in the laser point cloud. Similarly, the quality of the match can be evaluated by calculating the distance from the point to the plane.

[0156] Furthermore, once a matching line or plane is found in the local map, this matching information can be used to estimate the pose of the current point cloud relative to the local map, thereby updating the current local map based on the pose.

[0157] To further improve the performance of the matching algorithm, the observation probability corresponding to each voxel grid in the local map can also be stored. The observation probability is the probability that a voxel grid in the local map will be observed. It can be used to evaluate the reliability of the local map and can reflect whether the area is frequently visible or whether there is occlusion.

[0158] In one possible implementation, matching the laser point cloud and the local map based on the structural parameters includes: determining the target observation probability of each voxel grid to which each map point in the local map belongs; wherein the target observation probability is used to characterize the probability that the voxel grid is observed; and matching the laser point cloud and the local map based on the structural parameters and the target observation probability.

[0159] Understandably, after determining the voxel grid to which each map point belongs, an initial observation probability can be assigned to each voxel grid. Without adding new map point clouds to the local map, the target observation probability of each voxel grid is the initial observation probability.

[0160] For example, the initial observation probability can be set to 30%.

[0161] Furthermore, when there are newly added map point clouds in the local map, after determining the voxel grid to which each map point in the map point cloud belongs, the initial observation probability of each voxel grid in the local map can be updated.

[0162] In one possible implementation, determining the target observation probability of each voxel grid to which each map point belongs in the local map includes: when there is a newly added map point cloud in the local map, determining the voxel grid to which each map point to be added belongs in the map point cloud, and determining whether the voxel grid to which each map point to be added belongs belongs to an existing voxel grid in the local map; if it is determined that the voxel grid to which the map point to be added belongs belongs to an existing voxel grid in the local map, adding the map point to be added to its voxel grid, and increasing the initial observation probability corresponding to the voxel grid based on a preset growth gradient to obtain the target observation probability corresponding to the voxel grid; for other voxel grids in the local map other than the voxel grid to which the map point to be added belongs, decreasing the initial observation probability of the other voxel grids based on a preset decay gradient to obtain the target observation probability corresponding to the other voxel grids; if it is determined that the voxel grid to which the map point to be added belongs does not belong to an existing voxel grid in the local map, creating a new voxel grid in the local map, and determining the target observation probability corresponding to the new voxel grid as the initial observation probability.

[0163] Understandably, as mentioned above, in the absence of new map point clouds added to the local map, each voxel grid is set with an initial observation probability.

[0164] When a new map point cloud is added to a local map, the voxel grid to which each map point to be added belongs is determined. The determined voxel grid may be a voxel grid that already exists in the local map, i.e., it belongs to the local map, or it may be a newly added voxel grid, i.e., it does not belong to the local map.

[0165] If the identified voxel grid does not belong to the local map, a new voxel grid can be created in the local map, and the target observation probability of the new voxel grid can be set as the initial observation probability, for example, the target observation probability of the new voxel grid can be set to 30%.

[0166] If the identified voxel grid belongs to the local map, it means that the voxel grid has been newly observed. The initial observation probability of the voxel grid can then be increased based on a preset growth gradient to obtain the target observation probability corresponding to that voxel grid. For example, if the preset growth gradient is 2%, the target observation probability of the newly observed voxel grid is 31%.

[0167] For other voxel grids in the local map besides the newly observed voxel grids, the initial observation probability of these voxel grids can be decreased based on a preset decay gradient to obtain the target observation probability for each voxel grid. For example, if the preset decay gradient is 1%, the target observation probability of other voxel grids besides the newly observed voxel grids is 29%.

[0168] Furthermore, when there are newly added map point clouds in the local map, it indicates that the information in the current local map may be outdated or inaccurate. Therefore, in some embodiments, after each map point in the newly added map point cloud is added to its respective voxel grid, the initial observation probability of each voxel grid can be reduced according to a preset attenuation gradient.

[0169] In other words, if no new map points are added to a voxel grid belonging to the local map, the observation probability of that voxel grid will gradually decrease over time.

[0170] The aforementioned preset decay gradient refers to the rate at which the observation probability of the voxel grid gradually decreases over time. For example, the preset decay gradient can be set to 1%.

[0171] For example, if the initial observation probability of each voxel grid is set to 30% and the preset decay gradient is 1%, when there are newly added map point clouds in the local map, after each map point in the map point cloud is added to its respective voxel grid, the initial observation probability of each voxel grid can be decreased according to the decay gradient of 1%, so as to obtain the target observation probability corresponding to each voxel grid, which is 29%.

[0172] Furthermore, since there are newly added map point clouds in the local map, the points in the newly added map point clouds are the most recently observed points. After determining the voxel grid to which the map points in the newly added map point clouds belong, the target observation probability corresponding to the voxel grid can be increased based on a preset growth gradient.

[0173] In other words, if a new map point is added to a voxel grid, the observation probability of that voxel grid will increase.

[0174] The preset growth gradient mentioned above refers to the rate at which the observation probability of the voxel grid gradually increases over time. For example, the preset growth gradient can be set to 2%.

[0175] For example, if the initial observation probability of each voxel grid is set to 30%, the preset decay gradient is 1%, and the preset growth gradient is 2%, when a new map point cloud is added to the local map, after adding each map point in the map point cloud to its corresponding voxel grid, the initial observation probability of each voxel grid can be decreased by a 1% decay gradient to obtain the target observation probability of each voxel grid, which is 29%. If it is determined that the newly added map point belongs to voxel grid A in the local map, the target observation probability of voxel grid A can be increased by a preset growth gradient, resulting in an updated target observation probability of 31% for voxel grid A.

[0176] The method described above first decreases the observation probability for all voxel grids, which helps reduce the importance of unobserved voxel grids and ensures they do not have excessive weight in subsequent observations. Then, it increases the observation probability for observed voxel grids, which reinforces newly observed information and ensures the latest observations are given sufficient attention. Furthermore, it avoids spending excessive time determining which specific voxel grids are unobserved; by first decreasing the observation probability for all voxel grids and then increasing it for observed voxel grids, the observation probability of every voxel grid in the local map is effectively updated.

[0177] In some embodiments, the target observation probability corresponding to a voxel grid can be determined based on the number of times a voxel grid in a local map is newly observed and the aforementioned preset growth gradient.

[0178] It is understandable that the number of times a voxel grid is newly observed can be interpreted as the number of times the voxel grid is moved to the head of the voxel list when new map point clouds are added in the local map. The product of the number of times the voxel grid is newly observed and the preset growth gradient can be used as the increase in the target observation probability of the voxel grid.

[0179] For example, if a new map point cloud is added to the local map, and voxel grid B is moved to the head of the voxel list 3 times, it means that voxel grid B has been newly observed 3 times. If the preset growth gradient is 2%, then the increase in the target observation probability of voxel grid B is determined to be 6%. If the original target observation probability of voxel grid B was 29%, then the target observation probability of the updated target voxel grid is determined to be 35%.

[0180] It is evident that some voxel grids in a local map may be observed repeatedly, and the target observation probability corresponding to these voxel grids will continue to increase. However, for map points located in unstable regions such as dynamic targets, noise points, glass, or object edges, their voxel grids may be observed less frequently, and the target observation probability corresponding to these voxel grids will remain near the initial probability or decrease to a low state.

[0181] Furthermore, after determining the target observation probability corresponding to each voxel grid in the local map, the target observation probability corresponding to each voxel grid can be stored in a preset storage area for use in subsequent matching of laser point clouds and local maps.

[0182] For example, when matching the laser point cloud currently acquired by the lidar with the local map, the target observation probability corresponding to each voxel grid can be obtained, and voxel grids with low target observation probabilities can be filtered out to remove dynamic targets and noise points.

[0183] Furthermore, when matching the laser point cloud currently collected by the lidar with the local map, a series of matching constraints are usually established.

[0184] Among them, matching constraints refer to the correspondence between laser points in the laser point cloud and map points or structural parameters in the local map.

[0185] In some embodiments, the target observation probability of the voxel grid can be used to weight the matching constraints. The purpose of weighting the matching constraints is to give higher weights to static and stable regions, i.e., regions with high target observation probabilities, and lower weights to unstable regions, i.e., regions with low target observation probabilities. In this way, when performing laser point cloud and local map matching, it can be ensured that the matching algorithm pays more attention to stable and reliable point cloud data, thereby improving the accuracy of the matching.

[0186] Understandably, for each voxel grid, the matching constraints are weighted according to the target observation probability corresponding to that voxel grid. Matching constraints in voxel grids with high observation probabilities have higher weights, while matching constraints in voxel grids with low observation probabilities have lower weights.

[0187] For map points in unstable regions such as dynamic targets, noise points, or object edges in the local map, as well as newly expanded map edge regions, these map points are observed less frequently. Therefore, their observation probability will be near the initial probability or decrease to a low state, and their probability weight will decrease. Weighting the matching residual based on the smaller probability weight can reduce the matching constraint strength corresponding to laser points with small probability weights. For map points in static and stable regions of the environment, they are observed repeatedly, and their observation probability will continuously increase, resulting in a larger probability weight. Weighting the matching residual based on the larger probability weight can increase the matching constraint strength corresponding to laser points with large probability weights. Ultimately, this can cause the matching results of the laser point cloud to move closer to static, high-probability regions in the map, reducing the impact of unstable observations on the matching results.

[0188] Figure 4 This is a schematic diagram of the structure of a local map matching device provided in an embodiment of this application.

[0189] For example, such as Figure 4 As shown, the device 400 includes:

[0190] Module 401 is used to determine the voxel grid to which each map point in the local map belongs;

[0191] The fitting module 402 is used to fit local structural features based on map points inside the voxel grid, obtain the structural parameters corresponding to the voxel grid, and store the structural parameters in a preset storage area.

[0192] The matching module 403 is used to obtain structural parameters when matching the laser point cloud and local map currently acquired by the lidar, and to match the laser point cloud and local map based on the structural parameters.

[0193] In one possible implementation, the fitting module is specifically used to: determine whether the number of map points inside the voxel grid is greater than or equal to a preset number; if the number of map points is greater than or equal to the preset number, perform local structural feature fitting on the map points inside the voxel grid to obtain the structural parameters corresponding to the voxel grid; if the number of map points is less than the preset number, search for map points in the nearest neighbor voxel grids adjacent to the voxel grid, with the voxel grid as the center, and perform local structural feature fitting on the map points inside the voxel grid and the map points in the nearest neighbor voxel grids to obtain the structural parameters corresponding to the voxel grid.

[0194] Optionally, the device further includes: an update module, used to perform local structural feature fitting on the map points inside the voxel grid when there are newly added map point clouds in the local map, if the number of map points inside the voxel grid increases and the number of map points after the increase is greater than or equal to a preset number, to obtain the updated structural parameters corresponding to the voxel grid; and to update the structural parameters stored in the preset storage area to the updated structural parameters.

[0195] Optionally, the device also includes an adding module for determining the voxel grid to which each map point to be added belongs in the map point cloud when there is a newly added map point cloud in the local map, and adding each map point to be added to the voxel grid to which it belongs.

[0196] In one possible implementation, the adding module is specifically used to determine whether the sum of the number of map points to be added and the number of map points in the voxel grid to which the map points to be added belong is less than or equal to a preset number; if the sum of the numbers is less than or equal to the preset number, then the map points to be added are added to the voxel grid to which they belong.

[0197] In one possible implementation, the adding module is further configured to move the voxel grid to which the map point to be added belongs to the head of the voxel linked list; wherein the voxel linked list is used to store the voxel grids in the local map according to the observation order of each voxel grid; if the data capacity in the voxel linked list is greater than the preset capacity, the voxel grid at the tail of the voxel linked list is deleted.

[0198] In one possible implementation, the matching module is specifically used to determine the target observation probability of each voxel grid to which each map point in the local map belongs; wherein the target observation probability is used to characterize the probability that the voxel grid is observed; and to match the laser point cloud and the local map based on the structural parameters and the target observation probability.

[0199] In one possible implementation, the matching module is further configured to: determine the voxel grid to which each map point to be added belongs in the map point cloud when a new map point cloud exists in the local map, and determine whether the voxel grid to which each map point to be added belongs belongs to the local map; if it is determined that the voxel grid to which the map point to be added belongs belongs to the local map, add the map point to be added to the voxel grid to which it belongs, and increment the initial observation probability corresponding to the voxel grid based on a preset growth gradient to obtain the target observation probability corresponding to the voxel grid; for other voxel grids in the local map other than the voxel grid to which the map point to be added belongs, decrement the initial observation probability of the other voxel grids based on a preset decay gradient to obtain the target observation probability corresponding to the other voxel grids; if it is determined that the voxel grid to which the map point to be added belongs does not belong to the local map, create a new voxel grid in the local map, and determine the target observation probability corresponding to the new voxel grid as the initial observation probability.

[0200] In one possible implementation, the device further includes a first mapping module for determining the first voxel coordinates of the voxel grid to which each map point belongs; mapping the first voxel coordinates to a first hash key value based on a hash function; wherein the first hash key value is used as an index to access a preset storage area and to look up the structural parameters corresponding to the voxel grid within the preset storage area.

[0201] In one possible implementation, when there are multiple map points in the voxel grid, the determination module is specifically used to divide the voxel grid into multiple voxel subgrids and determine the voxel subgrid to which each map point belongs; wherein the resolution of the divided voxel subgrid is higher than the resolution of the voxel grid before division; and one map point is retained in each voxel subgrid.

[0202] Optionally, the device further includes a second mapping module for determining the second voxel coordinates of the voxel subgrid to which each map point belongs; mapping the second voxel coordinates to a second hash key value based on a hash function; wherein the second hash key value is used as an index to access a preset storage area and to look up the structural parameters corresponding to the voxel subgrid within the preset storage area.

[0203] Figure 5 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application.

[0204] For example, such as Figure 5 As shown, the electronic device 500 includes a memory 501 and a processor 502. The memory 501 stores executable program code 5011, and the processor 502 is used to call and execute the executable program code 5011 to perform a local map matching method.

[0205] Furthermore, embodiments of this application also protect an apparatus that may include a memory and a processor, wherein the memory stores executable program code, and the processor is used to call and execute the executable program code to perform a local map matching method provided in embodiments of this application.

[0206] This embodiment can divide the device into functional modules based on the above method example. For example, each module can correspond to a separate function, or two or more functions can be integrated into one processing module. The integrated module can be implemented in hardware. It should be noted that the module division in this embodiment is illustrative and only represents one logical functional division. In actual implementation, there may be other division methods.

[0207] When each functional module is divided according to its corresponding function, the device may further include a determination module, a fitting module, and a matching module. It should be noted that all relevant content regarding the steps involved in the above method embodiments can be referenced from the functional descriptions of the corresponding functional modules, and will not be repeated here.

[0208] It should be understood that the apparatus provided in this embodiment is used to execute the above-described local map matching method, and therefore can achieve the same effect as the above-described implementation method.

[0209] When using integrated units, the device may include a processing module and a storage module. When applied to an electronic device, the processing module can be used to control and manage the operation of the electronic device. The storage module can be used to support the execution of relevant program code and data by the electronic device.

[0210] The processing module may be a processor or a controller, which can implement or execute various exemplary logic blocks, modules, and circuits shown in conjunction with the disclosure of this application. The processor may also be a combination of functions that implement computing capabilities, such as a combination of one or more microprocessors, a combination of digital signal processing (DSP) and a microprocessor, etc., and the storage module may be a memory.

[0211] In addition, the device provided in the embodiments of this application may specifically be a chip, component or module. The chip may include a connected processor and a memory. The memory is used to store instructions. When the processor calls and executes the instructions, the chip can execute a local map matching method provided in the above embodiments.

[0212] This embodiment also provides a computer-readable storage medium storing computer program code. When the computer program code is run on a computer, the computer executes the aforementioned method steps to implement the local map matching method provided in the above embodiment.

[0213] This embodiment also provides a computer program product that, when run on a computer, causes the computer to perform the aforementioned related steps to implement a local map matching method provided in the above embodiment.

[0214] In this embodiment, the device, computer-readable storage medium, computer program product, or chip are all used to execute the corresponding methods provided above. Therefore, the beneficial effects they can achieve can be referred to the beneficial effects in the corresponding methods provided above, and will not be repeated here.

[0215] Through the above description of the embodiments, those skilled in the art will understand that, for the sake of convenience and brevity, only the division of the above functional modules is used as an example. In actual 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.

[0216] In the embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another device, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0217] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for matching local maps, characterized in that, The method includes: Determine the voxel grid to which each map point in the local map belongs; Local structural features are fitted based on map points inside the voxel grid to obtain the structural parameters corresponding to the voxel grid, and the structural parameters are stored in a preset storage area. When matching the laser point cloud currently acquired by the lidar with the local map, the structural parameters are obtained, and the laser point cloud and the local map are matched based on the structural parameters.

2. The method according to claim 1, characterized in that, The step of fitting local structural features based on map points within the voxel grid to obtain the structural parameters corresponding to the voxel grid includes: Determine whether the number of map points inside the voxel grid is greater than or equal to a preset number; If the number of map points is greater than or equal to the preset number, local structural features are fitted to the map points inside the voxel grid to obtain the structural parameters corresponding to the voxel grid. If the number of map points is less than the preset number, map points in neighboring voxel grids are searched with the voxel grid as the center. Local structural features are fitted to the map points inside the voxel grid and the map points in the neighboring voxel grids to obtain the structural parameters corresponding to the voxel grid.

3. The method according to claim 2, characterized in that, After performing local structural feature fitting on the map points inside the voxel grid and the map points in the nearest voxel grid to obtain the structural parameters corresponding to the voxel grid, the method further includes: If there are newly added map point clouds in the local map, and if the number of map points inside the voxel grid increases and the number of map points after the increase is greater than or equal to the preset number, then the local structural features of the map points inside the voxel grid after the increase are fitted to obtain the updated structural parameters corresponding to the voxel grid. Update the structure parameters stored in the preset storage area to the updated structure parameters.

4. The method according to claim 1, characterized in that, The method further includes: If a new map point cloud is added to the local map, the voxel grid to which each map point to be added belongs is determined, and each map point to be added is added to its respective voxel grid.

5. The method according to claim 4, characterized in that, Adding each of the map points to be added to its corresponding voxel grid includes: Determine whether the sum of the number of map points to be added and the number of map points within the voxel grid to which the map points to be added belong is less than or equal to a preset number; If the sum of the quantities is less than or equal to the preset quantity, the map point to be added is added to its corresponding voxel grid.

6. The method according to claim 4, characterized in that, After determining the voxel grid to which each map point to be added belongs in the map point cloud, the method further includes: Move the voxel grid to which the map point to be added belongs to the head of the voxel list; wherein the voxel list is used to store the voxel grids in the local map according to the observation order of each voxel grid; If the data capacity in the voxel list is greater than the preset capacity, then delete the voxel grid at the end of the voxel list.

7. The method according to claim 1, characterized in that, The matching of the laser point cloud and the local map based on the structural parameters includes: Determine the target observation probability for each voxel grid to which each map point belongs in the local map; wherein the target observation probability is used to characterize the probability that the voxel grid is observed; Based on the structural parameters and the target observation probability, the laser point cloud and the local map are matched.

8. The method according to claim 7, characterized in that, Determining the target observation probability for each voxel grid to which each map point belongs in the local map includes: In the case where there are newly added map point clouds in the local map, determine the voxel grid to which each map point to be added belongs in the map point cloud, and determine whether the voxel grid to which each map point to be added belongs belongs to the local map. If it is determined that the voxel grid to which the map point to be added belongs belongs to the local map, the map point to be added is added to the voxel grid to which it belongs, and the initial observation probability corresponding to the voxel grid is increased based on a preset growth gradient to obtain the target observation probability corresponding to the voxel grid; for other voxel grids in the local map other than the voxel grid to which the map point to be added belongs, the initial observation probability of the other voxel grids is decreased based on a preset decay gradient to obtain the target observation probability corresponding to the other voxel grids; If it is determined that the voxel grid to which the map point to be added belongs does not belong to the local map, a new voxel grid is created in the local map, and the target observation probability corresponding to the new voxel grid is determined as the initial observation probability.

9. The method according to claim 1, characterized in that, After determining the voxel grid to which each map point in the local map belongs, the method further includes: Determine the first voxel coordinates of the voxel grid to which each map point belongs; Based on a hash function, the first voxel coordinates are mapped to a first hash key value; wherein, the first hash key value is used as an index to access the preset storage area and to find the structural parameters corresponding to the voxel mesh within the preset storage area.

10. The method according to claim 1, characterized in that, In the case where there are multiple map points in the voxel grid, determining the voxel grid to which each map point in the local map belongs includes: The voxel grid is divided into multiple voxel subgrids, and the voxel subgrid to which each map point belongs is determined; wherein the resolution of the divided voxel subgrid is higher than the resolution of the voxel grid before division; and one map point is retained in each voxel subgrid.

11. The method according to claim 10, characterized in that, After determining the voxel subgrid to which each of the plurality of map points belongs, the method further includes: Determine the second voxel coordinates of the voxel subgrid to which each map point belongs; Based on a hash function, the second voxel coordinates are mapped to a second hash key value; wherein, the second hash key value is used as an index to access the preset storage area and to find the structural parameters corresponding to the voxel subgrid within the preset storage area.

12. A local map matching device, characterized in that, The device includes: The determination module is used to determine the voxel grid to which each map point in the local map belongs; The fitting module is used to fit local structural features based on map points inside the voxel grid, obtain structural parameters corresponding to the voxel grid, and store the structural parameters in a preset storage area. The matching module is used to obtain the structural parameters when matching the laser point cloud currently acquired by the lidar with the local map, and to match the laser point cloud and the local map based on the structural parameters.

13. An electronic device, characterized in that, The electronic device includes: Memory, used to store executable program code; A processor for calling and running the executable program code from the memory, causing the electronic device to perform the method as described in any one of claims 1 to 11.

14. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed, implements the method as described in any one of claims 1 to 11.

Images