A method and apparatus for simultaneous localization and mapping

Abstract

The application discloses a method and device for simultaneous localization and mapping, to solve the problems of localization drift and instability in the prior art visual SLAM localization method. The method comprises the following steps: a SLAM device acquires perception data of an environment, wherein the perception data comprises image data, point cloud data and inertial navigation data; a 3D sub-map of the environment is established according to the image data and the inertial navigation data, and a 3D global map of the environment is established according to the point cloud data and the inertial navigation data; a plurality of features are extracted from the 3D sub-map and the 3D global map respectively; and the positions of the features in the 3D sub-map are optimized according to the plurality of features extracted from the 3D sub-map and the 3D global map respectively, so as to obtain a 3D sub-map used for providing localization information.

Description

[0001] This application is a divisional application of Chinese patent application No. 201810508563.X, filed on May 24, 2018, entitled "A method and apparatus for simultaneous positioning and mapping". Technical Field

[0002] This invention relates to the field of Simultaneous Localization and Mapping (SLAM), and particularly to a method and apparatus for simultaneous localization and mapping. Background Technology

[0003] In recent years, intelligent vehicles, or autonomous vehicles, have been increasingly adopted. A key challenge in the various applications of autonomous vehicles is achieving stable and smooth positioning in a large-scale outdoor environment. For land vehicles operating outdoors, such as an autonomous vehicle, the primary sensor for acquiring positioning information is the Global Positioning System (GPS). However, a well-known problem is that GPS satellite signals are unstable in urban environments, and their accuracy can be affected by multipath effects caused by factors such as tall buildings or trees blocking the signal.

[0004] In response to this, many assisted positioning methods have been developed to compensate for the inability to locate in urban environments using GPS signals.

[0005] Visual SLAM-based methods model a map and use an inertial navigation system to locate the user based on the constructed map. However, existing visual SLAM-based localization methods exhibit drift after prolonged operation, meaning the difference between the located position and the actual position increases over time.

[0006] It is evident that existing visual SLAM localization methods suffer from localization drift and instability. Summary of the Invention

[0007] In view of this, embodiments of the present invention provide a method and apparatus for simultaneous localization and mapping to solve the problems of localization drift and unstable localization in the prior art of visual SLAM.

[0008] According to one aspect of this application, a method for simultaneous localization and mapping is provided, comprising:

[0009] The SLAM device acquires perception data of an environment, which includes image data, point cloud data, and inertial navigation data.

[0010] A 3D sub-map of the environment is built based on image data and inertial navigation data, and a 3D global map of the environment is built based on point cloud data and inertial navigation data.

[0011] Multiple features are extracted from the 3D sub-map and the 3D global map respectively;

[0012] Based on multiple features extracted from the 3D sub-map and the 3D global map respectively, the positions of the features in the 3D sub-map are optimized to obtain a 3D sub-map used to provide positioning information.

[0013] According to another aspect of this application, an apparatus for simultaneous localization and mapping is provided, comprising: a processor and at least one memory, wherein the at least one memory stores at least one machine-executable instruction, and the processor reads and executes the at least one machine-executable instruction to implement:

[0014] Acquire perception data of an environment, including image data, point cloud data, and inertial navigation data;

[0015] A 3D sub-map of the environment is built based on image data and inertial navigation data, and a 3D global map of the environment is built based on point cloud data and inertial navigation data.

[0016] Multiple features are extracted from the 3D sub-map and the 3D global map respectively;

[0017] Based on multiple features extracted from the 3D sub-map and the 3D global map respectively, the positions of the features in the 3D sub-map are optimized to obtain a 3D sub-map used to provide positioning information.

[0018] According to another aspect of this application, an apparatus for simultaneous positioning and mapping is provided, comprising:

[0019] The data acquisition module is used to acquire perception data of an environment, including image data, point cloud data, and inertial navigation data.

[0020] The mapping module is used to create a 3D sub-map of the environment based on image data and inertial navigation data, and to create a 3D global map of the environment based on point cloud data and inertial navigation data.

[0021] The positioning module is used to extract multiple features from the 3D sub-map and the 3D global map respectively; based on the multiple features extracted from the 3D sub-map and the 3D global map respectively, the position of the features in the 3D sub-map is optimized to obtain the 3D sub-map used to provide positioning information.

[0022] According to the technical solution provided in the embodiments of this application, the SLAM device establishes a 3D sub-map of the environment based on image data and inertial navigation data, and establishes a 3D global map of the environment based on point cloud data and inertial navigation data. Multiple features are extracted from both the 3D sub-map and the 3D global map, and the positions of the features in the 3D sub-map are optimized based on the extracted features to obtain a 3D sub-map used to provide positioning information. The 3D global map is established based on point cloud data. The physical measurement information of point cloud data is more accurate than that of image data. Optimizing the positions of the features in the 3D sub-map based on the extracted features enables the 3D sub-map to have more accurate physical measurement information. Compared with the existing visual SLAM technology that only uses image data for positioning, it can provide more accurate position information; thus, it can solve the problems of positioning drift and unstable positioning existing in the existing visual SLAM technology. Attached Figure Description

[0023] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the embodiments of the invention to explain the invention and do not constitute a limitation thereof.

[0024] Figure 1 A flowchart illustrating the method for simultaneous localization and mapping provided in this application embodiment;

[0025] Figure 2 In order to be in Figure 1 The flowchart for aligning the 3D sub-map and the 3D global map after step 103;

[0026] Figure 3 for Figure 1 Flowchart of step 105 in the middle;

[0027] Figure 4 for Figure 1 The processing flowchart for step 107;

[0028] Figure 5 for Figure 4 Flowchart of step 1071 in the middle;

[0029] Figure 6 for Figure 4 Flowchart of step 1072 in the middle;

[0030] Figure 7 Structural block diagram of the device for simultaneous positioning and mapping provided in the embodiments of this application;

[0031] Figure 8 Another structural block diagram of the apparatus for simultaneous positioning and mapping provided in the embodiments of this application. Detailed Implementation

[0032] To enable those skilled in the art to better understand the technical solutions of this invention, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of this invention.

[0033] To address the issues of positioning drift and instability in existing visual SLAM technologies, this application provides a SLAM method and apparatus to solve these problems. According to the technical solution provided in this application, the SLAM apparatus establishes a 3D sub-map of the environment based on image data and inertial navigation data, and establishes a 3D global map of the environment based on point cloud data and inertial navigation data. Multiple features are extracted from both the 3D sub-map and the 3D global map, and the positions of features in the 3D sub-map are optimized based on the extracted features to obtain a 3D sub-map used to provide positioning information. The 3D global map is established based on point cloud data. The physical measurement information of point cloud data is more accurate than that of image data. Optimizing the positions of features in the 3D sub-map based on the extracted features enables the 3D sub-map to have more accurate physical measurement information. Compared to existing visual SLAM technologies that rely solely on image data for positioning, this provides more accurate positional information, thereby solving the positioning drift and instability problems inherent in existing visual SLAM technologies.

[0034] The above is the core idea of ​​the present invention. In order to enable those skilled in the art to better understand the technical solutions in the embodiments of the present invention, and to make the above-mentioned objectives, features and advantages of the embodiments of the present invention more obvious and understandable, the technical solutions in the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0035] Figure 1 The diagram illustrates the processing flow of the simultaneous localization and mapping method provided in this application, including:

[0036] Step 101: The SLAM device acquires environmental perception data, which includes image data, point cloud data, and inertial navigation data.

[0037] Among them, image data can be acquired through at least one camera, point cloud data can be acquired through LiDAR, and inertial navigation data can be pose data acquired through Global Navigation Satellite System-Inertial Measurement Unit (GNSS-IMU);

[0038] Before the SLAM device acquires sensing data, the relevant sensors and cameras can be calibrated and time-synchronized. The calibration and time synchronization methods can be used before or after this application, and this application does not make specific limitations.

[0039] Step 103: Create a 3D sub-map of the environment based on image data and inertial navigation data, and create a 3D global map of the environment based on point cloud data and inertial navigation data;

[0040] The 3D sub-map can be built using visual SLAM technology based on image data and inertial navigation data, and the 3D global map can be built using LiDAR mapping technology based on point cloud data and inertial navigation data. The methods for building the 3D sub-map and the 3D global map can be any map building methods prior to this application or map building methods after this application, and this application does not impose strict limitations on them.

[0041] Step 105: Extract multiple features from the 3D sub-map and the 3D global map respectively;

[0042] Step 107: Based on the multiple features extracted from the 3D sub-map and the 3D global map respectively, optimize the position of the features in the 3D sub-map to obtain the 3D sub-map used to provide positioning information.

[0043] according to Figure 1 The method shown uses point cloud data to create a 3D global map. The physical measurement information of point cloud data is more accurate than that of image data. By optimizing the position of features in the 3D sub-map based on the extracted features, the 3D sub-map can have more accurate physical measurement information. Compared with the existing visual SLAM technology, which only uses image data for positioning, it can provide more accurate position information. This can solve the problems of positioning drift and unstable positioning in the existing visual SLAM technology.

[0044] The following is about Figure 1 The processing procedure will be explained in detail.

[0045] exist Figure 1 Based on the method shown, in some embodiments, after establishing the 3D sub-map and 3D global map in step 103 above, the 3D sub-map is further aligned with the 3D global map. This process includes, for example: Figure 2 The process shown:

[0046] Step 103a: Select at least one point in the 3D sub-map;

[0047] The selected point can be any point in the 3D sub-map, such as selecting a center point or multiple feature points, or selecting a center point and multiple feature points.

[0048] Step 103b: Determine the longitude, latitude, and altitude data of at least one selected point based on the inertial navigation data;

[0049] Step 103c: Based on the longitude, latitude, and height data of at least one selected point, convert the coordinate system of the 3D sub-map into the coordinate system of the 3D global map.

[0050] The coordinate system transformation can be performed using methods prior to or subsequent to this application; no strict limitation is imposed here. Aligning the 3D submap with the 3D global map makes the positional information of the 3D submap more accurate, providing a better foundation for subsequent processing.

[0051] Figure 3 The detailed processing procedure of step 105 above is shown in the figure, including:

[0052] Step 1051: Voxelize the 3D sub-map and the 3D global map according to the predetermined voxel size to obtain a 3D sub-map and a 3D global map that each include multiple voxels.

[0053] Voxelization is the process of dividing 3D space into a three-dimensional mesh according to a predetermined voxel size, with each three-dimensional mesh being a voxel.

[0054] Step 1052: Determine the 3D points included in each voxel in the 3D sub-map and the 3D points included in each voxel in the 3D global map;

[0055] Among them, the 3D sub-map is built based on image data. By projecting the features or textures in the image data onto 3D space, 3D points that express the features or textures can be obtained.

[0056] Step 1053: Determine the features expressed by the distribution of 3D points in each voxel;

[0057] Features are objects represented by a collection of 3D points, which can include lines, curves, planes, surfaces, etc.

[0058] In this application, a probabilistic model can be used to estimate the features expressed by the distribution of 3D points in a voxel, and either a probabilistic model prior to this application or a probabilistic model after this application can be used; this application does not impose strict limitations on this.

[0059] Step 1054: If the features expressed by the 3D points included in the voxel are predefined features, then the voxel is determined to be a feature voxel.

[0060] Among them, the predefined features can be specifically set according to the needs of the actual application scenario;

[0061] Step 1055: Extract the features included in the feature voxels of the 3D sub-map and the 3D global map respectively;

[0062] Step 1056: Determine the category to which each extracted feature belongs according to the predefined feature categories.

[0063] pass Figure 3 The processing shown can determine a feature category for each voxel in the 3D sub-map and the 3D global map, that is, determine a semantic category for each voxel, so that the voxel has semantic information.

[0064] Figure 4 It shows Figure 1 The processing flow of step 107 includes:

[0065] Step 1071: Match multiple features extracted from the 3D sub-map with multiple features extracted from the 3D global map to establish the correspondence between the features of the 3D sub-map and the features of the 3D global map.

[0066] Step 1072: Based on the correspondence between the features of the established 3D sub-map and the features of the 3D global map, optimize the position of the features in the 3D sub-map to obtain a 3D sub-map for providing positioning information.

[0067] Figure 4 The processing shown aligns the 3D sub-map and the 3D global map based on the matched features with corresponding relationships, enabling the features in the 3D sub-map to have more accurate location information obtained through physical measurements.

[0068] for Figure 4 In step 1071, multiple features extracted from the 3D sub-map are matched with multiple features extracted from the 3D global map to establish the correspondence between the features of the 3D sub-map and the features of the 3D global map. Figure 5 The processing flow for this step, as shown in the diagram, includes:

[0069] Step 51: For features belonging to the same category, calculate the matching score between each feature extracted from that category in the 3D sub-map and each feature extracted from that category in the 3D global map.

[0070] In some embodiments, the matching score between two features includes the similarity between the two features;

[0071] Then, the matching score between each feature extracted from the category in the 3D sub-map and each feature extracted from the category in the 3D global map can be calculated separately, which can be done by calculating the similarity between each feature extracted from the category in the 3D sub-map and each feature extracted from the category in the 3D global map.

[0072] For example, for category A, the 3D sub-map includes three features of category A, namely feature 1, feature 2, and feature 3, while the 3D global map includes four features of category A, namely feature a, feature b, feature c, and feature d. The similarity between feature 1 and feature a, feature 1 and feature b, feature 1 and feature c, and feature 1 and feature d are determined respectively. Similarly, the similarity between feature 2 and features a, b, c, and d is calculated respectively, and the similarity between feature 3 and features a, b, c, and d is calculated respectively.

[0073] For the matching score, this application may also include other quantities that can measure the similarity between two features, and can be set according to the needs of specific application scenarios.

[0074] Step 52: For each extracted feature in the 3D sub-map, select the feature with the highest matching score in the 3D global map as the candidate feature pair;

[0075] Continuing from the previous example, among the similarities between feature 1 and feature a, feature 1 and feature b, feature 1 and feature c, and feature 1 and feature d, select the feature with the highest similarity value. For example, if feature 1 and feature c have the highest similarity, then feature 1 and feature c are selected as the candidate feature pair.

[0076] Step 53: Determine the distance between the two features in each candidate feature pair. If the distance is less than or equal to a predetermined threshold, determine the two selected features as valid features and establish the correspondence between the two features.

[0077] Specifically, prior to step 53, further adjustments can be made based on, for example... Figure 2 The processing flow shown aligns the 3D sub-map and the 3D global map, and in step 53, the distance between the two features in each candidate feature pair is determined based on the aligned 3D sub-map and the 3D global map. Since the 3D sub-map and the 3D global map have been aligned, the accurate distance between the two features in the candidate feature pair can be determined based on the aligned 3D sub-map and the 3D global map.

[0078] Continuing the previous example, determine the position of feature 1 in the 3D sub-map and the position of feature c in the 3D global map. If the difference between these two positions is less than or equal to a predetermined threshold, then feature 1 and feature c are valid features, and a correspondence is established between them; otherwise, feature 1 and feature c are invalid features. Invalid features can be discarded, or no further processing can be performed on invalid feature pairs.

[0079] for Figure 4 Step 1072 in the process, wherein the operation of optimizing the position of features in the 3D sub-map, such as Figure 6 As shown, it can specifically include:

[0080] Step 61: For features in the 3D sub-map and features in the 3D global map that have a corresponding relationship, take the position of the corresponding feature as the input of a predefined objective function. The objective function outputs a cost value, which is the sum of the distances between the corresponding features. The objective function is a function that expresses the relative positional relationship between the 3D sub-map and the 3D global map based on the positions of the corresponding features in the 3D sub-map and the 3D global map.

[0081] Among them, the positions of the corresponding features input into the objective function can be selected from all the positions of the corresponding features or from some of the positions of the corresponding features, depending on the needs of the specific application scenario.

[0082] Step 62: If the cost value is greater than a predetermined first convergence threshold, perform iterative optimization: that is, iteratively modify the position of features in the 3D sub-map and iteratively update the cost value of the objective function; if the cost value is less than or equal to the first convergence threshold, or if the difference in cost value between two adjacent iterations is less than or equal to a predetermined second convergence threshold, end the iterative process.

[0083] Iterative optimization algorithms can be used to achieve iterative optimization. The iterative optimization algorithm can be an optimization algorithm prior to this application or an optimization algorithm after this application, and this application does not make a specific limitation on it; for example, the Leverberg-Mariquardt algorithm (hereinafter referred to as the LM algorithm) can be used to iteratively modify the position of features in the 3D sub-map, so that the position of features in the 3D sub-map continuously approaches the position in the 3D global map.

[0084] Furthermore, since the objective function is expressed based on the positions of corresponding features, modifying the position of the 3D submap will also modify the objective function. The optimized objective function can more accurately express the relative positional relationship between the 3D submap and the 3D global map.

[0085] Through the above iterative processing, based on the correspondence between the features in the established 3D sub-map and the 3D global map, the position of the 3D sub-map is optimized. This continuously modifies the position of the features in the 3D sub-map to approximate the accurate position of the corresponding features in the 3D global map, resulting in a 3D sub-map with accurate position information. This 3D sub-map can be used to provide position information.

[0086] Therefore, the method provided in this application embodiment enables the 3D sub-map to have more accurate physical measurement information. Compared with the existing visual SLAM which only uses image data for positioning, it can provide more accurate location information. This can solve the problems of positioning drift and unstable positioning in the existing visual SLAM.

[0087] Based on the same inventive concept, embodiments of this application also provide a device for simultaneous positioning and mapping.

[0088] Figure 7 The structure of the simultaneous localization and mapping apparatus provided in this application embodiment is shown. The apparatus includes a processor 71 and at least one memory 72. The at least one memory 72 stores at least one machine-executable instruction. The processor 71 reads and executes the at least one machine-executable instruction to implement:

[0089] Acquire perception data of an environment, including image data, point cloud data, and inertial navigation data;

[0090] A 3D sub-map of the environment is built based on image data and inertial navigation data, and a 3D global map of the environment is built based on point cloud data and inertial navigation data.

[0091] Multiple features are extracted from the 3D sub-map and the 3D global map respectively;

[0092] Based on multiple features extracted from the 3D sub-map and the 3D global map respectively, the positions of the features in the 3D sub-map are optimized to obtain a 3D sub-map used to provide positioning information.

[0093] In some embodiments, after the processor 71 executes at least one machine-executable instruction to establish a 3D sub-map and a 3D global map of the environment, the process further includes aligning the 3D sub-map and the 3D global map, including: selecting at least one point in the 3D sub-map; determining the longitude, latitude, and altitude data of the selected at least one point based on inertial navigation data; and converting the coordinate system of the 3D sub-map to the coordinate system of the 3D global map based on the longitude, latitude, and altitude data of the selected at least one point.

[0094] In some embodiments, the processor 71 executes at least one machine-executable instruction to extract multiple features from a 3D sub-map and a 3D global map, respectively, including: voxelizing the 3D sub-map and the 3D global map according to a predetermined voxel size to obtain a 3D sub-map and a 3D global map that respectively include multiple voxels; determining the 3D points included in each voxel in the 3D sub-map and the 3D points included in each voxel in the 3D global map; determining the features expressed by the distribution of 3D points in each voxel; extracting the feature if the feature expressed by the 3D points included in the voxel is a predefined feature; and determining the category to which each extracted feature belongs according to a predefined feature category.

[0095] In some embodiments, the processor 71 executes at least one machine-executable instruction to determine the features expressed by the distribution of 3D points in each voxel, including: estimating the features expressed by the distribution of 3D points in the voxel using a probabilistic model.

[0096] In some embodiments, the processor 71 executes at least one machine-executable instruction to optimize the position of features in the 3D sub-map based on multiple features extracted from the 3D sub-map and the 3D global map, including: matching multiple features extracted from the 3D sub-map with multiple features extracted from the 3D global map to establish a correspondence between the features of the 3D sub-map and the features of the 3D global map; and optimizing the position of features in the 3D sub-map based on the established correspondence between the features of the 3D sub-map and the features of the 3D global map.

[0097] In some embodiments, the processor 71 executes at least one machine-executable instruction to match multiple features extracted from a 3D sub-map with multiple features extracted from a 3D global map, and establish a correspondence between the features of the 3D sub-map and the features of the 3D global map, including: for features belonging to the same category, calculating the matching score between each feature of that category extracted from the 3D sub-map and each feature of that category extracted from the 3D global map; for each feature extracted from the 3D sub-map, selecting the feature with the highest matching score in the 3D global map as a candidate feature pair; determining the distance between two features in each candidate feature pair, and if the distance is less than or equal to a predetermined threshold, determining the two selected features as valid features, and establishing a correspondence between the two features.

[0098] In some embodiments, before the processor 71 executes at least one machine-executable instruction to determine the distance between two features in each candidate feature pair, it further includes aligning the 3D submap and the 3D global map; then, the processor 71 executes at least one machine-executable instruction to determine the distance between two features in each candidate feature pair, including: determining the distance between two features in each candidate feature pair based on the aligned 3D submap and the 3D global map.

[0099] In some embodiments, the matching score between two features includes the similarity between the two features.

[0100] In some embodiments, the processor 71 executes at least one machine-executable instruction to optimize the position of features in a 3D submap based on the established correspondence between features, including: taking the position of the corresponding features as input to a predefined objective function, the objective function outputting a cost value, the cost value being the sum of distances between the corresponding features; wherein the objective function is a function that expresses the relative positional relationship between the 3D submap and the 3D global map based on the positions of the corresponding features in the 3D submap and the 3D global map; iteratively modifying the position of features in the 3D submap if the cost value is greater than a predetermined first convergence threshold; and ending the iterative process if the cost value is less than or equal to the first convergence threshold, or if the difference in cost values ​​between two adjacent iterations is less than or equal to a predetermined second convergence threshold.

[0101] In some embodiments, the processor 71 executes at least one machine-executable instruction to iteratively modify the position of features in the 3D submap, including: iteratively modifying the position of features in the 3D submap using the Leverberg-Mariquardt algorithm.

[0102] The apparatus provided in this application embodiment enables 3D sub-maps to have more accurate physical measurement information. Compared with the existing visual SLAM which only uses image data for positioning, it can provide more accurate location information. This solves the problems of positioning drift and unstable positioning in the existing visual SLAM.

[0103] Based on the same inventive concept, embodiments of this application also provide a device for simultaneous positioning and mapping.

[0104] Figure 8 The structure of the simultaneous positioning and mapping apparatus provided in an embodiment of this application is shown. The apparatus includes:

[0105] The data acquisition module 81 is used to acquire perception data of an environment, including image data, point cloud data and inertial navigation data.

[0106] The mapping module 82 is used to create a 3D sub-map of the environment based on image data and inertial navigation data, and to create a 3D global map of the environment based on point cloud data and inertial navigation data.

[0107] The positioning module 83 is used to extract multiple features from the 3D sub-map and the 3D global map respectively; based on the multiple features extracted from the 3D sub-map and the 3D global map respectively, optimize the position of the features in the 3D sub-map to obtain the 3D sub-map used to provide positioning information.

[0108] In some embodiments, after establishing a 3D sub-map and a 3D global map of the environment, the mapping module 82 is further used to align the 3D sub-map and the 3D global map, specifically including: selecting at least one point in the 3D sub-map; determining the longitude, latitude, and altitude data of the selected at least one point based on inertial navigation data; and converting the coordinate system of the 3D sub-map to the coordinate system of the 3D global map based on the longitude, latitude, and altitude data of the selected at least one point.

[0109] In some embodiments, the positioning module 83 extracts multiple features from the 3D sub-map and the 3D global map, respectively, including: voxelizing the 3D sub-map and the 3D global map according to a predetermined voxel size to obtain a 3D sub-map and a 3D global map that respectively include multiple voxels; determining the 3D points included in each voxel in the 3D sub-map and the 3D points included in each voxel in the 3D global map; determining the features expressed by the distribution of 3D points in each voxel; extracting the feature if the feature expressed by the 3D points included in the voxel is a predefined feature; and determining the category to which each extracted feature belongs according to a predefined feature category.

[0110] In some embodiments, the localization module 83 determines the features expressed by the distribution of 3D points in each voxel, including: estimating the features expressed by the distribution of 3D points in the voxel using a probability model.

[0111] In some embodiments, the positioning module 83 optimizes the position of features in the 3D sub-map based on multiple features extracted from the 3D sub-map and the 3D global map, including: matching multiple features extracted from the 3D sub-map with multiple features extracted from the 3D global map to establish a correspondence between the features of the 3D sub-map and the features of the 3D global map; and optimizing the position of features in the 3D sub-map based on the established correspondence between the features of the 3D sub-map and the features of the 3D global map.

[0112] In some embodiments, the positioning module 83 matches multiple features extracted from the 3D sub-map with multiple features extracted from the 3D global map to establish a correspondence between the features of the 3D sub-map and the features of the 3D global map. This includes: for features belonging to the same category, calculating the matching score between each feature of that category extracted from the 3D sub-map and each feature of that category extracted from the 3D global map; for each feature extracted from the 3D sub-map, selecting the feature with the highest matching score in the 3D global map as a candidate feature pair; determining the distance between two features in each candidate feature pair, and if the distance is less than or equal to a predetermined threshold, determining the two selected features as valid features and establishing a correspondence between the two features.

[0113] In some embodiments, before determining the distance between two features in each candidate feature pair, the positioning module 83 is further configured to: align the 3D sub-map and the 3D global map; then, the positioning module 83 determines the distance between two features in each candidate feature pair by: determining the distance between two features in each candidate feature pair based on the aligned 3D sub-map and the 3D global map.

[0114] In some embodiments, the matching score between two features includes the similarity between the two features.

[0115] In some embodiments, the positioning module 83 optimizes the position of features in the 3D sub-map based on the established correspondence between features, including: for features in the 3D sub-map and features in the 3D global map that have a correspondence, taking the position of the features with a correspondence as input to a predefined objective function, the objective function outputting a cost value, the cost value being the sum of the distances between the features with a correspondence; wherein, the objective function is a function that expresses the relative positional relationship between the 3D sub-map and the 3D global map based on the positions of the features with a correspondence in the 3D sub-map and the 3D global map; if the cost value is greater than a predetermined first convergence threshold, iteratively modifying the position of features in the 3D sub-map; if the cost value is less than or equal to the first convergence threshold, or if the difference in cost value between two adjacent iterations is less than or equal to a predetermined second convergence threshold, ending the iteration process.

[0116] In some embodiments, the positioning module 83 iteratively modifies the position of features in the 3D submap, including: iteratively modifying the position of features in the 3D submap using the Leverberg-Mariquardt algorithm.

[0117] The apparatus provided in this application embodiment enables 3D sub-maps to have more accurate physical measurement information. Compared with the existing visual SLAM which only uses image data for positioning, it can provide more accurate location information. This solves the problems of positioning drift and unstable positioning in the existing visual SLAM.

[0118] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A system for simultaneous localization and mapping, comprising: At least one processor; as well as At least one memory, including computer program instructions, which, when executed by the at least one processor, cause the system to at least: Based on multiple images from the camera, a first map is generated that includes multiple first features; A second map is generated based on data from an optical ranging sensor, which includes a second set of features. as well as Based on the comparison of the first plurality of features and the second plurality of features, the position of the first map relative to the second map is determined; Wherein, the at least one memory further includes computer program instructions, which, when executed by the at least one processor, cause the system to at least: The first plurality of features are compared with the second plurality of features to generate a plurality of scores, each of the plurality of scores including the distance between one of the first plurality of features and one of the second plurality of features; Wherein, the at least one memory further includes computer program instructions, which, when executed by the at least one processor, cause the system to at least: Determine that a first distance in one of the plurality of scores is greater than a threshold distance; and In response to the determination that the first distance is greater than the threshold distance, the correspondence between one of the first plurality of features and one of the second plurality of features is removed.

2. The system of claim 1, wherein, To determine the position of the first map relative to the second map, the at least one memory further includes computer program instructions that, when executed by the at least one processor, cause the system to at least: Extract the first plurality of features from the first map; And extract the second plurality of features from the second map.

3. The system of claim 2, wherein, The first plurality of features extracted from the first map include structured features and unstructured features, and the second plurality of features extracted from the second map include structured features and unstructured features.

4. The system of claim 3, wherein, The structured features include at least one of planes, lines, and curves, and the unstructured features include three-dimensional points.

5. The system of claim 2, wherein, To extract the first plurality of features from the first map and the second plurality of features from the second map, the at least one memory further includes computer program instructions that, when executed by the at least one processor, cause the system to at least: The first map is voxelized to generate a first plurality of voxels; The second map is voxelized to generate a second set of voxels; as well as Based on the probability model, the distribution of three-dimensional points within the first plurality of voxels and the second plurality of voxels is generated.

6. The system of claim 2, wherein, To extract the first plurality of features from the first map and the second plurality of features from the second map, the at least one memory further includes computer program instructions that, when executed by the at least one processor, cause the system to at least: The first plurality of features and the second plurality of features are classified.

7. The system of claim 1, wherein, The at least one memory further includes computer program instructions that, when executed by the at least one processor, cause the system to at least: Based on the information received from the inertial navigation module, the position of the first map relative to the second map is determined.

8. The system of claim 1, wherein, To generate the first map, the at least one memory further includes computer program instructions that, when executed by the at least one processor, cause the system to at least: Perform visual localization and mapping simultaneously.

9. The system of claim 1, wherein, The second map includes a city-scale 3D map.

10. The system of claim 1, wherein, To generate the second map, the at least one memory further includes computer program instructions that, when executed by the at least one processor, cause the system to at least: Receive vehicle attitude information from the inertial navigation module, the vehicle attitude information including vehicle position and orientation information; and The second map is generated based on the received vehicle attitude information.

11. A method for simultaneous localization and mapping, comprising: Based on multiple images from the camera, a first map is generated that includes multiple first features; A second map is generated based on data from an optical ranging sensor, which includes a second set of features. as well as Based on the comparison of the first plurality of features and the second plurality of features, the position of the first map relative to the second map is determined; The method further includes: The first plurality of features are compared with the second plurality of features to generate a plurality of scores, each of the plurality of scores including the distance between a first feature among the first plurality of features and a corresponding second feature among the second plurality of features; Determining the position of the first map relative to the second map includes performing iterative estimation of the position of the first map relative to the second map until the distance between the first plurality of features and the corresponding features among the second plurality of features is less than a threshold distance.

12. The method of claim 11, wherein, The distance between the first feature and the corresponding second feature is determined by a trained classifier.

13. The method of claim 11, further comprising: Extract the first plurality of features from the first map; Extract the second plurality of features from the second map; as well as The first plurality of features and the second plurality of features are classified into multiple categories.

14. The method of claim 13, wherein, The first plurality of features extracted from the first map include structured features and unstructured features, and the second plurality of features extracted from the second map include structured features and unstructured features.

15. The method of claim 13, wherein, Extracting the first plurality of features from the first map and extracting the second plurality of features from the second map further includes: The first map is voxelized to generate a first plurality of voxels; The second map is voxelized to generate a second plurality of voxels; and Based on the probability model, the distribution of three-dimensional points within the first plurality of voxels and the second plurality of voxels is generated.

16. A non-transitory computer-readable medium storing instructions that, when executed by at least one processor, cause operations including: Based on multiple images from the camera, a first map is generated that includes multiple first features; A second map is generated based on data from an optical ranging sensor, which includes a second set of features. as well as Based on the comparison of the first plurality of features and the second plurality of features, the position of the first map relative to the second map is determined; When executed by at least one processor, the instruction further causes operations including the following: The first plurality of features are compared with the second plurality of features to generate a plurality of scores, each of the plurality of scores including the distance between a first feature among the first plurality of features and a corresponding second feature among the second plurality of features; Determining the position of the first map relative to the second map includes performing iterative estimation of the position of the first map relative to the second map until the distance between the first plurality of features and the corresponding features among the second plurality of features is less than a threshold distance.

Images