A LiDAR edge feature prediction method based on indoor feature degradation environment

Abstract

The present invention discloses a laser radar edge feature prediction method in an indoor feature degradation environment, which is applied to the field of three-dimensional high-precision simultaneous positioning and composition in this environment, and aims at the problem that the composition and positioning mapping effects of the prior art have feature degradation in an indoor feature degradation environment. The present invention extracts edge features and plane features according to the curvature value of the original laser point cloud; then, according to the structural information of the environment, the extracted plane features are divided into surface feature points, ground similar feature points and non-ground feature points; secondly, the present invention uses a point cloud segmentation method based on Euclidean clustering growth to further segment these three types of plane feature points, and uses a random sampling consistency method to fit these segmented three-dimensional point clouds with a straight line model, and then predicts virtual edge points by using the method of fitting straight lines intersecting at points; finally, these predicted virtual edge feature points are merged with edge feature points based on curvature, and used as the final edge features.

Description

Technical Field

[0001] The present invention belongs to the field of indoor environment three-dimensional map surveying and mapping, and in particular relates to a three-dimensional high-precision simultaneous positioning and mapping technology in an indoor feature-degraded environment. Background Art

[0002] Simultaneous localization and mapping (SLAM) plays an important role in autonomous driving, robot measurement, and map layout reconstruction. Today, many SLAM algorithms use cameras and lidar as the main sensor units. Compared with cameras, lidar has higher robustness and longer-range detection capabilities. With the development of autonomous driving, lidar SLAM has been widely studied and applied (Hesch, JA; Mirzaei, FM; Mariottini, GL; Roumeliotis, SI;

[0003] Alaser-aided inertial navigation system (L-INS) for human localization in unknown indoor environments. In Proceedings of 2010IEEE International Conference on Robotics and Automation (ICRA), Anchorage, AK, USA, 3-7May 2010; pp.5376–5382.).

[0004] Generally, the LiDAR SLAM framework includes many common steps, such as point cloud matching, LiDAR front-end odometer, back-end nonlinear optimization, loop detection, map construction, etc. Among them, point cloud matching is one of the most important steps, because the accuracy of the front-end odometer directly depends on the quality of point cloud matching. According to the point cloud matching strategy, LiDAR SLAM algorithms can be divided into two categories. The first category is called direct matching method, and the second category is called feature-based matching method. For the former, a matching model is directly established at the original point. In the paper (Low, KL; Linear least-squares optimization for point-to-plane ICP surface registration. Department of Computer Science, Chapel Hill, NCSU, USA, Tech. Rep. TR04-004, 2004.), the authors established an iterative closest point (ICP) matching model and matched the close points of two adjacent frames to estimate the optimal pose state of the LiDAR. The high accuracy of ICP matching is due to the use of all points for matching. In order to further improve the matching accuracy, researchers proposed an improved matching model based on ICP, namely the point-to-line PL-ICP model (Censi, A.; An ICP variant using a point-to-line metric. In Proceedings of 2008 IEEE International Conference on Robotics and Automation (ICRA), Pasadena, CA, USA, 19-23 May 2008; pp. 19-25.). In PL-ICP, the authors proposed a point-to-line matching model. Experimental results show that PL-ICP has higher accuracy than ICP. However, ICP-based methods are time-consuming, susceptible to noise, and difficult to build maps in real time.

[0005] Unlike direct point matching methods, feature-based matching methods match through features extracted from original points. Since feature points can well represent the outline of the scene and are relatively small in number, real-time performance is greatly improved. A representative algorithm using the feature matching method is LOAM (Ji, Z.; Singh, S.; Low-drift and Real-time LiDAROdometry and Mapping. Autonomous Robots. 2017, 41, 401-416.). In LOAM, feature points are divided into two categories according to the size of the curvature value, namely plane feature points and edge feature points. The matching process of plane and edge feature points is carried out in a point-to-plane and point-to-line manner, respectively. Experiments show that LOAM has good real-time performance. In order to achieve higher positioning accuracy while maintaining real-time performance, another LiDAR mileage mapping framework LeGO LOAM was proposed (Shan, T.; Englot, B.; LeGO-LOAM: Lightweight and ground-optimized LiDAR odometry and mapping on variable terrain. In Proceedings of 2018 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 1-5 Oct 2018; pp. 4758-4765.). The authors of Lego loam improved the feature point matching model by adopting the two-step Levenberg-Marquardt (Marquardt, DW; An algorithm for least-squares estimation of nonlinear parameters. Journal of the society for Industrial and Applied Mathematics. 1963, 11, 431-441.) optimization method, and added graph optimization in the back end to correct the global pose of the LiDAR. Experiments show that the map reconstruction accuracy of LeGO LOAM is better than LOAM.

[0006] It is worth noting that the above-mentioned SLAM algorithm based on feature matching is only applicable to mechanical rotating laser radars. Due to the difference in scanning models, it cannot be directly used for solid-state laser radars with small field of view. In order to integrate the LOAM algorithm into solid-state laser radars, a high-precision laser radar algorithm Livox LOAM was proposed (Lin, J.; Zhang, F.; Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV. In Proccedings of 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 31 May-31 Aug 2020; pp. 3126-3131.), Livox LOAM changes the feature matching mode of LOAM, and removes the point matching constraints on different scanning lines so that the feature matching model can be used for solid-state laser radars. In addition, the authors also proposed a new outlier filtering method, and the composition results of Livox LOAM have higher scene detail representation than LOAM.

[0007] However, for mapping applications in indoor feature-degraded environments (such as long corridors and indoor halls), the feature extraction results will be degraded due to the high repetitiveness and unclear edge features in such environments (Lin, J.; Zhang, F.; Afast, complete, point cloud based loop closure for lidar odometry and mapping.arXiv.org, e-Print Arch. 2019.). In addition, the field of view of solid-state lidar is also limited in this environment. Therefore, in this scenario, the positioning and mapping accuracy of Livox LOAM is reduced. Summary of the invention

[0008] In order to solve the above technical problems, the present invention proposes a laser radar edge feature prediction method based on indoor feature degradation environment, which is used for indoor feature degradation environment. Figure 3 Dimensional reconstruction can effectively solve the edge feature problem in this environment and improve the quantity and quality of edge features.

[0009] The technical solution adopted by the present invention is: a laser radar edge feature prediction method based on an indoor feature degradation environment, comprising:

[0010] S1, collect indoor feature degradation environment data through LiDAR;

[0011] S2, extracting the plane feature point cloud F and the edge feature point cloud according to the indoor feature degradation environment data collected in step S1

[0012] S3, classify the extracted plane feature point cloud F according to the contour information of the indoor feature degradation environment, specifically including the following three types of plane points: ground point Ground-like points and non-ground points

[0013] S4, obtaining virtual edge feature points corresponding to the indoor feature degradation environment according to the three types of plane points classified in step S3;

[0014] S5, compare the virtual edge feature points corresponding to the indoor feature degradation environment with the edge feature point cloud in step S2 Merge as the final edge features.

[0015] The beneficial effects of the present invention are as follows: the present invention first extracts edge features and plane features according to the curvature value of the original laser point cloud; then, according to the structural information of the environment, the extracted plane features are divided into three categories, namely ground feature points, ground similar feature points and non-ground feature points; based on these three types of feature points, the present invention further proposes an edge feature prediction method to predict virtual edge feature points of the structural environment, the method uses a point cloud segmentation method based on Euclidean clustering growth to further segment the three types of plane feature points, and uses a random sampling consistency method to fit these segmented three-dimensional point clouds with a straight line model, and finally predicts virtual edge points by intersecting the fitted straight lines at the points; finally, these predicted virtual edge feature points are merged with the edge feature points based on curvature as the final edge features; the present invention can improve the performance of edge feature extraction based on curvature, and improve the simultaneous positioning and composition accuracy of small-field-of-view solid laser radars in indoor environments such as long corridors, indoor halls, rooftops, etc. where edge features are not obvious, especially reducing the positioning error in the gravity direction. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 This is the flowchart of the edge feature prediction method;

[0017] Figure 2 The laser radar and vehicle-mounted system in the specific implementation mode;

[0018] Figure 3 It is a schematic diagram of an experimental scene in a specific implementation mode;

[0019] Among them, (a) is the optical image of the first perspective in the large indoor corridor experimental scene, (b) is the optical image of the second perspective in the large indoor corridor experimental scene, (c) is the optical image of the third perspective in the large indoor corridor experimental scene, and (d) is the optical image of the fourth perspective in the large indoor corridor experimental scene;

[0020] Figure 4 The result output by the single-frame edge feature prediction method in the specific implementation manner;

[0021] Among them, (a) is the single frame laser data collected in experimental scene 1, (b) is the single frame laser data collected in experimental scene 2, and (c) is the single frame laser data collected in experimental scene 3;

[0022] Figure 5 It is the multi-frame accumulation result of edge feature points in the map reconstruction process in the specific implementation mode;

[0023] Figure 6 The results of simultaneous positioning and composition of the experimental scene in the specific implementation method;

[0024] Figure 7 The optical image comparison results of the laser radar map reconstruction result embedded in the experimental scene in the specific implementation method, and the corresponding composition details display results. DETAILED DESCRIPTION

[0025] The algorithm flow of the present invention is as follows Figure 1 The following is a specific implementation of the present invention using a corridor of a large building as an experimental scene: the vehicle-mounted system is equipped with a solid-state laser radar (such as Figure 2 The proposed feature prediction method is integrated into the Livox LOAM mapping framework to realize simultaneous positioning and mapping functions (Lin, J.; Zhang, F.; Loam livox: A fast, robust, high-precision LiDARodometry and mapping package for LiDARs of small FoV. In Proccedings of 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, 31 May-31 Aug 2020; pp. 3126-3131.).

[0026] The optical image of the experimental scene is as follows Figure 3 As shown, (a)-(b) are optical images of a long corridor of a large building in four directions. The specific processing method is detailed in steps S1-S5;

[0027] Step S1: When the vehicle-mounted system is started, the laser radar starts to collect data. The vehicle-mounted system drives along the corridor once and returns to the starting point before stopping, thus ending data collection.

[0028] Step S2: Extract the plane feature point cloud F and edge feature point cloud according to the data preprocessing and feature extraction method in Livox LOAM

[0029] Step S3: further divide the plane feature point cloud F extracted in step S2 into three types: ground points Ground-like points and non-ground points

[0030] First, calculate the normal vector set U corresponding to all points in F. For a plane point f i ∈F(i=1,…,N F ), find the nearest N M points, this experiment takes N M =5, calculate the centroid of these 5 points and record it as Where N F is the number of points in the set F, N F The value of depends on the plane feature extraction method in step S2. Then, The covariance matrix of It can be obtained by the following formula:

[0031]

[0032] The superscript T indicates transposition;

[0033] Solving for Σ via QR decomposition i The corresponding eigenvalues ​​and eigenvectors, then f i The corresponding normal vector u i ∈U is Σ i The eigenvector corresponding to the minimum eigenvalue. For all points f i ∈F(i=1,…,N F ), we can get the corresponding normal vector set U.

[0034] After obtaining the normal vector set U, the plane point F will be classified according to the normal vector direction. Let the gravity vector in the global coordinate system {G} be represents a 3*1 matrix, and g L is the gravity vector in the current radar coordinate system {L}, where Represents a three-dimensional vector. Gravity vector g in radar coordinate system L Through the transformation matrix Sure:

[0035]

[0036] in and They are the rotation matrix and translation vector from the previous frame radar coordinate system to the world coordinate system, represents a 3*3 matrix, and is known. The gravity vector g L The calculation of is as follows,

[0037]

[0038] in, Represents R k-1 The inverse matrix of

[0039] Finally, F can be divided into three different types of plane feature points by the following formula: Ground points, ground-like points and non-ground points Right now,

[0040]

[0041] where ||·|| is the Euclidean distance, is the ground plane fitting method. This method is used to determine whether the input point is a ground point. If f i is a ground point, then σ is a fixed threshold, and in this embodiment, the value is 0.3.

[0042] Step S4: First, taking the radar coordinates as the origin, the current radar coordinate system {L} is cut into many subspaces along the X-axis and Y-axis directions. The subspaces on the X-axis and Y-axis can be expressed as,

[0043] Plane points F will be classified into different subspaces according to their corresponding coordinates, that is,

[0044]

[0045] where d ζ is the subspace interval, which is 0.2m in this embodiment, σ ζ is the subspace width, and in this embodiment, the value is 0.1m. i (a) is the vector f i The ath value of N ζ is the maximum number of X-axis and Y-axis subspaces. In this embodiment, N ζ =150. and Center distance and (For example, the subspace Center distance is d ζ ), n=1,…,N ζ .

[0046] After classifying the points in F into different subspaces, the point cloud in each subspace will be segmented separately. Here, the distance-based region growing segmentation method is used. This method can be used to segment a point cloud and obtain m point cloud subsets ψ(m) = {δ1,δ2,…,δ m}(m depends on the region growing segmentation method), where δ i ,i=1,…,m is the i-th point cloud set in ψ(m). Assume that the input point cloud set in the regional clustering growth method is P in , for point p j ,p i ∈P in The specific segmentation criteria are as follows:

[0047] Condition 1: ||p j -p i || <d GR .

[0048] ·Condition 2: ||u j ×u i || <d u .

[0049] Condition 3: p j With p i Also belongs to or or One of the collections.

[0050] Among them, d GR and d u are fixed parameters of the region growing segmentation method. In this embodiment, the values ​​are 0.5m and 0.3 respectively. j and u i is corresponding to p j and p i The operator × is the cross product. If the above three conditions are met at the same time, p j With p i Belong to the same set.

[0051] At this time, for each subspace, there are m point cloud sets δ i , i = 1, ..., m, use RANSANC to fit the m point clouds separately, and we can get the corresponding m straight line sets η i,i=1,…,m. Use this set of m straight lines to intersect each other at one point in three-dimensional space to obtain the virtual edge feature points of this subspace.

[0052] After that, the virtual feature points obtained in each subspace are added together to obtain the final virtual edge feature points. Finally, the curvature-based edge feature points extracted in step S2 are merged The virtual edge feature points of this step The final edge feature point E is obtained. The effect of single frame data extraction is as follows Figure 4 shown.

[0053] In order to verify the effectiveness of the edge feature prediction method, the present invention uses the extracted edge feature points E to replace the edge feature points in the LivoxLOAM mapping algorithm to reconstruct the long corridor map. The final multi-frame cumulative composition result of the edge feature points in the map reconstruction process is as follows: Figure 5 As shown in the figure, the dotted arrows point to the multi-frame accumulation results of edge feature points, which shows that it has a good representation of the corridor contour. Figure 6 As shown in Figure 2, the loop closure error shows that the positioning and mapping results have high accuracy. Finally, the mapping results are embedded in the optical top view of the actual experimental scene for comparison, and the specific composition details are shown, such as Figure 7 The experimental results show that the proposed method has a high map reconstruction performance in a long corridor environment and provides high-resolution three-dimensional building layout details.

[0054] The laser radar used in the present invention is DJI Livox horizon, which has a detection range of 60m, an angular resolution of 0.05°, a distance resolution of 0.02m, and a laser radar field of view of 81.7° (horizontal) × 25.1° (vertical). In this experiment, the laser radar coordinate system is set with the x-axis in front, the z-axis above, and the y-axis on the left.

[0055] In the present invention, it is considered that for environments with fewer indoor features (such as long corridors, rooftops, etc.), the laser radar feature extraction effect is degraded (Hesch, JA; Mirzaei, FM; Mariottini, GL; Roumeliotis, SI; A laser-aided inertial navigation system (L-INS) for human localization in unknown indoor environments. In Proceedings of 2010 IEEE International Conference on Robotics and Automation (ICRA), Anchorage, AK, USA, 3-7 May 2010; pp. 5376–5382.), which makes the simultaneous positioning and composition effect in this environment worse. The laser radar edge feature prediction method for feature-degraded indoor environments proposed in the present invention can predict more virtual feature points through the structural features of the environment. Experimental results show that the algorithm can reconstruct feature-degraded indoor environments such as long corridors and rooftops with high precision.

[0056] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the scope of protection of the present invention is not limited to such specific statements and embodiments. For those skilled in the art, the present invention may have various changes and variations. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A laser radar edge feature prediction method based on indoor feature degradation environment, characterized in that: include: S1, collect indoor feature degradation environment data through LiDAR; S2, extracting the plane feature point cloud F and the edge feature point cloud according to the indoor feature degradation environment data collected in step S1 S3, classify the extracted plane feature point cloud F, specifically including the following three types of plane points: ground points Ground-like points and non-ground points S4, according to the three types of plane points classified in step S3, obtain virtual edge feature points corresponding to the indoor feature degradation environment; step S4 specifically includes the following sub-steps: S41, take the radar coordinates as the origin, and d ζ is the subspace interval, and the current radar coordinate system {L} is cut into several subspaces along the X-axis and Y-axis directions; S42, classifying all feature points in the plane feature point cloud F into different subspaces according to their corresponding coordinates; S43, segmenting the point cloud in each subspace to obtain a plurality of point cloud sets corresponding to each subspace; S44, performing straight line fitting according to the point cloud in each point cloud set after the subspace segmentation to obtain a straight line, and intersecting these straight lines in pairs in the three-dimensional space to obtain virtual edge feature points of the subspace; S45, adding the virtual edge feature points of each subspace to obtain the final virtual edge feature point S5, compare the virtual edge feature points corresponding to the indoor feature degradation environment with the edge feature point cloud in step S2 Merge as the final edge features.

2. The laser radar edge feature prediction method based on indoor feature degradation environment according to claim 1 is characterized in that: Step S2 specifically calculates the curvature value of the original point to extract the plane feature point cloud F and the edge feature point cloud 3. The laser radar edge feature prediction method based on indoor feature degradation environment according to claim 2 is characterized in that: Step S3 includes the following sub-steps: S31, calculating a normal vector set U corresponding to all plane feature points in the plane feature point cloud F; S32. The plane feature points in the plane feature point cloud F are classified with reference to the corresponding normal vector directions: in, is the ground plane fitting method, · ​​is the Euclidean distance, u i f i The corresponding normal vector, g L is the gravity vector in the current radar coordinate system {L}, and σ is a fixed threshold.

4. The laser radar edge feature prediction method based on indoor feature degradation environment according to claim 3 is characterized in that: The specific implementation process of step S31 is as follows: For the current plane feature point in the plane feature point cloud F, find the N nearest M points, and calculate these N M The centroid of the points is obtained to obtain the covariance matrix of the centroid; Solve the eigenvalues ​​and eigenvectors corresponding to the covariance matrix of the centroid through QR decomposition; The normal vector corresponding to the current plane feature point is obtained as the eigenvector corresponding to the minimum eigenvalue of the covariance matrix of its centroid; According to the feature vectors of all plane feature points in the plane feature point cloud F, the corresponding normal vector set U is obtained.

5. The laser radar edge feature prediction method based on indoor feature degradation environment according to claim 4 is characterized in that: Step S43 uses a distance-based region growing segmentation method to segment the point cloud in each subspace.

Images