CN115371663A

CN115371663A - Laser mapping method and device, electronic equipment and computer readable storage medium

Info

Publication number: CN115371663A
Application number: CN202211012748.4A
Authority: CN
Inventors: 赵清华
Original assignee: Zhidao Network Technology Beijing Co Ltd
Current assignee: Zhidao Network Technology Beijing Co Ltd
Priority date: 2022-08-23
Filing date: 2022-08-23
Publication date: 2022-11-22

Abstract

The application discloses a laser mapping method, a laser mapping device, an electronic device and a computer readable storage medium, wherein the method comprises the following steps: acquiring laser point cloud data of a current frame, and performing feature extraction on the laser point cloud data of the current frame to obtain feature point data of the current frame; acquiring a voxel map, and matching the feature point data of the current frame with the voxel map, wherein each voxel in the voxel map corresponds to the same feature; updating the voxel map according to the matching result to obtain an updated voxel map; and optimizing the pose data to be optimized by using a preset optimization algorithm based on the updated voxel map so as to obtain a laser point cloud map according to the optimized pose data. The method adopts the voxel map with a specific structure, on one hand, the voxel map is applied to the matching algorithm at the front end to speed up the matching process, so that the laser mapping efficiency is improved, on the other hand, the voxel map is applied to the optimization algorithm at the rear end to further optimize the pose, so that the laser mapping precision is improved.

Description

Laser mapping method and device, electronic equipment and computer readable storage medium

Technical Field

The present application relates to the field of map construction technologies, and in particular, to a laser map construction method and apparatus, an electronic device, and a computer-readable storage medium.

Background

The laser SLAM (synchronous positioning And Mapping) is a Mapping scheme realized based on point cloud data acquired by a laser radar, and provides important support for ensuring accurate positioning of an automatic driving vehicle in scenes without satellite positioning signals or with poor satellite positioning signal quality And the like.

The method is characterized in that a LOAM (laser Odometry and Mapping in Real-time laser radar odometer and Mapping) is a commonly used laser SLAM method, an algorithm for establishing a laser radar odometer Map is carried out based on the Point-surface characteristics of the LOAM, the operation efficiency is higher than that of an ICP (Iterative Closest Point) algorithm and an NDT (Normal distribution Transform) algorithm under the condition of basically not losing precision, so that the method is a mainstream laser radar odometer Map establishing scheme at present, and the basic idea is to register Point cloud maps into a Point cloud Map by adopting a Scan-to-Map Point-surface characteristic matching mode. But its drift will gradually accumulate as time increases.

The idea of Bundle Adjustment (BA for short) in visual SLAM is to associate feature points with pixel points and set a sliding window, so as to optimize the reprojection error in the sliding window. However, since the resolution of the visual image is high, it is easy to find the associated point, and the laser point cloud is sparse, it is difficult to apply the Bundle Adjustment concept directly to the laser SLAM.

Disclosure of Invention

The embodiment of the application provides a laser mapping method and device, electronic equipment and a computer readable storage medium, so as to improve laser mapping efficiency and mapping precision.

The embodiment of the application adopts the following technical scheme:

in a first aspect, an embodiment of the present application provides a laser mapping method, where the laser mapping method includes:

acquiring laser point cloud data of a current frame, and performing feature extraction on the laser point cloud data of the current frame to obtain feature point data of the current frame;

acquiring a voxel map, and matching the feature point data of the current frame with the voxel map, wherein each voxel in the voxel map corresponds to the same feature;

updating the voxel map according to the matching result to obtain an updated voxel map;

and optimizing the pose data to be optimized by using a preset optimization algorithm based on the updated voxel map so as to obtain a laser point cloud map according to the optimized pose data.

Optionally, the matching the feature point data of the current frame with the voxel map includes:

performing distance matching on the feature point data of the current frame and the voxel map to obtain a target voxel corresponding to the feature point data of the current frame;

and matching the feature point data of the current frame with the target voxel to obtain the matching result.

Optionally, the voxel map is of an octree structure, the matching result includes a matching pose, and the updating the voxel map according to the matching result to obtain an updated voxel map includes:

converting the feature point data of the current frame into a voxel map coordinate system according to the matching pose to obtain converted feature point data;

searching whether the feature point in the converted feature point data is located in a leaf node of an octree structure of the voxel map or not in the voxel map, wherein the feature points in the voxels corresponding to the leaf node belong to the same feature;

if the feature point in the converted feature point data is located in a leaf node of the octree structure and the feature point in the voxel corresponding to the leaf node belongs to the same feature, adding the feature point in the converted feature point data into the voxel corresponding to the leaf node;

and otherwise, creating a new octree structure based on the feature points in the converted feature point data.

Optionally, the optimizing the pose data to be optimized by using a preset optimization algorithm based on the updated voxel map to obtain the laser point cloud map according to the optimized pose data includes:

determining whether the frame number of the pose data to be optimized reaches the size of a preset sliding window;

acquiring pose data to be optimized in a preset sliding window under the condition that the frame number of the pose data to be optimized reaches the size of the preset sliding window;

and optimizing the pose data to be optimized in the preset sliding window by utilizing the preset optimization algorithm based on the updated voxel map to obtain the optimized pose data.

Optionally, the optimizing the pose data to be optimized in the preset sliding window by using the preset optimization algorithm based on the updated voxel map to obtain the optimized pose data includes:

constructing a nonlinear optimization model by using a beam adjustment method based on the updated voxel map, wherein an item to be optimized of the nonlinear optimization model is pose data to be optimized in a preset sliding window, and a residual item of the nonlinear optimization model is a distance residual sum corresponding to the preset sliding window;

and performing local optimization on the pose data to be optimized in the preset sliding window by using the nonlinear optimization model to obtain the optimized pose data.

Optionally, the constructing a nonlinear optimization model based on the updated voxel map by using a beam adjustment method includes:

determining multi-frame feature point data corresponding to the preset sliding window, wherein the multi-frame feature point data respectively comprise a plurality of feature points;

determining the characteristic type of a target voxel corresponding to each characteristic point;

according to the feature type of the target voxel corresponding to each feature point, constructing a distance residual error between each feature point and the corresponding target voxel;

determining the sum of distance residuals corresponding to the feature point data of each frame according to the distance residuals from each feature point to the corresponding target voxel;

and determining the distance residual sum corresponding to the preset sliding window according to the distance residual sum corresponding to each frame of feature point data.

Optionally, the feature type includes a line feature and a surface feature, and the constructing a distance residual between each feature point and a corresponding target voxel according to the feature type of the target voxel corresponding to each feature point includes:

if the characteristic type of the target voxel is a line characteristic, constructing a distance residual error from a point to a line according to the distance from each characteristic point to the corresponding target voxel;

and if the characteristic type of the target voxel is a surface characteristic, constructing a distance residual between a point and a surface according to the distance between each characteristic point and the corresponding target voxel.

In a second aspect, an embodiment of the present application further provides a laser mapping apparatus, where the laser mapping apparatus includes:

the acquisition unit is used for acquiring laser point cloud data of a current frame and extracting the characteristics of the laser point cloud data of the current frame to obtain characteristic point data of the current frame;

the matching unit is used for obtaining a voxel map and matching the feature point data of the current frame with the voxel map, wherein each voxel in the voxel map corresponds to the same feature;

the first updating unit is used for updating the voxel map according to the matching result to obtain an updated voxel map;

and the optimization unit is used for optimizing the pose data to be optimized by using a preset optimization algorithm based on the updated voxel map so as to obtain a laser point cloud map according to the optimized pose data.

In a third aspect, an embodiment of the present application further provides an electronic device, including:

a processor; and

a memory arranged to store computer executable instructions that, when executed, cause the processor to perform any of the laser mapping methods described above.

In a fourth aspect, embodiments of the present application further provide a computer-readable storage medium storing one or more programs, which when executed by an electronic device including a plurality of application programs, cause the electronic device to perform any one of the laser mapping methods described above.

The embodiment of the application adopts at least one technical scheme which can achieve the following beneficial effects: the laser mapping method comprises the steps of firstly obtaining laser point cloud data of a current frame, and carrying out feature extraction on the laser point cloud data of the current frame to obtain feature point data of the current frame; then obtaining a voxel map, and matching the feature point data of the current frame with the voxel map, wherein each voxel in the voxel map corresponds to the same feature; then updating the voxel map according to the matching result to obtain an updated voxel map; and finally, optimizing the pose data to be optimized by using a preset optimization algorithm based on the updated voxel map so as to obtain a laser point cloud map according to the optimized pose data. According to the laser mapping method, the voxel map with the specific structure is adopted, on one hand, the voxel map is applied to the matching algorithm at the front end to accelerate the matching process, so that the laser mapping efficiency is improved, on the other hand, the voxel map is applied to the optimization algorithm at the rear end to further optimize the pose, so that the laser mapping precision is improved.

Drawings

The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this application, illustrate embodiment(s) of the application and together with the description serve to explain the application and not to limit the application. In the drawings:

FIG. 1 is a schematic flow chart of a laser mapping method according to an embodiment of the present disclosure;

FIG. 2 is a schematic illustration of a laser patterning process in an embodiment of the present application;

fig. 3 is a schematic structural diagram of a laser mapping apparatus in an embodiment of the present application;

fig. 4 is a schematic structural diagram of an electronic device in an embodiment of the present application.

Detailed Description

In order to make the objects, technical solutions and advantages of the present application more apparent, the technical solutions of the present application will be described in detail and completely with reference to the following specific embodiments of the present application and the accompanying drawings. It should be apparent that the described embodiments are only a few embodiments of the present application, and not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.

The technical solutions provided by the embodiments of the present application are described in detail below with reference to the accompanying drawings.

The embodiment of the present application provides a laser mapping method, and as shown in fig. 1, provides a schematic flow chart of the laser mapping method in the embodiment of the present application, where the laser mapping method at least includes the following steps S110 to S140:

step S110, obtaining laser point cloud data of the current frame, and performing feature extraction on the laser point cloud data of the current frame to obtain feature point data of the current frame.

When laser mapping is performed, laser point cloud data of a current frame needs to be obtained first, the laser point cloud data can be acquired by a laser radar on a vehicle, then a certain point cloud feature extraction algorithm is used for performing feature extraction on the laser point cloud data of the current frame to obtain feature point data of the current frame, the point cloud feature extraction algorithm can be realized by using a feature extraction algorithm based on an LOAM in the prior art, and for example, feature point data such as line features and surface features of the current frame can be obtained by calculating the curvature of the feature points.

And step S120, acquiring a voxel map, and matching the feature point data of the current frame with the voxel map, wherein each voxel in the voxel map corresponds to the same feature.

The final purpose of the embodiment of the application is to construct a laser point cloud map in a certain area so as to provide a basis for positioning of subsequent automatic driving vehicles. However, for the existing scheme of constructing the point cloud Map based on the Scan-to-Map feature matching mode adopted in the LOAM algorithm, with the arrival of new laser point cloud data, the data volume contained in the point cloud Map is larger and larger, and if the new laser point cloud data is matched with all data in the whole point cloud Map at each time, a large amount of time is consumed, so that the Map construction efficiency is low.

Based on this, a voxel map (VoxelMap) with a certain structure is constructed in the embodiment of the application, the voxel map is composed of a plurality of voxels, each voxel can be regarded as a point cloud map block composed of a three-dimensional space, but compared with the point cloud map, the voxel map stores characteristic point information, information about voxel structures, characteristic point attributes and the like, and support is provided for improving map construction matching efficiency.

In addition, each voxel in the voxel map of the embodiment of the application represents the same feature, that is, the feature point data belonging to the same feature is divided into one voxel, and the voxel map structure reduces the time for searching the corresponding feature in the laser map building process, because only the voxel where the feature point is located or nearby is needed to be searched, and the closest point is not needed to be searched in more detail, thereby greatly improving the operation efficiency. After the feature point data of the current frame is obtained, the feature point data of the current frame is matched with each voxel in the voxel map, namely Scan-to-VoxelMap, so that the voxel matched with the feature point data of the current frame can be quickly determined.

And step S130, updating the voxel map according to the matching result to obtain an updated voxel map.

The matching process is to determine the voxels in the voxel map that are matched with the feature point data of the current frame, and the higher the matching degree is, the more probable the feature point data of the current frame and the corresponding voxels represent the same feature, so that the data in the voxel map can be updated, that is, the feature point data of the current frame is registered in the voxel map based on the matching result.

And step S140, optimizing the pose data to be optimized by using a preset optimization algorithm based on the updated voxel map so as to obtain a laser point cloud map according to the optimized pose data.

In the step, the feature point data of the current frame is matched with each voxel in the voxel map, so that the matching efficiency is improved, but only one preliminary matching pose can be obtained in the way, and in order to further improve the accuracy of the matching pose, the embodiment of the application can optimize the pose data to be optimized by adopting a preset optimization algorithm, so that point cloud splicing is performed according to the optimized pose data, and the final laser point cloud map is obtained.

The preset optimization algorithm can adopt a nonlinear optimization algorithm, for example, the idea of Bundle Adjustment in the visual SLAM can be directly applied to the laser mapping optimization process in the embodiment of the present application, because the voxel map structure constructed in the embodiment of the present application provides powerful support for matching of feature points. Of course, which optimization algorithm is specifically adopted can be flexibly selected by those skilled in the art according to actual requirements, and is not specifically limited herein.

In addition, in order to facilitate the use of the voxel map by the feature matching of the front end and the pose optimization of the back end, the voxel map constructed in the embodiment of the present application may be provided to the feature matching process of the front end and the pose optimization process of the back end in a memory sharing manner.

The laser mapping method provided by the embodiment of the application adopts the voxel map with a specific structure, and on one hand, the voxel map is applied to the matching algorithm at the front end to speed up the matching process, so that the laser mapping efficiency is improved, and on the other hand, the voxel map is applied to the optimization algorithm at the rear end to further optimize the pose, so that the laser mapping precision is improved.

In some embodiments of the present application, the matching the feature point data of the current frame with the voxel map includes: performing distance matching on the feature point data of the current frame and the voxel map to obtain a target voxel corresponding to the feature point data of the current frame; and matching the feature point data of the current frame with the target voxel to obtain the matching result.

When the feature point data of the current frame is matched with the voxel map, the distance between the feature point data of the current frame and each voxel in the voxel map is calculated for each feature point in the feature point data of the current frame, and then the voxel with the closest distance is used as a target voxel corresponding to the feature point. And then, carrying out feature matching on the feature point data of the current frame and the target voxel, thereby obtaining data such as matching pose.

In some embodiments of the present application, the voxel map includes center points of a plurality of voxels, and the distance matching the feature point data of the current frame with the voxel map to obtain a target voxel corresponding to the feature point data of the current frame includes: determining the distance between the feature point in the feature point data of the current frame and the center point of each voxel; and determining a target voxel corresponding to the feature point data of the current frame according to the distance between the feature point in the feature point data of the current frame and the center point of each voxel.

When the distance matching is carried out on the feature points in the feature point data of the current frame and the voxel map, the center point of each voxel in the voxel map can be used as the reference of the distance matching, and the center point of each voxel can be stored in the process of constructing the voxel map. By calculating the distance between the feature point and the center point of each voxel, the voxel with the smallest distance can be used as the target voxel.

In some embodiments of the present application, the obtaining a voxel map comprises: determining whether each feature point in the feature point data of the current frame corresponds to the same feature; if yes, storing the feature point data of the current frame into a voxel of the voxel map; if not, the feature point data of the current frame is divided for a plurality of times based on an octree structure to obtain a plurality of voxels, so that each finally obtained voxel corresponds to the same feature.

When the voxel map is obtained, the voxel map can be initialized by using a preset initialization strategy, and the structure of the voxel map is mainly constructed on the basis of the principle that one voxel only corresponds to the same feature, so that a certain detection algorithm can be adopted to detect whether each feature point in feature point data of a current frame corresponds to the same feature.

The attribute of the feature point data is mainly divided into a line feature and a surface feature, and first, the distinction between the line feature and the surface feature can be realized by calculating the curvature in the prior art, for example, the curvature can be calculated according to the lengths (the distance from the laser point to the radar) of 5 points before and after and the current point. For a point on a smooth plane in three-dimensional space, the difference in size between the point and the surrounding points is small, and the curvature is low, and therefore the point can be regarded as a surface feature point, while for a point on a sharp edge in three-dimensional space, the difference in size between the point and the surrounding points is large, and the curvature is high, and therefore the point can be regarded as a line feature point.

Secondly, for the distinction between different line features or different surface features, the detection can be carried out in a way that the average value of the laser point cloud data of the current frame is calculated, then the covariance matrix C of the current frame is calculated according to the average value of the laser point cloud data of the current frame, and finally 3 feature values of the covariance matrix C are calculated, if 1 feature value is obviously larger than the other 2 feature values, the feature points in the feature point data of the current frame belong to the same line feature, if 2 feature values are obviously larger than the other 1 feature value, the feature points in the feature point data of the current frame belong to the same surface feature, otherwise, the feature points do not belong to the same line feature/surface feature.

Whether a plurality of feature points contained in the feature point data of the current frame correspond to the same line feature/surface feature or not can be judged through the method, if the feature points correspond to the same line feature/surface feature, the plurality of feature points contained in the feature point data of the current frame are directly stored in one voxel, and if the feature points do not correspond to the same line feature/surface feature, the feature point data of the current frame can be divided for a plurality of times based on an octree structure, for example, the feature point data can be equally divided into 8 voxels according to the preset space size, then whether the feature point data contained in the 8 voxels correspond to the same line feature/surface feature or not is respectively judged, and the process is repeated until all the finally obtained voxels respectively correspond to the same feature or reach the minimum voxel size. Of course, the size of the minimum voxel size can be flexibly set by those skilled in the art according to actual needs, and is not particularly limited herein.

Each voxel map of the embodiment of the application is composed of octree structures indexed by a plurality of hash tables, and the octree structures are used for reducing the depth of the tree and further improving the search speed. Each octree-structured voxel map may correspond to a non-empty cube of a default voxel size in space, and different octrees may have different depths, depending on the geometry of the cubes in space. Each leaf node in the octree, i.e., the last level node, holds all feature point data corresponding to the same feature. In addition, the voxel map of the octree structure constructed in the embodiment of the application can respectively construct a corresponding line feature voxel map and a corresponding surface feature voxel map based on the line feature and the surface feature.

The voxel map with the self-adaptive octree structure designed by the embodiment of the application can greatly reduce the memory consumption of a matching algorithm, and can be naturally compatible with the existing data structures such as octrees, and the like, thereby greatly facilitating the realization and the use efficiency of the voxel map.

In some embodiments of the present application, the voxel map is in an octree structure, the matching result includes a matching pose, the updating the voxel map according to the matching result, and obtaining the updated voxel map includes: converting the feature point data of the current frame into a voxel map coordinate system according to the matching pose to obtain converted feature point data; searching whether the feature point in the converted feature point data is located in a leaf node of an octree structure of the voxel map or not in the voxel map, wherein the feature points in the voxels corresponding to the leaf node belong to the same feature; if the feature point in the converted feature point data is located in a leaf node of the octree structure and the feature point in the voxel corresponding to the leaf node belongs to the same feature, adding the feature point in the converted feature point data into the voxel corresponding to the leaf node; and otherwise, creating a new octree structure based on the feature points in the converted feature point data.

The feature point data of the current frame corresponds to a laser radar coordinate system, the voxel map corresponds to a voxel map coordinate system, and the voxel map coordinate system is the coordinate system established by taking the first frame of laser point cloud data as the reference as the laser point cloud map coordinate system to be finally constructed, so that the feature point data of the current frame can be converted into the coordinate system of the voxel map by pose transformation, and then whether the feature points in the converted feature point data are positioned in leaf nodes of an octree structure or not is searched in all the octree structures contained in the voxel map, and different updating strategies are adopted according to different searching results.

The principle of the update strategy adopted in the embodiment of the present application is the same as that of the initialization strategy in the foregoing embodiment, and all the principles are to make each voxel obtained by division in the voxel map correspond to the same feature, so that when searching is performed in the voxel map, if a feature point of a current frame falls into a voxel corresponding to a certain existing leaf node, and feature points in the voxel corresponding to the leaf node belong to the same feature, for example, all belong to the same line feature/the same plane feature, at this time, data of the feature point can be directly added to the voxel corresponding to the leaf node, thereby realizing the update of the voxel corresponding to the leaf node.

If the feature point of the current frame falls into the voxel corresponding to any existing leaf node, the feature point is not matched with the feature point data in the existing leaf node, and at the moment, the feature point can be used as a root node to create a new octree structure, and the root of the new octree structure is indexed in a hash table.

By the adoption of the mode of adaptively updating the octree-structured voxel map, each voxel in the voxel map can be guaranteed to correspond to the same feature, only the voxel at or near the feature point needs to be searched in the laser mapping process, the closest point does not need to be searched in more detail, the time for searching the feature is shortened, and the matching efficiency is improved.

In some embodiments of the present application, the optimizing pose data to be optimized by using a preset optimization algorithm based on the updated voxel map to obtain a laser point cloud map according to the optimized pose data includes: determining whether the frame number of the pose data to be optimized reaches the size of a preset sliding window; acquiring pose data to be optimized in a preset sliding window under the condition that the frame number of the pose data to be optimized reaches the size of the preset sliding window; and optimizing the pose data to be optimized in the preset sliding window by utilizing the preset optimization algorithm based on the updated voxel map to obtain the optimized pose data.

In the embodiment of the application, when performing back-end optimization, a strategy of presetting a sliding window can be used for optimization, that is, local optimization is performed based on the size of the presetting sliding window, so that in an initial stage, it can be judged whether the frame number of pose data to be optimized, which is currently acquired, has reached the size of the presetting sliding window, for example, the size of the presetting sliding window is 20 frames, if the frame number of the pose data to be optimized, which is currently acquired, is 19 frames, it indicates that the optimization requirement is not met, and if the frame number of the pose data to be optimized, which is currently acquired, is 20 frames, it indicates that the optimization requirement is met, at this time, pose optimization can be performed on all pose data to be optimized in the presetting sliding window by using a preset optimization algorithm based on an updated voxel map.

The preset sliding window can be regarded as a queue structure, and once the number of data frames in the window exceeds the size of the preset sliding window, older frames are removed, for example, if the number of frames of pose data to be optimized, which has been currently acquired, is 21 frames, the 1 st frame of data is removed from the preset sliding window, that is, it is always ensured that 20 frames of data which are acquired most recently are optimized. Of course, the size of the preset sliding window can be flexibly set according to actual requirements, and is not specifically limited herein.

In some embodiments of the present application, the optimizing the pose data to be optimized in the preset sliding window by using the preset optimization algorithm based on the updated voxel map, and obtaining the optimized pose data includes: constructing a nonlinear optimization model by using a beam adjustment method based on the updated voxel map, wherein an item to be optimized of the nonlinear optimization model is pose data to be optimized in a preset sliding window, and a residual item of the nonlinear optimization model is a distance residual sum corresponding to the preset sliding window; and performing local optimization on the pose data to be optimized in the preset sliding window by using the nonlinear optimization model to obtain the optimized pose data.

The preset optimization algorithm adopted by the embodiment of the application can be realized by adopting a nonlinear optimization model constructed based on a BA algorithm, the BA algorithm is mainly applied to the visual SLAM at present, and the BA algorithm in the visual SLAM is optimized by associating the characteristic points with the pixel points and constructing the reprojection error. However, the above process is easy to find the associated point due to the high resolution of the visual image, and in the visual SLAM, the laser point cloud is sparse, so it is difficult to apply the BA algorithm directly to the laser SLAM.

Based on this, the embodiment of the present application utilizes the voxel map constructed in the foregoing embodiment to overcome the problem of difficult application of the BA algorithm in laser SLAM, because each voxel in the voxel map structure represents the same feature, and therefore, the structure based on the voxel map can also find the matched feature easily and quickly.

The nonlinear optimization model constructed based on the BA algorithm mainly needs to determine an item to be optimized and a residual error item, and a local optimization strategy of a preset sliding window is adopted, so that all pose data to be optimized in the preset sliding window correspond to the item to be optimized, the residual error item can be obtained by constructing a distance residual error sum according to the distance matching result of corresponding multi-frame feature point data and voxels in the preset sliding window, and the pose data to be optimized in the preset sliding window can be optimized in a mode of minimizing the distance residual error sum.

In some embodiments of the present application, the constructing a nonlinear optimization model based on the updated voxel map by using a beam adjustment method includes: determining multi-frame feature point data corresponding to the preset sliding window, wherein the multi-frame feature point data respectively comprise a plurality of feature points; determining the characteristic type of a target voxel corresponding to each characteristic point; according to the feature type of the target voxel corresponding to each feature point, constructing a distance residual error between each feature point and the corresponding target voxel; determining the sum of distance residuals corresponding to the feature point data of each frame according to the distance residuals from each feature point to the corresponding target voxel; and determining the distance residual sum corresponding to the preset sliding window according to the distance residual sum corresponding to each frame of feature point data.

The pose data to be optimized are stored in the preset sliding window, each frame of pose to be optimized corresponds to one frame of feature point data, each frame of feature point data comprises a plurality of feature points, and each feature point is matched with a voxel with the closest distance.

Based on this, when a nonlinear optimization model is constructed, the nonlinear optimization model can be constructed in two dimensions, the first dimension is a dimension of a single data frame, and for any frame of feature point data, the distance residual between each feature point and the corresponding target voxel can be constructed by combining the feature type of the target voxel corresponding to each feature point, so that the distance residual of a plurality of feature points can be obtained, the distance residuals of the plurality of feature points are respectively summed in the dimension of each data frame, and then the distance residual sum corresponding to each frame of feature point data can be obtained. And after the distance residual sum corresponding to each frame of feature point data is obtained, further summing the distance residual sums of each frame of feature point data corresponding to the preset sliding window, thereby obtaining the distance residual sum of the multi-frame of feature point data, namely the distance residual sum of the preset sliding window.

In some embodiments of the present application, the feature types include line features and surface features, and constructing the distance residuals from each feature point to the corresponding target voxel according to the feature type of the target voxel corresponding to each feature point includes: if the characteristic type of the target voxel is a line characteristic, constructing a distance residual error from a point to a line according to the distance from each characteristic point to the corresponding target voxel; and if the characteristic type of the target voxel is a surface characteristic, constructing a distance residual between a point and a surface according to the distance between each characteristic point and the corresponding target voxel.

The feature types of the embodiment of the present application mainly include line features and surface features, and when distance residuals from each feature point to corresponding target voxels are constructed, different feature types can be distinguished for construction, for example, if a matched target voxel is a line feature, distance residuals from a point to a line can be constructed, and if a matched target voxel is a surface feature, distance errors from a point to a surface can be constructed.

In some embodiments of the present application, after optimizing pose data to be optimized by using a preset optimization algorithm based on the updated voxel map, the laser mapping method further includes: and updating the position information of the feature points in the updated voxel map according to the optimized pose data.

Because the voxel map also stores information such as position coordinates of the feature points in a world coordinate system, after the optimized pose data is obtained, the position information of the feature points stored in the voxel map can be updated by using the optimized position information, so that the updated voxel map is used to participate in the next Scan-to-VoxelMap matching process.

In order to facilitate understanding of the embodiments of the present application, as shown in fig. 2, a schematic diagram of a laser mapping process in the embodiments of the present application is provided, and the whole laser mapping process of the present application is mainly divided into 3 threads:

1) In the thread 1, based on a LOAM algorithm, line/surface feature extraction is carried out on currently received laser point cloud data independently, and feature point data is stored for other threads to use;

2) In the thread 2, based on the LOAM feature matching idea, feature matching of Scan-to-VoxelMap is executed, and the laser odometer pose is output at a certain frequency such as 10Hz, wherein the difference from the traditional LOAM feature matching process is that the VoxelMap is used for matching, but not a global point cloud map, so that the matching efficiency can be greatly improved;

3) And the thread 3 executes the sliding window pose optimization at the rear end based on the VoxelMap and BA algorithm, obtains the optimized pose data in the sliding window, then splices the point cloud map based on the optimized pose data, and outputs the point cloud map at a certain frequency such as 2Hz, thereby obtaining the final laser point cloud map.

In summary, the laser mapping method of the present application at least achieves the following technical effects:

1) The voxel map with shared memory is applied to the LOAM algorithm at the front end to accelerate the matching process, and is also applied to the BA algorithm at the rear end to further optimize the pose, so that the voxel map is utilized to the maximum extent, and the laser mapping efficiency and mapping precision are improved;

2) The matching method of the traditional LOAM is improved, namely the matching method of the Scan-to-Map is adjusted to the matching method of the Scan-to-VoxelMap, and the matching speed can be greatly improved on the premise of not reducing the matching precision;

3) The BA thought in the visual SLAM is applied to the rear-end sliding window optimization of the laser SLAM, so that the matching pose and the map building precision are improved;

4) And multithreading is adopted to respectively execute feature extraction, pose matching and pose optimization, so that the map building speed is further improved, and real-time map building can be carried out.

The embodiment of the present application further provides a laser mapping apparatus 300, as shown in fig. 3, which provides a schematic structural diagram of the laser mapping apparatus in the embodiment of the present application, where the laser mapping apparatus 300 includes: an obtaining unit 310, a matching unit 320, a first updating unit 330, and an optimizing unit 340, wherein:

the acquiring unit 310 is configured to acquire laser point cloud data of a current frame, and perform feature extraction on the laser point cloud data of the current frame to obtain feature point data of the current frame;

a matching unit 320, configured to obtain a voxel map, and match the feature point data of the current frame with the voxel map, where each voxel in the voxel map corresponds to the same feature;

a first updating unit 330, configured to update the voxel map according to a matching result, to obtain an updated voxel map;

and the optimizing unit 340 is configured to optimize the pose data to be optimized by using a preset optimization algorithm based on the updated voxel map, so as to obtain a laser point cloud map according to the optimized pose data.

In some embodiments of the present application, the matching unit 320 is specifically configured to: performing distance matching on the feature point data of the current frame and the voxel map to obtain a target voxel corresponding to the feature point data of the current frame; and matching the feature point data of the current frame with the target voxel to obtain the matching result.

In some embodiments of the present application, the voxel map is an octree structure, the matching result includes a matching pose, and the first updating unit 330 is specifically configured to: converting the feature point data of the current frame into a voxel map coordinate system according to the matching pose to obtain converted feature point data; searching whether the feature point in the converted feature point data is located in a leaf node of an octree structure of the voxel map or not in the voxel map, wherein the feature points in the voxels corresponding to the leaf node belong to the same feature; if the feature point in the converted feature point data is located in a leaf node of the octree structure and the feature point in the voxel corresponding to the leaf node belongs to the same feature, adding the feature point in the converted feature point data into the voxel corresponding to the leaf node; and otherwise, creating a new octree structure based on the feature points in the converted feature point data.

In some embodiments of the present application, the optimization unit 340 is specifically configured to: determining whether the frame number of the pose data to be optimized reaches the size of a preset sliding window; acquiring pose data to be optimized in a preset sliding window under the condition that the frame number of the pose data to be optimized reaches the size of the preset sliding window; and optimizing the pose data to be optimized in the preset sliding window by utilizing the preset optimization algorithm based on the updated voxel map to obtain the optimized pose data.

In some embodiments of the present application, the optimization unit 340 is specifically configured to: constructing a nonlinear optimization model by using a beam balancing method based on the updated voxel map, wherein an item to be optimized of the nonlinear optimization model is pose data to be optimized in a preset sliding window, and a residual item of the nonlinear optimization model is a distance residual sum corresponding to the preset sliding window; and performing local optimization on the pose data to be optimized in the preset sliding window by using the nonlinear optimization model to obtain the optimized pose data.

In some embodiments of the present application, the optimization unit 340 is specifically configured to: determining multi-frame feature point data corresponding to the preset sliding window, wherein the multi-frame feature point data respectively comprise a plurality of feature points; determining the characteristic type of a target voxel corresponding to each characteristic point; according to the feature type of the target voxel corresponding to each feature point, constructing a distance residual error between each feature point and the corresponding target voxel; determining the sum of distance residuals corresponding to the feature point data of each frame according to the distance residuals from each feature point to the corresponding target voxel; and determining the distance residual sum corresponding to the preset sliding window according to the distance residual sum corresponding to each frame of feature point data.

In some embodiments of the present application, the feature types include line features and face features, and the optimization unit 340 is specifically configured to: if the characteristic type of the target voxel is a line characteristic, constructing a distance residual error from a point to a line according to the distance from each characteristic point to the corresponding target voxel; and if the characteristic type of the target voxel is a surface characteristic, constructing a distance residual between a point and a surface according to the distance between each characteristic point and the corresponding target voxel.

In some embodiments of the present application, the laser mapping apparatus further includes: and the first updating unit is used for updating the position information of the feature points in the updated voxel map according to the optimized pose data.

It can be understood that the laser mapping apparatus can implement each step of the laser mapping method provided in the foregoing embodiment, and the explanations related to the laser mapping method are applicable to the laser mapping apparatus, and are not repeated here.

Fig. 4 is a schematic structural diagram of an electronic device according to an embodiment of the present application. Referring to fig. 4, at a hardware level, the electronic device includes a processor, and optionally further includes an internal bus, a network interface, and a memory. The Memory may include a Memory, such as a Random-Access Memory (RAM), and may further include a non-volatile Memory, such as at least 1 disk Memory. Of course, the electronic device may also include hardware required for other services.

The processor, the network interface, and the memory may be connected to each other via an internal bus, which may be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, an EISA (Extended Industry Standard Architecture) bus, or the like. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one double-headed arrow is shown in FIG. 4, but that does not indicate only one bus or one type of bus.

And the memory is used for storing programs. In particular, the program may include program code comprising computer operating instructions. The memory may include both memory and non-volatile storage and provides instructions and data to the processor.

The processor reads a corresponding computer program from the nonvolatile memory into the memory and then runs the computer program to form the laser mapping device on a logic level. The processor is used for executing the program stored in the memory and is specifically used for executing the following operations:

The method performed by the laser mapping apparatus according to the embodiment shown in fig. 1 of the present application may be implemented in a processor or implemented by a processor. The processor may be an integrated circuit chip having signal processing capabilities. In implementation, the steps of the above method may be performed by integrated logic circuits of hardware in a processor or instructions in the form of software. The Processor may be a general-purpose Processor, including a Central Processing Unit (CPU), a Network Processor (NP), and the like; but also Digital Signal Processors (DSPs), application Specific Integrated Circuits (ASICs), field Programmable Gate Arrays (FPGAs) or other Programmable logic devices, discrete Gate or transistor logic devices, discrete hardware components. The various methods, steps, and logic blocks disclosed in the embodiments of the present application may be implemented or performed. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The steps of the method disclosed in connection with the embodiments of the present application may be directly implemented by a hardware decoding processor, or implemented by a combination of hardware and software modules in the decoding processor. The software module may be located in ram, flash memory, rom, prom, or eprom, registers, etc. storage media as is well known in the art. The storage medium is located in a memory, and a processor reads information in the memory and completes the steps of the method in combination with hardware of the processor.

The electronic device may further execute the method executed by the laser mapping apparatus in fig. 1, and implement the functions of the laser mapping apparatus in the embodiment shown in fig. 1, which are not described herein again in this embodiment of the present application.

An embodiment of the present application further provides a computer-readable storage medium storing one or more programs, where the one or more programs include instructions, which, when executed by an electronic device including a plurality of application programs, enable the electronic device to perform the method performed by the laser mapping apparatus in the embodiment shown in fig. 1, and are specifically configured to perform:

acquiring a voxel map, and matching the feature point data of the current frame with the voxel map, wherein each voxel in the voxel map corresponds to the same feature respectively;

As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.

The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

In a typical configuration, a computing device includes one or more processors (CPUs), input/output interfaces, network interfaces, and memory.

The memory may include forms of volatile memory in a computer readable medium, random Access Memory (RAM) and/or non-volatile memory, such as Read Only Memory (ROM) or flash memory (flash RAM). Memory is an example of a computer-readable medium.

Computer-readable media, including both non-transitory and non-transitory, removable and non-removable media, may implement information storage by any method or technology. The information may be computer readable instructions, data structures, modules of a program, or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static Random Access Memory (SRAM), dynamic Random Access Memory (DRAM), other types of Random Access Memory (RAM), read Only Memory (ROM), electrically Erasable Programmable Read Only Memory (EEPROM), flash memory or other memory technology, compact disc read only memory (CD-ROM), digital Versatile Discs (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices, or any other non-transmission medium that can be used to store information that can be accessed by a computing device. As defined herein, a computer readable medium does not include a transitory computer readable medium such as a modulated data signal and a carrier wave.

It should also be noted that the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising a … …" does not exclude the presence of another identical element in a process, method, article, or apparatus that comprises the element.

As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.

The above description is only an example of the present application and is not intended to limit the present application. Various modifications and changes may occur to those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.

Claims

1. A laser mapping method, wherein the laser mapping method comprises the following steps:

2. The laser mapping method of claim 1, wherein the matching the feature point data of the current frame with the voxel map comprises:

3. The laser mapping method of claim 1, wherein the voxel map is of an octree structure, the matching result includes a matching pose, the updating of the voxel map according to the matching result includes:

4. The laser mapping method of claim 1, wherein the optimizing pose data to be optimized based on the updated voxel map by using a preset optimization algorithm to obtain a laser point cloud map according to the optimized pose data comprises:

5. The laser mapping method according to claim 1, wherein the optimizing pose data to be optimized in the preset sliding window by using the preset optimization algorithm based on the updated voxel map to obtain optimized pose data comprises:

6. The laser mapping method of claim 5, wherein the constructing a non-linear optimization model using a beam-balancing method based on the updated voxel map comprises:

7. The laser mapping method according to claim 6, wherein the feature types include line features and surface features, and the constructing distance residuals from each feature point to the corresponding target voxel according to the feature types of the target voxels corresponding to each feature point includes:

and if the characteristic type of the target voxel is a surface characteristic, constructing a distance residual error from a point to a surface according to the distance from each characteristic point to the corresponding target voxel.

8. A laser mapping apparatus, wherein the laser mapping apparatus comprises:

9. An electronic device, comprising:

a processor; and

a memory arranged to store computer executable instructions which, when executed, cause the processor to perform the laser mapping method of any of claims 1 to 7.

10. A computer readable storage medium storing one or more programs which, when executed by an electronic device comprising a plurality of application programs, cause the electronic device to perform the laser mapping method of any of claims 1-7.