A SLAM front-end registration method and device based on ball tree and DBSCAN clustering

Abstract

The present invention relates to the technical field of simultaneous positioning and map construction, and in particular to a SLAM front-end registration method and device based on ball tree and DBSCAN clustering, the method comprising: during the operation of a laser radar, extracting the current point cloud as a key frame at a certain distance, and splicing a certain number of key frames into a local point cloud; registering the local point cloud into a ball tree framework as a target point cloud in an NDT algorithm; using the current frame as a source point cloud in the NDT algorithm, taking the target point cloud as a reference, and using the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud; using a principal component analysis algorithm to determine the geometric information of the clustered point cloud, and weighting the geometric information; based on the weighted geometric information, performing NDT point cloud probability density score calculation on the current frame, and completing the calculation of the gradient and Hessian matrix. The present invention can improve the adaptability of the algorithm under different shape features, and can improve the accuracy of the positioning effect.

Description

Technical Field

[0001] The present invention relates to the technical field of simultaneous positioning and map construction, and in particular to a SLAM front-end registration method and device based on ball tree and DBSCAN clustering. Background Art

[0002] In recent years, with the continuous development of industries such as autonomous driving and mobile robotics, Simultaneous Localization and Mapping (SLAM) technology has become an indispensable technical means in these fields. Usually, the SLAM system includes front-end and back-end modules. The front-end mainly measures the movement of the robot by analyzing data from external sensors, while the back-end uses filtering or graphics optimization technology to refine the map and correct the position of the robot, reducing the cumulative error in the front-end operation. The scanning matching accuracy of the front-end largely affects the overall positioning accuracy of the SLAM technology.

[0003] Normal Distribution Transform (NDT), as a commonly used scan matching algorithm in SLAM front-end, has the advantages of low computational cost and little environmental impact, and is widely used in the field of autonomous driving. However, this method may lead to unreasonable division of point clouds in some areas, thus affecting the final accuracy and stability. Summary of the invention

[0004] In order to solve the technical problem that some areas of the prior art are unreasonable in dividing point clouds, resulting in poor accuracy and stability, the embodiment of the present invention provides a SLAM front-end registration method and device based on ball tree and DBSCAN clustering. The technical solution is as follows:

[0005] On the one hand, a SLAM front-end registration method based on ball tree and DBSCAN clustering is provided, the method is implemented by a SLAM front-end registration device based on ball tree and DBSCAN clustering, and the method includes:

[0006] S1. During the operation of the laser radar, the current point cloud is extracted as a key frame at a certain distance, and a certain number of key frames are combined into a local point cloud;

[0007] S2, registering the local point cloud into the ball tree framework as the target point cloud in the NDT algorithm;

[0008] S3, taking the current frame as the source point cloud in the NDT algorithm, taking the target point cloud as the benchmark, and using the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud;

[0009] S4. Determine the geometric information of the clustered point cloud using a principal component analysis algorithm, and perform weighted processing on the geometric information;

[0010] S5. Based on the weighted geometric information, the point cloud probability density score of the NDT is calculated for the current frame, and the calculation of the gradient and Hessian matrix is ​​completed.

[0011] On the other hand, a SLAM front-end registration device based on ball tree and DBSCAN clustering is provided, and the device is applied to a SLAM front-end registration method based on ball tree and DBSCAN clustering, and the device comprises:

[0012] An extraction unit is used to extract the current point cloud as a key frame at a certain distance during the operation of the laser radar, and to combine a certain number of key frames into a local point cloud;

[0013] A registration unit, used for registering the local point cloud into a ball tree framework as a target point cloud in an NDT algorithm;

[0014] The clustering unit is used to take the current frame as the source point cloud in the NDT algorithm, and take the target point cloud as the benchmark, and use the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud;

[0015] A processing unit, used to determine the geometric information of the clustered point cloud by using a principal component analysis algorithm, and perform weighted processing on the geometric information;

[0016] The calculation unit is used to calculate the point cloud probability density score of the NDT for the current frame according to the weighted geometric information, and complete the calculation of the gradient and Hessian matrix.

[0017] On the other hand, a SLAM front-end registration device based on ball tree and DBSCAN clustering is provided, and the SLAM front-end registration device based on ball tree and DBSCAN clustering comprises: a processor; a memory, on which computer-readable instructions are stored, and when the computer-readable instructions are executed by the processor, any one of the above-mentioned SLAM front-end registration methods based on ball tree and DBSCAN clustering is implemented.

[0018] On the other hand, a computer-readable storage medium is provided, wherein at least one instruction is stored in the storage medium, and the at least one instruction is loaded and executed by a processor to implement any one of the above-mentioned SLAM front-end registration methods based on ball tree and DBSCAN clustering.

[0019] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0020] In an embodiment of the present invention, during the operation of the laser radar, the current point cloud is extracted as a key frame at a certain distance, and a certain number of key frames are spliced ​​into a local point cloud; the local point cloud is registered in the ball tree framework as the target point cloud in the NDT algorithm; the current frame is used as the source point cloud in the NDT algorithm, and the target point cloud is used as a reference, and the DBSCAN clustering method is used to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud; the principal component analysis algorithm is used to determine the geometric information of the clustered point cloud, and the geometric information is weighted; according to the weighted geometric information, the point cloud probability density score of the NDT is calculated for the current frame, and the calculation of the gradient and Hessian matrix is ​​completed. The present invention is used to adapt the ball tree to the 3D point cloud so that it is more suitable for processing the point cloud environment. The DBSCAN clustering method is added on the basis of the ball tree to improve the speed of the point cloud clustering algorithm. The ball tree and DBSCAN clustering methods are used to improve the accuracy of NDT scan matching when calculating the probability density score locally. The introduction of weighted shapes and density weights allows for a more detailed classification of the surface shapes of local point clouds, improves the algorithm's adaptability to different shape features, and achieves a more accurate positioning effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0022] Figure 1 It is a flow chart of a SLAM front-end registration method based on ball tree and DBSCAN clustering provided by an embodiment of the present invention;

[0023] Figure 2 It is a flowchart of a SLAM front-end registration method based on ball tree and DBSCAN clustering provided by an embodiment of the present invention;

[0024] Figure 3 It is a schematic diagram of a target point cloud divided into two leaf nodes by a principal component analysis algorithm provided by an embodiment of the present invention;

[0025] Figure 4 A schematic diagram of DBSCAN cluster search in a ball tree framework provided by an embodiment of the present invention

[0026] Figure 5 It is a block diagram of a SLAM front-end registration device based on ball tree and DBSCAN clustering provided by an embodiment of the present invention;

[0027] Figure 6It is a structural schematic diagram of a SLAM front-end registration device based on ball tree and DBSCAN clustering provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0028] The technical solution of the present invention is described below in conjunction with the accompanying drawings.

[0029] In the embodiments of the present invention, words such as "exemplarily" and "for example" are used to indicate examples, illustrations or explanations. Any embodiment or design described as "example" in the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of the word "example" is intended to present the concept in a specific way. In addition, in the embodiments of the present invention, the meaning expressed by "and / or" can be both, or it can be either of the two.

[0030] In the embodiments of the present invention, "image" and "picture" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same. "of", "corresponding, relevant" and "corresponding" can sometimes be used interchangeably. It should be noted that when the difference between them is not emphasized, the meanings they intend to express are the same.

[0031] In the embodiments of the present invention, sometimes a subscript such as W1 may be written as a non-subscript such as W1. When the difference is not emphasized, the meanings to be expressed are the same.

[0032] In order to make the technical problems, technical solutions and advantages to be solved by the present invention more clear, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0033] The embodiment of the present invention provides a SLAM front-end registration method based on ball tree and DBSCAN clustering, which can be implemented by a SLAM front-end registration device based on ball tree and DBSCAN clustering, and the SLAM front-end registration device based on ball tree and DBSCAN clustering can be a terminal or a server. Figure 1 The flowchart of the SLAM front-end registration method based on ball tree and DBSCAN clustering is shown in Figure 2 The flowchart of the SLAM front-end registration method based on ball tree and DBSCAN clustering is shown. The processing flow of the method may include the following steps:

[0034] S1. During the operation of the laser radar, the current point cloud is extracted as a key frame at a certain distance, and a certain number of key frames are combined into a local point cloud.

[0035] Optionally, the specific operation of S1 may include the following steps S11-S12:

[0036] S11. Record the real-time point cloud obtained from the laser radar as a key frame at a preset distance, and stitch multiple groups of key frames into an initial local point cloud. The maximum number of stitched key frames is n. When the number is less than n, continue the process. When the number is more than n, remove the earliest key frame.

[0037] S12, subjecting the initial local point cloud to voxel grid filtering processing, and determining the processed initial local point cloud as the local point cloud.

[0038] In one possible implementation, voxel grid filtering is a method of processing data in three-dimensional space, usually used for three-dimensional images or voxel data sets. Voxel is short for volume element, which is a pixel point in three-dimensional space, similar to a pixel point in a two-dimensional image. In voxel grid filtering, data is divided into many small cubes or voxels, each of which contains a set of data points.

[0039] The main purposes of voxel grid filtering are:

[0040] Downsampling: Reduce the amount of data, increase processing speed, and retain important features.

[0041] Smoothing: Reduce noise and make data smoother.

[0042] Feature Extraction: Identifying and emphasizing specific features or patterns in data.

[0043] S2. Register the local point cloud into the ball tree framework as the target point cloud in the NDT algorithm.

[0044] In one possible implementation, the Octree Framework is a data structure for organizing and accessing three-dimensional spatial data. It is particularly suitable for processing and storing volume data, such as three-dimensional models in medical imaging, geographic information systems (GIS), and computer graphics. The basic idea of ​​the Octree Framework is to divide the three-dimensional space into multiple spheres, each of which can be further subdivided into smaller spheres to form a tree structure.

[0045] Optionally, the specific operation of S2 may include the following steps S21-S26:

[0046] S21. Set parameters, the level is represented by i, i=0, b=0, t=1, m=1, and take the local point cloud as the root node of the ball tree, denoted as the bth t Node point cloud.

[0047] S22, calculate the bth t The extreme value of the node point cloud in the 3D direction, the coordinate extreme value is determined as shown in the following formula (1):

[0048] (1).

[0049] According to the following formula (2) and the coordinate extreme value, determine the b t The geometric center of the node point cloud :

[0050] (2).

[0051] According to the following formula (3) and the marking value, determine the geometric center The distribution radius of the sphere center :

[0052] (3).

[0053] S23, according to the geometric center , distribution radius And the principal component analysis method, the b t The node point clouds are evenly divided into two groups along the direction of the first principal component, and t=t×2 is set. The two groups of point clouds are input into b t and b t+1 Among the two child nodes, determine the b t The coordinate extreme value of the b child node and determine the b t The geometric center of the child nodes and distribution radius r t .

[0054] In a feasible implementation manner, according to Figure 3 The first principal component direction shown divides the node point cloud into two groups evenly.

[0055] S24, t=t÷2, determine whether t is greater than or equal to m, if not, then t=t+1, jump to execute S22, if yes, then execute S25.

[0056] S25, t=t+1, determine the b t The distribution radius r of the child node point cloud t Is it less than the preset node radius threshold? If yes, execute S26, if no, i=i+1. m=m+2 i , jump to execute S22.

[0057] S26, stop segmentation, use the m nodes segmented as leaf nodes of the ball tree, complete the ball tree construction process, and determine the point cloud under the constructed ball tree framework as the target point cloud.

[0058] In a feasible implementation manner, the above steps S21-S26 are explained below in a more popular description:

[0059] S21', set parameter i = 0, which represents the number of binary tree layers, and use the local point cloud as the root node of the ball tree, denoted by b r (or b0) where r represents root, i.e. the root node of the ball tree, and b r It is basically divided into two parts, the left and right nodes are denoted as b iL and b iR For example, 1L This is the left subtree of the first layer. If we continue to split b 1L b 1R To the left and right nodes, the left and right nodes can be recorded as b 2LL 、b 2LR 、b 2RL and b 2RR ... and so on, this representation can be summarized as b iSi , S represents the symbol, and the tail i represents the number of symbols;

[0060] S22', calculate b iSi The extreme value of the node point cloud in the 3D direction, the coordinate extreme value is determined as shown in the following formula (1):

[0061] (1);

[0062] According to the following formula (2) and the coordinate extreme value, determine the b iSi The geometric center of the node point cloud :

[0063] (2);

[0064] According to the following formula (3) and the marking value, determine the geometric center The distribution radius of the sphere center :

[0065] (3);

[0066] S23', according to the geometric center , distribution radius And the principal component analysis method, the b iSi The node point clouds are evenly divided into two groups along the direction of the first principal component, and i=i+1 is set. The two groups of point clouds are input into b iSi (L) and b iSi(R) Determine the coordinate extremes of the two child nodes and the geometric center of the current child node. and distribution radius ;

[0067] S24', respectively determine the two child nodes in S24' Is it less than the preset node radius threshold? If yes, execute S25'; if no, let the two child nodes execute S22' respectively;

[0068] S25', , judge the b iSi The distribution radius of the child node point cloud Is it less than the preset node radius threshold? If yes, execute S26', if no, i=i+1. m=m+2 i , jump to execute S22';

[0069] S26', stop segmentation, use the m nodes segmented as leaf nodes of the ball tree, complete the ball tree construction process after all recursive processes are completed, and determine the point cloud under the constructed ball tree framework as the target point cloud.

[0070] S3. Use the current frame as the source point cloud in the NDT algorithm, take the target point cloud as the benchmark, and use the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud.

[0071] Optionally, the specific operation of S3 may include the following steps S31-S35:

[0072] S31, performing voxel grid filtering on the current frame to obtain a filtered source point cloud.

[0073] In a feasible implementation, voxel grid filtering is performed to reduce the amount of data processing.

[0074] S32, taking the target point cloud as a reference, determining any point in the source point cloud as the initial node, searching for the initial node The closest ball tree leaf node within the range.

[0075] S33, according to the DBSCAN clustering algorithm, with the ball tree leaf node as the center, Search within the range to find other related ball tree leaf nodes around, determine them as nodes to be searched, and integrate them into the cluster.

[0076] Among them, DBSCAN (Density-Based Spatial Clustering of Applications with Noise) is a density-based clustering algorithm that can divide areas with sufficiently high density into clusters and treat points that do not belong to any cluster as noise. DBSCAN is particularly suitable for discovering clusters of arbitrary shapes and has good robustness to noise points in the data. The main concepts of DBSCAN include core points, boundary points, and noise points:

[0077] Core Point: If a point has no less than MinPts (minimum number of points) of neighbor points within the ε (Epsilon) distance, then the point is considered a core point.

[0078] Border Point: If a point is not a core point, but it is within the ε distance of a core point, it is considered a border point.

[0079] Noise Point: If a point is neither a core point nor a boundary point, it is considered a noise point.

[0080] In a feasible implementation, Figure 4 Shown is a schematic diagram of DBSCAN searching clusters in a ball tree framework.

[0081] S34, determine whether the distance between the node to be searched and the initial node is greater than the search threshold, if yes, execute S35, if not, determine the node to be searched as a ball tree leaf node, and jump to execute S33.

[0082] S35. Stop searching, obtain a clustered point cloud based on each point of the source point cloud, record the number of original points in the clustered point cloud, and obtain the covariance matrix .

[0083] In one feasible implementation, the search threshold may be set to 2 times ϵ.

[0084] S4. Using the principal component analysis algorithm, determine the geometric information of the clustered point cloud and perform weighted processing on the geometric information.

[0085] Optionally, the specific operation of S4 may include the following steps S41-S44:

[0086] S41. For each point in the clustered point cloud, the coordinate extreme value of the point cloud is obtained through the information of the point stored in the point cloud, and the length, width and height of the clustered point cloud are determined according to the coordinate extreme value as follows:

[0087] (4) Among them, The filter grid parameters for voxel grid filtering.

[0088] According to the cluster formation principle, the volume V of the rectangle occupied by the cluster point cloud is obtained.

[0089] S42, by performing principal component analysis on the clustered point cloud, the characteristic values ​​of the point cloud in the three-dimensional direction are obtained , by comparing the eigenvalues, determine the shape of the cluster point cloud and perform weighted processing:

[0090] Given formula , , As a criterion, along the corresponding eigenvector The standard deviation of ,right Make a comparison.

[0091] when , When , the point cloud shows a linear trend, that is .when and When , the point cloud shows a planar trend, that is, .when When , the point cloud is three-dimensional, that is, .

[0092] Shape Weight As shown in formula (5):

[0093] (5) Among them is the dimension index of the grid, expressed as: .

[0094] S43, determining the density of the clustered point cloud by the volume V of the rectangular body occupied by the clustered point cloud and the number of original points of the clustered point cloud :

[0095] Average value of point density after filtering based on voxel grid and median Determining density weights :

[0096] if :

[0097] ;

[0098] if :

[0099] .

[0100] S44. For the shape and density of the clustered point cloud, a shape weight and a density weight are added to improve the relevant weight of the NDT registration score.

[0101] S5. Based on the weighted geometric information, the point cloud probability density score of the NDT is calculated for the current frame, and the calculation of the gradient and Hessian matrix is ​​completed.

[0102] Optionally, the specific operation of S5 may include the following steps:

[0103] The volume of the obtained clustered point cloud , shape weight and density weight , imported into the gradient , During the calculation of the matrix, the probability density score calculation process is updated. The specific formula is as follows:

[0104] in is the registration score, H ij for matrix, is the likelihood function to be optimized The weight parameter in is recorded as:

[0105] ;

[0106] ;

[0107] Ratio is a ratio value in the interval [0,1]. The Gaussian approximation of the likelihood function is used to obtain the pose optimization function .

[0108] is the pose optimization function The parameters are expressed as:

[0109] ;

[0110] ;

[0111] ;

[0112] In a feasible implementation, posture iteration is performed using Newton's method to obtain a final posture T.

[0113] In an embodiment of the present invention, during the operation of the laser radar, the current point cloud is extracted as a key frame at a certain distance, and a certain number of key frames are spliced ​​into a local point cloud; the local point cloud is registered in the ball tree framework as the target point cloud in the NDT algorithm; the current frame is used as the source point cloud in the NDT algorithm, and the target point cloud is used as a reference, and the DBSCAN clustering method is used to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud; the principal component analysis algorithm is used to determine the geometric information of the clustered point cloud, and the geometric information is weighted; according to the weighted geometric information, the point cloud probability density score of the NDT is calculated for the current frame, and the calculation of the gradient and Hessian matrix is ​​completed. The present invention is used to adapt the ball tree to the 3D point cloud so that it is more suitable for processing the point cloud environment. The DBSCAN clustering method is added on the basis of the ball tree to improve the speed of the point cloud clustering algorithm. The ball tree and DBSCAN clustering methods are used to improve the accuracy of NDT scan matching when calculating the probability density score locally. The introduction of weighted shapes and density weights allows for a more detailed classification of the surface shapes of local point clouds, improves the algorithm's adaptability to different shape features, and achieves a more accurate positioning effect.

[0114] Figure 5 The block diagram of a SLAM front-end registration device based on ball tree and DBSCAN clustering according to an exemplary embodiment is shown. The device is used in a SLAM front-end registration method based on ball tree and DBSCAN clustering. Figure 5 , the device comprises:

[0115] The extraction unit 510 is used to extract the current point cloud as a key frame at a certain distance during the operation of the laser radar, and to combine a certain number of key frames into a local point cloud;

[0116] A registration unit 520, used to register the local point cloud into a ball tree framework as a target point cloud in an NDT algorithm;

[0117] The clustering unit 530 is used to use the current frame as the source point cloud in the NDT algorithm, and the target point cloud as a reference, and use the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud;

[0118] A processing unit 540 is used to determine the geometric information of the clustered point cloud by using a principal component analysis algorithm, and perform weighted processing on the geometric information;

[0119] The calculation unit 550 is used to calculate the point cloud probability density score of the NDT for the current frame according to the weighted geometric information, and complete the calculation of the gradient and the Hessian matrix.

[0120] Optionally, the extraction unit 510 is used to:

[0121] S11, recording the real-time point cloud obtained by the laser radar once every distance m as a key frame, and stitching multiple groups of key frames into an initial local point cloud. The maximum number of stitched key frames is n. When the number is less than n, continue to process. When the number is more than n, remove the earliest key frame;

[0122] S12, subjecting the initial local point cloud to voxel grid filtering processing, and determining the processed initial local point cloud as the local point cloud.

[0123] Optionally, the registration unit 520 is used to:

[0124] S21, set parameters i=0, b=0, t=1, m=1, and take the local point cloud as the root node of the ball tree, denoted as the bth t Node point cloud;

[0125] S22, calculate the bth t The extreme value of the node point cloud in the 3D direction, the coordinate extreme value is determined as shown in the following formula (1):

[0126] (1);

[0127] According to the following formula (2) and the coordinate extreme value, determine the b t The geometric center of the node point cloud :

[0128] (2);

[0129] According to the following formula (3) and the marking value, determine the geometric center The distribution radius of the sphere center :

[0130] (3);

[0131] S23, according to the geometric center , distribution radius And the principal component analysis method, the b t The node point clouds are evenly divided into two groups along the direction of the first principal component, and t=t×2 is set. The two groups of point clouds are input into b t and b t+1 Among the two child nodes, determine the b t The coordinate extreme value of the b child node and determine the b t The geometric center of the child nodes and distribution radius r t ;

[0132] S24, t=t÷2, determine whether t is greater than or equal to m, if not, then t=t+1, jump to S22, if yes, then execute S25;

[0133] S25, t=t+1, determine the b t The distribution radius r of the child node point cloud t Is it less than the preset node radius threshold? If yes, execute S26, if no, i=i+1; m=m+2 i , jump to execute S22;

[0134] S26, stop segmentation, use the m nodes segmented as leaf nodes of the ball tree, complete the ball tree construction process, and determine the point cloud under the constructed ball tree framework as the target point cloud.

[0135] Optionally, the clustering unit 530 is used to:

[0136] S31, performing voxel grid filtering on the current frame to obtain a filtered source point cloud;

[0137] S32, taking the target point cloud as a reference, determining any point in the source point cloud as the initial node, searching for the initial node The closest ball tree leaf node within the range;

[0138] S33, according to the DBSCAN clustering algorithm, with the ball tree leaf node as the center, Search within the range to find other related ball tree leaf nodes around, determine them as nodes to be searched, and integrate them into the cluster;

[0139] S34, determine whether the distance between the node to be searched and the initial node is greater than the search threshold, if yes, execute S35, if not, determine the node to be searched as a ball tree leaf node, and jump to execute S33;

[0140] S35. Stop searching, obtain a clustered point cloud based on each point of the source point cloud, record the number of original points in the clustered point cloud, and obtain the covariance matrix .

[0141] Optionally, the processing unit 540 is configured to:

[0142] S41, for each point in the clustered point cloud, obtain the coordinate extreme value of the point cloud through the information of the point stored in the point cloud, and obtain the volume V of the rectangular body occupied by the clustered point cloud according to the clustering formation principle;

[0143] S42, by performing principal component analysis on the clustered point cloud, the characteristic values ​​of the point cloud in the three-dimensional direction are obtained , by comparing the eigenvalues, determine the shape of the cluster point cloud and perform weighted processing:

[0144] Given formula , , As a criterion, along the corresponding eigenvector The standard deviation of ,right Make a comparison.

[0145] when , When , the point cloud shows a linear trend, that is ;when and , the point cloud shows a planar trend, i.e. ;when When , the point cloud is three-dimensional, that is, ;

[0146] S43, determining the density of the clustered point cloud by the volume V of the rectangular body occupied by the clustered point cloud and the number of original points of the clustered point cloud ;

[0147] Average value of point density after filtering based on voxel grid and median Determining density weights

[0148] S44. For the shape and density of the clustered point cloud, a shape weight and a density weight are added to improve the relevant weight of the NDT registration score.

[0149] In an embodiment of the present invention, during the operation of the laser radar, the current point cloud is extracted as a key frame at a certain distance, and a certain number of key frames are spliced ​​into a local point cloud; the local point cloud is registered in the ball tree framework as the target point cloud in the NDT algorithm; the current frame is used as the source point cloud in the NDT algorithm, and the target point cloud is used as a reference, and the DBSCAN clustering method is used to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud; the principal component analysis algorithm is used to determine the geometric information of the clustered point cloud, and the geometric information is weighted; according to the weighted geometric information, the point cloud probability density score of the NDT is calculated for the current frame, and the calculation of the gradient and Hessian matrix is ​​completed. The present invention is used to adapt the ball tree to the 3D point cloud so that it is more suitable for processing the point cloud environment. The DBSCAN clustering method is added on the basis of the ball tree to improve the speed of the point cloud clustering algorithm. The ball tree and DBSCAN clustering methods are used to improve the accuracy of NDT scan matching when calculating the probability density score locally. The introduction of weighted shapes and density weights allows for a more detailed classification of the surface shapes of local point clouds, improves the algorithm's adaptability to different shape features, and achieves a more accurate positioning effect.

[0150] Figure 6 is a structural diagram of a SLAM front-end registration device based on ball tree and DBSCAN clustering provided by an embodiment of the present invention, such as Figure 6 As shown, the SLAM front-end registration device based on ball tree and DBSCAN clustering may include the above Figure 5 The SLAM front-end registration device based on ball tree and DBSCAN clustering is shown. Optionally, the SLAM front-end registration device based on ball tree and DBSCAN clustering 610 may include a first processor 2001 .

[0151] Optionally, the SLAM front-end registration device 610 based on ball tree and DBSCAN clustering may further include a memory 2002 and a transceiver 2003 .

[0152] The first processor 2001, the memory 2002 and the transceiver 2003 may be connected via a communication bus.

[0153] Combine the following Figure 6 The components of the SLAM front-end registration device 610 based on ball tree and DBSCAN clustering are specifically introduced:

[0154] The first processor 2001 is the control center of the SLAM front-end registration device 610 based on ball tree and DBSCAN clustering, which can be a processor or a general term for multiple processing elements. For example, the first processor 2001 is one or more central processing units (CPUs), or an application specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of the present invention, such as one or more microprocessors (digital signal processors, DSPs), or one or more field programmable gate arrays (field programmable gate arrays, FPGAs).

[0155] Optionally, the first processor 2001 can perform various functions of the SLAM front-end registration device 610 based on ball tree and DBSCAN clustering by running or executing a software program stored in the memory 2002 and calling data stored in the memory 2002.

[0156] In a specific implementation, as an embodiment, the first processor 2001 may include one or more CPUs, such as Figure 6 CPU0 and CPU1 are shown in FIG.

[0157] In a specific implementation, as an embodiment, the SLAM front-end registration device 610 based on ball tree and DBSCAN clustering may also include multiple processors, such as Figure 6 The first processor 2001 and the second processor 2004 are shown in FIG. Each of these processors may be a single-core processor (single-CPU) or a multi-core processor (multi-CPU). The processor here may refer to one or more devices, circuits, and / or processing cores for processing data (e.g., computer program instructions).

[0158] The memory 2002 is used to store the software program for executing the solution of the present invention, and is controlled to be executed by the first processor 2001. The specific implementation method can refer to the above method embodiment, which will not be repeated here.

[0159] Optionally, the memory 2002 may be a read-only memory (ROM) or other types of static storage devices that can store static information and instructions, a random access memory (RAM) or other types of dynamic storage devices that can store information and instructions, or an electrically erasable programmable read-only memory (EEPROM), a compact disc read-only memory (CD-ROM) or other optical disc storage, optical disc storage (including compressed optical disc, laser disc, optical disc, digital versatile disc, Blu-ray disc, etc.), a magnetic disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store the desired program code in the form of instructions or data structures and can be accessed by a computer, but is not limited thereto. The memory 2002 may be integrated with the first processor 2001, or may exist independently, and may be connected to the computer through the interface circuit ( Figure 6 (not shown) is coupled to the first processor 2001, which is not specifically limited in this embodiment of the present invention.

[0160] The transceiver 2003 is used to communicate with a network device or a terminal device.

[0161] Optionally, the transceiver 2003 may include a receiver and a transmitter ( Figure 6 The receiver is used to implement a receiving function, and the transmitter is used to implement a sending function.

[0162] Optionally, the transceiver 2003 may be integrated with the first processor 2001, or may exist independently, and may be connected to the first processor 2001 through the interface circuit ( Figure 6 (not shown) is coupled to the first processor 2001, which is not specifically limited in this embodiment of the present invention.

[0163] It should be noted that Figure 6 The structure of the SLAM front-end registration device 610 based on ball tree and DBSCAN clustering shown in the figure does not constitute a limitation on the router. The actual knowledge structure recognition device may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently.

[0164] In addition, the technical effects of the SLAM front-end registration device 610 based on ball tree and DBSCAN clustering can refer to the technical effects of the SLAM front-end registration method based on ball tree and DBSCAN clustering described in the above method embodiment, which will not be repeated here.

[0165] It should be understood that the first processor 2001 in the embodiment of the present invention may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), field programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0166] It should also be understood that the memory in the embodiments of the present invention may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory may be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of random access memory (RAM) are available, such as static RAM (SRAM), dynamic random access memory (DRAM), synchronous DRAM (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link DRAM (SLDRAM), and direct rambus RAM (DR RAM).

[0167] The above embodiments can be implemented in whole or in part by software, hardware (such as circuits), firmware or any other combination. When implemented by software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, the process or function described in the embodiment of the present invention is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from one website, computer, server or data center to another website, computer, server or data center by wired (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that contains one or more available media sets. The available medium can be a magnetic medium (for example, a floppy disk, a hard disk, a tape), an optical medium (for example, a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state hard disk.

[0168] It should be understood that the term "and / or" in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. A and B can be singular or plural. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship, but it may also indicate an "and / or" relationship. Please refer to the context for specific understanding.

[0169] In the present invention, "at least one" means one or more, and "plurality" means two or more. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single items or plural items. For example, at least one of a, b, or c can mean: a, b, c, ab, ac, bc, or abc, where a, b, and c can be single or multiple.

[0170] It should be understood that in various embodiments of the present invention, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0171] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0172] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described equipment, devices and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0173] In the several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another device, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0174] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0175] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0176] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk, and other media that can store program codes.

[0177] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A SLAM front-end registration method based on ball tree and DBSCAN clustering, characterized in that: The method comprises: S1. During the operation of the laser radar, the current point cloud is extracted as a key frame at a certain distance, and a certain number of key frames are combined into a local point cloud; S2, registering the local point cloud into the ball tree framework as the target point cloud in the NDT algorithm; S3, taking the current frame as the source point cloud in the NDT algorithm, taking the target point cloud as the benchmark, and using the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud; S4. Determine the geometric information of the clustered point cloud using a principal component analysis algorithm, and perform weighted processing on the geometric information; S5. Calculate the probability density score of the point cloud of the NDT for the current frame according to the weighted geometric information, and complete the calculation of the gradient and Hessian matrix; The S2 registers the local point cloud into the ball tree framework as the target point cloud in the NDT algorithm, including: S21. Set parameters, use i to represent the level, i=0, b=0, t=1, m=1, and take the local point cloud as the root node of the ball tree, denoted as the bth t Node point cloud; S22, calculate the bth t The extreme value of the node point cloud in the 3D direction is determined by the following formula (1): ((x min ,y min ,z min ),(x max ,y max ,z max ))(1); According to the following formula (2) and the coordinate extreme value, determine the b t The geometric center c of the node point cloud t : According to the following formula (3) and the marking value, determine the geometric center c t The distribution radius r is the center of the sphere t : S23, according to the geometric center c t , distribution radius r t And the principal component analysis method, the b t The node point clouds are evenly divided into two groups along the direction of the first principal component, and t = t × 2 is set. The two groups of point clouds are input into b t and b t+1 Among the two child nodes, determine the b t The coordinate extreme value of the b child node and determine the b t The geometric center c of the child nodes t and distribution radius r t ; S24, t=t÷2, determine whether t is greater than or equal to m, if not, then t=t+1, jump to S22, if yes, then execute S25; S25, t=t+1, determine the bth t The distribution radius r of the child node point cloud t Is it less than the preset node radius threshold r? max If yes, execute S26, if no, i=i+1; m=m+2 i , jump to execute S22; S26, stop segmentation, use the segmented m nodes as leaf nodes of the ball tree, complete the ball tree construction process, and determine the point cloud under the constructed ball tree framework as the target point cloud; The step S4 uses a principal component analysis algorithm to determine geometric information of the clustered point cloud and performs weighted processing on the geometric information, including: S41, for each point in the clustered point cloud, obtain the coordinate extreme value of the point cloud through the information of the point stored in the point cloud, determine the length, width and height of the clustered point cloud according to the coordinate extreme value, and obtain the volume V of the rectangular body occupied by the clustered point cloud according to the cluster formation principle; S42, by performing principal component analysis on the clustered point cloud, the eigenvalues ​​λ1, λ2, and λ3 of the point cloud in three dimensions are obtained, and by comparing the eigenvalues, the shape of the clustered point cloud is determined and weighted processing is performed: Given formula As a criterion, along the corresponding eigenvector The standard deviation of to a 1D 、a 2D 、a 3D make comparisons; When σ1>>σ2, a 1D >>a 2D , a 1D >>a 3D When , the point cloud shows a linear trend, that is, d * =a 1D ; When σ1, and a 2D >>a 1D , a 3D , the point cloud shows a planar trend, that is, d * =a 2D ;when When the point cloud is three-dimensional, that is, d * =a 3D ; S43, determining the density λ of the clustered point cloud by the volume V of the rectangular body occupied by the clustered point cloud and the number of original points of the clustered point cloud p ; The average value of the point density after filtering according to the voxel grid λ mean and the median λ median Determine the density weight W d ; S44. For the shape and density of the clustered point cloud, a shape weight and a density weight are added to improve the relevant weight of the NDT registration score.

2. The SLAM front-end registration method based on ball tree and DBSCAN clustering according to claim 1, characterized in that, During the operation of the laser radar, S1 extracts the current point cloud as a key frame at a certain distance, and combines a certain number of key frames into a local point cloud, including: S11, recording the real-time point cloud obtained by the laser radar once at a preset distance as a key frame, and stitching multiple groups of key frames into an initial local point cloud, the maximum number of stitched key frames is n, and when the number is less than n, continue to process, and when the number is more than n, remove the earliest key frame; S12, subjecting the initial local point cloud to voxel grid filtering processing, and determining the processed initial local point cloud as the local point cloud.

3. The SLAM front-end registration method based on ball tree and DBSCAN clustering according to claim 2, characterized in that, The S3 uses the current frame as the source point cloud in the NDT algorithm, takes the target point cloud as the benchmark, and uses the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud, including: S31, performing voxel grid filtering on the current frame to obtain a filtered source point cloud; S32, taking the target point cloud as a reference, determining any point in the source point cloud as the initial node, and searching for the nearest ball tree leaf node within a range of ∈ for the initial node; S33, according to the DBSCAN clustering algorithm, with the ball tree leaf node as the center, search within the range of ∈, find other related ball tree leaf nodes around, determine them as nodes to be searched, and integrate them into the cluster; S34, determine whether the distance between the node to be searched and the initial node is greater than the search threshold, if yes, execute S35, if not, determine the node to be searched as a ball tree leaf node, and jump to execute S33; S35, stop searching, obtain a clustered point cloud based on each point of the source point cloud, record the number of original points of the clustered point cloud, and obtain the covariance matrix Σ.

4. A SLAM front-end registration device based on ball tree and DBSCAN clustering, wherein the SLAM front-end registration device based on ball tree and DBSCAN clustering is used to implement the SLAM front-end registration method based on ball tree and DBSCAN clustering as claimed in any one of claims 1 to 3, characterized in that: The device comprises: An extraction unit is used to extract the current point cloud as a key frame at a certain distance during the operation of the laser radar, and to combine a certain number of key frames into a local point cloud; A registration unit, used for registering the local point cloud into a ball tree framework as a target point cloud in an NDT algorithm; The clustering unit is used to take the current frame as the source point cloud in the NDT algorithm, and take the target point cloud as the benchmark, and use the DBSCAN clustering method to aggregate the point cloud information around each point in the source point cloud to form a clustered point cloud; A processing unit, used to determine the geometric information of the clustered point cloud by using a principal component analysis algorithm, and perform weighted processing on the geometric information; The calculation unit is used to calculate the point cloud probability density score of the NDT for the current frame according to the weighted geometric information, and complete the calculation of the gradient and Hessian matrix.

5. The SLAM front-end registration device based on ball tree and DBSCAN clustering according to claim 4, characterized in that: The extraction unit is used for: S11, recording the real-time point cloud obtained by the laser radar once every distance m as a key frame, and stitching multiple groups of key frames into an initial local point cloud. The maximum number of stitched key frames is n. When the number is less than n, continue to process. When the number is more than n, remove the earliest key frame; S12, subjecting the initial local point cloud to voxel grid filtering processing, and determining the processed initial local point cloud as the local point cloud.

6. A SLAM front-end registration device based on ball tree and DBSCAN clustering, characterized in that: The SLAM front-end registration device based on ball tree and DBSCAN clustering includes: processor; A memory having computer-readable instructions stored thereon, wherein when the computer-readable instructions are executed by the processor, the method according to any one of claims 1 to 3 is implemented.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores program codes, which can be called by a processor to execute the method according to any one of claims 1 to 3.

Images