Laser radar odometer method based on step-by-step feature extraction

Abstract

The invention provides a laser radar odometer method based on step-by-step feature extraction, and relates to the technical field of computer vision. The method comprises the following steps: constructing a concentric clustering model based on combination of fan-shaped voxels, and processing an original three-dimensional point cloud acquired from a laser radar to obtain a laser radar point cloud composed of stable large objects; step-by-step feature extraction is carried out on the laser radar point cloud composed of the stable large objects; constructing feature pairs of the current frame and the local feature map based on the feature point clouds extracted step by step, and estimating pose transformation through minimizing a feature pair cost function by using the distance between the feature pairs and a progressive non-convex truncated least square method; and based on the feature stability, updating the local feature map through the feature point cloud extracted from the current frame and the current local feature map. According to the invention, the precision and robustness of the pose estimation task based on the laser radar can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of computer vision technology, and in particular to a laser radar odometer method based on step-by-step feature extraction. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is one of the most core tasks in the field of intelligent LiDAR sensors, and a large number of various methods have emerged. According to the type of sensor, the existing SLAM methods can be divided into vision-based SLAM methods, LiDAR-based SLAM methods, and multi-sensor fusion-based SLAM methods. For vision-based SLAM methods, binocular camera-based SLAM methods and depth camera-based SLAM methods are generally more robust and accurate than monocular camera-based methods, but their performance is still affected by external factors such as lighting conditions. The positioning accuracy of LiDAR-based SLAM methods is generally higher than that of vision-based SLAM methods, and they are not easily affected by lighting conditions. They have been widely used in fields such as LiDAR sensor environmental detection.

[0003] LiDAR-based SLAM methods can be divided into traditional methods and deep learning methods. With the successful application of deep learning technology in the field of images in recent years, researchers have proposed a LiDAR SLAM method based on deep learning. LO-Net first completed the LiDAR odometer task based on deep learning. LO-Net is an end-to-end method without geometric constraints. Its input is two sets of LiDAR point clouds corresponding to adjacent frames, and its output is the relative pose transformation between the two sets of point clouds. LiDAR SLAM based on deep learning is prone to overfitting, lacks stability, and has a high deployment cost, which greatly limits the application scenarios of LiDAR SLAM methods based on deep learning. Therefore, in comparison, traditional LiDAR SLAM methods are more widely used in practice.

[0004] Traditional LiDAR SLAM methods can be divided into direct registration methods and feature matching-based methods. Direct registration methods mainly include methods based on Normal Distribution Transformation (NDT) and methods based on Iterative Closet Point (ICP). The NDT-based SLAM method converts the reference point cloud into a normal distribution of multidimensional variables, and calculates the optimal pose transformation by maximizing the possibility that the currently scanned point is located on the reference point cloud surface. The ICP-based SLAM method obtains the optimal pose transformation between point clouds by minimizing the distance between the corresponding point cloud pairs between two point clouds. This type of method also includes ICP-based variants such as GICP and VGICP. Since direct registration methods often directly input the original point cloud, their computational cost is high. In contrast, feature matching-based LiDAR SLAM methods may perform better. Feature matching-based SLAM methods first extract point cloud features, that is, detect geometric feature points such as edge points and plane points from the LiDAR point cloud; then, by minimizing the distance between feature point cloud pairs corresponding to adjacent frames, the pose transformation of the LiDAR sensor between adjacent frames is obtained. Among the LiDAR SLAM methods based on feature matching, the LOAM method is one of the most representative methods. Many researchers have made improvements based on the LOAM method and proposed many variant methods. The LOAM framework consists of three parallel links, namely feature extraction, pose estimation and mapping. This framework structure is computationally efficient and is also the basic structure followed by most feature matching-based methods. The LOAM method first extracts edge and plane features from continuous LiDAR point cloud frames, and then estimates the pose transformation by minimizing the distances from edge points to edges and plane points to planes corresponding to adjacent frames. The feature extraction module of the LOAM method effectively improves the efficiency and accuracy of the method, but its shortcomings are also obvious. The LOAM method realizes feature extraction by calculating the local smoothness of the point cloud on the same scan line. This method is prone to extracting erroneous feature information, thereby reducing the accuracy of the odometer. The LOAM method obtains a low-precision odometer by matching adjacent frames at high frequency (10Hz), and effectively improves the accuracy by optimizing the pose through low-frequency (1Hz) mapping at the back end. However, the maintenance cost of the back-end mapping optimization is high, and real-time performance is difficult to guarantee. Summary of the invention

[0005] In order to solve the technical problem of low accuracy of odometers in the prior art, an embodiment of the present invention provides a laser radar odometer method based on step-by-step feature extraction. The technical solution is as follows:

[0006] On the one hand, a laser radar odometer method based on step-by-step feature extraction is provided, the method is implemented by a laser radar odometer device, and the method includes:

[0007] A concentric clustering model based on fan-shaped voxels is constructed to process the original three-dimensional point cloud obtained from the lidar to obtain a lidar point cloud composed of stable large objects;

[0008] Perform step-by-step feature extraction on LiDAR point clouds consisting of stable large objects;

[0009] Based on the feature point cloud extracted step by step, the feature pair of the current frame and the local feature map is constructed. The distance between the feature pairs and the truncated least squares method with progressive non-convexity are used to estimate the pose transformation by minimizing the feature pair cost function.

[0010] Based on feature stability, the local feature map is updated through the feature point cloud extracted from the current frame and the current local feature map.

[0011] On the other hand, a laser radar odometer device is provided, which includes: a processor; a memory, wherein the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, any one of the above-mentioned laser radar odometer methods based on step-by-step feature extraction is implemented.

[0012] 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 laser radar odometer methods based on step-by-step feature extraction.

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

[0014] The embodiment of the present invention constructs a concentric clustering model based on a combination of fan-shaped voxels to process the original three-dimensional point cloud obtained from the laser radar, so as to obtain a laser radar point cloud composed of stable large objects; performs step-by-step feature extraction on the laser radar point cloud composed of stable large objects; constructs feature pairs of the current frame and the local feature map based on the step-by-step extracted feature point cloud, and estimates the pose transformation by minimizing the feature pair cost function using the truncated least squares method with respect to the distance and asymptotic non-convexity between the feature pairs; based on feature stability, updates the local feature map through the feature point cloud extracted from the current frame and the current local feature map; in this way, the accuracy and robustness of the pose estimation task based on the laser radar can be effectively improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] 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.

[0016] Figure 1 is a flow chart of a laser radar odometer method based on step-by-step feature extraction provided by an embodiment of the present invention;

[0017] Figure 2 A schematic diagram of the overall framework of a laser radar odometer method based on step-by-step feature extraction provided by an embodiment of the present invention;

[0018] FIG3( a) is a schematic front view of a concentric clustering model provided in an embodiment of the present invention;

[0019] FIG3( b ) is a schematic cross-sectional view of a concentric clustering model provided by an embodiment of the present invention;

[0020] FIG4(a) is a schematic top view of three radially adjacent sector-shaped voxels provided by an embodiment of the present invention;

[0021] FIG4( b ) is a schematic side view of three radially adjacent sector-shaped voxels provided by an embodiment of the present invention;

[0022] FIG5( a ) is a schematic diagram of an original point cloud before inputting a concentric clustering model provided by an embodiment of the present invention;

[0023] FIG5( b ) is a schematic diagram of a point cloud processed by a concentric clustering model provided in an embodiment of the present invention;

[0024] FIG6( a ) is a schematic diagram of points p and q being continuous on the same scene surface provided by an embodiment of the present invention;

[0025] FIG6( b ) is a schematic diagram showing that a point p and a point q do not satisfy the continuity condition on the same scene surface provided by an embodiment of the present invention;

[0026] FIG7(a) is a schematic diagram of edge points p1 and p2 observing adjacent point sets on one side provided by an embodiment of the present invention, where p1 observes a backward adjacent point set and p2 observes a forward adjacent point set;

[0027] FIG7( b ) is a schematic diagram of a set of adjacent points on both sides observed by an edge point p3 provided by an embodiment of the present invention;

[0028] Figure 8 A schematic diagram of an edge feature point cloud image extracted by an edge feature point detection method based on the continuity of adjacent points on the same scene surface;

[0029] Fig. 9 A schematic diagram of a plane feature point cloud image extracted from a non-edge feature point cloud based on local smoothness;

[0030] Fig.10 A schematic diagram comparing the trajectory estimated by the laser radar odometer method based on step-by-step feature extraction and the actual trajectory provided by an embodiment of the present invention;

[0031] Fig.11 It is a structural schematic diagram of a laser radar odometer device provided in an embodiment of the present invention. DETAILED DESCRIPTION

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

[0033] 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.

[0034] 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.

[0035] 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.

[0036] 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.

[0037] The embodiment of the present invention provides a laser radar odometer method based on step-by-step feature extraction, which can be implemented by a laser radar odometer device, which can be a terminal or a server. Figure 1 and Figure 2 As shown, the processing flow of the method may include the following steps:

[0038] S1, constructs a concentric clustering model based on fan-shaped voxels, processes the original three-dimensional point cloud obtained from the lidar, and obtains a lidar point cloud composed of stable large objects;

[0039] In this embodiment, the fan-shaped voxel is constructed based on the distribution characteristics of the laser radar point cloud. Therefore, the constructed concentric clustering model meets the distribution characteristics of the laser radar point cloud. The front view and cross-sectional view of the concentric clustering model are shown in Figure 3 (a) and Figure 3 (b) respectively. The top view and side view of three radially adjacent fan-shaped voxels are shown in Figure 4 (a) and Figure 4 (b) respectively. As a spatial primitive, the fan-shaped voxel can efficiently divide the point cloud space. The size of the fan-shaped voxel gradually increases with the increase of the distance from the laser sensor, which can meet the distribution characteristics of the laser radar point cloud, and the boundary of the fan-shaped voxel is determined by three parameters: the length ρ of the voxel, the height h of the voxel, and the polar angle θ.

[0040] In this embodiment, in the concentric clustering model, all fan-shaped voxels have the same polar angle and voxel height, and different lengths. As the distance from the fan-shaped voxel to the laser radar increases, the length and arc length of the fan-shaped voxel gradually increase.

[0041] In this embodiment, the construction of a concentric clustering model based on fan-shaped voxels processes the original three-dimensional point cloud obtained from the laser radar to obtain a laser radar point cloud composed of stable large objects, which may specifically include the following steps:

[0042] Traverse the original 3D point cloud obtained from the LiDAR, and find the sector voxel to which the traversed point belongs through coordinate mapping. The sector voxel is denoted as V i，j，k ; Where i, j, and k represent the indices of the voxel in radial, polar, and directions, respectively;

[0043] In voxel V i，j，k The voxel V in the 3*3 neighborhood centered i′，j′，k′ Perform labeling; among them, voxel V i′，j′，k′ The coordinate index (i′, j′, k′) is expressed as:

[0044] {(i′,j′,k′)|i′∈{i-1,i,i+1},j′∈{j-1,j,j+1},k′∈{k-1,k,k+1}}

[0045] Let the neighborhood voxel be the voxel set S i，j，k ; Among them, if S i，j，k When there are fan-shaped voxels that have been marked as a certain class, S i，j，k The voxels in the i，j，k The earliest created class; if S i，j，k If there is no fan-shaped voxel marked as a certain class, a new class is created and S i，j，k The voxels in are labeled as this new class;

[0046] After traversing the original 3D point cloud obtained from the LiDAR, all non-empty voxels in the concentric clustering model are labeled as a certain class;

[0047] Set a quantity threshold θ N , the number of points is less than θ N The point cloud with a number greater than θ is regarded as an unstable, small-category point cloud and is removed from the original 3D point cloud, leaving the point cloud with a number greater than θ as a stable, large-category point cloud. N The point cloud of the concentric clustering model is used to obtain the lidar point cloud composed of stable large objects.

[0048] In this embodiment, the laser radar point cloud is passed through a concentric clustering model to remove unstable, small-class point clouds and retain stable, large-class point clouds. Figure 5(a) is a schematic diagram of the original point cloud before input into the concentric clustering model, and Figure 5(b) is a schematic diagram of the point cloud processed by the concentric clustering model. The boxes in Figures 5(a) and 5(b) indicate the same position. The upper boxes are some small objects that are unstable to observe, and it is not easy to find corresponding points in subsequent point cloud frames. The upper boxes in Figure 5(b) indicate that these point clouds are filtered out by the concentric clustering model. Similarly, the lower box indicates a bus, which should be a large object left in the point cloud map. Figures 5(a) and 5(b) confirm that the concentric clustering model only filters out a small part of the points and will not affect the large objects that occupy most of the point cloud. At the same time, those small objects that are difficult to have a positive effect on the subsequent odometer estimation will be filtered out.

[0049] S2, performing step-by-step feature extraction on the LiDAR point cloud composed of stable large objects; specifically, the following steps may be included:

[0050] The adjacent point continuity judgment method based on isosceles triangle constraint is used to judge whether two adjacent points are continuous on the same scene surface;

[0051] Based on the edge feature point detection method of adjacent points on the same scene surface continuity (CSSS), the edge feature point cloud (abbreviated as edge point cloud) is extracted from the output point cloud of the concentric clustering model, and the remaining point cloud that does not belong to the edge feature point cloud is divided into non-edge feature point cloud (abbreviated as non-edge point cloud);

[0052] Plane feature point clouds (abbreviated as: plane point clouds) are detected in non-edge feature point clouds based on local smoothness.

[0053] In this embodiment, the method for determining the continuity of adjacent points based on isosceles triangle constraints to determine whether two adjacent points are continuous on the same scene surface includes:

[0054] Define the origin of the laser radar as point o. For any point p and its adjacent point q on the same laser beam, as shown in Figure 6(a), if point p and point q are continuous on the same scene surface, then the distance d from point p to point o is po Approximately equal to the distance d from point q to point o qo , point o, point p and point q form an approximate isosceles triangle as vertices, and the base b of the triangle is pq It is expressed as:

[0055] b pq =2(1-cosα)[min(d po , d qo )] 2

[0056] Among them, α is the angle formed by the two line segments from point o to point p and from point o to point q, d po and d qo are the distances from point p to point o and from point q to point o; the length of the base of the triangle is b pq Approximately the distance d from point p to point q pq ,Right now Among them, d pq The size of is the Euclidean distance between point p and point q;

[0057] Define the function g(p,q) to determine whether point p and point q are continuous on the same scene surface. g(p,q) is expressed as:

[0058]

[0059] Where η is a constant, η>1; as shown in Figure 6(b), if point p and point q are not continuous on the same scene surface, then d pq >>b pq , g(p,q)=0; if point p and point q are continuous on the same scene surface, then g(p,q)=1.

[0060] In this embodiment, the edge feature point detection method based on the continuity of adjacent points on the same scene surface, extracting the edge feature point cloud from the output point cloud of the concentric clustering model includes:

[0061] Based on the defined function g(p, q), a Boolean function G(S) is defined to determine whether point p is continuous with the points in the adjacent point set S on one side on the same scene surface. G(S) is expressed as:

[0062]

[0063] When G(S) = 1, it means that point p is continuous with all points in S on the same scene surface;

[0064] Definition Sf and S b are the forward neighboring point set and the backward neighboring point set of point p, respectively. When point p is similar to point p1 and point p2 in Figure 7(a), that is, point p is continuous with only one side of the neighboring point set on the same scene surface, G(S f ) and G(S b )satisfy:

[0065]

[0066] Point p is considered as an edge feature point, where Represents the exclusive OR operation;

[0067] When point p is similar to point p3 in Figure 7(b), it is continuous with the adjacent points on both sides on the two scene surfaces, that is, G(S f ) and G(S b )satisfy:

[0068] G(S f )=G(S b )=1

[0069] Defining functions Calculate S f and S b The main direction vector and The angle size and set an angle threshold θ fb ,when When , point p is considered as an edge feature point; It is expressed as:

[0070]

[0071] Among them, ||·||2 represents the Euclidean norm of the vector, the main direction vector and By S f With point S b Their respective covariance matrices and Perform feature decomposition to obtain .

[0072] like Figure 8 As shown, it is a schematic diagram of an edge feature point cloud image extracted by an edge feature point detection method based on the continuity of adjacent points on the same scene surface.

[0073] In this embodiment, detecting a plane feature point cloud in a non-edge feature point cloud based on local smoothness includes:

[0074] Set point is any point of the non-edge feature point cloud, For point A collection of adjacent points on the same laser beam, The points in are evenly distributed on the On both sides of The local smoothness It is expressed as:

[0075]

[0076] in, is the size of the set of adjacent points, Indicate point The distance to the lidar sensor origin, Indicate point To point The distance Is a collection Points in the C ,like point is considered as a plane feature point. Fig. 9 , which is a schematic diagram of a plane feature point cloud image extracted from a non-edge feature point cloud based on local smoothness.

[0077] S3, based on the feature point cloud extracted step by step, construct feature pairs (i.e., data association) between the current frame and the local feature map, and estimate the pose transformation by minimizing the feature pair cost function using the truncated least squares method of the distance and progressive non-convexity between the feature pairs; specifically, the following steps may be included:

[0078] For any edge feature point in the edge feature point cloud of the current frame Find the corresponding adjacent point set S on the edge feature point cloud in the current local feature map ε , calculate S ε The geometric center point And the covariance matrix Covariance matrix Perform eigenvalue decomposition to obtain the main direction vector Along the main direction vector and through the geometric center Select the line segment at the geometric center Two points on both sides and point The distance between the feature pair formed by the edge feature point cloud in the current local feature map is expressed as:

[0079]

[0080] in, is the edge feature distance, is the world coordinate of the lidar sensor at the kth frame, yes The coordinates in the world coordinate system are is the expected pose transformation;

[0081] For any plane feature point in the plane feature point cloud of the current frame Find the corresponding set of adjacent points on the plane feature point cloud in the current local feature map calculate The geometric center point and the covariance matrix, Covariance matrix Perform eigenvalue decomposition to obtain the normal vector point The distance between the feature pair formed by the plane feature point cloud in the current local feature map is expressed as:

[0082]

[0083] in, is the plane feature distance, yes The coordinates in the world coordinate system are

[0084] The edge feature distance Distance from plane feature As the residual in the optimization process, the truncated least squares method based on asymptotic non-convexity minimizes the feature pair cost function through cross optimization To estimate robust and accurate pose transformations in, It is expressed as:

[0085]

[0086] in, Lie algebras representing special Euclidean groups; To transform the posture into The distance between feature pairs in the case of p i is any feature point; w i ∈[0, 1] represents the residual of each feature (i.e., the edge feature distance d ε Or the plane feature distance d S ) weight; i = 1, 2, 3, ..., N, N is the total number of feature points, Φ ρ (w i ) is a penalty term with weights given by asymptotic non-convexity.

[0087] In this embodiment, Φ ρ (w i ) is expressed as:

[0088]

[0089] Among them, μ is the control parameter that changes continuously during the iterative optimization process, is a fixed cutoff threshold; at the first iteration, the initial value μ0 of the control parameter μ is:

[0090]

[0091] in, It indicates the maximum value of all feature distance pairs after calculating all feature pair distances for the first time; It is expressed as:

[0092]

[0093] in, is the set of edge feature points in the current frame, is the set of plane feature points in the current frame.

[0094] In this embodiment, cross optimization is to and weights Cross optimization, where the optimized pose When the weight is fixed The process is expressed as,

[0095]

[0096] Among them, the optimization weight When the fixed position The process is expressed as:

[0097]

[0098] in, is the weight after t-1 iterations, It is the penalty term of the weight given by the asymptotic non-convexity after t-1 iterations. Through the above cross optimization iteration, the feature pair cost function is minimized To estimate robust and accurate pose transformations

[0099] In this embodiment, based on the constructed feature pair of the current frame and the local feature map, not only can the local feature map be updated, but also the pose estimation can be performed to achieve the output of the pose and map.

[0100] In order to prove the effectiveness of the LiDAR odometer method based on step-by-step feature extraction provided in this embodiment, an experimental evaluation is performed on the KITTI odometer dataset. The KITTI odometer dataset is one of the most widely used outdoor positioning estimation datasets. The KITTI odometer dataset provides the true values ​​of 11 sequences (Seq.00~Seq.10), which are provided by GPS / IMU. The LiDAR used in this dataset is the Velodyne HDL-64E LiDAR. These 11 sequences contain a variety of scenes, including highway scenes (01), urban scenes (00, 06, 07, 08) and rural scenes (02, 03, 04, 05, 09, 10). In order to ensure the fairness of the results, the method provided in this embodiment and the method compared below are both pure LiDAR real-time SLAM methods that do not use loop detection. The test environment is based on ROS Noetic and Ubuntu 20.04. The method provided in this embodiment is compared with some state-of-the-art methods: LeGo-LOAM, SUMA, GICP, ISC-LOAM, A-LOAM, DMLO, LiTAMIN2 and F-LOAM. In order to obtain fair results, pure lidar algorithms are used, and the evaluation indicators used are absolute trajectory error (Absolute Trajectory Error, ATE, m / 100m). The performance results of all methods on the KITTI00~10 sequences are shown in Table 1. From the results in Table 1, the method provided in this embodiment (Ours) has a clear advantage in accuracy. Except for the method provided in this embodiment, the experimental results of other methods are derived from the results of tests in their respective papers or other papers. The method provided in this embodiment performs best on sequences 02, 03, 04 and 10. In addition, the comparison between the estimated trajectory and the true trajectory by the method provided in this embodiment is as follows. Fig.10 shown.

[0101] Table 1

[0102]

[0103] S4, based on feature stability, updating the local feature map through the feature point cloud extracted from the current frame and the current local feature map; specifically, the following steps may be included:

[0104] In this embodiment, the same stability evaluation index is used for the edge feature points and plane feature points extracted step by step, and the local feature map is updated according to the evaluated stability: the function is defined as Determine a feature point that appears in the current local feature map starting from the k0th frame Observability at the kth frame (k ≥ k0), It is expressed as:

[0105]

[0106] in, is the set of edge feature points in the current frame, is the set of plane feature points in the current frame, p k is any feature point in the current frame, Yes k The set of corresponding points in the local feature map; express It can still be observed in the kth frame, that is, in the kth frame, there is a feature point p k and the points in the local feature map correspond;

[0107] based on Observability, definition judge stability, It is expressed as:

[0108]

[0109] Among them, k i ∈[k0, k], γ∈[0, 1] is a control time interval pair Influencing parameters,Compared with plane feature points, edge feature points are more difficult to extract, therefore,γ is set larger to balance the stability of the two features in observation;

[0110] Finally, set the stability threshold θ F , for any feature point like is retained in the local feature map; otherwise, Will The present invention retains or deletes the feature points in the local feature map through the above stability evaluation method, thereby updating the local feature map.

[0111] In summary, the laser radar odometer method based on step-by-step feature extraction provided by the embodiment of the present invention has at least the following beneficial effects:

[0112] 1) The spatial primitive of the common uniform voxel clustering model is the cubic voxel. This spatial primitive has the characteristic of isotropy and does not meet the distribution characteristics of the laser radar point cloud. In order to improve the correctness of point cloud clustering, the efficiency and accuracy of registration, the embodiment of the present invention provides a concentric clustering model constructed based on the distribution characteristics of the laser radar point cloud. Clustering the laser radar point cloud by the concentric clustering model can effectively improve the efficiency and accuracy of the laser radar point cloud registration.

[0113] 2) The present invention proposes a method for detecting edge feature points based on the continuity of adjacent points on the same scene surface. Since the lidar point cloud is sparsely distributed in the vertical direction and densely distributed in the horizontal direction, the features extracted in the horizontal direction, i.e. on the same scan line, are more significant and less likely to be misdetected. Some researchers have extracted edges and plane geometric features by calculating the local smoothness of the point cloud on the same scan line. However, the edges extracted by this feature extraction method alone are prone to discontinuity, thereby reducing the accuracy of the odometer. In order to solve this problem, this embodiment, based on the edge feature point detection method based on the continuity of adjacent points on the same scene surface, proposes a technical solution for extracting features in steps to achieve a more efficient and accurate feature extraction effect, which helps to improve the accuracy of pose transformation estimation (i.e., odometer);

[0114] 3) By minimizing the distance between corresponding feature pairs, the pose transformation between frames is estimated, thereby improving the accuracy of pose transformation estimation;

[0115] 4) Evaluation experiments were conducted on the KITTI dataset; the experimental results show that the method of this embodiment performs best in most sequences, and its average performance exceeds all the most advanced laser radar odometer methods for comparison. Therefore, the laser radar odometer method based on step-by-step feature extraction provided in this embodiment can improve the accuracy of the laser radar-based pose estimation task.

[0116] Fig.11 is a schematic diagram of the structure of a laser radar odometer device provided by an embodiment of the present invention, such as Fig.11 Optionally, the laser radar odometer device 410 may include a first processor 2001 .

[0117] Optionally, the laser radar odometer device 410 may also include a memory 2002 and a transceiver 2003 .

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

[0119] Combine the following Fig.11 The components of the laser radar odometer device 410 are described in detail:

[0120] The first processor 2001 is the control center of the laser radar odometer device 410, 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 an embodiment of the present invention, such as one or more microprocessors (digital signal processors, DSPs), or one or more field programmable gate arrays (FPGAs).

[0121] Optionally, the first processor 2001 can perform various functions of the lidar odometer device 410 by running or executing a software program stored in the memory 2002 and calling data stored in the memory 2002.

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

[0123] In a specific implementation, as an embodiment, the laser radar odometer device 410 may also include multiple processors, such as Fig.11 The first processor 2001 and the second processor 2004 are shown in FIG. Each of these processors can be a single-core processor (single-CPU) or a multi-core processor (multi-CPU). The processor here can refer to one or more devices, circuits, and / or processing cores for processing data (e.g., computer program instructions).

[0124] 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.

[0125] 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 accessed through the interface circuit ( Fig.11 (not shown) is coupled to the first processor 2001, which is not specifically limited in this embodiment of the present invention.

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

[0127] Optionally, the transceiver 2003 may include a receiver and a transmitter ( Fig.11 The receiver is used to implement a receiving function, and the transmitter is used to implement a sending function.

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

[0129] It should be noted that Fig.11 The structure of the laser radar odometer device 410 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.

[0130] In addition, the technical effects of the laser radar odometer device 410 can refer to the technical effects of the laser radar odometer method based on step-by-step feature extraction described in the above method embodiment, and will not be repeated here.

[0131] 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. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0132] 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).

[0133] 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.

[0134] 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.

[0135] In the present invention, "at least one" means one or more, and "more than one" 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 or plural items. For example, at least one of a, b, or c can be represented by: a, b, c, ab, ac, bc, or abc, where a, b, c can be single or multiple.

[0136] 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.

[0137] 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.

[0138] 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.

[0139] 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.

[0140] 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.

[0141] 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 can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a 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: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0142] 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 laser radar odometer method based on step-by-step feature extraction, characterized in that: The method comprises: A concentric clustering model based on fan-shaped voxels is constructed to process the original three-dimensional point cloud obtained from the lidar to obtain a lidar point cloud composed of stable large objects; Perform step-by-step feature extraction on LiDAR point clouds consisting of stable large objects; Based on the feature point cloud extracted step by step, the feature pair of the current frame and the local feature map is constructed. The distance between the feature pairs and the truncated least squares method with progressive non-convexity are used to estimate the pose transformation by minimizing the feature pair cost function. Based on feature stability, the local feature map is updated through the feature point cloud extracted from the current frame and the current local feature map.

2. The laser radar odometer method based on step-by-step feature extraction according to claim 1, characterized in that: The fan-shaped voxels are constructed based on the distribution characteristics of the laser radar point cloud; In the concentric clustering model, all fan-shaped voxels have the same polar angle and voxel height, but different lengths. As the distance from the fan-shaped voxel to the lidar increases, the length and arc length of the fan-shaped voxel gradually increase.

3. The laser radar odometer method based on step-by-step feature extraction according to claim 1, characterized in that: The concentric clustering model based on the combination of fan-shaped voxels is constructed to process the original three-dimensional point cloud obtained from the laser radar, and the laser radar point cloud composed of stable large objects is obtained, including: Traverse the original 3D point cloud obtained from the LiDAR, and find the sector voxel to which the traversed point belongs through coordinate mapping. The sector voxel is recorded as V i,j,k ; Where i, j, and k represent the indices of the voxel in radial, polar, and directions, respectively; In voxel V i,j,k The voxel V in the 3*3 neighborhood centered i′,j′,k′ Perform labeling; among them, voxel V i′,j′,k′ The coordinate index (i′, j′, k′) is expressed as: {(i′,j′,k′)∣i′∈{i-1,i,i+1},j′∈{j-1,j,j+1},k′∈{k-1,k,k+1}} Let the neighborhood voxel be the voxel set S i,j,k ; Among them, if S i,j,k When there are fan-shaped voxels that have been marked as a certain class, S i,j,k The voxels in the i,j,k The earliest created class; if S i,j,k If there is no fan-shaped voxel marked as a certain class, a new class is created and S i,j,k The voxels in are labeled as this new class; After traversing the original 3D point cloud obtained from the LiDAR, all non-empty voxels in the concentric clustering model are labeled as a certain class; Set a quantity threshold θ N , the number of points is less than θ N The point cloud with a number greater than θ is regarded as an unstable, small-category point cloud and is removed from the original 3D point cloud, leaving the point cloud with a number greater than θ as a stable, large-category point cloud. N The point cloud of the concentric clustering model is used to obtain the lidar point cloud composed of stable large objects.

4. The laser radar odometer method based on step-by-step feature extraction according to claim 3, characterized in that: The step-by-step feature extraction of the laser radar point cloud composed of stable large objects includes: The adjacent point continuity judgment method based on isosceles triangle constraint is used to judge whether two adjacent points are continuous on the same scene surface; The edge feature point detection method based on the continuity of adjacent points on the same scene surface extracts the edge feature point cloud from the output point cloud of the concentric clustering model, and the remaining point cloud that does not belong to the edge feature point cloud is divided into non-edge feature point cloud; Detecting planar feature points in non-edge feature clouds based on local smoothness.

5. The laser radar odometer method based on step-by-step feature extraction according to claim 4, characterized in that: The method for determining the continuity of adjacent points based on isosceles triangle constraints to determine whether two adjacent points are continuous on the same scene surface includes: Define the origin of the laser radar as point o. For any point p and its adjacent point q on the same laser beam, if point p and point q are continuous on the same scene surface, then the distance d from point p to point o is po Approximately equal to the distance d from point q to point o qo , point o, point p and point q form an approximate isosceles triangle as vertices, and the base b of the triangle is pq It is expressed as: b pq =2(1-cosα)[min(d po ,d qo )] 2 Among them, α is the angle formed by the two line segments from point o to point p and from point o to point q, d po and d qo are the distances from point p to point o and from point q to point o; the length of the base of the triangle is b pq Approximately the distance d from point p to point q pq ,Right now Among them, d pq The size of is the Euclidean distance between point p and point q; Define the function g(p,q) to determine whether points p and q are continuous on the same scene surface. g(p,q) is expressed as: Wherein, η is a constant, η>1; if point p and point q are not continuous on the same scene surface, then g(p,q)=0; if point p and point q are continuous on the same scene surface, then g(p,q)=1.

6. The laser radar odometer method based on step-by-step feature extraction according to claim 5, characterized in that: The edge feature point detection method based on the continuity of adjacent points on the same scene surface extracts the edge feature point cloud from the output point cloud of the concentric clustering model, including: Based on the defined function g(p,q), a Boolean function G(S) is defined to determine whether point p is continuous with the points in the adjacent point set S on one side on the same scene surface. G(S) is expressed as: When G(S) = 1, it means that point p is continuous with all points in S on the same scene surface; Definition S f and S b are the forward neighboring point set and the backward neighboring point set of point p respectively. When point p is continuous with only one side of the neighboring point set on the same scene surface, G(S f ) and G(S b )satisfy: Point p is considered as an edge feature point, where Represents the exclusive OR operation; When point p is continuous with the adjacent points on both sides on the two scene surfaces, that is, G(S f ) and G(S b )satisfy: G(S f )=G(S b )=1 Defining functions Calculate S f and S b The main direction vector and The angle size and set an angle threshold θ fb ,when When , point p is considered as an edge feature point; It is expressed as: Among them, ‖·‖2 represents the Euclidean norm of the vector, the main direction vector and By S f With point S b Their respective covariance matrices and Perform feature decomposition to obtain .

7. The laser radar odometer method based on step-by-step feature extraction according to claim 6, characterized in that: The detecting of the plane feature point cloud in the non-edge feature point cloud based on the local smoothness comprises: Set point is any point of the non-edge feature point cloud, For point A collection of adjacent points on the same laser beam, The points in are evenly distributed on the On both sides of The local smoothness It is expressed as: in, is the size of the set of adjacent points, Indicate point The distance to the lidar sensor origin, Indicate point To point The distance Is a collection Points in the C ,like point are considered as plane feature points.

8. The laser radar odometer method based on step-by-step feature extraction according to claim 1, characterized in that: The feature point cloud extracted step by step is used to construct feature pairs of the current frame and the local feature map, and the truncated least squares method of the distance between the feature pairs and the progressive non-convexity is used to estimate the pose transformation by minimizing the feature pair cost function. The method includes: For any edge feature point in the edge feature point cloud of the current frame Find the corresponding adjacent point set S on the edge feature point cloud in the current local feature map ε , calculate S ε The geometric center point and the covariance matrix Covariance matrix Perform eigenvalue decomposition to obtain the main direction vector Along the main direction vector and through the geometric center Select the line segment at the geometric center Two points on both sides and point The distance between the feature pair formed by the edge feature point cloud in the current local feature map is expressed as: in, is the edge feature distance, is the world coordinate of the lidar sensor at the kth frame, yes The coordinates in the world coordinate system are is the expected pose transformation; For any plane feature point in the plane feature point cloud of the current frame Find the corresponding set of adjacent points on the plane feature point cloud in the current local feature map calculate The geometric center point and the covariance matrix Covariance matrix Perform eigenvalue decomposition to obtain the normal vector point The distance between the feature pair formed by the plane feature point cloud in the current local feature map is expressed as: in, is the plane feature distance, yes The coordinates in the world coordinate system are The edge feature distance Distance from plane feature As the residual in the optimization process, the truncated least squares method based on asymptotic non-convexity minimizes the feature pair cost function through cross optimization To estimate the pose transformation in, It is expressed as: in, Lie algebras representing special Euclidean groups; The pose transformation is The distance between feature pairs in the case of p i is any feature point; w i ∈[0,1] represents the weight of each feature residual; i=1,2,3,…,N, N is the total number of feature points, φ ρ (w i ) is a penalty term with weights given by asymptotic non-convexity.

9. The laser radar odometer method based on step-by-step feature extraction according to claim 8, characterized in that: φ ρ (w i ) is expressed as: Among them, μ is the control parameter that changes continuously during the iterative optimization process, is a fixed cutoff threshold; at the first iteration, the initial value μ0 of the control parameter μ is: in, It indicates the maximum value of all feature distance pairs after calculating all feature pair distances for the first time; It is expressed as: in, is the set of edge feature points in the current frame, is the set of plane feature points in the current frame.

10. The laser radar odometer method based on step-by-step feature extraction according to claim 1, characterized in that: For the edge feature points and plane feature points extracted step by step, the same stability evaluation index is used, and the local feature map is updated according to the evaluated stability: define the function Determine a feature point that appears in the current local feature map starting from the k0th frame Observability at the kth frame (k ≥ k0), It is expressed as: in, is the set of edge feature points in the current frame, is the set of plane feature points in the current frame, p k is any feature point in the current frame, Yes k The set of corresponding points in the local feature map; express It can still be observed in the kth frame, that is, in the kth frame, there is a feature point p k and the points in the local feature map correspond; based on Observability, definition judge stability, It is expressed as: Among them, k i ∈[k0,k], γ∈[0,1] is a control time interval pair Influencing parameters; Set the stability threshold θ F , for any feature point like is retained in the local feature map; otherwise, Will Remove from the local feature map.

Images