Method and System for Reconstructing Mine Scenes by a Mobile Robot Based on SLAM

Abstract

The present disclosure provides a method and system for reconstructing a mine scene by a mobile robot based on SLAM. The method includes the steps of acquiring laser point cloud data and visual point cloud data measured by a mobile robot and synchronously calibrated, fusing the acquired laser point cloud data and visual point cloud data, performing a point cloud motion distortion removal process and a point cloud filtering process on the fused point cloud data, and using a multiple constraint factor graph algorithm based on graph optimization based on the processed point cloud data, adding IMU pre-integration data, point cloud keyframe data and GNSS data to a constraint factor graph and performing loop closure detection to obtain a reconstructed 3D map. The present disclosure can effectively realize 3D reconstruction of a mine scene, and finally obtain a colored point cloud map, thereby improving the accuracy of the mine scene reconstruction.

Description

Technical Field

[0001] The present disclosure relates to the technical field of scene reconstruction, and in particular, to a method and a system for reconstructing a mine scene by a mobile robot based on SLAM.

Background Art

[0002] The description of this part is only for providing the background art related to the present disclosure, and it is not necessarily a constituent of the prior art.

[0003] With the advent of the 5G generation and the era of autonomous driving, the technology of simultaneous localization and mapping (SLAM) has been widely applied in the fields of positioning for autonomous driving, high-precision map collection, augmented reality (AR), and geodetic map creation work.

[0004] According to the inventor's discovery, in the mine environment, there are a lot of dust and suspended particulate matter in the air, so the data collection by lidar and vision sensors has low accuracy, and the application of SLAM to the mine scene is greatly limited. In addition, the lidar can only obtain the distance information and photometric information of the object, and cannot obtain the color information of the object. Therefore, the generated three-dimensional reconstructed map has insufficient information volume, cannot be accurately displayed, and cannot meet the requirements for remote operation of construction machinery in the virtual scene. On the other hand, although the vision camera can obtain a rich colored point cloud, due to the low accuracy of the measurement data, a single sensor alone cannot meet the requirements for creating a SLAM map in the mine environment.

Summary of the Invention

[0005] In order to solve the deficiencies of the prior art, the present disclosure provides a method and a system for reconstructing a mine scene by a mobile robot based on SLAM, which can effectively realize three-dimensional reconstruction in the mine scene, finally obtain a colored point cloud map, and improve the accuracy of the mine scene reconstruction.

[0006] To achieve the above object, the present disclosure adopts the following technical means.

[0007] The first aspect of the present disclosure is obtaining synchronized laser point cloud data and visual point cloud data measured by a mobile robot; fusing the obtained laser point cloud data and visual point cloud data; performing point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data; based on the processed point cloud data, using a multiple-constraint factor graph algorithm based on graph optimization, adding IMU (Inertial Measurement Unit) pre-integration data, point cloud key frame data, and GNSS (Global Navigation Satellite System) data to the constraint factor graph, and performing loop closure detection to obtain a reconstructed 3D map; A method for reconstructing a mine scene by a mobile robot based on SLAM is provided, including the above steps.

[0008] Furthermore, iterative optimization is performed using the feature information of the current laser frame and the feature information of the map, adding IMU pre-integration data, point cloud key frame data, and GNSS data as factors of the graph optimization algorithm to the factor graph, and performing factor graph optimization to update all key frame poses and finally obtain the optimized poses and the reconstructed 3D map.

[0009] Furthermore, find key frames from past key frames, perform matching of point clouds of the current frame and multiple frames near the key frame to obtain transformation poses, construct closed-loop factor graph data, and perform optimization in addition to adding to the factor graph.

[0010] Furthermore, among the past key frames, frames with a distance smaller than a predetermined value and a time interval larger than a predetermined value are used as key frames.

[0011] Furthermore, the step of fusing the acquired laser point cloud data and visual point cloud data uses the timestamp interpolation algorithm to obtain the matched laser point cloud data and visual point cloud data, projects the laser point cloud onto the camera plane by the keyhole imaging method, and matches the laser point cloud projected onto the camera plane and the visual point cloud according to the XYZ coordinates to obtain the fused point cloud PointCloud:XYZRIRGB, where X is the x-axis coordinate of the laser point cloud, Y is the y-axis coordinate of the laser point cloud, Z is the z-axis coordinate of the laser point cloud, I is the photometric intensity of the laser point cloud, R is the beam number of each laser beam, and RGB is the color information of the visual point cloud.

[0012] Furthermore, the point cloud motion distortion removal process takes the start time of the laser scan of one frame as time T and the end time of the laser scan of this frame as time T+ΔT, performs pre-integration on the angular velocity data and acceleration data of the three axes of the IMU, obtains the relative motion of the robot within the period ΔT, and then converts all the laser points within the period ΔT to the first laser point, thereby realizing the laser point cloud motion distortion removal.

[0013] Furthermore, the point cloud filtering process is the step of projecting the laser point cloud for each frame as a depth image. Assuming that the scanning beam of the multi-beam laser radar is r beams and n laser points are obtained by the scanning of each beam for each frame, the depth image becomes an r×n matrix, and the coordinate information and photometric intensity information of the point cloud are included in each matrix, obtains the positional relationship between adjacent point clouds from the depth image, searches for adjacent points of the point cloud using the kd-tree search algorithm, and performs filtering processing on the ground points and noise according to the geometric relationship between the point clouds.

[0014] The second aspect of the present disclosure is a data acquisition module arranged to acquire laser point cloud data and visual point cloud data measured by a mobile robot that are synchronously calibrated, a point cloud fusion module arranged to fuse the acquired laser point cloud data and visual point cloud data, a point cloud data processing module arranged to perform point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data, a three-dimensional map reconstruction module arranged to add IMU pre-integration data, point cloud key frame data, and GNSS data to a constraint factor graph using a multi-constraint factor graph algorithm based on graph optimization based on the processed point cloud data, and perform loop closure detection to obtain a reconstructed three-dimensional map, and provides a mine scene reconstruction system by a mobile robot based on SLAM comprising the above.

[0015] The third aspect of the present disclosure is a computer-readable storage medium storing a program, which, when executed by a processor, realizes the steps in the method for reconstructing a mine scene by a mobile robot based on SLAM described in the first aspect of the present disclosure, and provides a computer-readable storage medium.

[0016] The fourth aspect of the present disclosure is an electronic device including a memory, a processor, and a program stored in the memory and executable by the processor, which, when the program is executed by the processor, realizes the steps in the method for reconstructing a mine scene by a mobile robot based on SLAM described in the first aspect of the present disclosure, and provides an electronic device.

[0017] Compared with the prior art, the present disclosure has the following beneficial effects.

[0018] 1. According to the method, system, medium or electronic device described in the present disclosure, three-dimensional reconstruction in a mine scene can be effectively realized, and finally, a colored point cloud map can be obtained, improving the accuracy of the scene reconstruction of the mine.

[0019] 2. According to the method, system, medium or electronic device described in the present disclosure, integration of multiple types of sensors is realized by a lidar, IMU, Beidou positioning system, and visual camera, compensating for the drawback of low accuracy in map creation by a single sensor in complex scenes such as mines, and realizing stable and highly reliable SLAM map creation.

[0020] 3. According to the method, system, medium or electronic device described in the present disclosure, a three-dimensional reconstruction map with color information can be constructed by fusing the point clouds of the lidar and the visual camera, meeting the requirements of remote immersive construction.

[0021] 4. According to the method, system, medium or electronic device described in the present disclosure, for a mine environment, 3D laser point clouds are projected as depth images, and ground points are extracted. For the situation of a dusty environment in the mine, accurate filtering processing against noise is realized by a clustering algorithm.

[0022] 5. According to the method, system, medium or electronic device described in the present disclosure, using the GTSAM open source library, IMU pre-integration factors, laser point clouds, Beidou positioning information, and visual point clouds are added to constraint factors to realize real-time update of poses and accurate map creation.

[0023] Advantages of additional aspects of the present disclosure are partly provided in the following description, partly clarified by the following description, or understood by the practice of the present disclosure.

[0024] The accompanying drawings that form a part of this disclosure are provided to offer a further understanding of this disclosure. The schematic examples and their descriptions of this disclosure are for interpreting this disclosure and do not unduly limit this disclosure.

Brief Description of the Drawings

[0025]

Figure 1

Figure 2

Figure 3

Figure 4

Modes for Carrying Out the Invention

[0026] Hereinafter, this disclosure will be further described with reference to the accompanying drawings and examples.

[0027] It should be noted that all of the following detailed descriptions are exemplary and are intended to provide a further explanation of this disclosure. Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by those skilled in the technical field to which this disclosure belongs.

[0028] It should be noted that the terms used here are merely for explaining specific embodiments and are not intended to limit the exemplary embodiments according to this disclosure. As used here, unless otherwise specified in the context, the singular forms are intended to include the plural forms. Also, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, mechanisms, members, and / or combinations thereof.

[0029] Unless there is a contradiction, the embodiments in the present disclosure and the features in the embodiments can be combined with each other.

Embodiment

[0030] As shown in FIG. 1, Embodiment 1 of the present disclosure provides a method for reconstructing a mine scene by a mobile robot based on SLAM, which includes the following steps.

[0031] In S1, synchronization and calibration of the laser point cloud and the visual point cloud are performed by a timestamp, a new data structure is defined, and fusion of the point cloud data is realized.

[0032] In S2, motion distortion removal is performed on the laser point cloud by fusing a multi-beam laser radar and an IMU, and point cloud filtering processing is performed on the mine scene.

[0033] In S3, using a multi-constraint factor graph algorithm based on graph optimization, constraint information such as an IMU, a laser radar, and a GNSS is added to a constraint factor graph, and backend loop closure detection and map creation are realized.

[0034] S1 mainly includes the following content.

[0035] As shown in FIG. 2, synchronization and calibration of the laser point cloud and the visual point cloud are performed by a timestamp, a new data structure is defined, and fusion of the point cloud data is realized.

[0036] Through a GNSS timing system, time hard synchronization is performed on the laser radar and the camera, and spatial calibration is performed on the laser radar and the camera by fixing a calibration board. After obtaining an external parameter matrix, the camera is projected into the laser radar coordinate system. Using the PCL point cloud library, a new point cloud data structure is defined as follows. PointCloud:XYZRIRGB (the data structure of the laser point cloud is PointCloud:XYZRI).

[0037] X is the x-axis coordinate of the laser point cloud, Y is the y-axis coordinate of the laser point cloud, Z is the z-axis coordinate of the laser point cloud, I is the luminous intensity of the laser point cloud, R is the beam number of each laser beam, and RGB is the color information of the visual point cloud.

[0038] When the time and space calibration of the lidar and the camera is completed, the laser data for each frame and the visual camera data for each frame are held in a container, and the time stamp interpolation algorithm is used to obtain the matched laser point cloud and visual point cloud. By the keyhole imaging method, the laser point cloud is projected onto the camera plane. According to the XYZ coordinates, the laser point cloud projected onto the camera plane and the visual point cloud are matched to obtain the fused point cloud. In this fused point cloud, the laser point cloud data is XYZRI, the visual point cloud is RGB information, and since the RGB information is only used for subsequent map creation, it can be treated as a laser point cloud. Due to the operating mechanism of the camera, the colored point cloud is only a small part of the entire laser point cloud, so hereinafter, it is still collectively referred to as the laser point cloud.

[0039] S2 mainly includes the following content.

[0040] Taking the start time of the laser scan of one frame as time T and the end time of the laser scan of this frame as time T+ΔT, performing pre-integration on the angular velocity and acceleration data of the three axes of the IMU, and after obtaining the relative motion (including linear motion and non-linear motion) of the robot within the period ΔT, by converting all the laser points within the period ΔT to the first laser point, motion distortion removal of the laser point cloud is realized. By this method, the motion distortion of the laser point cloud can be effectively removed.

[0041] The PCL library projects the laser point cloud for each frame as a depth image. Let the scanning beam of the multi-beam laser radar be the r beam, and assume that n laser points are obtained by scanning with each beam for each frame. In this case, the depth image is an r×n matrix, and each matrix contains the coordinate information and photometric information of the point cloud. From the depth image, the positional relationship between adjacent point clouds can be obtained, and the adjacent points of the point cloud are searched using the kd-tree search algorithm. Filtering processing is performed on the ground points and noise according to the geometric relationship between the point clouds.

[0042] Regarding the geometric relationship for ground point removal, as shown in Figure 3, the center of the coordinate system is at the geometric center of the laser radar.

[0043]

Number

[0044] The geometric relationship for related noise removal is shown in Figure 4.

[0045]

Number

[0046] S3 mainly includes the following content.

[0047] Using a multi-constraint factor graph algorithm based on graph optimization, constraint information such as IMU, laser point cloud, and GNSS is added to the constraint factor graph to realize backend loop closure detection and map creation.

[0048] By scan-to-map, repeated optimization is performed using the feature information of the current laser frame and the map, and all key frame poses are updated.

[0049] Using the gtsam open-source library, IMU pre-integrated data, key frame data, and GNSS data are added to the factor graph as factors of the graph optimization algorithm, and by performing factor graph optimization, all key frame poses are updated.

[0050] Among the past key frames in the past about 20s, frames with a short distance and a long time interval are selected as key frames, and by performing matching of the point clouds of the current frame and multiple frames near the key frame, a transformation pose is obtained, a closed-loop factor graph data is constructed, and optimization is performed in addition to the factor graph.

[0051] Using the gtsam open-source library, the final optimized pose is obtained and the construction of the 3D point cloud is realized.

[0052] The fused point cloud has the characteristics that the measurement data of the laser point cloud is accurate while the color and texture of the visual point cloud are rich, and finally, a 3D reconstructed map with color information can be obtained.

Example

[0053] Example 2 of the present disclosure is A data acquisition module arranged to acquire synchronized laser point cloud data and visual point cloud data measured by a mobile robot, A point cloud fusion module arranged to fuse the acquired laser point cloud data and visual point cloud data; A point cloud data processing module arranged to perform point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data; Based on the processed point cloud data, using a multiple-constraint factor graph algorithm based on graph optimization, adding IMU pre-integration data, point cloud key frame data, and GNSS data to a constraint factor graph, and performing loop closure detection to obtain a reconstructed 3D map, a 3D map reconstruction module arranged as such; Provided is a scene reconstruction system for a mine by a mobile robot based on SLAM, comprising the above.

[0054] The operation method of the said system is the same as the scene reconstruction method for a mine by a mobile robot based on SLAM provided by Example 1, so it will not be repeatedly described here.

Example

[0055] Example 3 of the present disclosure is a computer-readable storage medium storing a program, and when the program is executed by a processor, it realizes the steps in the scene reconstruction method for a mine by a mobile robot based on SLAM described in Example 1 of the present disclosure, providing a computer-readable storage medium.

Example

[0056] Example 4 of the present disclosure is an electronic device including a memory, a processor, and a program stored in the memory and executable by the processor, and when the program is executed by the processor, it realizes the steps in the scene reconstruction method for a mine by a mobile robot based on SLAM described in Example 1 of the present disclosure, providing an electronic device.

[0057] As will be understood by those skilled in the art, the embodiments of the present disclosure can be provided as a method, system, or computer program product. Accordingly, the present disclosure may be in the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware. Additionally, the present disclosure may be a computer program product implemented on one or more computer-readable storage media (including, but not limited to, magnetic disk memory and optical memory) containing computer-usable program code.

[0058] The present disclosure has been described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present disclosure. It should be noted that each step and / or block in the flowcharts and / or block diagrams, as well as the combinations of steps and / or blocks in the flowcharts and / or block diagrams, may be implemented by computer program commands. These computer program commands can be provided to the processors of general-purpose computers, special-purpose computers, embedded processors, or other programmable data processing devices to generate an apparatus for realizing the functions specified by one or more steps in the flowchart and / or one or more blocks in the block diagram by the commands executed by the processors of the computer or other programmable data processing devices.

[0059] These computer program commands may be stored in a computer-readable memory that can guide a computer or other programmable data processing device to perform specific operations. Thereby, a product including a command apparatus for realizing the functions specified by one or more steps in the flowchart and / or one or more blocks in the block diagram is generated by the commands stored in the computer-readable memory.

[0060] These computer program commands may be installed in a computer or other programmable data processing device to cause a series of operation steps to be executed on the computer or other programmable device, thereby generating processing by the computer. Thus, the commands executed on the computer or other programmable device provide steps for realizing the functions specified by one or more steps of the flowchart and / or one or more blocks of the block diagram.

[0061] As can be understood by those skilled in the art, all or some of the steps in the method of the above embodiments may be completed by instructing the relevant hardware by a computer program. The said program may be stored in a computer-readable storage medium. The program may include the steps of the embodiments of the above respective methods when executed. The said storage medium may be a magnetic disk, an optical disk, a read-only memory (ROM), a random access memory (RAM), or the like.

[0062] The preferred embodiments of the present disclosure have been described above, but these are not for limiting the present disclosure. For those skilled in the art, the present disclosure is capable of various changes and modifications. Any changes, equivalent substitutions, improvements, etc. made without departing from the spirit and principles of the present disclosure should all be included within the protection scope of the present disclosure.

Claims

1. A step of acquiring laser point cloud data and visual point cloud data measured by a mobile robot that are synchronously corrected; A step of fusing the acquired laser point cloud data and visual point cloud data; A step of performing point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data; Based on the processed point cloud data, using a multiple constraint factor graph algorithm based on graph optimization, adding IMU pre-integration data, point cloud key frame data, and GNSS data to the constraint factor graph, and performing loop closure detection to obtain a reconstructed 3D map; including The point cloud filtering process is A step of projecting the laser point cloud for each frame as a depth image. Assuming that the scanning beam of the multi-beam laser radar is r beams, and n laser points are obtained by the scanning of each beam for each frame, the depth image becomes an r×n matrix, and the coordinate information and photometric information of the point cloud are included in each matrix; Obtaining the positional relationship between adjacent point clouds from the depth image, using the kd-tree search algorithm to search for adjacent points of the point cloud, and performing filtering processing on the ground points and noise according to the geometric relationship between the point clouds; The step of performing filtering processing on the ground points and noise is Regarding the geometric relationship for ground point removal, the center of the coordinate system is at the geometric center of the laser radar, 【Number 3】 However, OA and OB are the distances at which the laser points of two adjacent laser beams reach the ground at the same time, α and β are the angles between the two laser beams and the horizontal plane. Since the ground of the mine is uneven, γ is smaller than 2.5°, and the height difference between two adjacent points, that is, ΔZ is smaller than 5 cm, it is regarded as a ground point. Calculations are performed for each angle α and ΔZ. When the point cloud is calculated as a ground point, the ground point is removed by filtering. The geometric relationship for removing related noise is [Article 4] However, OA and OB are the depths of two laser beams. Considering the large amount of dust in the mine scene, the threshold value of θ is set as Θ. When θ > Θ, it is regarded as an outlier value; when θ < Θ, it is regarded as the same object. Calculate respectively based on the rows and columns of the depth image. When the number of laser points regarded as the same object exceeds 30, it is regarded as the same object. A method for reconstructing a mine scene by a mobile robot based on SLAM, characterized in that.

2. Repeated optimization is performed using the feature information of the current laser frame and the feature information of the map. The IMU pre-integrated data, point cloud key frame data, and GNSS data are added to the factor graph as factors of the factor graph optimization algorithm. By executing the factor graph optimization, all key frame poses are updated, and finally the optimized pose and the reconstructed 3D map are obtained. A method for reconstructing a mine scene by a mobile robot based on SLAM according to claim 1, characterized in that.

3. Find key frames from past key frames, obtain the transformation pose by performing matching on the point clouds of the current frame and multiple frames near the key frame, construct the closed-loop factor graph data, and perform optimization in addition to the factor graph. A method for reconstructing a mine scene by a mobile robot based on SLAM according to claim 2, characterized in that.

4. Among the past key frames, frames with a distance smaller than a predetermined value and a time interval larger than a predetermined value are used as key frames. A method for reconstructing a mine scene by a mobile robot based on SLAM according to claim 3, characterized in that.

5. The step of fusing the acquired laser point cloud data and visual point cloud data is Using a timestamp interpolation algorithm, obtain the matched laser point cloud data and visual point cloud data. Project the laser point cloud onto the camera plane by the keyhole imaging method, and match the laser point cloud projected onto the camera plane and the visual point cloud according to the XYZ coordinates to obtain the fused point cloud PointCloud: XYZRIRGB. X is the x-axis coordinate of the laser point cloud, Y is the y-axis coordinate of the laser point cloud, Z is the z-axis coordinate of the laser point cloud, I is the photometric intensity of the laser point cloud, R is the beam number of each laser beam, and RGB is the color information of the visual point cloud. The method for reconstructing a mine scene by a mobile robot based on SLAM according to claim 1, characterized in that.

6. The point cloud motion distortion removal process is Taking the start time of the laser scan of one frame as time T and the end time of the laser scan of this frame as time T+ΔT, perform pre-integration on the angular velocity data and acceleration data of the 3 axes of the IMU. After obtaining the relative motion of the robot within the period ΔT, realize the removal of laser point cloud motion distortion by converting all laser points within the period ΔT to the first laser point. The method for reconstructing a mine scene by a mobile robot based on SLAM according to claim 1, characterized in that.

7. A data acquisition module arranged to acquire laser point cloud data and visual point cloud data measured by a mobile robot that are synchronously calibrated, A point cloud fusion module arranged to fuse the acquired laser point cloud data and visual point cloud data, A point cloud data processing module arranged to perform point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data, Based on the processed point cloud data, using a multi-constraint factor graph algorithm based on graph optimization, add IMU pre-integration data, point cloud key frame data, and GNSS data to the constraint factor graph, and perform loop closure detection to obtain a reconstructed 3D map. A 3D map reconstruction module arranged as such. equipped with The point cloud filtering process is A step of projecting the laser point cloud for each frame as a depth image. Assuming that the scanning beam of the multi-beam laser radar is r beams, and n laser points are obtained by scanning with each beam for each frame, the depth image becomes an r×n matrix, and the coordinate information and photometric information of the point cloud are included in each matrix. Obtain the positional relationship between adjacent point clouds from the depth image, use the kd-tree search algorithm to search for adjacent points of the point cloud, and perform filtering processing on the ground points and noise according to the geometric relationship between the point clouds. The step of performing filtering processing on the ground points and noise is Regarding the geometric relationship for ground point removal, the center of the coordinate system is at the geometric center of the laser radar. [Equation 5] However, OA and OB are the distances at which the laser points of two adjacent laser beams reach the ground at the same time, α and β are the angles between the two laser beams and the horizontal plane. Since the ground of the mine is uneven, γ is smaller than 2.5°, and when the height difference between two adjacent points, that is, ΔZ, is smaller than 5 cm, it is regarded as a ground point. Calculate for each angle α and ΔZ. When the point cloud is calculated as a ground point, remove the ground point by filtering. The geometric relationship for related noise removal is [Equation 6] However, OA and OB are the depths of two laser beams. Considering that there is a lot of dust in the mine scene, the threshold value of θ is set as Θ. When θ > Θ, it is regarded as an outlier value. When θ < Θ, it is regarded as the same object. It is calculated respectively by the rows and columns of the depth image. When the number of laser points regarded as the same object exceeds 30, it is regarded as the same object. A mine scene reconstruction system using a mobile robot based on SLAM, characterized by the above. Claim 8 A computer-readable storage medium storing a program, wherein when the program is executed by a processor, the steps in the method for reconstructing a mine scene by a mobile robot based on SLAM according to any one of claims 1 to 6 are realized. A computer-readable storage medium, characterized by the above. Claim 9 An electronic device including a memory, a processor, and a program stored in the memory and executable by the processor, wherein when the program is executed by the processor, the steps in the method for reconstructing a mine scene by a mobile robot based on SLAM according to any one of claims 1 to 6 are realized. An electronic device, characterized by the above.

Images