A Dynamic Environment Localization and Mapping Method for Mobile Service Robots Based on Deep Clustering

Abstract

This invention claims protection for a dynamic environment localization and mapping method for mobile service robots based on deep clustering, belonging to the field of autonomous navigation for intelligent robots. It includes the following steps: S1, acquiring environmental information using an RGB-D depth camera; S2, preprocessing the input image information, assigning depth information to each feature point during feature point extraction; S3, acquiring image semantic information using an object detection network, and combining this with geometric methods to obtain dynamic regions to be removed; S4, clustering feature points within the dynamic regions based on the depth information to determine and remove the feature points to be removed; S5, using the ORB-SLAM2 algorithm to locate and navigate the remaining feature points. Validation on the public dataset TUM shows that the proposed SLAM system can achieve more accurate localization and navigation in dynamic environments.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of intelligent robot autonomous navigation, and particularly relates to a mobile service robot dynamic environment positioning and mapping method based on depth clustering. BACKGROUND

[0002] Simultaneous Localization and Mapping (SLAM) of a mobile service robot is a key technology for realizing autonomous movement of the robot.

[0003] Most current SLAM methods are carried out in a static environment. For example, ORB-SLAM proposed by MUR-ARTAL et al. is a feature point-based SLAM system, which has three threads of tracking, mapping and loop detection. In 2017, the authors extended the original monocular system to binocular and RGB-D, called ORB-SLAM2. The ORB-SLAM series algorithm is one of the SLAM systems with high accuracy and robustness, but still cannot cope with the interference of dynamic environments. However, the real environment is often dynamic, and if the robot ignores the dynamic objects in the environment during operation, it is easy to cause large errors in positioning and mapping. At the same time, moving objects in the dynamic environment will reduce the stability of the SLAM system, and even cause the system to fail to operate normally. A few visual SLAM systems specially designed, such as adding a neural network, can adapt to dynamic environments, but are often limited by the algorithm itself and the system computing performance, and have poor real-time performance, which is difficult to meet the needs in actual scenes. Therefore, it is particularly important to overcome the influence of dynamic objects in the environment in order to realize the positioning and mapping of the robot in the actual scene.

[0004] CN113516664A, a visual SLAM method based on semantic segmentation dynamic points, adopts a Mask R-CNN segmentation network and a multi-view geometric constraint algorithm to remove dynamic feature points, thereby improving the positioning accuracy and robustness of the system in a dynamic environment. However, Mask R-CNN belongs to an instance segmentation network, and its segmentation speed is very slow, and the multi-view geometric method is also very time-consuming.

[0005] CN112435262A, a dynamic environment information detection method based on a semantic segmentation network and multi-view geometry, adopts a lightweight semantic segmentation network FcHarDnet and a multi-view geometric constraint algorithm to remove dynamic feature points, thereby improving the robustness of the system. However, the semantic segmentation network used in this method still has a slow segmentation processing speed, resulting in that the system does not meet the real-time requirements.

[0006] CN113066129A, a visual positioning and mapping system based on target detection in a dynamic environment, uses the target detection frame of the target detection network to screen feature points, which is fast, but since the target detection frame only roughly divides the region, it is also easy to exclude static feature points, resulting in lower accuracy than other systems that introduce semantic segmentation and instance segmentation.

[0007] CN115187614A, a real-time simultaneous localization and mapping method based on STDC semantic segmentation network, belongs to the field of intelligent robot autonomous navigation. It includes the following steps: S1, obtaining environmental information using an RGB-D depth camera; S2, preprocessing the input image information, and extracting feature points using the ORB algorithm; S3, obtaining image semantic information using the STDC semantic segmentation network, and removing dynamic feature points using semantic information; S4, using the ORB-SLAM3 algorithm to position and navigate the remaining feature points. This method uses only semantic information to determine dynamic feature points, which can easily lead to false removal. For example, if the method classifies people as high-dynamic targets, but in fact the people in the field of view are in a stationary state, the method will still remove the feature points on their bodies, but the invention overcomes this problem using geometric information. At the same time, this method removes feature points by generating a mask using semantic information, but the invention does not need to generate an image mask based on semantic information, but directly analyzes the feature points by clustering, which is more real-time. SUMMARY

[0008] The present invention aims to solve the problems of the above prior art. A mobile service robot dynamic environment positioning and mapping method based on depth clustering is proposed. The technical solution of the present invention is as follows:

[0009] A mobile service robot dynamic environment positioning and mapping method based on depth clustering, comprising the following steps:

[0010] S1, directly capturing RGB image information and image depth information using an RGB-D depth camera;

[0011] S2, preprocessing the input image information, and assigning depth information to each feature point when extracting feature points. First, use the ORB algorithm to extract feature points from the RGB image, then project the depth information of the extracted feature points using the depth image based on the RGB-D camera model;

[0012] S3, use the YOLO target detection network to obtain image semantic information, and use the semantic information and geometric information to obtain the dynamic region where the dynamic object is located;

[0013] S4, cluster analysis of the region based on the depth information of the feature points in the dynamic region to distinguish and remove the dynamic feature points that need to be removed;

[0014] S5, the remaining feature points are positioned and navigated using an ORB-SLAM2 algorithm, wherein the ORB-SLAM2 is a feature point-based real-time SLAM algorithm, and the algorithm includes a tracking thread, a local mapping thread, and a loop closure thread.

[0015] Further, the step S2 pre-processes the input image information, and depth information is assigned to each feature point when the feature points are extracted; the specific steps are as follows:

[0016] 1. First, the FAST algorithm is used to find the corner points, i.e., key points, in the image;

[0017] 2. Corner response sorting, i.e., calculating the Harris corner response value, and then sorting the detected corner points according to the response value, so as to screen out the most significant corner points;

[0018] 3. Key point direction assignment: calculate the direction of each key point, so that the descriptor has rotational invariance;

[0019] 4. Generating a rotation-invariant BRIEF descriptor: using the direction-corrected BRIEF algorithm, a rotation-invariant descriptor is generated from the pixel values around the key point;

[0020] 5. Key point and descriptor matching: matching the ORB descriptors in two different images to find similar feature points;

[0021] 6. Assigning depth values to the feature points according to the camera model.

[0022] Further, after extracting and screening the FAST key points, the gray centroid of the image block is used to add a rotation description to the feature point, and the moment of the image block A is defined as:

[0023]

[0024]

[0025] In the formula, I(x, y) is the gray value of the image pixel point (x, y), m x , and m y respectively represent the moments in the horizontal direction and the vertical direction;

[0026] After that, the direction of the feature point at the pixel point (x, y) is defined, and the BRIEF descriptor is calculated, by randomly selecting N point pairs around the feature point P(x, y), and comparing the gray values in the point pairs:

[0027]

[0028] In the formula, A, B are the gray value of the point a, b respectively. The BRIEF descriptor is as follows:

[0029]

[0030] In the formula: n represents the nth position of the feature point descriptor.

[0031] Further, the corner response is sorted, that is, the Harris corner response value is calculated, and then the detected corner points are sorted according to the response value; for each FAST corner point obtained, the gradient matrix G in the x and y directions is obtained first, and then the Harris response value R is calculated by the following formula:

[0032] R = det(G) - k * (trace(G)) 2

[0033] Where det(G) is the determinant of the matrix G, trace(G) is the trace of the matrix G, and k is an empirical parameter. According to the calculated Harris response value, the response value is sorted in descending order. The feature points with larger Harris response value will be retained.

[0034] Further, the camera model is used to assign a depth value to the feature point, specifically including: the depth value of each RGB image pixel point (x, y) is the gray value of the point (x, y) in the depth image, so the spatial position of the feature point extracted from the image can be represented by the Cartesian coordinate system:

[0035] P(x, y, depth)

[0036] Further, the step S3 uses the YOLO target detection network to obtain image semantic information, and uses the semantic information and the geometric information to obtain the dynamic region where the dynamic object is located, specifically including:

[0037] The YOLOv5 target detection network is selected to detect the target object in the image;

[0038] The dynamic region confirmation method is confirmed by the semantic information and the geometric information. First, the static region is confirmed by the semantic information; then the possible dynamic object is found by the semantic information, and only the relative position difference between the object frame region and the static region is calculated to judge the dynamic characteristics of the object. The displacement size of the object in the detection frame is calculated by the following formula:

[0039]

[0040] In the formula: P d(u,v) are the pixel coordinates of the object within the detection box; d represents the dynamic range, u,v are the coordinates in the pixel coordinate system; M, M-1 are consecutive image frames.

[0041] The displacements of feature points in the background of static objects in the preceding and following frames are as follows:

[0042]

[0043] In the formula, N s This represents the number of feature points in the static background of the image; S represents static, and i is the i-th point. i This represents the i-th static point. Z represents a static feature point in a three-dimensional spatial coordinate system; Z is the depth information of the feature point in the depth map; and K is the camera intrinsic parameter. and This represents a four-dimensional matrix containing camera translation and rotation. The Lie algebra representing the camera pose transformation matrix.

[0044] Finally, the following formula is used to determine whether the bounding box belongs to the dynamic region, where ρ represents the decision coefficient. If the formula is satisfied, then the bounding box belongs to the dynamic region.

[0045]

[0046] Furthermore, step S4 involves performing cluster analysis on the region based on depth information within the dynamic region to remove dynamic feature points, specifically including:

[0047] The K-Means clustering method is used to cluster deep information, with the idea of ​​minimizing the distance between points within the same cluster.

[0048]

[0049] In the formula, D represents the depth distance between two points, considering only depth and ignoring x and y; X i Let C(X) represent the i-th point in the cluster. i ) contains point X i The center point of the cluster.

[0050] Furthermore, the process of removing dynamic feature points based on deep clustering is as follows: First, it is assumed that there is a difference in depth values ​​between the dynamic object and the static background in the detection box, and this difference is sufficient to distinguish the object from the background. Then, it is assumed that the dynamic object occupies a larger proportion of the area in the dynamic region, and the number of dynamic object feature points is relatively large. Therefore, after removing outliers, the classification result obtained by using the K-Means deep clustering method is the cluster with the largest number of points and the smallest average depth value.

[0051] When performing dynamic feature point removal, if the obtained dynamic feature point clustering result meets the condition of having the largest number and the smallest average depth value, then only the feature points in this class are removed. If this condition is not met, then all feature points in the dynamic region are removed.

[0052] Furthermore, step S5 uses the ORB-SLAM2 algorithm to locate and navigate the remaining feature points. The specific steps are as follows:

[0053] Tracking thread: Searches for and matches feature points in the local map, and minimizes reprojection error using the bundle adjustment (BA) algorithm to determine the position and pose of the camera in each frame;

[0054] Local mapping thread: Optimizes camera position and feature point cloud using local BA algorithm;

[0055] Loop closure detection thread: finds loops and eliminates accumulated drift errors by optimizing the pose graph; after completing the pose graph optimization, it starts the global BA algorithm thread to calculate the optimal structure and motion results of the entire system.

[0056] The advantages and beneficial effects of this invention are as follows:

[0057] This invention addresses the problems of low segmentation efficiency in SLAM systems integrating semantic segmentation and instance segmentation networks, and low accuracy in SLAM systems integrating object detection networks. It proposes a dynamic environment localization and mapping method for mobile service robots based on deep clustering. Firstly, this invention employs the YOLO object detection network to enhance semantic information, improving the accuracy and speed of semantic information acquisition. Since directly utilizing semantic information is inaccurate, and multi-view geometric methods are slow, this paper uses semantic information combined with epipolar constraints to confirm dynamic regions, achieving high accuracy and speed. Secondly, this paper utilizes deep clustering to subdivide feature points within the dynamic region, enabling the use of object detection networks to achieve similar results to semantic segmentation. Furthermore, feature point clustering is faster; the time consumption of combining object detection networks with feature point clustering is significantly lower than that of using semantic segmentation or instance segmentation networks.

[0058] Finally, through testing on the public dataset TUM, experiments show that the dynamic environment localization and mapping method for mobile service robots based on deep clustering has good localization accuracy and robustness, and its time consumption in object detection and deep clustering is low, meeting real-time requirements and achieving superior performance. In feature point classification and removal, other methods often generate masks for the image and then remove feature points based on the mask image. However, this method, because it previously assigns depth information to feature points, can directly classify them without generating masks. When performing image semantic segmentation or instance segmentation and generating masks, the system consumes a lot of resources, often taking tens or hundreds of milliseconds to process a single frame. In contrast, simple clustering to process feature points with depth information in an image takes less than one millisecond. Even including the time consumed by the object detection network, the system's real-time performance is far better than other methods using semantic segmentation or instance segmentation networks. At the same time, the system's accuracy is higher than methods using only object detection. Attached Figure Description

[0059] Figure 1 This invention provides a preferred embodiment of a method framework for dynamic environment localization and mapping of mobile service robots based on deep clustering;

[0060] Figure 2 This is a flowchart of the deep clustering method. Detailed Implementation

[0061] The technical solutions of the embodiments of the present invention will be clearly and thoroughly described below with reference to the accompanying drawings. The described embodiments are merely some embodiments of the present invention.

[0062] The technical solution of the present invention to solve the above-mentioned technical problems is:

[0063] like Figure 1 As shown, a dynamic environment localization and mapping method for mobile service robots based on deep clustering is presented. This method utilizes the YOLO object detection network and geometric information to detect dynamic regions in images, and leverages deep clustering to eliminate the impact of dynamic objects on the stability of the SLAM system in real-world environments, thereby improving the localization accuracy and robustness of SLAM. Specifically, the method includes the following steps:

[0064] S1 uses an RGB-D depth camera to directly capture RGB image information and image depth information.

[0065] S2, preprocesses the input image information, assigning depth information to each feature point during feature point extraction; specifically including:

[0066] 1. First, use the FAST algorithm to find corner points (keypoints) in the image; 2. Corner point response sorting: calculate the Harris corner response value, and then sort the detected corner points according to the response value to filter out the most significant corner points; 3. Keypoint orientation assignment: calculate the orientation of each keypoint to make the descriptor rotation invariant; 4. Generate rotation-invariant BRIEF descriptors: use the orientation-corrected BRIEF algorithm to generate rotation-invariant descriptors from the pixel values ​​around the keypoints; 5. Keypoint and descriptor matching: match the ORB descriptors in two different images to find similar feature points; 6. Assign depth values ​​to feature points according to the camera model.

[0067] After extracting and filtering FAST key points, a rotation description is added to the feature point using the gray-level centroid of the image patch. The moments of image patch A are defined as follows:

[0068]

[0069]

[0070] In the formula: I(x,y) is the gray value at pixel (x,y) in the image, m x m y These represent the moments in the horizontal direction and the moments in the vertical direction, respectively.

[0071] After this, a direction θ is defined for the feature point at pixel (x,y), and the BRIEF descriptor is calculated by randomly selecting N pairs of points around the feature point P(x,y) and comparing the gray values ​​within the pairs:

[0072]

[0073] In the formula, A and B represent the grayscale values ​​at points a and b, respectively. Therefore, the BRIEF descriptor can be obtained as follows:

[0074]

[0075] In the formula: n represents the nth descriptor of the feature point;

[0076] Then, the corner responses are sorted. For each FAST corner, its gradient matrix G in the x and y directions is obtained first, and then the Harris response value R is calculated using the following formula:

[0077] R = det(G) - k*(trace(G)) 2

[0078] Here, det(G) is the determinant of matrix G, trace(G) is the trace of matrix G, and k is an empirical parameter. Based on the calculated Harris response values, the response values ​​are sorted in descending order. Feature points with larger Harris response values ​​are retained.

[0079] The difference between this method and ordinary ORB feature extraction lies in assigning depth information to the final extracted ORB feature points. Using an RGB-D camera model, we know that in a depth image, the depth value corresponding to each RGB pixel (x, y) is the grayscale value at that point (x, y). Therefore, the spatial location of the extracted feature points can be represented using a Cartesian coordinate system.

[0080] P(x,y,depth) (20)

[0081] S3 utilizes the YOLO object detection network to acquire semantic information from the image, and then uses this semantic and geometric information to identify the dynamic regions where dynamic objects reside. The YOLOv5 object detection network is selected; YOLOv5 is a robust and efficient model that can effectively detect target objects in images. Furthermore, this algorithm was trained on the MS COCO dataset, which includes over 80 object categories, essentially covering common indoor objects. The official training results are sufficient for basic SLAM usage in typical indoor environments.

[0082] Furthermore, the dynamic region identification method uses a combination of semantic and geometric information. First, static regions are identified using semantic information; objects such as tables, monitors, and computers are considered static. Then, possible dynamic objects, such as people, cats, and dogs, are identified using semantic information. The relative positional difference between the object's bounding box and the static region in consecutive frames is calculated only for possible dynamic objects to determine their dynamic characteristics. The displacement of the object within the detection box is calculated using the following formula:

[0083]

[0084] In the formula: P d (u,v) are the pixel coordinates of the object within the detection box, d represents the dynamic range, u,v are the coordinates in the pixel coordinate system; M, M-1 are consecutive image frames.

[0085] The displacements of feature points in the background of static objects in the preceding and following frames are as follows:

[0086]

[0087] In the formula, N s This represents the number of feature points in the static background of the image; S represents static, i is the i-th point, and s iThis represents the i-th static point. Z represents a static feature point in a three-dimensional spatial coordinate system; Z is the depth information of the feature point in the depth map; and K is the camera intrinsic parameter. and This represents a four-dimensional matrix containing camera translation and rotation. The Lie algebra representing the camera pose transformation matrix.

[0088] Finally, the following formula is used to determine whether the bounding box belongs to a dynamic region, where ρ represents the decision coefficient. If the formula is satisfied, then the area within the bounding box belongs to the dynamic region.

[0089]

[0090] S4. Based on the depth information within the dynamic region, cluster analysis is performed to remove dynamic feature points. Further, the K-Means clustering method is used to cluster the depth information, the idea being to minimize the distance between points within the same cluster.

[0091]

[0092] In the formula, D represents the depth distance between two points, considering only depth and ignoring x and y; X i Let C(X) represent the i-th point in the cluster. i ) contains point X i The center point of the cluster.

[0093] Furthermore, the process of removing dynamic feature points based on deep clustering specifically involves: firstly, assuming a difference in depth values ​​between the dynamic object and the static background within the detection box, sufficient to distinguish the object from the background; then, assuming that the dynamic object occupies a larger area within the dynamic region, resulting in a greater number of dynamic object feature points. Therefore, after removing outliers and classifying using the K-Means deep clustering method, the cluster with the largest number of points and the smallest average depth value is the cluster containing the dynamic feature points.

[0094] Furthermore, when performing dynamic feature point removal, if the obtained dynamic feature point clustering result meets the condition of having the largest number and the smallest average depth value, then only the feature points within this class are removed; otherwise, all feature points within the dynamic region are removed.

[0095] S5. The remaining feature points are located and navigated using the ORB-SLAM2 algorithm. ORB-SLAM2 is a real-time SLAM algorithm based on feature points, which includes a tracking thread, a local mapping thread, and a loop closure detection thread.

[0096] Step S5 uses the ORB-SLAM2 algorithm to locate and navigate the remaining feature points. The specific steps are as follows:

[0097] Tracking thread: Searches for and matches feature points in the local map, and uses the Bundle Adjustment (BA) algorithm to minimize reprojection error, thereby determining the position and orientation of the camera in each frame.

[0098] Local mapping thread: Optimizes the camera position and feature point cloud using the local BA algorithm.

[0099] Loop closure detection thread: Identifies loops and eliminates accumulated drift errors by optimizing the pose graph. After pose graph optimization, the global BA algorithm thread is started to calculate the optimal structure and motion results for the entire system.

[0100] The systems, devices, modules, or units described in the above embodiments can be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, a computer can be, for example, a personal computer, laptop computer, cellular phone, camera phone, smartphone, personal digital assistant, media player, navigation device, email device, game console, tablet computer, wearable device, or any combination of these devices.

[0101] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0102] The above embodiments should be understood as illustrative only and not as limiting the scope of protection of the present invention. After reading the description of the present invention, those skilled in the art can make various alterations or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A method for dynamic environment localization and mapping of mobile service robots based on deep clustering, characterized in that, The method comprises the following steps: S1, obtaining RGB image information and image depth information by directly shooting with an RGB-D depth camera; S2, pre-processing the input image information, and assigning depth information to each feature point when extracting the feature points; first, extracting feature points from the RGB image using the ORB algorithm, and then projecting the extracted feature points with depth information according to the RGB-D camera model; the depth value of each RGB image pixel point (x, y) is the gray value of the point (x, y) in the depth image, so that the spatial position of the extracted feature points in the image can be represented by a Cartesian coordinate system: ； S3, obtaining image semantic information by using a YOLO target detection network, and obtaining a dynamic region where a dynamic object is located by using the semantic information and geometric information; S4, performing clustering analysis on the region according to the depth information of the feature points in the region to distinguish and remove dynamic feature points; S5, positioning and navigating the remaining feature points by using an ORB-SLAM2 algorithm, wherein the ORB-SLAM2 is a real-time SLAM algorithm based on feature points, and the algorithm comprises a tracking thread, a local mapping thread and a loop detection thread; The S3 specifically comprises: selecting a YOLOv5 target detection network for detecting target objects in the image; The dynamic region confirmation method is jointly confirmed by semantic information and geometric information; first, the static region is confirmed by the semantic information; then, possible dynamic objects are found by the semantic information, and the dynamic characteristics of the objects are judged by only calculating the relative position difference between the object frame region and the static region in front and back frames; the displacement of the object in the detection frame is calculated by the following formula: wherein: is the pixel coordinate of the object within the detection frame, is the coordinate of the pixel coordinate system; , is a consecutive image frame; The feature point displacement of the static object background in the front and back frames is as follows: wherein, denotes the number of feature points in the static background of the image; S represents static, and i is the i-th point, denotes the i-th static point, denotes the static feature point in the three-dimensional space coordinate system; is the depth information corresponding to the feature point in the depth map; is the camera intrinsic parameter; and denotes a four-dimensional matrix containing the camera translation and rotation amount, denotes the Lie algebra of the camera pose transformation matrix; Finally, whether the object frame belongs to the dynamic region is determined by the following formula, wherein represents a decision coefficient, and the formula is satisfied. In this case, the detection frame belongs to the dynamic region. 。 2. The method of claim 1, wherein, The step S2 specifically comprises the following steps: First, find the corner points, i.e., key points, in the image by using the FAST algorithm; Corner point response sorting, i.e., calculating the Harris corner point response value, and then sorting the detected corner points according to the response value to screen out the most prominent corner points; Key point direction assignment: calculating the direction of each key point to make the descriptor rotationally invariant; Generating a rotationally invariant BRIEF descriptor: using the direction-corrected BRIEF algorithm to generate a rotationally invariant descriptor from the pixel values around the key point; Key point and descriptor matching: matching the ORB descriptors in two different images to find similar feature points; Assigning a depth value to the feature point according to the camera model.

3. The method of claim 2, wherein, After finding the key points by using the FAST algorithm, add a rotation description to the feature point by using the gray centroid of the image block, and define the moment of the image block A as follows: In the formula: is the gray value of the image pixel point at the position , respectively represents the horizontal moment and the vertical moment. To define the direction of the feature point at the pixel point and calculate the BRIEF descriptor, N point pairs are randomly selected around the feature point , and the gray values in the point pairs are compared. wherein, , are the gray value sizes at the points , , respectively; from which the BRIEF descriptor is obtained as follows: In the formula, n represents the nth descriptor of the feature point.

4. The method of claim 2, wherein, In the step S2, the corner point response sorting, i.e., calculating the Harris corner point response value, and then sorting the detected corner points according to the response value; For each FAST corner point obtained, first get its gradient matrix in x and y directions Then calculate the Harris response value R by the following formula: wherein, is the determinant of the matrix , is the trace of the matrix , and k is an empirical parameter; according to the calculated Harris response value, the response value is sorted in descending order; the feature points with a Harris response value greater than a preset threshold value are retained.

5. The method of claim 1, wherein, The S4 specifically comprises: Clustering the depth information by using a K-Means depth clustering method to minimize the distance between points in the same class: wherein, represents the depth distance between two points, only considering depth, not x and y; denotes the i-th point in the cluster, is the center point of the cluster containing the point .

6. The method of claim 5, wherein the method further comprises: According to the depth clustering, the dynamic feature points are specifically removed as follows: after removing outliers, the category with the most number and the smallest average depth value in the classification result obtained by means of the K-Means depth clustering method is the category of the dynamic feature points. When the dynamic feature points are removed, if the obtained dynamic feature point clustering result exactly meets the condition of the most number and the smallest average depth value, only the feature points in the category are removed, and if the condition is not met, all the feature points in the dynamic region are removed.

7. The method of claim 6, wherein the method further comprises: The step S5 realizes positioning and navigation of the remaining feature points through the ORB-SLAM2 algorithm, and the specific steps are as follows: The tracking thread: searching for feature points in the local map and matching, minimizing the re-projection error through the bundle adjustment (BA) algorithm, so as to determine the position and pose of each frame camera; The local mapping thread: optimizing the position of the camera and the feature point cloud through the local BA algorithm; The loop detection thread: finding loops and eliminating accumulated drift errors through optimizing the pose graph; after completing the pose graph optimization, the global BA algorithm thread is started to calculate the optimal structure and motion result of the whole system.

Images