A LiDAR Odometry Method Based on Step-by-Step Feature Extraction

Abstract

The present invention provides a laser radar odometry method based on step-by-step feature extraction, relating to the field of computer vision technology. The method comprises: constructing a concentric clustering model based on fan-shaped voxels, processing the original three-dimensional point cloud acquired from the laser radar, and obtaining a laser radar point cloud composed of stable large objects; performing step-by-step feature extraction on the laser radar point cloud composed of stable large objects; constructing feature pairs between the current frame and a local feature map based on the step-by-step extracted feature point cloud, and estimating the pose transformation by minimizing the feature pair cost function using a truncated least squares method based on the distance between the feature pairs and the progressive non-convexity; and updating the local feature map based on feature stability using the feature point cloud extracted from the current frame and the current local feature map. The present invention can effectively improve the accuracy and robustness of pose estimation tasks based on laser radar.

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, 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 recent development of deep learning technology and its successful application in the field of image processing, researchers have proposed deep learning-based LiDAR SLAM methods. LO-Net was the first to accomplish the deep learning-based LiDAR odometry task. LO-Net is an end-to-end method that does not require 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. Deep learning-based LiDAR SLAM is prone to overfitting, lacks stability, and has a high deployment cost, which significantly limits the application scenarios of deep learning-based LiDAR SLAM methods. Therefore, in comparison, traditional LiDAR SLAM methods are more widely used in practice.

[0004] Traditional LiDAR SLAM methods can be categorized as direct registration and feature matching-based methods. Direct registration methods primarily include those based on Normal Distribution Transformation (NDT) and Iterative Closest Point (ICP). NDT-based SLAM methods transform a reference point cloud into a multidimensional normal distribution and calculate the optimal pose transformation by maximizing the probability that the currently scanned point lies on the reference point cloud surface. ICP-based SLAM methods minimize the distance between corresponding point cloud pairs to obtain the optimal pose transformation between point clouds. Other ICP-based methods include GICP and VGICP. Because direct registration methods often directly input the raw point cloud, they are computationally expensive. In contrast, feature matching-based LiDAR SLAM methods may perform better. Feature matching-based SLAM methods first extract point cloud features, detecting geometric feature points such as edge points and plane points from the LiDAR point cloud. They then determine the pose transformation of the LiDAR sensor between adjacent frames by minimizing the distance between corresponding feature point cloud pairs. Among feature-matching-based lidar SLAM methods, the LOAM method is one of the most representative. Many researchers have improved upon the LOAM method and proposed numerous variants. The LOAM framework consists of three parallel steps: feature extraction, pose estimation, and mapping. This computationally efficient framework serves as the foundation for most subsequent feature-matching-based methods. The LOAM method first extracts edge and plane features from consecutive lidar point cloud frames. It then estimates the pose transformation by minimizing the edge-to-edge and plane-to-plane distances corresponding to adjacent frames. The LOAM method's feature extraction module effectively improves its efficiency and accuracy, but it also has significant drawbacks. The LOAM method extracts features by calculating the local smoothness of the point cloud along the same scan line. This method is prone to extracting erroneous feature information, thus reducing odometry accuracy. The LOAM method achieves low-precision odometry through high-frequency (10 Hz) matching between adjacent frames, and effectively improves accuracy through back-end low-frequency (1 Hz) mapping to optimize the pose. However, the back-end mapping optimization has high maintenance costs and is difficult to guarantee real-time performance. Summary of the Invention

[0005] In order to solve the technical problem of low odometer accuracy 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 odometry method based on step-by-step feature extraction is provided, the method being implemented by a laser radar odometry device, the method comprising:

[0007] A concentric clustering model based on fan-shaped voxels is constructed to process the original 3D 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 pairs of the current frame and the local feature map are 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 odometry device is provided, comprising: a processor; and 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 odometry methods based on step-by-step feature extraction is implemented.

[0012] On the other hand, a computer-readable storage medium is provided, wherein the storage medium stores at least one instruction, and the at least one instruction is loaded and executed by a processor to implement any one of the above-mentioned lidar odometry 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] In an embodiment of the present invention, a concentric clustering model based on a combination of 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; features are extracted step by step on the lidar point cloud composed of stable large objects; based on the step-by-step extracted feature point cloud, feature pairs of the current frame and the local feature map are constructed, and the truncated least squares method of the distance and progressive non-convexity between the feature pairs is used to estimate the pose transformation by minimizing the feature pair cost function; based on feature stability, the local feature map is updated using 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 lidar 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 This 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 odometry 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 by 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 into 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 according to 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 point p and point q do not satisfy the continuity condition on the same scene surface according to 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 the backward adjacent point set and p2 observes the forward adjacent point set;

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

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

[0029] Figure 9 Schematic diagram of a plane feature point cloud image extracted from a non-edge feature point cloud based on local smoothness;

[0030] Figure 10 A schematic diagram comparing the trajectory estimated by the lidar odometry method based on step-by-step feature extraction and the actual trajectory provided by an embodiment of the present invention;

[0031] Figure 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 an "exemplary" in the present invention should not be interpreted as being preferred or advantageous over other embodiments or designs. Rather, the use of the word "exemplary" is intended to present concepts in a concrete manner. Furthermore, in the embodiments of the present invention, "and / or" can mean both or either of the two.

[0034] In the embodiments of the present invention, the terms "image" and "picture" may be used interchangeably. It should be noted that, when the distinction between them is not emphasized, their intended meanings are the same. The terms "of," "corresponding," and "corresponding" may be used interchangeably. It should be noted that, when the distinction between them is not emphasized, their intended meanings 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 clearer, 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 voxels are constructed based on the distribution characteristics of the LiDAR point cloud. Therefore, the constructed concentric clustering model satisfies the distribution characteristics of the LiDAR point cloud. The front view and cross-sectional view of the concentric clustering model are shown in Figures 3(a) and 3(b), respectively. The top view and side view of three radially adjacent fan-shaped voxels are shown in Figures 4(a) and 4(b), respectively. As spatial primitives, fan-shaped voxels can efficiently divide the point cloud space. The size of the fan-shaped voxels gradually increases with the distance from the laser sensor, which can meet the distribution characteristics of the LiDAR point cloud, which is dense near and sparse far. The boundary of the fan-shaped voxel is determined by three parameters: the voxel length ρ, the voxel height h, and the polar angle θ.

[0040] In this embodiment, in the concentric clustering model, all sector voxels have the same polar angle and voxel height, but different lengths. As the distance from the sector voxel to the laser radar increases, the length and arc length of the sector 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 lidar to obtain a lidar point cloud composed of stable large objects. Specifically, the following steps may be included:

[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 recorded as V i，j，k ; Where i, j, and k represent the index of the voxel in radial, polar, and direction, respectively;

[0043] In voxel V i，j，k The voxel V in the 3*3 neighborhood centered i′，j′，k′ Perform labeling work; 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 is a sector voxel that has 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 When there is no fan-shaped voxel that has been 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 of is considered as unstable, small class point cloud, and is removed from the original 3D point cloud, leaving the number of stable, large classes greater than θ 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 LiDAR point cloud is passed through a concentric clustering model to remove unstable, small-category point clouds, while retaining stable, large-category 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 location. The upper boxes represent small objects with unstable observations, and corresponding points are difficult to find in subsequent point cloud frames. The upper boxes in Figure 5(b) indicate that these point clouds were filtered out by the concentric clustering model. Similarly, the lower box indicates a bus, which should be the large object remaining in the point cloud image. Figures 5(a) and 5(b) confirm that the concentric clustering model only filters out a small number of points and does not affect the large objects that occupy the majority of the point cloud. At the same time, small objects that are unlikely to have a positive effect on the subsequent odometry estimation are 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 determine 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 approximately isosceles triangle as vertices, and the base b of the triangle is pq 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 They are the distances from point p to point o and the distance 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 points p and 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 extracts the edge feature point cloud from the output point cloud of the concentric clustering model, including:

[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] Define 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 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 a function Calculate S f and S b The main direction vector and and set an angle threshold θ fb ,when When , point p is considered to be an edge feature point; 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 eigendecomposition to obtain .

[0072] like Figure 8 The figure shows 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 up a point is any point of the non-edge feature point cloud, for point A set of adjacent points on the same laser beam, The points in the are evenly distributed on the On both sides of Local smoothness Expressed as:

[0075]

[0076] in, is the size of the set of adjacent points, Indicates a point The distance to the lidar sensor origin, Indicates a point Arrive distance, is a collection Points in, set a smoothness threshold θ C ,like point is considered as a plane feature point. Figure 9 As shown, it 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 step-by-step extracted feature point cloud, constructs feature pairs (i.e., data association) between the current frame and the local feature map, and estimates the pose transformation by minimizing the feature pair cost function using the truncated least squares method with distance between feature pairs and progressive non-convexity. 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 passing through the geometric center Select the line segment at the geometric center Two points on both sides and point The distance between the feature pairs 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 to 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, Expressed as:

[0085]

[0086] 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 residual of each feature (i.e., the edge feature distance d ε Or 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, Indicates the maximum value of all feature distance pairs after calculating all feature pair distances for the first time; 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 fixed weight 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 a penalty term based on the weight given by the progressive 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 demonstrate the effectiveness of the laser radar odometry method based on step-by-step feature extraction provided in this embodiment, an experimental evaluation was conducted on the KITTI odometry dataset. The KITTI odometry dataset is one of the most widely used outdoor positioning estimation datasets. The KITTI odometry dataset provides the true values of 11 sequences (Seq.00 to Seq.10), which are provided by GPS / IMU. The laser radar used in this dataset is the Velodyne HDL-64E laser radar. 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 laser radar 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 of the most advanced 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 (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, it can be seen that 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 of the method provided in this embodiment is as follows: Figure 10 shown.

[0101] Table 1

[0102]

[0103] S4, based on feature stability, updates the local feature map using the feature point cloud extracted from the current frame and the current local feature map; specifically, this may include the following steps:

[0104] In this embodiment, the same stability evaluation index is used for 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), 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, It is p 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, 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, if 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 odometry 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, which is isotropic and does not meet the distribution characteristics of LiDAR point clouds. To improve the accuracy of point cloud clustering, registration efficiency, and accuracy, the present invention provides a concentric clustering model based on the distribution characteristics of LiDAR point clouds. Clustering LiDAR point clouds using this concentric clustering model can effectively improve the efficiency and accuracy of LiDAR 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 detected incorrectly. 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 more efficient and accurate feature extraction effects, 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, outperforming all state-of-the-art lidar odometry methods on average. Therefore, the lidar odometry method based on step-by-step feature extraction provided in this embodiment can improve the accuracy of pose estimation tasks based on lidar.

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

[0117] Optionally, the lidar 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] The following combination Figure 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 and can be a single processor or a collective term for multiple processing elements. For example, the first processor 2001 can be 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 software programs 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 Figure 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 Figure 11 1 and 2. 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). A processor herein 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 by the first processor 2001 for execution. The specific implementation method can refer to the above method embodiment and will not be repeated here.

[0125] Alternatively, the memory 2002 may be a read-only memory (ROM) or other type of static storage device that can store static information and instructions, a random access memory (RAM) or other type of dynamic storage device 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 compact 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 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 exist independently and accessed through the interface circuit ( Figure 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 ( Figure 11 (not shown separately in the figure). 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 ( Figure 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 Figure 11 The structure of the lidar 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 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 through software, hardware (such as circuits), firmware, or any other combination. When implemented using 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 program are loaded or executed on a computer, the processes or functions described in accordance with the embodiments of the present invention are 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. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via a wired method (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 a collection of one or more available media. The available medium can be a magnetic medium (such as a floppy disk, hard disk, or magnetic tape), an optical medium (such as a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state drive.

[0134] It should be understood that the term "and / or" as used herein simply describes a relationship between associated objects, indicating that three possible relationships exist. For example, "A and / or B" can represent: A alone, A and B together, or B alone. A and B can be singular or plural. Furthermore, the character " / " as used herein generally indicates an "or" relationship between the associated objects, but it may also indicate an "and / or" relationship. For specific understanding, please refer to the context.

[0135] In this disclosure, "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 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 plural.

[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. Professionals and technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present invention.

[0138] Those skilled in the art will 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 merely illustrative. For example, the division of the units is merely a logical function division. In actual implementation, there may be other division methods, 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 interface, indirect coupling or communication connection of the device or unit, which can be electrical, mechanical or other forms.

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

[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, 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 and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment 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 description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A laser radar odometry 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 3D 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 pairs of the current frame and the local feature map are 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 lidar, and the lidar 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 index of the voxel in radial, polar, and direction, respectively; In voxel V i,j,k The voxel V in the 3*3 neighborhood centered i′,j′,k′ Perform labeling work; 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 is a sector voxel that has 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 When there is no fan-shaped voxel that has been 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 of is considered as unstable, small class point cloud, and is removed from the original 3D point cloud, leaving the number of stable, large classes greater than θ 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 lidar point cloud composed of stable large objects includes: The adjacent point continuity judgment method based on isosceles triangle constraint is used to determine 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 point clouds in non-edge feature point 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 approximately isosceles triangle as vertices, and the base b of the triangle is pq 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 They are the distances from point p to point o and the distance 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; Define 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 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 a function Calculate S f and S b The main direction vector and and set an angle threshold θ fb ,when When , point p is considered to be an edge feature point; 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 eigendecomposition 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 a plane feature point cloud in a non-edge feature point cloud based on local smoothness includes: Set up a 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 the are evenly distributed on the On both sides of Local smoothness Expressed as: in, is the size of the set of adjacent points, Indicates a point The distance to the lidar sensor origin, Indicates a point Arrive distance, is a collection Points in, set a smoothness threshold θ 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 step-by-step extraction of feature point clouds constructs feature pairs between the current frame and the local feature map, and the pose transformation is estimated by minimizing the feature pair cost function using the truncated least squares method with distance between feature pairs and progressive non-convexity. 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 passing through the geometric center Select the line segment at the geometric center Two points on both sides and point The distance between the feature pairs 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 to 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, 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, Indicates the maximum value of all feature distance pairs after calculating all feature pair distances for the first time; 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), 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, It is p 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, 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, if Will Remove from the local feature map.

Images