Robot RGB-D SLAM Method Based on Grid Segmentation and Dual-Map Coupling

Abstract

An RGB-D SLAM method based on grid segmentation and dual-map coupling, applicable to indoor dynamic environments, based on homography motion compensation and bidirectional compensated optical flow method, obtains grid-based motion segmentation according to geometric relationships and depth value clustering results, ensuring the rapidity of the algorithm. The camera pose estimation is obtained by minimizing the reprojection error of feature points in the static region. Combining the camera pose, RGB-D images, and grid-based motion segmentation images, a sparse point cloud map and a static octree map of the scene are constructed and coupled simultaneously. A method based on grid segmentation and octree map ray traversal is used to filter static map points on key frames to update the sparse point cloud map, ensuring the positioning accuracy. The present invention can effectively improve the accuracy of camera pose estimation in indoor dynamic scenes and realize the real-time construction and update of the static octree map of the scene.

Description

Technical Field

[0001] The present invention relates to a method for visual simultaneous localization and mapping of a mobile robot in an indoor dynamic environment. Background Art

[0002] Simultaneous localization and mapping (SLAM) is the basis for a mobile robot to achieve autonomous navigation and complete complex environmental interaction tasks, enabling the robot to determine its own position on the one hand and construct a map of the scene on the other hand in an unknown environment. An RGB-D (RGB-Depth) camera can directly capture color and depth images and has a low cost. Therefore, a visual SLAM system based on an RGB-D camera is widely used in fields such as indoor mobile robots. In an actual indoor scene, various moving objects are often included, which interfere with both pose estimation and map construction of the SLAM system.

[0003] Most of the existing visual SLAM systems assume that the scene is static and simplify pose estimation based on this assumption. These SLAM algorithms add dynamic feature points to pose calculation, generating corresponding incorrect map points in a sparse point cloud map, causing deviation in pose estimation. Therefore, an effective method is needed to distinguish dynamic features or regions. Moving objects will also affect the dense mapping of the SLAM system. Incorrect pose estimation causes the algorithm to incorrectly superimpose multi-frame observation information, resulting in map distortion. Even if the pose estimation is correct, information of the same moving object at different times is added to the map multiple times, forming a "ghost image" of the object's movement, which cannot correctly reflect the true state of the scene.

[0004] For an indoor dynamic environment, most of the existing solutions have deficiencies in dense mapping or computational efficiency. Therefore, it is of great significance to implement visual SLAM with real-time dense mapping ability while ensuring computational efficiency and not relying on high-performance hardware. Summary of the Invention

[0005] In order to overcome the deficiencies of the prior art, such as pose estimation drift and map construction errors caused by interference from dynamic objects in a dynamic environment, the present invention proposes a robot RGB-D SLAM method based on grid segmentation and dual-map coupling, which is applicable to an indoor dynamic environment and improves the performance of the mobile robot SLAM method under this working condition.

[0006] The technical solution adopted by the present invention to solve its technical problems is as follows:

[0007] A robot RGB-D SLAM method based on grid segmentation and dual-map coupling, comprising the following steps:

[0008] Step 1: For the images captured by the RGB-D camera, extract and match the ORB feature points in the grayscale images, perform homography motion compensation on the images based on the principle of homography transformation, and use the bidirectional compensation optical flow method designed according to the optical flow theory to identify the dynamic feature points in the images. The process is as follows:

[0009] Step 1.1: For the images captured by the RGB-D camera, extract the ORB feature points in the grayscale images and perform feature matching;

[0010] Step 1.2: First, solve the homography matrix. The homography matrix describes the mapping relationship between two planes. The coordinate correspondence of points before and after homography transformation is calculated by Equation (1). In the case where the image contains a clear foreground and background and the moving foreground does not occupy most of the pixels in the image, the homography transformation can compensate for the background change of the image caused by the camera's own motion.

[0011] x t+1 =H t+1,t x t (1)

[0012] Where, H t+1,t is the 3×3 homography matrix between the t-th frame and the (t + 1)-th frame, x t and x t+1 are the homogeneous coordinates of the corresponding points between the t-th frame and the (t + 1)-th frame respectively. Since x t and x t+1 are homogeneous coordinates, this equation is a homogeneous coordinate equation, that is, multiplying both sides of the equation by any non-zero constant still holds. Express Equation (1) in the form of (2);

[0013]

[0014] Where, x t =[x t y t 1] T ,x t+1 =[x t+1 y t+1 1] T ,h 11 to h 33 are the elements of the homography matrix H t+1,t ,k is any non-zero constant;

[0015] The calculation of the homography matrix for two frames of images depends on the corresponding point information between the two frames. Since the feature point pairs in the actual image cannot all satisfy the same rigid body perspective transformation, the LMedS (Least Median of Squares) method is used to determine the inliers participating in the calculation of the homography matrix, and the Levenberg-Marquardt method is used to iteratively solve the homography matrix;

[0016]

[0017] Among them, (x i,t , y i,t ) and (x i,t+1 , y i,t+1 ) are the corresponding coordinates in the i-th t-frame and t+1-frame respectively, and Median represents taking the median of all data samples;

[0018] As shown in Equation (3), the pixel distance between the transformed coordinate sum and the actual corresponding coordinate is used as the error term, and the solution of LMedS minimizes the median of the selected point set error;

[0019] The required corresponding point information between two frames is obtained by the optical flow method; the ORB feature points extracted from the t-th frame are tracked to the t+1-th frame using the Lucas-Kanade sparse optical flow method to obtain the corresponding point positions;

[0020] Step 1.3: Propose a bidirectional compensation optical flow method based on the optical flow principle. In Step 1.2, the forward optical flow calculation has been performed on the previous and subsequent frames t and t+1. According to the corresponding point information, calculate the homography matrix, apply this homography transformation to the t-th frame image to obtain the compensated image, and at this time, perform backward optical flow tracking from the t+1-th frame to the compensated image to obtain the backward optical flow field that eliminates interference;

[0021] During the bidirectional compensation optical flow, denote the time of the t-th frame as t. The pixel at (x, y) at time t moves to (x+dx, y+dy) at time t+dt. According to the gray-scale invariance assumption in the optical flow method, there is the forward optical flow gray-scale relational expression (4), where I(x, y, t) represents the pixel gray-scale at (x, y) at time t, and I(x+dx, y+dy, t+dt) represents the pixel gray-scale at (x+dx, y+dy) at time t+dt. The main significance of the forward optical flow is to establish the corresponding point connection between the previous and subsequent frames and calculate the homography matrix;

[0022] I(x+dx, y+dy, t+dt) = I(x, y, t) (4)

[0023] I(x b , y b , t) = I(x+dx, y+dy, t+dt) (5)

[0024] During the backward optical flow process, the corresponding point position (x b , y b ) is calculated from the pixel position (x+dx, y+dy) at time t+dt in the t-th frame image after homography compensation, and use I(x b , y b, t) represents the pixel gray value at that moment and position, and there is the reverse optical flow gray value relation formula (5);

[0025] The optical flow vector of the forward optical flow is formula (6), and the optical flow vector of the reverse optical flow is defined as the opposite vector of the original optical flow loss in this process, as shown in formula (7);

[0026] v forward =(x + dx, x + dy)-(x, y)=(dx, dy) (6)

[0027]

[0028] Among them, v forward is the forward optical flow vector, and v backward is the reverse optical flow vector;

[0029] The difference between the two vectors (x–x b , y - y b ) characterizes the homography motion compensation for the background motion caused by the camera's own motion, making v backward close to the displacement of the moving foreground;

[0030] Compared with the result of the forward optical flow, the movement of the feature points in the static background after the reverse optical flow calculation is effectively canceled, so the reverse optical flow vector can characterize the motion direction and speed of a feature point to a certain extent;

[0031] From the perspective of a multi-frame image sequence, in order to improve the reliability, the bidirectional compensation optical flow method calculates the reverse optical flow field 2 between frame t and frame t + 2 on the basis of calculating the reverse optical flow field 1 between frame t and frame t + 1, and compares and analyzes the optical flow loss in the two optical flow fields;

[0032] Except for some significantly incorrect points, the time span of the reverse optical flow field is longer, and the movement of the moving foreground is more significant. Determine whether the feature points are moving stably according to the following conditions:

[0033] 1) The norms of both vectors are greater than a certain threshold δ;

[0034] 2) The norm of the vector in field 2 is greater than the norm of the corresponding vector in field 1;

[0035] 3) The included angle between the two vectors is less than 180°, that is, the inner product is greater than 0;

[0036]

[0037] The above conditions are expressed as formula (8), v1 and v2 are the corresponding optical flow vectors in the two optical flow fields respectively, and δ is selected as a very small value, which is less affected by the degree of motion;

[0038] Step 2: Divide the input image into rectangular grid regions, convert the dynamic feature points into dynamic region representations, and optimize the dynamic regions according to the geometric connectivity and depth value clustering method to obtain the final grid-based motion segmentation image. The process is as follows:

[0039] Step 2.1: Divide the input image into 20×20 rectangular grid regions. Convert the dynamic feature points into dynamic regions through grid division, mark the blocks containing dynamic feature points as suspected dynamic blocks, and count the number of dynamic feature points in each rectangular region at the same time;

[0040] Step 2.2: Based on the obtained dynamic region results, process the isolated blocks and enclosed blocks therein based on geometric connectivity; if a dynamic block is isolated and the count of dynamic feature points is less than the set threshold (taken as 3), mark this block as static; if a static block is surrounded by a large number of dynamic blocks, mark this block as moving;

[0041] Step 2.3: Downsample the depth image to a resolution of 20×20, corresponding to the divided image grid, to reduce the computational amount of the clustering algorithm; use the depth value as a feature and perform depth value clustering using the K-Means++ method. The clustering result can, to a certain extent, distinguish objects at different depth levels. Expand and fill the dynamic regions with reference to the depth clustering result. If the overlap degree between a connected region in the dynamic region and a connected region in a certain clustering cluster is higher than the set threshold ε, mark the non-dynamic regions belonging to the same clustering cluster around the dynamic region as dynamic regions;

[0042]

[0043] The calculation method of the overlap degree r is shown in Equation (9), where s is the area of the overlapping region, s1 is the area of the connected region of the clustering cluster, and s2 is the area of the dynamic region;

[0044] Step 3: Make a judgment on the motion nature of the feature points in the current frame according to the grid segmentation, initialize the sparse point cloud map, and calculate the camera pose of the current frame removing the dynamic influence by minimizing the reprojection error. The process is as follows:

[0045] Step 3.1: If the input image is the first frame in the video stream, calculate the three-dimensional point coordinates using the depth measurement values of the feature points in this frame, insert them into the map, and initialize the sparse point cloud map;

[0046] Step 3.2: After obtaining the dynamic region segmentation result by combining the RGB image and the depth image, make a judgment on the motion nature of the feature points in the current frame according to the grid segmentation. The feature points falling within the dynamic region are dynamic feature points, denoted as the set χ s ;

[0047] Step 3.3: Calculate the camera pose of the current frame by minimizing the reprojection error, as shown in Equation (10). Among them, the error term is not calculated for dynamic feature points and is not added to the optimization problem;

[0048]

[0049] Among them, T cw is the coordinate transformation from the world coordinate system to the camera coordinate system, that is, the camera pose. ρ(·) is the Huber robust loss function, and x i is the coordinate of the static feature point, and P i is the corresponding three-dimensional point coordinate;

[0050] π(·) is the projection function of the camera. For a certain three-dimensional point [X Y Z] T , there is:

[0051]

[0052] The other variables involved are the camera internal parameters;

[0053] Σ is the information matrix, and there is:

[0054] Σ = n·E (12)

[0055] n is the number of layers of the image pyramid where the current feature point is located, and E is a 3×3 identity matrix;

[0056] Use the Gauss-Newton method to solve the problem of minimizing the reprojection error and optimize to obtain the camera pose estimation;

[0057] Step 4: Combine the camera pose, RGB-D image, and grid-based motion segmentation image to construct a static octree map of the scene. The process is as follows:

[0058] Step 4.1: In order to reduce the computational load and redundancy, when updating the map, downsample the image by 1 / 4, and select 1 pixel out of every 4 pixels in the row and column for update; Apply the grid-based motion segmentation to the establishment of the octree map. The algorithm only updates the octree map according to the image information in the static area and does not insert the corresponding nodes of the dynamic area into the octree map, to a certain extent avoiding the information of dynamic objects being recorded in the map;

[0059] Step 4.2: When updating the map, adopt a ray traversal method. Project a ray from the camera optical center O to the spatial point P i corresponding to the plane pixel point p i and traverse all the nodes passed by the ray. There are the following three situations:

[0060] (4.2.1) If the light passes through a certain number of occupied nodes, then all the occupied nodes on the path are set to empty, and the end node is set to occupied;

[0061] (4.2.2) If the light does not pass through an occupied node, but the end point lands on an occupied node, then the map is not updated;

[0062] (4.2.3) If the light does not pass through an occupied node, and the end point does not land on an occupied node, then the end node is set to occupied;

[0063] The ray traversal calculation in the octree is implemented by a three-dimensional numerical differentiation algorithm;

[0064] Step 5: Gradually construct the sparse point cloud map of the scene. In this process, the octree map is combined for dual-map coupling. On the key frames, a method based on grid segmentation and octree map ray traversal is used to filter the static map points and update the sparse point cloud map to ensure the positioning accuracy. The process is as follows:

[0065] Step 5.1: On the key frames, based on the depth measurement results of the RGB-D camera, restore the coordinates of some ORB feature points in the three-dimensional space and add them to the sparse point cloud map for feature matching and pose calculation;

[0066] After the map points belonging to dynamic objects are added to the map, it will cause incorrect matching during pose tracking at this position later, affecting the pose estimation accuracy. To reduce this effect, the dynamic region mask generated by motion segmentation can be used to help the sparse point cloud map filter out some dynamic map points. In addition, a method based on the octree map ray traversal method will be used to further remove some dynamic map points to achieve the coupling between the two maps and guide the sparse point cloud mapping through the previous understanding of the three-dimensional structure of the scene;

[0067] According to the two-dimensional coordinates of the feature points, the corresponding depth value can be queried in the depth map, denoted as z c ; Further, the two-dimensional coordinates of the feature point P = [u v 1] T are back-projected into the three-dimensional coordinates P in the camera coordinate system c = [x c y c z c T , where the elements correspond to the components of each axis of the coordinate, and K is the camera internal parameter matrix:

[0068] P c = z c K -1 P (13)

[0069] According to the camera pose transformation T cwObtain the coordinates P of this point in the world coordinate system w = [x y z] T :

[0070] P w = T cw -1 P c = R -1 (P c - t)(14)

[0071] The camera pose transformation T cw is represented as a rotation matrix R and a translation vector t. The coordinates P of the camera in the world coordinate system cam is t, represented in the form of three-axis components, as follows:

[0072] P cam = t = [x cam y cam z cam T (15)

[0073] Perform a ray traversal in the octree map from P cam to P w in the direction. The projected ray is a ray, not just traversing the nodes between two points; Denote the coordinates of the first traversed node as P w ' = [x' y' z'] T , transform it to the camera coordinate system to obtain the depth value z c ' of this point; Since the information in the octree map is updated according to the previous key frames, the depth value obtained by ray traversal in the octree and the depth value measured by the camera may have a large difference for map points that are likely to move between key frames;

[0074] The RGB-D camera has a large error when measuring the depth of distant objects. Therefore, only map points within 4m are checked for depth values. When the traversed depth z c ' is within the error range near the measured depth z c , the map point is retained and inserted into the sparse point cloud map. Represent this process as Equation (16):

[0075]

[0076] where Δ is the size of the error range. The error of this process comes from the measurement error of the RGB-D camera used, the error caused by pose estimation, and the accuracy of octree modeling, expressed as:

[0077] Δ = Δ z + Δ t + Δ o (17) ​

[0078] Among them, Δ z is the measurement error of depth by the RGB-D camera, and Δ t is the error of pose estimation, and Δ o is the error of the octree modeling accuracy. Δ z is a function of the actual measured depth. The greater the actual depth, the greater the error. A relational expression can be obtained through reasonable modeling and error analysis of the used RGB-D camera.

[0079] The technical concept of the present invention is as follows: The present invention is a robot RGB-D SLAM method based on grid segmentation and dual-map coupling applicable to indoor dynamic environments. A grid segmentation method is proposed to replace pixel-level segmentation. A homography motion compensation and a bidirectional compensation optical flow method designed according to the optical flow theory are introduced. Combining geometric connectivity and depth value clustering, frame-by-frame dynamic region segmentation is performed to avoid dynamic feature points from participating in pose optimization and significantly optimize the calculation speed. At the same time, a sparse point cloud map and a static octree map are constructed and coupled. A method based on grid segmentation and octree map ray traversal is used to screen static map points on key frames to update the sparse point cloud map and ensure the positioning accuracy.

[0080] The beneficial effects of the present invention are mainly manifested in: (1) low requirements for hardware, eliminating dynamic scene interference and having high positioning accuracy; (2) high calculation efficiency; (3) dynamic object segmentation and marking; (4) online dense mapping. Brief Description of the Drawings

[0081] Figure 1 is the system flowchart of the specific embodiment of the present invention;

[0082] Figure 2 is the schematic diagram of the principle of the bidirectional compensation optical flow in the specific embodiment of the present invention;

[0083] Figure 3 is the schematic diagram of the comparison and fusion of multi-frame optical flow information in the specific embodiment of the present invention;

[0084] Figure 4 is the grid-based motion segmentation obtained in the specific embodiment of the present invention;

[0085] Figure 5 is the schematic diagram of the principle of the ray traversal method in the specific embodiment of the present invention;

[0086] Figure 6 is the sparse point cloud map of the scene constructed in the specific embodiment of the present invention;

[0087] Figure 7 is the octree map of the scene constructed in the specific embodiment of the present invention. Specific Embodiment

[0088] The present invention will be further described below with reference to the accompanying drawings.

[0089] Referring to Figures 1 to 7 , a robot RGB-D SLAM method based on grid segmentation and dual-map coupling includes the following steps:

[0090] Step 1: For the images captured by the RGB-D camera, extract and match the ORB feature points in the grayscale image, perform homography motion compensation on the image based on the principle of homography transformation, and use the bidirectional compensation optical flow method designed according to the optical flow theory to identify the dynamic feature points in the image. The process is as follows:

[0091] Step 1.1: For the images captured by the RGB-D camera, extract the ORB feature points in the grayscale image and perform feature matching.

[0092] Step 1.2: First, solve the homography matrix. The homography matrix describes the mapping relationship between two planes. The coordinate correspondence of points before and after homography transformation is calculated by Equation (1). In the case where the image contains a clear foreground and background and the moving foreground does not occupy most of the pixels in the image, the homography transformation compensates for the background change of the image caused by the camera's own movement.

[0093] x t+1 = H t+1,t x t (1)

[0094] where H t+1,t is the 3×3 homography matrix between the t-th frame and the (t + 1)-th frame, x t and x t+1 are the homogeneous coordinates of the corresponding points between the t-th frame and the (t + 1)-th frame respectively. Since x t and x t+1 are homogeneous coordinates, this equation is a homogeneous coordinate equation, that is, multiplying both sides of the equation by any non-zero constant still holds. Express Equation (1) in the form of (2);

[0095]

[0096] where x t = [x t y t 1] T , x t+1 = [x t+1 y t+1 1] T , h 11 to h 33 are the elements of the homography matrix H t+1,t respectively, and k is any non-zero constant;

[0097] The calculation of the homography matrix between two frames depends on the corresponding point information between the two frames. Since all the feature point pairs in the actual image cannot satisfy the same rigid body perspective transformation, the LMedS (Least Median of Squares) method is used to determine the inliers participating in the homography matrix calculation, and the Levenberg-Marquardt method is used to iteratively solve the homography matrix;

[0098]

[0099] where, (x i,t , y i,t ) and (x i,t+1 , y i,t+1 ) are the corresponding coordinates in the i-th frame t and frame t + 1 respectively, and Median represents taking the median of all data samples;

[0100] As shown in Equation (3), the pixel distance between the transformed coordinates sum and the actual corresponding coordinates is used as the error term, and the solution of LMedS minimizes the median of the selected point set error;

[0101] The corresponding point information between the two frames required in this step is obtained by the optical flow method; the ORB feature points extracted in the t-th frame are tracked to the (t + 1)-th frame using the Lucas-Kanade sparse optical flow method to obtain the corresponding point positions;

[0102] Step 1.3: Propose a bidirectional compensation optical flow method based on the optical flow principle. In Step 1.2, the forward optical flow has been calculated for the two consecutive frames t and t + 1. According to the corresponding point information, the homography matrix is calculated and the homography transformation is applied to the t-th frame image to obtain a compensated image. At this time, the reverse optical flow tracking is performed from the (t + 1)-th frame to the compensated image to obtain a reverse optical flow field that eliminates interference;

[0103] During the bidirectional compensation optical flow, denote the time of the t-th frame as t. The pixel located at (x, y) at time t moves to (x + dx, y + dy) at time t + dt; according to the gray-scale invariance assumption in the optical flow method, there is a forward optical flow gray-scale relational equation (4), where I(x, y, t) represents the pixel gray-scale at (x, y) at time t. The significance of the forward optical flow is mainly to establish the corresponding point connection between the two consecutive frames and calculate the homography matrix;

[0104] I(x + dx, y + dy, t + dt) = I(x, y, t) (4)

[0105] I(x b , y b , t) = I(x + dx, y + dy, t + dt) (5)

[0106] During the reverse optical flow process, the corresponding point position (x b , y b ) is calculated in the image at time t with homography compensation from the pixel position (x + dx, y + dy) at time t + dt. The pixel gray value at this time and position is represented by I(x b , y b , t), and there is the reverse optical flow gray value relation formula (5).

[0107] The optical flow vector of the forward optical flow is formula (6), and the optical flow vector of the reverse optical flow is defined as the opposite vector of the original optical flow loss in this process, as shown in formula (7);

[0108] v forward = (x + dx, x + dy) - (x, y) = (dx, dy) (6)

[0109] v backward = -((x b , y b ) - (x + dx, y + dy)) (7)

[0110] = (dx + x - x b , dy + y - y b )

[0111] where v forward is the forward optical flow vector and v backward is the reverse optical flow vector;

[0112] The difference between the two vectors (x – x b , y - y b ) characterizes the compensation of the homography motion compensation for the background motion caused by the camera's own motion, making v backward close to the displacement of the moving foreground;

[0113] Compared with the result of the forward optical flow, the movement of the feature points in the static background after the reverse optical flow calculation is effectively canceled, so that the reverse optical flow vector can characterize the movement direction and speed of a feature point to a certain extent;

[0114] From the perspective of a multi-frame image sequence, in order to improve the reliability, the bidirectional compensation optical flow method calculates the reverse optical flow field 1 between frame t and frame t + 1, and similarly calculates the reverse optical flow field 2 between frame t and frame t + 2 and compares and analyzes the optical flow loss in the two optical flow fields;

[0115] Except for some significantly incorrect points, the time span of the reverse optical flow field 2 is longer and the movement of the moving foreground is more significant. The following conditions are used to judge whether the feature points are moving stably:

[0116] 1) The norms of both vectors are greater than a certain threshold δ;

[0117] 2) The magnitude of the vector in the reverse optical flow field 2 is greater than the magnitude of the corresponding vector in the reverse optical flow field 1;

[0118] 3) The included angle between the two vectors is less than 180°, that is, the inner product is greater than 0.

[0119]

[0120] The above conditions are expressed as Equation (8), where v1 and v2 are the corresponding optical flow vectors in the two optical flow fields respectively, and δ is selected as a very small value, which is less affected by the degree of motion.

[0121] As Figure 2 shown, it is a schematic diagram of the principle of bidirectional compensation optical flow, which describes the principle and implementation process of the bidirectional compensation optical flow method proposed by the present invention.

[0122] As Figure 3 shown, it is a schematic diagram of the comparison and fusion of multi-frame optical flow information, which describes the way of the present invention to fuse the reverse optical flow field information between multiple frames, takes into account the consistency of the object motion direction and speed in a short time, and removes the interference of the optical flow method's incorrect tracking and the camera's high-frequency jitter.

[0123] Step 2: Divide the input image into rectangular grid regions, convert the dynamic feature points into dynamic region representations, and optimize the dynamic regions according to the geometric connectivity and depth value clustering method to obtain the final grid-based motion segmentation image. The process is as follows:

[0124] Step 2.1: Divide the input image into 20×20 rectangular grid regions. Convert the dynamic feature points into dynamic regions through grid division. Mark the blocks containing dynamic feature points as suspected dynamic blocks, and at the same time count the number of dynamic feature points in each rectangular region;

[0125] Step 2.2: Based on the obtained dynamic region results, process the isolated blocks and the surrounded blocks therein based on geometric connectivity; if a dynamic block is isolated and the count of dynamic feature points is less than the set threshold (taken as 3), mark this block as static; if a static block is surrounded by more dynamic blocks, mark this block as moving;

[0126] Step 2.3: Downsample the depth image to a resolution of 20×20, corresponding to the divided image grid, to reduce the computational amount of the clustering algorithm; use the K-Means++ method for depth value clustering with the depth value as the feature. The clustering result can, to a certain extent, distinguish objects at different depth levels, and expand and fill the dynamic regions with reference to the depth clustering result; if the coincidence degree between a connected region in the dynamic region and a connected region in a certain clustering cluster is higher than a certain threshold ε, mark the non-dynamic regions belonging to the same clustering cluster around the dynamic region as dynamic regions.

[0127]

[0128] The calculation method of the overlap degree is shown in Equation (9), where s is the area of the overlapping region, s1 is the area of the connected region of the clustering cluster, and s2 is the area of the dynamic region;

[0129] As Figure 4 shown, it is the grid-based motion segmentation image optimized from the specific implementation manner of the present invention. In the figure, the dynamic region in the picture is marked by the grid.

[0130] Step 3: Make a judgment on the motion nature of the feature points in the current frame according to the grid segmentation, initialize the sparse point cloud map, and calculate the camera pose of the current frame after removing the dynamic influence by minimizing the reprojection error. The process is as follows:

[0131] Step 3.1: If the input image is the first frame in the video stream, calculate the three-dimensional point coordinates using the depth measurement values of the feature points in this frame, insert them into the map, and initialize the sparse point cloud map;

[0132] Step 3.2: After obtaining the dynamic region segmentation result by combining the RGB image and the depth image, make a judgment on the motion nature of the feature points in the current frame according to the grid segmentation. The feature points falling within the dynamic region are dynamic feature points, denoted as the set χ s ;

[0133] Step 3.3: Calculate the camera pose of the current frame by minimizing the reprojection error, as shown in Equation (10). Among them, the error term is not calculated for the dynamic feature points and is not added to the optimization problem;

[0134]

[0135] Among them, T cw is the coordinate transformation from the world coordinate system to the camera coordinate system, that is, the pose of the camera, ρ(·) is the Huber robust loss function, x i is the coordinate of the static feature point, and P i is the corresponding three-dimensional point coordinate;

[0136] π(·) is the projection function of the camera. For a certain three-dimensional point [X Y Z] T , there is:

[0137]

[0138] The other variables involved therein are the camera internal parameters;

[0139] Σ is the information matrix, and there is:

[0140] Σ = n·E (12)

[0141] n is the layer number of the image pyramid where the current feature point is located, and E is a 3×3 identity matrix;

[0142] The Gauss-Newton method is used to solve the problem of minimizing the reprojection error, and the pose estimation of the camera is optimized;

[0143] Step 4: Combine the camera pose, RGB-D image, and grid-based motion segmentation image to construct a static octree map of the scene. The process is as follows:

[0144] Step 4.1: To reduce the computational load and redundancy, when updating the map, the image is downsampled by a factor of 1 / 4, and one pixel is selected from every 4 pixels in both the row and column directions for updating. The grid-based motion segmentation is applied to the construction of the octree map. The algorithm updates the octree map only based on the image information in the static region, and does not insert the corresponding nodes of the dynamic region into the octree map, thus avoiding to a certain extent the recording of the information of dynamic objects into the map;

[0145] Step 4.2: When updating the map, a ray traversal method is adopted. A ray is projected from the camera optical center O to the spatial point P i corresponding to the planar pixel point p i to traverse all the nodes crossed by the ray. There are the following three situations:

[0146] (4.2.1) If the ray passes through a certain number of occupied nodes, then all the occupied nodes on the path are set to empty, and the end node is set to occupied;

[0147] (4.2.2) If the ray does not pass through an occupied node, but the end point falls on an occupied node, then the map is not updated;

[0148] (4.2.3) If the ray does not pass through an occupied node, and the end point does not fall on an occupied node, then the end node is set to occupied;

[0149] The ray traversal calculation in the octree is implemented by a three-dimensional numerical differentiation algorithm.

[0150] As Figure 5 shown, it is a schematic diagram of the principle of the ray traversal method in the specific embodiment of the present invention. The figure shows three situations where the ray passes through the occupied nodes during the ray traversal process.

[0151] As Figure 7 shown, it is the scene octree map constructed in the specific embodiment of the present invention. Since the pose estimation is not interfered by dynamic objects, the map does not show distortion, and the room structure is clear. The obtained octree map has a high resolution, contains the color information of the nodes, and restores the original appearance of the scene. The map effectively eliminates the "ghost images" caused by dynamic objects according to the motion segmentation and subsequent observations, and only contains the static elements in the scene, which is suitable for applications such as mobile robot navigation.

[0152] Step 5: Gradually construct the sparse point cloud map of the scene. In this process, combine the octree map for dual-map coupling. Use the method based on grid segmentation and octree map ray traversal to filter static map points on key frames, update the sparse point cloud map, and ensure the positioning accuracy. The process is as follows:

[0153] Step 5.1: On key frames, restore the coordinates of some ORB feature points in the three-dimensional space through the depth measurement results of the RGB-D camera, and add them to the sparse point cloud map for feature matching and pose calculation;

[0154] As Figure 6 shown, it is the sparse point cloud map of the scene constructed in the specific embodiment of the present invention;

[0155] After the map points belonging to dynamic objects are added to the map, it will cause incorrect matching during subsequent pose tracking at this position, affecting the pose estimation accuracy. To mitigate this effect, the dynamic region mask generated by motion segmentation can be used to help the sparse point cloud map filter out some dynamic map points. In addition, a method based on the octree map ray traversal method will be used to further eliminate some dynamic map points to achieve the coupling between the two maps, and guide the sparse point cloud mapping through the prior knowledge of the three-dimensional structure of the scene.

[0156] According to the two-dimensional coordinates of the feature points, the corresponding depth value can be queried in the depth map, denoted as z c , and further project the two-dimensional coordinates of the feature points P = [u v 1] T back to the three-dimensional coordinates P c = [x c y c z c in the camera coordinate system. The elements correspond to the component of each axis of the coordinate, and K is the camera internal parameter matrix: T P c = z c K -1 P (13)

[0158] According to the camera pose transformation T cw w obtain the coordinates P T = [x y z] w of this point in the world coordinate system: cw :

[0159] P -1 = T c -1 P c = R cw (P cam - t) (14)

[0160] The camera pose transformation T cw can be represented as a rotation matrix R and a translation vector t. The coordinate P of the camera in the world coordinate system cam is t, expressed in the form of three-axis components, as follows:

[0161] P cam = t = [x cam y cam z cam T (15)

[0162] In the octree map, perform a ray traversal in the direction from P cam to P w . Different from before, the projected ray is a ray, not just traversing the nodes between two points. Denote the coordinates of the first traversed node as P w ′ = [x′ y′ z′] T , transform it to the camera coordinate system to obtain the depth value z c ′. Since the information in the octree map is updated according to the previous key frames, map points with a large difference between the depth values obtained by ray traversal in the octree and the depth values measured by the camera are likely to move between key frames;

[0163] The RGB-D camera has a large error when measuring the depth of distant objects. Therefore, only map points within 4m are checked for depth values. When the traversed depth z c ′ is within the error range near the measured depth z c , the map point is retained and inserted into the sparse point cloud map. Express this process as Equation (16):

[0164]

[0165] where Δ is the size of the error range. The error of this process mainly comes from the measurement error of the RGB-D camera used, the error caused by pose estimation, and the accuracy of octree modeling, expressed as:

[0166] Δ = Δ z + Δ t + Δ o (17)

[0167] where, Δ z is the measurement error of the RGB-D camera for depth, Δ t is the error of pose estimation, Δ o is the error of octree modeling accuracy, Δ z ​It is a function of the actual depth measured. The greater the actual depth, the greater the error. The relationship can be obtained by reasonable modeling and error analysis of the RGB-D camera used.

[0168] Table 1 shows the absolute trajectory error statistics of the present invention on the TUM dataset

[0169]

[0170] Table 1

[0171] The test results of the specific embodiments of the present invention on the TUM data set are shown in Table 1. The RMSE (root mean square error) of the absolute trajectory error is used as an evaluation index to evaluate the ability of the present invention to accurately locate in an indoor dynamic environment, and the ORB-SLAM3 algorithm is used as a benchmark for comparison. The "sitting" image sequence contains only very few human limb movements, which is called a low-dynamic sequence, and the "walking" image sequence contains more human walking scenes, which is called a high-dynamic sequence. In the low-dynamic sequence, the present invention has similar positioning accuracy to ORB-SLAM3. In the high-dynamic sequence, ORB-SLAM3 is severely disturbed by dynamic objects in multiple scenes, resulting in posture drift and high positioning errors, while the results output by the present invention are greatly improved compared to the ORB-SLAM3 method.

[0172] The contents described in the embodiments of this specification are merely enumerations of implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A robot RGB-D SLAM method based on grid segmentation and dual-map coupling, characterized in that, The method includes the following steps: Step 1: For the images captured by the RGB-D camera, extract and match the ORB feature points in the grayscale images, perform homography motion compensation on the images based on the principle of homography transformation, and use the bidirectional compensation optical flow method designed according to the optical flow theory to identify the dynamic feature points in the images; Step 2: Divide the input images into rectangular grid regions, convert the dynamic feature points into dynamic region representations, and optimize the dynamic regions according to the geometric connectivity and depth value clustering method to obtain the final grid-based motion segmentation images; Step 3: Judge the motion properties of the feature points in the current frame according to the grid segmentation, initialize the sparse point cloud map, and calculate the camera pose of the current frame removing the dynamic influence by minimizing the reprojection error; Step 4: Combine the camera pose, RGB-D images, and grid-based motion segmentation images to construct a static octree map of the scene; Step 5: Gradually construct the sparse point cloud map of the scene. In this process, perform dual-map coupling by combining the octree map, use the method based on grid segmentation and octree map ray traversal to filter the static map points on the key frames, update the sparse point cloud map, and ensure the positioning accuracy; The process of Step 1 is as follows: Step 1.1: For the images captured by the RGB-D camera, extract the ORB feature points in the grayscale images and perform feature matching; Step 1.2: First, solve the homography matrix. The homography matrix describes the mapping relationship between two planes. The coordinate correspondence of points before and after homography transformation is calculated by Equation (1). In the case where the image contains a clear foreground and background and the moving foreground does not occupy most of the pixels in the image, the homography transformation compensates for the background change of the image caused by the camera's own motion; x t+1 = H t+1,t x t (1) Among them, H t+1,t is the 3×3 homography matrix between the t-th frame and the (t + 1)-th frame, x t and x t+1 are the homogeneous coordinates of the corresponding points between the t-th frame and the (t + 1)-th frame. Since x t and x t+1 are homogeneous coordinates, this equation is a homogeneous coordinate equation, that is, multiplying both sides of the equation by any non-zero constant still holds. Express Equation (1) in the form of (2); where x t = [x t y t 1] T , x t+1 = [x t+1 y t+1 1] T , h 11 to h 33 are the elements of the homography matrix H t+1,t , and k is an arbitrary non-zero constant; The calculation of the homography matrix between two frames depends on the corresponding point information between the two frames. Since the feature point pairs in the actual images cannot all satisfy the same rigid body perspective transformation, the LMedS method is used to determine the inliers participating in the homography matrix calculation, and the LMedS method is used to iteratively solve the homography matrix; Among them, (x i,t , y i,t ) and (x i,t+1 , y i,t+1 ) are the corresponding coordinates in the i-th t-th frame and the (t + 1)-th frame respectively, and Median means taking the median of all data samples; As shown in Equation (3), take the pixel distance between the transformed coordinate sum and the actual corresponding coordinate as the error term. The solution of LMedS minimizes the median of the selected point set errors; The required corresponding point information between two frames is obtained by the optical flow method; the ORB feature points extracted in the t-th frame are tracked to the (t + 1)-th frame using the Lucas-Kanade sparse optical flow method to obtain the corresponding point positions; Step 1.3: Propose a bidirectional compensation optical flow method based on the optical flow principle. In Step 1.2, the forward optical flow calculation has been performed on the two frames t and t + 1. According to the corresponding point information, calculate the homography matrix, apply the homography transformation to the t-th frame image to obtain the compensated image. At this time, perform the reverse optical flow tracking from the (t + 1)-th frame to the compensated image to obtain the reverse optical flow field eliminating interference; During the two-way compensated optical flow, the time of the t-th frame is denoted as t. The pixel located at (x, y) at time t moves to (x + dx, y + dy) at time t + dt. According to the gray-scale invariance assumption in the optical flow method, there is the forward optical flow gray-scale relation formula (4), where I(x, y, t) represents the pixel gray-scale at (x, y) at time t. The significance of the forward optical flow mainly lies in establishing the correspondence between the front and back two frames and calculating the homography matrix; I(x + dx, y + dy, t + dt) = I(x, y, t) (4) I(x b ,y b ,t) = I(x + dx, y + dy, t + dt) (5) During the reverse optical flow process, the corresponding point position (x b , y b ) is calculated in the image at time t compensated by the homography from the pixel position (x + dx, y + dy) at time t + dt. The pixel gray value at this time and position is represented by I(x b , y b , t), and there is the reverse optical flow gray value relation formula (5); The optical flow vector of the forward optical flow is formula (6), and the optical flow vector of the backward optical flow is defined as the opposite vector of the original optical flow loss in this process, as shown in formula (7); v forward = (x + dx, x + dy) - (x, y) = (dx, dy) (6) Among them, v forward is the forward optical flow vector, and v backward is the backward optical flow vector; The difference between two vectors (x–x b , y–y b ) represents the compensation of the homography motion compensation for the background motion caused by the camera's own motion, making v backward close to the displacement of the moving foreground; On the basis of calculating the backward optical flow field 1 between the t-th frame and the (t + 1)-th frame, similarly calculate the backward optical flow field 2 between the t-th frame and the (t + 2)-th frame and compare and analyze the optical flow losses in the two optical flow fields; Judge whether the feature points make stable movements according to the following conditions: 1) The moduli of both vectors are greater than a certain threshold δ; 2) The modulus of the vector in field 2 is greater than the modulus of the corresponding vector in field 1; 3) The included angle between the two vectors is less than 180°, that is, the inner product is greater than 0; The above conditions are expressed as formula (8), where v1 and v2 are the corresponding optical flow vectors in the two optical flow fields respectively, and δ is selected as a very small value and is less affected by the degree of movement.

2. The robot RGB-D SLAM method based on grid segmentation and dual-map coupling according to claim 1, characterized in that,The process of step 2 is as follows: Step 2.1: Divide the input image into 20×20 rectangular grid regions. Through grid division, convert the dynamic feature points into dynamic regions, mark the blocks containing dynamic feature points as suspected dynamic blocks, and at the same time count the number of dynamic feature points in each rectangular region; Step 2.2: Based on the obtained dynamic region results, process the isolated blocks and the surrounded blocks based on geometric connectivity; if a dynamic block is isolated and the count of dynamic feature points is less than the set threshold, mark this block as static; If a static block is surrounded by many dynamic blocks, mark this block as moving; Step 2.3: Downsample the depth image to 20×20 resolution, corresponding to the divided image grid, to reduce the computational complexity of the clustering algorithm; use the depth value as a feature and use the K-Means++ method for depth value clustering. If the coincidence degree between a connected region in the dynamic region and a connected region in a certain clustering cluster is higher than the set threshold ε, mark the non-dynamic regions belonging to the same clustering cluster around the dynamic region as dynamic regions; The calculation method of the coincidence degree r is shown in formula (9), where s is the area of the coincidence region, s1 is the area of the connected region of the clustering cluster, and s2 is the area of the dynamic region.

3. The robot RGB-D SLAM method based on grid segmentation and dual-map coupling according to claim 2, wherein, The process of step 3 is as follows: Step 3.1: If the input image is the first frame in the video stream, use the depth measurement value of the feature points of this frame to calculate the three-dimensional point coordinates, insert them into the map, and initialize the sparse point cloud map; Step 3.2: After obtaining the dynamic region segmentation result by combining the RGB image and the depth image, judge the motion nature of the feature points in the current frame according to the grid segmentation. The feature points falling within the dynamic region are dynamic feature points, denoted as set χ s ; Step 3.3: Calculate the camera pose of the current frame by minimizing the reprojection error, such as formula (10), where the error term is not calculated for dynamic feature points and is not added to the optimization problem; Among them, T cw is the coordinate transformation from the world coordinate system to the camera coordinate system, that is, the pose of the camera, ρ(·) is the Huber robust loss function, and x i is the coordinate of the static feature point, and P i is the corresponding three-dimensional point coordinate; π(·) is the projection function of the camera. For a certain 3D point [X Y Z] T , we have: The other variables involved therein are the camera internal parameters; Σ is the information matrix, and there is: Σ = n·E (12) n is the layer number of the image pyramid where the current feature point is located, and E is a 3×3 identity matrix; The Gauss-Newton method is used to solve the problem of minimizing the reprojection error, and the pose estimation of the camera is optimized.

4. The robot RGB-D SLAM method based on grid segmentation and dual-map coupling according to claim 3, wherein, The process of step 4 is as follows: Step 4.1: When updating the map, downsample the image by a factor of 1 / 4, and select 1 pixel out of every 4 pixels in both rows and columns for update; apply grid-based motion segmentation to the construction of the octree map. The algorithm only updates the octree map based on the image information in the static area, and does not insert the corresponding nodes of the dynamic area into the octree map, to a certain extent avoiding the information of dynamic objects being recorded in the map; Step 4.2: When updating the map, adopt a ray traversal method to project a ray from the camera optical center O to the spatial point P corresponding to the planar pixel point p, and traverse all the nodes passed through by the ray. There are the following three cases: i The corresponding spatial point P i There are the following three cases: (4.2.1) If the ray passes through a certain number of occupied nodes, then set all the occupied nodes on the path to empty and set the end node to occupied; (4.2.2) If the ray does not pass through an occupied node but the end point falls on an occupied node, then do not update the map; (4.2.3) If the ray does not pass through an occupied node and the end point does not fall on an occupied node, then set the end node to occupied; The ray traversal calculation in the octree is implemented by a three-dimensional numerical differentiation algorithm.

5. The robot RGB-D SLAM method based on grid segmentation and dual-map coupling according to claim 4, wherein, The process of step 5 is as follows: Step 5.1: On the key frame, restore the coordinates of some ORB feature points in the three-dimensional space through the depth measurement results of the RGB-D camera, and add them to the sparse point cloud map for feature matching and pose calculation; Step 5.2: Use the dynamic area mask generated by motion segmentation to help the sparse point cloud map filter out some dynamic map points. A method based on the octree map ray traversal method will also be used to further eliminate some dynamic map points to achieve the coupling between the two maps, and guide the sparse point cloud mapping through the previous understanding of the three-dimensional structure of the scene; Query the corresponding depth value in the depth map according to the two-dimensional coordinates of the feature points, denoted as z c ; Further, the two-dimensional coordinates of the feature point P = [u v 1] T Back-project it into the three-dimensional coordinates P in the camera coordinate system c = [x c y c z c T , where the elements correspond to the component of each axis of the coordinate, and K is the camera intrinsic matrix:​ P c = z c K -1 P (13) According to the camera pose transformation T cw obtain the coordinates P of this point in the world coordinate system w = [x y z] T : P w = T cw -1 P c = R -1 (P c - t)(14) Camera pose transformation T cw Expressed as a rotation matrix R and a translation vector t, the coordinates P of the camera in the world coordinate system cam Is t, expressed in the form of three-axis components, there is: P cam = t = [x cam y cam z cam T (15)​ In the octree map, perform a ray traversal from P cam to P w in the direction. The projected ray is a ray, not just traversing the nodes between two points; Denote the coordinates of the first traversed node as P w ′ = [x′ y′ z′] T . Transform it to the camera coordinate system to obtain the depth value z c ′ of this point; Since the information in the octree map is updated according to the previous key frames, the map points with a large difference between the depth values obtained by ray traversal in the octree and the depth values measured by the camera are likely to move between key frames; RGB-D cameras have large errors when measuring the depth of distant objects. Therefore, depth value checks are only performed on map points within 4m. When traversing the depth z c ′ when measuring the depth z c within the error range near, the map point is retained and inserted into the sparse point cloud map. This process is expressed as Equation (16): where Δ is the error range. The error in this process comes from the measurement error of the RGB-D camera used, the error brought by pose estimation, and the accuracy of octree modeling, which is expressed as: Δ = Δ z + Δ t + Δ o (17) Among them, Δ z is the measurement error of depth by the RGB-D camera, and Δ t is the error of pose estimation, and Δ o is the error of the octree modeling accuracy.

Images