Laser radar SLAM method, device and system for autonomous vehicle

Abstract

The invention relates to the technical field of automatic driving positioning, and particularly discloses a laser radar SLAM method, device and system for an automatic driving vehicle, and the method comprises the steps: obtaining the original point cloud data of a laser radar, and carrying out the adaptive roughness evaluation and feature screening of the original point cloud data of the laser radar, and obtaining feature information; performing dynamic outlier detection on the feature information to screen out outlier feature points to obtain effective feature information; attitude estimation is carried out on the effective feature information according to a double-peak geometric primitive constraint mechanism, a six-degree-of-freedom pose is obtained, and the double-peak geometric primitive constraint mechanism is obtained through construction according to the feature information of the current frame and corresponding feature information in a local map; and according to the six-degree-of-freedom poses, local map construction and global map construction are carried out in sequence, and positioning information of the autonomous vehicle is obtained. According to the laser radar SLAM method for the autonomous vehicle provided by the invention, the precision and reliability of laser radar SLAM can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving positioning, and in particular, to a lidar SLAM method for autonomous vehicles, a lidar SLAM device for autonomous vehicles, and a lidar SLAM system for autonomous vehicles. Background Art

[0002] Autonomous vehicles rely on precise positioning technology, which involves multiple aspects such as environmental perception, path planning, and motion control. To achieve high-precision positioning, vehicles are usually equipped with multiple sensors, such as GNSS (Global Navigation Satellite System), IMU (Inertial Measurement Unit), cameras, and lidar. In places where GNSS signals are poor, such as urban canyons or underground garages, pre-built high-precision maps can help solve the pose estimation problem, but this method is not applicable to scenarios with large environmental changes. Therefore, Simultaneous Localization and Mapping (SLAM) is very important for adapting to unknown or dynamically changing environments.

[0003] In recent years, although vision-based methods have been applied in the field of robotics, they are difficult to meet the requirements of autonomous driving due to being greatly affected by lighting conditions and inaccurate depth estimation. In contrast, lidar is not affected by lighting and can provide accurate geometric information. Lidar SLAM is divided into front-end odometry and back-end optimization, and the front-end odometry is mainly divided into methods based on ICP (Iterative Closest Point) and methods based on feature extraction. The ICP method directly uses raw point cloud data for registration, with a large amount of calculation and difficult to achieve real-time processing; while the NDT (Normal Distribution Transform) algorithm reduces the computational burden by introducing voxel filtering, but sacrifices a certain degree of accuracy. Feature-based methods reduce the consumption of computing resources by extracting key points in the point cloud and constructing constraints. Although they perform better than the ICP method, they still face problems such as the instability of feature points and vector errors.

[0004] Therefore, how to improve the accuracy and reliability of lidar SLAM has become a technical problem that needs to be solved urgently by those skilled in the art. Summary of the Invention

[0005] The present invention provides a lidar SLAM method for autonomous vehicles, a lidar SLAM device for autonomous vehicles, and a lidar SLAM system for autonomous vehicles, which solve the problems of low accuracy and poor reliability of lidar SLAM in the related art.

[0006] As the first aspect of the present invention, there is provided a lidar SLAM method for autonomous vehicles, which includes:

[0007] Obtain the original lidar point cloud data, and perform adaptive roughness evaluation and feature screening on the original lidar point cloud data to obtain feature information;

[0008] Perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information;

[0009] Perform pose estimation on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, where the bimodal geometric primitive constraint mechanism is constructed based on the feature information of the current frame and the corresponding feature information in the local map;

[0010] Perform local map construction and global map construction in sequence according to the six-degree-of-freedom pose to obtain the positioning information of the autonomous driving vehicle.

[0011] Further, performing adaptive roughness evaluation and feature screening on the original lidar point cloud data to obtain feature information includes:

[0012] Project the original lidar point cloud data onto a range image;

[0013] Estimate the roughness of each point corresponding to the original lidar point cloud data in the range image according to the adaptive roughness evaluation function to obtain the roughness estimation value of each point in the range image;

[0014] Sort each point in the range image according to the roughness estimation value of each point;

[0015] Perform feature screening on the adjacent point sets in the range image to obtain feature information.

[0016] Further, estimating the roughness of each point corresponding to the original lidar point cloud data in the range image according to the adaptive roughness evaluation function includes:

[0017] Scale the geometric configuration between each point in the range image to obtain point roughness;

[0018] Perform a neighboring point search in the range image according to a distance threshold to obtain consecutive neighboring points with variable numbers of points in the same row;

[0019] Classify the neighboring points with point roughness in each row according to a roughness threshold to obtain candidate plane points and / or candidate line points.[[ID=…]]

[0020] Further, performing feature screening on the adjacent point sets in the range image includes:

[0021] Obtain the adjacent point sets in the range image according to the roughness estimation value;

[0022] Feature screening is performed on the validity of each candidate feature in the adjacent point set according to multiple adjacent points to obtain feature points and feature vectors.

[0023] Further, dynamic outlier detection is performed on the feature information to screen out outlier feature points and obtain effective feature information, including:

[0024] Search for the nearest point in the corresponding local map according to the feature category;

[0025] Perform dynamic outlier detection on the searched nearest point according to the dynamic distance threshold and the distance from the point to the plane or the point to the line to obtain preliminary feature information;

[0026] Perform feature stability evaluation on the preliminary feature information according to the vector in the local map and the feature information to obtain effective feature information.

[0027] Further, perform dynamic outlier detection on the searched nearest point according to the dynamic distance threshold and the distance from the point to the plane or the point to the line to obtain preliminary feature information, including:

[0028] Determine the dynamic distance threshold, where the expression of the dynamic distance threshold is:

[0029] δ o =e θ ||p i ||+e t ,

[0030] where, δ o represents the dynamic distance threshold, e θ represents a preset rotation error, e t represents a preset translation error, p i represents the feature point;

[0031] Compare the reprojection distance from the point to the plane and the reprojection distance from the point to the line with the dynamic distance threshold respectively to achieve dynamic outlier detection.

[0032] Further, perform feature stability evaluation on the preliminary feature information according to the vector in the local map and the feature information to obtain effective feature information, including:

[0033] Perform plane feature stability evaluation and line feature stability evaluation on the preliminary feature information respectively according to the neighborhood vector distribution in the local map and the feature vector in the feature information. Among them, the evaluation function expression of the plane feature stability is:

[0034]

[0035] Among them, and both represent the normal vectors of the K nearest points related to the planar feature in the local map;

[0036] The expression of the evaluation function for the stability of the line feature is:

[0037]

[0038] wherein, and both represent the normal vectors of the K nearest points related to the line feature in the local map.

[0039] Furthermore, attitude estimation is performed on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, including:

[0040] Construct a geometric primitive matrix based on the features in the current frame and the corresponding feature information in the local map;

[0041] Iterate the residuals from points to planes and points to lines in the geometric primitive matrix to minimize the residuals and obtain the attitude estimation value.

[0042] As another aspect of the present invention, there is provided a lidar SLAM device for an autonomous driving vehicle, which is used to implement the lidar SLAM method for an autonomous driving vehicle described above, wherein, it includes:

[0043] A feature extraction module, configured to obtain the original lidar point cloud data, and perform adaptive roughness evaluation and feature screening on the original lidar point cloud data to obtain feature information;

[0044] A feature association module, configured to perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information;

[0045] An attitude estimation module, configured to perform attitude estimation on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, wherein the bimodal geometric primitive constraint mechanism is constructed based on the feature information in the current frame and the corresponding feature information in the local map;

[0046] A map construction module, configured to perform local map construction and global map construction in sequence according to the six-degree-of-freedom pose to obtain the positioning information of the autonomous driving vehicle.

[0047] As another aspect of the present invention, there is provided a lidar SLAM system for an autonomous vehicle, which includes: a lidar device and the aforementioned lidar SLAM device for an autonomous vehicle. The lidar device is communicatively connected to the lidar SLAM device for an autonomous vehicle. The lidar device is used to collect lidar raw point cloud data, and the lidar SLAM device for an autonomous vehicle is used to perform feature extraction, feature association, pose estimation, and map construction based on the lidar raw point cloud data to obtain the positioning information of the autonomous vehicle.

[0048] The lidar SLAM method for an autonomous vehicle provided by the present invention performs a scaling process on the geometric configuration between lidar point clouds through an adaptive roughness calculation method to enhance the robustness of lidar point cloud roughness calculation and improve the stability of feature recognition. Then, a dynamic distance threshold is used to check the correspondence of each feature, thereby significantly improving the accuracy of outlier rejection. Finally, in order to reduce the influence of vector error, according to the feature vector and vector distribution of the current frame. The lidar SLAM device for an autonomous vehicle first constructs a more robust and accurate geometric metric primitive constraint framework by using highly stable feature points, thereby significantly improving the stability and accuracy of the positioning system; adopts an adaptive point cloud roughness calculation method and a technical means of dynamically adjusting the outlier threshold to further improve the stability and accuracy of feature points; in addition, combined with a bimodal geometric primitive constraint mechanism, it can effectively enhance the robustness of the system to feature vector errors, create a point cloud map containing the feature vector distribution, and use it as part of the geometric primitive constraint, which not only helps to intuitively display the spatial distribution of feature information, but also can effectively improve the accuracy and reliability of lidar SLAM. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The drawings are used to provide a further understanding of the present invention, and constitute a part of the specification. Together with the following specific embodiments, they are used to explain the present invention, but do not constitute a limitation to the present invention.

[0050] Figure 1 It is a flowchart of the lidar SLAM method for an autonomous vehicle provided by the present invention.

[0051] Figure 2 It is a flowchart of the adaptive roughness evaluation and feature screening provided by the present invention.

[0052] Figure 3 It is a flowchart of the roughness estimation provided by the present invention.

[0053] Figure 4Schematic comparison diagram of the adaptive roughness provided by the present invention and the roughness point cloud classification used in the prior art.

[0054] Figure 5 Method flowchart of the dynamic outlier detection provided by the present invention.

[0055] Figure 6 Method flowchart of the pose estimation provided by the present invention.

[0056] Figure 7 Block diagram of the lidar SLAM device for autonomous vehicles provided by the present invention.

[0057] Figure 8 Block diagram of the specific implementation structure of the lidar SLAM device for autonomous vehicles provided by the present invention.

[0058] Figure 9 Block diagram of the lidar SLAM system for autonomous vehicles provided by the present invention. Specific implementation

[0059] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments may be combined with each other. The present invention will be described in detail below with reference to the drawings and in combination with the embodiments.

[0060] In order to enable those skilled in the art to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0061] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above drawings are used to distinguish similar objects, and do not necessarily need to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present invention described here. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device comprising a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0062] In this embodiment, a lidar SLAM method for autonomous vehicles is provided. Figure 1is a flowchart of a lidar SLAM method for an autonomous vehicle according to an embodiment of the present invention, as Figure 1 shown, including:

[0063] S100. Obtain the original lidar point cloud data, and perform adaptive roughness evaluation and feature screening on the original lidar point cloud data to obtain feature information;

[0064] In an embodiment of the present invention, the original lidar point cloud data of the lidar device is obtained, and corresponding robust feature data is output after feature extraction.

[0065] S200. Perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information;

[0066] In an embodiment of the present invention, feature association is performed on the above-mentioned feature information, specifically evaluating the feature correspondence between the current frame and the local map, and removing incorrect correspondences.

[0067] S300. Perform pose estimation on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, where the bimodal geometric primitive constraint mechanism is constructed according to the feature information of the current frame and the corresponding feature information in the local map;

[0068] In an embodiment of the present invention, using the effectively associated features, the relative pose transformation between the current frame and the local map is found. To reduce the influence of unstable feature vectors on the accuracy of pose estimation, the pose can also be weighted, and a reprojection constraint is constructed using the feature stability parameters obtained from feature association. At the same time, a bimodal reprojection constraint is also constructed according to the vectors in the local map and the vectors of the current frame to improve the robustness of pose estimation.

[0069] S400. Perform local map construction and global map construction in sequence according to the six-degree-of-freedom pose to obtain the positioning information of the autonomous vehicle.

[0070] Through key frame detection, and registering the features in the key frames to the corresponding local maps according to the feature categories; in addition, through loop closure detection between key frames, and optimizing the pose graph based on loop closure constraints to improve the relative poses between historical key frames. Both the local map and the global map are composed of feature points and corresponding vectors, and the vector distribution in the map can be used for feature stability evaluation and bimodal structure construction.

[0071] In summary, the lidar SLAM method for autonomous vehicles provided by the present invention performs scaling processing on the geometric configuration between lidar point clouds through adaptive roughness calculation to enhance the robustness of lidar point cloud roughness calculation and improve the stability of feature recognition. Then, the corresponding relationship of each feature is checked using a dynamic distance threshold, thereby significantly improving the accuracy of outlier removal. Finally, in order to reduce the influence of vector error, according to the feature vector and vector distribution of the current frame. The lidar SLAM device for autonomous vehicles first constructs a more robust and accurate geodetic primitive constraint framework by using highly stable feature points, thereby significantly improving the stability and accuracy of the positioning system. It adopts an adaptive point cloud roughness calculation method and a technical means of dynamically adjusting the outlier threshold to further improve the stability and accuracy of feature points. In addition, combined with the bimodal geometric primitive constraint mechanism, it can effectively enhance the robustness of the system to feature vector errors, create a point cloud map containing the feature vector distribution, and use it as part of the geometric primitive constraint, which not only helps to intuitively display the spatial distribution of feature information, but also can effectively improve the accuracy and reliability of lidar SLAM.

[0072] In an embodiment of the present invention, after performing adaptive roughness evaluation and feature screening on the lidar raw point cloud data, feature information is obtained, as Figure 2 shown, including:

[0073] S110. Project the lidar raw point cloud data onto a ranging image;

[0074] S120. Estimate the roughness of each point corresponding to the lidar raw point cloud data in the ranging image according to the adaptive roughness evaluation function to obtain the roughness estimation value of each point in the ranging image;

[0075] It should be understood that the lidar raw point cloud data is projected onto a ranging image, and the roughness of each point is estimated using an adaptive roughness evaluation function that is robust to the lidar viewing point.

[0076] More specifically, estimating the roughness of each point corresponding to the lidar raw point cloud data in the ranging image according to the adaptive roughness evaluation function, as Figure 3 shown, including:

[0077] S121. Scale the geometric configuration between each point in the ranging image to obtain point roughness;

[0078] It should be understood that by scaling the geometric configuration between points, point roughness independent of the lidar viewing angle can be obtained. It should be understood that the local sparsity of the point cloud is closely related to the lidar viewing point. The farther away from the lidar viewing point, the sparser the point cloud in its vicinity, and the greater the difference in geometric information between adjacent points.

[0079] S122. Perform a neighboring point search in the ranging image according to a distance threshold to obtain consecutive neighboring points with variable numbers of points in the same row;

[0080] Specifically, in the neighboring point search, the distance threshold δ is specifically used d instead of a point number threshold to obtain consecutive neighboring points with variable numbers of points in the same row, which can greatly weaken the adverse impact of point cloud sparsity on roughness estimation.

[0081] In addition, p i-1 and p i+1 The geometric configuration between adjacent points on both sides is scaled so that the roughness evaluation function is insensitive to the range of points and has a roughness c that is robust to the lidar viewing angle i is defined as follows:

[0082]

[0083] where N i represents the number of adjacent points on both sides of p i The calculation formula is:

[0084]

[0085] S123. Classify the neighboring points with point roughness in each row according to a roughness threshold to obtain candidate plane points and / or candidate line points.

[0086] Specifically, use the roughness threshold δ c to classify the point cloud. As Figure 4 shown, points with roughness less than δ c are marked as candidate plane points, and vice versa as candidate line points. Since the distance threshold δ d is used to scale the geometric structure between each pair of adjacent points, the roughness c i is consistent with δ d . Therefore, the roughness threshold δ c is set by δ c = ω c δ d ω c represents the coefficient of δ d Preferably, ω c = 1.

[0087] As Figure 4As shown, the blue dots represent candidate plane points, and the red dots represent candidate line points. (a) and (b) represent the classification of roughness in the prior art, and (c) and (d) represent the classification of adaptive roughness used in the embodiments of the present invention. Under the same conditions, both (c) and (d) can accurately identify the category of points.

[0088] S130. Sort each point in the ranging image according to the estimated roughness value of each point;

[0089] S140. Perform feature screening on the adjacent point sets in the ranging image to obtain feature information.

[0090] In the embodiments of the present invention, considering that the feature classification based on roughness can only utilize the geometric information between adjacent points in a single row, PCA (Principal Components Analysis) is introduced to check the effectiveness of each candidate feature by using multiple rows of adjacent points. At the same time, the calculated feature vectors will be used for feature stability evaluation and bimodal pose constraint construction.

[0091] Specifically, performing feature screening on the adjacent point sets in the ranging image includes:

[0092] 1) Obtain the adjacent point sets in the ranging image according to the roughness estimation value;

[0093] 2) Perform feature screening on the effectiveness of each candidate feature in the adjacent point sets according to multiple rows of adjacent points to obtain feature points and feature vectors.

[0094] In the embodiments of the present invention, specifically, first, perform standardization processing on the original data. Here, standardization specifically means subtracting the value of the data point from the average value of each candidate feature and then dividing by the standard deviation; second, calculate the covariance matrix of the data. Here, the covariance matrix is used to describe the correlation between each dimension in the data. If two features are highly correlated, then the two may carry similar information, so one feature can be replaced by the other; third, calculate the eigenvalues and eigenvectors. Specifically, the eigenvectors of the covariance matrix determine the principal component directions of the data, and the corresponding eigenvalues determine the importance in these directions. The larger the eigenvalue, the greater the variance on the corresponding eigenvector; fourth, select the principal components. Specifically, according to the size of the eigenvalues, select the eigenvectors corresponding to the top k largest eigenvalues. These k eigenvectors are the bases of the new, dimension-reduced feature space; fifth, construct new feature data. Specifically, project the original data onto these eigenvectors to obtain the dimension-reduced data. These new features are called principal components, and these principal components are the stable features selected.

[0095] In the embodiment of the present invention, dynamic outlier detection is performed on the feature information to screen out outlier feature points, and effective feature information is obtained, such as Figure 5 shown in the following, including:

[0096] S210. Search for the nearest point in the corresponding local map according to the feature category;

[0097] S220. Perform dynamic outlier detection on the searched nearest point according to the dynamic distance threshold and the distance from the point to the plane or the point to the line, and obtain preliminary feature information;

[0098] It should be understood that the reproduction geometric error increases with the increase of the point range. To better identify outliers, a dynamic distance threshold δ for outlier detection is introduced o .

[0099] Specifically, performing dynamic outlier detection on the searched nearest point according to the dynamic distance threshold and the distance from the point to the plane or the point to the line, and obtaining preliminary feature information, including:

[0100] (1) Determine the dynamic distance threshold, where the expression of the dynamic distance threshold is:

[0101] δ o = e θ ||p i || + e t ,

[0102] where δ o represents the dynamic distance threshold, e θ represents the preset rotation error, e t represents the preset translation error, and p i represents the feature point;

[0103] It should be understood that the rotation error and the translation error can be continuously reduced through iterative optimization.

[0104] (2) Compare the reprojection distance from the point to the plane and the reprojection distance from the point to the line with the dynamic distance threshold respectively to implement dynamic outlier detection.

[0105] In the embodiment of the present invention, due to the limitation of the lidar resolution, it is difficult to repeatedly scan the same point during the movement. The reprojection distance from point to point is naturally large, making it difficult to reflect the true reprojection distance, and some ground objects with correct corresponding relationships are identified as outliers. Based on this, in the embodiment of the present invention, based on the vectors saved in the local map, the reprojection distance d p from the point to the plane and the reprojection distance d l from the point to the line are respectively introduced:

[0106]

[0107] Among them, represents the transformation matrix between the current frame and the local map, and respectively represent the surface feature points and line feature points of the current frame. The point is the closest point of the point in the local map after passing through the pose transformation matrix is the normal vector of the closest surface feature point Similarly, is the line feature point after passing through the pose transformation matrix the closest point in the local map, is the main vector of the line feature at the closest point For planar features, greater than δ o will be considered an incorrect association, while greater than δ o will also be considered an incorrect association.

[0108] S230. Evaluate the feature stability of the preliminary feature information according to the vectors in the local map and the feature information to obtain effective feature information.

[0109] It should be understood that the consistency of the vector distribution between adjacent features reveals the feature stability. Stable feature vectors are particularly important for constructing robust point-to-plane constraints or point-to-line constraints. Based on this, the neighborhood vector distribution in the map is used to estimate the stability of each feature.

[0110] Specifically, evaluating the feature stability of the preliminary feature information according to the vectors in the local map and the feature information to obtain effective feature information includes:

[0111] Evaluating the planar feature stability and the line feature stability of the preliminary feature information respectively according to the neighborhood vector distribution in the local map and the feature vectors in the feature information. Among them, the evaluation function expression of the planar feature stability is:

[0112]

[0113] Among them, and both represent the normal vectors of K closest points related to the planar feature in the local map;

[0114] The expression of the evaluation function of the line feature stability is:

[0115]

[0116] Among them, and both represent the normal vectors of the K nearest points in the local map related to the line feature associated with.

[0117] In the embodiment of the present invention, attitude estimation is performed on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, as Figure 6 shown, including:

[0118] S310. Construct a geometric primitive matrix according to the features in the current frame and the corresponding feature information in the local map;

[0119] S320. Iterate the residuals from points to planes and points to lines in the geometric primitive matrix to minimize the residuals and obtain an attitude estimation value.

[0120] Specifically, using the effectively associated features, find the relative pose transformation between the current frame and the local map. To reduce the influence of unstable feature vectors on the accuracy of attitude estimation, the attitude is weighted. A reprojection constraint is constructed using the feature stability parameters obtained from the feature association module. At the same time, a dual-mode reprojection constraint is also constructed according to the vectors in the local map and the vectors in the current frame to improve the robustness of attitude estimation.

[0121] In the embodiment of the present invention, the features in the current frame and the corresponding points in the local map are used to construct a geometric primitive matrix, and then the residuals from points to planes and points to lines are iteratively minimized, and finally the best transformation estimation value is output. Although in the previous embodiments, adaptive roughness and PCA filtered out the unstable features, due to the change of the lidar viewing angle and the sparsity of the point cloud, the feature vectors will be different between each frame. For the residuals from points to planes and points to lines, the gradients decrease along the normal direction of the plane and the orthogonal direction of the line vector respectively. Therefore, in the embodiment of the present invention, an attitude optimization function based on bimodal geometric primitive constraints is proposed, which is constructed using the feature vectors in the current frame and the corresponding vectors in the local map. The gradient direction is given by the sum vector of the current frame feature vector and the map feature vector, which can improve the attitude optimization function's tolerance to feature vector errors. For each line feature in the current frame The bimodal point-to-line residual can be defined as:

[0122]

[0123] Similarly, for each line feature in the current frame The bimodal point-to-line residual can be defined as:

[0124]

[0125] In addition, the feature stability parameters and are used to penalize the constraints with large inconsistencies in the feature vectors. Therefore, the pose estimation T * can be expressed as a weighted least squares optimization problem with bimodal geometric primitive constraints:

[0126]

[0127] In the embodiment of the present invention, after the above pose estimation, a six-degree-of-freedom pose is obtained. Next, the positioning of the autonomous vehicle is realized based on local map construction and global map construction in sequence.

[0128] Specifically, when constructing the local map, key frames are detected, and the features in the key frames are registered into the corresponding local maps according to the feature categories.

[0129] The local map is used for feature matching in lidar odometry. The global map provides historical environmental measurement data for loop closure detection. Both the local map and the global map contain the three-dimensional coordinates of the features and the corresponding vectors. In order to reduce the number of loop closure detections and map data at the same time, key frames are extracted based on translation increments and rotation increments. The local map is updated by adding the latest key frame and cropping the earliest key frame. For key frame cropping, it must be ensured that the number of key frames is greater than a pre-set numerical threshold, and the distance between the latest key frame and the earliest key frame is greater than a pre-set distance threshold.

[0130] In addition, for lidar global map construction, loop closure detection between key frames (specifically, this loop closure detection can be implemented using the description of loop closure detection Scan_Context) is performed, and pose graph optimization is performed based on loop closure constraints (specifically, it can be implemented using the graph optimization library GT-SAM) to improve the relative poses between historical key frames. Both the local map and the global map are composed of feature points and the corresponding vectors. The vector distribution in the map can be used for feature stability evaluation and bimodal structure construction.

[0131] The global map stores all key frame point cloud data. In order to reduce pose drift, candidate loop closure frames can be specifically used for identification. Since the pose drift between consecutive frames within a short time is small, a local map of the loop closure scene is formed by selecting a group of adjacent key frames of the candidate loop closure frames. Then, the relative transformation matrix of the loop closure is solved by minimizing the proposed weighted bimodal residual. In order to check the reliability of the loop closure constraints and remove false loop closures, the percentage threshold of the correctly associated features occupying all features is utilized. Finally, loop closure constraints and trajectory constraints are added, and a consistent global point cloud map is constructed using the refined key frame poses.

[0132] In summary, the lidar SLAM method for autonomous vehicles provided by the present invention extracts features that are not affected by the lidar viewing angle by introducing an adaptive roughness function, and improves the accuracy of the correspondence relationship through a feature association dynamic threshold strategy. The lidar SLAM method for autonomous vehicles can extract more robust features in various challenging environments, thereby significantly improving the system performance. In addition, the lidar SLAM method for autonomous vehicles also constructs a point map containing feature vectors to evaluate the stability of the features and introduces it into the weighted bimodal geometric constraint. Therefore, the lidar SLAM method for autonomous vehicles provided by the present invention can effectively improve the accuracy and reliability of lidar SLAM.

[0133] As another embodiment of the present invention, there is provided a lidar SLAM device 100 for autonomous vehicles, which is used to implement the lidar SLAM method for autonomous vehicles described above. Among them, as Figure 7 shown, it includes:

[0134] A feature extraction module 110, configured to obtain lidar raw point cloud data, and perform adaptive roughness evaluation and feature screening on the lidar raw point cloud data to obtain feature information;

[0135] A feature association module 120, configured to perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information;

[0136] An attitude estimation module 130, configured to perform attitude estimation on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, where the bimodal geometric primitive constraint mechanism is constructed according to the feature information of the current frame and the corresponding feature information in the local map;

[0137] A map construction module 140, configured to perform local map construction and global map construction in sequence according to the six-degree-of-freedom pose to obtain the positioning information of the autonomous vehicle.

[0138] The lidar SLAM device for autonomous vehicles provided by the present invention performs scaling processing on the geometric configuration between lidar point clouds through an adaptive roughness calculation method to enhance the robustness of lidar point cloud roughness calculation and improve the stability of feature recognition. Then, the corresponding relationship of each feature is checked using a dynamic distance threshold, thereby significantly improving the accuracy of outlier removal. Finally, in order to reduce the influence of vector error, based on the feature vector and vector distribution of the current frame. This lidar SLAM method for autonomous vehicles first constructs a more robust and accurate geometric metric primitive constraint framework by using highly stable feature points, thereby significantly improving the stability and accuracy of the positioning system. It adopts an adaptive point cloud roughness calculation method and a technique for dynamically adjusting the outlier threshold, further improving the stability and accuracy of feature points. Additionally, combined with a bimodal geometric primitive constraint mechanism, it can effectively enhance the robustness of the system to feature vector errors, create a point cloud map containing the feature vector distribution, and utilize it as part of the geometric primitive constraint. This not only helps to intuitively display the spatial distribution of feature information but also can effectively improve the accuracy and reliability of lidar SLAM.

[0139] Specifically, as Figure 8 shown, it is the structural block diagram of the specific implementation manner of the lidar SLAM device for autonomous vehicles. In this implementation manner, in the feature extraction module, an adaptive roughness function is introduced to extract features that are not affected by the lidar viewing point. Additionally, in the feature association module, the accuracy of the corresponding relationship is improved through a feature association dynamic threshold strategy. In the pose estimation module, a point map containing feature vectors is constructed to evaluate the stability of features and introduced into the weighted bimodal geometric constraint. This weighted bimodal pose estimation can effectively enhance the robustness of the system to feature vector errors, thereby enabling more robust features to be extracted in various challenging environments and achieving the purpose of significantly improving system performance. Finally, the positioning information of the autonomous vehicle is obtained through local map construction and global map construction, achieving the purpose of improving the accuracy and reliability of lidar SLAM.

[0140] Regarding the specific working principle of the lidar SLAM device for autonomous vehicles of the present invention, reference can be made to the description of the lidar SLAM method for autonomous vehicles in the previous text, which will not be elaborated here.

[0141] As another embodiment of the present invention, a lidar SLAM system 10 for autonomous vehicles is provided, wherein, as Figure 9As shown in the figure, it includes: a lidar device 200 and the aforementioned lidar SLAM device 100 for autonomous vehicles. The lidar device 200 is communicatively connected to the lidar SLAM device 100 for autonomous vehicles. The lidar device 200 is used to collect lidar raw point cloud data, and the lidar SLAM device 100 for autonomous vehicles is used to perform feature extraction, feature association, pose estimation, and map construction based on the lidar raw point cloud data to obtain the positioning information of the autonomous vehicle.

[0142] The lidar SLAM system for autonomous vehicles provided by the present invention performs scaling processing on the geometric configuration between lidar point clouds through an adaptive roughness calculation method to enhance the robustness of lidar point cloud roughness calculation and improve the stability of feature recognition. Then, a dynamic distance threshold is used to check the correspondence of each feature, thereby significantly improving the accuracy of outlier rejection. Finally, in order to reduce the influence of vector error, according to the feature vector and vector distribution of the current frame. The lidar SLAM system for autonomous vehicles first constructs a more robust and accurate geodetic primitive constraint framework by using highly stable feature points, thereby significantly improving the stability and accuracy of the positioning system; adopts an adaptive point cloud roughness calculation method and a technical means of dynamically adjusting the outlier threshold to further improve the stability and accuracy of feature points; in addition, combined with a bimodal geometric primitive constraint mechanism, it can effectively enhance the robustness of the system to feature vector errors, create a point cloud map containing the feature vector distribution, and use it as part of the geometric primitive constraint, which not only helps to intuitively display the spatial distribution of feature information, but also can effectively improve the accuracy and reliability of lidar SLAM.

[0143] Regarding the specific working principle of the lidar SLAM system for autonomous vehicles provided by the present invention, reference can be made to the description of the lidar SLAM method for autonomous vehicles above, and details are not described herein again.

[0144] It can be understood that the above embodiments are merely exemplary embodiments adopted to illustrate the principle of the present invention. However, the present invention is not limited thereto. For those of ordinary skill in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also regarded as the protection scope of the present invention.

Claims

1. A lidar SLAM method for autonomous vehicles, characterized in that, Including: Obtaining the original point cloud data of the lidar, and obtaining feature information after performing adaptive roughness evaluation and feature screening on the original point cloud data of the lidar; Performing dynamic outlier detection on the feature information to screen out outlier feature points, and obtaining effective feature information; Performing pose estimation on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, where the bimodal geometric primitive constraint mechanism is constructed based on the feature information of the current frame and the corresponding feature information in the local map; Successively performing local map construction and global map construction according to the six-degree-of-freedom pose to obtain the positioning information of the autonomous driving vehicle.

2. The lidar SLAM method for autonomous vehicles according to claim 1, wherein Obtaining feature information after performing adaptive roughness evaluation and feature screening on the original point cloud data of the lidar, including: Projecting the original point cloud data of the lidar onto a range image; Estimating the roughness of each point corresponding to the original point cloud data of the lidar in the range image according to an adaptive roughness evaluation function, and obtaining the roughness estimation value of each point in the range image; Sorting each point in the range image according to the roughness estimation value of each point; Performing feature screening on adjacent point sets in the range image to obtain feature information.

3. The lidar SLAM method for an autonomous vehicle according to claim 2, wherein, Estimating the roughness of each point corresponding to the original point cloud data of the lidar in the range image according to an adaptive roughness evaluation function, including: Scaling the geometric configuration between each point in the range image to obtain point roughness; Performing a neighboring point search in the range image according to a distance threshold to obtain continuous neighboring points with variable numbers of points in the same row; Classifying the neighboring points with point roughness in each row according to a roughness threshold to obtain candidate plane points and / or candidate line points.

4. The lidar SLAM method for an autonomous vehicle according to claim 2, wherein Performing feature screening on adjacent point sets in the range image, including: Obtaining adjacent point sets in the range image according to the roughness estimation value; Performing feature screening on the effectiveness of each candidate feature in the adjacent point set according to multiple rows of adjacent points to obtain feature points and feature vectors.

5. The lidar SLAM method for an autonomous vehicle according to any one of claims 1 to 4, characterized in that, Performing dynamic outlier detection on the feature information to screen out outlier feature points, and obtaining effective feature information, including: Searching for the nearest points in the corresponding local map according to the feature category; Performing dynamic outlier detection on the searched nearest points according to a dynamic distance threshold and the distance from the point to the plane or the point to the line, and obtaining preliminary feature information; Performing feature stability evaluation on the preliminary feature information according to the vector in the local map and the feature information to obtain effective feature information.

6. The lidar SLAM method for an autonomous vehicle according to claim 5, wherein, Performing dynamic outlier detection on the searched nearest points according to a dynamic distance threshold and the distance from the point to the plane or the point to the line, and obtaining preliminary feature information, including: Determining a dynamic distance threshold, where the expression of the dynamic distance threshold is: δ o = e θ ||p i || + e t , Among them, δ o represents the dynamic distance threshold, e θ represents the preset rotation error, e t represents the preset translation error, p i represents the feature point; Comparing the reprojection distance from the point to the plane and the reprojection distance from the point to the line with the dynamic distance threshold respectively to achieve dynamic outlier detection.

7. The lidar SLAM method for an autonomous vehicle according to claim 5, wherein Performing feature stability evaluation on the preliminary feature information according to the vector in the local map and the feature information to obtain effective feature information, including: Perform planar feature stability evaluation and line feature stability evaluation on the preliminary feature information respectively according to the neighborhood vector distribution in the local map and the feature vectors in the feature information. Among them, the expression of the evaluation function for the planar feature stability is as follows: Among them, and both represent the normal vectors of the K nearest points in the local map related to the planar feature ; The expression of the evaluation function for the line feature stability is as follows: Among them, and both represent the normal vectors of the K nearest points in the local map related to the line feature respectively.

8. The lidar SLAM method for an autonomous vehicle according to any one of claims 1 to 4, characterized in that, Perform pose estimation on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, including: Construct a geometric primitive matrix according to the features in the current frame and the corresponding feature information in the local map; Iterate the residuals from points to planes and points to lines in the geometric primitive matrix to minimize the residuals and obtain the pose estimation value.

9. A lidar SLAM device for an autonomous vehicle, which is used to implement the lidar SLAM method for an autonomous vehicle according to any one of claims 1 to 8, characterized in that, Including: A feature extraction module, configured to obtain the original lidar point cloud data, and perform adaptive roughness evaluation and feature screening on the original lidar point cloud data to obtain feature information; A feature association module, configured to perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information; A pose estimation module, configured to perform pose estimation on the effective feature information according to the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, where the bimodal geometric primitive constraint mechanism is constructed according to the feature information in the current frame and the corresponding feature information in the local map; A map construction module, configured to perform local map construction and global map construction in sequence according to the six-degree-of-freedom pose to obtain the positioning information of the autonomous vehicle.

10. A lidar SLAM system for an autonomous vehicle, characterized in that, Including: A lidar device and the lidar SLAM device for an autonomous vehicle according to claim 9. The lidar device is communicatively connected to the lidar SLAM device for an autonomous vehicle. The lidar device is configured to collect the original lidar point cloud data, and the lidar SLAM device for an autonomous vehicle is configured to perform feature extraction, feature association, pose estimation, and map construction according to the original lidar point cloud data to obtain the positioning information of the autonomous vehicle.

Images