Three-dimensional laser synchronous mapping and positioning method based on rotary scanning

Abstract

The invention discloses a three-dimensional laser synchronous mapping and positioning method based on rotary scanning. The method comprises the following steps that S10, a three-dimensional laser radar obtains original point cloud data in a well in real time; s20, correcting the intensity value and carrying out cylindrical projection to obtain a grayscale image; s30, extracting feature points on the grey-scale map by using an ORB method; s40, calculating a point-point residual error, and obtaining an inter-frame pose by using ICP (Inductively Coupled Plasma); and S50, releasing the laser point cloud after inter-frame pose transformation. According to the method, a three-dimensional laser mapping system is adopted to help an underground well chamber and other limited spaces to carry out visual information collection, and the integrity and accuracy of information collection of the limited spaces are improved based on rotary scanning.

Description

Technical Field

[0001] The invention relates to a three-dimensional laser synchronous mapping and positioning method based on rotation scanning. Background Art

[0002] In modern engineering, construction, geological exploration and other fields, the visual information collection of underground wells and other confined spaces faces significant challenges of narrow space, insufficient light and sparse features. These environments are usually compactly designed with limited height and width, making it difficult to move in and set up traditional large-scale fixed 3D laser scanners, while restricting the activities of operators and reducing the flexibility of equipment operation and data collection. Insufficient light is another key issue. Due to the lack of natural light sources in underground environments and limited artificial lighting conditions, photosensitive equipment cannot capture clear images in low-light environments, affecting the extraction of feature points and the reliability of data. In addition, the scarcity of features also increases the difficulty of information collection. The structural features in underground wells are single and unevenly distributed, and traditional laser 3D SLAM methods are prone to positioning and mapping failures. Therefore, it is urgent to develop new, portable and flexible equipment and technologies that combine augmented reality and intelligent algorithms to meet the information collection needs in these complex environments and improve the efficiency of data collection and the accuracy of results. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide a three-dimensional laser mapping system to help collect visual information in limited spaces such as underground wells, and to increase the integrity and accuracy of information collection in limited spaces by a three-dimensional laser synchronous mapping and positioning method based on rotational scanning.

[0004] The technical solution of the present invention is to provide a three-dimensional laser synchronous mapping and positioning method based on rotation scanning with the following structure, comprising the following steps:

[0005] S10. 3D laser radar acquires the original point cloud data in the well in real time;

[0006] S20. Correcting the intensity value and performing cylindrical projection to obtain a grayscale image;

[0007] S30. Extract feature points on the grayscale image using the ORB method;

[0008] S40. Calculate the point-to-point residual and use ICP to obtain the inter-frame pose;

[0009] S50. Publish the laser point cloud after the inter-frame pose transformation.

[0010] Step S20 includes the following two steps:

[0011] (1) The intensity value correction steps are as follows: Use local normal vector analysis to process the impact of the incident angle and calculate the local normal vector of each laser point p:

[0012]

[0013] Among them, p1 and p2 are the neighboring points of p respectively;

[0014] The incident angle θ is calculated as:

[0015] (2) The steps of cylindrical projection are as follows:

[0016] (2.1) First, we need to calculate the row and column coordinates of each point in the laser point cloud on the two-dimensional plane;

[0017] (2.1.1) For each laser point, we first need to convert it into the horizontal angle and vertical angle of the spherical coordinate system:

[0018]

[0019] in, φ are the horizontal and vertical angles of the laser point respectively;

[0020] x, y, and z are the three-dimensional coordinates of the laser point;

[0021] (2.1.2), the row coordinate u of each point corresponds to the vertical scanning angle of the laser radar, and the column coordinate v corresponds to the horizontal scanning angle of the laser radar;

[0022] (2.2) Use the intensity value of each point to fill the corresponding two-dimensional grayscale image, thereby realizing the cylindrical projection of the three-dimensional laser point cloud:

[0023] I(u,v)=I corrected

[0024] Among them, I corrected is the corrected intensity value.

[0025] The steps of extracting feature points on the grayscale image using the ORB method in step S30 are as follows:

[0026] (1) Feature point detection: ORB uses the FAST algorithm to detect corner points. For each pixel in the image, the circular area around it is detected, and the intensity values ​​of these pixels are compared with the intensity value of the central pixel to determine whether it is a corner point. By setting a threshold t, if there is a group of continuous pixels in the circular area, and the absolute value of the difference between its intensity value and the intensity value of the central pixel exceeds the threshold t, then the point is considered a feature point.

[0027] (2) Feature point direction calculation: In order to calculate the direction of each feature point, the ORB algorithm estimates the direction by calculating the image gradient of the area around the feature point; the feature point direction is used for the rotation invariance of the feature descriptor;

[0028]

[0029] Among them, G y , G x Respectively represent the horizontal and vertical gradients of the area around the feature point;

[0030] (3) Feature descriptor generation: ORB uses the BRIEF algorithm to generate feature descriptors by comparing random pixel pairs around the feature points to generate a binary feature vector;

[0031]

[0032] Where p1 and p2 are randomly sampled pixels around the feature point; I(p1) and I(p2) are the grayscale values ​​of the corresponding pixels.

[0033] Step S40: Calculate point-to-point residuals and use ICP to calculate inter-frame poses. The steps are as follows: The ICP algorithm is a common algorithm for aligning two point clouds. It minimizes the distance between corresponding points of the two point clouds through iterative optimization, thereby calculating the inter-frame pose transformation;

[0034] (1) Nearest neighbor search: For each point in the target frame, find the nearest point in the historical frame as the corresponding point. This system uses data structures such as KD trees for efficient nearest neighbor search;

[0035] (2) Calculate the point-to-point residual: For each set of corresponding points (p i , p j ), calculate the point-to-point Euclidean distance as the residual:

[0036] residual i =|p i -q i | 2

[0037] (3) Minimize the residual and solve the rigid body transformation: By minimizing the residual of all corresponding point pairs, solve the rigid body transformation between the two point clouds (rotation matrix R and translation vector t):

[0038]

[0039] The solution is obtained by nonlinear optimization method (LM method is used in this system).

[0040] Step S50: The steps of publishing the laser point cloud are as follows: the inter-frame pose transformation calculated by the ICP algorithm can update the laser point cloud of the current frame to the global coordinate system to obtain the point cloud in the world coordinate system; for each point p i , using the optimal estimate (R, t) obtained by ICP solution to transform:

[0041]

[0042] The updated point cloud data updates the 3D map of the environment in real time.

[0043] After adopting the above structure, the present invention has the following advantages:

[0044] 1. The intensity value of the lidar point cloud is related to the distance and the angle of incidence, and the distance effect is mostly corrected inside the radar. However, the impact of the incident angle on the point cloud intensity is also significant, often resulting in significant changes in the point cloud intensity at different positions of the same object. Therefore, the use of local normal vector analysis is an effective way to deal with the impact of the incident angle. Specifically, calculating the local normal vector of each laser point p can understand the relative angle between the laser beam and the measured surface. By analyzing the local normal vector, the difference in intensity values ​​caused by different incident angles can be identified. After eliminating the impact of the incident angle and distance effects on the intensity measurement value, the overall quality and reliability of the lidar intensity data can be significantly improved.

[0045] 2. In order to realize the cylindrical expansion projection of the laser point cloud, it is first necessary to calculate the row and column coordinates of each point in the laser point cloud on the two-dimensional plane. These coordinates are determined by the vertical scanning beam angle and horizontal scanning resolution of the laser radar. The vertical scanning beam angle determines the row position of the point cloud in the two-dimensional projection, while the horizontal scanning resolution determines the column position. Through this transformation, the laser point cloud in a complex three-dimensional space can be projected onto a two-dimensional plane to form a form that is convenient for subsequent processing and analysis. This two-dimensional projection not only retains the spatial relationship of the scene, but also greatly simplifies the data processing process, and is suitable for applications such as feature extraction and target recognition. This method can significantly improve the speed and accuracy of data analysis. By retaining important spatial information and simplifying it to a two-dimensional plane, a large amount of point cloud data can be processed more efficiently, ensuring that the three-dimensional structure of the object can still be clearly presented on the two-dimensional plane.

[0046] 3. After the intensity value of the laser point cloud is modified and cylindrically projected, the row coordinate of each point corresponds to the vertical scanning angle of the laser radar, and the column coordinate corresponds to the horizontal scanning angle of the laser radar. The intensity value of each point is then used to fill the corresponding two-dimensional grayscale image, thereby realizing the cylindrical projection of the three-dimensional laser point cloud. The laser point cloud in the three-dimensional space can be projected onto a two-dimensional plane to form a two-dimensional grayscale image containing the intensity information of the laser point cloud. By mapping the point cloud data to a two-dimensional plane, mature image processing algorithms and tools can be used for further analysis and processing, such as feature extraction, pattern recognition, and scene understanding.

[0047] 4. Feature point detection based on grayscale images (FAST algorithm): The ORB (Oriented FAST and Rotated BRIEF) algorithm uses the FAST (Features from Accelerated Segment Test) algorithm to detect corner points. For each pixel in the image, the FAST algorithm detects a circular area around it (this system uses 16 pixels) and compares the intensity values ​​of these pixels with the intensity value of the central pixel to determine whether the point is a corner point. Specifically, if there is a group of continuous pixels in a circular area, and the absolute value of the difference between its intensity value and the intensity value of the central pixel exceeds a set threshold t (this system is set to 15), then the point is considered to be a feature point. In order to calculate the direction of each feature point, the ORB algorithm estimates the direction by calculating the image gradient of the area around the feature point. This system uses the Sobel operator to calculate the image gradient. The Sobel operator is a commonly used edge detection operator that can effectively calculate the horizontal and vertical gradients of each pixel in the image. After determining the direction of the feature point, the ORB algorithm uses the BRIEF (Binary Robust Independent Elementary Features) algorithm to generate feature descriptors. The BRIEF algorithm generates a binary descriptor by comparing the pixel pairs around the feature point. The ORB algorithm achieves rotation invariance by aligning the BRIEF descriptor with the direction of the feature point.

[0048] 5. The inter-frame pose estimation method of point cloud data mainly includes three key steps: nearest neighbor search, point-to-point residual calculation, and rigid body transformation solution. First, for each point in the target frame, the system uses the KD tree data structure to efficiently find the nearest neighbor point in the historical frame as the corresponding point. Then, the difference between the two point clouds is quantified by calculating the point-to-point residual between the target frame and the historical frame. In order to minimize these residuals, the system uses the Levenberg-Marquardt (LM) algorithm in the nonlinear optimization method to iteratively solve the rigid body transformation between the two point clouds, that is, the rotation matrix and the translation vector. By minimizing the residuals of all corresponding point pairs, the LM method can accurately estimate the optimal rigid body transformation between the target frame and the historical frame, thereby achieving high-precision point cloud registration. In addition, the system also takes into account the influence of the dynamic environment, and further improves the robustness and accuracy of the registration by eliminating dynamic objects. This method is not only suitable for static scenes, but also can effectively handle the pose estimation problem in dynamic environments, ensuring the accuracy and stability of point cloud registration. By combining efficient nearest neighbor search, accurate residual calculation and robust optimization methods, our system can provide reliable inter-frame pose estimation in different application scenarios.

[0049] 6. The inter-frame pose transformation calculated by the ICP (Iterative Closest Point) algorithm can not only achieve accurate alignment of the laser point cloud of the current frame with the historical frame, but also update the point cloud data of the current frame to the global coordinate system, thereby obtaining the point cloud in the world coordinate system. This process ensures the continuity and consistency of the point cloud data in the global space. The updated point cloud data can not only capture the dynamic changes of the environment, but also update the three-dimensional map of the environment in real time. This real-time updated three-dimensional map provides operators with accurate environmental information, whether in a small indoor space or in an open outdoor environment, it can accurately reflect the actual position and posture of the object. By continuously updating the point cloud in the world coordinate system, the system can construct a high-precision three-dimensional map and improve the robustness and reliability of the overall system. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 The present invention is a flow chart of the three-dimensional laser synchronous mapping and positioning method based on rotation scanning. DETAILED DESCRIPTION

[0051] The present invention will be further described below in conjunction with the accompanying drawings.

[0052] The three-dimensional laser synchronous mapping and positioning method based on rotation scanning of the present invention increases the integrity and accuracy of information collection in a limited space.

[0053] like Figure 1 As shown, the main principle steps of the present invention are as follows:

[0054] S10. 3D laser radar acquires the original point cloud data in the well in real time;

[0055] S20. Correcting the intensity value and performing cylindrical projection to obtain a grayscale image;

[0056] S30. Extract feature points on the grayscale image using the ORB method;

[0057] S40. Calculate the point-to-point residual and use ICP to obtain the inter-frame pose;

[0058] S50. Publish the laser point cloud after the inter-frame pose transformation.

[0059] Step S20, the principle is as follows:

[0060] S201, the principle steps of strength value correction are as follows:

[0061] In the point cloud of the LiDAR, the intensity value of each point is affected by the distance and the angle of incidence. The distance is usually compensated inside the radar, while the angle of incidence effect needs to be corrected by calculating the local normal vector. Specifically, by calculating the normal vector n, the intensity attenuation caused by the angle of incidence can be corrected. Calculate the local normal vector of each laser point p:

[0062]

[0063] Where p1 and p2 are the neighboring points of p. The incident angle θ is calculated as:

[0064]

[0065] By calculating and correcting the incident angle of each laser point, an intensity value that can restore the original laser point is obtained.

[0066] S202, cylindrical projection principle steps are as follows:

[0067] To realize the cylindrical expansion projection of the laser point cloud, it is first necessary to calculate the row and column coordinates of each point in the laser point cloud on the two-dimensional plane. These coordinates are determined based on the vertical scanning beam angle and horizontal scanning resolution of the laser radar.

[0068] (1) For each laser point, it is first necessary to convert it into the horizontal angle and vertical angle of the spherical coordinate system:

[0069]

[0070] in, φ is the horizontal angle and vertical angle of the laser point; x, y, z are the three-dimensional coordinates of the laser point. By calculating the horizontal and vertical angles, each laser point can be arranged regularly according to the parameters of the laser radar to obtain the corresponding two-dimensional matrix.

[0071] (2) The row coordinate u of each point corresponds to the vertical scanning angle of the laser radar, while the column coordinate v corresponds to the horizontal scanning angle of the laser radar. Then, the intensity value of each point is used to fill the corresponding two-dimensional grayscale image, thereby realizing the cylindrical projection of the three-dimensional laser point cloud.

[0072] I(u,v)=I corrected

[0073] Among them, I corrected is the corrected intensity value, which is mapped to [0,255] to match the grayscale image pixel value range. This grayscale image can reflect the reflection intensity characteristics of the laser point cloud and is more suitable for image processing technology. In this way, the intensity information of the three-dimensional laser point cloud is reflected on the two-dimensional plane, realizing the cylindrical projection from three-dimensional to two-dimensional. This process effectively integrates spatial information and intensity information, providing basic data for subsequent feature extraction and environmental understanding.

[0074] Step S30: The principle of extracting feature points on the grayscale image using the ORB method is as follows:

[0075] (1) Feature point detection (FAST algorithm): This algorithm analyzes each pixel in the image, especially setting a circular area consisting of 16 pixels around the pixel. By comparing the intensity values ​​of these circumferential pixels with the intensity value of the central pixel, we can determine whether the pixel can be regarded as a feature point. In order to enhance the accuracy of detection, we set a threshold t, which is set to 15 in this system.

[0076] Specifically, if there is a group of continuous pixels in the circular area, and the absolute value of the difference between the intensity of these pixels and the intensity of the central pixel is greater than the set threshold t, then the central pixel will be marked as a feature point. This means that it may contain significant feature information that can be used for subsequent image analysis and feature matching. This step ensures that stable and recognizable feature points are extracted from the grayscale image, providing a reliable basis for subsequent posture estimation and matching.

[0077] (2) Calculation of feature point direction:

[0078] In order to calculate the direction of each feature point, the ORB algorithm estimates the direction by calculating the image gradient of the area around the feature point (this system uses the Sobel operator). The feature point direction is used for the rotation invariance of the feature descriptor.

[0079]

[0080] Among them, G y , G x Respectively represent the horizontal and vertical gradients of the area around the feature point.

[0081] 3. Feature descriptor generation (BRIEF algorithm):

[0082] The ORB algorithm uses the BRIEF algorithm in the process of generating feature descriptors. BRIEF (Binary Robust Independent Elementary Features) is a method for generating binary feature descriptors based on a simple and effective pixel contrast technique. Specifically, BRIEF constructs a descriptor by randomly selecting a pair of pixels around a feature point and comparing the grayscale values ​​of the pair of pixels. For each pair of sampling points, if the grayscale value of the first pixel is less than that of the second pixel, it is assigned a value of 1; otherwise, it is assigned a value of 0. Through a series of such pixel pair comparisons, a binary feature vector can be generated for each feature point. This descriptor vector is not only simple to calculate, but also has high matching efficiency, making it very suitable for real-time applications. This binary descriptor form of the BRIEF algorithm makes it perform well in both calculation and storage, improving the speed and robustness of the ORB algorithm in the feature matching process.

[0083]

[0084] Where p1 and p2 are randomly sampled pixels around the feature point; I(p1) and I(p2) are the grayscale values ​​of the corresponding pixels.

[0085] Step S40, calculating the point-to-point residual using ICP to calculate the inter-frame pose is as follows:

[0086] The ICP algorithm is a commonly used algorithm for aligning two point clouds. It minimizes the distance between corresponding points of the two point clouds through iterative optimization, thereby calculating the pose transformation between frames.

[0087] (1) Nearest neighbor search:

[0088] For each point in the target frame, the system needs to find a closest corresponding point in the historical frame point cloud. This is the key step for the ICP algorithm to align two point clouds. In order to improve the efficiency of the nearest neighbor search, the system uses data structures such as KD trees to quickly find and match each point pair. In this structure, all points are first organized into a multidimensional tree structure, so that the operation of finding the nearest neighbor in high-dimensional space can be performed more quickly.

[0089] (2) Calculate the point-to-point residual:

[0090] For each set of corresponding points (p i , p j ), calculate the point-to-point Euclidean distance as the residual:

[0091] redidual i =|p i -q i | 2

[0092] (3) Minimize the residual and solve the rigid body transformation:

[0093] By minimizing the residual of all corresponding point pairs, the rigid body transformation (rotation matrix R and translation vector t) between the two point clouds is solved:

[0094]

[0095] The optimal inter-frame pose estimation is obtained by solving the problem using a nonlinear optimization method (this system uses the LM method).

[0096] The principles of S50's laser point cloud publishing steps are as follows:

[0097] The inter-frame pose transformation calculated by the ICP (Iterative Closest Point) algorithm can update the current frame laser point cloud data obtained by the sensor to the global coordinate system. This process realizes the transition from the device's own local coordinate system to the broader world coordinate system, so that the 3D map of the entire environment can be continuously updated. All updated point cloud data not only more accurately describes the current environment layout, but also ensures the continuity and consistency of multi-frame data.

[0098] For each point p i , using the optimal estimate (R, t) obtained by ICP solution to transform:

[0099]

[0100] The updated point cloud data can update the 3D map of the environment in real time, providing operators with accurate environmental information.

Claims

1. A three-dimensional laser synchronous mapping and positioning method based on rotation scanning, characterized in that: The following steps are involved: S10. 3D laser radar acquires the original point cloud data in the well in real time; S20. Correcting the intensity value and performing cylindrical projection to obtain a grayscale image; S30. Extract feature points on the grayscale image using the ORB method; S40. Calculate the point-to-point residual and use ICP to obtain the inter-frame pose; S50. Publish the laser point cloud after the inter-frame pose transformation.

2. The method for three-dimensional laser synchronous mapping and positioning based on rotation scanning according to claim 1, characterized in that: Step S20 includes the following two steps: (1) The intensity value correction steps are as follows: Use local normal vector analysis to process the impact of the incident angle and calculate the local normal vector of each laser point p: Among them, p1 and p2 are the neighboring points of p respectively; The incident angle θ is calculated as: (2) The steps of cylindrical projection are as follows: (2.1) First, we need to calculate the row and column coordinates of each point in the laser point cloud on the two-dimensional plane; (2.1.1) For each laser point, we first need to convert it into the horizontal angle and vertical angle of the spherical coordinate system: in, φ are the horizontal and vertical angles of the laser point respectively; x, y, and z are the three-dimensional coordinates of the laser point; (2.1.2), the row coordinate u of each point corresponds to the vertical scanning angle of the laser radar, and the column coordinate v corresponds to the horizontal scanning angle of the laser radar; (2.2) Use the intensity value of each point to fill the corresponding two-dimensional grayscale image, thereby realizing the cylindrical projection of the three-dimensional laser point cloud: I(u,v)=I corrected Among them, I correcM is the corrected intensity value.

3. The method for three-dimensional laser synchronous mapping and positioning based on rotation scanning according to claim 1, characterized in that: The steps of extracting feature points on the grayscale image using the ORB method in step S30 are as follows: (1) Feature point detection: ORB uses the FAST algorithm to detect corner points. For each pixel in the image, the circular area around it is detected, and the intensity values ​​of these pixels are compared with the intensity value of the central pixel to determine whether it is a corner point. By setting a threshold t, if there is a group of continuous pixels in the circular area, and the absolute value of the difference between its intensity value and the intensity value of the central pixel exceeds the threshold t, then the point is considered a feature point. (2) Feature point direction calculation: In order to calculate the direction of each feature point, the ORB algorithm estimates the direction by calculating the image gradient of the area around the feature point; the feature point direction is used for the rotation invariance of the feature descriptor; Among them, G y , G x Respectively represent the horizontal and vertical gradients of the area around the feature point; (3) Feature descriptor generation: ORB uses the BRIEF algorithm to generate feature descriptors by comparing random pixel pairs around the feature points to generate a binary feature vector; Where p1 and p2 are randomly sampled pixels around the feature point; I(p1)I(p1) and I(p2) are the grayscale values ​​of the corresponding pixels.

4. The method for three-dimensional laser synchronous mapping and positioning based on rotation scanning according to claim 1, characterized in that: Step S40: Calculate point-to-point residuals and use ICP to calculate inter-frame poses. The steps are as follows: ICP algorithm is a common algorithm for aligning two point clouds. It minimizes the distance between corresponding points of two point clouds through iterative optimization, thereby calculating the inter-frame pose transformation; (1) Nearest neighbor search: For each point in the target frame, find the nearest point in the historical frame as the corresponding point. This system uses data structures such as KD trees for efficient nearest neighbor search; (2) Calculate the point-to-point residual: For each set of corresponding points (p i , p j ), and calculate the point-to-point Euclidean distance as the residual: residual i =|p i -q i | 2 (3) Minimize the residual and solve the rigid body transformation: By minimizing the residual of all corresponding point pairs, solve the rigid body transformation between the two point clouds (rotation matrix R and translation vector t): The solution is obtained by nonlinear optimization method (LM method is used in this system).

5. The method for three-dimensional laser synchronous mapping and positioning based on rotation scanning according to claim 1, characterized in that: Step S50: The steps of publishing the laser point cloud are as follows: the inter-frame pose transformation calculated by the ICP algorithm can update the laser point cloud of the current frame to the global coordinate system to obtain the point cloud in the world coordinate system; for each point p i , using the optimal estimate (R, t) obtained by ICP solution to transform: The updated point cloud data updates the 3D map of the environment in real time.

Images