Lidar Point Cloud Localization Method in Degraded Environments Based on Feature Point Enhancement

Abstract

The present invention discloses a lidar point cloud positioning method in a degraded environment based on feature point enhancement. The method comprises the following steps: adopting a tightly coupled method to fuse the lidar and the IMU sensor to compensate for the motion distortion of the lidar point cloud data; adaptively extracting the line and plane features of the tunnel point cloud and local features in the tunnel such as signboards and lighting systems. It is proposed to perform feature enhancement by predicting virtual points. Feature matching and registration are carried out by combining feature points and fusing the pose matrix of IUM pre-integration. Coordinate transformation is performed to convert the point cloud data from the absolute coordinate system of the lidar to the world coordinate system by introducing a reference point; a factor graph optimization algorithm is introduced at the backend to improve the mapping and positioning accuracy. The present invention fuses the lidar and the IMU sensor and adopts a method of adaptive feature extraction for feature matching, and can complete high-precision mapping and positioning in a degraded environment such as a long tunnel.

Description

Technical Field

[0001] The present invention relates to the technical field of mobile robot map construction methods, and particularly to a lidar point cloud positioning method in a degraded environment based on feature point enhancement. Background Art

[0002] With the rapid development of communication technology and robot technology, the SLAM technology based on lidar (LiDAR) plays an important role in solving problems of high-precision, real-time positioning, navigation, and map construction. However, in degraded scenarios, the laser SLAM technology faces many challenges. Environmental degradation refers to the gradual disappearance or instability of features in the environment, such as in aging, damaged, or adverse weather conditions, resulting in a decline in the quality of data obtained by the laser sensor. In this case, traditional laser SLAM methods may encounter problems of reduced positioning accuracy and inaccurate map construction. First of all, the laser SLAM method in a degraded environment needs to deal with the instability of sensor data.

[0003] For this reason, many researchers have tried to improve the stability of the system by introducing robust algorithms, such as by improving data preprocessing and noise filtering techniques to reduce the impact of sensor noise on map construction and positioning. Secondly, feature extraction and matching algorithms are also key research directions. In a degraded environment, environmental features may become sparse or blurred, so new feature extraction and matching algorithms need to be developed to improve the performance of the system in an environment lacking clear features. Some studies suggest combining visual sensors with lidar to enhance the environmental feature recognition ability.

[0004] In addition, map update and optimization methods also need to be improved in the case of environmental degradation. For example, the SLAM method based on graph optimization can adapt to environmental changes and dynamically update map information to reflect the actual state of the environment. New optimization algorithms such as robust graph optimization and adaptive algorithms have been proposed to cope with environmental uncertainties. Finally, fusing other sensor data is also an effective strategy. For example, combining IMU (Inertial Measurement Unit) and GPS data can improve the accuracy and stability of positioning, especially in the case of fewer environmental features. Generally speaking, in a degraded environment scenario, the research on laser SLAM technology mainly focuses on improving data robustness, optimizing feature extraction and matching, improving map update and optimization methods, and fusing other sensor data to cope with the challenges of degraded environment to SLAM performance.

[0005] The following are the main disadvantages in the prior art when performing laser SLAM mapping in a long-distance tunnel environment: 1) The situation of insufficient and missing features in a degraded environment; 2) The cumulative error situation caused by the lack of GNSS signals in a long tunnel environment, which makes it impossible to perform initial pose estimation on the pose of the lidar, and the point cloud distortion caused by vehicle movement. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a lidar point cloud positioning method based on feature point enhancement in a degraded environment that can complete high-precision mapping and positioning in a long tunnel environment.

[0007] To solve the above technical problem, the technical solution adopted by the present invention is: a lidar point cloud positioning method based on feature point enhancement in a degraded environment, including the following steps:

[0008] S1, Data acquisition and data preprocessing: Obtain lidar point cloud data and the acceleration, angular velocity and direction information of the IMU measurement device, complete data synchronization, compensate for the motion distortion of the lidar point cloud data, and provide an initial pose estimate for point cloud feature matching;

[0009] S2, Adaptive point cloud feature extraction in a degraded environment: Adaptive selection of different types of feature extraction, relying on structural features when there are few significant features in the tunnel; while when there are significant features locally, emphasizing local features;

[0010] S3, Constructing pseudo point clouds to enhance point cloud features: In view of the insufficient point cloud features in the tunnel environment, perform edge point prediction and construct virtual points to enhance point cloud features;

[0011] S4, Fusing the IMU pre-integrated pose matrix for feature matching: Select key frames to obtain the pose matrix through IMU pre-integration to provide an initial pose estimate for point cloud registration;

[0012] S5, Introducing a factor graph optimization algorithm at the backend: The odometry factor between frames, the global constraint factor between frames and the global map, and the IMU pre-integration factor are used to optimize the point cloud map.

[0013] The beneficial effects produced by adopting the above technical solution are as follows: The method described in this application uses adaptive feature extraction for matching, extracts line and surface features in the tunnel environment and combines a small number of local features in the tunnel such as signboards and lighting systems, which can effectively avoid insufficient features and missing features in the degraded environment; the method proposes a laser SLAM method for lidar-IMU sensors and introduces factor graph constraints to improve mapping accuracy, which can effectively avoid the situation of cumulative errors caused by the lack of GNSS signals in the long tunnel environment and the inability to perform initial pose estimation on the pose of the lidar, and the problem of point cloud distortion caused by vehicle movement, and can complete high-precision mapping and positioning in the long tunnel environment. Brief Description of the Drawings

[0014] The present invention will be further described in detail below with reference to the drawings and specific embodiments.

[0015] Figure 1It is a flowchart of the method implemented in the present invention;

[0016] Figure 2 It is a schematic diagram of line-plane features in the method implemented in the present invention;

[0017] Figure 3 It is a schematic diagram of pseudo-edge point prediction in the method implemented in the present invention;

[0018] Figure 4 It is a schematic diagram of the factor graph constraint structure in the method implemented in the present invention. Detailed implementation manners

[0019] Next, in combination with the accompanying drawings in the embodiments of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0020] Many specific details are set forth in the following description in order to provide a thorough understanding of the present invention, but the present invention may be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the connotation of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed below.

[0021] As Figure 1 shown, the embodiments of the present invention disclose a lidar point cloud positioning method in a degraded environment based on feature point enhancement, including the following steps:

[0022] S1, Data acquisition and data preprocessing: Obtain lidar point cloud data and the acceleration, angular velocity, and direction information of the IMU measurement device, complete data synchronization, compensate for the motion distortion of the lidar point cloud data, and provide an initial pose estimate for the point cloud.

[0023] S2, Adaptive point cloud feature extraction in a degraded environment: Adaptively select different types of feature extraction. When there are fewer significant features in the tunnel, more rely on structural features (lines, planes); while when there are significant features locally, focus on local features (such as signs, etc.).

[0024] S3, Construct pseudo-point cloud to enhance point cloud features: For the situation of insufficient point cloud features in the tunnel environment, perform edge point prediction and construct pseudo-point cloud to enhance point cloud features.

[0025] S4, Feature matching by fusing the IMU pre-integrated pose matrix: Select key frames to obtain the pose matrix through IMU pre-integration to provide an initial pose estimate for point cloud registration.

[0026] S5, Introduce factor graph constraints at the backend: odometry factors between frames, global constraint factors between frames and the global map, IMU pre-integration factors, and optimize the point cloud map.

[0027] Furthermore, the method for compensating for point cloud motion distortion in S1 is as follows:

[0028] Based on the acceleration and angular velocity measured by the IMU sensor, the motion of the device can be integrated to estimate the pose change of the device at each moment:

[0029]

[0030]

[0031] where R(t) is the rotation matrix of the device at time t, P(t) is the displacement at time t, w(t) is the angular velocity, and v(t) is the velocity. At time, the lidar starts scanning. is the time of the point cloud data. At this time, the pose of the device has changed, and it is necessary to transform the point cloud coordinate system at time to the reference coordinate system at time, that is:

[0032]

[0033] Furthermore, the specific process of adaptively extracting line, plane, and local features of the point cloud in S2 is as follows:

[0034] S2-1, Perform point cloud sparsity and curvature analysis, extract line features and plane features from the parts of the point cloud with low sparsity and curvature, and perform fusion extraction of local features and line-plane features for the regions with high curvature and dense features locally.

[0035] S2-2, Calculate the curvature of the point cloud data. For each point P, use KNN to find the local neighborhood of each point to construct a neighborhood point set , calculate the covariance matrix C of the point set , where is the centroid of the neighborhood point set, is the number of the neighborhood point set.

[0036]

[0037] Solve the eigenvalues of the covariance matrix C to obtain the eigenvalues , , , if is the minimum value representing the minimum direction of local surface change, then the curvature of point P is

[0038]

[0039] If c is relatively large, it is considered to have the characteristics of a spatial curve. Among them, in the schematic diagram of the line-plane characteristics, the spatial point i is the currently selected current point, and neighborhood points A, B, and C are selected near the spatial point i. The plane points are discriminated by calculating the normal vector consistency of this area. The normal vector usually corresponds to the eigenvector of the covariance matrix corresponding to the smallest eigenvalue. Let the eigenvector be the eigenvector of the smallest eigenvalue, and let the eigenvector be .

[0040] For the normal vectors calculated for each neighborhood point, compare the directions of the normal vectors. If the angle between the normal vectors is less than a certain set threshold, these points are considered to belong to the same plane. Let the angle between the normal vectors be , and the angle calculation formula is:

[0041]

[0042] where, and are two normal vectors respectively, is the dot product operation, and are the magnitudes of the normal vectors. If the angle is less than the set threshold, it is considered that these two normal vectors are consistent, indicating that the area where the points are located is a plane area.

[0043] In S2-3, adaptive point cloud feature extraction is performed. The point cloud is jointly analyzed for sparsity and curvature and divided into four regions: sparse-low curvature, sparse-high curvature, dense-low curvature, and dense-high curvature. Then, the dynamic window size is adaptively adjusted. For the sparse-low curvature, a larger window is used to focus on extracting line features and plane features. For the sparse-high curvature, a larger window is used to fuse and extract line features, plane features, and local features of high curvature. For the dense-low curvature, a smaller window is used to mainly extract plane features. For the dense-high curvature, line features, plane features, and local features are fused and extracted. The schematic diagram of the line-plane characteristics is as shown in Figure 2 .

[0044] Furthermore, the specific process of predicting the edge points of the point cloud in S3 is as follows:

[0045] In S3-1, according to the above curvature calculation, the point cloud data is divided into plane features and edge features. For the plane feature points, according to the normal vector of the plane points and the defined direction of the gravity vector, usually set the gravity vector . The plane points are further divided into ground points and non-ground points. For each plane point, calculate the angle between the normal vector and the gravity vector. If Then this planar point is considered as a ground point, and other planar points are classified as tunnel wall points. Calculate through the dot product formula

[0046]

[0047] S3-2. For the two types of ground points divided, subspace division is performed on the X-axis, and the subspace interval is , fix the X coordinate within each subspace and implement line fitting through the RANSAC algorithm. Generate pseudo point clouds by the intersection of the lines fitted by the two types of ground points, and merge the predicted pseudo point clouds with the original point clouds. The schematic diagram of pseudo-edge point prediction is as Figure 3 shown.

[0048] Furthermore, the key frame extraction principle in S4 is as follows: Select key frames according to the time interval. When the set time threshold is exceeded, the current frame is determined as a key frame. Select key frames according to the feature points of the point cloud. When the number of feature points in the environment changes significantly, determine the current frame as a key frame.

[0049] Furthermore, the principle of IMU pre-integration providing initial pose estimation for point cloud matching in S4 is as follows: Calculate the relative pose from the current frame time to the next frame time. Obtain the pose matrix by pre-integrating the attitude, position, and velocity. Among them, the attitude pre-integration uses the angular velocity data obtained by the IMU sensor for integration to obtain the rotation change between key frames:

[0050]

[0051] Among them is the angular velocity data measured by the IMU, and R(t) is the rotation matrix obtained by IMU pre-integration. The velocity pre-integration calculates the velocity change by pre-integrating the acceleration:

[0052]

[0053] The pose pre-integration uses the acceleration and attitude information to perform double integration on the velocity to obtain the displacement change between key frames:

[0054]

[0055] Among them, v(t) is the device velocity data measured by the IMU, and a(t) is the acceleration data. Use the above rotation change, displacement change, and velocity change to obtain the pose change matrix:

[0056]

[0057] Further, the core utilization point of feature matching in S4 constructs an optimization problem by using the distance errors from point to line and point to plane, and optimizes the relative pose between two frames by minimizing the distance error between the current frame and the reference frame point cloud. The error function is constructed as follows:

[0058]

[0059] where represents the line segment direction vector, A is the coordinate of a point on the line segment, is the plane normal vector, and Q is the coordinate of a point on the plane. The above error function is used to optimize the minimum error through the Levenberg-Marquardt algorithm.

[0060] Further, in S5, the factor graph optimization algorithm is introduced at the backend. The odometry pose output by the front-end odometry is used as the odometry factor, and the IMU pre-integration factor is obtained by pre-integrating the motion state measured by the IMU, so as to further improve the mapping and positioning accuracy.

[0061] Factor graph optimization is introduced at the backend to improve the mapping accuracy. The schematic diagram of the factor graph structure is as shown in Figure 4 The odometry pose output by the front-end odometry is used as the odometry factor, and the IMU pre-integration factor is obtained by pre-integrating the motion state measured by the IMU.

[0062] Further, the pose output by the front-end odometry is the relative pose between consecutive point cloud frames and the global pose of the key frame matching the global map as the odometry factor.

[0063]

[0064] where is the feature point in the i-th frame, is the feature point in the (i + 1)-th frame, and T is the relative pose between two frames.

[0065] The absolute pose factor of the key frame matching the global map is obtained by matching the key frame with the global map point cloud, and the feature point method is also selected as the matching method. The global matching is based on the obtained relative change. All feature points in the local point cloud map centered on the current key frame are extracted as matching points in the global map. Assume is the key frame point cloud, is the local point cloud selected in the area centered on the current key frame. The global pose factor is constructed in the same way as the relative pose factor.

[0066] Furthermore, the principle of the IMU pre-integration factor in S5 is as follows: The IMU measurement model is a dynamic system used to describe the motion process of the device. However, to combine the IMU output with the motion model, it is necessary to discretize the continuous-time motion model of the IMU, that is, to perform discrete integration on the motion model in continuous time. The IMU motion mathematical model is:

[0067]

[0068]

[0069] where is the position of the device, is the velocity, is the acceleration, is the attitude. It is assumed that the pose change of the IMU pre-integration within the time period , is . The difference between the true pose and the IMU pre-integration estimate is expressed as a residual function:

[0070] .

[0071] The method described in this application can effectively avoid insufficient features and missing features in a degraded environment; it can effectively avoid the situation of GNSS signal loss in a long tunnel environment, the cumulative error caused by the inability to perform initial pose estimation on the pose of the lidar, and the problem of point cloud distortion caused by vehicle movement, and can complete high-precision mapping and positioning in a long tunnel environment.

Claims

1. A laser radar point cloud positioning method in a degraded environment based on feature point enhancement, characterized in that The steps include: S1, data acquisition and data preprocessing: LiDAR point cloud data acquisition and IMU measurement equipment acceleration, angular velocity and direction information, complete data synchronization, compensate for the motion distortion of LiDAR point cloud data, and provide initial pose estimation for point cloud feature matching; S2, Adaptive point cloud feature extraction in degraded environment: Comprehensively consider local geometric information and global distribution characteristics, dynamically adjust the threshold and weight distribution strategy of feature point extraction, rely on structural features when there are fewer significant features in the tunnel; and focus on local features when there are significant features locally; S2-1, perform point cloud sparsity and curvature analysis, extract line features and surface features from sparse and low-curvature parts of the point cloud, and perform fusion extraction of local features and line and surface features from areas with high curvature and dense features; S2-2, calculate the curvature of the point cloud data. For each point P, use the KNN algorithm to find the local neighborhood of each point and construct the neighborhood point set N i Calculate the neighborhood point set N i The covariance matrix C of is the centroid of the neighborhood point set, |N i | is the number of neighborhood point sets; Solve the eigenvalue of the covariance matrix C to obtain the eigenvalues ​​λ1, λ2, λ3. If λ3 is the minimum value, it means the minimum direction of local surface change. Then the curvature of point P is: S2-3, perform adaptive point cloud feature extraction, conduct a joint analysis of sparsity and curvature on the point cloud, and divide it into four regions: sparse-low curvature, sparse-high curvature, dense-low curvature, and dense-high curvature; Then, the window size is adaptively adjusted dynamically. For sparse and low curvature, a larger window is used to extract line features and surface features. For sparse and high curvature, a larger window is used to fuse and extract line features, surface features and local features with high curvature. For dense and low curvature, a smaller window is used to extract surface features. For dense and high curvature, line features, surface features and local features are fused and extracted. S3, pseudo point cloud enhanced point cloud features: in view of the insufficient point cloud features in the tunnel environment, point cloud edge point prediction is performed to construct pseudo point cloud enhanced point cloud features; S4, fusion of IMU pre-integrated pose matrix for feature matching: select key frames and obtain pose matrix through IMU pre-integration to provide initial pose estimation for point cloud registration; S5, the backend introduces a factor graph optimization algorithm: the odometer factor between frames, the global constraint factor between frames and the global map, and the IMU pre-integration factor to optimize the point cloud map.

2. The laser radar point cloud positioning method in a degraded environment based on feature point enhancement as claimed in claim 1, characterized in that The method for compensating the motion distortion of the laser radar point cloud data in S1 is: The acceleration and angular velocity of the device are measured by the IMU sensor, the movement of the device is integrated, and the position change of the device at each moment is estimated: Where R(t) is the rotation matrix of the device at time t, P(t) is the displacement at time t, w(t) is the angular velocity, and v(t) is the velocity. At time t0, the laser radar starts scanning, and p i is time t i Point cloud data, the device’s posture changes at this time, and t i The point cloud coordinate system at time t is converted to the reference coordinate system at time t0:

3. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 1 is characterized in that: The specific method for predicting edge points in the point cloud in S3 is: S3-1, according to the curvature calculation result, the point cloud data is divided into plane features and edge features. For the plane feature points, the plane points are subdivided into ground points and non-ground points according to the normal vector of the plane point and the direction of the gravity vector; S3-2, for the two types of plane points divided, subspace division is performed on the X-axis, and the subspace interval is δ i ,In each subspace, the X coordinate is fixed and the straight line fitting is realized by the RANSAC algorithm. The pseudo edge points are generated by the intersection of the straight lines fitted by the two types of ground points, and the predicted pseudo point cloud is merged with the original point cloud.

4. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 1 is characterized in that: The method of obtaining a pose matrix by IMU pre-integration in S4 to provide an initial pose estimation for point cloud registration includes the following steps: Calculate the relative pose from the current frame to the next frame; obtain the pose matrix by pre-integrating the pose, position, and velocity; The attitude pre-integration integrates the angular velocity data obtained by the IMU sensor to obtain the rotation change between key frames: Among them, ω(t) is the angular velocity data measured by the IMU, and R(t) is the rotation matrix obtained by IMU pre-integration; velocity pre-integration calculates the velocity change by pre-integrating the acceleration: Pose pre-integration uses acceleration and attitude information to integrate the velocity twice to obtain the displacement change between key frames: Among them, v(t) is the device velocity data measured by IMU, and a(t) is the acceleration data. The above rotation change, displacement change, and velocity change are used to obtain the posture change matrix:

5. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 1 is characterized in that: The feature matching in S4 is optimized by using the distance error between point to line and point to surface. The relative position between the two frames is optimized by minimizing the distance error between the current frame and the reference frame point cloud, and the error function is constructed: Among them, P represents the coordinates of the points in the point set, represents the direction vector of the line segment, A is the coordinate of the point on the line segment, is the plane normal vector, Q is the coordinate of the point on the plane, and the above error function is used to optimize the minimum error through the Levenberg-Marquardt algorithm.

6. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 1 is characterized in that: In S5: The backend integrates the factor graph optimization algorithm, and uses the odometer pose output by the front-end odometer as the odometer factor, including the relative pose constraints between consecutive frames and the global constraints between the keyframes and the global point cloud. The motion state measured by the IMU is pre-integrated to obtain the IMU pre-integration factor.

7. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 6, characterized in that: The pose output by the front-end odometer is the relative pose between consecutive point cloud frames and the global pose matched between the keyframe and the global map as the odometer factor: in is the feature point in the i-th frame, is the feature point in the i+1th frame, and T is the relative pose between the two frames; The global constraint factor for matching the keyframe with the global map is to match the keyframe with the global map point cloud. The global matching is based on the relative change obtained. All feature points in the local point cloud centered on the current keyframe are extracted from the global map as matching points.

8. The laser radar point cloud positioning method in a degraded environment based on feature point enhancement according to claim 6 is characterized in that: The IMU pre-integration factor in S5 is implemented by the following method: The IMU motion mathematical model is: where p i is the location of the device, v i is the speed, a i is the acceleration, q i for posture; Set in the time period [t i ,t i+1 ] The IMU pre-integrated posture change is ΔT IMU , the difference between the true pose and the IMU pre-integrated estimate is expressed as a residual function: γ IMU (x i ,x i+1 )=T i+1 -T i ·ΔT IMU 。

Images