Indoor robot sensing method based on point cloud multi-sensor fusion mapping

Abstract

The invention discloses an indoor robot sensing method based on point cloud multi-sensor fusion mapping, and relates to an indoor intelligent robot sensing method, which comprises the following steps of: obtaining multi-frame point cloud images of surrounding environment and obstacles by using a binocular camera and a laser radar; processing and fusing the three-dimensional point cloud images respectively acquired by the two three-dimensional point cloud images; in order to improve the robustness of perception, the three-dimensional point cloud is preprocessed before fusion, and interference of point cloud noise on subsequent matching is avoided; based on the target point cloud data corresponding to each frame of image, the point cloud map is accurately registered, so that the intelligent robot can specifically understand the external environment. According to the method, fusion mapping is carried out based on the point cloud data obtained by different sensors, the environment information of the intelligent equipment is determined, the wrong point cloud determined according to the depth corresponding to each point cloud is filtered out from the target point cloud data, and the mapping effect is optimized by filtering out the wrong point cloud influencing the accuracy of the volume measurement result, so that the accuracy of the volume measurement result is improved. The mapping error is obviously reduced, the sensing precision of the robot is improved, and the robustness of the system is improved.

Description

Technical Field

[0001] The present invention relates to an indoor intelligent robot perception method, and in particular to an indoor robot perception method based on point cloud multi-sensor fusion mapping. Background Art

[0002] With the rapid development of the indoor service robot industry, the requirements for navigation systems in small and medium-sized indoor environments are also increasing. An efficient navigation system usually includes three main parts: perception module, positioning module and planning module. The perception module uses various types of sensors to obtain various information in the environment, which provides an important basis for the robot to make decisions. In terms of real-time mapping, SLAM (Simultaneous Localization and Mapping) technology plays a vital role. In terms of back-end optimization, filtering technologies such as EKF (Extended Kalman Filter) are widely used. However, due to the limitations of lidar in certain specific environments, such as in complex and changeable indoor environments, its measurement accuracy may be affected, so multi-sensor fusion technology has gradually become a hot topic of research. Multi-sensor fusion technology can significantly improve the efficiency and accuracy of control tasks by sharing and fusing information from different sensors. Accurate maps are crucial for robot positioning and path planning, and multi-sensor fusion technology achieves this goal in this way. Summary of the invention

[0003] The purpose of the present invention is to provide an indoor robot perception method based on point cloud multi-sensor fusion mapping, which adopts multi-sensor fusion perception and filters and matches the perception point cloud to achieve accurate mapping of indoor small and medium-sized complex environments with high obstacle density and many obstacles.

[0004] The objective of the present invention is achieved through the following technical solutions:

[0005] An indoor robot perception method based on point cloud multi-sensor fusion mapping, comprising:

[0006] Step 1: Use LiDAR to perceive the surrounding environment in three dimensions and obtain 3D point cloud information based on LiDAR.

[0007] Step 2: Use a binocular camera to perceive the surrounding environment in three dimensions and obtain 3D point cloud information based on binocular vision.

[0008] Step 3: pre-process the point cloud information obtained in the above steps.

[0009] Among them, the voxel grid downsampling method is used to filter the 3D point cloud map, divide the point cloud into grids of fixed size, and then use the average value of the points in each grid or randomly select a point as a representative.

[0010] Step 4: perform rough matching on the two point cloud information.

[0011] The SAC-IA (Sample Consensus Initial Alignment) algorithm is used for rough matching. SAC-IA is an improved algorithm based on RANSAC, which uses FPFH features for point pair matching. First, the key points in the point cloud are extracted and their FPFH features are calculated, and then the RANSAC algorithm is used to find corresponding point pairs in the feature space. Preliminary alignment is achieved by estimating the rigid transformation of these point pairs.

[0012] Step 5: Accurately align the point cloud and draw a global environment map based on the odometer information.

[0013] Among them, the iterative closest point (ICP) algorithm is used for point cloud registration. The ICP algorithm minimizes the distance between two point clouds through iterative optimization. It first finds the closest point pair in the two sets of point clouds, and then aligns the source point cloud with the target point cloud through rigid transformation (translation and rotation). The process will continue to iterate until it converges to the preset threshold or reaches the maximum number of iterations. :

[0014] In steps 1 and 2, the intelligent sensing mobile device is equipped with a laser radar, a binocular camera, and an IMU inertial unit, and uses the Cartographer 3D laser radar SLAM and ORB-SLAM visual SLAM algorithms to achieve 3D perception of the environment.

[0015] In step 3, downsampling is performed using a voxel grid downsampling method, wherein the voxel size is set to 3 mm, the point cloud space is divided into 200 voxel units, and the coordinates of the points are mapped to corresponding voxel indices.

[0016] In step 4, the SAC-IA algorithm is used for rough matching, and the ISS key point detection algorithm is used to extract key points in the point cloud. When using the SAC-IA algorithm, the important parameters are set as follows: the search radius is set to 0.1 times the point cloud scale, the maximum corresponding distance is set to 0.2 times the point cloud scale, the minimum inlier ratio is set to 0.2, and the maximum number of iterations is set to 500.

[0017] In step 5, the iterative closest point (ICP) algorithm is used to perform accurate point cloud registration, and the important parameters are set as follows: the maximum number of iterations is set to 50 times, the convergence threshold is set to 1e-5, the maximum corresponding distance is set to 0.05 times the point cloud scale, the minimum number of point pairs is set to 5%, and the mean square error stopping condition is set to 1e-5.

[0018] The advantages and effects of the present invention are:

[0019] 1. Aiming at complex indoor environments, the present invention uses multi-sensor technology to improve the efficiency and accuracy of mapping. In order to further improve the quality of the map, the present invention also proposes an improved collaborative point cloud registration method. This method can identify and process dynamic obstacles in real time through machine learning algorithms, thereby ensuring the accuracy and reliability of the map. In this way, the robot can complete various tasks more flexibly and efficiently in complex and changing indoor environments.

[0020] 2. The multi-sensor technology of the present invention includes a combination of multiple sensors such as laser radar, camera, infrared sensor, etc. These sensors operate at different operating frequencies and wavelengths and can capture different information in the environment. For example, laser radar is good at obtaining accurate distance information, while the camera can provide rich color and texture information. By fusing these different types of information, a more comprehensive and accurate environmental perception result can be obtained.

[0021] 3. The present invention also introduces an improved collaborative point cloud registration method. This method uses machine learning algorithms, especially deep learning technology, to process and analyze point cloud data. By training the neural network model, the robot can identify dynamic obstacles in real time, classify and track them. In this way, the robot can avoid these dynamic obstacles when planning the path, thereby improving the accuracy and safety of navigation.

[0022] 4. The present invention significantly improves the navigation performance of indoor service robots in complex environments by adopting multi-sensor technology and an improved collaborative point cloud registration method. This not only improves the robot's work efficiency, but also enhances its reliability and flexibility in practical applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is an exemplary system architecture diagram to which the present invention is applied;

[0024] Figure 2 A flow chart of a downsampling algorithm using the method provided by the present invention;

[0025] Figure 3 is a schematic diagram of a three-dimensional point cloud of an object provided by an embodiment of the present invention;

[0026] Figure 4 is a schematic diagram of a three-dimensional point cloud of an object after preprocessing provided by an embodiment of the present invention;

[0027] Figure 5 A point cloud registration flow chart using the method provided by the present invention;

[0028] Figure 6 is a schematic structural diagram of an embodiment of a point cloud-based object volume measurement device according to the present application;

[0029] Figure 7 This is a basic structural block diagram of the computer device in this embodiment. DETAILED DESCRIPTION

[0030] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0031] The present invention provides an indoor robot perception method based on point cloud multi-sensor fusion mapping, which can realize multi-sensor robust and accurate environmental perception of complex indoor environments. Among them, the multi-sensor refers to a laser radar, a binocular camera and an IMU inertial unit. The present invention uses an intelligent perception device equipped with a three-dimensional laser radar and a binocular camera to use a SLAM algorithm to realize three-dimensional information perception of an unknown environment, and respectively constructs respective point cloud maps in combination with the mileage information provided by the IMU inertial unit. Because the laser radar has a wide field of view but rough mapping details, and the binocular camera has a slow mapping and limited field of view but complete mapping details. Therefore, the present invention uses a point cloud processing and fusion algorithm to filter and denoise the point cloud map constructed by the two, and then align and fuse them, so as to realize the fusion of multi-sensor point cloud maps and the improvement of map detail areas, and finally generate a global map with a small error.

[0032] The mapping method of the present invention is divided into three parts: point cloud map acquisition, point cloud map preprocessing and point cloud map fusion. The method block diagram is as follows: Figure 1 As shown, the specific steps are as follows:

[0033] Step 1: Point cloud map collection:

[0034] The present invention uses multiple sensors to build a fusion mapping system, and uses laser radar and binocular cameras to perform three-dimensional perception of the environment, respectively, to obtain laser-perceived 3D point clouds and visual-perceived 3D point clouds. In this embodiment, a three-dimensional laser radar SLAM algorithm and a binocular vision SLAM algorithm are used to perform three-dimensional perception of the environment, and the perception information is stored in the form of a 3D point cloud map for subsequent fusion processing. Specifically, the smart devices are equipped with Velodyne 16-line laser radars, kinect 2 binocular cameras, and NVIDIA Jetson Nano processors, and Cartographer three-dimensional laser radar SLAM and ORB-SLAM visual SLAM algorithms are deployed to achieve three-dimensional perception of the surrounding environment.

[0035] Step 2: Point cloud map preprocessing:

[0036] Point cloud data is usually collected by lidar, 3D scanner or other sensors, and may contain noise points or outliers, which are usually caused by equipment errors, reflection interference or environmental factors. At the same time, point cloud data usually contains millions or even hundreds of millions of points. It is computationally expensive to process this data directly, and such a high resolution is not required for map drawing. Denoising and outlier removal, through statistical filtering, radius filtering and other methods to remove points that do not conform to the overall point cloud distribution, to ensure the quality of the data. Improve the accuracy of the point cloud and avoid noise points interfering with subsequent feature extraction, registration and other steps. The voxel grid downsampling method can be used to filter the point cloud map. First, the point cloud is divided into voxels, and then the center of mass of the non-empty voxel is calculated to replace all the points in the voxel to achieve point cloud downsampling.

[0037] First, we calculate the dimensions of the voxel grid. Based on the voxel size and the overall bounding box of the point cloud, we can calculate how many voxels are needed to divide this space. The number of voxels in each direction can be calculated using the following formula:

[0038]

[0039] Among them, n x 、n y 、n z They are the number of voxels in the x, y, and z directions respectively, and the result is rounded up.

[0040] The points are then assigned to voxels, and for each point in the point cloud, the position of the point in the voxel grid is calculated by mapping the point's coordinates to the corresponding voxel index.

[0041]

[0042] Finally, the centroid method is used to select the representative point of each voxel. Given a non-empty voxel V containing m points, the centroid P of the voxel iscentroid (x centroid y centroid z centroid )for:

[0043]

[0044] After space division and representative point selection, the new point cloud is composed of representative points of all voxel units. This is the point cloud data after downsampling by the voxel network. The number of points is significantly reduced while maintaining the approximate geometric structure of the original point cloud.

[0045] Step 3: Point cloud map fusion

[0046] (1) Use the SAC-IA algorithm for rough matching

[0047] The algorithm is based on the principle of point feature histogram, so before implementing the algorithm, the FPFH features of the point cloud need to be calculated first. The basic steps of the algorithm are as follows:

[0048] 1. Select n sampling points from the point cloud P to be registered. To ensure that the selected sampling points have different FPFH features, the distance between the sampling points should be greater than the preset minimum distance threshold d.

[0049] 2. Find one or more points in the target point cloud Q that have similar FPFH features to the sampling points in the point cloud P, and randomly select a point from these similar points as the corresponding point of the point cloud P in the target point cloud Q.

[0050] 3. Calculate the rigid body transformation matrix between the corresponding points, and then evaluate the performance of the current registration transformation by solving the "distance error sum" function after the corresponding point transformation. The distance error sum function here is often expressed using the Huber penalty function, denoted as in:

[0051]

[0052] In the formula, m l represents a preset constant, and l i Represents the value of the distance difference of the i-th group of corresponding points after transformation. The ultimate goal of the above registration process is to find a set of optimized transformations among all possible transformations to minimize the value of the error function. At this point, the transformation obtained is the final registration transformation matrix, and then the registration result can be obtained. The transformation matrix obtained by the SAC-IA algorithm is not accurate enough, so it is only suitable for the preliminary registration stage. The SAC-IA algorithm can be implemented in the registration module of the PCL library.

[0053] When the amount of point cloud data is large, the speed of calculating FPFH features will slow down, resulting in a significant decrease in the efficiency of the SAC-A algorithm. Therefore, in step 2, the point cloud is first downsampled to reduce the number of points.

[0054] (2) Use iterative closest point algorithm for accurate registration

[0055] The two point clouds P' (source point cloud after coordinate transformation) and Q after initial registration are used as the initial point set for fine registration; for each point p in the source point cloud P' i , find the nearest corresponding point q in the target point cloud Q i , as the corresponding point of the point in the target point cloud, forming the initial corresponding point pair; find the Euclidean transformation R,t so that

[0056]

[0057] The ICP algorithm performs iterative calculation based on the least squares method, so that the sum of squared errors reaches a minimum value. That is, the mean square error between corresponding point sets is minimized.

[0058] in,

[0059] Set the convergence threshold to 1e-5 and the maximum number of iterations to 50, apply the rigid body transformation obtained in the previous step to the source point cloud P', obtain the new point cloud P", calculate the distance error between P" and Q, if the error of two iterations is less than the threshold: or the current number of iterations is greater than 50, the iteration ends, otherwise the initial registered point set is updated to P" and Q, and the above steps are repeated until the convergence conditions are met.

[0060] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. An indoor robot perception method based on point cloud multi-sensor fusion mapping, characterized in that: The method comprises the following steps: Step 1: Use laser radar to perceive the surrounding environment in three dimensions and obtain 3D point cloud information based on laser radar; Step 2: Use a binocular camera to perceive the surrounding environment in three dimensions and obtain a 3D point cloud signal based on binocular vision; Step 3, preprocessing the point cloud information obtained in the above steps; Among them, voxel grid downsampling is used to filter the 3D point cloud map, divide the point cloud into grids of fixed size, and then use the average value of the points in each grid or randomly select a point as a representative; Step 4, roughly matching the two point cloud information; Among them, the SAC-IA (Sample Consensus Initial Alignment) algorithm is used for rough matching; SAC-IA is an improved algorithm based on RANSAC, which uses FPFH features for point pair matching; first, the key points in the point cloud are extracted and their FPFH features are calculated, and then the RANSAC algorithm is used to find corresponding point pairs in the feature space; preliminary alignment is achieved by estimating the rigid transformation of these point pairs; Step 5: Accurately register the point cloud and draw a global environment map in combination with the odometer information; Among them, the iterative closest point (ICP) algorithm is used for point cloud registration; the ICP algorithm minimizes the distance between two point clouds through iterative optimization; it first finds the nearest point pair in the two sets of point clouds, and then aligns the source point cloud with the target point cloud through rigid transformation (translation and rotation); the process will continue to iterate until it converges to the preset threshold or reaches the maximum number of iterations.

2. The indoor robot perception method based on point cloud multi-sensor fusion mapping as claimed in claim 1, characterized in that: In step 1 and step 2, the intelligent sensing mobile device is equipped with a laser radar, a binocular camera and an IMU inertial unit at the same time, and uses the Cartographer 3D laser radar SLAM and ORB-SLAM visual SLAM algorithms to achieve 3D perception of the environment.

3. The indoor robot perception method based on point cloud multi-sensor fusion mapping as claimed in claim 1, characterized in that: In the step 3, downsampling is performed using a voxel grid downsampling method.

4. The indoor robot perception method based on point cloud multi-sensor fusion mapping as claimed in claim 1, characterized in that: In step 4, the SAC-IA algorithm is used for coarse matching, and the ISS key point detection algorithm is used to extract key points in the point cloud.

5. The indoor robot perception method based on point cloud multi-sensor fusion mapping as claimed in claim 1, characterized in that: In step 4, when using the SAC-IA algorithm, the important parameters are set as follows: the search radius is set to 0.1 times the point cloud scale, the maximum corresponding distance is set to 0.2 times the point cloud scale, the minimum inlier ratio is set to 0.2, and the maximum number of iterations is set to 500.

6. The indoor robot perception method based on point cloud multi-sensor fusion mapping as claimed in claim 1, characterized in that: In step 5, an iterative closest point algorithm is used to perform accurate point cloud registration.

7. The indoor robot perception method based on point cloud multi-sensor fusion mapping as claimed in claim 1, characterized in that: In step 5, when using the iterative closest point algorithm, the important parameters are set as follows: the maximum number of iterations is set to 50 times, the convergence threshold is set to 1e-5, the maximum corresponding distance is set to 0.05 times the point cloud scale, the minimum number of point pairs is set to 5%, and the mean square error stopping condition is set to 1e-5.

Images