Intensity-enhanced three-dimensional laser mapping method

Abstract

The invention discloses an intensity enhanced three-dimensional laser mapping method. The method comprises the following steps: S10, acquiring three-dimensional laser radar point cloud data, IMU measurement data and motor encoder data in real time; s20, removing rotation distortion and motion distortion of the motor; s30, constructing and managing an intensity enhanced voxel map; s40, calculating a point-surface geometric sum intensity residual error, and carrying out inter-frame pose estimation; and S50, releasing the laser point cloud data under the global coordinate system. In conclusion, the defects in the prior art are overcome, the scanning efficiency is effectively improved through the rotating motor, and the high social use value and application prospects are achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of laser mapping, and particularly to an intensity-enhanced three-dimensional laser mapping method. Background Art

[0002] Existing handheld three-dimensional mapping devices are limited by the cost and weight of lidar, and usually only equipped with a single lidar. Due to the limited viewing angle of lidar, during the mapping process, operators need to frequently move the device to obtain complete scene data, which not only increases the operation complexity but also seriously affects the mapping efficiency. In addition, existing Simultaneous Localization and Mapping (SLAM) algorithms mainly rely on the spatial geometric information of point clouds for mapping. In an environment with rich geometric structures, this method can work effectively, but in an environment with sparse geometric features or many repetitive structures (such as tunnels, underground spaces, etc.), point cloud registration is prone to failure, resulting in incomplete mapping results or serious cumulative errors, and the system robustness is poor.

[0003] In view of the above problems, the present invention proposes an efficient intensity-enhanced three-dimensional laser mapping system. By integrating the intensity information and spatial geometric information of lidar, combined with improved data processing and mapping algorithms, this system not only improves the integrity of data acquisition and mapping efficiency but also significantly enhances the robustness and stability of the system in environments with sparse or complex geometric features. Summary of the Invention

[0004] To solve the problems mentioned in the above background art, the present invention provides an intensity-enhanced three-dimensional laser mapping method.

[0005] To achieve the above object, the present invention adopts the following technical solutions: An intensity-enhanced three-dimensional laser mapping method, comprising the following steps: S10. Real-time obtain three-dimensional lidar point cloud data, IMU measurement data, and motor encoder data; S20. Remove motor rotation distortion and motion distortion; S30. Construct and manage an intensity-enhanced voxel map; S40. Calculate point-plane geometry and intensity residuals, and perform inter-frame pose estimation; S50. Publish laser point cloud data in the global coordinate system.

[0006] Preferably, the step S20 specifically includes: 1) Remove motor rotation distortion: It is necessary to use the motor encoder data to perform time synchronization interpolation and coordinate transformation on the point cloud, and rotate all the point cloud data to the coordinate system at the start time of the point cloud; 2) Remove motion distortion: Predict the motion trajectory of the carrier during scanning based on the IMU measurement data, and use the pose difference to perform reverse compensation on the point cloud to eliminate motion distortion.

[0007] Preferably, the removal of motor rotation distortion specifically includes: Let the th frame of point cloud collected by the lidar be , which is expressed as the th point in the point cloud, with a timestamp of . To obtain the encoder data corresponding to the sampling moment of point , linear interpolation is used for calculation: ; where, through timestamp indexing, the two encoder timestamps nearest to and are found in the encoder timestamp sequence, and corresponding encoder data are and , where ; Let the encoder value corresponding to the starting point of the point cloud be , and the range of the encoder is . Map the encoder data to the rotation radians, and calculate the rotation angle of point relative to the starting moment: ; Considering that the laser rotates along the axis, the corresponding rotation matrix can be calculated: ; To rotate the point cloud to the starting coordinate system, reverse rotation of the point cloud is required. The specific operation is: ; where, is the point after distortion compensation.

[0008] Preferably, the removal of motion distortion specifically includes: The IMU measurement data has acceleration and angular velocity for integrating and predicting the pose of the carrier during scanning. The rotation at each moment is , the velocity is and the position is . The calculation methods are as follows: ; ; ; Among them, is the exponential mapping of popularity of, and are the bias errors of acceleration and angular velocity respectively, is the gravitational acceleration; Assume that the scanner coordinate system {B} coincides with the IMU coordinate system {I}. First, transform the point cloud from the lidar coordinate system {L} to {I}; Through rotational distortion compensation, each point in the point cloud has been transformed to the coordinate system at the start point of the point cloud, and calculate the difference between the encoder value of the first point and the encoded value of zero : ; Calculate the rotation matrix between the radar coordinate system {L} and the coordinate system {M} with an encoder value of zero : ; For each laser point, through coordinate transformation to the coordinate system at the end of the point cloud frame, motion distortion compensation is achieved: ; Preferably, the step S30 specifically includes: S301. Geometric plane feature extraction; S302. Intensity information assisted segmentation.

[0009] Preferably, the step S301 specifically includes: For each filled voxel, if the minimum eigenvalue of the covariance matrix of all points in the voxel is lower than the set threshold, it means that these points are approximately coplanar, then regard this plane point set as a plane feature; If the point set does not meet the plane condition, further divide the voxel into eight sub-voxels, and repeat the above plane extraction and segmentation operations for each sub-voxel until the preset maximum subdivision level is reached. Suppose the voxel contains the point set , then the mean value of the point set and the covariance matrix are calculated as follows: ; ; Perform eigenvalue decomposition on the covariance matrix through principal component analysis (PCA), and the eigenvector corresponding to the minimum eigenvalue is the normal vector of the voxel plane , and the center point is the mean value of the point cloud.

[0010] Preferably, the step S302 specifically includes: Suppose the voxel contains the point set , where the intensity value of the point is , the average intensity of the point set and the intensity variance are calculated as follows: ; ; The average intensity reflects the overall intensity characteristics of the point set within the voxel; the intensity variance is used to determine whether the point set belongs to the same object surface. If the variance is small, it indicates that the intensity consistency of the point set is high and it may come from the same material or object.

[0011] Preferably, S401. Calculate the point-plane geometric residual For a point in the current frame , transform it to the global coordinate system {W} through the transformation matrix : ; Assume that the corresponding plane in the previous frame is defined by the unit normal vector and a point on the plane, then the geometric residual from the point to the plane is defined as: ; S402. Calculate the intensity residual The plane feature within the voxel preserves the average intensity and the intensity variance , the intensity value of the point in the current frame is , then the intensity residual is defined as: ; Among them, is a very small positive number; S403. The joint optimization objective combines the two residuals by weighting to form an overall optimization objective: ; Among them, and are the weights of the geometric residual and the intensity residual respectively, used to balance the importance of the two. Through non-linear least squares optimization, is iteratively updated, and finally the optimal pose transformation between the current frame and the previous frame is obtained.

[0012] Preferably, the transformation matrix of the point cloud of each frame relative to the global coordinate system can be obtained through pose estimation; Using this transformation matrix, the point cloud data of each frame can be seamlessly transformed from the lidar coordinate system {L} to the global coordinate system {W}, realizing the unified alignment and accumulation of the point clouds of each frame.

[0013] Compared with the prior art, the beneficial effects of the present invention are: 1. When the viewing angle of the lidar is limited, the scanning efficiency is effectively improved by a rotating motor; a mapping algorithm matching the intensity information is invented.

[0014] 2. By sequentially performing rotation compensation on all points in the point cloud, the distortion generated by the lidar during the rotation of the motor can be effectively eliminated. Based on the integration of IMU data, the motion trajectory of the carrier during scanning is predicted, and the point cloud is reversely compensated using the pose difference to eliminate motion distortion. After these two compensations, the point cloud data can more accurately reflect the real environment, significantly improving the mapping accuracy and robustness of the SLAM system. 3. The inter-frame pose estimation ensures the spatial alignment of the point cloud through the point-plane geometric residual and enhances the robustness under illumination and texture changes through the intensity residual. The joint optimization of the two helps to achieve high-precision and robust pose estimation in environments with simple structures or insufficient textures.

[0015] In summary, the present invention overcomes the deficiencies of the prior art, effectively improves the scanning efficiency through a rotating motor, and has high social use value and application prospects. Specific Embodiments

[0016] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention are clearly and completely described below. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0017] Embodiment 1

[0018] This embodiment discloses an intensity-enhanced three-dimensional laser mapping method, including the following steps: S10. Real-time acquisition of three-dimensional lidar point cloud data, IMU measurement data, and motor encoder data. S20. Removal of motor rotation distortion and motion distortion. S30. Construction and management of an intensity-enhanced voxel map. S40. Calculation of point-plane geometry and intensity residuals for inter-frame pose estimation. S50. Publishing of lidar point cloud data in the global coordinate system.

[0019] In a specific embodiment, the specific content of step S20 is as follows: S201. Removal of motor rotation distortion: To eliminate the point cloud distortion generated during the rotation of the lidar, it is necessary to use the motor encoder data in step S10 to perform time synchronization interpolation and coordinate transformation on the point cloud, and rotate all the point cloud data to the coordinate system at the start time of the point cloud. Assume that the th frame of the point cloud collected by the lidar is , where , represents the th point in the point cloud, and the timestamp is . Through the timestamp index, find the two encoder timestamps that are closest to and in the encoder timestamp sequence. The encoder data corresponding to and are and , where . To obtain the encoder data corresponding to the sampling time of point , perform linear interpolation calculation: ; Using the obtained by interpolation, the point can be transformed to the reference coordinate system (start / end point coordinate system) to achieve compensation for the motor rotation distortion. Assume that the encoder value corresponding to the start point of the point cloud is , and the range of the encoder is . Map the encoder data to the rotation radian and calculate the rotation angle of point relative to the starting time: ;

[0020] Considering that the laser rotates along the axis, the corresponding rotation matrix can be calculated: ; To rotate the point cloud to the starting coordinate system, it is necessary to rotate the point cloud in the reverse direction. The specific operation is: ; where is the point after distortion compensation.

[0021] By performing rotation compensation on all the points in the point cloud in turn, the distortion generated during the motor rotation of the lidar can be effectively eliminated, thereby improving the accuracy and consistency of the point cloud data, and providing more reliable data support for subsequent mapping and positioning.

[0022] S202. Motion distortion removal: During the rotation of the motor, the scanner itself is also in motion, which causes motion distortion in the points of the point cloud when sampled at different times. To compensate for this distortion, it is necessary to combine the IMU measurement data described in S10 to predict the motion trajectory of the carrier.

[0023] The IMU measurement data includes acceleration and angular velocity which are used for integration to predict the pose of the carrier during scanning. The rotation at each moment is and the velocity is and the position is calculated as follows: ; ; ; where is the exponential map of the Lie group , and are the bias errors of acceleration and angular velocity respectively, and is the gravitational acceleration.

[0024] Assume that the scanner coordinate system {B} coincides with the IMU coordinate system {I}. To perform motion compensation using the predicted carrier pose above, first what we need is to transform the point cloud from the lidar coordinate system {L} to {I}. Each point in the point cloud has been transformed to the coordinate system at the start of the point cloud through rotational distortion compensation, and the difference between the encoder value of the first point and the encoded value of zero is calculated. Calculate the rotation matrix between the radar coordinate system {L} and the coordinate system {M} with an encoder value of zero: ; ; For each laser point, motion distortion compensation is achieved by transforming it to the coordinate system at the end of the point cloud frame through coordinate transformation: ; where is the point after motion compensation. Based on the IMU measurement data, the motion trajectory of the carrier during scanning is predicted, and the point cloud is compensated backward using the pose difference to eliminate motion distortion.

[0025] After these two steps of compensation, the point cloud data can more accurately reflect the real environment, significantly improving the mapping accuracy and robustness of the SLAM system.

[0026] In a specific embodiment, the specific content of step S30 is: To efficiently manage and represent environmental information, the system first divides the space in the global coordinate system into voxel units with a coarse map resolution. For the LiDAR scan data that first defines the world coordinate system, all point cloud data is assigned to the corresponding voxels according to their spatial positions. The voxels filled with point cloud data are indexed and stored in a hash table for quick access and update.

[0027] Specific steps for constructing and managing an intensity-enhanced voxel map: S301. Geometric plane feature extraction. For each filled voxel, if the minimum eigenvalue of the covariance matrix of all points in the voxel is lower than the set threshold, it indicates that these points are approximately coplanar. Then, this set of plane points is regarded as a plane feature, and its plane parameters (normal vector and center point ) are calculated, and its uncertainty (covariance matrix) is recorded. If the point set does not meet the plane condition, the voxel is further subdivided into eight sub-voxels, and the above-mentioned plane extraction and segmentation operations are repeated for each sub-voxel until the preset maximum subdivision level is reached. Suppose the voxel contains a point set , then the mean of the point set and the covariance matrix are calculated as follows: ; ; The covariance matrix is decomposed by principal component analysis (PCA). The eigenvector corresponding to the minimum eigenvalue is the normal vector of the voxel plane, and the center point is the mean of the point cloud. The uncertainty takes into account both the point cloud acquisition noise and the pose estimation error to ensure the accuracy and robustness of the model.

[0028] S302. Intensity information-assisted segmentation. For the point set that meets the plane condition, the system calculates and stores the average intensity and intensity variance of the point cloud. If the intensity variance is lower than the set threshold, it indicates that this plane may come from the same object and no further subdivision is required; otherwise, the current voxel is continued to be subdivided into eight sub-voxels, and the plane intensity analysis and voxel segmentation are repeated within each sub-voxel until the condition is met or the maximum subdivision level is reached. Suppose the voxel contains a point set , where the intensity value of the point is , then the average intensity and intensity variance of the point set are calculated as follows: ; ; The average intensity reflects the overall intensity characteristics of the point set within the voxel; the intensity variance is used to determine whether the point set belongs to the same object surface. If the variance is small, it indicates that the intensity consistency of the point set is high, and it may come from the same material or object.

[0029] In a specific embodiment, the specific content of step S40 is as follows: In inter-frame pose estimation, we accurately estimate the relative pose between the current frame and the previous frame by minimizing the point-plane geometric residual and the intensity residual. The overall residual consists of two parts: the geometric residual and the intensity residual. The calculation of the overall residual includes the following steps: S401. Calculate the point-plane geometric residual: The point-plane geometric residual is used to constrain the geometric relationship between the laser point and the plane in the previous frame, ensuring the alignment of the point cloud in the current frame with the plane feature in the map. For a point in the current frame, it is transformed into the global coordinate system {W} through the transformation matrix : ; Assuming that the corresponding plane in the previous frame is defined by the unit normal vector and a point on the plane, the geometric residual from the point to the plane is defined as: ; S402. Calculate the intensity residual: The intensity residual is used to constrain the light intensity information of the point cloud and enhance the adaptability to illumination changes or texture features. The plane feature within the voxel preserves the average intensity and the intensity variance . The intensity value of the point in the current frame is . Then the intensity residual is defined as: ; Among them, is a very small positive number used to avoid a zero denominator. This residual represents the deviation of the current point from the target plane in terms of light intensity information. Minimizing this residual helps improve the robustness of the state estimation.

[0030] S403. Joint optimization objective: To optimize both the geometric and light intensity information simultaneously, we combine the two residuals with weights to form an overall optimization objective: ; Among them, and are the weights of the geometric residual and the intensity residual respectively, used to balance the importance of the two. Through non-linear least squares optimization (such as the Gauss-Newton or Levenberg-Marquardt algorithm) for Iterative updates are performed to finally obtain the optimal pose transformation between the current frame and the previous frame.

[0031] Inter-frame pose estimation ensures the spatial alignment of the point cloud through point-plane geometric residuals and enhances the robustness under illumination and texture changes through intensity residuals. The joint optimization of the two helps to achieve high-precision and robust pose estimation in environments with simple structures or insufficient textures.

[0032] In a specific embodiment, the principle of the step of publishing the lidar point cloud data in the global coordinate system described in step S50 is as follows: The transformation matrix of each frame of point cloud relative to the global coordinate system can be obtained through pose estimation , and this matrix contains rotation and translation information. Using this transformation matrix, each frame of point cloud data can be seamlessly transformed from the lidar coordinate system {L} to the global coordinate system {W}, realizing the unified alignment and accumulation of each frame of point cloud.

[0033] By continuously updating the pose transformation matrix of each frame of point cloud and accurately superimposing all frames of point cloud in the global coordinate system, a continuous and complete three-dimensional map can be effectively constructed. This process not only ensures the spatial consistency of the point cloud data but also lays a foundation for subsequent map optimization and loop detection modules.

[0034] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. An intensity-enhanced three-dimensional laser mapping method, characterized in that: The following steps are involved: S10. Acquire 3D laser radar point cloud data, IMU measurement data and motor encoder data in real time; S20. Remove motor rotation distortion and motion distortion; S30. Build and manage intensity-enhanced voxel maps; S40. Calculate point-surface geometry and intensity residuals to perform inter-frame pose estimation; S50. Publish the laser point cloud data in the global coordinate system.

2. The intensity-enhanced 3D laser mapping method according to claim 1, characterized in that: The step S20 specifically includes: 1) Remove motor rotation distortion: Use the motor encoder data to perform time synchronization interpolation and coordinate transformation on the point cloud, and rotate all point cloud data to the coordinate system at the start time of the point cloud; 2) Remove motion distortion: Predict the motion trajectory of the carrier during scanning based on IMU measurement data, and use the posture difference to reversely compensate the point cloud to eliminate motion distortion.

3. The intensity-enhanced 3D laser mapping method according to claim 2, characterized in that: The method of removing the motor rotation distortion specifically includes: assuming that the laser radar collects the first The frame point cloud is , Represented as the first points, with a timestamp of , to obtain the point Encoder data corresponding to the sampling time , calculated using linear interpolation: ； Among them, through the timestamp index, find the corresponding The two closest encoder timestamps and , and The corresponding encoder data is and ,in ; Set the encoder value corresponding to the starting point of the point cloud to , the encoder range is , maps the encoder data to rotation radians, and calculates the point Rotation angle relative to the starting time: ； Considering that the laser moves along Axis rotation, the corresponding rotation matrix can be calculated: ； In order to rotate the point cloud to the starting coordinate system, the point cloud needs to be rotated in the opposite direction. The specific operation is: ； in, is the point after distortion compensation.

4. The intensity-enhanced 3D laser mapping method according to claim 2, characterized in that: The removal of motion distortion specifically includes: IMU measurement data has acceleration and angular velocity It is used to integrally predict the position of the carrier during the scanning process. The rotation at each moment is , speed is and location The calculation is as follows: ； ； ； in, For popular The exponential mapping of and are the bias errors of acceleration and angular velocity, is the acceleration due to gravity; Assuming that the scanner coordinate system {B} coincides with the IMU coordinate system {I}, first transform the point cloud from the lidar coordinate system {L} to {I}; Each point in the point cloud has been transformed to the coordinate system of the starting point of the point cloud by rotating the distortion compensation point cloud, and calculating the difference between the encoder value of the first point and the encoder value of zero : ； Calculate the rotation matrix between the radar coordinate system {L} and the coordinate system {M} where the encoder value is zero : ； For each laser point, motion distortion compensation is achieved by transforming the coordinate system to the coordinate system of the point cloud frame cutoff: 。 5. The intensity-enhanced 3D laser mapping method according to claim 1, characterized in that: The step S30 specifically includes: S301. Geometric plane feature extraction; S302. Intensity information assisted segmentation.

6. The intensity-enhanced 3D laser mapping method according to claim 5, characterized in that: The step S301 specifically includes: for each filled voxel, if the minimum eigenvalue of the covariance matrix of all points in the voxel is lower than a set threshold, it means that these points are approximately coplanar, and the plane point set is regarded as a plane feature; If the point set does not meet the plane condition, the voxel is further subdivided into eight sub-voxels, and the above plane extraction and segmentation operations are repeated for each sub-voxel until the preset maximum subdivision level is reached. Suppose the voxel contains the point set , then the mean of the point set and the covariance matrix The calculation is as follows: ； ； The covariance matrix is ​​decomposed by principal component analysis (PCA), and the eigenvector corresponding to the minimum eigenvalue is the normal vector of the voxel plane. , center point is the mean of the point cloud.

7. The intensity-enhanced 3D laser mapping method according to claim 5, characterized in that: The step S302 specifically includes: assuming that the voxel contains a point set , where the intensity value of the point is , then the average intensity of the point set is and intensity variance The calculation is as follows: ； ； The average intensity reflects the overall intensity characteristics of the point set within the voxel; the intensity variance is used to determine whether the point set belongs to the same object surface. If the variance is small, it means that the intensity consistency of the point set is high and may come from the same material or object.

8. The intensity-enhanced 3D laser mapping method according to claim 1, characterized in that: The step S40 specifically includes: S401. Calculate point-surface geometry residuals For a point in the current frame , which is transformed by the transformation matrix Transform to the global coordinate system {W}: ； Assuming that the corresponding plane in the previous frame is defined by a unit normal vector and a point on the plane, the geometric residual from the point to the plane is defined as: ； S402. Calculate the intensity residual Planar features within a voxel preserve the average intensity and intensity variance , current frame point The strength value is , then the strength residual is defined as: ； in, is a very small positive number; S403. Joint optimization objective: The two residuals are weighted and combined into an overall optimization objective: ； in, and are the weights of the geometric residual and the intensity residual, respectively, to balance the importance of the two. Perform iterative updates and finally obtain the optimal pose transformation between the current frame and the previous frame.

9. The intensity-enhanced 3D laser mapping method according to claim 1, characterized in that: The step S50 specifically includes: Through pose estimation, the transformation matrix of each frame point cloud relative to the global coordinate system can be obtained ; Using this transformation matrix, each frame of point cloud data can be seamlessly converted from the lidar coordinate system {L} to the global coordinate system {W}, achieving unified alignment and accumulation of point clouds in each frame.

Images