Laser radar repositioning system and method based on three-dimensional point cloud map of transformer substation
Patent Information
- Application Number
- CN202410988677.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-23
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-07-23
Smart Images

Figure CN118915079A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of three-dimensional laser point cloud rapid repositioning, and in particular to a system and method for laser radar repositioning based on a three-dimensional point cloud map of a substation. Background Art
[0002] The current mainstream solution for the substation positioning problem of robots is: in a high-precision map, the robot matches the current map with radar data or odometer information to obtain its precise position in the map, which makes it convenient for staff to obtain the robot's operating position in a timely manner and ensures that the robot works safely and stably. However, in actual applications, most robots need to be manually given the initial position of the robot in the map. The consequence of this is that when the robot fails to match itself, or after power failure and restart, the robot will cause global positioning failure, affecting the normal operation of the robot.
[0003] Robot relocalization technology can overcome this problem. When the robot loses its global positioning, it can be quickly re-matched through relocalization to obtain the robot's precise position in the map, solving the problem of the robot's global positioning being lost and unable to be restored. This is of great significance to the stable and safe operation of the robot.
[0004] However, the traditional point cloud matching algorithm directly matches 3D and 3D point clouds, but the amount of 3D point cloud data is huge. When the map is large, it will occupy a large amount of memory and hard disk. At the same time, the processing speed is not fast enough and cannot be applied to high-speed motion scenes. Therefore, it is urgent to propose a fast global repositioning method so that when the robot's global positioning is lost, it can quickly achieve accurate global repositioning of the robot. Summary of the invention
[0005] The purpose of the present invention is to provide a system and method for laser radar repositioning based on a three-dimensional point cloud map of a substation. The present invention greatly improves the efficiency of laser radar repositioning.
[0006] To achieve this purpose, the present invention designs a laser radar relocation system based on a three-dimensional point cloud map of a substation, which includes:
[0007] The point cloud map loading module is used to construct a three-dimensional point cloud map of the substation containing lidar by converting the constructed three-dimensional model of the substation through the model-to-point cloud algorithm;
[0008] The slicing point cloud library construction module is used to slice the three-dimensional point cloud map of the substation containing the laser radar in the direction of vertical gravity acceleration to obtain a slicing point cloud, and to construct a slicing point cloud library according to the slicing point cloud;
[0009] The laser radar attitude acquisition module is used to obtain the roll angle R and pitch angle P of the laser radar through the built-in gyroscope of the laser radar;
[0010] The slicing point cloud acquisition module is used to slice the point cloud of the laser radar at the current moment perpendicular to the direction of gravity acceleration with the laser radar as the origin to obtain the current slice point cloud C;
[0011] The point cloud matching module is used to perform ICP matching on the current slice point cloud C and the slice point clouds in the slice point cloud library, and obtain 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 the threshold is selected as the matched slice point cloud C. mh ;
[0012] The laser radar repositioning acquisition module is used to obtain the laser radar's translation amount x in the x-axis direction on the ground of the substation according to the relative posture conversion matrix T. T , the translation amount y of the laser radar in the y-axis direction on the ground of the substation T , the yaw angle Y of the laser radar, where the translation x T , translation y T And slice point cloud C mh The height mh is the position of the laser radar after relocation, and the roll angle R, pitch angle P and yaw angle Y are the postures of the laser radar after relocation.
[0013] Beneficial effects of the present invention:
[0014] The large amount of calculation in 3D and 3D point cloud matching is simplified to approximately finite (n) 2D and 2D point cloud matching, that is, the current slice point cloud is matched with the slice point cloud in the slice point cloud library by ICP matching, which reduces the amount of calculation from n times of 3D and 3D point cloud matching to n times. 2 The number of times is reduced to n times between 2D and 2D, which greatly reduces the amount of calculation, improves the speed of positioning calculation, and improves the efficiency of lidar repositioning. BRIEF DESCRIPTION OF THE DRAWINGS
[0015] Figure 1 It is a structural schematic diagram of the present invention;
[0016] Figure 2 is a flow chart of the present invention;
[0017] Figure 3 Schematic diagram of point cloud slicing. DETAILED DESCRIPTION
[0018] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:
[0019] Example 1
[0020] A laser radar relocation system based on a three-dimensional 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 constructed substation 3D model into a point cloud algorithm to construct a substation 3D point cloud map containing LiDAR (the scope of the substation 3D point cloud is generally bounded by the substation wall);
[0022] The slicing point cloud library construction module is used to slice the substation 3D point cloud map containing the laser radar in the vertical gravity acceleration direction to obtain a slice point cloud, and to construct a slice point cloud library based on the slice point cloud. The substation 3D point cloud with a large amount of data is converted into a 2D approximate slice point cloud with a small amount of data and a limited number in advance, so that the calculation amount is reduced from n to n. 2 The number of times is reduced to n times between 2D and 2D. This design is used to simplify the calculation process and directly calculate the slice point cloud, greatly reducing the amount of calculation;
[0023] The laser radar attitude acquisition module is used to obtain the roll angle R and pitch angle P of the laser radar through the built-in gyroscope of the laser radar. This design is used to obtain two of the three values represented by the final positioning laser radar attitude;
[0024] The slicing point cloud acquisition module is used to slice the point cloud of the laser radar at the current moment perpendicular to the direction of gravity acceleration with the laser radar as the origin, and obtain the current slicing point cloud C. The laser radar is held in the substation to collect the point cloud of the laser radar at the current moment. This design is used to obtain the current slicing point cloud C, and then perform ICP matching with the slicing point cloud in the slicing point cloud library;
[0025] The point cloud matching module is used to perform ICP matching (a method of point cloud registration, where lipointmatcher is an open source library with a relatively high degree of completion of the point cloud ICP registration algorithm. Two point clouds are input, and then a relative pose transformation matrix T is obtained, which can make one point cloud overlap with the other point cloud as much as possible after the relative pose transformation matrix changes) between the current slice point cloud C and the slice point cloud in the slice point cloud library. The relative pose transformation matrix T and ICP score are obtained, and the slice point cloud Cmh with the lowest ICP score and less than the threshold (the threshold is 0.03) is selected as the matching slice point cloud Cmh (if the slice point cloud does not meet the minimum ICP score and is less than the threshold at the same time, it is recalculated until it is satisfied). A limited number of slices are pre-stored, and the current slice point cloud is compared with the stored slice point cloud for matching, and the most similar one is selected, that is, the current slice point cloud and the matching slice point cloud are at the same height, within the allowable error range. This design can not only ensure accuracy, but also reduce the amount of calculation, and the calculation result has better real-time performance;
[0026] The laser radar repositioning acquisition module is used to obtain the laser radar's translation amount x in the x-axis direction on the ground of the substation according to the relative posture conversion matrix T. T , the translation amount y of the laser radar in the y-axis direction on the ground of the substation T , the yaw angle Y of the laser radar, where the translation x T , translation y T And slice point cloud C mh The height mh is the position of the laser radar after relocation, the roll angle R, pitch angle P and yaw angle Y are the postures of the laser radar after relocation, and the relocation posture of the laser radar is (x T ,y T , mh, R, P, Y), that is, the laser radar coordinates are (x T ,y T , mh), the attitude is the roll angle R, pitch angle P, yaw angle Y. This design is used to calculate the position and attitude after the laser radar is relocated, and the final calculation result is the laser radar positioning result.
[0027] In the above technical solution, the three-dimensional model of the substation is obtained by laser scanning the substation equipment, facilities and structures with a three-dimensional laser scanner, and then the three-dimensional model is constructed using three-dimensional real-scene modeling software.
[0028] In the above technical solution, the three-dimensional point cloud map of the substation is sliced in the direction perpendicular to the gravity acceleration, and the specific method for constructing the sliced point cloud library is:
[0029] The constructed three-dimensional point cloud map of the substation is sliced in the direction perpendicular to the gravity acceleration. The interval between each point cloud is h, the slice thickness is t, and the slice point clouds C0, Ch, C2h...Cnh are obtained. The center heights of the slice point clouds are 0, h, 2h...nh respectively.
[0030] The z-axis of the constructed substation three-dimensional point cloud map is the opposite direction of gravity acceleration, the xoy plane is the substation ground, all slice point clouds are stored, and a slice point cloud library is constructed. 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 longitudinal direction of the substation ground.
[0031] In the above technical solution, the built-in gyroscope reads the roll angle R and pitch angle P of the laser radar, wherein 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] According to the relative posture transformation matrix T and the following formula:
[0035] Y = atan2(r 21 ,r 11 )
[0036] Get the x-axis translation x T , y-axis translation y T , rotated around the z axis by an angle Y, that is, the yaw angle is Y, where T is the transformation matrix obtained by ICP matching, r 11 is the data of the first row and first column in the relative posture transformation matrix T, r 21 It is the data in the second row and first column of the relative posture transformation matrix T.
[0037] In the above technical solution, the specific process of obtaining the ICP score is as follows:
[0038]
[0039] In the formula, |p s | is the number of points of the slice point cloud Cnh in the slice point cloud library; is the i-th point of the previous slice point cloud C; is 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 position and posture after the lidar is relocated.
[0043] Example 2
[0044] A method for laser radar relocation based on substation 3D point cloud map, such as Figure 2 As shown,
[0045] The specific method of laser radar relocation includes the following steps:
[0046] The substation 3D model that has been built is used to construct a 3D point cloud map of the substation containing LiDAR through the model-to-point cloud algorithm;
[0047] Slicing the three-dimensional point cloud map of the substation containing the laser radar in a direction perpendicular to gravity acceleration to obtain a slice point cloud, and constructing a slice point cloud library according to the slice point cloud;
[0048] The roll angle R and pitch angle P of the laser radar are obtained through the built-in gyroscope of the laser radar;
[0049] Taking the laser radar as the origin, slice the laser radar point cloud at the current moment perpendicular to the direction of gravity acceleration to obtain the current slice point cloud C;
[0050] Perform ICP matching on the current slice point cloud C and the slice point clouds in the slice point cloud library respectively 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 slice point cloud library. Select the slice point cloud with the lowest ICP score and less than the threshold as the matched slice point cloud C. mh ;
[0051] According to the relative posture conversion matrix T, the translation amount x of the laser radar in the x-axis direction of the substation ground is obtained. T , the translation amount y of the laser radar in the y-axis direction on the ground of the substation T , the yaw angle Y of the laser radar, where the translation x T , translation y T And slice point cloud C mh The height mh is the position of the laser radar after relocation, and the roll angle R, pitch angle P and yaw angle Y are the postures of the laser radar after relocation.
[0052] Example 3
[0053] A computer program product comprises a computer program, wherein when the computer program is executed by a processor, the steps of the method described in Embodiment 2 are implemented.
[0054] Example 4
[0055] An electronic device comprises: a memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the steps of the method described in Example 2 by executing the computer instructions.
[0056] Example 5
[0057] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method described in Example 2 are implemented.
[0058] The contents not described in detail in this specification belong to the prior art known to professional and technical personnel in this field.
Claims
1. A laser radar relocation system based on a three-dimensional point cloud map of a substation, characterized in that: It includes: The point cloud map loading module is used to construct a three-dimensional point cloud map of the substation containing lidar by converting the constructed three-dimensional model of the substation through the model-to-point cloud algorithm; The slicing point cloud library construction module is used to slice the three-dimensional point cloud map of the substation containing the laser radar in the direction of vertical gravity acceleration to obtain a slicing point cloud, and to construct a slicing point cloud library according to the slicing point cloud; The laser radar attitude acquisition module is used to obtain the roll angle R and pitch angle P of the laser radar through the built-in gyroscope of the laser radar; The slicing point cloud acquisition module is used to slice the point cloud of the laser radar at the current moment perpendicular to the direction of gravity acceleration with the laser radar as the origin to obtain the current slice point cloud C; The point cloud matching module is used to perform ICP matching on the current slice point cloud C and the slice point clouds in the slice point cloud library, and obtain 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 the threshold is selected as the matched slice point cloud C. mh ; The laser radar repositioning acquisition module is used to obtain the laser radar's translation amount x in the x-axis direction on the ground of the substation according to the relative posture conversion matrix T. T , the translation amount y of the laser radar in the y-axis direction on the ground of the substation T , the yaw angle Y of the laser radar, where the translation x T , translation y T And slice point cloud C mh The height mh is the position of the laser radar after relocation, and the roll angle R, pitch angle P and yaw angle Y are the postures of the laser radar after relocation.
2. The system for laser radar relocation based on substation three-dimensional point cloud map according to claim 1 is characterized in that: 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 real-scene modeling software to construct the three-dimensional model.
3. The system for laser radar relocation based on substation three-dimensional point cloud map according to claim 1 is characterized in that: The three-dimensional point cloud map of the substation is sliced in the direction perpendicular to the gravity acceleration. The specific method for constructing the sliced point cloud library is as follows: The constructed three-dimensional point cloud map of the substation is sliced in the direction perpendicular to the gravity acceleration. The interval between each point cloud is h, the slice thickness is t, and the slice point clouds C0, Ch, C2h...Cnh are obtained. The center heights of the slice point clouds are 0, h, 2h...nh respectively. The z-axis of the constructed three-dimensional point cloud map of the substation is the opposite direction of gravity acceleration, the xoy plane is the ground of the substation, all slice point clouds are stored, and a slice point cloud library is constructed.
4. The system for laser radar relocation based on substation three-dimensional point cloud map according to claim 1 is characterized in that: The built-in gyroscope reads the roll angle R and pitch angle P of the laser radar, 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.
5. The system for laser radar relocation based on substation three-dimensional point cloud map according to claim 1 is 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: According to the relative posture transformation matrix T and the following formula: Y=atan2 r 21 ,r 11 ) Get the x-axis translation x T , y-axis translation y T , rotated around the z axis by an angle Y, that is, the yaw angle is Y, where T is the transformation matrix obtained by ICP matching, r 11 is the data of the first row and first column in the relative posture transformation matrix T, r 21 It is the data in the second row and first column of the relative posture transformation matrix T.
6. The system for laser radar relocation based on substation three-dimensional point cloud map according to claim 1 or 5, characterized in that: The specific process of obtaining the ICP score is as follows: In the formula, |p s | is the number of points of the slice point cloud Cnh in the slice point cloud library; is the i-th point of the previous slice point cloud C; is 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 laser radar relocation based on a three-dimensional point cloud map of a substation, characterized in that: It follows the steps: The substation 3D model that has been built is used to construct a 3D point cloud map of the substation containing LiDAR through the model-to-point cloud algorithm; Slicing the three-dimensional point cloud map of the substation containing the laser radar in a direction perpendicular to gravity acceleration to obtain a slice point cloud, and constructing a slice point cloud library according to the slice point cloud; The roll angle R and pitch angle P of the laser radar are obtained through the built-in gyroscope of the laser radar; Taking the laser radar as the origin, slice the laser radar point cloud at the current moment perpendicular to the direction of gravity acceleration to obtain the current slice point cloud C; Perform ICP matching on the current slice point cloud C and the slice point clouds in the slice point cloud library respectively 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 slice point cloud library. Select the slice point cloud with the lowest ICP score and less than the threshold as the matched slice point cloud C. mh ; According to the relative posture conversion matrix T, the translation amount x of the laser radar in the x-axis direction of the substation ground is obtained. T , the translation amount y of the laser radar in the y-axis direction on the ground of the substation T , the yaw angle Y of the laser radar, where the translation x T , translation y T And slice point cloud C mh The height mh is the position of the laser radar after relocation, and the roll angle R, pitch angle P and yaw angle Y are the postures of the laser radar after relocation.
8. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method described in claim 7 are implemented.
9. An electronic device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the steps of the method as claimed in claim 7 by executing the computer instructions.
10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to claim 7 are implemented.
