Point cloud mapping method and system of area array Geiger APD laser radar
By transforming the coordinate system and global map registration of Geiger APD laser point clouds, the problem of sparse long-distance imaging of Geiger APD lidar is solved, the accuracy and success rate of point cloud registration are improved, and the density of the target scene is achieved. Create a picture.
Patent Information
- Application Number
- CN202310529042.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-11
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2043-05-11
AI Technical Summary
The point clouds of Geiger APD lidar are sparse and susceptible to noise, resulting in the failure of long-distance laser mapping algorithms and cannot support target detection and tracking tasks.
By transforming the depth direction of the Geiger APD laser point cloud coordinate system, converting it into a relative coordinate system, weakening the impact of the depth large value on attitude estimation, and registering the current frame point cloud with the global map to keep the points in Within the preset range, the computing resource pressure is reduced, the registration accuracy is improved, and dense imaging of long-distance targets is finally achieved through projection transformation.
The success rate of long-distance Geiger APD laser point cloud registration is improved, the cumulative error is reduced, the dense point cloud mapping results of the target scene are achieved, and the distance image resolution of the detector is enhanced.
Smart Images

Figure CN118938249A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of laser mapping, and more specifically, relates to a point cloud mapping method and system for an area array Geiger APD laser radar. Background Art
[0002] Laser Construction Figure 1 Generally, it means obtaining three-dimensional point cloud information by scanning the surrounding environment through LiDAR, estimating its own posture by extracting its three-dimensional features and performing point cloud registration, and then realizing the construction of a three-dimensional map of the surrounding environment. It is a popular research direction and is widely used in many fields such as autonomous driving, robot navigation, three-dimensional reconstruction and three-dimensional target detection.
[0003] Geiger APD laser radar has the advantages of high frame rate, long detection distance, and high detection accuracy, and has important applications in both military and civilian fields. However, because Geiger APD laser radar is easily affected by noise, the signal-to-noise ratio of the detection data is low. At the same time, due to its own design problems, the resolution of the collected distance image is low. In order to deal with the noise problem, the laser data needs to be denoised, which makes the obtained point cloud more sparse. Even if hundreds of frames of the point cloud collected by the laser detector are superimposed, the obtained point cloud is still relatively sparse and cannot support subsequent tasks, such as: target detection and target tracking.
[0004] Laser mapping is performed by registering the point cloud obtained by scanning the Geiger APD laser radar, and the spatial dimension information is enhanced with the time dimension information to enrich the three-dimensional structure information of the scene to meet the needs of downstream tasks. However, due to the sparse point cloud of the Geiger APD laser radar, simply performing point cloud registration can easily result in incorrect registration results, leading to mapping failure. The reasons are as follows: First, the detection distance of the Geiger APD laser radar is long, and the point cloud registration algorithm is prone to failure. Secondly, the scanned point cloud is sparse and the scene features are not rich, which also increases the difficulty of point cloud registration. Finally, as the laser detector continues to move, the spatial resolution of the point cloud has changed significantly, and the imbalance of the features also hinders the point cloud registration process. How to achieve an accurate and robust long-distance Geiger APD laser mapping algorithm is still an urgent problem to be solved. Summary of the invention
[0005] In view of the above defects or improvement needs of the prior art, the present invention provides a point cloud mapping method for an area array Geiger APD laser radar, the purpose of which is to transform the coordinate system of the Geiger APD laser point cloud in the depth direction and transform it into a relative coordinate system for alignment, thereby weakening the dominant influence of large depth values on posture estimation; directly aligning the current frame point cloud with the relative global map can reduce the cumulative error in the alignment process; then maintaining the number of points in the global map within a preset range to reduce the pressure on computing resources and improve the accuracy of the alignment, thereby achieving a dense point cloud mapping result for the target scene; and projecting the enhanced point cloud to finally achieve dense laser imaging of long-distance targets, thereby solving the technical problem of sparse long-distance imaging of the existing Geiger APD laser mapping algorithm.
[0006] To achieve the above object, according to one aspect of the present invention, a point cloud mapping method for an area array Geiger APD laser radar is provided, comprising:
[0007] S1: transform the absolute laser coordinate system of the current frame point cloud in the laser echo signal in the depth direction to convert the current frame point cloud in the laser coordinate system into the current frame point cloud in the relative laser coordinate system This weakens the effect of large depth values;
[0008] S2: Use the point cloud of the current frame and the previous frame in the relative laser coordinate system to obtain the precise posture transformation from the current frame to the previous frame i is the current frame number, p is the identifier of the precise posture;
[0009] S3: Take the relative laser coordinate system of the first frame point cloud as the world coordinate system, and use the 0th to i-1th frame point cloud in the world coordinate system to construct the relative global map of the previous frame right Perform point cloud downsampling; use the precise posture transformation T from the previous frame to the first frame i-1→0 The exact pose transformation from the current frame to the previous frame Get the rough pose transformation from the current frame to the first frame r is the rough pose identifier; using the downsampled and Calculate the exact pose transformation from the current frame to the first frame
[0010] S4: Using the current frame point cloud With the corresponding Build a relative global map of the current frame set up The point threshold in is within the preset range;
[0011] S5: The relative global pose H of the current frame iRestore to the absolute global pose L i ;H i for The inverse matrix of i and Obtaining an enhanced point cloud in the absolute laser coordinate system of the current frame; and projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a dense depth map of the target scene.
[0012] In one embodiment, the S1 includes:
[0013] S11: Obtaining a count value map of a current frame in a laser echo signal, and calculating an absolute depth map of the current frame using the count value map and a corresponding delay time;
[0014] S12: Back-project the absolute depth map of the current frame into the corresponding laser coordinate system to obtain a point cloud of the current frame in the absolute laser coordinate system;
[0015] S13: transforming the absolute laser coordinate system in the depth direction to convert the current frame point cloud in the absolute laser coordinate system into the current frame point cloud in the relative laser coordinate system.
[0016] In one embodiment, the S13 includes:
[0017] use The coordinates of the current frame point cloud in the absolute laser coordinate system [XYZ 1] T Transformed into the coordinates of the current frame point cloud in the relative laser coordinate system [X′ Y′ Z′ 1] T , A is the transformation matrix in the depth direction of the coordinate system, R represents the rotation matrix, t represents the translation matrix, c is the speed of light, t d The delay time of the current frame.
[0018] In one embodiment, the S2 includes:
[0019] S21: performing voxel downsampling on the current frame point cloud in the relative laser coordinate system to obtain a preprocessed point cloud, extracting a fast point feature histogram FPFH feature corresponding to the preprocessed point cloud, and estimating a surface normal vector of the preprocessed point cloud;
[0020] S22: Based on the RANSAC algorithm, the FPFH features are roughly aligned between point clouds to obtain a rough posture transformation from the i-th frame to the i-1-th frame.
[0021] S23: Based on the point-to-plane ICP algorithm, the i-1th frame point cloud, the i-th frame point cloud and the rough posture transformation Perform precise registration between laser point clouds to obtain accurate posture transformation from the i-th frame to the i-1-th frame
[0022] In one embodiment, the S3 includes:
[0023] The relative laser coordinate system of the first frame is used as the world coordinate system, and the point cloud of the 0th to i-1th frames is used to construct a relative global map. in,
[0024] Relative global map Downsample the point cloud data in voxels and estimate the surface normal vector of the point cloud after voxel downsampling;
[0025] Utilize the accurate pose transformation from the previous frame to the first frame The exact pose transformation from the current frame to the previous frame Get the rough pose transformation from the current frame to the first frame
[0026] Based on the point-to-plane ICP algorithm, using the relative global map Current frame point cloud and the rough pose transformation from the current frame to the first frame Calculate the exact pose transformation from the current frame to the first frame
[0027] In one embodiment, the S4 includes:
[0028] Using the formula Constructing a relative global map set up The point count threshold in is always within the preset range; where N is the point cloud The midpoint number threshold, k is the frame number, i represents the current frame number, Represents the k-th frame point cloud The coordinates of the jth point in .
[0029] In one embodiment,
[0030] When the number of points in the point cloud is greater than the threshold N, the starting point cloud is deleted first to keep the number of points in the point cloud within the threshold N; when the number of points in the point cloud is less than or equal to the threshold N, the points in the point cloud are not deleted; specifically expressed as:
[0031] in, Represents the relative global map constructed by the point cloud of frames 0 to i, It indicates the point cloud that needs to be deleted when enriching the global map for the i-th time. card(*) is used to calculate the number of elements in the set.
[0032] In one embodiment, the S5 includes:
[0033] S51: Using formula The relative global pose H i Restore to the absolute global pose L i ;H i for The inverse matrix, A i is the transformation matrix in the depth direction of the current frame; A0 is the transformation matrix in the depth direction of the first frame;
[0034] S52: Using the absolute global pose L corresponding to the current frame i and relative global map Obtain the enhanced point cloud in the absolute laser coordinate system of the current frame;
[0035] S53: Projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a depth map of the target scene.
[0036] In one embodiment, the S53 includes:
[0037] Using the formula Projecting the enhanced point cloud in the absolute laser coordinate system of the current frame into the pixel coordinate system to obtain a depth map of the target scene;
[0038] Among them, [uv 1] T Represents the homogeneous coordinates of the pixel coordinate system of the current frame depth map, K′ is the super-resolution projection matrix, β is the adjustable resolution coefficient, its value is greater than or equal to 1, [XYZ] T Represents the coordinates of the enhanced point cloud in the absolute laser coordinate system of the current frame;
[0039] When β is equal to 1, the projection obtains a dense depth map of the original resolution; when β is greater than 1, the projection obtains a dense depth map of higher resolution.
[0040] According to another aspect of the present invention, a point cloud mapping system for an area array Geiger APD laser radar is provided, comprising:
[0041] The point cloud pre-operation module is used to transform the absolute laser coordinate system of the current frame point cloud in the laser echo signal in the depth direction, so as to convert the current frame point cloud in the laser coordinate system into the current frame point cloud in the relative laser coordinate system. This weakens the effect of large depth values;
[0042] Laser odometry module, used to obtain the precise posture transformation from the current frame to the previous frame using the point cloud of the current frame and the previous frame in the relative laser coordinate system i is the current frame number, p is the identifier of the precise posture;
[0043] The global attitude estimation module is used to use the relative laser coordinate system of the first frame point cloud as the world coordinate system and use the 0th to i-1th frame point cloud to construct the relative global map of the previous frame right Perform point cloud downsampling; use the precise posture transformation T from the previous frame to the first frame i-1→0 The exact pose transformation from the current frame to the previous frame Get the rough pose transformation from the current frame to the first frame r is the rough pose identifier; using the downsampled and Calculate the exact pose transformation from the current frame to the first frame
[0044] Mapping module, used to use the current frame point cloud With the corresponding Build a relative global map of the current frame set up The point threshold in is within the preset range;
[0045] The projection module is used to transform the relative global pose H of the current frame i Restore to the absolute global pose L i ;H i for The inverse matrix of i and Obtaining an enhanced point cloud in the absolute laser coordinate system of the current frame; and projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a depth map of the target scene.
[0046] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:
[0047] (1) The present invention provides a point cloud mapping method for a planar Geiger APD laser radar. The coordinate system of the Geiger APD laser point cloud is transformed in the depth direction and then converted to a relative coordinate system for registration, thereby weakening the dominant influence of large depth values on posture estimation and improving the success rate of Geiger APD laser point cloud registration for long-distance imaging. The current frame point cloud is directly registered with the relative global map to reduce the cumulative error in the registration process. The number of points in the global map is then maintained within a preset range to reduce the pressure on computing resources and improve the accuracy of the registration, thereby achieving a dense point cloud mapping result for the target scene. The enhanced point cloud is projected and transformed to ultimately achieve dense laser imaging of long-distance targets, thereby solving the technical problem of sparse long-distance imaging in the existing Geiger APD laser mapping algorithm.
[0048] (2) This scheme uses the count value map and the corresponding delay time to represent the absolute depth map of the current frame point cloud, so as to obtain the absolute geometric information of the target scene; the absolute depth map of the current frame is back-projected to the corresponding laser coordinate system, and then the absolute laser coordinate system is transformed in the depth direction. Instead of directly using the point cloud in the absolute laser coordinate system for alignment, it is transformed into a relative coordinate system for alignment, thereby weakening the dominant influence of large depth values on posture estimation and improving the success rate of Geiger APD laser point cloud alignment for long-distance imaging.
[0049] (3) This scheme uses the designed depth-direction transformation operation of the coordinate system of the Geiger APD laser point cloud to subtract half of the product of the delay time of the current frame and the speed of light from the detection distance, thereby weakening the dominant influence of large depth values on posture estimation; the influence of the delay time is considered for each frame, which can improve the accuracy of inter-frame registration.
[0050] (4) This solution uses the RANSAC algorithm to perform coarse registration between point clouds of the FPFH features to obtain a rough posture transformation corresponding to the i-th frame to the i-1-th frame; then uses the point-to-plane based ICP algorithm to perform precise registration between the laser point clouds for the i-1-th frame point cloud, the i-th frame point cloud, and the rough posture transformation. The two registrations can improve the accuracy of inter-frame registration. In addition, the two algorithms have low computational complexity and can be well adapted to inter-frame registration.
[0051] (5) This solution is based on the point-to-plane ICP algorithm and uses a relative global map Current frame point cloud and the rough pose transformation from the current frame to the first frame Calculate the exact pose transformation from the current frame to the first frame The computational complexity is low and it can adapt well to the inter-frame registration. This scheme can effectively reduce the cumulative error in the point cloud registration process under the same target scene and improve the registration accuracy.
[0052] (6) This plan sets The point count threshold in is always within the preset range. By assuming that the scene does not change significantly within a certain time range, the number of points in the global map is kept within the specified threshold, which can reduce the pressure on computing resources and improve the accuracy of registration.
[0053] (7) When the number of points in the point cloud is greater than the threshold N, this scheme preferentially deletes the initial point cloud so that the number of points in the point cloud remains within the threshold N; deleting the initial point cloud can better adapt to the changes in the target scene in the current frame.
[0054] (8) This scheme uses the formula The relative global pose H i Restore to the absolute global pose Li ; Using the absolute global pose L corresponding to the current frame i and relative global map Obtain enhanced point cloud in the absolute laser coordinate system of the current frame; be able to obtain dense three-dimensional information of the target scene in the current frame perspective.
[0055] (9) The point cloud super-resolution projection algorithm designed in this scheme projects the enhanced point cloud in the absolute laser coordinate system of the current frame onto a higher resolution depth map, which can achieve super-resolution of the depth map, enhance the detail information of the depth map, and improve the defect of low resolution of the distance image obtained by the Geiger APD detector. BRIEF DESCRIPTION OF THE DRAWINGS
[0056] Figure 1 A flowchart of a point cloud mapping method for a Geiger APD laser radar provided in an embodiment of the present invention.
[0057] Figure 2a and Figure 2b A visible light image of a power plant target at a close distance provided by an embodiment of the present invention.
[0058] Figure 3a and Figure 3b This is the target point cloud mapping result of a power plant provided in an embodiment of the present invention.
[0059] Figure 4 The comparison result of the original depth map and the enhanced depth map of the power plant target provided by the embodiment of the present invention.
[0060] Figure 5 This is the super-resolution projection result of the power plant target point cloud provided by an embodiment of the present invention.
[0061] Figure 6 A block diagram of a point cloud mapping system for an area array Geiger APD laser radar provided in an embodiment of the present invention. DETAILED DESCRIPTION
[0062] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.
[0063] Figure 1 This is a flow chart of a point cloud mapping method for a planar Geiger APD laser radar provided in an embodiment of the present invention. Figure 1 , the point cloud mapping method of the area array Geiger APD laser radar in this embodiment is described in detail, and the method includes S1-S5.
[0064] In this embodiment, the data collected by the airborne Geiger APD laser radar about 3500m away from a power plant is used as the implementation data. The visible light image of the power plant target at a close distance is as follows: Figure 2a and Figure 2b As shown, the red box is the specific target.
[0065] S1, transform the absolute laser coordinate system of the current frame point cloud in the laser echo signal in the depth direction to convert the current frame point cloud in the laser coordinate system into the current frame point cloud in the relative laser coordinate system This weakens the effect of large depth values.
[0066] According to an embodiment of the present invention, S1 includes S11-S13.
[0067] In S11, a count value map of the current frame in the laser echo signal is obtained, and an absolute depth map of the current frame is calculated using the count value map and the corresponding delay time.
[0068] In this embodiment, the count value map and the corresponding delay time of the accumulated 400 frames of the Geiger APD laser radar after denoising of the power plant are obtained, and the resolution of the value map is 64×64. The absolute depth map is calculated from the count value map and the delay time, and the absolute depth map D with a resolution of 64×64 is obtained. a It can be expressed as:
[0069]
[0070] Among them, D c Represents the count value diagram of the laser radar output. α is a coefficient whose value is related to the laser radar multi-frame cumulative imaging. Here, it is 0.18. c represents the speed of light, and t d Indicates the delay time, in seconds, M c Binary mask representing the count value map, D c Pixels greater than 0 in M c Set the center pixel to 1 and the rest of the pixels to 0.
[0071] In S12, the absolute depth map of the current frame is back-projected and transformed into the corresponding laser coordinate system to obtain a point cloud of the current frame in the absolute laser coordinate system.
[0072] In this embodiment, the absolute depth map obtained in S11 is back-projected and transformed into the corresponding laser coordinate system to obtain a point cloud in the absolute laser coordinate system. The transformation method is expressed as follows:
[0073]
[0074] Among them, [uv 1] T represents the homogeneous pixel coordinates in the pixel coordinate system of the absolute depth map Da. K represents the intrinsic parameter matrix of the laser detector, fx The value is 4000, f y The value is 4000, c x The value is 31.5, c y The value is 31.5. [XYZ] T Represents the point cloud coordinates of the absolute depth map back-projected to the absolute laser coordinate system.
[0075] In S13, the absolute laser coordinate system is transformed in the depth direction to convert the current frame point cloud in the absolute laser coordinate system into the current frame point cloud in the relative laser coordinate system.
[0076] In one embodiment, the point cloud in the absolute laser coordinate system obtained in S12 is transformed in the depth direction to obtain a point cloud in the relative coordinate system, and the transformation method is expressed as follows:
[0077]
[0078] Where [XYZ 1] T Represents the homogeneous coordinates of the current frame point cloud in the absolute laser coordinate system, A is the transformation matrix in the depth direction of the coordinate system, R represents the rotation matrix, t represents the translation matrix, [X′ Y′ Z′ 1] T Represents the homogeneous coordinates of the current frame point cloud relative to the laser coordinate system.
[0079] S2, using the point cloud of the current frame and the previous frame in the relative laser coordinate system to obtain the precise posture transformation from the current frame to the previous frame i is the current frame number, p is the identifier of the precise posture;
[0080] According to an embodiment of the present invention, S2 includes S21-S23.
[0081] In S21, voxel downsampling is performed on the current frame point cloud in the relative laser coordinate system to obtain a preprocessed point cloud, fast point feature histogram FPFH features corresponding to the preprocessed point cloud are extracted, and a surface normal vector of the preprocessed point cloud is estimated.
[0082] In one embodiment, the Geiger APD laser point cloud in the relative coordinate system obtained in S1 is downsampled, and the voxel size is 1. The fast point feature histogram feature points are displayed for the downsampled point cloud, and the feature radius is set to 5. Similarly, the normal vector is estimated for the downsampled point cloud, and the normal vector radius is set to 2.
[0083] In S22, the FPFH features are roughly registered between point clouds based on the RANSAC algorithm to obtain a rough posture transformation from the i-th frame to the i-1-th frame.
[0084] In one embodiment, the point cloud preprocessed in S21 is coarsely registered between point cloud frames. A rough pose transformation from the i-th frame point cloud to the i-1-th frame point cloud Expressed as:
[0085]
[0086] Among them, RANSAC(*) represents the RANSAC algorithm mapping function, and the distance threshold of the RANSAC algorithm is set to 1. FPFH(*) represents the extraction of point cloud fast point feature histogram FPFH feature operation, i∈N + , represents the i-1th frame point cloud after preprocessing, Represents the i-th frame point cloud after preprocessing.
[0087] In S23, the point cloud of the i-1th frame, the point cloud of the i-th frame and the rough posture transformation are performed based on the point-to-plane ICP algorithm. Perform precise registration between laser point clouds to obtain accurate posture transformation from the i-th frame to the i-1-th frame
[0088] In one embodiment, the rough pose transformation between point cloud frames obtained in S22 is used as the initial matrix. The point-to-plane ICP algorithm is used to achieve precise registration between point clouds, and a more accurate pose transformation matrix between frames is obtained. The precise pose transformation from the i-th frame to the i-1-th frame point cloud Expressed as:
[0089]
[0090] Wherein, ICP represents the mapping function of the ICP algorithm from point to surface, and the maximum corresponding point distance of the ICP algorithm is set to 5.
[0091] S3, taking the relative laser coordinate system of the first frame point cloud as the world coordinate system, and using the 0th to i-1th frame point cloud in the world coordinate system to construct the relative global map of the previous frame right Perform point cloud downsampling; use the precise pose transformation from the previous frame to the first frame The exact pose transformation from the current frame to the previous frame Get the rough global pose from the current frame to the first frame r is the rough pose identifier; using the downsampled and Calculate the exact pose transformation from the current frame to the first frame
[0092] In one embodiment, similar to S21, the point cloud data relative to the global map is downsampled, and the voxel size is 1. Similarly, the normal vector is estimated for the downsampled point cloud, and the normal vector radius is set to 2.
[0093] In one embodiment, the accurate pose transformation from the previous frame to the first frame is used The exact pose transformation from the current frame to the previous frame Get the rough pose transformation from the current frame to the first frame It is used as the initial pose of the point-to-plane ICP algorithm, and the rough pose transformation from the current frame to the first frame is Expressed as:
[0094]
[0095] Use the estimated normal vector for precise registration to obtain the precise posture transformation from the current frame to the first frame Expressed as:
[0096]
[0097] in, Represents the relative global map constructed for frames 0 to i-1.
[0098] S4, using the current frame point cloud With the corresponding Build a relative global map of the current frame set up The point threshold in is within the preset range.
[0099] In one embodiment, a relative global map of the power plant target is constructed. The relative laser coordinate system of the 0th frame is used as the world coordinate system, and the relative global map constructed by the point cloud of the 0th to ith frames Expressed as:
[0100]
[0101] in, Represents the k-th frame point cloud The coordinates of the jth point in .
[0102] As the global map of the power plant target is continuously enriched, when the number of points in the point cloud is greater than the threshold N, the initial point cloud is deleted first to keep the number of points in the point cloud within the threshold N; when the number of points in the point cloud is less than or equal to the threshold N, the points in the point cloud are not deleted, where N is 8000. The above can be expressed as:
[0103] in, Represents the global map constructed by the point cloud of frames 0 to i, It represents the point cloud that needs to be deleted when enriching the global map for the i-th time. The schematic diagram of the target point cloud mapping result of the power plant is as follows: Figure 3a and Figure 3b shown.
[0104] S5: The relative global pose H of the current frame i Restore to the absolute global pose L i ;H i for The inverse matrix of i and Obtaining an enhanced point cloud in the absolute laser coordinate system of the current frame; and projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a dense depth map of the target scene.
[0105] According to an embodiment of the present invention, S5 includes S51 - S53 .
[0106] S51, using the formula The relative global pose H i Restore to the absolute global pose L i ;H i for The inverse matrix, A i is the transformation matrix in the depth direction of the current frame; A0 is the transformation matrix in the depth direction of the first frame.
[0107] S52, using the absolute global pose L corresponding to the current frame i and relative global map Obtain the enhanced point cloud in the absolute laser coordinate system of the current frame.
[0108] In one embodiment, the enhanced point cloud in the absolute laser coordinate system of the current frame is obtained, which is expressed as:
[0109]
[0110] Where [X′ Y′ Z′ 1] T Represents the homogeneous coordinates of the point cloud relative to the global map coordinates, [XYZ 1] T Represents the homogeneous coordinates of the enhanced point cloud in the absolute laser coordinate system of the current frame.
[0111] S53, projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a depth map of the target scene.
[0112] In one embodiment, using the formula Projecting the enhanced point cloud in the absolute laser coordinate system of the current frame into the pixel coordinate system to obtain a depth map of the target scene;
[0113] Among them, [uv 1]T Represents the homogeneous coordinates of the pixel coordinate system of the current frame depth map, K′ is the super-resolution projection matrix, β is the adjustable resolution coefficient, its value is greater than or equal to 1, [XYZ] T Represents the coordinates of the enhanced point cloud in the absolute laser coordinate system of the current frame.
[0114] When β is equal to 1, the projection obtains a dense depth map of the original resolution; when β is greater than 1, the projection obtains a dense depth map of higher resolution.
[0115] During the projection process, when there are multiple depth values at the same pixel coordinate, the smallest depth value is selected as the real depth value at this point in the depth map. Figure 4 The schematic diagram of the super-resolution projection result of the power plant target point cloud is shown in Figure 5 As shown, β=2, that is, the resolution of the depth map increases from 64×64 to 128×128.
[0116] Figure 6 This is a block diagram of a point cloud mapping system for an area array Geiger APD laser radar provided in an embodiment of the present invention. Figure 6 The area array Geiger APD laser radar point cloud mapping system 600 includes a point cloud pre-operation module 610, a laser odometer module 620, a global attitude estimation module 630, a mapping module 640 and a projection module 650.
[0117] The point cloud pre-operation module 610, for example, executes S1 to transform the absolute laser coordinate system of the current frame point cloud in the laser echo signal in the depth direction, so as to transform the current frame point cloud in the laser coordinate system into the current frame point cloud in the relative laser coordinate system. This weakens the effect of large depth values.
[0118] The laser odometer module 620, for example, executes S2 to obtain the precise posture transformation from the current frame to the previous frame using the point cloud of the current frame and the previous frame in the relative laser coordinate system. i is the current frame number, and p is the identifier of the precise posture.
[0119] The global pose estimation module 630 executes S3, for example, to use the relative laser coordinate system of the first frame point cloud as the world coordinate system, and use the 0th to i-1th frame point clouds to construct the relative global map of the previous frame. right Perform point cloud downsampling; use the precise posture transformation T from the previous frame to the first frame i-1→0 The exact pose transformation from the current frame to the previous frame Get the rough global pose from the current frame to the first frame r is the rough pose identifier; using the downsampled and Calculate the exact pose transformation from the current frame to the first frame
[0120] The mapping module 640, for example, executes S4 to use the current frame point cloud With the corresponding Build a relative global map of the current frame set up The point threshold in is within the preset range.
[0121] The projection module 650, for example, executes S5 to convert the relative global pose H of the current frame into i Restore to the absolute global pose L i ;H i for The inverse matrix of i and Obtaining an enhanced point cloud in the absolute laser coordinate system of the current frame; and projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a depth map of the target scene.
[0122] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. A point cloud mapping method for area array Geiger APD laser radar. It is characterized in that include: S1: transform the absolute laser coordinate system of the current frame point cloud in the laser echo signal in the depth direction to convert the current frame point cloud in the laser coordinate system into the current frame point cloud in the relative laser coordinate system This weakens the effect of large depth values; S2: Use the point cloud of the current frame and the previous frame in the relative laser coordinate system to obtain the precise posture transformation from the current frame to the previous frame is the current frame number, and p is the identifier of the precise posture; S3: Take the relative laser coordinate system of the first frame point cloud as the world coordinate system, and use the 0th to i-1th frame point cloud in the world coordinate system to construct the relative global map of the previous frame right Perform point cloud downsampling; use the precise pose transformation from the previous frame to the first frame The exact pose transformation from the current frame to the previous frame Get a rough pose estimate from the current frame to the first frame r is the rough pose identifier; using the downsampled and Calculate the exact pose transformation from the current frame to the first frame S4: Using the current frame point cloud With the corresponding Build a relative global map of the current frame set up The point threshold in is within the preset range; S5: The relative global pose H of the current frame i Restore to the absolute global pose L i ;H i for The inverse matrix of i and Obtaining an enhanced point cloud in the absolute laser coordinate system of the current frame; and projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a dense depth map of the target scene.
2. The point cloud mapping method of the area array Geiger APD laser radar according to claim 1, It is characterized in that The S1 includes: S11: Obtaining a count value map of a current frame in a laser echo signal, and calculating an absolute depth map of the current frame using the count value map and a corresponding delay time; S12: Back-project the absolute depth map of the current frame into the corresponding laser coordinate system to obtain a point cloud of the current frame in the absolute laser coordinate system; S13: transforming the absolute laser coordinate system in the depth direction to convert the current frame point cloud in the laser coordinate system into the current frame point cloud in the relative laser coordinate system.
3. The point cloud mapping method of the area array Geiger APD laser radar as claimed in claim 2, It is characterized in that The S13 includes: use The coordinates of the current frame point cloud in the absolute laser coordinate system [XYZ 1] T Transformed into the coordinates of the current frame point cloud in the relative laser coordinate system [X′ Y′ Z′ 1] T , A is the transformation matrix in the depth direction of the coordinate system, R represents the rotation matrix, t represents the translation matrix, c is the speed of light, t d The delay time of the current frame.
4. The point cloud mapping method of the area array Geiger APD laser radar according to claim 1, It is characterized in that The S2 includes: S21: performing voxel downsampling on the current frame point cloud in the relative laser coordinate system to obtain a preprocessed point cloud, extracting a fast point feature histogram FPFH feature corresponding to the preprocessed point cloud, and estimating a surface normal vector of the preprocessed point cloud; S22: Based on the RANSAC algorithm, the FPFH features are roughly aligned between point clouds to obtain a rough posture transformation from the i-th frame to the i-1-th frame. S23: Based on the point-to-plane ICP algorithm, the i-1th frame point cloud, the i-th frame point cloud and the rough posture transformation Perform precise registration between laser point clouds to obtain accurate posture transformation from the i-th frame to the i-1-th frame 5. The point cloud mapping method of the area array Geiger APD laser radar according to claim 1, It is characterized in that The S3 includes: The relative laser coordinate system of the first frame is used as the world coordinate system, and the point cloud of the 0th to i-1th frames is used to construct a relative global map. in, Relative global map Downsample the point cloud data in voxels and estimate the surface normal vector of the point cloud after voxel downsampling; Utilize the accurate pose transformation from the previous frame to the first frame The exact pose transformation from the current frame to the previous frame Get a rough pose estimate from the current frame to the first frame Based on the point-to-plane ICP algorithm, using the relative global map Current frame point cloud and the rough pose estimation from the current frame to the first frame Calculate the exact pose transformation from the current frame to the first frame 6. The point cloud mapping method of the area array Geiger APD laser radar as claimed in claim 1, It is characterized in that The S4 includes: Using the formula Constructing a relative global map set up The point count threshold in is always within the preset range; where N is the point cloud The midpoint number threshold, k is the frame number, i represents the current frame number, Represents the k-th frame point cloud The coordinates of the jth point in .
7. The point cloud mapping method of the area array Geiger APD laser radar as claimed in claim 6, It is characterized in that The S4 further comprises: When the number of points in the point cloud is greater than the threshold N, the starting point cloud is deleted first to keep the number of points in the point cloud within the threshold N; when the number of points in the point cloud is less than or equal to the threshold N, the points in the point cloud are not deleted; specifically expressed as: in, Represents the relative global map constructed by the point cloud of frames 0 to i, represents the point cloud that needs to be deleted when enriching the global map for the i-th time, Used to count the number of elements in a collection.
8. The point cloud mapping method of the area array Geiger APD laser radar as claimed in claim 1, It is characterized in that The S5 includes: S51: Using formula The relative global pose H i Restore to the absolute global pose L i ;H i for The inverse matrix, A i A is the transformation matrix in the depth direction of the current frame; 0 is the transformation matrix in the depth direction of the first frame; S52: Using the absolute global pose L corresponding to the current frame i and relative global map Obtain the enhanced point cloud in the absolute laser coordinate system of the current frame; S53: Projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a dense depth map of the target scene.
9. The point cloud mapping method of the area array Geiger APD laser radar as claimed in claim 8, It is characterized in that The S53 includes: Using the formula Projecting the enhanced point cloud in the absolute laser coordinate system of the current frame into the pixel coordinate system to obtain a depth map of the target scene; Among them, [uv 1] T Represents the homogeneous coordinates of the pixel coordinate system of the current frame depth map, K′ is the super-resolution projection matrix, β is the adjustable resolution coefficient, its value is greater than or equal to 1, [XYZ] T Represents the coordinates of the enhanced point cloud in the absolute laser coordinate system of the current frame; When β is equal to 1, the projection obtains a dense depth map of the original resolution; when β is greater than 1, the projection obtains a dense depth map of higher resolution.
10. A point cloud mapping system for area array Geiger APD laser radar. It is characterized in that include: The point cloud pre-operation module is used to transform the absolute laser coordinate system of the current frame point cloud in the laser echo signal in the depth direction, so as to convert the current frame point cloud in the laser coordinate system into the current frame point cloud in the relative laser coordinate system. This weakens the effect of large depth values; Laser odometry module, used to obtain the precise posture transformation from the current frame to the previous frame using the point cloud of the current frame and the previous frame in the relative laser coordinate system is the current frame number, and p is the identifier of the precise posture; The global attitude estimation module is used to use the relative laser coordinate system of the first frame point cloud as the world coordinate system and use the 0th to i-1th frame point cloud to construct the relative global map of the previous frame right Perform point cloud downsampling; use the precise posture transformation T from the previous frame to the first frame i-1→0 The exact pose transformation from the current frame to the previous frame Get the rough pose transformation from the current frame to the first frame r is the rough pose identifier; using the downsampled and Calculate the exact pose transformation from the current frame to the first frame Mapping module, used to use the current frame point cloud With the corresponding Build a relative global map of the current frame set up The point threshold in is within the preset range; The projection module is used to transform the relative global pose H of the current frame i Restore to the absolute global pose L i ;H i for The inverse matrix of i and Obtaining an enhanced point cloud in the absolute laser coordinate system of the current frame; and projecting the enhanced point cloud in the absolute laser coordinate system of the current frame to obtain a dense depth map of the target scene.
Citation Information
Patent Citations
Positioning and mapping method and system based on fusion of laser radar and inertial measurement unit
CN113066105A
Point cloud map construction method based on laser radar data fusion of total station
CN113138395A
Laser SLAM method and system based on spherical projection characteristics
CN114814874A
Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method
CN115718303A
Calibration and mapping method of fusion laser radar and rotating motor thereof
CN115902932A
