Construction site robot positioning method based on laser radar

Abstract

The invention relates to a construction site robot positioning method based on a laser radar, and the method comprises the following steps: S1, obtaining a feature point pair: extracting a matching feature point pair from point cloud data of a front frame and a rear frame collected in a preset time interval; and S2, estimating the pose of the robot: carrying out inversion on the position difference between the two feature points in the feature point pair to obtain the pose change of the laser light source, namely, the pose of the construction site robot and a transformation matrix of the position. Therefore, attitude estimation of the construction site robot is completed. According to the method, the pose of the construction site robot can be autonomously calculated through the laser radar installed on the construction site robot in place, data acquisition and data calculation are completed on the construction site robot body, and external input or external assistance is not needed, so that the stability of a pose calculation task can be ensured.

Description

Technical Field

[0001] The present invention relates to the technical field of robot posture positioning, and in particular to a construction site robot positioning method based on laser radar. Background Art

[0002] The autonomous positioning capability of a mobile robot is the key to its autonomy and directly affects the safety of its movements. As a mobile robot working in a special environment, construction robots have even higher requirements for their autonomous positioning capabilities. Currently, most outdoor mobile robots rely on satellite signals for autonomous positioning. However, due to the large number of obstructions on construction sites, satellite signals are shielded, making it impossible to achieve stable autonomous positioning. Using its own sensors to estimate the robot's mobile posture is the key to solving this problem. Summary of the invention

[0003] In view of the problem of insufficient stability of current robot autonomous positioning devices, the present invention provides a local construction site robot positioning method based on laser radar.

[0004] The present application provides a construction site robot positioning method based on laser radar, comprising the following steps: S1, step of obtaining feature point pairs: extracting matching feature point pairs from point cloud data of two frames before and after collected within a preset time interval; S2, robot posture estimation step: invert the position difference between two feature points in the feature point pair to obtain the posture change of the laser light source, that is, the posture and position transformation matrix of the construction site robot, thereby completing the posture estimation of the construction site robot.

[0005] Preferably, the step S1 of acquiring feature point pairs includes the following sub-steps: S11, point cloud two-dimensional mapping step: mapping the three-dimensional point cloud spatial information into a two-dimensional depth map; S12, image feature extraction step: applying an image feature extraction algorithm to identify and extract all feature points of a two-dimensional depth map; S13, feature point matching step: applying the geometric planarity consistency principle to two frames of two-dimensional depth maps corresponding to the point cloud data of the two frames before and after collected within a preset time interval to obtain feature point pairs, and construct a set of feature point pairs.

[0006] Preferably, in the step S13 of feature point matching, Extracted in sequence Frame and The set of two-dimensional depth feature points of the frame is and , in the corresponding two-dimensional depth map, the depth values ​​corresponding to the two points can be quickly indexed and , calculate the deviation from geometric flatness: ; If the geometric flatness deviation of the current feature point pair satisfies , retain the feature point pairs, otherwise remove the feature point pairs; among them, is the geometric flatness consistency threshold.

[0007] Preferably, in the step of S11, two-dimensional point cloud mapping, Point Cloud , and are respectively the highest and lowest vertical field of view angles of the current laser radar, is the number of laser radar lines, is the number of points per circle of the laser radar. According to the position of the point cloud in three-dimensional space and its generation principle, each point cloud Row index in 2D Can be defined as:

[0008] in, For point The Euclidean distance to the origin can be expressed as ; Likewise, each point cloud Column index in 2D Can be defined as:

[0009] Therefore, the encoding result of each point cloud in two-dimensional space can be expressed as .

[0010] The result is saved in a two-dimensional matrix to obtain a two-dimensional depth map of the three-dimensional point cloud data.

[0011] Preferably, the step S1 of acquiring feature point pairs includes the following sub-steps: S11, point cloud two-dimensional mapping step: mapping the three-dimensional point cloud reflection intensity information into a two-dimensional intensity map; S12, image feature extraction step: applying an image feature extraction algorithm to identify and extract all feature points of a frame of a two-dimensional intensity image; S13, feature point matching step: applying the reflection intensity consistency principle to two frames of two-dimensional intensity images corresponding to the point cloud data of the two frames before and after collected within a preset time interval to obtain feature point pairs, and construct a set of feature point pairs.

[0012] Preferably, in the step S13 of feature point matching, Extracted in sequence Frame and The set of two-dimensional intensity feature points of the frame is and , in the corresponding two-dimensional intensity map, the reflection intensity values ​​corresponding to the two points can be quickly indexed and , the deviation of the reflection intensity values ​​at two points It can be defined as:

[0013] in, and The most rapid way to index the reflection intensity value to the intensity value corresponding to two points and , If the deviation of the reflection intensity value of the current feature point pair satisfies , retain the feature point pairs, otherwise remove the feature point pairs; among them, is the geometric flatness consistency threshold.

[0014] Preferably, the step of S11 of two-dimensional point cloud mapping further includes the step of correcting the reflection intensity of the two-dimensional intensity map: Based on the Gaussian distribution relationship between the incident angle and the reflection intensity value of the point cloud, the reflection intensity value of the point cloud is corrected. , the measured reflection intensity value is , the measured reflection intensity value is Corrected to :

[0015] in, is the incident angle, Expressed as the corrected reflection intensity value, and is the parameter of the given reflected intensity Gaussian distribution.

[0016] Preferably, in the step of S11 two-dimensional mapping of the point cloud, the reflection intensity value is stored as The two-dimensional intensity map of is obtained as follows: Point Cloud , and are respectively the highest and lowest vertical field of view angles of the current laser radar, is the number of laser radar lines, is the number of points per circle of the laser radar. According to the position of the point cloud in three-dimensional space and its generation principle, each point cloud Row index in 2D Can be defined as:

[0017] in, For point The Euclidean distance to the origin can be expressed as ; Likewise, each point cloud Column index in 2D Can be defined as:

[0018] The encoding result of the reflection intensity of each point cloud in two-dimensional space can be expressed as .

[0019] Preferably, the step S1 of acquiring feature point pairs includes the following sub-steps: S11, point cloud two-dimensional mapping step: mapping the three-dimensional point cloud spatial information into a two-dimensional depth map, and mapping the three-dimensional point cloud reflection intensity information into a two-dimensional intensity map; S12, image feature extraction step: applying an image feature extraction algorithm to identify and extract all feature points of a frame of a two-dimensional depth map, and obtain all feature points of a corresponding two-dimensional intensity map; S13, feature point matching step: construct a set of feature point pairs for two frames of two-dimensional depth maps corresponding to the point cloud data of the two frames before and after collected within a preset time interval, wherein the feature point pairs simultaneously satisfy the geometric flatness consistency principle and the reflection intensity consistency principle.

[0020] Preferably, the step S2 of estimating the robot's posture performs iterative closest point registration on the selected frame feature point pairs to obtain the construction site robot posture estimation results of two adjacent frames. .

[0021] The present invention can realize autonomous solution of the posture of the construction site robot through the laser radar installed on the construction site robot. Since data acquisition and data solution are completed on the construction site robot body, no external input or external assistance is required, thus ensuring the stability of the posture solution task. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 This is a general schematic diagram of the laser radar-based construction site robot positioning method of the present application; Figure 2 This is a schematic diagram of the laser radar point cloud mapping of this application; Figure 3 This is a flow chart of an embodiment of a method for positioning a construction robot based on laser radar according to the present application; Figure 4 Schematic diagram of posture estimation of the laser radar-based construction robot positioning method of the present application. DETAILED DESCRIPTION

[0023] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. In this specification, the size ratios in the drawings do not represent the actual size ratios, but are only used to reflect the relative position relationship and connection relationship between the components. Components with the same name or the same number represent similar or identical structures and are only for illustrative purposes.

[0024] Figure 1 The overall flow chart of the laser radar-based construction site robot positioning method of the present application is as follows.

[0025] S1. Step of obtaining feature point pairs: extracting matching feature point pairs from the point cloud data of the two frames collected within a certain period of time.

[0026] S2. Estimation of robot posture step. The position difference between two feature points in the feature point pair is inverted by the corresponding algorithm, and the posture change of the light source is obtained, that is, the posture and position transformation matrix of the construction robot with the laser radar fixed. Thus, the posture estimation of the construction robot is completed. Furthermore, the end position after the frame ends is obtained by multiplying the initial position of the construction robot before the start of the frame by the corresponding transformation matrix.

[0027] In the step of obtaining feature point pairs in S1, in order to improve the processing speed and reduce the calculation load, the three-dimensional point cloud data can be first converted into two-dimensional point cloud data. Then, the feature points of the single frame data are obtained based on the two-dimensional point cloud data. Based on the obtained feature points, the image features of the two frames before and after a certain period of time are matched to obtain a set of matching feature point pairs.

[0028] Specifically, the process first includes a step of S11 point cloud two-dimensional mapping. Figure 2 , point cloud , and are respectively the highest and lowest vertical field of view angles of the current laser radar, For the laser radar harness, is the number of points per circle of the laser radar. According to the position of the point cloud in three-dimensional space and its generation principle, each point cloud Row index in 2D Can be defined as:

[0029] in, For point The Euclidean distance to the origin can be expressed as ; Likewise, each point cloud Column index in 2D Can be defined as:

[0030] Therefore, the encoding result of each point cloud in two-dimensional space can be expressed as .

[0031] The result is saved in a two-dimensional matrix to obtain a two-dimensional depth map of the three-dimensional point cloud data.

[0032] Perform S12 image feature extraction step based on the two-dimensional depth map to obtain feature points of the two-dimensional depth map. This process can be based on the existing feature extraction method based on the ORB image feature extraction algorithm to identify and extract all feature points in a frame of data.

[0033] Finally, the feature point matching step S13 is performed in the two frames before and after to obtain the feature point pairs that meet the conditions. The geometric flatness consistency principle is applied here. For the two frames of data before and after, the extracted Frame and The set of two-dimensional depth feature points of the frame is and ,,In the corresponding depth map, the depth values ​​corresponding to the two points can be quickly indexed and , with each point as the center, can be respectively Frame and In the depth map of the frame, construct the neighborhood depth mean for each point and calculate the deviation from geometric flatness:

[0034] If the geometric flatness deviation of the current feature point pair satisfies This preset value indicates that the spatial distance information of the feature point pair matches, and the feature point pair meets the requirements and is retained; otherwise, if it does not meet the conditions, it means that there is a large difference in the geometric flatness information of the current feature point pair, which does not meet the consistency requirements of the feature point pair, and the feature point pair is removed. is the geometric flatness consistency threshold, which can be measured through a large number of experimental statistics.

[0035] In this way, the final feature point pairs can be determined, and multiple feature point pairs constitute the required set of feature point pairs.

[0036] We also provide a second technical solution for the step S1 of obtaining feature point pairs, which performs feature matching based on the reflection intensity information of the point cloud.

[0037] It first constructs the corresponding reflection intensity matrix according to the laser point cloud index relationship in the previous scheme, that is, the two-dimensional intensity map I of the point cloud. For this application, the two-dimensional intensity map and the two-dimensional depth map D are structurally consistent, except that the data elements of one are intensity values ​​and the data elements of the other are depth values.

[0038] S11 Point cloud 2D mapping step.

[0039] Spatial encoding of point cloud, mapping 3D point cloud to 2D plane, and constructing 2D intensity map of point cloud. , and are respectively the highest and lowest vertical field of view angles of the current laser radar, For the laser radar harness, is the number of points per circle of the laser radar. According to the position of the point cloud in three-dimensional space and its generation principle, each point cloud Row index in 2D Can be defined as:

[0040] in, For point The Euclidean distance to the origin can be expressed as ; Similarly, each point cloud Column index in 2D Can be defined as:

[0041] The encoding result of the reflection intensity of each point cloud in two-dimensional space can be expressed as , the original three-dimensional point cloud reflection intensity can be converted into a two-dimensional space representation, which reduces the storage space of the point cloud and facilitates the search of the neighborhood point clouds of each point cloud.

[0042] It is particularly important to note that the point cloud intensity needs to be corrected.

[0043] Based on the Gaussian distribution relationship between the incident angle and the reflection intensity value of the point cloud, the reflection intensity value of the point cloud is corrected and a two-dimensional reflection intensity map of the point cloud is constructed. , the measured reflection intensity value is Since there is a nearly Gaussian distribution relationship between the incident angle and the reflection intensity of the point cloud, the measured reflection intensity value can be Corrected to :

[0044] in, It is expressed as the angle of incidence, which is generally known and can be replaced by the laser line launch angle. Expressed as the corrected reflection intensity value. and The parameters (standard deviation, mean) of the Gaussian distribution of the reflection intensity mainly include the mean, which can be obtained through a large number of experimental measurements.

[0045] If necessary, the angle of incidence can be calculated Abandoning the time-consuming plane fitting method, the present invention adopts the eight-neighborhood approximate fitting method, using the calculated two-dimensional space encoding results to search the point cloud The eight points around it are used to calculate the incident angle :

[0046] Will Substitute the incident angle into the correction formula to obtain the corrected reflection intensity value. ,Right now . That is, the corrected two-dimensional intensity map.

[0047] S12 image feature extraction step.

[0048] Perform S12 image feature extraction step based on the two-dimensional intensity map to obtain feature points of the two-dimensional intensity map. This process can be based on the existing feature extraction method based on the ORB image feature extraction algorithm to identify and extract all feature points in a frame of data.

[0049] S13: feature point matching step.

[0050] According to the principle of consistency of reflection intensity, the extracted feature point pairs are screened. Frame and The two-dimensional intensity feature point pair of the frame is and , the reflection intensity values ​​corresponding to the two points can be quickly indexed in the reflection intensity map obtained in S22 and , the deviation of the reflection intensity values ​​at two points It can be defined as:

[0051] in, and is the maximum and minimum value of the reflection intensity value, which can be obtained by analyzing a large number of multi-frame lidar point cloud reflection intensity data; If the deviation of the reflection intensity value of the current feature point pair satisfies , indicating that the feature point pair not only matches the spatial distance information, but also matches the reflection intensity information. This pair of feature points meets the requirements and will be retained. On the contrary, if it does not meet the conditions, it means that there is a large difference in the reflection intensity information of the current feature point pair, which does not meet the consistency requirements of the feature point pair, and the pair of feature points will be removed. Among them, is the reflection intensity consistency threshold, which can be measured through a large number of experimental statistics.

[0052] In this way, the final feature point pairs can be determined, and multiple feature point pairs constitute the required set of feature point pairs.

[0053] like Figure 3 As shown, the present application also provides a technical solution combining the two embodiments, that is, applying a two-dimensional depth map and a two-dimensional intensity map at the same time, which simultaneously performs the operations in the above embodiments, and determines the set of feature point pairs in the S13 feature point matching step to meet the following criteria. That is, the deviation of the reflection intensity value of the current feature point pair satisfies This preset value, and at the same time, the geometric flatness deviation of the current feature point pair satisfies This default value.

[0054] S2. Estimating robot pose step.

[0055] The existing ICP (Iterative Closest Point) algorithm is used to perform iterative closest point registration on the selected feature point pairs of two adjacent frames to obtain the pose estimation results of the construction site robot in two adjacent frames. That is, the transformation matrix of the lens (construction site robot) is obtained through the ICP algorithm. If its initial posture in the first frame image is , then the posture corresponding to the second frame image is .

[0056] like Figure 4 , if it is a multi-step situation, to For 5 moments, to is the pose estimation result between two adjacent moments of these five moments, to Multiply it to get the current time. From the initial position The position change result of the current construction site robot can be obtained.

[0057] Therefore, the current pose estimation result By multiplying the pose estimation result of the historical frame, we can get the autonomous positioning result of the current construction site robot:

[0058] in, This is the autonomous positioning result of the current construction site robot for the current final output.

[0059] The above content is only a description of the preferred embodiments of the present invention, and does not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. A construction site robot positioning method based on laser radar, characterized in that: The steps include: S1, step of obtaining feature point pairs: extracting matching feature point pairs from point cloud data of two frames before and after collected within a preset time interval; S2, robot posture estimation step: invert the position difference between two feature points in the feature point pair to obtain the posture change of the laser light source, that is, the posture and position transformation matrix of the construction site robot, and complete the posture estimation of the construction site robot.

2. The method for positioning a construction site robot based on laser radar as claimed in claim 1, characterized in that: The step S1 of obtaining feature point pairs includes the following sub-steps: S11, point cloud two-dimensional mapping step: mapping the three-dimensional point cloud spatial information into a two-dimensional depth map; S12, image feature extraction step: applying an image feature extraction algorithm to identify and extract all feature points of a two-dimensional depth map; S13, feature point matching step: applying the geometric planarity consistency principle to two frames of two-dimensional depth maps corresponding to the point cloud data of the two frames before and after collected within a preset time interval to obtain feature point pairs, and construct a set of feature point pairs.

3. The method for positioning a construction robot based on laser radar as claimed in claim 2, characterized in that: In the step S13 of feature point matching, Extracted in sequence Frame and The set of two-dimensional depth feature points of the frame is and , in the corresponding two-dimensional depth map, the depth values ​​corresponding to the two points can be quickly indexed and , calculate the deviation from geometric flatness: ； If the geometric flatness deviation of the current feature point pair satisfies , retain the feature point pairs, otherwise remove the feature point pairs; among them, is the geometric flatness consistency threshold.

4. The method for positioning a construction site robot based on laser radar as claimed in claim 2, characterized in that: In the step of S11, two-dimensional point cloud mapping, Point Cloud , and are respectively the highest and lowest vertical field of view angles of the current laser radar, is the number of laser radar lines, is the number of points per circle of the laser radar. According to the position of the point cloud in three-dimensional space and its generation principle, each point cloud Row index in 2D Can be defined as: in, For point The Euclidean distance to the origin can be expressed as ; Similarly, each point cloud Column index in 2D Defined as: Therefore, the encoding result of each point cloud in two-dimensional space can be expressed as . The result is saved in a two-dimensional matrix to obtain a two-dimensional depth map of the three-dimensional point cloud data.

5. The method for positioning a construction site robot based on laser radar as claimed in claim 1, characterized in that: The step S1 of obtaining feature point pairs includes the following sub-steps: S11, point cloud two-dimensional mapping step: mapping the three-dimensional point cloud reflection intensity information into a two-dimensional intensity map; S12, image feature extraction step: applying an image feature extraction algorithm to identify and extract all feature points of a frame of a two-dimensional intensity image; S13, feature point matching step: applying the reflection intensity consistency principle to two frames of two-dimensional intensity images corresponding to the point cloud data of the two frames before and after collected within a preset time interval to obtain feature point pairs, and construct a set of feature point pairs.

6. The method for positioning a construction robot based on laser radar as claimed in claim 5, characterized in that: In the step S13 of feature point matching, Extracted in sequence Frame and The set of two-dimensional intensity feature points of the frame is and , in the corresponding two-dimensional intensity map, the reflection intensity values ​​corresponding to the two points can be quickly indexed and , the deviation of the reflection intensity values ​​at two points It can be defined as: in, and The most rapid way to index the reflection intensity value to the intensity value corresponding to two points and , If the deviation of the reflection intensity value of the current feature point pair satisfies , retain the feature point pairs, otherwise remove the feature point pairs; among them, is the geometric flatness consistency threshold.

7. The method for positioning a construction robot based on laser radar as claimed in claim 5, characterized in that: The step of S11 two-dimensional point cloud mapping also includes a step of correcting the reflection intensity of the two-dimensional intensity map: Based on the Gaussian distribution relationship between the incident angle and the reflection intensity value of the point cloud, the reflection intensity value of the point cloud is corrected. , the measured reflection intensity value is , the measured reflection intensity value is Corrected to : in, is the incident angle, Expressed as the corrected reflection intensity value, and is the parameter of the given reflected intensity Gaussian distribution.

8. The method for positioning a construction robot based on laser radar as claimed in claim 7, characterized in that: In the S11 point cloud two-dimensional mapping step, the reflection intensity value is saved as The two-dimensional intensity map of is obtained as follows: Point Cloud , and are respectively the highest and lowest vertical field of view angles of the current laser radar, is the number of laser radar lines, is the number of points per circle of the laser radar. According to the position of the point cloud in three-dimensional space and its generation principle, each point cloud Row index in 2D Can be defined as: in, For point The Euclidean distance to the origin can be expressed as ; Similarly, each point cloud Column index in 2D Can be defined as: The encoding result of the reflection intensity of each point cloud in two-dimensional space can be expressed as .

9. The method for positioning a construction site robot based on laser radar as claimed in claim 1, characterized in that: The step S1 of obtaining feature point pairs includes the following sub-steps: S11, point cloud two-dimensional mapping step: mapping the three-dimensional point cloud spatial information into a two-dimensional depth map, and mapping the three-dimensional point cloud reflection intensity information into a two-dimensional intensity map; S12, image feature extraction step: applying an image feature extraction algorithm to identify and extract all feature points of a frame of a two-dimensional depth map, and obtain all feature points of a corresponding two-dimensional intensity map; S13, feature point matching step: construct a set of feature point pairs for two frames of two-dimensional depth maps corresponding to the point cloud data of the two frames before and after collected within a preset time interval, wherein the feature point pairs simultaneously satisfy the geometric flatness consistency principle and the reflection intensity consistency principle.

10. The method for positioning a construction robot based on laser radar as claimed in claim 1, characterized in that: The step S2 of estimating the robot's posture performs iterative closest point registration on the selected frame feature point pairs to obtain the construction site robot posture estimation results of two adjacent frames. .

Images