WO2022257801A1

WO2022257801A1 - Slam-based mobile robot mine scene reconstruction method and system

Info

Publication number: WO2022257801A1
Application number: PCT/CN2022/095982
Authority: WO
Inventors: 周军; 赵一凡; 欧金顺; 皇攀凌; 孟广辉; 高新彪; 李留昭; 林乐彬
Original assignee: 山东大学; 山东亚历山大智能科技有限公司
Priority date: 2021-06-09
Filing date: 2022-05-30
Publication date: 2022-12-15
Also published as: CN113379910B; CN113379910A

Abstract

The present disclosure provides a SLAM-based mobile robot mine scene reconstruction method and system: acquiring synchronously calibrated laser point cloud data and visual point cloud data measured by a mobile robot; fusing the acquired 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; using a graph optimization-based multi-constraint factor graph algorithm in combination with the processed point cloud data, adding IMU pre-integration data, point cloud key frame data and GNSS data into a constraint subgraph, and performing loop closure detection to obtain a reconstructed 3D map; the present disclosure can effectively implement 3D reconstruction in a mine scenario, and ultimately obtain a point cloud map with color, improving the precision of mine scene reconstruction.

Description

Mine scene reconstruction method and system for mobile robot based on SLAM

technical field

The present disclosure relates to the technical field of scene reconstruction, in particular to a SLAM-based mobile robot mine scene reconstruction method and system.

Background technique

The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art.

With the advent of the 5G era and the unmanned driving era, Simultaneous Localization and Mapping (SLAM) technology has been widely used in unmanned driving positioning, high-precision map collection, AR (Augmented Reality, augmented reality), surveying and mapping and other fields. Wide range of applications.

The inventors found that in the mine environment, due to the large amount of dust and floating particles in the air, the data collection of lidar and visual sensors is not accurate, which greatly restricts the application of SLAM in mine scenarios; in addition, lidar can only obtain The distance information and light intensity information of the object cannot obtain the color information of the object, and the generated 3D reconstruction map is not rich enough to meet the needs of people remotely commanding construction machinery in the virtual scene, while the visual camera can obtain rich colored point clouds. But its measurement data is not accurate enough, therefore, a single sensor cannot meet the needs of SLAM mapping in the mine environment.

Contents of the invention

In order to solve the deficiencies of the prior art, the present disclosure provides a SLAM-based mobile robot mine scene reconstruction method and system, which can effectively realize the three-dimensional reconstruction in the mine scene, and finally obtain a colored point cloud map, which improves the mine environment. Accuracy of scene reconstruction.

In order to achieve the above purpose, the present disclosure adopts the following technical solutions:

The first aspect of the present disclosure provides a SLAM-based mobile robot mine scene reconstruction method.

A SLAM-based mobile robot mine scene reconstruction method, including the following processes:

Obtain synchronously calibrated laser point cloud data and visual point cloud data measured by mobile robots;

Fusion of acquired laser point cloud data and visual point cloud data;

Perform point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data;

Combined with the processed point cloud data, using a multi-constraint factor graph algorithm based on graph optimization, the IMU (Inertial Measurement Unit, inertial measurement unit) pre-integration data, point cloud key frame data and GNSS (Global Navigation Satellite System, global navigation satellite system) data into the constrained submap, and the reconstructed 3D map is obtained after loop closure detection.

Further, use the feature information of the current laser frame and the feature information of the map to iteratively optimize, add the IMU pre-integration data, point cloud key frame data and GNSS data as the factors of the graph optimization algorithm to the factor graph, and execute the factor graph Optimize, update all key frame poses, and finally get the optimized pose and reconstructed 3D map

Further, look for keyframes in historical keyframes, match the current frame with several frames of point clouds near the keyframes, obtain transformed poses, construct closed-loop factor graph data, and add factor graphs for optimization.

Furthermore, among the historical key frames, frames whose distance is smaller than a preset value and whose time interval is larger than a preset value are used as key frames.

Further, the acquired laser point cloud data and visual point cloud data are fused, including:

Use the time stamp interpolation algorithm to obtain matching laser point cloud data and visual point cloud data, use the principle of pinhole imaging to project the laser point cloud to the camera plane, and project the laser point and visual point cloud on the camera plane according to the XYZ coordinates Perform matching to obtain the fused point cloud PointCloud: XYZRIRGB;

Among them, 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 light intensity of the laser point cloud, and R is the beam number of each laser beam , RGB is the color information of the visual point cloud.

Further, point cloud motion distortion removal processing includes:

Assuming that time T is the start time of a frame of laser scanning, and time T+ΔT is the end time of laser scanning of this frame, pre-integrate the angular velocity data and acceleration data of the three axes of the IMU to obtain the relative motion of the robot within ΔT time, and then calculate the relative motion of the robot within ΔT time All the laser points of the laser point are converted to the first laser point to realize the removal of laser point cloud motion distortion.

Further, point cloud filtering processing includes:

Project each frame of laser point cloud into a depth image, the scanning line bundle of multi-line lidar is r lines, and each line scans each frame to obtain n laser points, and the depth image is an r*n matrix, each matrix contains the point cloud Coordinate information and light intensity information;

Use the depth image to obtain the positional relationship between adjacent point clouds, and use the kd-tree search algorithm to search the adjacent points of the point cloud, and filter the ground points and noise points according to the geometric relationship between the point clouds.

The second aspect of the present disclosure provides a SLAM-based mobile robot mine scene reconstruction system.

A SLAM-based mobile robot mine scene reconstruction system, including:

The data acquisition module is configured to: acquire synchronously calibrated laser point cloud data and visual point cloud data measured by the mobile robot;

The point cloud fusion module is configured to: fuse the acquired laser point cloud data and visual point cloud data;

The point cloud data processing module is configured to: carry out point cloud motion distortion removal processing and point cloud filter processing to the point cloud data after fusion;

The 3D map reconstruction module is configured to: combine the processed point cloud data, adopt a multi-constraint factor graph algorithm based on graph optimization, add IMU pre-integration data, point cloud key frame data and GNSS data into the constraint subgraph, and perform loop closure detection After that, the reconstructed 3D map is obtained.

The third aspect of the present disclosure provides a computer-readable storage medium, on which a program is stored, and when the program is executed by a processor, the steps in the SLAM-based mobile robot mine scene reconstruction method described in the first aspect of the present disclosure are implemented. .

The fourth aspect of the present disclosure provides an electronic device, including a memory, a processor, and a program stored in the memory and operable on the processor, and the processor implements the program described in the first aspect of the present disclosure when executing the program. Steps in the SLAM-based mine scene reconstruction method for mobile robots.

Compared with the prior art, the beneficial effects of the present disclosure are:

1. The method, system, medium or electronic device described in the present disclosure can effectively realize the three-dimensional reconstruction in the mine scene, and finally obtain a colored point cloud map, which improves the accuracy of the mine scene reconstruction.

2. The method, system, medium or electronic device described in this disclosure uses laser radar, IMU, Beidou positioning, and visual cameras to achieve multi-sensor fusion, which makes up for the inaccurate shortcomings of a single sensor in complex scenes such as mines, and realizes Reliable and stable SLAM mapping.

3. The method, system, medium or electronic device described in the present disclosure integrates the laser radar and the visual camera point cloud to construct a three-dimensional reconstruction map with color information, which meets the needs of remote immersive construction.

4. The method, system, medium or electronic device described in this disclosure projects the 3D laser point cloud into a depth map for the mine environment, and extracts the ground points, and uses a clustering algorithm to realize Accurate filtering of noise.

5. The method, system, medium or electronic device described in this disclosure uses the GTSAM open source library to add IMU pre-integration factors, laser point clouds, Beidou positioning information, and visual point clouds to the constraint factors, realizing real-time pose update and precise mapping.

Advantages of additional aspects of the disclosure will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the disclosure.

Description of drawings

The accompanying drawings constituting a part of the present disclosure are used to provide a further understanding of the present disclosure, and the exemplary embodiments and descriptions of the present disclosure are used to explain the present disclosure, and do not constitute improper limitations to the present disclosure.

FIG. 1 is a schematic flowchart of a SLAM-based mobile robot mine scene reconstruction method provided by Embodiment 1 of the present disclosure.

FIG. 2 is a schematic diagram of a laser point cloud and visual point cloud fusion method provided by Embodiment 1 of the present disclosure.

FIG. 3 is a schematic diagram of the geometric relationship of ground points removed according to Embodiment 1 of the present disclosure.

FIG. 4 is a schematic diagram of a method for removing noise points by a clustering algorithm provided in Embodiment 1 of the present disclosure.

Detailed ways

The present disclosure will be further described below in conjunction with the accompanying drawings and embodiments.

It should be noted that the following detailed description is exemplary and intended to provide further explanation of the present disclosure. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure belongs.

It should be noted that the terminology used herein is only for describing specific embodiments, and is not intended to limit the exemplary embodiments according to the present disclosure. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural, and it should also be understood that when the terms "comprising" and/or "comprising" are used in this specification, they mean There are features, steps, operations, means, components and/or combinations thereof.

In the case of no conflict, the embodiments in the present disclosure and the features in the embodiments can be combined with each other.

Example 1:

As shown in Figure 1, Embodiment 1 of the present disclosure provides a method for reconstructing a mine scene based on SLAM for a mobile robot, including the following process:

S1: Synchronize and calibrate laser point cloud and visual point cloud by using time stamp, and define a new data structure to realize point cloud data fusion.

S2: Use the multi-line laser radar and IMU fusion to remove the motion distortion of the laser point cloud and filter the point cloud for the mine scene;

S3: Using a multi-constraint factor graph algorithm based on graph optimization, IMU, laser radar, GNSS, and other constraint information are added to the constraint subgraph to realize back-end loopback detection and map building.

In S1, it mainly includes the following contents:

The laser point cloud and the visual point cloud are synchronized and calibrated by using the time stamp, and a new data structure is defined to realize point cloud data fusion, as shown in Figure 2.

The GNSS timing system is used to perform time hard synchronization of the lidar and the camera, and the space calibration of the lidar and the camera is performed by using a fixed calibration plate. After obtaining the external parameter matrix, the camera is projected to the lidar coordinate system. Definition Use the PCL point cloud library to define a new point cloud data structure:

PointCloud: XYZRIRGB (the data structure of the laser point cloud is PointCloud: XYZRI).

After the laser radar and camera space-time calibration is completed, save each frame of laser data and each frame of visual camera data in the container, and use the time stamp interpolation algorithm to obtain the matching laser point cloud and visual point cloud, and use the principle of pinhole imaging , project the laser point cloud to the camera plane, and match the laser point projected to the camera plane with the visual point cloud according to the XYZ coordinates to obtain the fused point cloud. The fused point cloud XYZIR takes laser point cloud data, RGB information takes visual point cloud, and RGB information only participates in subsequent mapping, so it can still be processed as a laser point cloud. Due to the working principle of the camera, the colored point cloud is still a small part of the entire laser point cloud, so the laser point cloud is still used as a general term below.

In S2, it mainly includes the following contents:

Time T is the start time of a frame of laser scanning, and time T+ΔT is the end time of laser scanning of this frame. Pre-integrate the angular velocity and acceleration data of the three axes of the IMU to obtain the relative motion of the robot (including linear motion, nonlinear Movement), and then convert all the laser points within the ΔT time to the first laser point to realize the removal of laser point cloud motion distortion. This method can effectively remove the motion distortion of the laser point cloud.

Use the PCL library to project each frame of laser point cloud into a depth image. The scanning line beam of the multi-line lidar is r lines, and n laser points are obtained by scanning each line of each frame. Then the depth image is an r*n matrix, and each matrix contains coordinate information and light intensity information of the point cloud. The positional relationship between adjacent point clouds can be obtained by using the depth image, and the kd-tree search algorithm is used to search the adjacent points of the point cloud. According to the geometric relationship between the point clouds, the ground points and noise points are filtered.

Among them, the geometric relationship of removing ground points is shown in Figure 3, and the center of the coordinate system is at the geometric center of the lidar.

Among them, OA and OB are the distances between two adjacent laser beams hitting the ground at the same time, and α and β are the angles between the two laser beams and the horizontal plane. Since the mine ground is rough, when α<2.5°, and The height difference between two adjacent points, that is, Δ _z < 5cm, is regarded as a ground point. Calculate each angle α and Δ _z , and when the point cloud is calculated as a ground point, filter and remove the ground point.

Among them, the geometric relationship of related noise points is removed, as shown in FIG. 4 .

Among them, OA and OB are the depths of the two laser beams. Considering that there is a lot of dust in the mine scene, the threshold of α is set to A. When α>A, it is considered to be an outlier; when α<A, it is considered to be the same Objects are calculated separately in the rows and columns of the depth map. When the number of laser points considered to be the same object is >30, it is considered to be the same object.

In S3, it mainly includes the following contents:

Using a multi-constraint factor graph algorithm based on graph optimization, IMU, laser point cloud, GNSS and other constraint information are added to the constraint subgraph to realize back-end loopback detection and map building.

Use scan-to-map to iteratively optimize using the feature information of the current laser frame and the feature information of the map, and update all key frame poses;

Using the gtsam open source library, IMU pre-integration, lidar point cloud key frames, and GNSS information are used as factors of the graph optimization algorithm, added to the factor graph, and factor graph optimization is performed to update all key frame poses;

Among the historical key frames in the past 20s or so, look for frames with a closer distance and a longer time interval as the key frame, and match the current frame with the point clouds of several frames near the key frame to obtain the converted pose and construct a closed-loop factor map data, adding factor graphs for optimization.

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

The fused point cloud not only has the characteristics of accurate laser point cloud measurement data, but also has the characteristics of rich color and texture of visual point cloud, and finally a 3D reconstruction map with color information can be obtained.

Example 2:

Embodiment 2 of the present disclosure provides a SLAM-based mobile robot mine scene reconstruction system, including:

The point cloud data processing module is configured to: perform point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data;

The working method of the system is the same as the SLAM-based mobile robot mine scene reconstruction method provided in Embodiment 1, and will not be repeated here.

Example 3:

Embodiment 3 of the present disclosure provides a computer-readable storage medium on which a program is stored, and when the program is executed by a processor, the steps in the method for reconstructing a mine scene based on SLAM for a mobile robot as described in Embodiment 1 of the present disclosure are implemented. .

Example 4:

Embodiment 4 of the present disclosure provides an electronic device, including a memory, a processor, and a program stored in the memory and operable on the processor. When the processor executes the program, the implementation as described in Embodiment 1 of the present disclosure Steps in the SLAM-based mine scene reconstruction method for mobile robots.

Those skilled in the art should understand that the embodiments of the present disclosure may be provided as methods, systems, or computer program products. Accordingly, the present disclosure may take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present disclosure may take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to magnetic disk storage, optical storage, etc.) having computer-usable program code embodied therein.

The present disclosure is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the present disclosure. It should be understood that each procedure and/or block in the flowchart and/or block diagram, and a combination of procedures and/or blocks in the flowchart and/or block diagram can be realized by computer program instructions. These computer program instructions may be provided to a general purpose computer, special purpose computer, embedded processor, or processor of other programmable data processing equipment to produce a machine such that the instructions executed by the processor of the computer or other programmable data processing equipment produce a An apparatus for realizing the functions specified in one or more procedures of the flowchart and/or one or more blocks of the block diagram.

These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing apparatus to operate in a specific manner, such that the instructions stored in the computer-readable memory produce an article of manufacture comprising instruction means, the instructions The device realizes the function specified in one or more procedures of the flowchart and/or one or more blocks of the block diagram.

These computer program instructions can also be loaded onto a computer or other programmable data processing device, causing a series of operational steps to be performed on the computer or other programmable device to produce a computer-implemented process, thereby The instructions provide steps for implementing the functions specified in the flow chart or blocks of the flowchart and/or the block or blocks of the block diagrams.

Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be implemented through computer programs to instruct related hardware, and the programs can be stored in a computer-readable storage medium. During execution, it may include the processes of the embodiments of the above-mentioned methods. Wherein, the storage medium may be a magnetic disk, an optical disk, a read-only memory (Read-Only Memory, ROM) or a random access memory (Random AccessMemory, RAM), etc.

The above descriptions are only preferred embodiments of the present disclosure, and are not intended to limit the present disclosure. For those skilled in the art, the present disclosure may have various modifications and changes. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present disclosure shall be included within the protection scope of the present disclosure.

Claims

A kind of mobile robot mine scene reconstruction method based on SLAM, it is characterized in that: comprise following process:

Obtain synchronously calibrated laser point cloud data and visual point cloud data measured by mobile robots;

Fusion of acquired laser point cloud data and visual point cloud data;

Perform point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data;

Combined with the processed point cloud data, the multi-constraint factor graph algorithm based on graph optimization is used to add the IMU pre-integration data, point cloud key frame data and GNSS data into the constrained subgraph, and the reconstructed 3D map is obtained after loop closure detection.
The SLAM-based mobile robot mine scene reconstruction method according to claim 1, characterized in that:

Use the characteristic information of the current laser frame and the characteristic information of the map to perform iterative optimization, and use the IMU pre-integration data, point cloud key frame data and GNSS data as the factors of the graph optimization algorithm, add them to the factor graph, and perform factor graph optimization, update All key frame poses, and finally the optimized pose and the reconstructed 3D map.
The SLAM-based mobile robot mine scene reconstruction method as claimed in claim 2, characterized in that:

Find the key frame in the historical key frame, match the current frame with the point cloud of several frames near the key frame, get the transformed pose, construct the closed-loop factor map data, and add the factor map for optimization.
The mobile robot mine scene reconstruction method based on SLAM as claimed in claim 3, characterized in that:

Among the historical key frames, the frames whose distance is smaller than the preset value and whose time interval is larger than the preset value are used as key frames.
The mobile robot mine scene reconstruction method based on SLAM as claimed in claim 1, characterized in that:

Fusion of acquired laser point cloud data and visual point cloud data, including:

Use the time stamp interpolation algorithm to obtain matching laser point cloud data and visual point cloud data, use the principle of pinhole imaging to project the laser point cloud to the camera plane, and project the laser point and visual point cloud on the camera plane according to the XYZ coordinates Perform matching to obtain the fused point cloud PointCloud: XYZRIRGB;

Among them, 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 light intensity of the laser point cloud, and R is the beam number of each laser beam , RGB is the color information of the visual point cloud.
The SLAM-based mobile robot mine scene reconstruction method according to claim 1, characterized in that:

Point cloud motion distortion removal processing, including:

Assuming that time T is the start time of a frame of laser scanning, and time T+ΔT is the end time of laser scanning of this frame, pre-integrate the angular velocity data and acceleration data of the three axes of the IMU to obtain the relative motion of the robot within ΔT time, and then calculate the relative motion of the robot within ΔT time All the laser points of the laser point are converted to the first laser point to realize the removal of laser point cloud motion distortion.
The mobile robot mine scene reconstruction method based on SLAM as claimed in claim 1, characterized in that:

Point cloud filtering processing, including:

Project each frame of laser point cloud into a depth image, the scanning line bundle of multi-line lidar is r lines, and each line scans each frame to obtain n laser points, and the depth image is an r*n matrix, each matrix contains the point cloud Coordinate information and light intensity information;

Use the depth image to obtain the positional relationship between adjacent point clouds, and use the kd-tree search algorithm to search the adjacent points of the point cloud, and filter the ground points and noise points according to the geometric relationship between the point clouds.
A SLAM-based mobile robot mine scene reconstruction system is characterized in that: comprising:

The data acquisition module is configured to: acquire synchronously calibrated laser point cloud data and visual point cloud data measured by the mobile robot;

The point cloud fusion module is configured to: fuse the acquired laser point cloud data and visual point cloud data;

The point cloud data processing module is configured to: perform point cloud motion distortion removal processing and point cloud filtering processing on the fused point cloud data;

The 3D map reconstruction module is configured to: combine the processed point cloud data, adopt a multi-constraint factor graph algorithm based on graph optimization, add IMU pre-integration data, point cloud key frame data and GNSS data into the constraint subgraph, and perform loop closure detection Then get the reconstructed 3D map.
A computer-readable storage medium on which a program is stored, characterized in that, when the program is executed by a processor, the steps in the SLAM-based mobile robot mine scene reconstruction method according to any one of claims 1-7 are realized .
An electronic device, comprising a memory, a processor, and a program stored in the memory and operable on the processor, wherein the processor implements the program described in any one of claims 1-7 when executing the program Steps in the SLAM-based mine scene reconstruction method for mobile robots.