Laser radar point cloud positioning method in degraded environment based on feature point enhancement

Abstract

The invention discloses a laser radar point cloud positioning method in a degraded environment based on feature point enhancement, and the method comprises the following steps: fusing a laser radar and an IMU sensor through employing a tight coupling method, and compensating the motion distortion of laser radar point cloud data; line and plane features of tunnel point clouds and local features in tunnels, such as signs and lighting systems, are extracted in a self-adaptive manner. Feature enhancement is carried out by predicting a virtual point. And carrying out feature matching registration by combining the feature points and fusing the pose matrix pre-integrated by the IUM. Coordinate conversion is carried out to convert the point cloud data from an absolute coordinate system of the laser radar to a world coordinate system by introducing a reference point; and a factor graph optimization algorithm is introduced at the rear end to improve the mapping and positioning precision. According to the method, the laser radar and the IMU sensor are fused, and adaptive feature extraction is adopted for feature matching, so that high-precision mapping and positioning can be completed in a long tunnel degradation environment.

Description

Technical Field

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

[0002] With the rapid development of communication technology and robotics technology, SLAM technology based on LiDAR plays an important role in solving high-precision, real-time positioning, navigation and map construction problems. However, laser SLAM technology faces many challenges in degraded scenarios. Environmental degradation refers to the gradual disappearance or instability of features in the environment, such as aging, damage or bad weather conditions, which leads to a decrease in the quality of data acquired by laser sensors. In this case, traditional laser SLAM methods may encounter problems such as reduced positioning accuracy and inaccurate map construction. First of all, laser SLAM methods in degraded environments need to deal with the instability of sensor data.

[0003] To this end, many researchers have tried to improve the stability of the system by introducing robust algorithms, such as reducing the impact of sensor noise on map construction and positioning by improving data preprocessing and noise filtering techniques. Secondly, feature extraction and matching algorithms are also key research directions. In degraded environments, 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 environments lacking clear features. Some studies suggest combining visual sensors with lidar to enhance the ability to recognize environmental features.

[0004] In addition, map updating and optimization methods also need to be improved in the case of environmental degradation. For example, SLAM methods 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 positioning accuracy and stability, especially when there are fewer environmental features. In general, in scenarios with environmental degradation, research on laser SLAM technology mainly focuses on improving data robustness, optimizing feature extraction and matching, improving map updating and optimization methods, and fusing other sensor data to cope with the challenges of degraded environments to SLAM performance.

[0005] The existing technology has the following main shortcomings when performing laser SLAM mapping in long-distance tunnel environments: 1) Insufficient and missing features in degraded environments; 2) The lack of GNSS signals in long tunnel environments makes it impossible to perform initial pose estimation of the lidar, resulting in accumulated errors and point cloud distortion caused by vehicle motion. Summary of the invention

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

[0007] In order to solve the above technical problems, the technical solution adopted by the present invention is: a laser radar point cloud positioning method in a degraded environment based on feature point enhancement, comprising the following steps: 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 environments: Adaptively select different types of feature extraction. When there are fewer significant features in the tunnel, it relies on structural features; when there are significant features locally, it focuses on local features. S3, construct pseudo point cloud to enhance point cloud features: in view of the insufficient point cloud features in the tunnel environment, edge point prediction is performed to construct virtual point to enhance 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.

[0008] The beneficial effects of adopting the above technical scheme are: the method described in the present application adopts 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 signs, lighting systems, etc., which can effectively avoid insufficient features and feature loss in the degraded environment; the method proposes a laser SLAM method of a lidar-IMU sensor, and introduces factor graph constraints to improve mapping accuracy, which can effectively avoid the loss of GNSS signals in long tunnel environments, the cumulative error caused by the inability to perform initial pose estimation of the lidar pose, and the point cloud distortion caused by vehicle movement, and can complete high-precision mapping and positioning in long tunnel environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0010] Figure 1 A flow chart for implementing the method of the present invention; Figure 2 A schematic diagram of line and surface features in the method for implementing the present invention; Figure 3 A schematic diagram of pseudo edge point prediction in the method for implementing the present invention; Figure 4 The figure is a schematic diagram of the factor graph constraint structure in the method implemented in the present invention. DETAILED DESCRIPTION

[0011] The following is a clear and complete description of the technical solutions in the embodiments of the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0012] In the following description, many specific details are set forth to facilitate a full understanding of the present invention, but the present invention may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0013] like Figure 1 As shown, an embodiment of the present invention discloses a laser radar point cloud positioning method in a degraded environment based on feature point enhancement, comprising the following steps: S1, data acquisition and data preprocessing: The LiDAR point cloud data is acquired from the acceleration, angular velocity and direction information of the IMU measurement device, data synchronization is completed, motion distortion of the LiDAR point cloud data is compensated, and an initial pose estimation is provided for the point cloud.

[0014] S2, Adaptive point cloud feature extraction in degraded environment: Adaptively select different types of feature extraction. When there are fewer significant features in the tunnel, it relies more on structural features (lines, surfaces); when there are significant features locally, it focuses on local features (such as signboards, etc.).

[0015] S3, construct pseudo point cloud to enhance point cloud features: In view of the insufficient point cloud features in the tunnel environment, edge point prediction is performed to construct pseudo point cloud to enhance point cloud features.

[0016] S4, integrate 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.

[0017] S5, the backend introduces factor graph constraints: odometer factors between frames, global constraint factors between frames and global maps, and IMU pre-integration factors to optimize the point cloud map.

[0018] Furthermore, the method for compensating point cloud motion distortion in S1 is: The acceleration and angular velocity measured by the IMU sensor can be used to integrate the movement of the device and estimate the position change of the device at each moment: 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 the moment the laser radar starts scanning, For the moment Point cloud data, the device's posture changes at this time, and it is necessary to The point cloud coordinate system at the moment is converted to The reference coordinate system at the moment is: Furthermore, the specific process of adaptively extracting line and surface features and local features of the point cloud in S2 is as follows: S2-1, perform point cloud sparsity and curvature analysis, extract line features and surface features from sparse point cloud and low curvature parts, and perform fusion extraction of local features and line and surface features from areas with high curvature and dense features.

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

[0020] Solve the eigenvalue of the covariance matrix C and get the eigenvalue , , ,like The minimum value represents the minimum direction of local surface change, so the curvature of point P is If c is large, it is considered to have spatial curve features, where the spatial point i in the line-surface feature diagram is the current point currently selected, and neighboring points A, B, and C are selected near the spatial point i, and the plane point is determined by calculating the consistency of the normal vector in this area. The normal vector usually corresponds to the covariance matrix The eigenvector of the minimum eigenvalue, let the eigenvector .

[0021] For each normal vector calculated for a neighborhood point, compare the direction of the normal vector. If the angle between the normal vectors is less than a certain threshold, then these points are considered to belong to the same plane. The angle between the normal vectors is set to , the angle calculation formula is:

[0022] in, and are two normal vectors, is the dot product operation, and is the magnitude of the normal vector. If the angle If the value is smaller than the set threshold, the two normal vectors are considered to be consistent, indicating that the area where the point is located is a plane area.

[0023] 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 areas: sparse-low curvature, sparse-high curvature, dense-low curvature, and dense-high curvature. Then, the dynamic window size is adaptively adjusted. For sparse-low curvature, a larger window is used to focus on extracting line features and surface features. For sparse-high curvature, a larger window is used to fuse the extracted line features, surface features, and local features of high curvature. For dense-low curvature, a smaller window is used to mainly extract surface features. For dense-high curvature, line features, surface features, and local features are fused and extracted. The schematic diagram of line and surface features is shown as follows Figure 2 shown.

[0024] Furthermore, the specific process of predicting edge points in the point cloud in S3 is as follows: 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, the direction of the gravity vector is defined according to the normal vector and the direction of the gravity vector. For the plane feature points, the direction of the gravity vector is defined according to the normal vector and the direction of the gravity vector. Usually, the gravity vector is set . Subdivide the plane points into ground points and non-ground points. For each plane point, calculate the angle between the normal vector and the gravity vector ,like The plane point is considered to be a ground point, and the other plane points are classified as tunnel wall points.

[0025] S3-2, for the two types of ground points, subspace division is performed on the X-axis, and the subspace interval is In each subspace, the X coordinate is fixed and the straight line fitting is realized by the RANSAC algorithm. The pseudo point cloud is 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. The schematic diagram of pseudo edge point prediction is shown in the figure. Figure 3 shown.

[0026] Furthermore, the key frame extraction principle in S4 is: select key frames according to the time interval, and when it exceeds the set time threshold, determine the current frame as a key frame; select key frames according to point cloud feature points, and when the number of feature points in the environment changes significantly, determine the current frame as a key frame.

[0027] Furthermore, the principle of IMU pre-integration in S4 to provide initial pose estimation for point cloud matching is: calculate the relative pose from the current frame time to the next frame time. The pose matrix is ​​obtained by pre-integrating the pose, position, and velocity. The pose pre-integration integrates the angular velocity data obtained by the IMU sensor to obtain the rotation change between key frames: in 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: Where v(t) is the device velocity data measured by the 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: Furthermore, the core of feature matching in S4 uses the distance error between point to line and point to surface to construct an optimization problem, and optimizes the relative pose between the two frames by minimizing the distance error between the current frame and the reference frame point cloud. Construct the error function: in 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. The above error function is used to optimize the minimum error through the Levenberg-Marquardt algorithm.

[0028] Furthermore, the back-end of S5 references the factor graph optimization algorithm, uses the odometer posture output by the front-end odometer as the odometer factor, and pre-integrates the motion state measured by the IMU to obtain the IMU pre-integration factor, so as to further improve the mapping and positioning accuracy.

[0029] The backend introduces factor graph optimization to improve the accuracy of graph construction. The schematic diagram of the factor graph structure is as follows: Figure 4As shown, the odometer posture output by the front-end odometer is used as the odometer factor, and the motion state measured by the IMU is pre-integrated to obtain the IMU pre-integration factor.

[0030] Furthermore, the pose output by the front-end odometry is the relative pose between consecutive point cloud frames and the global pose matched between the keyframes and the global map as the odometry factor.

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

[0032] The absolute pose factor of the keyframe matching with the global map is to match the keyframe with the global map point cloud. The matching method also uses the feature point method. 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. Assume is the keyframe point cloud, is the local point cloud of the selected area centered on the current keyframe. The global pose factor is constructed using the same method as the relative pose factor.

[0033] Furthermore, the principle of the IMU pre-integration factor in S5 is: the IMU measurement model is a dynamic system used to describe the motion process of the device, but in order to combine the IMU output with the motion model, the continuous time motion model of the IMU needs to be discretized, that is, the motion model under continuous time needs to be discretely integrated. The IMU motion mathematical model is: in is the location of the device, It's speed. is the acceleration, For posture. Set in the time period [ , ]The pose change of IMU pre-integration is , the difference between the true pose and the IMU pre-integrated estimate is expressed as a residual function: .

[0034] The method described in the present application can effectively avoid insufficient features and feature loss in degraded environments; it can effectively avoid the loss of GNSS signals in long tunnel environments, the cumulative error caused by the inability to perform initial pose estimation of the lidar, and the point cloud distortion problem caused by vehicle motion, and can complete high-precision mapping and positioning in long tunnel environments.

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; S3, construct pseudo point cloud to enhance point cloud features: in view of the insufficient point cloud features in the tunnel environment, the point cloud edge points are predicted and pseudo point cloud to enhance point cloud features is constructed; 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 the moment the laser radar starts scanning, For the moment Point cloud data, the device's posture changes at this time, and it is necessary to The point cloud coordinate system at time is converted to The reference coordinate system at the moment is: 。 3. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 1, characterized in that: The adaptive point cloud feature extraction in S2 includes the following steps: S2-1, perform point cloud sparsity and curvature analysis, extract line features and surface features from sparse point cloud and low curvature parts, 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 a neighborhood point set. Calculate neighborhood point set The covariance matrix C of is: ； in, is the centroid of the neighborhood point set, is the number of neighborhood point sets; Solve the eigenvalue of the covariance matrix C and get the eigenvalue , , ,like The minimum value represents the minimum direction of local surface change, and the curvature of point P is: ； S2-3, perform adaptive point cloud feature extraction, conduct joint analysis of sparsity and curvature on the point cloud, and divide it into four areas: sparse-low curvature, sparse-high curvature, dense-low curvature, and dense-high curvature; then adaptively and dynamically adjust the window size, use a larger window to extract line features and surface features for sparse-low curvature, use a larger window for sparse-high curvature, and fuse the extracted line features, surface features, and local features of high curvature; use a smaller window to extract surface features for dense-low curvature; and fuse the extracted line features, surface features, and local features for dense-high curvature.

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 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 ,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.

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 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: ； in, 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: 。 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: 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: ； in, 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.

7. 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.

8. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 7, 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.

9. The laser radar point cloud positioning method based on feature point enhancement in a degraded environment according to claim 7, characterized in that: The IMU pre-integration factor in S5 is implemented by the following method: The IMU motion mathematical model is: ； ； in is the location of the device, It's speed. is the acceleration, for posture; Set in time period [ , ]The pose change of IMU pre-integration is , the difference between the true pose and the IMU pre-integrated estimate is expressed as a residual function: 。

Images