System and method for lidar relocalization based on substation three-dimensional point cloud map

Abstract

This invention discloses a system for lidar relocalization based on a 3D point cloud map of a substation. It includes a point cloud map loading module to construct a 3D point cloud map of the substation from a 3D model; a sliced ​​point cloud library construction module to slice the substation point cloud and construct a sliced ​​point cloud library; a lidar attitude acquisition module to read the lidar's roll and pitch angles; a sliced ​​point cloud acquisition module to slice the lidar's point cloud at the current moment, with the lidar as the origin and perpendicular to the direction of gravitational acceleration; a point cloud matching module to match the current sliced ​​point cloud of the lidar with the sliced ​​point clouds in the sliced ​​point cloud library, generating a matching score and a relative pose transformation matrix; and a lidar relocalization acquisition module to obtain the lidar's position and attitude based on the matching score, the relative pose transformation matrix, the gyroscope angle, and the slice height. This invention improves the calculation speed and efficiency of lidar relocalization based on a 3D point cloud map of a substation.

Description

Technical Field

[0001] This invention relates to the field of rapid relocation technology for three-dimensional laser point clouds, specifically to a system and method for laser radar relocation based on a three-dimensional point cloud map of a substation. Background Technology

[0002] The current mainstream solution for robot localization in substations involves matching radar data or odometer information with a high-precision map to determine the robot's precise location. This allows staff to quickly locate the robot and ensures its safe and stable operation. However, in practice, most robots require manual input of their initial map position. This leads to a problem where the robot's localization fails after matching or after a power outage and restart, disrupting its normal operation.

[0003] Robot relocation technology can overcome this problem. When a robot loses its global localization, it can be quickly re-matched through relocation to obtain the robot's precise location on the map. This solves the problem of the robot's global localization being lost and unrecoverable, and is of great significance for the stable and safe operation of the robot.

[0004] However, traditional point cloud matching algorithms directly match 3D point clouds, but the amount of 3D point cloud data is enormous. When the map is large, it occupies a significant amount of memory and hard drive space, and the processing speed is not fast enough to be suitable for high-speed motion scenarios. Therefore, there is an urgent need to propose a fast global relocalization method so that accurate global relocalization of the robot can be achieved quickly when global localization is lost. Summary of the Invention

[0005] The purpose of this invention is to provide a system and method for lidar relocation based on a three-dimensional point cloud map of a substation. This invention greatly improves the efficiency of lidar relocation.

[0006] To achieve this objective, the present invention provides a lidar relocalization system based on a 3D point cloud map of a substation, comprising:

[0007] The point cloud map loading module is used to construct a 3D point cloud map of a substation, including LiDAR, from an existing 3D substation model using a model-to-point cloud algorithm.

[0008] The slice point cloud library construction module is used to slice the three-dimensional point cloud map of the substation containing lidar in the vertical gravity acceleration direction to obtain slice point clouds, and construct a slice point cloud library based on the slice point clouds;

[0009] The lidar attitude acquisition module is used to obtain the lidar's roll angle R and pitch angle P through the lidar's built-in gyroscope.

[0010] The point cloud slice acquisition module is used to slice the point cloud of the lidar at the current moment with the lidar as the origin and perpendicular to the direction of gravitational acceleration, and obtain the current slice point cloud C;

[0011] The point cloud matching module performs ICP matching between the current slice point cloud C and the slice point clouds in the slice point cloud library, obtaining the relative pose transformation matrix T and ICP score between the current slice point cloud C and the slice point clouds in the slice point cloud library. The slice point cloud with the lowest ICP score and less than a threshold is selected as the matched slice point cloud C. mh ;

[0012] The lidar repositioning acquisition module is used to calculate the lidar's translation x along the x-axis of the substation ground based on the relative pose transformation matrix T. T The translational amount y of the lidar on the ground in the y-axis direction of the substation. T The yaw angle Y of the lidar, where the translation amount x T Translation amount y T and slice point cloud C mh The altitude mh represents the position after repositioning by the lidar, and the roll angle R, pitch angle P, and yaw angle Y represent the attitude after repositioning by the lidar.

[0013] The beneficial effects of this invention are:

[0014] The computational complexity of 3D-to-3D point cloud matching has been simplified, approximating it as a finite number (n) 2D-to-2D point cloud matching operations. This involves performing ICP matching (Integrated Point Calculation) between the current slice point cloud and slice point clouds in the slice point cloud library, thus reducing the computational cost from n times that of 3D-to-3D point cloud matching. 2 The computational complexity is reduced from 2D to n times, greatly reducing the computational load, increasing the speed of positioning calculations, and improving the efficiency of lidar repositioning. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the structure of the present invention;

[0016] Figure 2 This is a flowchart of the present invention;

[0017] Figure 3 This is a schematic diagram of a point cloud slice. Detailed Implementation

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

[0019] Example 1

[0020] A lidar relocalization system based on a 3D point cloud map of a substation, such as... Figure 1 As shown, it includes:

[0021] The point cloud map loading module is used to convert the existing 3D model of the substation into a 3D point cloud map containing LiDAR through a model-to-point cloud algorithm (the range of the 3D point cloud of the substation is generally bounded by the substation wall).

[0022] The sliced ​​point cloud library construction module is used to slice the 3D point cloud map of the substation containing lidar along the vertical gravitational acceleration direction to obtain sliced ​​point clouds. Based on these sliced ​​point clouds, a sliced ​​point cloud library is constructed. The module pre-converts the large amount of 3D point cloud data from the substation into smaller, limited-quantity approximate 2D sliced ​​point clouds, thus reducing the computational burden from n-fold matching of 3D point clouds. 2 The computational complexity is reduced from 2D to n-fold, which simplifies the computation process by directly computing slice points, greatly reducing the computational load.

[0023] The lidar attitude acquisition module is used to obtain the lidar's roll angle R and pitch angle P through the lidar's built-in gyroscope. This design is used to obtain two of the three values ​​of the final positioning lidar attitude representation.

[0024] The slice point cloud acquisition module is used to slice the point cloud of the lidar at the current moment with the lidar as the origin and perpendicular to the direction of gravitational acceleration, and obtain the current slice point cloud C. The lidar is handheld in the substation, and the point cloud of the lidar at the current moment is collected. This design is used to obtain the current slice point cloud C, and then perform ICP matching with the slice point cloud in the slice point cloud library.

[0025] The point cloud matching module is used to perform ICP matching (a method of point cloud registration) between the current slice point cloud C and the slice point clouds in the slice point cloud library. lipointmatcher is an open-source library with a relatively high level of ICP registration algorithm completion. It takes two point clouds as input and obtains a relative pose transformation matrix T, which ensures that one point cloud, after transformation by the relative pose transformation matrix, has the highest possible overlap with the other point cloud. This yields the relative pose transformation matrix T and ICP scores between the current slice point cloud C and the slice point clouds in the slice point cloud library. The slice point cloud Cmh with the lowest ICP score and a threshold (0.03) is selected as the matched slice point cloud. (If the slice point cloud does not simultaneously meet the requirement of the lowest ICP score and a threshold, it is recalculated until it does.) A limited number of slices are pre-stored. The current slice point cloud is compared with the stored slice point clouds, and the most similar slice point cloud is selected, i.e., the current slice point cloud and the matched slice point cloud are at the same height. Within the allowable error range, this design ensures accuracy while reducing computational load, and the calculation results have better real-time performance.

[0026] The lidar repositioning acquisition module is used to calculate the lidar's translation x along the x-axis of the substation ground based on the relative pose transformation matrix T. T The translational amount y of the lidar on the ground in the y-axis direction of the substation. T The yaw angle Y of the lidar, where the translation amount x T Translation amount y T and slice point cloud C mh The altitude mh represents the position after lidar repositioning, and the roll angle R, pitch angle P, and yaw angle Y represent the attitude after lidar repositioning. The lidar repositioning pose is (x... T y T The coordinates of the lidar are (x, mh, R, P, Y), i.e., (x, mh, R, P, Y). T y T The attitude is defined by roll angle R, pitch angle P, and yaw angle Y. This design is used to calculate the position and attitude of the lidar after repositioning, and the final calculation result is the lidar positioning result.

[0027] In the above technical solution, the three-dimensional model of the substation is constructed by using a three-dimensional laser scanner to perform laser scanning on the substation equipment, facilities, and structures, and then using three-dimensional reality modeling software.

[0028] In the above technical solution, the specific method for slicing the three-dimensional point cloud map of the substation along the direction of vertical gravitational acceleration and constructing the sliced ​​point cloud library is as follows:

[0029] The already constructed 3D point cloud map of the substation is sliced ​​along the vertical direction of gravity acceleration. The interval between each point cloud slice is h, and the slice thickness is t. The slice point clouds C0, Ch, C2h...Cnh are obtained, and the center height of the slice point cloud is 0, h, 2h...nh respectively.

[0030] The constructed 3D point cloud map of the substation has the z-axis in the opposite direction of gravitational acceleration, the xoy plane as the substation ground, and stores all slice point clouds to build a slice point cloud library. The x-axis, y-axis and origin o can be calibrated on the substation ground as needed. In this embodiment, the x-axis is the horizontal direction of the substation ground and the y-axis is the vertical direction of the substation ground.

[0031] In the above technical solution, a built-in gyroscope reads the roll angle R and pitch angle P of the lidar, where the roll angle R is the rotation angle around the x-axis and the pitch angle P is the rotation angle around the y-axis.

[0032] In the above technical solution, the relative pose transformation matrix T between the current slice point cloud C and each slice point cloud in the slice point cloud library is:

[0033]

[0034] Based on the relative pose transformation matrix T and the following formula:

[0035] Y = atan2(r) 21 ,r 11 )

[0036] The x-axis translation amount x is obtained T y-axis translation amount y T The rotation angle Y around the z-axis is the yaw angle, where T is the transformation matrix obtained by ICP matching, and r 11 For the data in the first row and first column of the relative pose transformation matrix T, r 21 This represents the data in the second row and first column of the relative pose transformation matrix T.

[0037] The specific process for obtaining the ICP score in the above technical solution is as follows:

[0038]

[0039] In the formula, |p s | represents the number of points in the slice point cloud Cnh in the slice point cloud library; Let i be the i-th point of the point cloud C in the previous slice; Let i be the i-th point of the slice point cloud Cnh;

[0040] Where R represents the rotation matrix; t represents the translation vector:

[0041]

[0042] The above design is used to obtain the pose after lidar repositioning.

[0043] Example 2

[0044] A method for lidar relocalization based on a 3D point cloud map of a substation, such as... Figure 2 As shown,

[0045] The specific method for lidar repositioning includes the following steps:

[0046] The existing 3D model of the substation is used to construct a 3D point cloud map of the substation, including LiDAR, through a model-to-point cloud algorithm.

[0047] The three-dimensional point cloud map of the substation containing lidar is sliced ​​along the vertical gravitational acceleration direction to obtain sliced ​​point clouds, and a sliced ​​point cloud library is constructed based on the sliced ​​point clouds;

[0048] The roll angle R and pitch angle P of the lidar are obtained by using the built-in gyroscope of the lidar.

[0049] Using the lidar as the origin, slice the lidar point cloud at the current moment perpendicular to the direction of gravitational acceleration to obtain the current slice point cloud C;

[0050] Perform ICP matching between the current slice point cloud C and the slice point clouds in the slice point cloud library to obtain the relative pose transformation matrix T and ICP score between the current slice point cloud C and the slice point clouds in the library. Select the slice point cloud with the lowest ICP score that is less than the threshold as the matched slice point cloud C. mh ;

[0051] Based on the relative pose transformation matrix T, the translation amount x of the lidar in the x-axis direction on the substation ground is obtained. T The translational amount y of the lidar on the ground in the y-axis direction of the substation. T The yaw angle Y of the lidar, where the translation amount x T Translation amount y T and slice point cloud C mh The altitude mh represents the position after repositioning by the lidar, and the roll angle R, pitch angle P, and yaw angle Y represent the attitude after repositioning by the lidar.

[0052] Example 3

[0053] A computer program product includes a computer program that, when executed by a processor, implements the steps of the method described in Embodiment 2.

[0054] Example 4

[0055] An electronic device includes a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the computer instructions to perform the steps of the method as described in Embodiment 2.

[0056] Example 5

[0057] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the method as described in Embodiment 2.

[0058] The contents not described in detail in this specification are existing technologies known to those skilled in the art.

Claims

1. A lidar relocation system based on a 3D point cloud map of a substation, characterized in that, It includes: The point cloud map loading module is used to construct a 3D point cloud map of a substation, including LiDAR, from an existing 3D substation model using a model-to-point cloud algorithm. The slice point cloud library construction module is used to slice the three-dimensional point cloud map of the substation containing lidar in the vertical gravity acceleration direction to obtain slice point clouds, and construct a slice point cloud library based on the slice point clouds; The lidar attitude acquisition module is used to obtain the lidar's roll angle R and pitch angle P through the lidar's built-in gyroscope. The point cloud slice acquisition module is used to slice the point cloud of the lidar at the current moment with the lidar as the origin and perpendicular to the direction of gravitational acceleration, and obtain the current slice point cloud C; The point cloud matching module performs ICP matching between the current slice point cloud C and the slice point clouds in the slice point cloud library, obtaining the relative pose transformation matrix T and ICP score between the current slice point cloud C and the slice point clouds in the slice point cloud library. The slice point cloud with the lowest ICP score and less than a threshold is selected as the matched slice point cloud C. mh ; The lidar repositioning acquisition module is used to calculate the lidar's translation x along the x-axis of the substation ground based on the relative pose transformation matrix T. T The translational amount y of the lidar on the ground in the y-axis direction of the substation. T The yaw angle Y of the lidar, where the translation amount x T Translation amount y T and slice point cloud C mh The altitude mh represents the position after repositioning by the lidar, and the roll angle R, pitch angle P, and yaw angle Y represent the attitude after repositioning by the lidar.

2. The lidar repositioning system based on a three-dimensional point cloud map of a substation according to claim 1, characterized in that: The three-dimensional model of the substation was constructed by using a three-dimensional laser scanner to scan the substation's equipment, facilities, and structures, and then using three-dimensional reality modeling software.

3. The lidar repositioning system based on a three-dimensional point cloud map of a substation according to claim 1, characterized in that: The specific method for slicing the 3D point cloud map of the substation along the direction of vertical gravitational acceleration and constructing the sliced ​​point cloud library is as follows: The already constructed 3D point cloud map of the substation is sliced ​​along the vertical direction of gravity acceleration. The interval between each point cloud slice is h, and the slice thickness is t. The slice point clouds C0, Ch, C2h...Cnh are obtained, and the center height of the slice point cloud is 0, h, 2h...nh respectively. The completed 3D point cloud map of the substation has the z-axis in the opposite direction of gravitational acceleration and the xoy plane as the substation ground. All slice point clouds are stored to build a slice point cloud library.

4. The lidar repositioning system based on a three-dimensional point cloud map of a substation according to claim 1, characterized in that: The built-in gyroscope reads the roll angle R and pitch angle P of the lidar. The roll angle R is the rotation angle around the x-axis, and the pitch angle P is the rotation angle around the y-axis.

5. The lidar repositioning system based on a three-dimensional point cloud map of a substation according to claim 1, characterized in that: The relative pose transformation matrix T between the current slice point cloud C and each slice point cloud in the slice point cloud library is: Based on the relative pose transformation matrix T and the following formula: Y=atan2 r 21 ,r 11 ) The x-axis translation amount x is obtained T y-axis translation amount y T The rotation angle Y around the z-axis is the yaw angle, where T is the transformation matrix obtained by ICP matching, and r 11 For the data in the first row and first column of the relative pose transformation matrix T, r 21 This represents the data in the second row and first column of the relative pose transformation matrix T.

6. The lidar repositioning system based on a three-dimensional point cloud map of a substation according to claim 1 or 5, characterized in that: The specific process for obtaining the ICP score is as follows: In the formula, |p s | represents the number of points in the slice point cloud Cnh in the slice point cloud library; Let i be the i-th point of the point cloud C in the previous slice; Let i be the i-th point of the slice point cloud Cnh; Where R represents the rotation matrix and t represents the translation vector:

7. A method for lidar relocalization based on a 3D point cloud map of a substation, characterized in that, It involves the following steps: The existing 3D model of the substation is used to construct a 3D point cloud map of the substation, including LiDAR, through a model-to-point cloud algorithm. The three-dimensional point cloud map of the substation containing lidar is sliced ​​along the vertical gravitational acceleration direction to obtain sliced ​​point clouds, and a sliced ​​point cloud library is constructed based on the sliced ​​point clouds; The roll angle R and pitch angle P of the lidar are obtained by using the built-in gyroscope of the lidar. Using the lidar as the origin, slice the lidar point cloud at the current moment perpendicular to the direction of gravitational acceleration to obtain the current slice point cloud C; Perform ICP matching between the current slice point cloud C and the slice point clouds in the slice point cloud library to obtain the relative pose transformation matrix T and ICP score between the current slice point cloud C and the slice point clouds in the library. Select the slice point cloud with the lowest ICP score that is less than the threshold as the matched slice point cloud C. mh ; Based on the relative pose transformation matrix T, the translation amount x of the lidar in the x-axis direction on the substation ground is obtained. T The translational amount y of the lidar on the ground in the y-axis direction of the substation. T The yaw angle Y of the lidar, where the translation amount x T Translation amount y T and slice point cloud C mh The altitude mh represents the position after repositioning by the lidar, and the roll angle R, pitch angle P, and yaw angle Y represent the attitude after repositioning by the lidar.

8. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method described in claim 7.

9. An electronic device, characterized in that, include: A memory and a processor are communicatively connected, the memory storing computer instructions, and the processor executing the computer instructions to perform the steps of the method as described in claim 7.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, it implements the steps of the method as described in claim 7.

Images