SLAM algorithm based on dynamic region elimination strategy and improved weighted geometric constraint

Abstract

The invention discloses an SLAM (Simultaneous Localization and Mapping) algorithm based on a dynamic region elimination strategy and improved weighted geometric constraints, and belongs to the technical field of mobile robot vision SLAM. On one hand, dynamic and static object areas on an image are detected through an improved YOLOv5s target detection network, a dynamic object bounding box and a static object bounding box are drawn, feature point recognition can be further carried out based on the generated dynamic bounding box, and then feature points recognized as dynamic feature points can be removed; on the other hand, a weighted geometric constraint method is provided to further screen potential dynamic feature points for a result detected in the first aspect; according to the method, the potential dynamic feature points detected by the target detection result and the improved weighted geometric constraint method are eliminated, so that the influence of the dynamic object on the visual SLAM system can be reduced, and the positioning precision and robustness of the system are improved.

Description

Technical Field

[0001] The invention relates to a SLAM algorithm based on a dynamic region elimination strategy and improved weighted geometric constraints, and belongs to the technical field of mobile robot vision SLAM. Background Art

[0002] With the rapid development of science and technology, the application fields of mobile robots are increasing. Many unfamiliar scenes require mobile robots to explore and perceive the surrounding environment and build maps in real time. The simultaneous localization and mapping (SLAM) technology is to receive sensor data, analyze and determine the position and posture, which is in line with the technology required by mobile robots. At present, the research and application of SLAM technology are concentrated in two major branches, namely laser SLAM and visual SLAM. Visual SLAM is more favored by scholars because of its simple equipment and low cost. However, traditional visual SLAM algorithms generally work well in static environments. When moving in dynamic environments, especially in some complex and narrow environments, the feature points of dynamic objects generate unrelated data pairs, which leads to poor camera pose calculation, seriously affecting the accuracy of the visual SLAM system, and even causing pose drift, positioning loss and other phenomena. Summary of the invention

[0003] The present invention provides a SLAM algorithm based on a dynamic region elimination strategy and improved weighted geometric constraints. The dynamic feature points and potential dynamic feature points in the environment are eliminated through an improved YOLOv5s target detection network and the weighted geometric constraint method proposed by the present invention, thereby reducing the impact on visual SLAM and improving the positioning accuracy and robustness of visual SLAM.

[0004] The technical solution of the present invention is:

[0005] A SLAM algorithm based on a dynamic area elimination strategy and improved weighted geometric constraints comprises the following steps: using a calibrated depth camera to collect RGB images; performing ORB feature point extraction on the collected RGB images to obtain feature points; detecting dynamic and static object areas from the RGB images through an improved YOLOv5s target detection network and drawing dynamic object bounding boxes and static object bounding boxes; performing feature point recognition according to the generated dynamic bounding boxes, eliminating feature points identified as dynamic feature points, and using the retained feature points as static feature points to be screened; using a weighted geometric constraint method to detect potential dynamic feature points for the static feature points to be screened and eliminating them, and using the retained feature points as static feature points; and constructing a visualized three-dimensional point cloud map based on the retained static feature points.

[0006] Furthermore, the method of using the calibrated depth camera to collect RGB images includes: selecting a circular checkerboard calibration plate; connecting the depth camera to an Ubuntu operating system in a computer; running a camera node to ensure that the calibration plate is always within the field of view; and using the Kalibr function package to complete the camera calibration work to obtain a calibrated depth camera.

[0007] Furthermore, the improved YOLOv5s target detection network includes three parts: Backone, Neck, and Head. The improved YOLOv5s target detection network takes the traditional YOLOv5s target detection network as the framework, and replaces the C3 module in the Backone network in the traditional YOLOv5s target detection network with the C3-SWC module to construct an improved YOLOv5s target detection network; the Backone network in the improved YOLOv5s target detection network generates five main output features, the first main output feature comes from the SPFF module after the pooling operation, the second main output feature comes from the fourth C3-SWC module, the third main output feature comes from the third C3-SWC module, the fourth main output feature comes from the second C3-SWC module, and the fifth main output feature comes from the first C3-SWC module.

[0008] Furthermore, the feature points are identified based on the generated dynamic bounding box, the feature points identified as dynamic feature points are eliminated, and the retained feature points are used as static feature points to be screened. Specifically, the feature points on the dynamic object corresponding to the generated dynamic object bounding box are used as dynamic feature points, the feature points identified as dynamic feature points are eliminated; the retained feature points are used as static feature points to be screened.

[0009] Furthermore, the weighted geometric constraint method is used to detect and remove potential dynamic feature points for the static feature points to be screened, specifically:

[0010] If the offset distance D of the static feature point to be screened i If the value is greater than the first threshold σ1, it is a potential dynamic feature point, and the potential dynamic feature point is eliminated;

[0011] If the offset distance D of the static feature point to be screened i If the value is less than or equal to the first threshold σ1, the second threshold σ2 is compared with δ. If δ>σ2, it is a potential dynamic feature point and the potential dynamic feature point is eliminated. Otherwise, if δ≤σ2, it is judged as a static feature point and retained.

[0012] Furthermore, the method of constructing a visualized three-dimensional point cloud map based on the retained static feature points is specifically as follows: feature matching is performed on the retained static feature points to obtain matching feature points and pose estimation is performed to obtain camera pose results and key frames; and based on the key frames, a three-dimensional point cloud map is constructed.

[0013] The beneficial effects of the present invention are:

[0014] On the one hand, the present invention detects dynamic and static object areas on the image through an improved YOLOv5s target detection network and draws dynamic object bounding boxes and static object bounding boxes, and can further identify feature points based on the generated dynamic bounding boxes, and then eliminate feature points identified as dynamic feature points; on the other hand, the present invention proposes a weighted geometric constraint method to further realize the screening of potential dynamic feature points after the detection result of the first aspect; based on the above, it can be seen that the present invention eliminates potential dynamic feature points detected by the target detection results and the improved weighted geometric constraint method, thereby reducing the impact of dynamic objects on the visual SLAM system and improving the positioning accuracy and robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a flow chart of the algorithm of the present invention;

[0016] Figure 2 It is the improved YOLOv5s target detection network structure diagram;

[0017] Figure 3 This is the detection result diagram of the improved YOLOv5s target detection network;

[0018] Figure 4 It is the construction of 3D point cloud map in real environment;

[0019] Figure 5 It is the trajectory comparison between the algorithm of the present invention and the ORB-SLAM3 algorithm in the high dynamic sequence data set;

[0020] Figure 6 It is a comparison of rectangular route trajectories in real environment. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical scheme and advantages of the embodiments of the present invention clearer, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. It should be noted that the embodiments in this application and the features in the embodiments can be combined with each other arbitrarily without conflict.

[0022] Example 1: Figure 1-6As shown, a SLAM algorithm based on a dynamic area elimination strategy and improved weighted geometric constraints includes the following steps: using a calibrated depth camera to collect RGB images; performing ORB feature point extraction on the collected RGB images to obtain feature points; detecting dynamic and static object areas from the RGB images through an improved YOLOv5s target detection network and drawing dynamic object bounding boxes and static object bounding boxes; performing feature point recognition based on the generated dynamic bounding boxes, eliminating feature points identified as dynamic feature points, and retaining feature points as static feature points to be screened; using a weighted geometric constraint method to detect potential dynamic feature points for the static feature points to be screened and eliminating them, and retaining feature points as static feature points; and constructing a visual three-dimensional point cloud map based on the retained static feature points.

[0023] Furthermore, the method of using the calibrated depth camera to collect RGB images includes: selecting a circular checkerboard calibration plate; connecting the depth camera to the Ubuntu 18.04 operating system in the computer; running the camera node to ensure that the calibration plate is always in the field of view; using the Kalibr function package to complete the camera calibration work, so as to obtain the calibrated depth camera to collect RGB images and DEPTH images.

[0024] Furthermore, the collected RGB image is subjected to ORB feature point extraction, including: for an image of an indoor environment captured by a camera in real time, feature points are extracted using an ORB algorithm (the traditional SURF and SIFT feature point extraction algorithms have a slow extraction speed, while the ORB algorithm is 100 times that of SIFT and 10 times that of SURF; therefore, the present invention adopts the ORB algorithm); ORB is improved by using the FAST feature detection algorithm and the BRIEF feature description algorithm, and determines whether point p is a corner point by detecting the grayscale values ​​of the pixels around point p. The brightness of point p has been set in advance, and only the brightness of point p needs to be compared during the judgment: first, a large number of coarse features are extracted, a point p is selected from the image, and a circle with a radius of 3 pixels is drawn with point p as the center. Assuming that the brightness of point p is Ip, if the grayscale values ​​of 12 consecutive pixels on the circumference exceed the grayscale value of point p [Ip-T, Ip+T] (T=20%Ip), point p is considered to be a feature point.

[0025] Furthermore, the improved YOLOv5s target detection network SWC-YOLOv5s (with Figure 2As shown in the figure, it can be used for target detection and object category judgment, including Backone (backbone network), Neck (neck network), and Head (detection head). The improved YOLOv5s target detection network takes the traditional YOLOv5s target detection network as the framework, and replaces the C3 module in the Backone network in the traditional YOLOv5s target detection network with the C3-SWC module to construct an improved YOLOv5s target detection network; the Backone network in the improved YOLOv5s target detection network generates five main output features, the first main output feature comes from the SPFF module after pooling operation, the second main output feature comes from the fourth C3-SWC module, and the third main output feature comes from From the third C3-SWC module, the fourth main output feature comes from the second C3-SWC module, and the fifth main output feature comes from the first C3-SWC module; the Neck part includes two upsampling modules, the first upsampling module receives the first main output feature map, and after upsampling processing, it is spliced ​​with the third main output feature map and output; the second upsampling module receives the output feature map of the first upsampling module, and after upsampling and C3F processing, it is spliced ​​with the fourth main output feature map and output; the output of the second upsampling module is processed by multiple C3F and CBS to output the final feature map, and in the Detect part, different detection boxes are generated according to the selected anchor, and the category of the object is output according to the confidence of the detection box for different categories.

[0026] It should be noted that dynamic and static objects are pre-set; Figure 3 As shown in the example, the preset dynamic object is a person, and the static objects are a chair, a keyboard, a mouse, and a monitor.

[0027] Furthermore, the feature points are identified according to the generated dynamic bounding box, and the feature points identified as dynamic feature points are removed, and the feature points retained are used as static feature points to be screened. Specifically, the feature points on the dynamic object corresponding to the generated dynamic object bounding box are used as dynamic feature points, and the feature points identified as dynamic feature points are removed; the feature points retained are used as static feature points to be screened. Figure 3 The left half is the identified feature points. Figure 3 The right half is the result after removing dynamic feature points.

[0028] Specifically: After the depth camera shoots the environment, it will obtain an RGB image, which is converted into a grayscale image and enters the visual SLAM system for ORB feature point extraction. After the image is sent to the improved YOLOv5s target detection network, a bounding box is drawn for each object in the image according to the trained weight file, and the confidence of the object is indicated. When the confidence of the object in the bounding box is greater than the preset confidence, the object category in the bounding box is the category recognized by the improved YOLOv5s target detection network. If the confidence of the object in the bounding box is less than the preset confidence, the object category in the bounding box is unclear, and the bounding box is not marked; when the object category is a pedestrian, the dynamic feature points on the dynamic object corresponding to the bounding box are removed to prevent the existence of dynamic objects from affecting the stability of the visual SLAM system.

[0029] Select any common feature point on two adjacent image frames X1 and X2, and mark them as P1 and P2 on X1 and X2 respectively. The formula is:

[0030] P1=K·[u1,v1,1] T (1)

[0031] P2=K·[u2,v2,1] T (2)

[0032] Among them, u1, v1, u2, v2 are the pixel coordinate values ​​of P1 and P2 respectively, and K is the intrinsic parameter matrix of the camera.

[0033] Determine the epipolar lines L1 and L2 according to the intersection lines between X1 and X2 and the epipolar plane, and assume that the epipolar line formula is expressed as:

[0034] ax+by+c=0 (3)

[0035] At this time, the offset distance from the feature point to be screened to the epipolar line is:

[0036]

[0037] If the feature point P to be screened i When it is a static feature point, its offset distance is 0 as can be obtained by formula (4). In the real environment, there will always be static feature points whose distance to the polar line should be 0, but due to different interference factors, the distance is between 0 and an adaptation threshold T. In addition, there will still be static feature points on the polar line but they are actually potential dynamic feature points. In view of this, the weighted geometric constraint method is used to detect potential dynamic feature points and remove them for the static feature points to be screened.

[0038] Furthermore, the weighted geometric constraint method is used to detect and remove potential dynamic feature points for the static feature points to be screened, specifically:

[0039] If the offset distance D of the static feature point to be screened i If the value is greater than the first threshold σ1, it is a potential dynamic feature point, and the potential dynamic feature point is eliminated;

[0040] If the offset distance D of the static feature point to be screened i If it is less than or equal to the first threshold σ1, let λ=1 and θ=1 be substituted into the δ expression, and then compare the second threshold σ2 with δ. If δ>σ2, it is a potential dynamic feature point and the potential dynamic feature point is eliminated; otherwise, if δ≤σ2, it is judged as a static feature point and retained.

[0041] The δ is the offset distance D i Vector parallel to the epipolar plane With perpendicular vector The cosine value between To establish, the formula is as follows:

[0042]

[0043] Where λ is the depth influence factor, θ is the offset distance influence factor, and D i Indicates the offset distance of the i-th static feature point to be screened, Representatives and The perpendicular vectors, C2 is the camera optical center, P i represents the projection of the i-th static feature point to be screened on X2, Represents the vector parallel to the epipolar plane.

[0044] By defining the cosine value as the standard for judging the distance between two vectors, a cosine value of 90° represents the closest depth distance between the projected feature point and the polar line, and the offset distance D i The smaller it is, the closer the offset distance between the projected feature point and the epipolar line is. The camera is affected by the surrounding environment during movement and cannot guarantee that the static feature point is located on the projection epipolar line.

[0045] Furthermore, the method of constructing a visual three-dimensional point cloud map based on the retained static feature points is specifically as follows: feature matching is performed on the retained static feature points to obtain matching feature points and pose estimation is performed to obtain camera pose results and key frames; based on the key frames, a three-dimensional point cloud map is constructed; thereby eliminating the influence of dynamic objects in the environment on the visual SLAM system and constructing a visual three-dimensional point cloud map.

[0046] Furthermore, a three-dimensional point cloud map is constructed based on the key frames, including: selecting key frames; generating point clouds of the selected key frames; splicing and filtering the generated point clouds; and generating a three-dimensional point cloud map that removes dynamic objects.

[0047] Furthermore, the selection of key frames includes: selecting the first sub-image frame in the image data as a reference frame, extracting local feature points of the reference frame, and comparing the local feature points with the subsequent three frames of images as basic feature points. If the similarity between the feature points in the subsequent image and the feature points in the reference frame exceeds 85%, the subsequent frame is judged as a redundant key frame and the redundant key frame is removed. If the similarity between the feature points in the subsequent image and the feature points in the reference frame does not exceed 85%, the frame is marked as a reference frame, and the judgment operation is continued until the reference frame as the key frame is selected.

[0048] Furthermore, the generated point cloud is stitched and filtered, including: merging point cloud data generated by different key frames together using a PCL library, optimizing the point cloud using a point cloud stitching optimization formula, and completing point cloud stitching; and processing the stitched point cloud through voxel filtering to form a complete point cloud model map.

[0049] Furthermore, the point cloud stitching optimization formula is:

[0050]

[0051] Among them, K j and q j are the corresponding points of the position point before point cloud splicing and the target position, R is the rotation matrix, t is the translation vector, and E is the error function. Let each point cloud calculate the demassing coordinate K according to its centroid coordinate j ′ and q′ j , substituting into formula (6) we get:

[0052]

[0053] Solve the optimal rotation matrix R and get the optimized point cloud.

[0054] Furthermore, the stitched point cloud is processed by voxel filtering, including:

[0055] (1) Divide voxel grid: First, divide all the spliced ​​point clouds into a voxel grid, and each voxel grid has the same size.

[0056] (2) Point cloud data allocation: divide the point cloud into matching voxel grids.

[0057] (3) Downsampling: A point is selected in each voxel as a representative point, which can represent all other points. This point is the average point or center point of all other points.

[0058] Further, generating a three-dimensional point cloud map with dynamic objects removed includes: fusing and projecting the filtered point cloud and the previous object category information in a three-dimensional space to complete the establishment of a three-dimensional map (such as Figure 4 as an example).

[0059] The high dynamic sequence Walking_xyz in the TUM data set is selected to evaluate the solution of this embodiment.

[0060] The TUM dataset is one of the most common datasets for evaluating visual SLAM systems. It contains 80 object categories, such as pedestrians, vehicles, monitors, mice, and keyboards, meeting all common daily office, study, and life items. The Walking_xyz dataset is a typical movement of two staff members moving back and forth around a table. The SLAM algorithm of the present invention can identify dynamic and static objects and draw bounding boxes in the dynamic and static object areas. The feature points of the dynamic objects corresponding to the bounding box area are all eliminated, and potential dynamic feature points are identified and eliminated through the weighted geometric constraint method.

[0061] The hardware device in the present invention is Lenovo laptop R9000P2021H, processor is AMD R7-5800H, graphics card is NVIDIA GeForce RTX3060, system environment is Ubuntu18.04 operating system, RGB-D camera model is Orbbec pro; the first threshold σ1 and the second threshold σ2 are set to 0.5 and 1 respectively.

[0062] Figure 5 This is a comparison experiment of the absolute trajectory error between the algorithm of the present invention and the ORB-SLAM3 algorithm on the Walking_xyz sequence ( Figure 5 The upper part, the left side of the upper part is ORB-SLAM3, and the right side of the upper part is the algorithm of the present invention) and the relative pose error experimental diagram ( Figure 5 In the lower middle part, the left side of the lower half is ORB-SLAM3, and the right side of the lower half is the algorithm of the present invention). Figure 5 The area of ​​the difference region corresponding to the upper and middle part represents the size of the error. The larger the area, the greater the error. From the figure, it can be seen that the error of the algorithm of the present invention is significantly smaller than that of ORB-SLAM3. Figure 5 In the lower part, the denser the APE lines are, the smaller the error is. Figure 5 It can be seen from the lower part that the lines of the present invention are denser, which means that the algorithm of the present invention has higher accuracy than the ORB-SLAM3 algorithm.

[0063] Table 1 shows the RMSE, Median and SD comparison between the Ours algorithm of the present invention and the ORB-SLAM3 algorithm on four common sequences Walking_xyz (W_xyz), walking_rpy (W_rpy), Walking_half (W_half), and Walking_static (W_staticc) in the TUM dataset.

[0064] RMSE is the root mean square error. The smaller it is, the higher the accuracy of the model and the better the error control.

[0065] Median is the median, which reflects that the median of the error is not affected by the maximum and minimum error values.

[0066] SD is the standard deviation. The smaller it is, the higher the accuracy of the model is and the more concentrated and dense the values ​​are.

[0067] Table 1. Comparison of absolute trajectory errors

[0068]

[0069] It can be seen from the data in Table 1 that compared with the ORB-SLAM3 algorithm, the RMSE and Median improvements of the algorithm of the present invention are both above 90%, and the comparison of SD also shows certain advantages, which shows that the improved YOLOv5s target detection network and the improved weighted geometric constraint method for eliminating front-end dynamic feature points proposed in the present invention can effectively improve the performance of visual SLAM in dynamic environments.

[0070] Figure 6 This is a comparison of the trajectories of the algorithm of the present invention and the ORB-SLAM3 algorithm in a real environment. Figure 6 The left side shows the rectangular route in the actual environment. Figure 6 On the right are the walking trajectories of the present invention and ORB-SLAM3. It can be seen that the present invention (dashed line) forms a complete closed-loop rectangle well, while ORB-SLAM3 (solid line) does not form a closed loop, and is not even a rectangle. It can be seen that the present invention can achieve good positioning accuracy in both public data sets and actual environment verification.

[0071] The specific implementation modes of the present invention are described in detail above in conjunction with the accompanying drawings, but the present invention is not limited to the above implementation modes, and various changes can be made within the knowledge scope of ordinary technicians in this field without departing from the purpose of the present invention.

Claims

1. A SLAM algorithm based on dynamic region elimination strategy and improved weighted geometric constraints, characterized in that: The following steps are involved: Use the calibrated depth camera to collect RGB images; Perform ORB feature point extraction on the collected RGB image to obtain feature points; use the improved YOLOv5s target detection network to detect dynamic and static object areas in the RGB image and draw dynamic object bounding boxes and static object bounding boxes; Feature points are identified according to the generated dynamic bounding box, and the feature points identified as dynamic feature points are eliminated, and the retained feature points are used as static feature points to be screened; the weighted geometric constraint method is used to detect potential dynamic feature points for the static feature points to be screened and eliminate them, and the retained feature points are used as static feature points; based on the retained static feature points, a visual three-dimensional point cloud map is constructed.

2. The SLAM algorithm based on dynamic area elimination strategy and improved weighted geometric constraints according to claim 1, characterized in that: The method of collecting RGB images using a calibrated depth camera includes: selecting a circular checkerboard calibration plate; connecting the depth camera to an Ubuntu operating system in a computer; running a camera node to ensure that the calibration plate is always in the field of view; and using a Kalibr function package to complete the camera calibration work to obtain a calibrated depth camera.

3. The SLAM algorithm based on dynamic area elimination strategy and improved weighted geometric constraints according to claim 1, characterized in that: The improved YOLOv5s target detection network includes three parts: Backone, Neck, and Head. The improved YOLOv5s target detection network takes the traditional YOLOv5s target detection network as the framework, and replaces the C3 module in the Backone network in the traditional YOLOv5s target detection network with the C3-SWC module to construct an improved YOLOv5s target detection network; the Backone network in the improved YOLOv5s target detection network generates five main output features, the first main output feature comes from the SPFF module after the pooling operation, the second main output feature comes from the fourth C3-SWC module, the third main output feature comes from the third C3-SWC module, the fourth main output feature comes from the second C3-SWC module, and the fifth main output feature comes from the first C3-SWC module.

4. The SLAM algorithm based on dynamic area elimination strategy and improved weighted geometric constraints according to claim 1, characterized in that: The feature points are identified according to the generated dynamic bounding box, the feature points identified as dynamic feature points are eliminated, and the retained feature points are used as static feature points to be screened. Specifically, the feature points on the dynamic object corresponding to the generated dynamic object bounding box are used as dynamic feature points, the feature points identified as dynamic feature points are eliminated; and the retained feature points are used as static feature points to be screened.

5. The SLAM algorithm based on dynamic area elimination strategy and improved weighted geometric constraints according to claim 1, characterized in that: The weighted geometric constraint method is used to detect and remove potential dynamic feature points for the static feature points to be screened, specifically: If the offset distance D of the static feature point to be screened i If the value is greater than the first threshold σ1, it is a potential dynamic feature point, and the potential dynamic feature point is eliminated; If the offset distance D of the static feature point to be screened i If the value is less than or equal to the first threshold σ1, the second threshold σ2 is compared with δ. If δ>σ2, it is a potential dynamic feature point and the potential dynamic feature point is eliminated. Otherwise, if δ≤σ2, it is judged as a static feature point and retained.

6. The SLAM algorithm based on dynamic area elimination strategy and improved weighted geometric constraints according to claim 1, characterized in that: The method of constructing a visualized three-dimensional point cloud map based on the retained static feature points is specifically as follows: feature matching is performed on the retained static feature points to obtain matching feature points and pose estimation is performed to obtain camera pose results and key frames; and the three-dimensional point cloud map is constructed based on the key frames.

Images