Mobile robot slam method based on binocular camera and two-dimensional laser radar

Abstract

This invention discloses a SLAM method for mobile robots based on a binocular camera and a 2D LiDAR. It includes the following steps: (1) Deploying the binocular camera and the 2D LiDAR, and obtaining the coordinate transformation matrix between the coordinate systems of the 2D LiDAR and the binocular camera according to the deployment method; (2) The SLAM module of the binocular camera acquires the sparse point cloud map grid g of the front-end pose for visual localization. ki (2) Output localization results, laser pose and grid map, of the SLAM module of the corresponding visual frame pose 2D LiDAR; (3) Perform pose map optimization on the front-end pose and laser pose obtained in step (2); (4) Optimize the sparse point cloud map grid g of the binocular camera obtained in step (2). ki (5) The local grid map of the two-dimensional lidar is fused; and the fused and optimized positioning trajectory and map are published. The tightly coupled and high real-time method of this invention improves the positioning accuracy and obtains a unified fused grid map.

Description

Technical Field

[0001] This invention belongs to the field of mobile robots, specifically relating to a SLAM method for mobile robots based on binocular cameras and two-dimensional LiDAR. Background Technology

[0002] As the nation deepens its promotion of intelligent manufacturing, the intelligent development of mobile robots has received increasing attention. Rising labor costs are driving mobile robots into businesses. China's mobile robot industry is currently in a new period of rapid growth and development, and specialized sectors such as logistics and transportation, consumer goods manufacturing and processing, and hazardous materials handling are placing increasingly urgent demands on the technology of mobile robots. This will continuously propel the development of mobile robots in China towards a safer and more reliable future.

[0003] SLAM (Simultaneous Localization and Mapping) aims to solve the problem of accurately positioning the robot in its environment within a mobile robot system and to create an environmental map to provide a foundation for subsequent navigation functions. Regarding SLAM sensors, the detection range of 2D LiDAR is limited to a plane, while camera sensors result in large errors in long-term localization and mapping results. As for SLAM algorithms, the paper "Research on Mobile Platform Localization by Fusion of LiDAR and Vision" by Zhang Pengfei et al., University of Electronic Science and Technology of China, integrates extended Kalman filters to fuse sensor localization data and perform map stitching, but suffers from low localization coupling and real-time performance, as well as inconsistent stitched maps. Summary of the Invention

[0004] The purpose of this invention is to provide an indoor mobile robot SLAM method based on binocular cameras and two-dimensional LiDAR. Based on graph optimization theory, the method performs localization fusion optimization, and fuses the visual point cloud map with the two-dimensional LiDAR mapping results after converting it into a raster map. This improves the localization accuracy and obtains a unified fused raster map in a tightly coupled and high real-time manner.

[0005] The technical solution to achieve the purpose of this invention is: a SLAM method for mobile robots based on binocular cameras and two-dimensional LiDAR, comprising the following steps:

[0006] Step (1): Deploy the binocular camera and the 2D LiDAR, and calibrate the coordinate transformation matrix of the coordinate system of the 2D LiDAR and the binocular camera according to the deployment method.

[0007] Step (2): The SLAM module of the binocular camera acquires the front-end pose for visual localization. Sparse point cloud map grid g ki and its corresponding visual frame pose The SLAM module of the 2D LiDAR outputs the localization result: laser pose. With grid maps;

[0008] Step (3): The front-end pose obtained from the visual localization in step (2) With laser pose Perform pose graph optimization:

[0009] Step (31): Perform pose coordinate transformation: Take the origin of the lidar coordinate system as the origin of the world coordinate system, and transform the pose of the front end of the vision positioning system. The coordinate transformation matrix in step (1) Perform the transformation.

[0010]

[0011] Step (32): Constructing visual odometry constraints in fused SLAM: Let the variable to be optimized be the laser pose. and The relative pose transformation T can be obtained j12 for,

[0012]

[0013] Step (33): Construct the visual SLAM module tracking pose from the corresponding time moment. relative transformation To constrain laser pose and

[0014]

[0015] Step (34): Calculate the constraint error: the constraint formation error e″ j12 for,

[0016]

[0017] Step (35): Construct the objective optimization function for the overall pose map of the fusion localization algorithm: The error e″ j12 The overall error optimization function for the fused pose graph, which is added to the pose graph of the SLAM module of the 2D LiDAR, is as follows:

[0018]

[0019] Among them, e ij and e′ mn The constraints within and outside the laser submap in the Cartographer algorithm are denoted by ρ(·), where ρ(·) is the Huber kernel function. Ξ k Let Ξ be the set of k pose nodes in a subgraph.p The set of pose variables to be optimized is output by p two-dimensional laser SLAM modules; the target optimization function is solved using the Ceres nonlinear solver to obtain the fused and optimized pose result, which is used as the localization trajectory of the mobile robot.

[0020] Step (4): Convert the sparse point cloud map grid g obtained from step (2) of the stereo camera into a single image. ki It is fused with the local grid map of the two-dimensional LiDAR;

[0021] Step (5): Publish the fused and optimized positioning trajectory and map.

[0022] Furthermore, the SLAM module of the binocular camera in step (2) is based on the ORB-SLAM2 algorithm.

[0023] Furthermore, the SLAM module of the two-dimensional LiDAR in step (2) is based on the Cartographer algorithm.

[0024] Furthermore, step (4) also includes the following steps:

[0025] Step (41): Convert the sparse point cloud map grid g ki In visual frame Position p in coordinate system i =[x i ,y i ] T Transform to world coordinates to obtain the raster position p wi :

[0026]

[0027] Step (42): Set the grid position p wi Convert to its corresponding coordinates p in the laser raster map gi :

[0028] p gi =μp wi p gi =μp wi

[0029] Where μ is the reciprocal of the actual size of the raster.

[0030] Step (43): According to the corresponding coordinate p gi The visual grid G ​​in a frame k ={g k1 ,g k2 Insert ,...} into the laser local raster map and update the raster values;

[0031] Step (44): Position Starting from position pgi As the endpoint, the grids in the subgraph for that frame are redrawn based on the Behamming algorithm, and the states of the remaining grids are updated.

[0032] Compared with the prior art, the significant advantages of this invention are:

[0033] (1) Based on graph optimization, tight-coupled positioning fusion is used to optimize the fusion pose by constructing visual odometry constraints, which improves the positioning progress and ensures the real-time performance of pose optimization.

[0034] (2) The visual point cloud was rasterized and fused with the laser raster. The visual point cloud was used to compensate for the blind spot of the two-dimensional laser, which improved the reliability of the map while ensuring the consistency of the map format. Attached Figure Description

[0035] Figure 1 This is a schematic diagram of the mobile robot SLAM method of the present invention.

[0036] Figure 2 This is a schematic diagram of the sensor arrangement and structure according to an embodiment of the present invention.

[0037] Figure 3 This is a raster-like image representing a point cloud conversion according to an embodiment of the present invention.

[0038] Figure 4 This is a local grid map created by the laser SLAM module in an embodiment of the present invention.

[0039] Figure 5 This is a factor graph structure diagram of pose fusion optimization according to the present invention.

[0040] Figure 6 The images show a comparison of the grid map effects before and after fusion in this embodiment of the invention; where (a) is the grid map established by the two-dimensional LiDAR SLAM module, and (b) is the grid map established after fusion optimization by this invention.

[0041] Explanation of reference numerals in the attached figures:

[0042] 1-Binocular camera, 2-2D LiDAR, 3-Mount. Detailed Implementation

[0043] The present invention will now be described in further detail with reference to the accompanying drawings.

[0044] The present invention discloses a method for simultaneous localization and mapping of an indoor mobile robot based on binocular vision and two-dimensional laser fusion, the steps of which are as follows:

[0045] Step 1: Deploy the binocular camera and 2D LiDAR: To avoid mutual occlusion between the binocular camera and 2D LiDAR, the deployment method used in this invention is as follows: Figure 2 As shown, the binocular camera is positioned in front of the fixed platform, and the 2D LiDAR is positioned above and behind the fixed platform. The coordinate transformation matrix between the 2D LiDAR and the binocular camera coordinate systems can be obtained by calibration according to the specific arrangement.

[0046] Step 2: The binocular camera SLAM module, based on the ORB-SLAM2 algorithm, can obtain the front-end pose for visual localization. Sparse point cloud map grid g ki and its corresponding visual frame pose and such Figure 3 The point cloud is converted into a raster. The 2D laser SLAM module, developed based on the Cartographer algorithm, can output the laser pose of the localization result. Compared to raster maps, two-dimensional laser raster maps, such as Figure 4 As shown, after obtaining the localization results and transformed raster from the visual SLAM module and the local map from the laser SLAM module, proceed to step 3.

[0047] Step 3: Obtain the front-end pose from the visual localization obtained in Step 2. Fusion optimization with laser pose estimation is performed. This includes the following steps:

[0048] The first step is to perform pose coordinate transformation. In the fused SLAM proposed in this invention, the origin of the LiDAR coordinate system is used as the origin of the world coordinate system, and the pose of the front end for visual positioning is transformed. The coordinate transformation matrix in step 1 Perform the transformation.

[0049]

[0050] The second step is to construct the visual odometry constraints in the fusion SLAM. Let's assume the variable to be optimized is the laser pose. and The relative pose transformation T can be obtained j12 for,

[0051]

[0052] The third step is to construct the pose tracked by the visual SLAM module at the corresponding time point. relative transformation To constrain laser pose and

[0053]

[0054] Step 4: Calculate the constraint error. The constraint error is e″. j12 for,

[0055]

[0056] Step 5: Construct the objective optimization function for the overall pose map of the fusion localization algorithm. The error e″ j12 Added to the pose graph in the 2D laser SLAM module, such as Figure 5 As shown. The constructed overall error optimization function for the fused pose graph is,

[0057]

[0058] Among them, e ij and e′ mn The constraints within and outside the laser submap in the Cartographer algorithm are denoted by ρ(·), where ρ(·) is the Huber kernel function. Ξ k Let Ξ be the set of k pose nodes in a subgraph. p Let p be the set of pose variables to be optimized output by two-dimensional laser SLAM modules. Solve equation (5) using the Ceres nonlinear solver to obtain the fused and optimized pose result, which is used as the localization trajectory of the mobile robot. After the solution is completed, proceed to step 4.

[0059] Step 4: Fuse the visual raster generated by the visual SLAM module in Step 3 with the local raster map from the laser SLAM module. This includes the following steps:

[0060] Step 1: Visual grid g ki In visual frame Position p in coordinate system i =[x i ,y i ] T Transform to world coordinates to obtain the raster position p wi :

[0061]

[0062] Step 2: Set the grid position p wi Convert to its corresponding coordinates p in the laser raster map gi :

[0063] p gi =μp wi p gi =μp wi (0.1)

[0064] Where μ is the reciprocal of the actual size of the raster.

[0065] Step 3: Based on the corresponding coordinates p from the previous step gi The visual grid G ​​in a framek ={g k1 ,g k2 Insert ,...} into the laser local raster map and update the raster values.

[0066] Step 4: Position Starting from position p gi As the endpoint, the grids in the subgraph for that frame are redrawn based on the Behamming algorithm, and the states of the remaining grids are updated.

[0067] Step 5: Publish the fused and optimized positioning trajectory and map. Compared with the grid map established by a single 2D LiDAR sensor, the fused grid map compensates for the blind spots of 2D LiDAR detection, such as... Figure 6 As shown, a more reliable map was created.

Claims

1. A SLAM method for mobile robots based on binocular cameras and two-dimensional LiDAR, characterized in that, Includes the following steps: Step (1): Deploy the binocular camera and the 2D LiDAR, and calibrate the coordinate transformation matrix of the coordinate system of the 2D LiDAR and the binocular camera according to the deployment method. Step (2): The SLAM module of the binocular camera acquires the front-end pose for visual localization. Sparse point cloud map grid g ki and its corresponding visual frame pose The SLAM module of the 2D LiDAR outputs the localization result: laser pose. With grid maps; Step (3): The front-end pose obtained from the visual localization in step (2) With laser pose Perform pose graph optimization: Step (31): Perform pose coordinate transformation: Take the origin of the lidar coordinate system as the origin of the world coordinate system, and transform the pose of the front end of the vision positioning system. The coordinate transformation matrix in step (1) Perform the transformation. Step (32): Constructing visual odometry constraints in fused SLAM: Let the variable to be optimized be the laser pose. and The relative pose transformation T can be obtained j12 for, Step (33): Construct the visual SLAM module tracking pose from the corresponding time moment. relative transformation To constrain laser pose and Step (34): Calculate the error of the constraint: error e of the constraint formation j 12 For,​ Step (35): Constructing the target optimization function of the fusion positioning algorithm overall pose graph: the error e j 12 The overall error optimization function of the constructed fusion pose graph is added to the SLAM module of the two-dimensional laser radar​ Among them, e ij and e′ mn The constraints within and outside the laser submap in the Cartographer algorithm are denoted by ρ(·), where ρ(·) is the Huber kernel function. Ξ k Let Ξ be the set of k pose nodes in a subgraph. p The set of pose variables to be optimized is output by p two-dimensional laser SLAM modules; the target optimization function is solved using the Ceres nonlinear solver to obtain the fused and optimized pose result, which is used as the localization trajectory of the mobile robot. Step (4): fusing the sparse point cloud map grid g of the binocular camera obtained in step (2) with the local grid map of the two-dimensional laser radar ki and the local grid map of the two-dimensional laser radar; Step (5): Publish the fused and optimized positioning trajectory and map.

2. The method according to claim 1, characterized in that, The SLAM module of the stereo camera in step (2) is based on the ORB-SLAM2 algorithm.

3. The method according to claim 2, characterized in that, The SLAM module of the two-dimensional LiDAR in step (2) is based on the Cartographer algorithm.

4. The method according to claim 3, characterized in that, Step (4) also includes the following steps: Step (41): Convert the sparse point cloud map grid g ki In visual frame Position p in coordinate system i =[x i ,y i ] T Transform to world coordinates to obtain the raster position p wi : Step (42): converting the grid position p wi to its corresponding coordinates p gi in the laser grid map p gi =μp wi p gi =μp wi Where μ is the reciprocal of the actual size of the raster. Step (43): According to the corresponding coordinate p gi The visual grid G ​​in a frame k ={g k1 ,g k2 Insert ,...} into the laser local raster map and update the raster values; Step (44): Position Starting from position p gi As the endpoint, the grids in the subgraph for that frame are redrawn based on the Behamming algorithm, and the states of the remaining grids are updated.

Images