Dynamic slam system and method based on image segmentation

Abstract

The invention discloses a dynamic slam system and method based on image segmentation. The system comprises an image segmentation thread module, a tracking thread module, a local mapping thread module and a loopback detection thread module. The invention designs a robot positioning and mapping system capable of running in real time in a high dynamic environment through an image segmentation thread, a tracking thread, a local mapping thread and a loop detection thread of the system. The system identifies an area in a high-dynamic scene, divides the area into a dynamic area and a static area, and then calculates a dynamic object in a new image frame through a dynamic area tracking algorithm, thereby establishing a static map, and ensuring that a camera track is not influenced by the dynamic object and accords with a real motion track. The method can be used for robot positioning in a dynamic scene and positioning of VR / AR and other virtual reality devices in a high-dynamic environment.

Description

Technical Field

[0001] The invention relates to robot technology, and in particular to a dynamic SLAM system and method based on image segmentation. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) is an important research direction in the field of robotics. Its purpose is to enable robots to perform localization and map construction simultaneously. Dynamic SLAM refers to a SLAM algorithm that can effectively identify and handle dynamic obstacles during the movement of the robot. With the rapid development of robotics, dynamic SLAM algorithms have become one of the research hotspots and have attracted the attention of many researchers.

[0003] SLAM, or simultaneous localization and mapping, refers to building a map of the environment in real time and determining the robot's own position in an unknown environment by moving the robot and using its sensor information. This is an important issue in the field of robotics and has a wide range of applications, such as autonomous navigation, unmanned vehicles, smart homes, industrial automation, etc. Figure 1 .

[0004] Dynamic SLAM refers to the technology of simultaneously positioning the robot and building an environmental map in a dynamic environment. Traditional SLAM algorithms usually assume that the environment is static, so using traditional SLAM algorithms in a dynamic environment may lead to inaccuracies in the robot's position and map. The background of dynamic SLAM technology can be traced back to the development of the field of robotics. With the continuous expansion of robot application scenarios, robots must be able to move and operate autonomously in complex dynamic environments. Therefore, dynamic SLAM technology has become an important research direction in the field of robotics.

[0005] In traditional static SLAM, the map constructed by the robot is usually based on static obstacles in the environment, without considering the dynamic obstacles in the environment. However, in practical applications, dynamic obstacles in the environment are often unavoidable, such as other moving robots, humans, animals, etc. These dynamic obstacles may affect the real-time positioning and mapping of the robot, and even cause the robot's movement to fail.

[0006] Therefore, studying a SLAM algorithm that can effectively handle dynamic obstacles is of great significance to the development of robotics technology. Summary of the invention

[0007] Purpose of the invention: The purpose of the present invention is to provide a dynamic SLAM system and method based on image segmentation.

[0008] Technical solution: On the one hand, the present invention provides a dynamic SLAM system based on image segmentation, including an image segmentation thread module, a tracking thread module, a local mapping thread module and a loop detection thread module.

[0009] The image segmentation thread module uses a seed filling algorithm to segment, establish feature descriptors for each region, and track dynamic regions, and screen out dynamic regions in real time;

[0010] The tracking thread module realizes visual SLAM through feature extraction and matching, pose estimation, map point depth estimation, relocation and tracking state management;

[0011] The local mapping thread module updates and optimizes the local map of the system in real time through feature extraction and matching, triangulation, key frame selection and map maintenance;

[0012] The loop detection thread module improves the robustness and accuracy of loop detection through key frame screening, feature matching, geometry verification, loop detection and optimization.

[0013] Another aspect of the present invention provides a method for using the dynamic SLAM system for image segmentation, comprising the following steps:

[0014] (1) Perform image segmentation thread;

[0015] (2) Perform image tracking thread;

[0016] (3) Perform local image mapping thread;

[0017] (4) Perform system loop detection thread.

[0018] Furthermore, the method of step (1) comprises:

[0019] A: Seed filling algorithm segmentation

[0020] The depth image and color image are segmented by the connected region analysis algorithm combined with the seed filling algorithm, all pixels are marked, and the segmented image is output;

[0021] B: Create feature descriptors for each region:

[0022] Through formulas 5-1, 5-2 and 5-3, the grayscale centroid of each area in the color image and the depth image are calculated respectively to obtain the centroid coordinates. The size of each area in the color image and the depth image is calculated to obtain the corresponding scale value. The difference between the centroid of each area and the centroid coordinates of its four areas above, below, left and right is calculated in the color image and the depth image. The features calculated above are stored in a vector in the order of grayscale image centroid, color image centroid, grayscale image area size, color image area size, grayscale image four-neighborhood distance, color image four-neighborhood distance. At this point, the feature descriptor of each area is obtained.

[0023]

[0024] C: Dynamic Area Tracking

[0025] According to each obtained area descriptor, find the area corresponding to the descriptor closest to it among all descriptors, complete the matching of each descriptor in the next frame image, and then filter out the dynamic area through geometric constraints and consistency detection to complete the tracking of the current dynamic area.

[0026] Furthermore, the method of step (2) comprises:

[0027] A: Feature extraction and matching

[0028] Use the FAST algorithm to extract feature points in the image and use the ORB descriptor to describe these feature points. Then, use the brute force matching algorithm to match the feature points between adjacent frames.

[0029] B: Pose Estimation

[0030] After obtaining the matching points between adjacent frames, the PnP algorithm is used to estimate the camera's pose, and the RANSAC algorithm is used to remove the mismatched points. In addition, when optimizing the reprojection error from 3D points to 2D points, the feature points in the dynamic area obtained by the segmentation thread are not optimized, so that the camera pose is not affected by the movement of dynamic objects, and the pose trajectory under static conditions is obtained;

[0031] C: Map point depth estimation

[0032] After pose estimation, the depth of map points is estimated using triangulation technology. The system uses triangulation with scale drift correction to solve the scale ambiguity problem.

[0033] D: Relocation

[0034] Determine the camera's position by matching the current frame with the keyframes in the map;

[0035] E: Tracking status management

[0036] The tracking thread uses a state machine to manage the tracking state and performs real-time tracking.

[0037] Furthermore, the method of step (3) comprises:

[0038] Step 1: Feature extraction and matching

[0039] The local mapping thread uses the ORB algorithm to extract feature points in the image and calculates the ORB descriptor of each feature point. It then uses brute force matching or FLANN-based matching methods to find matching point pairs between the current frame and the previous frames.

[0040] Step 2: Triangulation

[0041] After feature matching is completed, the local mapping thread calculates the 3D point cloud based on the matching point pairs and camera pose, and optimizes it by minimizing the reprojection error to improve the precision and accuracy of the 3D points;

[0042] Step 3: Keyframe selection

[0043] Select frames that can provide new information and differentiation as key frames, and use the bag-of-words model-based method to screen and match feature points to improve matching efficiency and accuracy;

[0044] Step 4: Map maintenance.

[0045] Furthermore, map maintenance includes removing redundant map points and keyframes to maintain the size and accuracy of the map. At the same time, the local mapping thread uses prediction and feedback control methods to adapt to scene changes and uncertainties.

[0046] Furthermore, the method of step (4) comprises:

[0047] Step 1: Keyframe Filtering

[0048] There are two types of key frames in this system: one is the key frames generated by motion and field of view changes, and the other is the key frames generated by loop closure. The first step of loop closure detection is to filter out the key frames that may be loop closures from the database.

[0049] The key frame database in this system adopts the bag-of-words model. When loop detection is performed, the words contained in the current frame are first obtained from the database, and then the key frames containing these words are found. These key frames are possible loop key frames.

[0050] Step 2: Feature matching

[0051] The second step of loop detection is to match ORB feature points. The system adopts the bag-of-words-based BoW matching method, which divides the ORB feature points contained in the current frame into different words and searches for key frames matching the current frame in the database. If the number of matching points exceeds a threshold, it is considered that there may be a loop between the two frames.

[0052] Step 3: Geometry Verification

[0053] After feature matching, the loop detection thread will perform geometric verification to ensure that the spatial relationship between matching points conforms to the loop constraint. The system adopts a RANSAC-based method to perform geometric verification by calculating the essential matrix or basic matrix, and improves the robustness of geometric verification by eliminating incorrect matching points.

[0054] Step 4: Loop Detection and Optimization

[0055] If the geometric verification passes, it is considered that there is a loop between the current frame and a previous frame. At this time, the system will perform loop optimization, that is, minimize the error caused by the loop by optimizing the posture relationship of the entire map and adding it to the previous map. Loop optimization usually adopts a method based on nonlinear optimization.

[0056] Furthermore, the method based on nonlinear optimization includes graph optimization or optimization relaxation.

[0057] Compared with the prior art, the present invention has the following beneficial effects:

[0058] The present invention combines image segmentation algorithms to design a robot positioning and mapping system that can run in real time in a high-dynamic environment. The system identifies areas in high-dynamic scenes and divides them into dynamic areas and static areas. It then calculates dynamic objects in new image frames through a dynamic area tracking algorithm, thereby establishing a static map and ensuring that the camera trajectory is not affected by dynamic objects and conforms to the real motion trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 Indoor navigation map for mobile robots;

[0060] Figure 2 The figure is a schematic diagram of the overall process of the motion trajectory tracking system. DETAILED DESCRIPTION

[0061] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be further described below.

[0062] The overall process of the motion trajectory tracking system of this embodiment is as follows: Figure 2 shown.

[0063] The dynamic SLAM system of this embodiment is divided into four parts, namely, image segmentation thread, tracking thread, local mapping thread and loop detection thread.

[0064] The specific process is as follows:

[0065] 1.1 The segmentation thread of this system is mainly divided into three stages:

[0066] Step 1: Segmentation using the seed filling algorithm

[0067] Description: The depth image and color image are segmented by the connected region analysis algorithm combined with the seed filling algorithm. First, the pixels of each frame of the image are traversed to determine whether the pixel has been marked. If so, it is skipped; otherwise, the pixel values ​​of the pixels in the four areas around the pixel are judged. If the pixel difference is less than a certain threshold, it is added to the queue corresponding to the current mark value. The cycle is repeated, and the corresponding pixel point is taken out from the queue each time. After the above judgment, until the queue is empty, the marking of an area is completed. When all pixels are marked, the algorithm ends and the segmented image is output.

[0068] Step 2: Create a feature descriptor for each region:

[0069] Note: Use equations 5-1, 5-2, and 5-3 to calculate the grayscale centroid of each area in the color image and depth image, and get the centroid coordinates. Calculate the size of each area in the color image and depth image, and get the corresponding scale value. Calculate the difference between the centroid of each area and the centroid coordinates of its four areas above, below, left, and right in the color image and depth image. Store the features calculated above into a vector in the order of grayscale image centroid, color image centroid, grayscale image area size, color image area size, grayscale image four-neighborhood distance, color image four-neighborhood distance, and thus obtain the feature descriptor of each area.

[0070]

[0071] In Formula 5-1, V(i, j) represents the grayscale value of the pixel in the i-th row and j-th column of the image. The zero-order moment is obtained by summing all the pixels. In Formula 5-2, the first-order moment in the x-direction and the first-order moment in the y-direction are obtained by multiplying the grayscale value of the pixel and the position coordinates of the pixel and then summing them. In Formula 5-3, the centroid coordinates in the x- and y-directions can be obtained by taking the quotient of the first-order moment and the zero-order moment in the x- and y-directions.

[0072] Step 3: Dynamic Region Tracking

[0073] Description: Traverse each region descriptor obtained in step 2, find the region corresponding to the closest descriptor among all descriptors, that is, the region corresponding to the current region in the next frame, and complete the matching of each descriptor in the next frame image. Then, filter out the dynamic region through geometric constraints and consistency detection, and complete the tracking of the current dynamic region.

[0074] In general, the image segmentation thread screens out dynamic areas in real time through seed filling and connected domain analysis algorithms, establishing feature descriptors, descriptor matching, and dynamic area tracking, in preparation for subsequent pose estimation.

[0075] 1.2 The tracking thread of this system is mainly divided into five stages:

[0076] Step 1: Feature extraction and matching

[0077] Description: This step uses the FAST algorithm to extract feature points in the image and uses the ORB descriptor to describe these feature points. Then, the brute force matching algorithm is used to match the feature points between adjacent frames.

[0078] Step 2: Pose Estimation

[0079] Note: After obtaining the matching points between adjacent frames, the PnP (Perspective-n-Points) algorithm can be used to estimate the camera pose. We use the RANSAC algorithm to remove mismatched points. In addition, when optimizing the reprojection error from 3D points to 2D points, the feature points in the dynamic area obtained by the segmentation thread are not optimized, so that the camera pose is not affected by the movement of dynamic objects, and the pose trajectory under static conditions is obtained.

[0080] Step 3: Map point depth estimation

[0081] Description: After pose estimation, the depth of map points can be estimated using triangulation techniques. The system uses triangulation with scale drift correction to address scale ambiguity.

[0082] Step 4: Relocation

[0083] Note: In some cases, such as when the camera moves violently or the scene changes dramatically, tracking may be lost. To solve this problem, we use relocalization technology. Relocalization determines the position of the camera by matching the current frame with the keyframes in the map.

[0084] Step 5: Tracking State Management

[0085] Description: The tracking thread uses a state machine to manage the tracking state. The state machine has the following states: initialization, tracking, loss, and relocation. Through the management of the state machine, the system can perform different operations in different situations to provide robust real-time tracking.

[0086] In summary, the tracking thread uses techniques such as feature extraction and matching, pose estimation, map point depth estimation, relocalization, and state management to achieve efficient, robust, and real-time visual SLAM.

[0087] 1.3 The local mapping thread of this system is mainly divided into four stages:

[0088] Step 1: Feature extraction and matching

[0089] Note: The local mapping thread will use the ORB algorithm to extract feature points in the image and calculate the ORB descriptor of each feature point, and then use brute force matching or FLANN-based matching methods to find matching point pairs between the current frame and the previous frames. In this process, the local mapping thread will also use the RANSAC algorithm to remove mismatched points to improve matching accuracy.

[0090] Step 2: Triangulation

[0091] Note: After feature matching is completed, the local mapping thread will calculate the 3D point cloud based on the matching point pairs and camera pose, and optimize it by minimizing the reprojection error to improve the precision and accuracy of the 3D points.

[0092] Step 3: Keyframe selection

[0093] Note: The local mapping thread selects key frames according to certain strategies. Generally, it selects frames that can provide new information and discrimination as key frames, and uses a bag-of-words model-based method to screen and match feature points to improve matching efficiency and accuracy.

[0094] Step 4: Map maintenance

[0095] Note: In order to maintain the density and accuracy of the map, the local mapping thread will maintain the map according to certain strategies, such as removing redundant map points and keyframes to maintain the size and accuracy of the map. At the same time, the local mapping thread will also use prediction and feedback control methods to adapt to changes and uncertainties in the scene.

[0096] In general, the local mapping thread is an important module that is used to update and optimize the local map of the system in real time, thereby improving the accuracy and robustness of the system. Specifically, this module calculates the pose of the current frame in the world coordinate system by matching the current frame with the existing key frames in the database, and determines whether the current frame should be added to the map as a new key frame. For new key frames, the local mapping thread needs to triangulate the map points and perform Bundle Adjustment optimization; for existing key frames, it is necessary to update the location and descriptor information of the map points through pose estimation and co-viewing relationship information, and perform Bundle Adjustment optimization. In addition, the local mapping thread also needs to perform map point screening and culling operations to ensure the density and quality of the map.

[0097] 1.4 The loop detection thread of this system is mainly divided into four stages:

[0098] Step 1: Keyframe Filtering

[0099] Note: There are two types of key frames in this system: one is generated by motion and field of view change, and the other is generated by loop closure. The first step of loop closure detection is to filter out key frames that may be loop closures from the database.

[0100] The keyframe database in this system adopts the Bag-of-Words model, that is, the ORB feature points of all keyframes are divided into different words through k-means clustering. When a new keyframe is added to the database, it will be matched with all words and a mapping relationship between keyframes and words will be established. When loop detection is performed, the words contained in the current frame are first obtained from the database, and then the keyframes containing these words are searched. These keyframes are possible loop keyframes.

[0101] Step 2: Feature matching

[0102] Note: The second step of loop detection is to match ORB feature points. The system uses a bag-of-words-based BoW matching method, which divides the ORB feature points contained in the current frame into different words and searches for key frames matching the current frame in the database. If the number of matching points exceeds a threshold, it is considered that there may be a loop between the two frames.

[0103] Step 3: Geometry Verification

[0104] Note: After feature matching, the loop detection thread will perform geometric verification to ensure that the spatial relationship between matching points complies with the loop constraint. The system uses a RANSAC-based method to perform geometric verification by calculating the essential matrix or basic matrix, and improves the robustness of geometric verification by eliminating incorrect matching points.

[0105] Step 4: Loop Detection and Optimization

[0106] Note: If the geometry verification passes, it is considered that there is a loop between the current frame and a previous frame. At this point, the system will perform loop optimization, that is, minimize the error caused by the loop by optimizing the pose relationship of the entire map and add it to the previous map. Loop optimization usually uses methods based on nonlinear optimization, such as graph optimization or optimization relaxation.

[0107] In summary, the loop detection thread of this system uses a variety of technical means, including bag-of-words model, BoW matching, RANSAC geometric verification and graph optimization, to improve the robustness and accuracy of loop detection. At the same time, since this system is a real-time system, the loop detection thread needs to maintain high efficiency while ensuring detection accuracy. To this end, we have adopted some optimization techniques, such as using multi-threading for parallel computing and using GPU acceleration algorithms.

[0108] In practical applications, the loop detection thread of the system needs to consider many factors, such as the impact of environmental lighting changes, object occlusion, dynamic scenes and other factors on ORB feature point matching, as well as the impact of map size and complexity on computing performance. Therefore, the optimization and improvement of the loop detection thread of the system is still an active research field, and new technical means and algorithms can be used to improve performance and efficiency.

[0109] The above is only a preferred embodiment of the present invention and does not limit the present invention in any way. Any technician in the relevant technical field, without departing from the scope of the technical solution of the present invention, makes any form of equivalent replacement or modification to the technical solution and technical content disclosed in the present invention, which does not depart from the content of the technical solution of the present invention and still falls within the protection scope of the present invention.

Claims

1. A dynamic SLAM system based on image segmentation, characterized in that: It includes image segmentation thread module, tracking thread module, local mapping thread module and loop detection thread module. The image segmentation thread module uses a seed filling algorithm to segment, establish feature descriptors for each region, and track dynamic regions, and screen out dynamic regions in real time; The tracking thread module realizes visual SLAM through feature extraction and matching, pose estimation, map point depth estimation, relocation and tracking state management; The local mapping thread module updates and optimizes the local map of the system in real time through feature extraction and matching, triangulation, key frame selection and map maintenance; The loop detection thread module improves the robustness and accuracy of loop detection through key frame screening, feature matching, geometry verification, loop detection and optimization.

2. The method for using the system of claim 1, characterized in that: The steps include: (1) Perform image segmentation thread; (2) Perform image tracking thread; (3) Perform local image mapping thread; (4) Perform system loop detection thread.

3. The method of use according to claim 2, characterized in that: The method of step (1) comprises: A: Seed filling algorithm segmentation The depth image and color image are segmented by the connected region analysis algorithm combined with the seed filling algorithm, all pixels are marked, and the segmented image is output; B: Create feature descriptors for each region: Through formulas 5-1, 5-2 and 5-3, the grayscale centroid of each area in the color image and the depth image are calculated respectively to obtain the centroid coordinates. The size of each area in the color image and the depth image is calculated to obtain the corresponding scale value. The difference between the centroid of each area and the centroid coordinates of its four areas above, below, left and right is calculated in the color image and the depth image. The features calculated above are stored in a vector in the order of grayscale image centroid, color image centroid, grayscale image area size, color image area size, grayscale image four-neighborhood distance, color image four-neighborhood distance. At this point, the feature descriptor of each area is obtained. C: Dynamic Area Tracking According to each obtained area descriptor, find the area corresponding to the descriptor closest to it among all descriptors, complete the matching of each descriptor in the next frame image, and then filter out the dynamic area through geometric constraints and consistency detection to complete the tracking of the current dynamic area.

4. The method of use according to claim 1, characterized in that: The method of step (2) comprises: A: Feature extraction and matching Use the FAST algorithm to extract feature points in the image and use the ORB descriptor to describe these feature points. Then, use the brute force matching algorithm to match the feature points between adjacent frames. B: Pose Estimation After obtaining the matching points between adjacent frames, the PnP algorithm is used to estimate the camera's pose, and the RANSAC algorithm is used to remove the mismatched points. In addition, when optimizing the reprojection error from 3D points to 2D points, the feature points in the dynamic area obtained by the segmentation thread are not optimized, so that the camera pose is not affected by the movement of dynamic objects, and the pose trajectory under static conditions is obtained; C: Map point depth estimation After pose estimation, the depth of map points is estimated using triangulation technology. The system uses triangulation with scale drift correction to solve the scale ambiguity problem. D: Relocation Determine the camera's position by matching the current frame with the keyframes in the map; E: Tracking status management The tracking thread uses a state machine to manage the tracking state and performs real-time tracking.

5. The method of use according to claim 3, characterized in that: The method of step (3) comprises: Step 1: Feature extraction and matching The local mapping thread uses the ORB algorithm to extract feature points in the image and calculates the ORB descriptor of each feature point. It then uses brute force matching or FLANN-based matching methods to find matching point pairs between the current frame and the previous frames. Step 2: Triangulation After feature matching is completed, the local mapping thread calculates the 3D point cloud based on the matching point pairs and camera pose, and optimizes it by minimizing the reprojection error to improve the precision and accuracy of the 3D points; Step 3: Keyframe selection Select frames that can provide new information and differentiation as key frames, and use the bag-of-words model-based method to screen and match feature points to improve matching efficiency and accuracy; Step 4: Map maintenance.

6. The method of use according to claim 5, characterized in that: Map maintenance includes removing redundant map points and keyframes to maintain the size and accuracy of the map. At the same time, the local mapping thread uses prediction and feedback control methods to adapt to scene changes and uncertainties.

7. The method of use according to claim 1, characterized in that: The method of step (4) comprises: Step 1: Keyframe Filtering There are two types of key frames in this system: one is the key frames generated by motion and field of view changes, and the other is the key frames generated by loop closure. The first step of loop closure detection is to filter out the key frames that may be loop closures from the database. The key frame database in this system adopts the bag-of-words model. When loop detection is performed, the words contained in the current frame are first obtained from the database, and then the key frames containing these words are found. These key frames are possible loop key frames. Step 2: Feature matching The second step of loop detection is to match ORB feature points. The system adopts the bag-of-words-based BoW matching method, which divides the ORB feature points contained in the current frame into different words and searches for key frames matching the current frame in the database. If the number of matching points exceeds a threshold, it is considered that there may be a loop between the two frames. Step 3: Geometry Verification After feature matching, the loop detection thread will perform geometric verification to ensure that the spatial relationship between matching points conforms to the loop constraint. The system adopts a RANSAC-based method to perform geometric verification by calculating the essential matrix or basic matrix, and improves the robustness of geometric verification by eliminating incorrect matching points. Step 4: Loop Detection and Optimization If the geometric verification passes, it is considered that there is a loop between the current frame and a previous frame. At this time, the system will perform loop optimization, that is, minimize the error caused by the loop by optimizing the posture relationship of the entire map and add it to the previous map. Loop optimization usually adopts a method based on nonlinear optimization.

8. The method of use according to claim 7, characterized in that: The nonlinear optimization based methods include graph optimization or optimization relaxation.

Images