Mapping and Localization Method Based on Multi-Sensor Fusion

Abstract

The present invention discloses a mapping and positioning method based on multi-sensor fusion. This method uses three sensors, namely LocalSense, 2D lidar, and RGB camera. When constructing a map in real time, LocalSense is used to track the rough pose of the mobile robot; candidate areas are screened around the rough pose, and the pose is corrected through the result of 2D laser information matching; the cartographer algorithm is used to establish a two-dimensional grid map; and the RGB image of the current pose is segmented, and each piece of the image is compressed into a one-dimensional vector through a convolutional autoencoder and stored, corresponding to the current pose. In the positioning stage, candidate RGB images are screened by feature matching, and then candidate positioning areas are screened according to the poses corresponding to the candidate images. Laser information matching is performed in the candidate positioning areas to obtain the accurate pose. The method of the present invention does not depend on the content and features of the images, can be applied to different types of images and different application scenarios, and can better adapt to some scenarios with fewer corners and edges.

Description

Technical Field

[0001] The present invention belongs to the field of simultaneous mapping and localization, and particularly relates to a mapping and localization method based on multi-sensor fusion. Background Art

[0002] In most cases, the map of the environment where the robot is located is unknown. Therefore, simultaneous localization and mapping is a necessary prerequisite for the robot to complete its work. The mapping and localization method based on multi-sensor fusion is a technology that integrates data from multiple sensors, and utilizes the advantages of multiple sensors to improve the accuracy and robustness of mapping and localization. The mapping and localization method based on multi-sensor fusion can effectively improve the application effects in fields such as robot navigation, autonomous driving, and indoor localization, and is a very practical and promising technology.

[0003] Commonly used sensors include lidar, cameras, inertial measurement units, and GPS, etc. Lidar is a commonly used sensor that can provide high-precision distance and angle information by scanning the surrounding environment and recording its geometric features, such as walls, furniture, etc. Visual sensors such as RGB cameras can provide rich visual information, such as color, texture, and shape, etc., which can be used to detect and track feature points in the environment, such as corner points and edges, etc. The inertial measurement unit can provide the acceleration and angular velocity information of the robot. During the process of simultaneous mapping and localization, the inertial measurement unit can be used to estimate the pose and motion state of the robot, and can perform pose adjustment and localization. However, over time, the pose will deviate. GPS can provide the position information of the robot, but the signal is often weak or unavailable in indoor environments. Therefore, in indoor environments, GPS usually cannot be used as the main positioning sensor. Summary of the Invention

[0004] The purpose of the present invention is to overcome the deficiencies of the prior art and provide a mapping and localization method based on multi-sensor fusion.

[0005] The technical solution for achieving the purpose of the present invention is:

[0006] The mapping and localization method based on multi-sensor fusion realizes mapping and localization through three sensors: LocalSense, 2D lidar, and RGB camera. The method includes a mapping stage and a localization stage. The mapping stage includes the following steps:

[0007] Step (1.1): Manipulate the robot to move, obtain the rough pose of the robot in real time, and divide the horizontal and vertical coordinate candidate matching regions, as well as the angle matching region, according to the rough pose.

[0008] In step (1.2), the 2D lidar information and RGB image at the current pose are obtained, and matching is performed through the candidate matching regions provided in step (1) to obtain the accurate pose.

[0009] In step (1.3), according to the obtained 2D lidar information and the accurate pose obtained in step (1.2), the cartographer algorithm is used to establish a two-dimensional grid map.

[0010] In step (1.4), the RGB image at the current pose is divided into blocks, and each block of the image is compressed into a one-dimensional vector through a convolutional autoencoder and stored at the current pose. Steps (1.1) to (1.4) are repeated until the mapping is completed and the robot no longer needs to move.

[0011] The positioning stage includes the following steps:

[0012] In step (2.1), the RGB image and 2D lidar information at the current pose are obtained.

[0013] In step (2.2), the candidate RGB images are screened out by using feature matching.

[0014] In step (2.3), the candidate positioning regions are screened out according to the poses corresponding to the candidate images.

[0015] In step (2.4), laser information matching is performed in the candidate positioning regions, and finally the accurate pose corresponding on the map is obtained.

[0016] Furthermore, in step (1.1), LocalSense is used to obtain the rough pose of the robot. Three tags are placed in the middle of the robot, and the three tags form an equilateral triangle. The position coordinates (x1, y1), (x2, y2), (x3, y3) of the three tags are obtained through LocalSense. According to the coordinate information of the three tags, the rough pose (x, y, θ) of the robot is calculated, where x and y are the horizontal and vertical coordinates of the robot respectively, and θ is the orientation of the robot. The calculation formula is: x = (x2 + x2 + x3) / 3, y = (y1 + y2 + y3) / 3. The candidate horizontal and vertical coordinate matching regions are divided according to the horizontal and vertical coordinates of the calculated rough pose, and the matching angle range is restricted according to the θ value.

[0017] Furthermore, after the lidar information and RGB image at the current pose are obtained in step (1.2), laser information matching is performed in the candidate regions and angles divided in step (1). The algorithm for laser information matching is the branch and bound algorithm.

[0018] Further, in step (1.4), the RGB image is divided into small blocks of 64 pixels by 64 pixels. Each small block image is input into the trained convolutional autoencoder to obtain the corresponding one-dimensional vector, which is stored at the current pose. The loss function of the convolutional autoencoder is where n refers to the dimension of the data, and p refers to the image after segmentation, and refers to the reconstructed image obtained by encoding and then decoding the segmented image by the convolutional autoencoder.

[0019] Further, in the robot positioning stage, the bag-of-words model is used for feature matching to screen out candidate RGB images with the number of feature matches greater than the threshold. The candidate positioning area is screened out according to the poses corresponding to the candidate RGB images. The branch and bound algorithm is used for laser information matching in the candidate positioning area, and finally the accurate pose of the robot is obtained to achieve the positioning process.

[0020] In the method of the present invention, three sensors, namely LocalSense, 2D lidar, and RGB camera, are adopted in the mapping stage. LocalSense is used to provide the global rough pose, thereby accelerating the mapping speed. Although the local accuracy of LocalSense is not as good as that of the inertial measurement unit, globally, the pose will not be overly offset due to time accumulation, and at the same time, the problem of poor GPS effect in the indoor environment can be avoided. In addition, the present invention selects to perform block compression on the RGB image. Compared with the method of compressing through feature points, this method does not depend on the content and features of the image, can be applied to different types of images and different application scenarios, and can better adapt to some scenarios with fewer corners and edges. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 is a schematic flow chart of the method of the present invention;

[0022] Figure 2 is a two-dimensional grid map constructed in real time by adopting the method of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0023] The following further elaborates on the content of the present invention in detail in conjunction with the drawings and embodiments, but does not limit the present invention.

[0024] Embodiment

[0025] Referring to Figure 1 , a mapping and positioning method based on multi-sensor fusion realizes mapping and positioning through three sensors, namely LocalSense, 2D lidar, and RGB camera. The method includes a mapping stage and a positioning stage. The mapping stage includes the following steps:

[0026] Step (1.1): Manipulate the robot to move, obtain the rough pose of the robot in real time, and divide the candidate matching regions for the horizontal and vertical coordinates, as well as the angle matching region, according to the rough pose.

[0027] Step (1.2): Obtain the 2D lidar information and RGB image at the current pose, perform matching through the candidate matching regions provided in step (1), and obtain the accurate pose.

[0028] Step (1.3): According to the obtained 2D lidar information and the accurate pose obtained in step (1.2), use the cartographer algorithm to establish a two-dimensional grid map.

[0029] Step (1.4): Divide the RGB image at the current pose into blocks, compress each block of the image into a one-dimensional vector through a convolutional autoencoder, and store it at the current pose. Repeat steps (1.1) to (1.4) until the mapping is completed and the robot no longer needs to move.

[0030] The positioning stage includes the following steps:

[0031] Step (2.1): Obtain the RGB image and 2D lidar information at the current pose.

[0032] Step (2.2): Use the bag-of-words model for feature matching to screen out the candidate RGB images with the number of feature matches greater than the threshold.

[0033] Step (2.3): Screen out the candidate positioning regions according to the poses corresponding to the candidate images.

[0034] Step (2.4): Use the branch and bound algorithm for laser information matching in the candidate positioning region, and finally obtain the accurate pose of the robot to achieve the positioning process.

[0035] In this embodiment, in step (1.1), LocalSense is used to obtain the rough pose of the robot. Place three tags in the middle of the robot. The three tags form an equilateral triangle. The direction of the perpendicular bisector of two of the tags points to the direction of the other tag, which is the orientation of the robot. Obtain the position coordinates (x1, y1), (x2, y2), (x3, y3) of the three tags through LocalSense, and calculate the rough pose (x, y, θ) of the robot according to the coordinate information of the three tags. x and y are the horizontal and vertical coordinates of the robot respectively, and θ is the orientation of the robot. The calculation formula is: x = (x2 + x2 + x3) / 3, y = (y1 + y2 + y3) / 3. Divide the candidate matching regions for the horizontal and vertical coordinates according to the calculated horizontal and vertical coordinates of the rough pose. Take a square region of 45 cm by 45 cm with (x, y) as the center, and limit the matching angle range to (θ - 60°, θ + 60°) according to the θ value.

[0036] In this embodiment, after obtaining the lidar information and RGB image in the current pose in step (1.2), the lidar information is matched in the candidate regions and angles divided in step (1), and the algorithm for lidar information matching is the branch and bound algorithm.

[0037] In this embodiment, in step (1.4), the RGB image obtained in the current pose is divided into small blocks of 64 pixels by 64 pixels. The convolutional autoencoder is used to compress each divided image into a one-dimensional vector. A bag-of-words model is constructed based on these one-dimensional vectors. Before using the bag-of-words model, a dictionary needs to be loaded, and this dictionary needs to be trained offline. The loss function of the convolutional autoencoder is n refers to the dimension of the data, and p refers to the divided image. refers to the reconstructed image of the divided image after being encoded and then decoded by the convolutional autoencoder.

[0038] To verify the mapping and positioning method of the present invention, the inventor manipulated the robot to move in a 7*10-meter indoor environment, and LocalSense was arranged at the four corners of this environment. After measuring the accuracy of LocalSense, a square area of 45 cm by 45 cm and an angular region of (θ - 60 ° , θ + 60 ° ) were selected to obtain the rough pose, so as to speed up the mapping speed. The real-time constructed two-dimensional grid map is as Figure 2 shown. At the same time, the center point in the experimental environment was aligned with the center of the map. According to the test, the positioning accuracy is about 5 cm.

Claims

1. A mapping and positioning method based on multi-sensor fusion, which realizes mapping and positioning through three sensors: LocalSense, 2D lidar, and RGB camera. It is characterized in that, The method includes a mapping stage and a positioning stage. The mapping stage includes the following steps: Step (1.1): Manipulate the robot to move, obtain the rough pose of the robot in real time, and divide the candidate matching regions of the horizontal and vertical coordinates and the angle matching region according to the rough pose; Specifically: Place three tags in the middle of the robot. The three tags form an equilateral triangle. Obtain the position coordinates of the three tags through LocalSense , , , and calculate the rough pose of the robot based on the coordinate information of the three tags , and are the horizontal and vertical coordinates of the robot respectively, is the orientation of the robot, and the calculation formula is: , , , divide the candidate horizontal and vertical coordinate matching areas according to the horizontal and vertical coordinates of the calculated rough pose, and limit the matching angle range according to the value; Step (1.2): Obtain the 2D lidar information and RGB image at the current pose, perform matching through the candidate matching regions provided in step (1), and obtain the accurate pose; Step (1.3): Use the cartographer algorithm to establish a two-dimensional grid map according to the obtained 2D lidar information and the accurate pose obtained in step (1.2); Step (1.4): Divide the RGB image at the current pose into blocks, and each block of the image is compressed into a one-dimensional vector through a convolutional autoencoder and stored at the current pose. Repeat steps (1.1) to (1.4) until the mapping is completed and the robot no longer needs to move; Specifically: the RGB image is divided into small blocks of 64 pixels by 64 pixels, and each small block image is input into the trained convolutional autoencoder to obtain the corresponding one-dimensional vector, which is stored at the current pose. The loss function of the convolutional autoencoder is , refers to the dimension of the data, refers to the image after segmentation, refers to the reconstructed image obtained by encoding and then decoding the segmented image by the convolutional autoencoder; The positioning stage includes the following steps: Step (2.1): Obtain the RGB image and 2D lidar information at the current pose; Step (2.2): Use the feature matching method to screen out the candidate RGB images; Step (2.3): Screen out the candidate positioning regions according to the poses corresponding to the candidate images; Step (2.4): Perform laser information matching in the candidate positioning regions to finally obtain the accurate pose corresponding on the map.

2. The mapping and positioning method based on multi-sensor fusion according to claim 1, characterized in that, Step (1.2) is specifically: Obtain the lidar information and RGB image at the current pose, perform laser information matching in the candidate regions and angles divided in step (1), and the algorithm for laser information matching is the branch and bound algorithm.

3. The mapping and positioning method based on multi-sensor fusion according to claim 1, characterized in that: The feature matching method in step (2.2) is the bag-of-words model.

4. The mapping and positioning method based on multi-sensor fusion according to claim 1, characterized in that: In step (2.4), the laser information matching in the candidate positioning regions is performed, and the matching algorithm is the branch and bound algorithm.

Images