Lidar slam method, apparatus, and system for autonomous vehicle

Abstract

The application 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, which comprises the following steps: acquiring laser radar original point cloud data, and obtaining feature information after adaptive roughness evaluation and feature screening of the laser radar original point cloud data; performing dynamic outlier detection on the feature information to screen out outlier feature points and obtain effective feature information; performing pose estimation on the effective feature information according to a double-peak geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, wherein the double-peak geometric primitive constraint mechanism is obtained by constructing corresponding feature information in a local map according to feature information of a current frame; sequentially performing local map construction and global map construction according to the six-degree-of-freedom pose to obtain positioning information of the automatic driving vehicle. The laser radar SLAM method for the automatic driving vehicle can improve the precision and reliability of laser radar SLAM.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving positioning technology, and in particular to a lidar SLAM method, lidar SLAM device, and lidar SLAM system for autonomous vehicles. Background Technology

[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 typically equipped with various sensors, such as GNSS (Global Navigation Satellite System), IMU (Inertial Measurement Unit), cameras, and LiDAR. In areas with poor GNSS signals, such as urban canyons or underground parking garages, pre-built high-precision maps can help solve attitude estimation problems, but this method is not suitable for scenarios with significant environmental changes. Therefore, Simultaneous Localization and Mapping (SLAM) is crucial for adapting to unknown or dynamically changing environments.

[0003] In recent years, while vision-based methods have seen some applications in robotics, their susceptibility to lighting conditions and inaccurate depth estimation make them unsuitable for autonomous driving. In contrast, LiDAR is unaffected by lighting conditions and provides precise geometric information. LiDAR SLAM consists of front-end odometry and back-end optimization. Front-end odometry is mainly divided into ICP (Iterative Closest Point) based methods and feature extraction based methods. ICP methods directly use raw point cloud data for registration, resulting in high computational costs and difficulty in real-time processing. The NDT (Normal Distribution Transform) algorithm reduces the computational burden by introducing voxel filtering, but at the cost of some accuracy. Feature-based methods reduce computational resource consumption by extracting key points from the point cloud and constructing constraints. Although they outperform ICP methods, they still face issues of feature point instability and vector errors.

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

[0005] This invention provides a lidar SLAM method, a lidar SLAM device, and a lidar SLAM system for autonomous vehicles, solving the problems of low accuracy and poor reliability of lidar SLAM in related technologies.

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

[0007] The raw point cloud data of the lidar is acquired, and feature information is obtained by adaptive roughness evaluation and feature filtering of the raw point cloud data of the lidar.

[0008] Dynamic outlier detection is performed on the feature information to filter out outlier feature points and obtain effective feature information;

[0009] The effective feature information is used to estimate the pose based on the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose. 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] Based on the six degrees of freedom pose, local map construction and global map construction are performed sequentially to obtain the positioning information of the autonomous vehicle.

[0011] Furthermore, feature information is obtained by performing adaptive roughness evaluation and feature filtering on the raw point cloud data of the lidar, including:

[0012] The raw point cloud data of the lidar is projected onto the ranging image;

[0013] The roughness of each point in the ranging image corresponding to the original point cloud data of the lidar is estimated according to the adaptive roughness evaluation function, so as to obtain the roughness estimate value of each point in the ranging image.

[0014] Sort each point in the ranging image according to the roughness estimate of each point;

[0015] Feature filtering is performed on the set of adjacent points in the ranging image to obtain feature information.

[0016] Further, roughness estimation is performed on each point in the ranging image corresponding to the original point cloud data of the lidar according to the adaptive roughness evaluation function, including:

[0017] Scaling the geometric configuration between points in the ranging image to obtain point roughness;

[0018] Based on a distance threshold, a neighbor search is performed in the ranging image to obtain consecutive neighbor points with a variable number of points in the same row;

[0019] The neighboring points with point roughness in each row are classified according to a roughness threshold to obtain candidate planar points and / or candidate straight points.

[0020] Further, feature filtering is performed on the set of neighboring points in the ranging image, including:

[0021] The set of neighboring points in the ranging image is obtained based on the roughness estimate;

[0022] Feature filtering is performed on the effectiveness of each candidate feature in the set of neighboring points based on the multiple rows of neighboring points to obtain feature points and feature vectors.

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

[0024] Search for the nearest point in the corresponding local map based on the feature category;

[0025] Dynamic outlier detection is performed on the nearest searched point based on 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] Based on the vectors in the local map and the aforementioned feature information, the stability of the preliminary feature information is evaluated to obtain effective feature information.

[0027] Furthermore, based on a dynamic distance threshold and the distance from a point to a plane or a point to a line, dynamic outlier detection is performed on the nearest searched point to obtain preliminary feature information, including:

[0028] Determine the dynamic distance threshold, wherein the expression for the dynamic distance threshold is:

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

[0030] Where, δ o Indicates the dynamic distance threshold, e θ e represents the preset rotational error. t p represents the preset translation error. i Represent feature points;

[0031] The reprojection distance from a point to a plane and the reprojection distance from a point to a line are compared with the dynamic distance threshold to achieve dynamic outlier detection.

[0032] Further, based on the vectors in the local map and the aforementioned feature information, a feature stability evaluation is performed on the preliminary feature information to obtain effective feature information, including:

[0033] Based on the neighborhood vector distribution in the local map and the feature vectors in the feature information, the preliminary feature information is evaluated for planar feature stability and line feature stability, respectively. The evaluation function expression for the planar feature stability is as follows:

[0034]

[0035] in, and Both represent planar features in a local map. The normal vectors of the K nearest points;

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

[0037]

[0038] in, and All represent line features in a local map. The normal vectors of the K nearest points.

[0039] Furthermore, attitude estimation is performed on the effective feature information based on 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] The residuals from points to surfaces and from points to lines in the geometric primitive matrix are iterated to minimize the residuals and obtain the attitude estimate.

[0042] As another aspect of the present invention, a LiDAR SLAM device for autonomous vehicles is provided, for implementing the LiDAR SLAM method for autonomous vehicles described above, wherein the device includes:

[0043] The feature extraction module is used to acquire the raw point cloud data of the lidar, and to obtain feature information by performing adaptive roughness evaluation and feature filtering on the raw point cloud data of the lidar.

[0044] The feature association module is used to perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information;

[0045] The attitude estimation module is used 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 of the current frame and the corresponding feature information in the local map.

[0046] The map building module is used to sequentially build local and global maps based on the six degrees of freedom poses to obtain the positioning information of the autonomous vehicle.

[0047] As another aspect of the present invention, a lidar SLAM system for autonomous vehicles is provided, comprising: a lidar device and the lidar SLAM device for autonomous vehicles described above, wherein the lidar device is communicatively connected to the lidar SLAM device for autonomous vehicles, the lidar device is used to collect raw lidar point cloud data, and the lidar SLAM device for autonomous vehicles is used to obtain the positioning information of autonomous vehicles by performing feature extraction, feature association, attitude estimation, and map construction based on the raw lidar point cloud data.

[0048] The LiDAR SLAM method for autonomous vehicles provided by this invention scales the geometric configuration between LiDAR point clouds using adaptive roughness calculation to enhance the robustness of LiDAR point cloud roughness calculation and improve the stability of feature recognition. Then, it uses a dynamic distance threshold to check the correspondence of each feature, thereby significantly improving the accuracy of outlier removal. Finally, it reduces the impact of vector errors based on the feature vectors and vector distribution of the current frame. This LiDAR SLAM device for autonomous vehicles first constructs a more robust and accurate original constraint framework for geometrics by utilizing highly stable feature points, thereby significantly improving the stability and accuracy of the localization system. It employs an adaptive point cloud roughness calculation method and a technique for dynamically adjusting the outlier threshold to further improve the stability and accuracy of feature points. Furthermore, by combining a bimodal geometric primitive constraint mechanism, it effectively enhances the system's robustness to feature vector errors, creating a point cloud map containing the feature vector distribution and utilizing it as part of the geometric primitive constraints. This not only helps to intuitively display the spatial distribution of feature information but also effectively improves the accuracy and reliability of LiDAR SLAM. Attached Figure Description

[0049] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the following detailed description to explain the invention, but do not constitute a limitation thereof.

[0050] Figure 1 A flowchart of a lidar SLAM method for autonomous vehicles provided by the present invention.

[0051] Figure 2 The flowchart for adaptive roughness evaluation and feature selection provided by the present invention.

[0052] Figure 3 A flowchart for roughness estimation provided by the present invention.

[0053] Figure 4This is a comparative diagram showing the adaptive roughness-based roughness point cloud classification provided by the present invention and the roughness point cloud classification used in the prior art.

[0054] Figure 5 The flowchart of the dynamic outlier detection method provided by the present invention is shown.

[0055] Figure 6 A flowchart of the attitude estimation method provided by the present invention.

[0056] Figure 7 This is a structural block diagram of a lidar SLAM device for autonomous vehicles provided by the present invention.

[0057] Figure 8 The diagram shows a structural block diagram of a specific embodiment of the lidar SLAM device for autonomous vehicles provided by the present invention.

[0058] Figure 9 This is a structural block diagram of a lidar SLAM system for autonomous vehicles provided by the present invention. Detailed Implementation

[0059] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

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

[0061] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of the invention described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0062] This embodiment provides a LiDAR SLAM method for autonomous vehicles. Figure 1This is a flowchart of a LiDAR SLAM method for autonomous vehicles provided according to an embodiment of the present invention, such as... Figure 1 As shown, it includes:

[0063] S100: Obtain the original point cloud data of the lidar, and obtain feature information by performing adaptive roughness evaluation and feature filtering on the original point cloud data of the lidar.

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

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

[0066] In this embodiment of the 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. The effective feature information is used to estimate the pose 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 of the current frame and the corresponding feature information in the local map.

[0068] In this embodiment of the invention, the relative pose transformation between the current frame and the local map is found by utilizing effectively correlated features. To reduce the impact of unstable feature vectors on the accuracy of pose estimation, the pose can be weighted, and reprojection constraints can be constructed using feature stability parameters obtained from feature correlation. Simultaneously, a bimodal reprojection constraint is constructed based on vectors in the local map and vectors in the current frame to improve the robustness of pose estimation.

[0069] S400: Based on the six degrees of freedom pose, perform local map construction and global map construction in sequence to obtain the positioning information of the autonomous vehicle.

[0070] Keyframe detection is performed, and features from keyframes are registered to the corresponding local maps based on feature categories. Additionally, loop closure detection between keyframes is conducted, and pose map optimization is performed based on loop closure constraints to refine the relative poses between historical keyframes. Both the local and global maps consist of feature points and corresponding vectors; the vector distribution in the maps can be used for feature stability evaluation and bimodal structure construction.

[0071] In summary, the LiDAR SLAM method for autonomous vehicles provided by this invention scales the geometric configuration between LiDAR point clouds using adaptive roughness calculation to enhance the robustness of LiDAR point cloud roughness calculation and improve the stability of feature recognition. Then, it uses a dynamic distance threshold to check the correspondence of each feature, thereby significantly improving the accuracy of outlier removal. Finally, it reduces the impact of vector errors based on the feature vectors and vector distribution of the current frame. This LiDAR SLAM device for autonomous vehicles first constructs a more robust and accurate original constraint framework for geometrics by utilizing highly stable feature points, thereby significantly improving the stability and accuracy of the localization system. It employs 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. Furthermore, by combining a bimodal geometric primitive constraint mechanism, it effectively enhances the system's robustness to feature vector errors, creating a point cloud map containing the feature vector distribution and utilizing it as part of the geometric primitive constraints. This not only helps to intuitively display the spatial distribution of feature information but also effectively improves the accuracy and reliability of LiDAR SLAM.

[0072] In this embodiment of the invention, feature information is obtained by performing adaptive roughness evaluation and feature filtering on the original point cloud data of the lidar, such as... Figure 2 As shown, it includes:

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

[0074] S120. Roughness estimation is performed on each point in the ranging image corresponding to the original point cloud data of the lidar 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 raw point cloud data of the lidar is projected onto the ranging image, and the roughness of each point is estimated using an adaptive roughness evaluation function that is robust to the lidar viewpoint.

[0076] More specifically, the roughness is estimated for each point in the ranging image corresponding to the original point cloud data of the lidar, based on an adaptive roughness evaluation function, such as... Figure 3 As shown, it includes:

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

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

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

[0080] Specifically, in the nearest neighbor search, a distance threshold δ is used. d Instead of using a point count threshold to obtain consecutive neighboring points with a variable number of points in the same row, the adverse effects of point cloud sparsity on roughness estimation can be greatly reduced.

[0081] In addition, p i-1 and p i+1 The geometric configuration between adjacent points on both sides is scaled, making the roughness evaluation function insensitive to the range of points, resulting in a roughness c that is robust to the lidar viewpoint. i The definition is as follows:

[0082]

[0083] Where, N i p i The number of adjacent points on both sides is calculated using the following formula:

[0084]

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

[0086] Specifically, using the roughness threshold δ c Classify point clouds. For example... Figure 4 As shown, the roughness is less than δ c Points that are true or false are marked as candidate planar points, while those that are false or false are marked as candidate straight-line points. This is because a distance threshold δ is used. d To scale the geometry between each pair of adjacent points, thus the roughness c i With δ d They are consistent. Therefore, the roughness threshold δ c From δ c =ω c δ d Settings. ω c δ d The coefficient, preferably, ω c =1.

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

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

[0089] S140. Perform feature filtering on the set of adjacent points in the ranging image to obtain feature information.

[0090] In this embodiment of the invention, considering that roughness-based feature classification can only utilize the geometric information between adjacent points in a single row, PCA (Principal Component Analysis) is introduced to use adjacent points in multiple rows to check the validity of each candidate feature. Simultaneously, the calculated feature vectors will be used for feature stability evaluation and bimodal attitude constraint construction.

[0091] Specifically, feature filtering is performed on the set of adjacent points in the ranging image, including:

[0092] 1) Obtain the set of neighboring points in the ranging image based on the roughness estimate;

[0093] 2) Based on the effectiveness of each candidate feature in the set of adjacent points, feature filtering is performed to obtain feature points and feature vectors.

[0094] In this embodiment of the invention, specifically, firstly, the original data is standardized, where standardization specifically refers to subtracting the value of the data point from the average value of each candidate feature and then dividing by the standard deviation; secondly, the covariance matrix of the data is calculated, where the covariance matrix is ​​used to describe the correlation between various dimensions in the data. If two features are highly correlated, they may carry similar information, so one feature can be replaced by the other; thirdly, eigenvalues ​​and eigenvectors are calculated, where the eigenvectors of the covariance matrix determine the principal component directions of the data, and the corresponding eigenvalues ​​determine the importance of these directions. The larger the eigenvalue, the larger the variance on the corresponding eigenvector; fourthly, principal components are selected, specifically, based on the magnitude of the eigenvalues, the eigenvectors corresponding to the k largest eigenvalues ​​are selected, and these k eigenvectors are the basis of the new, dimensionality-reduced feature space; fifthly, new feature data is constructed, specifically, the original data is projected onto these eigenvectors to obtain the dimensionality-reduced data. These new features are called principal components, and these principal components are the selected stable features.

[0095] In this embodiment of the invention, dynamic outlier detection is performed on the feature information to filter out outlier feature points and obtain effective feature information, such as... Figure 5 As shown, it includes:

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

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

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

[0099] Specifically, dynamic outlier detection is performed on the nearest searched points based on a dynamic distance threshold and the distance from a point to a plane or a point to a line to obtain preliminary feature information, including:

[0100] (1) Determine the dynamic distance threshold, wherein the expression for the dynamic distance threshold is:

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

[0102] Where, δ o Indicates the dynamic distance threshold, e θ e represents the preset rotational error. t p represents the preset translation error. i Represent feature points;

[0103] It should be understood that rotational and translational errors can be continuously reduced through iterative optimization.

[0104] (2) The reprojection distance from the point to the plane and the reprojection distance from the point to the line are compared with the dynamic distance threshold respectively to realize dynamic outlier detection.

[0105] In this embodiment of the invention, due to the resolution limitations of the lidar, it is difficult to repeatedly scan the same point during movement. The point-to-point reprojection distance is naturally large, making it difficult to reflect the true reprojection distance, and some correctly corresponding ground features are identified as outliers. Therefore, this embodiment of the invention uses vectors already stored in the local map to calculate the point-to-plane reprojection distance d. p and the reprojection distance d from the point to the line l Introduced separately:

[0106]

[0107] in, This represents the transformation matrix between the current frame and the local map. and These represent the surface feature points and line feature points of the current frame, respectively. The point is Point passing through pose transformation matrix The nearest point on the local map. The closest point to the surface feature The normal vector. Similarly, Line feature points After pose transformation matrix Then at the nearest point on the local map, Line features at the nearest point The principal vector. For planar features, Greater than δ o This would be considered an incorrect association, and Greater than δ o It would also be considered an incorrect association.

[0108] S230. Based on the vectors in the local map and the feature information, perform a feature stability evaluation on the preliminary feature information to obtain effective feature information.

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

[0110] Specifically, based on the vectors in the local map and the aforementioned feature information, a feature stability evaluation is performed on the preliminary feature information to obtain effective feature information, including:

[0111] Based on the neighborhood vector distribution in the local map and the feature vectors in the feature information, the preliminary feature information is evaluated for planar feature stability and line feature stability, respectively. The evaluation function expression for the planar feature stability is as follows:

[0112]

[0113] in, and Both represent planar features in a local map. The normal vectors of the K nearest points;

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

[0115]

[0116] in, and All represent line features in a local map. The normal vectors of the K nearest points.

[0117] In this embodiment of the invention, attitude estimation is performed on the effective feature information based on the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, such as... Figure 6 As shown, it includes:

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

[0119] S320. Iterate through the residuals from points to surfaces and from points to lines in the geometric primitive matrix to minimize the residuals and obtain the attitude estimate.

[0120] Specifically, by utilizing effectively correlated features, the relative pose transformation between the current frame and the local map is found. To reduce the impact of unstable feature vectors on the accuracy of pose estimation, the pose is weighted. Reprojection constraints are constructed using feature stability parameters obtained from the feature association module. Simultaneously, bimodal reprojection constraints are constructed based on vectors in the local map and vectors in the current frame to improve the robustness of pose estimation.

[0121] In this embodiment of the invention, a geometric primitive matrix is ​​constructed using features from the current frame and corresponding points in the local map. Then, the residuals from point to surface and point to line are iteratively minimized, ultimately outputting the optimal transformation estimate. Although adaptive roughness and PCA filter out unstable features in the previous embodiments, the feature vectors differ between frames due to variations in the LiDAR viewpoint and the sparsity of the point cloud. For the residuals from point to surface and point to line, the gradient decreases along the normal direction of the plane and the orthogonal direction of the line vector, respectively. Therefore, this embodiment proposes a pose optimization function based on bimodal geometric primitive constraints, constructed using feature vectors from the current frame and corresponding vectors in the local map. The gradient direction is given by the sum of the feature vectors from the current frame and the map feature vectors, which improves the pose optimization function's accuracy against 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, using characteristic stability parameters and Constraints with significant inconsistencies in eigenvectors are penalized. Therefore, the pose estimation T * This can be represented as a weighted least squares optimization problem with bimodal geometric primitive constraints:

[0126]

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

[0128] Specifically, during local map construction, keyframe detection is performed, and features in the keyframes are registered into the corresponding local maps based on feature categories.

[0129] The local map is used for feature matching in the LiDAR rangefinding method. The global map provides historical environmental measurement data for loop closure detection. Both the local and global maps contain the 3D coordinates and corresponding vectors of the features. To reduce the number of loop closure detections while reducing map data, keyframes are extracted based on translation and rotation increments. The local map is updated by adding the latest keyframe and cropping the oldest keyframe. For keyframe cropping, the number of keyframes must be greater than a pre-set numerical threshold, and the distance between the latest and oldest keyframes must be greater than a pre-set distance threshold.

[0130] Furthermore, when constructing a global map for LiDAR, loop closure detection is performed between keyframes (specifically, this loop closure detection can be implemented using the Scan_Context description loop closure detection method), and attitude graph optimization is performed based on loop closure constraints (specifically, this can be implemented using the graph optimization library GT-SAM) to improve the relative attitude between historical keyframes. Both the local and global maps consist of feature points and corresponding vectors, and the vector distribution in the map can be used for feature stability evaluation and bimodal structure construction.

[0131] A global map stores all keyframe point cloud data. To reduce pose drift, candidate loop-closing frames are identified. Since pose drift between consecutive frames is small in a short period, a local map of the loop-closing scene is formed by selecting a set of adjacent keyframes from the candidate loop-closing frames. The relative transformation matrix of the loop closure is then solved by minimizing the proposed weighted bimodal residual. To check the reliability of the loop closure constraints and remove false loops, a threshold of the percentage of correctly associated features occupying all features is used. Finally, loop closure constraints and trajectory constraints are added, and a consistent global point cloud map is constructed using refined keyframe poses.

[0132] In summary, the LiDAR SLAM method for autonomous vehicles provided by this invention introduces an adaptive roughness function to extract features unaffected by the LiDAR viewpoint; and improves the accuracy of correspondences through a feature association dynamic threshold strategy. This LiDAR SLAM method for autonomous vehicles can extract more robust features in various challenging environments, thereby significantly improving system performance. Furthermore, this LiDAR SLAM method for autonomous vehicles also constructs a point graph containing feature vectors to evaluate feature stability and incorporates it into a weighted bimodal geometric constraint. Therefore, the LiDAR SLAM method for autonomous vehicles provided by this invention can effectively improve the accuracy and reliability of LiDAR SLAM.

[0133] As another embodiment of the present invention, a LiDAR SLAM device 100 for autonomous vehicles is provided, for implementing the LiDAR SLAM method for autonomous vehicles described above, wherein, as Figure 7 As shown, it includes:

[0134] The feature extraction module 110 is used to acquire the original point cloud data of the lidar, and to obtain feature information by performing adaptive roughness evaluation and feature filtering on the original point cloud data of the lidar.

[0135] The feature association module 120 is used to perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information;

[0136] The attitude estimation module 130 is used 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 of the current frame and the corresponding feature information in the local map.

[0137] The map building module 140 is used to sequentially build local maps and global maps based on the six degrees of freedom pose to obtain the positioning information of the autonomous vehicle.

[0138] The LiDAR SLAM device for autonomous vehicles provided by this invention scales the geometric configuration between LiDAR point clouds using adaptive roughness calculation to enhance the robustness of LiDAR point cloud roughness calculation and improve the stability of feature recognition. Then, it uses a dynamic distance threshold to check the correspondence of each feature, thereby significantly improving the accuracy of outlier removal. Finally, it reduces the impact of vector errors based on the feature vectors and vector distribution of the current frame. This LiDAR SLAM method for autonomous vehicles first constructs a more robust and accurate original constraint framework for geometrics by utilizing highly stable feature points, thus significantly improving the stability and accuracy of the localization system. It employs 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. Furthermore, by combining a bimodal geometric primitive constraint mechanism, it effectively enhances the system's robustness to feature vector errors, creating a point cloud map containing the feature vector distribution and utilizing it as part of the geometric primitive constraints. This not only helps to intuitively display the spatial distribution of feature information but also effectively improves the accuracy and reliability of LiDAR SLAM.

[0139] Specifically, such as Figure 8 The diagram shows a structural block diagram of a specific implementation of a LiDAR SLAM device for autonomous vehicles. In this implementation, the feature extraction module introduces an adaptive roughness function to extract features unaffected by the LiDAR viewpoint. Furthermore, in the feature association module, a dynamic threshold strategy for feature association improves the accuracy of correspondences. In the pose estimation module, a point map containing feature vectors is constructed to evaluate feature stability and is incorporated into a weighted bimodal geometric constraint. This weighted bimodal pose estimation effectively enhances the system's robustness to feature vector errors, enabling the extraction of more robust features in various challenging environments and significantly improving system performance. Finally, local and global map construction is used to obtain the autonomous vehicle's positioning information, thereby improving the accuracy and reliability of LiDAR SLAM.

[0140] The specific working principle of the lidar SLAM device for autonomous vehicles of the present invention can be referred to the description of the lidar SLAM method for autonomous vehicles above, and will not be repeated here.

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

[0142] The LiDAR SLAM system for autonomous vehicles provided by this invention scales the geometric configuration between LiDAR point clouds using adaptive roughness calculation to enhance the robustness of LiDAR point cloud roughness calculation and improve the stability of feature recognition. Then, it uses a dynamic distance threshold to check the correspondence of each feature, thereby significantly improving the accuracy of outlier removal. Finally, it reduces the impact of vector errors based on the feature vectors and vector distribution of the current frame. This LiDAR SLAM system for autonomous vehicles first constructs a more robust and accurate original constraint framework for geometrics using highly stable feature points, thus significantly improving the stability and accuracy of the localization system. It employs 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. Furthermore, by combining a bimodal geometric primitive constraint mechanism, it effectively enhances the system's robustness to feature vector errors, creating a point cloud map containing the feature vector distribution and utilizing it as part of the geometric primitive constraints. This not only helps to intuitively display the spatial distribution of feature information but also effectively improves the accuracy and reliability of LiDAR SLAM.

[0143] The specific working principle of the LiDAR SLAM system for autonomous vehicles provided by this invention can be found in the description of the LiDAR SLAM method for autonomous vehicles above, and will not be repeated here.

[0144] It is understood that the above embodiments are merely exemplary implementations used to illustrate the principles of the present invention, and the present invention is not limited thereto. For those skilled 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 considered to be within the scope of protection of the present invention.

Claims

1. A lidar SLAM method for autonomous vehicles, characterized in that, include: The raw point cloud data of the lidar is acquired, and feature information is obtained by adaptive roughness evaluation and feature filtering of the raw point cloud data of the lidar. Dynamic outlier detection is performed on the feature information to filter out outlier feature points and obtain effective feature information; The effective feature information is used to estimate the pose based on the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose. 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. Based on the six degrees of freedom pose, local map construction and global map construction are performed sequentially to obtain the positioning information of the autonomous vehicle; Dynamic outlier detection is performed on the aforementioned feature information to filter out outlier feature points and obtain effective feature information, including: Search for the nearest point in the corresponding local map based on the feature category; Dynamic outlier detection is performed on the nearest searched point based on the dynamic distance threshold and the distance from the point to the plane or the point to the line to obtain preliminary feature information; Based on the vectors in the local map and the aforementioned feature information, the feature stability of the preliminary feature information is evaluated to obtain effective feature information; Dynamic outlier detection is performed on the nearest searched points based on a dynamic distance threshold and the distance from the point to the plane or from the point to the line to obtain preliminary feature information, including: Determine the dynamic distance threshold, wherein the expression for the dynamic distance threshold is: ， in, Indicates the dynamic distance threshold. This indicates the preset rotational error. This indicates the preset translation error. Represent feature points; The reprojection distance from a point to a plane and the reprojection distance from a point to a line are compared with the dynamic distance threshold to achieve dynamic outlier detection. Based on the vectors in the local map and the aforementioned feature information, the preliminary feature information is evaluated for feature stability to obtain effective feature information, including: Based on the neighborhood vector distribution in the local map and the feature vectors in the feature information, the preliminary feature information is evaluated for planar feature stability and line feature stability, respectively. The evaluation function expression for the planar feature stability is as follows: ， in, and Both represent planar features in a local map. Related The normal vector of the nearest point; The expression for the evaluation function of the line feature stability is: ， in, and All represent line features in a local map. Related The direction vectors of the nearest points.

2. The LiDAR SLAM method for autonomous vehicles according to claim 1, characterized in that, Feature information is obtained by performing adaptive roughness evaluation and feature filtering on the raw point cloud data of the lidar, including: The raw point cloud data of the lidar is projected onto the ranging image; The roughness of each point in the ranging image corresponding to the original point cloud data of the lidar is estimated according to the adaptive roughness evaluation function, so as to obtain the roughness estimate value of each point in the ranging image. Sort each point in the ranging image according to the roughness estimate of each point; Feature filtering is performed on the set of adjacent points in the ranging image to obtain feature information.

3. The LiDAR SLAM method for autonomous vehicles according to claim 2, characterized in that, Roughness estimation is performed on each point in the ranging image corresponding to the original point cloud data of the lidar according to the adaptive roughness evaluation function, including: Scaling the geometric configuration between points in the ranging image to obtain point roughness; Based on a distance threshold, a neighbor search is performed in the ranging image to obtain consecutive neighbor points with a variable number of points in the same row; The neighboring points with point roughness in each row are classified according to a roughness threshold to obtain candidate planar points and / or candidate straight points.

4. The LiDAR SLAM method for autonomous vehicles according to claim 2, characterized in that, Feature filtering of the neighboring point set in the ranging image includes: The set of adjacent points in the ranging image is obtained based on the roughness estimate; Feature filtering is performed on the effectiveness of each candidate feature in the set of neighboring points based on the multiple rows of neighboring points to obtain feature points and feature vectors.

5. The lidar SLAM method for autonomous vehicles according to any one of claims 1 to 4, characterized in that, The effective feature information is used to estimate the pose based on the bimodal geometric primitive constraint mechanism to obtain a six-degree-of-freedom pose, including: Construct a geometric primitive matrix based on the features in the current frame and the corresponding feature information in the local map; The residuals from points to surfaces and from points to lines in the geometric primitive matrix are iterated to minimize the residuals and obtain the attitude estimate.

6. A LiDAR SLAM device for autonomous vehicles, used to implement the LiDAR SLAM method for autonomous vehicles according to any one of claims 1 to 5, characterized in that, include: The feature extraction module is used to acquire the raw point cloud data of the lidar, and to obtain feature information by performing adaptive roughness evaluation and feature filtering on the raw point cloud data of the lidar. The feature association module is used to perform dynamic outlier detection on the feature information to filter out outlier feature points and obtain effective feature information; The attitude estimation module is used 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 of the current frame and the corresponding feature information in the local map. The map building module is used to sequentially build local and global maps based on the six degrees of freedom poses to obtain the positioning information of the autonomous vehicle.

7. A lidar SLAM system for autonomous vehicles, characterized in that, include: The lidar device and the lidar SLAM device for autonomous vehicles as described in claim 6 are communicatively connected. The lidar device is used to collect raw lidar point cloud data, and the lidar SLAM device for autonomous vehicles is used to obtain the positioning information of autonomous vehicles by performing feature extraction, feature association, attitude estimation, and map construction based on the raw lidar point cloud data.

Images