ICP registration methods, storage media, terminals and systems

By pre-acquiring the rotation and height information of offline ground point clouds and transforming them into new original point clouds for ICP registration, the problems of high computational resource consumption and low accuracy of the ICP algorithm are solved, achieving more efficient and accurate pose calculation.

CN117760415BActive Publication Date: 2026-11-17SIQIAN (SHANGHAI) TECH CO LTD
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202310582377.1
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2023-05-23
Publication Date
2026-11-17
Estimated Expiration
2043-05-23

AI Technical Summary

Technical Problem

When calculating 6-DOF pose in 3D space, the ICP algorithm consumes a lot of computational resources and takes a long time. At the same time, ignoring height information in 2D space leads to a decrease in the accuracy of the calculation results.

Method used

The rotation values ​​(roll and pitch) and elevation translation values ​​(z) of the target in the offline ground point cloud are obtained in advance. A pre-transformation matrix [R',T'] is constructed to transform the original point cloud into a new original point cloud P(s'). Point cloud data association and iterative calculation are performed in the 3D dimension, and only the yaw, x, and y values ​​are calculated to optimize the ICP process.

Benefits of technology

This reduces the amount of computation, ensures the accuracy of iterative calculations, improves the precision of pose calculations, and avoids errors caused by missing height information.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN117760415B_ABST
    Figure CN117760415B_ABST
Patent Text Reader

Abstract

This disclosure relates to an ICP registration method, storage medium, terminal, and system. The ICP registration method of this disclosure pre-obtains the rotation (roll) and pitch values, as well as the altitude translation (z) value of the target in an offline ground cloud. Then, it transforms the original point cloud in the online point cloud based on the roll, pitch, and z values ​​to create a new original point cloud. Using this new original point cloud as a reference, iterative calculations are performed on the target's yaw, x, and y values. This setup ensures that the target's altitude information is not lost during iterative calculations of yaw, x, and y values, preventing errors in finding the nearest neighbor and guaranteeing the accuracy of the iterative calculations. Furthermore, by optimizing the ICP process by always setting the roll, pitch, and z values ​​to 0, only the yaw, x, and y values ​​need to be calculated during ICP registration, significantly reducing the computational load.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This disclosure relates to the field of intelligent driving technology, and in particular to an ICP registration method, storage medium, terminal and system. Background Technology

[0002] Autonomous vehicles are typically equipped with multiple sensors such as GPS, IMU, LiDAR, and cameras, and achieve localization through fusion. LiDAR localization is an important method for achieving fusion localization. Currently, LiDAR localization often uses real-time registration between online point clouds and offline point cloud maps to achieve positioning, and the registration algorithm often uses ICP (Iterative Nearest Neighbor) to implement it.

[0003] The basic principle of the ICP algorithm is as follows: The original point cloud is transformed to the target point cloud using an initial rotation transformation matrix R and a translation transformation matrix T. Here, P(s) represents a point in the original point cloud, and P(t) represents a point in the target point cloud coordinate system. This transformation relationship can then be expressed as:

[0004] P(t)=R*P(s)+T

[0005] The specific calculation steps are as follows:

[0006] Step 1: Obtain P(t) = R*P(s) + T from the R and T matrices, which is the coordinate of the original point cloud in the target point cloud coordinate system;

[0007] Step 2: Find the point P(t') that is closest to P(t) in the target point cloud;

[0008] Step 3: Calculate the Euclidean distance error for the points matched in Step 2, i.e., ||R*P(s)+TP(t')||, and optimize the R and T matrices using the least squares method;

[0009] Step 4: Substitute the optimized R and T matrices from Step 3 back into Step 1, iterate continuously, and obtain the final optimized R and T matrices.

[0010] ICP calculates the pose of 6 degrees of freedom in 3D space, namely the translation of 3 degrees of freedom x, y, z and the rotation of 3 degrees of freedom roll, pitch, yaw. This results in high computational resource consumption and increased time consumption. On the other hand, in most cases, autonomous vehicles pay more attention to the changes in translation x, y and heading yaw, so it is impossible to guarantee the accuracy of translation x, y and heading yaw when estimating 6 variables at the same time.

[0011] If ICP registration is performed in two-dimensional space, only the translation x, y and heading yaw are calculated, that is, the point cloud height information is ignored and compressed into a plane. Although the amount of computation is reduced, the lack of height information may lead to errors in finding the nearest neighbor point, and the overall accuracy of the calculation results will be reduced. Summary of the Invention

[0012] This disclosure provides an ICP registration method to solve at least some of the problems in related technologies.

[0013] According to the first aspect of this disclosure, an ICP registration method is proposed, the specific steps of which are as follows:

[0014] S1: Obtain the z-value, roll value, and pitch value of the target in the offline ground point cloud, and obtain the yaw' value, x' value, and y' value of the target in the online point cloud;

[0015] S2: Construct a pre-transformation matrix [R',T'] = [0,0,z,roll,pitch,0] based on the z, roll, and pitch values ​​obtained in step S1, and transform the original point cloud P(s) into a new original point cloud P(s') using the formula P(s') = R'*P(s) + T'.

[0016] S3: The new original point cloud P(s') is associated with the yaw', x' and y' values ​​obtained in S1 in the 3D dimension and iterative calculation is performed to obtain the yaw, x and y values ​​of the target in the offline ground point cloud, thereby obtaining the current pose [x,y,z,roll,pitch,yaw] of the target in the offline ground point cloud.

[0017] Optionally, the specific steps of step S3 are as follows:

[0018] S31: Construct a transformation matrix [R,T] = [x',y',0,0,0,yaw'] based on the yaw', x' and y' values ​​obtained in step S1, and obtain the coordinates P(t) of the new original point cloud in the coordinate system of the offline ground point cloud based on the formula P(t) = R*P(s') + T.

[0019] S32: Find the point P(t') that is closest to P(t) in the online point cloud in three-dimensional space;

[0020] S33: Calculate the Euclidean distance error for the point P(t') matched in step S32, i.e., ||R*P(s')+TP(t')||, and optimize the R and T matrices;

[0021] S34: Substitute the optimized R and T matrices from step S33 back into step S31, iterating continuously to obtain the final optimized R and T matrices, thereby obtaining the yaw, x, and y values ​​of the target in the offline ground point cloud. Optionally, the specific steps for obtaining the z value in step S1 are as follows:

[0022] The map in the offline ground point cloud is rasterized into multiple smallest units, and the average height value of all point clouds in the smallest unit is used as the elevation value of the smallest unit, thereby obtaining an elevation map;

[0023] Obtain the target's X and Y values, which have errors, through satellite positioning;

[0024] Based on the obtained X and Y values, the smallest unit in the elevation map where the target is located is determined, and the elevation value of this smallest unit is the z value of the target's current position.

[0025] Optionally, the elevation map can be constructed by stitching together point clouds using GPS or by using SLAM.

[0026] Optionally, in step S1, the roll value and pitch value are obtained through the inertial measurement unit (IMU).

[0027] Optionally, the online point cloud is extracted and fitted to the offline ground point cloud using the RANSA method, and the ground point cloud normal vector n(a,b,c) is calculated. From the ground point cloud normal vector, pitch = arctan(b / c) and roll = -arctan(a / c) are obtained.

[0028] According to a second aspect of this disclosure, a computer-readable storage medium is provided, the storage medium storing a computer program that can be loaded by a processor and execute the ICP registration method as described above.

[0029] According to a third aspect of this disclosure, a smart terminal is provided, the smart terminal including, for example, a processor and a storage medium as described above, wherein a computer program stored on the storage medium can be loaded and executed by the processor.

[0030] According to a fourth aspect of this disclosure, an ICP registration system is provided, the ICP registration system comprising:

[0031] The data acquisition module includes one or more sensors such as LiDAR, camera, GPS, and IMU, which are used to acquire the z-value, roll value, and pitch value of the target in the offline ground point cloud, and to acquire the yaw' value, x' value, and y' value of the target in the online point cloud.

[0032] The data processing module constructs a pre-transformation matrix [R',T'] = [0,0,z,roll,pitch,0] from the z-values, roll-values, and pitch-values ​​obtained from the data acquisition module. It then transforms the original point cloud P(s) into a new original point cloud P(s') using the formula P(s') = R'*P(s) + T'. The new original point cloud P(s') is then associated with the yaw', x', and y' values ​​obtained in S1 in the 3D dimension and iteratively calculated to obtain the yaw, x, and y values ​​of the target in the offline ground point cloud, thereby obtaining the current pose [x,y,z,roll,pitch,yaw] of the target in the offline ground point cloud.

[0033] The technical solutions provided by the embodiments of this disclosure may include the following beneficial effects:

[0034] This disclosure pre-obtains the roll and pitch values ​​of the target in an offline ground cloud, as well as the z-value of its altitude translation. Then, it transforms the original point cloud in the online point cloud based on these roll, pitch, and z values ​​to create a new original point cloud. Using this new original point cloud as a reference, iterative calculations are performed on the target's yaw, x, and y values. This setup ensures that the target's altitude information is not lost during iterative calculations of yaw, x, and y values, preventing errors in finding the nearest neighbor and guaranteeing the accuracy of the iterative calculations. Furthermore, by optimizing the ICP process, the roll, pitch, and z values ​​are always set to 0, meaning that only yaw, x, and y values ​​need to be calculated during ICP registration, significantly reducing the computational load. In short, this disclosure combines offline ground point clouds to help online point clouds achieve higher pose calculation accuracy with less computation.

[0035] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit this disclosure. Attached Figure Description

[0036] The accompanying drawings, which are incorporated in and form a part of this specification, illustrate embodiments consistent with this disclosure and, together with the description, serve to explain the principles of this disclosure.

[0037] Figure 1 This is a schematic diagram of an ICP registration method in an exemplary embodiment of this disclosure. Detailed Implementation

[0038] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numerals in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this disclosure. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this disclosure as detailed in the appended claims.

[0039] The terminology used in this disclosure is for the purpose of describing particular embodiments only and is not intended to be limiting of the disclosure. The singular forms “a,” “the,” and “the” as used in this disclosure and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used herein refers to and includes any and all possible combinations of one or more of the associated listed items.

[0040] like Figure 1 The diagram shows a flowchart of an ICP registration method proposed in the first aspect of this disclosure, and its specific steps are as follows:

[0041] S1: Obtain the z-value, roll value, and pitch value of the target in the offline ground point cloud, and obtain the yaw' value, x' value, and y' value of the target in the online point cloud;

[0042] S2: Construct a pre-transformation matrix [R',T'] = [0,0,z,roll,pitch,0] based on the z, roll, and pitch values ​​obtained in step S1, and transform the original point cloud P(s) into a new original point cloud P(s') using the formula P(s') = R'*P(s) + T'.

[0043] S3: The new original point cloud P(s') is associated with the yaw', x' and y' values ​​obtained in S1 in the 3D dimension and iterative calculation is performed to obtain the yaw, x and y values ​​of the target in the offline ground point cloud, thereby obtaining the current pose [x,y,z,roll,pitch,yaw] of the target in the offline ground point cloud.

[0044] In the above ICP registration method, the rotation roll value, pitch value, and altitude translation z value of the target in the offline ground cloud are obtained in advance. The rotation roll value, pitch value, and altitude translation z value are used as the basis, which reduces the amount of computation in subsequent iterative calculations and ensures that the ICP registration has sufficient computational accuracy.

[0045] In step S1 above, this disclosure uses GPS stitching point cloud or SLAM to build an elevation map. Taking the SLAM mapping method based on LiDAR as an example, methods such as LOAM, Cartographer, blam, and interactive_slam can be used.

[0046] Specifically, the map in the offline ground point cloud is rasterized into multiple smallest units. The average height of all point clouds within each smallest unit is used as the elevation value of that smallest unit, thus obtaining an elevation map. The smallest unit can be a 1m x 1m cell, a 0.5m x 0.5m cell, or a 2m x 2m cell. This disclosure does not impose any limitations on this, as long as the cell area is controlled within a suitable range, neither increasing the computational load of elevation map creation due to small cell areas nor causing data inaccuracies due to large cell areas.

[0047] The target's X and Y values, which have errors, are obtained through satellite positioning; wherein, the satellite positioning can be GPS, GPRS, BeiDou satellite positioning, or other satellite positioning; this disclosure does not limit this.

[0048] Based on the obtained X and Y values, the smallest unit in the elevation map where the target is located is determined. The elevation value of this smallest unit is the z-value of the target's current position. In this way, a z-value with small error can be obtained with a very small amount of calculation.

[0049] In step S1 above, this disclosure uses an IMU sensor to obtain the roll and pitch values ​​of the target at its current position in the offline ground point cloud. The IMU sensor calculates the roll and pitch values ​​of the target at its current position in the offline ground point cloud using an EKF (Extended Kalman Filter); however, the IMU sensor can also obtain the target's roll and pitch values ​​using other methods; this disclosure does not impose any limitations on this.

[0050] Specifically, the online point cloud is extracted and fitted to the offline ground point cloud using the RANSA method, and the ground point cloud normal vector n(a,b,c) is calculated. From the ground point cloud normal vector, pitch = arctan(b / c) and roll = -arctan(a / c) can be obtained.

[0051] In step S1 above, this disclosure can obtain the yaw', x', and y' values ​​of the target in the online point cloud using one or more sensors such as LiDAR, camera, GPS, and IMU. This disclosure does not limit this.

[0052] In step S2 above, a pre-transformation matrix [R',T'] = [0,0,z,roll,pitch,0] is constructed using the z-value, roll-value, and pitch-value. The original point cloud P(s) is then transformed into a new original point cloud P(s') using the formula P(s') = R'*P(s) + T'. This ensures that the target's height information is not lost during the iterative calculation in step S3, and avoids errors in finding the nearest point in the online point cloud during step S3.

[0053] It should be noted that in step S3, during the process of associating the new original point cloud P(s') with the yaw', x', and y' values ​​obtained in S1 in the 3D dimension and performing iterative calculations, the z, roll, and pitch values ​​to be optimized must always be set to 0. This setting is because when using the new original point cloud P(s') as a reference, the yaw, x, and y values ​​already contain the target's height information, so there is no need to set additional z, roll, and pitch values, which would increase the computational load.

[0054] Specifically, the steps in step S3 are as follows:

[0055] S31: Construct a transformation matrix [R,T] = [x',y',0,0,0,yaw'] based on the yaw', x' and y' values ​​obtained in step S1, and obtain the coordinates P(t) of the new original point cloud in the coordinate system of the offline ground point cloud based on the formula P(t) = R*P(s') + T.

[0056] S32: Find the point P(t') that is closest to P(t) in the online point cloud in three-dimensional space;

[0057] S33: Calculate the Euclidean distance error for the point P(t') matched in step S32, i.e., ||R*P(s')+TP(t')||, and optimize the R and T matrices;

[0058] S34: Substitute the optimized R and T matrices from step S33 back into step S31 and iterate continuously to obtain the final optimized R and T matrices, thereby obtaining the yaw, x, and y values ​​of the target in the offline ground point cloud.

[0059] This disclosure pre-obtains the roll and pitch values ​​of the target in an offline ground cloud, as well as the z-value of its altitude translation. Then, it transforms the original point cloud in the online point cloud based on these roll, pitch, and z values ​​to create a new original point cloud. Using this new original point cloud as a reference, iterative calculations are performed on the target's yaw, x, and y values. This setup ensures that the target's altitude information is not lost during iterative calculations of yaw, x, and y values, preventing errors in finding the nearest neighbor and guaranteeing the accuracy of the iterative calculations. Furthermore, by optimizing the ICP process, the roll, pitch, and z values ​​are always set to 0, meaning that only yaw, x, and y values ​​need to be calculated during ICP registration, significantly reducing the computational load. In short, this disclosure combines offline ground point clouds to help online point clouds achieve higher pose calculation accuracy with less computation.

[0060] A second aspect of this disclosure provides a computer-readable storage medium storing a computer program capable of being loaded by a processor and executing the ICP registration method as described above. The computer-readable storage medium can be, for example—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples (a non-exhaustive list) of a computer-readable storage medium include: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this document, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0061] A third aspect of this disclosure provides a smart terminal, which includes, for example, a processor and a storage medium as described above, wherein a computer program stored on the storage medium can be loaded and executed by the processor. The smart terminal may be a mobile phone, computer, server, or network device, etc.; this disclosure does not impose any limitations on this.

[0062] According to a fourth aspect of this disclosure, an ICP registration system is provided, the ICP registration system comprising:

[0063] The data acquisition module includes one or more sensors such as LiDAR, camera, GPS, and IMU, which are used to acquire the z-value, roll value, and pitch value of the target in the offline ground point cloud, and to acquire the yaw' value, x' value, and y' value of the target in the online point cloud.

[0064] The data processing module, i.e. the processor, constructs a pre-transformation matrix [R',T'] = [0,0,z,roll,pitch,0] from the z-values, roll-values, and pitch-values ​​obtained from the data acquisition module. It then transforms the original point cloud P(s) into a new original point cloud P(s') using the formula P(s') = R'*P(s) + T'. The new original point cloud P(s') is then associated with the yaw', x', and y' values ​​obtained in S1 in the 3D dimension and iteratively calculated to obtain the yaw, x, and y values ​​of the target in the offline ground point cloud, thereby obtaining the current pose [x,y,z,roll,pitch,yaw] of the target in the offline ground point cloud.

[0065] In some implementations, the processor constructs a transformation matrix [R,T] = [x',y',0,0,0,yaw'] from the yaw', x', and y' values ​​obtained by the data acquisition module. From the R and T matrices, P(t) = R*P(s') + T is obtained, which is the coordinate of the new original point cloud in the coordinate system of the offline ground point cloud. Then, the processor finds the point P(t') closest to P(t) in the online point cloud in three-dimensional space. Then, the Euclidean distance error is calculated for point P(t'), i.e., ||R*P(s') + TP(t')||, and the R and T matrices are optimized. Then, the optimized R and T matrices in step S24 are brought back to step S31, and the process is iterated to obtain the final optimized R and T matrices, thereby obtaining the yaw, x, and y values ​​of the target in the offline ground point cloud; that is, the current pose [x,y,z,roll,pitch,yaw] of the target in the offline ground point cloud is obtained.

[0066] Other embodiments of this disclosure will readily occur to those skilled in the art upon consideration of the specification and practice of the technical solutions disclosed herein. This disclosure is intended to cover any variations, uses, or adaptations of this disclosure that follow the general principles of this disclosure and include common knowledge or customary techniques in the art not disclosed herein. The specification and embodiments are to be considered exemplary only, and the true scope and spirit of this disclosure are indicated by the following claims.

[0067] It should be understood that this disclosure is not limited to the precise structures described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of this disclosure is limited only by the appended claims.

Claims

1. An ICP registration method, characterized in that, The specific steps are as follows: S1: Obtain the z-value, roll value, and pitch value of the target in the offline ground point cloud, and obtain the yaw' value, x' value, and y' value of the target in the online point cloud; S2: Construct a pre-transformation matrix [R',T'] = [0,0,z,roll,pitch,0] based on the z, roll, and pitch values ​​obtained in step S1, and transform the original point cloud P(s) into a new original point cloud P(s') using the formula P(s') = R'*P(s) + T'. S3: The new original point cloud P(s') is associated with the yaw', x' and y' values ​​obtained in S1 in the 3D dimension and iterative calculation is performed to obtain the yaw, x and y values ​​of the target in the offline ground point cloud, thereby obtaining the current pose [x,y,z,roll,pitch,yaw] of the target in the offline ground point cloud.

2. The ICP registration method according to claim 1, characterized in that, The specific steps of step S3 are as follows: S31: Construct a transformation matrix [R,T] = [x',y',0,0,0,yaw'] based on the yaw', x' and y' values ​​obtained in step S1, and obtain the coordinates P(t) of the new original point cloud in the coordinate system of the offline ground point cloud based on the formula P(t) = R*P(s') + T. S32: Find the point P(t') that is closest to P(t) in the online point cloud in three-dimensional space; S33: Calculate the Euclidean distance error for the point P(t') matched in step S32, i.e., ||R*P(s')+TP(t')||, and optimize the R and T matrices; S34: Substitute the optimized R and T matrices from step S33 back into step S31 and iterate continuously to obtain the final optimized R and T matrices, thereby obtaining the yaw, x, and y values ​​of the target in the offline ground point cloud.

3. The ICP registration method according to claim 1, characterized in that, The specific steps for obtaining the z value in step S1 are as follows: The map in the offline ground point cloud is rasterized into multiple smallest units, and the average height value of all point clouds in the smallest unit is used as the elevation value of the smallest unit, thereby obtaining an elevation map; Obtain the target's X and Y values, which have errors, through satellite positioning; Based on the obtained X and Y values, the smallest unit in the elevation map where the target is located is determined, and the elevation value of this smallest unit is the z value of the target's current position.

4. The ICP registration method according to claim 3, characterized in that, The elevation map is constructed by stitching together point clouds using GPS or SLAM.

5. The ICP registration method according to claim 1, characterized in that, In step S1, the roll value and pitch value are obtained through the inertial measurement unit (IMU).

6. The ICP registration method according to claim 1, characterized in that, The online point cloud is extracted and fitted to the offline ground point cloud using the RANSAC method. At the same time, the ground point cloud normal vector n(a,b,c) is calculated. From the ground point cloud normal vector, pitch = arctan(b / c) and roll = -arctan(a / c) are obtained.

7. A computer-readable storage medium, characterized in that, The storage medium stores a computer program that can be loaded by a processor and execute the ICP registration method as described in any one of claims 1 to 6.

8. A smart terminal, characterized in that, The smart terminal includes a processor and a storage medium as described in claim 7, wherein a computer program stored on the storage medium can be loaded and executed by the processor.

9. An ICP registration system, characterized in that, The ICP registration system includes: The data acquisition module includes one or more sensors such as LiDAR, camera, GPS, and IMU, which are used to acquire the z-value, roll value, and pitch value of the target in the offline ground point cloud, and to acquire the yaw' value, x' value, and y' value of the target in the online point cloud. The data processing module constructs a pre-transformation matrix [R',T'] = [0,0,z,roll,pitch,0] from the z-values, roll-values, and pitch-values ​​obtained from the data acquisition module. It then transforms the original point cloud P(s) into a new original point cloud P(s') using the formula P(s') = R'*P(s) + T'. The new original point cloud P(s') is then associated with the yaw', x', and y' values ​​obtained in S1 in the 3D dimension and iteratively calculated to obtain the yaw, x, and y values ​​of the target in the offline ground point cloud, thereby obtaining the current pose [x,y,z,roll,pitch,yaw] of the target in the offline ground point cloud.

Citation Information

Patent Citations

  • Rescue robot three-dimensional environment map real-time construction method

    CN104240297A

  • Systems, devices, and methods for tracking moving targets

    US20140205140A1