Positioning method based on laser odometer system and laser odometer system

Abstract

The invention provides a positioning method based on a laser speedometer system and the laser speedometer system. The positioning method comprises the following steps: generating a hash table of a current frame image and a hash table of a current voxel map; determining voxels of the current voxel map matched with the voxels of the current frame image according to the voxel information of each voxel of the current frame image and the voxel information of each voxel of the current voxel map; determining the voxel type of the voxel of the current frame image according to the voxel information of the voxel of the current frame image, screening out a target voxel from the voxels of the current frame image, and determining the residual error of the target voxel according to the voxel type of the target voxel and the voxel of the matched current voxel map; and updating the coordinates of the feature points in the voxel of the current voxel map matched with the target voxel by using the residual error of the target voxel, and returning to execute the generation of the hash table of the current frame image and the hash table of the current voxel map so as to improve the positioning efficiency, response speed, accuracy, supported incremental updating and real-time processing suitable for a dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and more specifically, to a positioning method and a laser odometry system based on a laser odometry system. Background Technology

[0002] In autonomous driving environments, vehicles rely on a range of sensors to perceive their surroundings and make safe driving decisions. LiDAR, as a key sensor, provides high-precision 3D point cloud data to depict the vehicle's environment. Lidar Odometry (LO) systems process this point cloud data to help autonomous vehicles achieve precise positioning and continuous trajectory tracking, making them a core component ensuring their autonomous navigation capabilities. The core task of LO is to calculate the vehicle's displacement and rotation by analyzing point cloud changes across consecutive frames, providing real-time position updates. This is particularly important in areas without GPS signals or with unstable signals.

[0003] In existing technologies, existing laser odometry systems can achieve localization through point cloud matching or the extraction and matching of line and surface features. Point cloud matching utilizes the Iterative Closest Point (ICP) algorithm to solve for the optimal rigid body transformation by minimizing the distance between two sets of points, thereby achieving the corresponding point cloud matching. The extraction and matching of line and surface features involves first extracting geometric features, such as edge lines and planes, from the point cloud data, and then using these features for matching to estimate pose changes.

[0004] However, point cloud matching algorithms are sensitive to initial values ​​and prone to getting trapped in local optima, especially in complex and ever-changing urban traffic scenarios such as sharp turns and tunnel entrances / exits. This can lead to the accumulation of positioning errors. Furthermore, this approach has high computational complexity and is inefficient when processing large-scale point cloud data, limiting the system's response speed and failing to meet the real-time requirements of autonomous driving. Feature-based matching methods, while alleviating some computational burden, typically use KD-trees as the index structure during matching searches. However, KD-trees are not only inefficient in building and querying high-dimensional or large-scale point cloud data, but also difficult to perform efficient incremental updates. Summary of the Invention

[0005] In view of this, the present invention provides a positioning method and a laser odometry system based on a laser odometry system, with the aim of improving positioning efficiency, response speed, accuracy, supporting simple incremental updates, and real-time processing suitable for dynamic environments.

[0006] A first aspect of this application provides a positioning method based on a laser odometry system, the method comprising:

[0007] The current frame image and current voxel map of the vehicle are obtained, and a hash table of the current frame image and a hash table of the current voxel map are generated; wherein, the hash table of the current frame image includes at least one voxel of the current frame image and its voxel information; the hash table of the current voxel map includes at least one voxel of the current voxel map and its voxel information.

[0008] Based on the voxel information of each voxel in the current frame image and the voxel information of each voxel in the current voxel map, determine the voxel of the current voxel map that matches each voxel in the current frame image.

[0009] The voxel type of each voxel in the current frame image is determined based on the voxel information of each voxel in the current frame image.

[0010] Based on the voxel type of each voxel in the current frame image, at least one target voxel is selected from each voxel in the current frame image, and the residual of each target voxel is determined based on the voxel type of each target voxel and the voxel of the current voxel map that it matches.

[0011] The coordinates of each feature point within the voxel of the current voxel map matched with the target voxel are updated using the residuals of each target voxel, and the process returns to the step of obtaining the current frame image and the current voxel map of the vehicle, and generating a hash table of the current frame image and the hash table of the current voxel map.

[0012] Optionally, the step of acquiring the current frame image and the current voxel map of the vehicle, and generating a hash table for the current frame image and a hash table for the current voxel map, includes:

[0013] Obtain the current frame image and current voxel map of the vehicle;

[0014] For each image, the image is scanned to obtain a point cloud of the image, and multiple feature points are selected from each point in the point cloud; wherein, the image is the current frame image or the current voxel map;

[0015] Remove all points other than the aforementioned feature points from the point cloud of the image to obtain the target point cloud;

[0016] The target point cloud is divided into multiple initial voxels according to a preset resolution. Initial voxels without feature points are removed from each initial voxel to obtain at least one voxel.

[0017] For each voxel, the voxel position is calculated based on the coordinates of the center point within the voxel and the preset resolution.

[0018] The voxel position is used as the key to each feature point within the voxel;

[0019] Determine the feature point information for each of the aforementioned feature points, wherein the feature point information includes the centroid of the feature point, the total number of feature points, and the covariance of the feature points;

[0020] Voxel information is generated based on each feature point within the voxel, the key of each feature point, and the feature point information.

[0021] A hash table for the image is generated based on each voxel and its voxel information, wherein the hash table for the image is either the hash table for the current frame image or the hash table for the current voxel map.

[0022] Optionally, for each image, the image is scanned to obtain a point cloud of the image, and multiple feature points are selected from each point in the point cloud, including:

[0023] The image is scanned to obtain a point cloud of the image, wherein the point cloud includes multiple points;

[0024] Obtain the line bundle information of the point cloud, and decompose the point cloud into multiple line bundles based on the line bundle information; wherein each line bundle includes multiple points;

[0025] For each point, the curvature of the point is calculated based on the distances between the point and other points on the same line bundle.

[0026] Based on the curvature of each point, multiple feature points are selected from each point.

[0027] Optionally, determining the voxel type of each voxel in the current frame image based on the voxel information of each voxel in the current frame image includes:

[0028] For each voxel of the current frame image, construct the voxel covariance matrix of the current frame image based on the covariance of each feature point within the voxel;

[0029] Eigenvalue decomposition is performed on each feature in the covariance matrix to obtain the eigenvalues ​​and eigenvectors of each feature in the covariance matrix.

[0030] Determine whether the first eigenvalue of each feature in the covariance matrix is ​​greater than the second eigenvalue, wherein the first eigenvalue is the largest eigenvalue among the eigenvalues ​​of each feature in the covariance matrix, and the second eigenvalue is determined based on the eigenvalues ​​of each feature in the covariance matrix that are less than the first eigenvalue;

[0031] If it is greater than 1, the voxel type of the voxel in the current frame image is determined to be a line feature;

[0032] If it is not greater than, determine whether the eigenvalue that is only less than the first eigenvalue is greater than the third eigenvalue, wherein the third eigenvalue is determined based on the smallest eigenvalue among the eigenvalues ​​of each feature in the covariance matrix;

[0033] If it is greater than 1, the voxel type of the voxel in the current frame image is determined to be a surface feature;

[0034] If it is not greater than, the voxel type of the voxel in the current frame image is determined to be empty.

[0035] Optionally, determining the residual of each target voxel based on its voxel type and the voxels of the current voxel map it matches includes:

[0036] For each target voxel, determine whether the voxel type of the target voxel is a line feature or a surface feature;

[0037] If the target voxel is of the line feature type, the first virtual point and the second virtual point are established based on the feature vector of the feature with the largest eigenvalue in the covariance matrix of the target voxel.

[0038] Based on the distances between each feature point within the voxel of the current voxel map matched with the target voxel and the first and second virtual points, the residual of the target voxel is constructed.

[0039] If the target voxel is of surface feature type, the residual of the target voxel is constructed based on the eigenvector of the feature with the smallest eigenvalue in the covariance matrix of the target voxel and the centroid of the voxel in the current voxel map that matches the target voxel.

[0040] Optionally, before updating the coordinates of each feature point within the voxel of the current voxel map matched with the target voxel using the residuals of each of the target voxels, the method further includes:

[0041] Construct the Jacobian matrix based on the residuals of each target voxel;

[0042] Eigenvalue decomposition is performed on each feature of the Jacobian matrix to obtain the eigenvalues ​​of each feature of the Jacobian matrix.

[0043] Based on the eigenvalues ​​of each feature in the Jacobian matrix, determine whether the residuals of each target voxel satisfy the update conditions.

[0044] If satisfied, perform the step of updating the coordinates of each feature point within the voxel of the current voxel map matched with the target voxel using the residuals of each of the target voxels;

[0045] If the conditions are not met, the coordinates of each feature point within the voxel of the current voxel map will not be updated, and the process will return to the step of obtaining the current frame image and the current voxel map of the vehicle, and generating a hash table of the current frame image and the hash table of the current voxel map.

[0046] Optionally, the method further includes:

[0047] The feature point information of each feature point in each voxel in the hash table of the current voxel map is updated using the coordinates of each feature point in each voxel in the updated current voxel map.

[0048] A second aspect of this application provides a laser odometer system, the system comprising:

[0049] The acquisition unit is used to acquire the current frame image and the current voxel map of the vehicle, and generate a hash table for the current frame image and a hash table for the current voxel map; wherein, the hash table for the current frame image includes at least one voxel of the current frame image and its voxel information; the hash table for the current voxel map includes at least one voxel of the current voxel map and its voxel information.

[0050] The first determining unit is configured to determine the voxel of the current voxel map that matches the voxels of the current frame image based on the voxel information of each voxel of the current frame image and the voxel information of each voxel of the current voxel map.

[0051] The second determining unit is used to determine the voxel type of each voxel in the current frame image based on the voxel information of each voxel in the current frame image.

[0052] The residual generation unit is used to filter at least one target voxel from each voxel of the current frame image according to the voxel type of each voxel of the current frame image, and to determine the residual of each target voxel according to the voxel type of each target voxel and the voxel of the current voxel map that it matches.

[0053] The first update unit is used to update the coordinates of each feature point in the voxel of the current voxel map matched by the target voxel using the residuals of each target voxel, and then return to the execution acquisition unit.

[0054] Optionally, the acquisition unit includes:

[0055] The acquisition sub-unit is used to acquire the current frame image and current voxel map of the vehicle;

[0056] The first scanning subunit is used to scan the image for each image to obtain the point cloud of the image, and to select multiple feature points from each point in the point cloud; wherein the image is the current frame image or the current voxel map.

[0057] The first elimination subunit is used to eliminate points other than the feature points from the point cloud to obtain the target point cloud.

[0058] The second elimination subunit is used to divide the target point cloud into multiple initial voxels according to a preset resolution, and eliminate the initial voxels that do not have feature points from each of the initial voxels to obtain at least one voxel.

[0059] The first calculation subunit is used to calculate the voxel position of each voxel based on the coordinates of the center point within the voxel and the preset resolution.

[0060] The first determining subunit is used to use the voxel position as a key for each feature point within the voxel;

[0061] The second determining subunit is used to determine the feature point information of each of the feature points, wherein the feature point information includes the centroid of the feature point, the total number of feature points, and the covariance of the feature point;

[0062] The first generation subunit is used to generate the voxel information based on each feature point in the voxel, the key of each feature point and the feature point information.

[0063] The second generation subunit is used to generate a hash table of the image based on each voxel and its voxel information, wherein the hash table of the image is the hash table of the current frame image or the hash table of the current voxel map.

[0064] Optionally, the first scanning subunit includes:

[0065] The second scanning subunit is used to scan the image to obtain a point cloud of the image, wherein the point cloud includes multiple points;

[0066] A decomposition subunit is used to acquire the line bundle information of the point cloud and decompose the point cloud into multiple line bundles based on the line bundle information; wherein each line bundle includes multiple points.

[0067] The second calculation subunit is used to calculate the curvature of each point based on the distance between each point and other points belonging to the same line bundle as the point.

[0068] A filtering subunit is used to filter out multiple feature points from each of the points based on the curvature of each point.

[0069] This application discloses a positioning method and a laser odometry system based on a laser odometry system. The method acquires a current frame image and a current voxel map of a vehicle, and generates a hash table for the current frame image and a hash table for the current voxel map. The hash table for the current frame image includes at least one voxel of the current frame image and its voxel information; the hash table for the current voxel map includes at least one voxel of the current voxel map and its voxel information. Based on the voxel information of each voxel in the current frame image and the voxel information of each voxel in the current voxel map, the method determines the voxel of the current voxel map that matches each voxel in the current frame image; based on the current frame image… The process involves determining the voxel type of each voxel in the current frame image based on its voxel information; selecting at least one target voxel from the voxels in the current frame image based on their voxel types; determining the residual of each target voxel based on its voxel type and the voxels in the current voxel map it matches; updating the coordinates of each feature point within the voxel in the current voxel map it matches using the residuals of each target voxel; and returning to the step of acquiring the current frame image and current voxel map of the vehicle, and generating hash tables for the current frame image and current voxel map. Therefore, the technical solution provided in this application, by introducing voxels and hash tables, can provide fast index building and efficient spatial partitioning. It not only supports simple incremental updates but also is applicable to real-time processing in dynamic environments, thereby improving the corresponding positioning efficiency, response speed, and accuracy of the obtained voxel map, i.e., improving positioning accuracy. Attached Figure Description

[0070] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0071] Figure 1 A schematic flowchart illustrating a positioning method based on a laser odometry system provided in this application embodiment;

[0072] Figure 2 An example diagram of a first target point cloud provided in an embodiment of the present invention;

[0073] Figure 3 An example diagram of a first hash table provided in an embodiment of this application;

[0074] Figure 4 Example diagram of another hash table provided in the embodiments of this application;

[0075] Figure 5This is a schematic diagram of a laser odometer system provided in an embodiment of the present invention. Detailed Implementation

[0076] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0077] In this application, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes the element.

[0078] See Figure 1 This document illustrates a flowchart of a positioning method based on a laser odometer system, provided in an embodiment of this application. The method is applied to a laser odometer system, which can be a mobile platform equipped with a lidar sensor. This improves positioning accuracy and maintains real-time performance to a certain extent. The method specifically includes the following steps:

[0079] S101: Obtain the current frame image and the current voxel map of the vehicle, and generate a hash table for the current frame image and a hash table for the current voxel map, wherein the hash table for the current frame image includes at least one voxel of the current frame image and its voxel information; the hash table for the current voxel map includes at least one voxel of the current voxel map and its voxel information.

[0080] During the specific execution of step S101, the driving environment can be detected in real time during vehicle driving to obtain the current frame image of the driving environment. The current frame image is then scanned to obtain the point cloud of the current frame image. Based on the point cloud of the current frame image, voxels and their information are generated for the current frame image. This allows for the generation of a hash table for the current frame image based on each voxel and its information. Simultaneously, a current voxel map related to the vehicle can be acquired and scanned to obtain the point cloud of the current voxel map. Based on the point cloud of the current voxel map, voxels and their information are generated for the current voxel map. Finally, a hash table for the current voxel map is generated based on each voxel and its information.

[0081] It should be noted that voxel mapping is a three-dimensional spatial representation method that divides the environment into regular three-dimensional grids (voxels), with each cubic cell storing information about whether the area is occupied by obstacles.

[0082] Optionally, the process of acquiring the current frame image and current voxel map of the vehicle, and generating hash tables for the current frame image and current voxel map can be as follows: acquire the current frame image and current voxel map of the vehicle; for each image, scan the image to obtain the point cloud of the image, and select multiple feature points from each point in the point cloud of the image, wherein the image is the current frame image or the current voxel map; remove all points except the feature points from the point cloud of the image to obtain the target point cloud; divide the target point cloud into multiple initial voxels according to a preset resolution, and remove initial voxels from each initial voxel that do not contain feature points. A voxel is obtained, at least one voxel is obtained; for each voxel, the voxel position is calculated based on the coordinates of the center point within the voxel and the preset resolution; the voxel position is used as the key for each feature point within the voxel; the feature point information of each feature point is determined, wherein the feature point information of the feature point includes the centroid of the feature point, the total number of feature points, and the covariance of the feature points; the voxel information of the voxel is generated based on each feature point within the voxel, the key of each feature point, and the feature point information; a hash table of the image is generated based on each voxel and its voxel information, wherein the hash table of the image is the hash table of the current frame image or the hash table of the current voxel map.

[0083] It should be noted that after removing all points except for the feature points from the point cloud of the image to obtain the target point cloud, among the multiple initial voxels obtained by dividing the target point cloud, there may be empty initial voxels, that is, there are initial voxels that do not contain feature points. In order to obtain more accurate voxels, the initial voxels that do not contain feature points can be further removed.

[0084] As one implementation of this application, the process of scanning an image to obtain a point cloud and selecting multiple feature points from the points in the point cloud can be as follows: scanning the image to obtain a point cloud, wherein the point cloud includes multiple points; acquiring the line bundle information of the point cloud and decomposing the point cloud into multiple line bundles based on the line bundle information; wherein each line bundle includes multiple points; for each point, calculating the curvature of the point based on the distance between the point and other points belonging to the same line bundle; and selecting multiple feature points from each point based on the curvature of each point.

[0085] It should be noted that feature points can be line points or surface points.

[0086] In practical applications, for each point, the curvature of the point can be calculated by taking ten neighboring points on the line bundle to which it belongs and calculating the differences between the point and each of these other points. After obtaining the curvature of each point, points with curvature greater than a first curvature threshold are identified as line points, and points with curvature less than a second curvature threshold are identified as surface points. Points with curvature neither greater than the first curvature threshold nor less than the second curvature threshold are identified as noise points. Finally, all noise points in the point cloud are removed to obtain the corresponding target point cloud. The target point cloud includes multiple feature points, which can be line points or surface points. For example, the target point cloud can be as follows: Figure 2 As shown, Figure 2 The pink dots represent lines, and the green dots represent surfaces.

[0087] It should be noted that the second curvature threshold is smaller than the first curvature threshold.

[0088] In some embodiments, to effectively associate and quickly access a large amount of voxel information, this application introduces the concept of voxel hashing, i.e., constructing a corresponding hash table. Specifically, a corresponding preset resolution r can be preset so that the target point cloud is divided into a series of small cubes of fixed size according to the preset resolution, i.e., multiple initial voxels are obtained; initial voxels without feature points are removed from each initial voxel, i.e., empty initial voxels are removed, resulting in at least one voxel. Here, a voxel can be a voxel of the current frame image or a voxel of the current voxel map.

[0089] It should be noted that the side length of each voxel is equal to the preset resolution.

[0090] In some embodiments, each voxel can be identified by its position in space. Therefore, for each voxel, the center point of the voxel can be determined from each feature point of the voxel, and the voxel position can be calculated based on the position (coordinates) of the center point and the preset resolution. The voxel position is calculated as shown in formula (1).

[0091] (1)

[0092] in, It represents the voxel position, and can also represent the voxel's index value in the x, y, z directions; The coordinates are the center point of the voxel.

[0093] In some embodiments, since a hash table is a data structure, a specific key (the voxel's voxel position) can be used to quickly look up, insert, or delete the corresponding value (feature point information of each feature point within a voxel). Specifically, for each feature point within a voxel (a feature point within a voxel can also be called a voxel point), the voxel position of that voxel can be used as the key for that feature point; that is, the voxel's key is used as the key for each feature point within that voxel. The total number of feature points within each feature point in the voxel is counted, and the centroid and covariance of the feature points are calculated. Based on the covariance, centroid, and total number of feature points, feature point information for each feature point is generated. Finally, voxel information is generated based on each feature point within the voxel, the key of each feature point, and the feature point information. A hash table for the current frame is then generated based on each voxel and its voxel information. This hash table can be a hash table for the current frame image or a hash table for the current voxel map. The hash table for the current frame image can be as follows: Figure 3 As shown. The centroid and covariance of the feature points can be found in formulas (3) and (4) below.

[0094] It should be noted that, compared to the traditional method of storing point clouds, using the centroid, the covariance of feature points, and the total number of feature points to characterize the value of feature points (feature point information), and generating voxel information based on each feature point within a voxel, the key of each feature point, and the feature point information, and finally storing a hash table generated based on each voxel and its voxel information, can greatly reduce data storage requirements.

[0095] In other embodiments, the voxel positions can be combined to form a unique identifier as the voxel key. Specifically, the number of voxels in the x and y directions can be determined, and the voxel key can be calculated based on the voxel position, the number of voxels in the x direction and the number of voxels in the y direction. The specific calculation method is shown in formula (2).

[0096] (2)

[0097] Where Key is the voxel bond. voxel position Let x be the number of voxels in the x-direction. The number of voxels in the y-direction.

[0098] In summary, this application reduces data storage requirements and accelerates the search speed of nearest neighbor key points by dividing the corresponding point cloud into multiple voxels and constructing an efficient hash index based on the divided voxels.

[0099] S102: Based on the voxel information of each voxel in the current frame image and the voxel information of each voxel in the current voxel map, determine the voxel of the current voxel map that matches each voxel in the current frame image.

[0100] In the specific execution step S102, after obtaining the hash table of the current frame image and the hash table of the current voxel map, for each voxel in the hash table of the current frame image, the voxel information of that voxel can be obtained from the hash table of the current frame image. The voxel information of the voxel of the current frame map includes the keys of each feature point in the voxel of the current frame image. The hash table of the current voxel map is queried using the key of any feature point in the voxel of the current frame image to determine the key containing the same key as the feature point from the hash table of the current voxel map, and the voxel to which the feature point corresponding to the determined key belongs is determined as the voxel of the current voxel map that matches the voxel of the current frame image.

[0101] It should be noted that since the key of a feature point is the voxel position of the voxel to which the feature point belongs, it can be considered that the voxel position is used to query the hash table of the current voxel map to determine the voxel position that is the same as the voxel position in the hash table of the current voxel map, and the voxel corresponding to the voxel position that is the same as the voxel position in the hash table of the current voxel map is determined as the voxel of the current voxel map that matches the voxel. In other words, voxel matching between the current frame image and the current voxel map can be easily achieved through the voxel position.

[0102] S103: Determine the voxel type of each voxel in the current frame image based on the voxel information of each voxel in the current frame image.

[0103] In the specific execution of step S103, for each voxel of the current frame image, a covariance matrix of the voxel can be constructed based on its voxel information. This covariance matrix can then be analyzed to determine the voxel type. The voxel type of the voxel in the current frame image can be a line feature, a surface feature, or empty.

[0104] Optionally, the process of determining the voxel type of each voxel based on the voxel information of each voxel in the current frame image can be as follows: For each voxel in the current frame image, construct the voxel covariance matrix of the current frame image based on the covariance of each feature point within the voxel; perform eigenvalue decomposition on each feature within the covariance matrix to obtain the eigenvalues ​​and eigenvectors of each feature within the covariance matrix; determine whether the first eigenvalue of each feature within the covariance matrix is ​​greater than the second eigenvalue, where the first eigenvalue is the largest eigenvalue among the eigenvalues ​​of each feature within the covariance matrix, and the second eigenvalue is determined based on the eigenvalues ​​of each feature within the covariance matrix that are less than the first eigenvalue; if greater, determine that the voxel type of the current frame image is a line feature; if not greater, determine whether the eigenvalue that is only less than the first eigenvalue is greater than the third eigenvalue, where the third eigenvalue is determined based on the smallest eigenvalue among the eigenvalues ​​of each feature within the covariance matrix; if greater, determine that the voxel type of the current frame image is a surface feature; if not greater, determine that the voxel type of the current frame image is empty.

[0105] It should be noted that after obtaining the eigenvalues ​​of each feature within the covariance matrix, the eigenvalues ​​can be sorted from largest to smallest. Then, it is determined whether the first eigenvalue in the sorted sequence (the first eigenvalue) is greater than three times the second eigenvalue in the sorted sequence (the second eigenvalue). If so, the voxel type of the current frame image can be determined to be a line feature. If not, it is further determined whether the second eigenvalue in the sorted sequence is greater than three times the last eigenvalue in the sorted sequence (the third eigenvalue). If so, the voxel type of the current frame image can be determined to be a surface feature. Otherwise, the voxel type of the current frame image can be determined to be empty.

[0106] Furthermore, in this embodiment, after determining the voxel type of each voxel in the hash table of the current frame image, each voxel in the hash table can be labeled with a voxel type, such as... Figure 4 As shown.

[0107] S104: Select at least one target voxel from the voxels of the current frame image according to the voxel type of each voxel, and determine the residual of each target voxel according to the voxel type of each target voxel and the voxels of the current voxel map that they match.

[0108] In the specific execution of step 104, after obtaining the voxel type of each voxel in the current frame image, voxels with empty voxel types can be removed from the voxels in the current frame image to select at least one target voxel (for ease of differentiation, voxels with line or surface features are called target voxels). The target voxel's voxel type is either line or surface. For each target voxel, the residual of the target voxel is constructed based on the target voxel's voxel type, the target voxel's covariance matrix, and the voxels of the current voxel map that the target voxel matches. This can also be considered as constructing the residual of the voxels of the current voxel map that the target voxel matches.

[0109] Optionally, the process of determining the residual of each target voxel based on its voxel type and the voxels of the current voxel map it matches can be as follows: For each target voxel, determine whether its voxel type is a line feature or a surface feature; if the target voxel's voxel type is a line feature, establish a first virtual point and a second virtual point based on the eigenvector of the feature with the largest eigenvalue in the target voxel's covariance matrix; construct the residual of the target voxel based on the distances between each feature point in the voxel of the current voxel map it matches and the first and second virtual points; if the target voxel's voxel type is a surface feature, construct the residual of the target voxel based on the eigenvector of the feature with the smallest eigenvalue in the target voxel's covariance matrix and the centroid of the voxel in the current voxel map it matches.

[0110] In some embodiments, for each target voxel in the current frame image, if the voxel type of the target voxel is a line feature, the feature vector of the feature with the largest eigenvalue (first eigenvalue) in the covariance matrix of the target voxel can be used to represent the main direction of the target point cloud in the current frame image. Taking the average point of each feature point in the target voxel as the center, a preset distance (e.g., 10cm) is extended to the left and right along the line feature direction (main direction) to establish a first virtual point and a second virtual point. For each feature point in the voxel of the current voxel map that matches the target voxel, the distance between the feature point and the first virtual point and the second virtual point is calculated respectively, so as to construct the residual of the target voxel based on the distance between each feature point in the voxel of the current voxel map and the first virtual point and the second virtual point.

[0111] In other embodiments, when the target voxel of the current frame image is a surface feature, the eigenvector of the feature with the smallest eigenvalue in the covariance matrix of the target voxel represents the direction of the surface normal vector of the target point cloud of the current frame image. The dot product of the normal vector and the centroid of the voxel of the current voxel map that matches the target voxel is negative, and the distance from the current voxel map to the origin (centroid of the current frame image) is calculated using the normal square equation based on the negative value. Finally, the residual of the target voxel is constructed using the obtained distance.

[0112] S105: Update the coordinates of each feature point within the voxel of the current voxel map matched with the target voxel using the residuals of each target voxel.

[0113] In the specific execution of step S105, for each target voxel of the current frame image, after obtaining the residual of the target voxel, the coordinates of each feature point in the voxel of the current voxel map matched by the target voxel can be adjusted using the residual of the target voxel, so as to update the coordinates of each feature point of the current voxel map.

[0114] It should be noted that after updating the coordinates of each feature point in the voxel of the current voxel map matched with the target voxel using the residuals of each target voxel, the process can return to obtain the current frame image and the current voxel map of the vehicle, and generate the hash table of the current frame image and the hash table of the current voxel map, i.e., execute step S101.

[0115] It should also be noted that this application optimizes the coordinates of feature points based on the corresponding residuals, which can enhance the system performance and improve the accuracy of the obtained voxel map.

[0116] Furthermore, in this embodiment of the application, in order to ensure the accuracy of the obtained voxel map, that is, to ensure the output of reliable positioning results, the residuals of each target voxel can be further verified before updating the coordinates of each feature point in the voxel of the current voxel map matched with the target voxel using the residuals of each target voxel in the current frame image.

[0117] Optionally, construct a Jacobian matrix based on the residuals of each target voxel; perform eigenvalue decomposition on each feature of the Jacobian matrix to obtain the eigenvalues ​​of each feature; determine whether the residuals of each target voxel satisfy the update conditions based on the eigenvalues ​​of each feature in the Jacobian matrix; if satisfied, perform the step of updating the coordinates of each feature point in the voxel of the current voxel map matched with the target voxel using the residuals of each target voxel; if not satisfied, do not update the coordinates of each feature point in the voxel of the current voxel map, and return to perform the step of obtaining the current frame image and the current voxel map of the vehicle, and generating the hash table of the current frame image and the hash table of the current voxel map.

[0118] In practical applications, research has shown that in laser matching or SLAM problems, if the condition number of the Jacobian matrix is ​​very large, or the smallest eigenvalue among the eigenvalues ​​of each feature in the Jacobian matrix is ​​close to zero, it may be because the environmental features are insufficient to provide enough information to determine the accuracy of the localization, or the acquired image contains a lot of noise. In this case, in order to ensure the accuracy of the obtained localization, the coordinates of the feature points within the voxels of the current voxel map can be updated using the residuals of the target voxels.

[0119] Specifically, the ratio of the largest to the smallest eigenvalue among the eigenvalues ​​of each feature within a voxel in the current voxel map can be calculated to obtain the condition number of the Jacobian matrix. It is then determined whether this condition number is greater than a first preset threshold, or whether the smallest eigenvalue among the eigenvalues ​​of each feature within a voxel in the current voxel map is less than a second preset threshold. If the condition number is greater than the first preset threshold, or the smallest eigenvalue is less than the second preset threshold, it indicates that each target voxel meets the update conditions. The residuals of each target voxel can then be used to update the coordinates of each feature point within the voxel of the current voxel map that matches the target voxel. If the condition number is not greater than the first preset threshold, or the smallest eigenvalue is not less than the second preset threshold, it indicates that the residuals of each target voxel do not meet the update conditions.

[0120] It should be noted that the first preset threshold is greater than the second preset threshold, and the second preset threshold is close to zero.

[0121] It should also be noted that the stability of the residuals of each target voxel is determined by using the condition number and the smallest eigenvalue of the Jacobian matrix, thereby determining the accuracy of the final voxel map (localization result).

[0122] Furthermore, in this embodiment, the feature point information of each feature point in each voxel within the current voxel map can be updated using the coordinates of each feature point within each voxel of the updated current voxel map.

[0123] In practical applications, when a feature point is inserted into the current voxel map, the centroid, covariance, and total number of feature points of that feature point in the hash table of the current voxel map can be updated (in fact, the total number of feature points remains unchanged). Specifically, the mean of the previously inserted feature point is obtained, and the mean of the currently inserted feature point is calculated based on the mean of the previously inserted feature point, the currently inserted feature point, and the total number of currently inserted feature points. The specific calculation formula is shown in formula (3). The new covariance of the currently inserted feature point is calculated using the mean of the currently inserted feature point, the mean of the previously inserted feature point, the total number of currently inserted feature points, and the currently inserted feature point. The specific calculation method is shown in formula (4). Finally, the mean of the currently inserted feature point is used as the centroid of the currently inserted feature point so that the centroid and the new covariance can be used to update the centroid and covariance of the feature point in the hash table of the current voxel map.

[0124] (3)

[0125] in, The mean of the currently inserted feature points. Let be the mean of the previously inserted feature points, and n be the total number of currently inserted feature points. This refers to the currently inserted feature point.

[0126] (4)

[0127] in, Let be the mean of the previously inserted feature points, and n be the total number of currently inserted feature points. For the currently inserted feature point, The covariance of the currently inserted feature point. It is the covariance of the previously inserted feature point.

[0128] It should be noted that if the currently inserted feature point is the first inserted feature point, then the mean of the previously inserted feature point is the initial mean. The initial mean can be set according to the actual application, and this embodiment of the application does not limit it.

[0129] This application discloses a positioning method based on a laser odometry system. The method acquires a current frame image and a current voxel map of a vehicle, and generates a hash table for the current frame image and a hash table for the current voxel map. The hash table for the current frame image includes at least one voxel of the current frame image and its voxel information; the hash table for the current voxel map includes at least one voxel of the current voxel map and its voxel information. Based on the voxel information of each voxel in the current frame image and the voxel information of each voxel in the current voxel map, the method determines the voxel of the current voxel map that matches each voxel in the current frame image; based on the voxel information of each voxel in the current frame image... The voxel information determines the voxel type of each voxel in the current frame image; based on the voxel type of each voxel in the current frame image, at least one target voxel is selected from the voxels of the current frame image, and the residual of each target voxel is determined based on the voxel type of each target voxel and the voxels of the current voxel map that match it; the coordinates of each feature point in the voxel of the current voxel map that matches the target voxel are updated using the residuals of each target voxel, and the process returns to the step of obtaining the current frame image and the current voxel map of the vehicle, and generating a hash table for the current frame image and a hash table for the current voxel map. Therefore, the technical solution provided in this application, by introducing voxels and hash tables, can provide fast index building and efficient spatial partitioning, supporting not only simple incremental updates but also real-time processing in dynamic environments, thereby improving the corresponding positioning efficiency, response speed, and accuracy of the obtained voxel map, i.e., improving positioning accuracy.

[0130] Based on the positioning method based on the laser odometer system provided in the above embodiments of this application, correspondingly, the embodiments of this application also provide a laser odometer system, such as... Figure 5 As shown, the laser odometry system includes:

[0131] The acquisition unit 51 is used to acquire the current frame image and the current voxel map of the vehicle, and generate a hash table of the current frame image and a hash table of the current voxel map; wherein, the hash table of the current frame image includes at least one voxel of the current frame image and its voxel information; the hash table of the current voxel map includes at least one voxel of the current voxel map and its voxel information.

[0132] The first determining unit 52 is used to determine the voxels of the current voxel map that match the voxels of the current frame image based on the voxel information of each voxel of the current frame image and the voxel information of each voxel of the current voxel map.

[0133] The second determining unit 53 is used to determine the voxel type of each voxel in the current frame image based on the voxel information of each voxel in the current frame image.

[0134] The residual generation unit 54 is used to filter at least one target voxel from each voxel of the current frame image according to the voxel type of each voxel of the current frame image, and to determine the residual of each target voxel according to the voxel type of each target voxel and the voxel of the current voxel map that it matches.

[0135] The first update unit 55 is used to update the coordinates of each feature point in the voxel of the current voxel map matched with the target voxel using the residuals of each target voxel, and then return to the execution acquisition unit.

[0136] The specific principles and execution processes of each unit in the laser odometer system disclosed in the above embodiments of this application are the same as the positioning method based on the laser odometer system disclosed in the above embodiments of this application. Please refer to the corresponding parts of the positioning method based on the laser odometer system disclosed in the above embodiments of this application, and they will not be repeated here.

[0137] This application provides a laser odometry system that acquires a current frame image and a current voxel map of a vehicle, and generates a hash table for the current frame image and a hash table for the current voxel map. The hash table for the current frame image includes at least one voxel of the current frame image and its voxel information; the hash table for the current voxel map includes at least one voxel of the current voxel map and its voxel information. Based on the voxel information of each voxel in the current frame image and the voxel information of each voxel in the current voxel map, the system determines the voxel of the current voxel map that matches each voxel in the current frame image; based on the voxel information of each voxel in the current frame image... The voxel information determines the voxel type of each voxel in the current frame image; based on the voxel type of each voxel in the current frame image, at least one target voxel is selected from the voxels of the current frame image, and the residual of each target voxel is determined based on the voxel type of each target voxel and the voxels of the current voxel map that match it; the coordinates of each feature point in the voxel of the current voxel map that matches the target voxel are updated using the residuals of each target voxel, and the process returns to the step of obtaining the current frame image and the current voxel map of the vehicle, and generating a hash table for the current frame image and a hash table for the current voxel map. Therefore, the technical solution provided in this application, by introducing voxels and hash tables, can provide fast index building and efficient spatial partitioning, supporting not only simple incremental updates but also real-time processing in dynamic environments, thereby improving the corresponding positioning efficiency, response speed, and accuracy of the obtained voxel map, i.e., improving positioning accuracy.

[0138] Optionally, the acquisition unit includes:

[0139] The acquisition sub-unit is used to acquire the current frame image and current voxel map of the vehicle;

[0140] The first scanning subunit is used to scan the image for each image, obtain the point cloud of the image, and select multiple feature points from each point in the point cloud; wherein the image is the current frame image or the current voxel map.

[0141] The first elimination subunit is used to remove points other than each feature point from the point cloud to obtain the target point cloud;

[0142] The second elimination subunit is used to divide the target point cloud into multiple initial voxels according to a preset resolution, and eliminate the initial voxels that do not have feature points from each initial voxel to obtain at least one voxel.

[0143] The first calculation subunit is used to calculate the voxel position for each voxel based on the coordinates of the center point within the voxel and the preset resolution.

[0144] The first determining subunit is used to use the voxel position as the key to each feature point within the voxel;

[0145] The second determining subunit is used to determine the feature point information of each feature point, wherein the feature point information includes the centroid of the feature point, the total number of feature points, and the covariance of the feature points;

[0146] The first generation subunit is used to generate voxel information based on each feature point in the voxel, the key of each feature point, and the feature point information.

[0147] The second generation subunit is used to generate a hash table of the image based on each voxel and its voxel information, wherein the hash table of the image is the hash table of the current frame image or the hash table of the current voxel map.

[0148] Optionally, the first scan unit includes:

[0149] The second scanning subunit is used to scan the image to obtain a point cloud of the image, wherein the point cloud includes multiple points;

[0150] The decomposition subunit is used to obtain the line bundle information of the point cloud and decompose the point cloud into multiple line bundles based on the line bundle information; wherein each line bundle includes multiple points.

[0151] The second calculation subunit is used to calculate the curvature of each point based on the distance between the point and other points on the same line bundle as the point.

[0152] The filtering sub-unit is used to filter out multiple feature points from each point based on the curvature of each point.

[0153] Optionally, the second determining unit includes:

[0154] The covariance matrix construction sub-unit is used to construct the covariance matrix of the voxel of the current frame image for each voxel based on the covariance of each feature point within the voxel of the current frame image.

[0155] The eigenvalue decomposition subunit is used to perform eigenvalue decomposition on each feature in the covariance matrix to obtain the eigenvalues ​​and eigenvectors of each feature in the covariance matrix.

[0156] The first judgment subunit is used to perform eigenvalue decomposition on each feature in the covariance matrix to obtain the eigenvalues ​​and eigenvectors of each feature in the covariance matrix.

[0157] The third determining subunit is used to determine the voxel type of the voxel in the current frame image as a line feature if the value is greater than 0.

[0158] The second judgment subunit is used to determine whether the eigenvalue that is only less than the first eigenvalue is greater than the third eigenvalue if it is not greater than the first eigenvalue. The third eigenvalue is determined based on the smallest eigenvalue among the eigenvalues ​​of each eigenvalue in the covariance matrix.

[0159] The fourth determining subunit is used to determine the voxel type of the voxel in the current frame image as a surface feature if the value is greater than 0.

[0160] The fifth determining subunit is used to determine that the voxel type of the voxel in the current frame image is empty if it is not greater than 0.

[0161] Optionally, the residual generation unit includes:

[0162] The third judgment subunit is used to determine whether the voxel type of each target voxel is a line feature or a surface feature.

[0163] Virtual points are used to establish sub-units. If the target voxel type is a line feature, the first virtual point and the second virtual point are established based on the eigenvector of the feature with the largest eigenvalue in the covariance matrix of the target voxel.

[0164] The first residual construction subunit is used to construct the residual of the target voxel based on the distances between each feature point in the voxel of the current voxel map matched with the target voxel and the first virtual point and the second virtual point, respectively.

[0165] The second residual construction subunit is used to construct the residual of the target voxel if the voxel type is a surface feature, based on the eigenvector of the feature with the smallest eigenvalue in the covariance matrix of the target voxel and the centroid of the voxel in the current voxel map that matches the target voxel.

[0166] Optionally, the laser odometry system provided in this application further includes:

[0167] Jacobian matrix construction unit, used to construct Jacobian matrix based on the residuals of each target voxel;

[0168] The judgment unit is used to perform eigenvalue decomposition on each feature of the Jacobian matrix to obtain the eigenvalues ​​of each feature of the Jacobian matrix, and to determine whether the residuals of each target voxel meet the update conditions based on the eigenvalues ​​of each feature in the Jacobian matrix.

[0169] The first execution unit is used to execute the first update unit if the condition is met.

[0170] The second condition unit is used to prevent updating the coordinates of each feature point within each voxel of the current voxel map if the condition is not met, and then return to the execution acquisition unit.

[0171] Optionally, the laser odometry system provided in this application further includes:

[0172] The second update unit is used to update the feature point information of each feature point in each voxel in the hash table of the current voxel map using the coordinates of each feature point in each voxel in the updated current voxel map.

[0173] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on its differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. Units described as separate components may or may not be physically separate. Components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0174] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0175] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0176] The above are merely preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A positioning method based on a laser odometry system, characterized in that, The method comprises: acquiring a current frame image and a current voxel map of a vehicle, and generating a hash table of the current frame image and a hash table of the current voxel map; wherein the hash table of the current frame image comprises at least one voxel of the current frame image and voxel information of the voxel; and the hash table of the current voxel map comprises at least one voxel of the current voxel map and voxel information of the voxel; determining, according to the voxel information of each voxel of the current frame image and the voxel information of each voxel of the current voxel map, a voxel of the current voxel map matched with each voxel of the current frame image; determining a voxel type of each voxel of the current frame image according to the voxel information of each voxel of the current frame image; screening at least one target voxel from each voxel of the current frame image according to the voxel type of each voxel of the current frame image, and determining a residual of each target voxel according to the voxel type of each target voxel and the voxel of the current voxel map matched with the target voxel; updating the coordinates of each feature point in the voxel of the current voxel map matched with the target voxel by using the residual of each target voxel, and returning to the step of acquiring the current frame image and the current voxel map of the vehicle, and generating the hash table of the current frame image and the hash table of the current voxel map.

2. The method of claim 1, wherein, The acquiring the current frame image and the current voxel map of the vehicle, and generating the hash table of the current frame image and the hash table of the current voxel map comprises: acquiring the current frame image and the current voxel map of the vehicle; for each image, scanning the image to obtain a point cloud of the image, and screening a plurality of feature points from each point of the point cloud; wherein the image is a current frame image or a current voxel map; eliminating other points except each feature point from the point cloud of the image to obtain a target point cloud; dividing the target point cloud into a plurality of initial voxels according to a preset resolution, and eliminating an initial voxel without a feature point from each initial voxel to obtain at least one voxel; for each voxel, calculating a voxel position of the voxel according to the coordinates of a center point in the voxel and the preset resolution; taking the voxel position as a key of each feature point in the voxel; determining feature point information of each feature point, wherein the feature point information comprises a centroid of the feature point, a total number of feature points, and a covariance of the feature point; generating voxel information of the voxel according to each feature point in the voxel, the key and the feature point information of each feature point; generating a hash table of the image according to each voxel and the voxel information of each voxel, wherein the hash table of the image is a hash table of the current frame image or a hash table of the current voxel map.

3. The method of claim 2, wherein, for each image, scanning the image to obtain a point cloud of the image, and screening a plurality of feature points from each point of the point cloud, comprises: scanning the image to obtain a point cloud of the image, wherein the point cloud comprises a plurality of points; Acquire beam information of the point cloud, and decompose the point cloud into a plurality of beams according to the beam information of the point cloud; wherein each beam comprises a plurality of points; For each point, calculate the curvature of the point according to the distance between the point and each other point belonging to the same beam; According to the curvature of each point, a plurality of feature points are selected from each point.

4. The method of claim 2, wherein, The method further comprises: For each voxel of the current frame image, a covariance matrix of the voxel of the current frame image is constructed according to the covariance of each feature point in the voxel of the current frame image; Eigenvalue decomposition is performed on each feature in the covariance matrix to obtain the eigenvalue and eigenvector of each feature in the covariance matrix; Determine whether the first eigenvalue is greater than the second eigenvalue, wherein the first eigenvalue is the largest eigenvalue of each eigenvalue of the covariance matrix, and the second eigenvalue is determined according to the eigenvalue smaller than the first eigenvalue among the eigenvalues of each eigenvalue of the covariance matrix; If greater, determine that the voxel type of the voxel of the current frame image is a line feature; If not greater, determine whether the eigenvalue smaller than the first eigenvalue is greater than the third eigenvalue, wherein the third eigenvalue is determined according to the smallest eigenvalue among the eigenvalues of each eigenvalue of the covariance matrix; If greater, determine that the voxel type of the voxel of the current frame image is a surface feature; If not greater, determine that the voxel type of the voxel of the current frame image is empty.

5. The method of claim 4, wherein, The method further comprises: For each target voxel, determine whether the voxel type of the target voxel is a line feature or a surface feature; If the voxel type of the target voxel is a line feature, a first virtual point and a second virtual point are established according to the eigenvector of the feature with the largest eigenvalue in the covariance matrix of the target voxel; According to the distance between each feature point in the voxel of the current voxel map matched by the target voxel and the first virtual point and the second virtual point, the residual error of the target voxel is constructed; If the voxel type of the target voxel is a surface feature, the residual error of the target voxel is constructed according to the eigenvector of the feature with the smallest eigenvalue in the covariance matrix of the target voxel and the centroid of the voxel of the current voxel map matched by the target voxel.

6. The method of claim 1, wherein, Before updating the coordinates of each feature point in the voxel of the current voxel map matched by the target voxel using the residual error of each target voxel, the method further comprises: Construct a Jacobian matrix according to the residual error of each target voxel; Eigenvalue decomposition is performed on each feature of the Jacobian matrix to obtain the eigenvalue of each feature of the Jacobian matrix; According to the eigenvalue of each feature in the Jacobian matrix, determine whether the residual error of each target voxel satisfies the update condition; if the condition is met, updating coordinates of each feature point in a voxel of a current voxel map matched with each of the target voxels using a residual of each of the target voxels; if the condition is not met, not updating the coordinates of each feature point in the voxel of the current voxel map, and returning to perform the step of obtaining a current frame image and a current voxel map of a vehicle, and generating a hash table of the current frame image and a hash table of the current voxel map.

7. The method of claim 1, wherein, The method further comprises: updating feature point information of each feature point in each voxel in the hash table of the current voxel map using the coordinates of each feature point in each voxel in the updated current voxel map.

8. A laser odometry system, characterized by, The system comprises: an obtaining unit, configured to obtain a current frame image and a current voxel map of a vehicle, and generate a hash table of the current frame image and a hash table of the current voxel map; wherein the hash table of the current frame image comprises at least one voxel of the current frame image and voxel information of the voxel; and the hash table of the current voxel map comprises at least one voxel of the current voxel map and voxel information of the voxel; a first determining unit, configured to determine, according to the voxel information of each voxel of the current frame image and the voxel information of each voxel of the current voxel map, a voxel of the current voxel map matched with each voxel of the current frame image; a second determining unit, configured to determine, according to the voxel information of each voxel of the current frame image, a voxel type of each voxel of the current frame image; a residual generating unit, configured to select at least one target voxel from each voxel of the current frame image according to the voxel type of each voxel of the current frame image, and determine a residual of each target voxel according to the voxel type of each target voxel and the voxel of the current voxel map matched with the target voxel; a first updating unit, configured to update coordinates of each feature point in a voxel of a current voxel map matched with each of the target voxels using a residual of each of the target voxels, and return to perform the obtaining unit.

9. The system of claim 8, wherein, The obtaining unit comprises: an obtaining subunit, configured to obtain a current frame image and a current voxel map of a vehicle; a first scanning subunit, configured to, for each image, scan the image to obtain a point cloud of the image, and select a plurality of feature points from each point of the point cloud; wherein the image is a current frame image or a current voxel map; a first eliminating subunit, configured to eliminate, from the point cloud, points other than each feature point to obtain a target point cloud; a second eliminating subunit, configured to divide the target point cloud into a plurality of initial voxels according to a preset resolution, and eliminate an initial voxel in which no feature point exists from each initial voxel to obtain at least one voxel; a first calculating subunit, configured to, for each voxel, calculate a voxel position of the voxel according to a coordinate of a center point in the voxel and the preset resolution; a first determining subunit, configured to take the voxel position as a key of each feature point in the voxel. The second determining sub-unit is configured to determine feature point information of each feature point, wherein the feature point information comprises a centroid of the feature point, a total number of feature points, and a covariance of the feature point; The first generating sub-unit is configured to generate the voxel information according to each feature point in the voxel, the key of each feature point, and the feature point information; The second generating sub-unit is configured to generate a hash table of the image according to each voxel and the voxel information thereof, wherein the hash table of the image is a hash table of the current frame image or a hash table of the current voxel map.

10. The system of claim 9, wherein, The first scanning sub-unit comprises: The second scanning sub-unit is configured to scan the image to obtain a point cloud of the image, wherein the point cloud comprises a plurality of points; The decomposing sub-unit is configured to obtain beam information of the point cloud, and decompose the point cloud into a plurality of beams according to the beam information of the point cloud; wherein each beam comprises a plurality of points; The second calculating sub-unit is configured to calculate the curvature of each point according to the distance between each other point on the same beam as the point for each point; The screening sub-unit is configured to screen a plurality of feature points from each point according to the curvature of each point.

Images