Lightweight visual SLAM system and VSLAM method in dynamic scenes

Abstract

The present invention provides a visual SLAM system and a VSLAM method under a complex dynamic scene. The system includes a visual sensor, a tracking thread, a target detection module, a logic discrimination module, a local mapping thread, a loopback thread and all BA threads. The target detection module is an improved target detection network yolov5s, wherein the improved target detection network yolov5s includes an input end, a Backbone network, a Neck network and a Head network. The Backbone network adopts a lightweight network Ghostnet, which incorporates a CA attention mechanism to better capture dynamic factors in the environment. The target detection module is efficiently embedded in the tracking thread in the form of a thread, and the target detection module is used to detect object categories. The present invention quickly eliminates dynamic feature points at the front end of the visual SLAM system through the improved target detection network yolov5s and the logic discrimination module, thereby improving the accuracy and efficiency of the entire visual SLAM system and balancing the competing demands between positioning accuracy and computational complexity.

Description

Technical Field

[0001] The present invention relates to the technical field of visual SLAM systems, and in particular to a lightweight visual SLAM system in dynamic scenes and a lightweight VSLAM method in dynamic scenes. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) refers to the process of a robot sensing its environment and estimating its own state in an unknown environment using its sensors. Common sensors onboard robots include cameras, laser sensors, and inertial sensors. Cameras are widely used because images captured by cameras contain richer color and texture information, and they are inexpensive, compact, and consume low power. Systems using cameras as the primary sensor are called visual SLAM systems. In recent years, visual SLAM has been widely used in many application areas, including virtual reality (VR), augmented reality (AR), unmanned aerial vehicle (UAV) and unmanned ground vehicle (UGV) navigation, and autonomous mobile robots.

[0003] High accuracy and low computational cost are the two core requirements of visual SLAM. In recent years, many excellent solutions have been proposed for visual SLAM, such as DTAM (Dense tracking and mapping in real-time), LSD-SLAM (Large-scale direct monocular SLAM), SVO (Semidirect visual odometry), and ORB-SLAM2. Among them, ORB-SLAM2 is considered to be a relatively advanced and complete SLAM method. It is developed based on many outstanding works, such as the first real-time visual SLAM system PTAM (Parallel tracking and mapping), a fast location recognition method Bow2 (Bag of Words), and an efficient graph-based bundle adjustment (BA) algorithm based on co-visibility graphs. As a result, ORB-SLAM2 achieves better accuracy and robustness than other existing solutions. ORB-SLAM2 uses three threads: tracking, local mapping, and loop closure. The tracking part is considered the foundation of any SLAM system because it not only has a direct impact on accuracy and robustness, but also provides context information for the other two threads, which takes up most of the computing resources. The above SLAM solutions are all based on the assumption of static scenes, but the existence of dynamic objects in real scenes is inevitable. If dynamic objects have strong texture information, the system will extract a large number of features from the dynamic objects. When tracking unstable feature points, it will seriously affect the pose estimation, resulting in large trajectory errors or even tracking loss, making it difficult to guarantee the accuracy of the entire SLAM system.

[0004] To improve the accuracy of visual SLAM systems in dynamic scenes, some algorithms use the motion patterns or geometric models of cameras to eliminate the influence of dynamic objects. However, most algorithms have harsh constraints or low model accuracy, which limits the accuracy improvement. In recent years, with the development of deep learning, many scholars have used semantic segmentation networks to identify and remove dynamic objects in images. Although semantic segmentation networks can effectively segment dynamic objects, the large model architecture of most semantic segmentation networks greatly increases the system's computational cost and cannot meet real-time requirements. Therefore, it is particularly important to ensure the real-time performance of visual SLAM systems while accurately removing dynamic factors in the environment. Summary of the Invention

[0005] In order to solve the above technical problems, the present invention provides a lightweight visual SLAM system and a VSLAM method in dynamic scenes.

[0006] The technical solution adopted in the present invention is as follows:

[0007] A lightweight visual SLAM system in a dynamic scene includes a visual sensor, a tracking thread, a target detection module, a logic discrimination module, a local mapping thread, a loop thread and all BA threads, wherein the visual sensor adopts an RGBD camera, and the RGBD camera is used to collect scene images; the target detection module is an improved target detection network yolov5s, wherein the improved target detection network yolov5s includes an input end, a Backbone network, a Neck network and a Head network, the Backbone network adopts a lightweight network Ghostnet, and the lightweight network Ghostnet incorporates a CA attention mechanism, the target detection module is embedded in the tracking thread in the form of a thread, and the target detection module is used to detect the object category; the logic discrimination module is used to distinguish whether the object category detected by the target detection module is a dynamic target or a static target, and to distinguish whether it is a dynamic target or a static target. No, it is not eliminated; the tracking thread includes an ORB feature point extraction module, and the tracking thread extracts ORB feature points from the scene image captured by the RGBD camera through the ORB feature point extraction module. After eliminating the ORB feature points corresponding to the dynamic target, the remaining static target ORB feature points are feature matched to restore the RGBD camera pose, and local pose optimization is performed after obtaining the initial pose, and then the key frame is reviewed and generated; when the queue of key frames to be processed is not empty, the local mapping thread starts working, and the local mapping thread processes the generated key frames, deletes the map points that do not meet the conditions, and creates new map points with matching relationships to supplement them, and then performs local BA optimization to optimize all map points and delete redundant key frames; the loop thread is used to detect large loops and correct the accumulated errors by performing pose graph optimization. The loop thread starts the global BA thread after pose graph optimization; the global BA thread executes global BA to update the map.

[0008] A lightweight VSLAM method for dynamic scenes, which is applied to a lightweight visual SLAM system for dynamic scenes, comprises the following steps:

[0009] S1: Get the RGB-D scene image frame captured by the RGBD camera;

[0010] S2: Extraction of ORB feature points from RGB-D scene image frames and detection and removal of dynamic target feature points;

[0011] The specific steps include:

[0012] S21: The tracking thread extracts ORB feature points of the current RGB-D scene image frame through the ORB feature extraction module;

[0013] S22: Detecting the object category on the RGB-D scene image frame captured by the RGBD camera through the improved target detection network yolov5s, and determining whether the object is a dynamic target or a static target through the logic discrimination module. If the object is a dynamic target, all ORB feature points corresponding to the dynamic target are eliminated, and the ORB feature points corresponding to the static target are retained;

[0014] S3: Perform feature matching based on the ORB feature points of the static target to restore the RGBD camera pose. After obtaining the initial pose, perform local pose optimization, and then review and generate key frames.

[0015] S4: Generate key frames;

[0016] S5: The local mapping thread processes the generated keyframes, deletes map points that do not meet the conditions, and creates new map points with matching relationships to supplement them. Then, it performs local BA optimization to optimize all map points and delete redundant keyframes.

[0017] S6: The loop closure thread detects large loops and corrects the accumulated error by performing pose graph optimization;

[0018] S7: After the pose graph is optimized, the loopback thread starts the global BA thread to perform global BA and update the map.

[0019] The specific method for detecting and eliminating dynamic target ORB feature points in S22 is:

[0020] S221: The improved target detection network yolov5s generates a prediction box after reasoning on the RGB-D scene image frame. The prediction box consists of six parameters: position parameters x, y, w, h, confidence and classification results. Among them, x and y are the relative values ​​of the center of the prediction box and the RGB-D scene image, w and h represent the relative values ​​of the length and width of the prediction box and the RGB-D scene image, confidence represents the credibility of the prediction box containing an object and the accuracy of the prediction box position, and the classification result is determined according to the object category contained in the data set used during training;

[0021] S222: Convert the parameters output by the improved target detection network yolov5s into a formula for the coordinates of the prediction box in the RGB-D scene image. The formula is:

[0022]

[0023] Among them, l is the width of the RGB-D scene image; d is the height of the RGB-D scene image, the ORB feature points contained in the prediction box are all undetermined dynamic feature points based on prior knowledge, and the features outside the prediction box are all static feature points. If the set of all ORB feature points is P = {P1, P2, ..., P n}, the set of undetermined dynamic feature points R = {R1, R2, ..., R n}, the static feature point set is O={O1,O2,…,O n}, P = R∪O, all ORB feature points in the set R will participate in the screening of dynamic feature points, and the coordinate information (X, Y) of each ORB feature point is calculated by the ORB-SLAM2 system front end; (X A1 ,Y A1 ) and (X A2 ,Y A2 ) are the coordinates of the upper left vertex and the lower right vertex of the RGB-D scene image respectively;

[0024] S223: The logic discrimination module removes dynamic feature points using a dynamic feature point removal algorithm. The dynamic feature point removal algorithm is:

[0025] The object targets are divided into three states: high dynamic, medium dynamic and low dynamic, among which low dynamic is directly considered as static;

[0026] If the ORB feature point (x, y) belongs to the low dynamic feature category, it is determined to be a static feature point, otherwise the ORB feature point (x, y) is in the high dynamic feature range;

[0027] If the ORB feature point (x, y) belongs to the low dynamic feature category, it is determined to be a static feature point. Otherwise, the ORB feature point (x, y) is determined to be a dynamic feature point. The dynamic ORB feature point is eliminated and the static feature point is matched between frames.

[0028] S3 specifically includes the following steps:

[0029] S31: RGBD camera initialization;

[0030] S32: initial pose estimation;

[0031] After successful initialization, each time a new scene image is passed in, the initial pose is estimated first, then a local map is generated, and then BA is performed on the map points in the local map and the current pose. The initial pose is estimated using a constant velocity motion model, a reference frame pose estimation, or a relocalization pose estimation.

[0032] S33: tracking local map;

[0033] After successfully estimating the initial pose of the current RGB-D scene image frame, the local map is updated based on the current pose and the pose of the current RGB-D scene image frame is optimized.

[0034] S33 tracking local map includes the following steps:

[0035] S331: Update local map;

[0036] S332: Projecting the local map points onto the feature points of the current RGB-D scene image frame;

[0037] S333: BA optimizes the current frame pose;

[0038] S334: Update the observed values ​​of the map points and count the number of inliers;

[0039] S335: Determine whether tracking is successful based on the number of inliers.

[0040] Beneficial effects of the present invention:

[0041] The present invention applies the improved target detection network yolov5s as the parallel thread of the visual SLAM system in the present invention, uses the lightweight network Ghostnet as the backbone network of the target detection network yolov5s, and integrates the CA attention mechanism at the same time, which not only reduces the model parameter amount and computing power requirements, improves the reasoning speed on the CPU, but also can better cope with the challenges of complex dynamic environments. In the dynamic feature point removal part, the improved target detection network yolov5s and the logical discrimination module are combined to quickly remove the dynamic feature points at the front end of the visual SLAM system to improve the accuracy and efficiency of the entire visual SLAM system. The present invention balances the competing demands between positioning accuracy and computational complexity, and the present invention is improved in both real-time performance and precision. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 Schematic diagram of a lightweight visual SLAM system in a dynamic scene according to an embodiment of the present invention;

[0043] Figure 2 Schematic diagram of the structure of a traditional target detection network in an embodiment of the present invention;

[0044] Figure 3 This is a schematic diagram of the structure of the improved target detection network yolov5s in an embodiment of the present invention;

[0045] Figure 4 Schematic diagram of the structure of the lightweight network Ghostnet in an embodiment of the present invention;

[0046] Figure 5 Schematic diagram of the CA attention mechanism in an embodiment of the present invention;

[0047] Figure 6 Schematic diagram of the change in spatial position of dynamic factors in space according to an embodiment of the present invention;

[0048] Figure 7 This is a schematic diagram of how a highly dynamic object obtains higher attention in an embodiment of the present invention;

[0049] Figure 8 Schematic diagram showing the comparison of maximum error, minimum error, root mean square error and standard deviation between the two systems in an embodiment of the present invention;

[0050] Figure 9 Schematic diagram of training sample labels and confusion matrix in an embodiment of the present invention;

[0051] Figure 10 Schematic diagram comparing the estimated trajectory and the actual trajectory of the visual SLAM system of the present invention on the walking_xyz and walking_halfsphere datasets in an embodiment of the present invention;

[0052] Figure 11 Schematic diagram of error distribution of the visual SLAM system of the present invention on the walking_xyz and walking_halfsphere datasets in an embodiment of the present invention;

[0053] Figure 12 Schematic diagram of the estimated RPY error distribution of the visual SLAM system of the present invention on walking_xyz and walking_halfsphere in an embodiment of the present invention;

[0054] Figure 13 A data visualization diagram showing a statistical comparison of the error ATE between the visual SLAM system of the present invention and different SLAM systems in an embodiment of the present invention;

[0055] Figure 14 This is a schematic diagram of error analysis for a comprehensive comparison of the root mean square error (RMSE) of the visual SLAM system of the present invention under the three data of walking_static, walking_xyz, and walking_halfsphere in an embodiment of the present invention. DETAILED DESCRIPTION

[0056] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0057] like Figure 1 As shown, a lightweight visual SLAM system in a dynamic scene according to an embodiment of the present invention includes a visual sensor, a tracking thread, a target detection module, a logic discrimination module, a local mapping thread, a loop thread and all BA threads.

[0058] The visual sensor adopts an RGBD camera, and the RGBD camera is used to collect scene images.

[0059] The target detection module is an improved target detection network yolov5s, and the structure of the improved target detection network yolov5s is as follows: Figure 3 As shown, Figure 2 For the traditional target detection network, the target detection network yolov5s improved by the present invention is Figure 2 The improved target detection network yolov5s includes an input terminal, a backbone network, a neck network and a head network. The backbone network adopts a lightweight network Ghostnet. The structure of the lightweight network Ghostnet is as follows: Figure 4 As shown, the lightweight network Ghostnet incorporates the CA attention mechanism, which is as follows Figure 5 As shown, the target detection module is embedded in the tracking thread in the form of a thread, and the target detection module is used to detect object categories.

[0060] In the visual SLAM system, the movement of targets in a dynamic environment involves changes in spatial position, or moving along a parallel trajectory in front of the RGBD camera while maintaining an equidistant distance, or moving along a perpendicular trajectory in front of the RGBD camera with a distance from the camera that is either close or far. This is accompanied by the fact that dynamic factors in the image may be large or small. When the dynamic target becomes too small, it will affect the recognition accuracy and thus reduce the elimination accuracy. In order to better identify and eliminate dynamic factors, the visual SLAM system in the present invention introduces the CA attention mechanism in the system front-end image preprocessing, and integrates the CA attention mechanism with the lightweight network Ghostnet as the carrier to improve the visual SLAM system's attention to dynamic factors and thus improve recognition accuracy. Compared with the computational overhead of most attention mechanisms, the CA attention mechanism is more suitable for mobile networks with limited computing power. At the same time, the CA attention mechanism encodes channel relationships and long-term dependencies through precise position information, making up for the fact that other attention mechanisms only consider encoding information between channels and ignore the importance of position information, which is crucial for capturing dynamic target structures in SLAM visual tasks. For the complex mobile environment in visual SLAM scenarios, it can make the conversion as simple as possible. First, the CA attention mechanism can be flexibly inserted into the classic mobile network with almost no computational overhead; second, it can effectively capture the relationship between channels and make full use of the captured position information, so that the area of ​​interest (dynamic targets in the mobile environment) can be accurately captured.

[0061] A CA attention mechanism module can be viewed as a computational unit designed to enhance the expressiveness of features in mobile networks. It can take any intermediate feature tensor X = [x1, x2, ..., x C ]∈R C×H×W As input, it outputs Y=[y1,y2,...,y C ], the CA attention mechanism encodes channel relationships and long-term dependencies through precise location information. The specific operation is divided into two steps: coordinate information embedding and CA attention mechanism generation. The module structure of the CA attention mechanism is as follows Figure 5 shown.

[0062] The addition of the CA attention mechanism of the present invention improves three properties of the lightweight network Ghostnet. First, for applications in mobile network environments, the new transformation should be as simple as possible; second, it can make full use of the captured location information so that the area of ​​interest can be accurately captured; finally, it should also be able to effectively capture the relationship between channels. Figure 6 As shown, it can be seen that in the actual dynamic environment, the dynamic character on the right continuously moves from point A to point C and changes its position in space.

[0063] When dynamic targets constantly change their spatial positions in a mobile environment, the proportion of dynamic targets in the scene image on the RGB-D scene image frame input to the visual SLAM system will constantly change, which will affect the detection of dynamic targets by the lightweight network Ghostnet, and then affect the next step of elimination, and ultimately affect the overall performance of the system. Therefore, after the lightweight network Ghostnet is integrated with the CA attention mechanism and high-movement objects are taken as objects of interest, it will gain higher attention, that is, it will be easier to be perceived and captured by the network, thereby providing better results for the elimination of dynamic features. The higher attention dynamic capture effect is as follows: Figure 7 As shown in the figure, when a highly dynamic object is located in a static position in space and is farther and farther away from the camera during movement, it can still be better captured by the lightweight object detection network.

[0064] like Figure 8 As shown in the figure, the visual SLAM system with the CA attention mechanism has relatively good performance in the four accuracy error evaluation criteria of Max, Min, Rmse, and Std. It can be seen that the integration of the CA attention mechanism can effectively help the visual SLAM system reduce errors and improve accuracy.

[0065] The improved Backbone network (backbone network) is as follows Figure 4As shown in the figure, the current input frame is first processed into an image input of size 640×640×3, and then the model layer factor is defined as 0.33 to adjust the depth of the network, and the model channel factor is defined as 0.50 to adjust the depth of the network. First, the image is passed to the backbone network for processing. First, after Conv convolution processing, the size of the feature map becomes 1 / 2 of the original image, and then after Ghostconv lightweight convolution operation, the size of the feature map becomes 1 / 4 of the original image, and then after C3 structure processing, the input is processed in two ways. One is to obtain an output value output1 by the residual network, and the other is to directly obtain an output matrix output2 by the Conv convolution network. Finally, the two output values ​​are added, and the size of the feature map does not change after the C3 layer, and then the feature map size becomes 1 / 32 of the original image after three layers of Ghostconv processing, and then passes through the CA attention layer. The process is divided into two parallel stages: first, the input feature map is globally average pooled in the width and height directions respectively, and feature maps in the width and height directions are obtained respectively. Assume the input feature layer has a shape of [C, H, W]. After average pooling in the width direction, the resulting feature layer has a shape of [C, H, 1]. This maps the features to the high dimension. After average pooling in the height direction, the resulting feature layer has a shape of [C, 1, W]. This maps the features to the wide dimension. The two parallel stages are then merged, transposing the width and height to the same dimension. The layers are then stacked, combining the width and height features. The resulting feature layer is [C, 1, H + W]. Convolution, normalization, and activation functions are then used to obtain features. The process is then split into two parallel stages again, separating the width and height into [C, 1, H] and [C, 1, W]. These features are then transposed, resulting in two feature layers: [C, H, 1] and [C, 1, W]. A 1x1 convolution is then used to adjust the number of channels, followed by a sigmoid filter to obtain attention across the width and height dimensions. This is then multiplied by the original features to form the attention mechanism, which enhances the model's ability to correlate features across different channels. Finally, the SPPF layer improves the accuracy and speed of target detection. SPPF stands for Spatial Pyramid Pooling, a spatial pyramid pooling network structure that extracts receptive field features of varying sizes to cover targets of varying scales. This structure allows the network to better adapt to targets of varying scales, improving target detection accuracy while also ensuring rapid detection. The network then passes through the head network, undergoing sampling without changing the number of channels. The length and width of the feature map are doubled. The network then undergoes a Ghostconv lightweight convolution operation, followed by feature fusion with the outputs of the 4th, 10th, and 14th layers, resulting in outputs of three different sizes.

[0066] The logic discrimination module is used to determine whether the object detected by the target detection module is a dynamic target or a static target, and to determine whether to remove the dynamic target or the static target.

[0067] The tracking thread includes an ORB feature point extraction module, which extracts ORB feature points from the scene image captured by the RGBD camera through the ORB feature point extraction module. After removing the ORB feature points corresponding to the dynamic targets, the remaining static target ORB feature points are feature matched to restore the RGBD camera pose. After obtaining the initial pose, local pose optimization is performed, and then key frames are reviewed and generated.

[0068] When the queue of key frames to be processed is not empty, the local mapping thread starts working. The local mapping thread processes the generated key frames, deletes map points that do not meet the conditions, and creates new map points with matching relationships to supplement them. Then, local BA optimization is performed to optimize all map points and delete redundant key frames.

[0069] The loop closure thread is used to detect large loops and correct the accumulated error by performing pose graph optimization. The loop closure thread starts the global BA thread after the pose graph is optimized.

[0070] The global BA thread executes global BA and updates the map.

[0071] A lightweight VSLAM method for dynamic scenes, which is applied to a lightweight visual SLAM system for dynamic scenes, comprises the following steps:

[0072] S1: Obtain the RGB-D scene image frame captured by the RGBD camera.

[0073] The RGBD camera captures dynamic video, which is processed into multiple RGB-D scene image frames.

[0074] S2: Extraction of ORB feature points from RGB-D scene image frames and detection and removal of dynamic target feature points.

[0075] The extraction of ORB feature points in RGB-D scene image frames and the detection and removal of dynamic target feature points specifically include the following steps:

[0076] S21: The tracking thread extracts ORB feature points of the current RGB-D scene image frame through the ORB feature extraction module.

[0077] S22: Detect the object category on the RGB-D scene image frame captured by the RGBD camera through the improved target detection network yolov5s, and determine whether the object is a dynamic target or a static target through the logic discrimination module. If the object is a dynamic target, remove all ORB feature points corresponding to the dynamic target and retain the ORB feature points corresponding to the static target.

[0078] Specifically, the specific method for detecting and eliminating dynamic target ORB feature points is:

[0079] S221: The improved target detection network yolov5s generates a prediction box after inferring the RGB-D scene image frame. The prediction box consists of six parameters: position parameters x, y, w, h, confidence and classification results. Among them, x and y are the relative values ​​of the center of the prediction box and the RGB-D scene image, w and h represent the relative values ​​of the length and width of the prediction box and the RGB-D scene image, the confidence represents the credibility of the prediction box containing an object and the accuracy of the prediction box position, and the classification result is determined according to the object category contained in the data set used during training.

[0080] S222: Convert the parameters output by the improved target detection network yolov5s into a formula for the coordinates of the prediction box in the RGB-D scene image. The formula is:

[0081]

[0082] Among them, l is the width of the RGB-D scene image; d is the height of the RGB-D scene image, the ORB feature points contained in the prediction box are all undetermined dynamic feature points based on prior knowledge, and the features outside the prediction box are all static feature points. If the set of all ORB feature points is P = {P1, P2, ..., P n}, the set of undetermined dynamic feature points R = {R1, R2, ..., R n}, the static feature point set is O={O1,O2,…,O n}, P = R∪O, all ORB feature points in the set R will participate in the screening of dynamic feature points, and the coordinate information (X, Y) of each ORB feature point is calculated by the ORB-SLAM2 system front end; (X A1 ,Y A1 ) and (X A2 ,Y A2 ) are the coordinates of the upper left vertex and the lower right vertex of the RGB-D scene image, respectively.

[0083] S223: The logic discrimination module removes dynamic feature points using a dynamic feature point removal algorithm. The dynamic feature point removal algorithm is:

[0084] The object targets are divided into three states: high dynamic, medium dynamic and low dynamic, among which low dynamic is directly considered as static;

[0085] If the ORB feature point (x, y) belongs to the low dynamic feature category, it is determined to be a static feature point, otherwise the ORB feature point (x, y) is in the high dynamic feature range;

[0086] If the ORB feature point (x, y) belongs to the low dynamic feature category, it is determined to be a static feature point. Otherwise, the ORB feature point (x, y) is determined to be a dynamic feature point. The dynamic ORB feature point is eliminated and the static feature point is matched between frames.

[0087] S3: Perform feature matching based on the ORB feature points of the static target to restore the RGBD camera pose. After obtaining the initial pose, perform local pose optimization, and then review and generate key frames.

[0088] Specifically, step S3 includes the following steps:

[0089] S31: RGBD camera initialization.

[0090] The RGBD camera is a monocular camera, and the initialization steps of the RGBD camera are:

[0091] S311: If the RGBD camera does not create an initializer, create an initializer.

[0092] S312: If the number of ORB feature points of the previous RGB-D scene image frame is sufficient but the number of ORB feature points of the current RGB-D scene image frame is too small, the matching fails and the initializer is deleted.

[0093] S313: Perform feature matching between the initialization frame and the current frame.

[0094] S314: If the number of matched ORB feature points is too small, the matching fails and the initializer is deleted;

[0095] S315: Perform monocular initialization.

[0096] S316: Create an initialization map.

[0097] After initialization, two keyframes are inserted into the map, all map points are processed (adding map points, building bidirectional connections between keyframes and map points, etc.), global BA optimizes the poses of all keypoints and map points, normalizes the translation scale and coordinate point scale, and updates the map point coordinates and poses.

[0098] S32: Initial pose estimation.

[0099] After successful initialization, each time a new scene image is passed in, the initial pose is estimated first, then a local map is generated, and then BA is performed on the map points in the local map and the current pose. Among them, the initial pose estimation uses constant speed motion model estimation, reference frame pose estimation, or relocalization pose estimation.

[0100] S33: Tracking local map.

[0101] After successfully estimating the initial pose of the current RGB-D scene image frame, the local map is updated based on the current pose and the pose of the current RGB-D scene image frame is optimized. The specific steps are as follows:

[0102] S331: Update the local map.

[0103] S332: Project the local map points onto the feature points of the current RGB-D scene image frame.

[0104] S333: BA optimizes the current frame pose.

[0105] S334: Update the map point observation value and count the number of inliers.

[0106] S335: Determine whether tracking is successful based on the number of inliers.

[0107] S44: Generate key frames.

[0108] In the embodiment of the present invention, the requirements for generating key frames are:

[0109] The current RGB-D scene image frame must have enough map points, and the overlap with the reference keyframe should not have too much time interval. In addition, if relocalization is performed, the relocalized pose will not be too accurate and cannot be used as a reference frame. The reference frame is to set the newly created keyframe as the reference keyframe and the keyframe with the highest degree of co-viewing with the current keyframe as the reference keyframe.

[0110] S5: The local mapping thread processes the generated key frames, deletes map points that do not meet the conditions, and creates new map points with matching relationships to supplement them. Then, local BA optimization is performed to optimize all map points and delete redundant key frames.

[0111] Step S5 specifically includes the following steps:

[0112] S51: Check whether the buffer queue contains key frames.

[0113] S52: Process the first key frame in the buffer queue.

[0114] S53: Remove bad pixels.

[0115] In the embodiment of the present invention, if a map point has not been eliminated after three consecutive key frames, it is considered a good map point, wherein the elimination criteria are: recall rate < 0.25 and the number of observations within the three frames created is less than 2;

[0116] S54: Create new map points to supplement bad points.

[0117] Map points are generated by pairwise feature matching of the current keyframe with the top 10 (20 for monocular cameras) common view keyframes with the highest common view degree. For matched feature point pairs from binocular cameras, map points can be recovered based on the depth of the feature points in a particular frame, or by triangulating the map points based on the epipolar geometry between the two frames. Here, the method with the largest parallax angle is used to generate the map points.

[0118] S55: Fusing the current key frame with the common view key frame map point.

[0119] In the embodiment of the present invention, during the process of back-projecting the map points into the frame, there are the following two situations:

[0120] (1) If there is no map point at the corresponding position of the back-projection of the map point, add the observation directly;

[0121] (2) If there is a corresponding map point at the back-projected position of the map point, the two map points are merged into the one with more observations (replacement).

[0122] S56: Local BS optimization.

[0123] S57: Eliminate redundant key frames, wherein the standard for redundant key frames is: 90% of the map points of a key frame can be observed by more than three other key frames.

[0124] S58: Add the current key frame to the loop detection of the loop thread.

[0125] S6: The loop closure thread detects large loops and corrects the accumulated error by performing pose graph optimization.

[0126] S7: After the pose graph is optimized, the loopback thread starts the global BA thread to perform global BA and update the map.

[0127] Experiment and result analysis

[0128] The present invention uses three data sets in the RGB-D data set series provided by the Technical University of Munich for testing, namely walking_static, walking_xyz, and walking_halfsphere. The series is divided into high-dynamic scenes and low-dynamic scenes. In high-dynamic scenes, people will continue to walk in the scene; in low-dynamic scenes, people do not have obvious movements. The static, xyz, and halfsphere in the data set names represent different camera motion modes: the camera is basically stationary; the camera moves along the X and Y axes; the camera rotates on the pitch and yaw axes on the surface of a hemisphere with a diameter of 1m. The real trajectory in the data set is obtained by a motion capture system, which includes multiple high-speed cameras and an inertial measurement system, which can obtain data on the camera position and posture in real time.

[0129] 1. Dynamic target detection experiment

[0130] The target detection network is trained using the COCO dataset, which is a large-scale dataset that can be used for image detection, semantic segmentation, and image caption generation. It has more than 330K images (220K of which are labeled images), including 1.5 million targets, 80 target categories (pedestrians, cars, elephants, etc.), 91 material categories (grass, wall, sky, etc.), and each image contains five sentences describing the image. There are 250,000 pedestrians with key points labeled. This dataset is sufficient to detect dynamic targets, so as to better detect dynamic factors in the visual SLAM environment and remove them in the next step. The training sample labels and confusion matrix are as follows: Figure 9 As shown in the figure, the training process continues until the loss of prediction box, category, and confidence level steadily decreases. In order to achieve better training results, the batch training samples are then reduced and trained until the model converges.

[0131] 2. Pose Estimation Error Analysis Experiment

[0132] The pose estimation error analysis experiment uses the evo tool to test and compare the camera pose (CameraTrajectory) estimated by the visual SLAM system with the true pose (groundtruth) given by the dataset. The test indicator is the absolute trajectory error (ATE). The ATE indicator directly calculates the difference between the true value of the camera pose and the estimated value of the visual SLAM system, which can very intuitively reflect the accuracy of the algorithm and the global consistency of the trajectory. In the experiment, the root mean square error (RMSE) is used as the main evaluation criterion for system performance. The root mean square error is used to describe the deviation between the observed value and the true value. It is easily affected by large or occasional errors, so it can better reflect the robustness of the system. The ATE of the i-th frame is defined as follows:

[0133]

[0134] where P1,....,P n ∈SE(3) is the estimated pose, Q1,.....Q n ∈SE(3) is the true pose. It should be noted that the estimated pose and the true pose are usually not in the same coordinate system, so they need to be aligned. For the binocular SLAM system and RGB-DSLAM scale unification, a transformation matrix S∈SE(3) from the estimated pose to the true pose is calculated by the least squares method. The root mean square error RMSE is expressed as follows:

[0135]

[0136] Where t represents time (or frame), assuming that the estimated pose and the true pose are time-aligned, the total number of frames is the same, and Δ represents the interval. When the total number of frames n and the interval Δ are known, we can get m = n-Δ ATE, where trans(F i ) represents the translation error.

[0137] like Figure 10 and Figure 11 As shown in FIG, the estimated trajectory and error distribution diagram of the visual SLAM system of the present invention under the two data sets walking_xyz and walking_halfsphere are respectively. Figure 12 It is the estimated RPY error distribution of the visual SLAM system of the present invention on walking_xyz and walking_halfsphere.

[0138] The error comparison between the visual SLAM system of the present invention and different SLAM systems is carried out, and the error ATE is statistically compared, as shown in Table 1. The data visualization comparison is as follows: Figure 13 shown.

[0139] Table 1

[0140]

[0141] The visual SLAM system of the present invention is compared with other SLAM systems in dynamic scenes, including DynaSLAM and DS-SLAM systems based on semantic segmentation networks, DVO-SLAM that obtains constraints by optimizing the pose graph between key frames to minimize the luminosity and depth errors, OFD-SLAM and MR-SLAM, dynamic SLAM methods based on optical flow method, and the comparison results are shown in Table 2, where the bold part represents the best result. It can be found from the table that the best and second-best results mainly come from DynaSLAM and the visual SLAM system of the present invention. The root mean square error RMSE of the visual SLAM system of the present invention under the three data of walking_static, walking_xyz and walking_halfsphere is comprehensively compared, and the error analysis is shown in Table 2. Figure 14 shown.

[0142] Table 2

[0143]

[0144] The absolute trajectory error comparison between ORB-SLAM3 and the visual SLAM system of the present invention is shown in Table 3:

[0145]

[0146]

[0147] Let the error of ORB-SLAM3 be m and the error of the visual SLAM system of the present invention be n, then the calculation formula of the relative improvement rate R is:

[0148]

[0149] As shown in Table 3, compared with the ORB-SLAM3 system, the visual SLAM system of the present invention achieved an average RMSE improvement of 84.04% across the three datasets. Regarding the issue that the improvement in the walking_static dataset was not as good as in the other two datasets, by observing the images processed by the target detection network and the actual state of the system during operation, it was found that in the walking_static dataset, a considerable number of image frames had people occupying too large an area, resulting in fewer detected static feature points. Furthermore, the dataset also suffers from image distortion caused by excessive image rotation angles, which prevents some portraits from being detected by the improved target detection network, yolov5s, ultimately leading to a decrease in estimation accuracy.

[0150] The present invention applies the improved target detection network yolov5s as the parallel thread of the visual SLAM system in the present invention, uses the lightweight network Ghostnet as the backbone network of the target detection network yolov5s, and integrates the CA attention mechanism at the same time, which not only reduces the model parameter amount and computing power requirements, improves the reasoning speed on the CPU, but also can better cope with the challenges of complex dynamic environments. In the dynamic feature point removal part, the improved target detection network yolov5s and the logical discrimination module are combined to quickly remove the dynamic feature points at the front end of the visual SLAM system to improve the accuracy and efficiency of the entire visual SLAM system. The present invention balances the competing demands between positioning accuracy and computational complexity. The experimental results on the TUM data set show that compared with the ORB-SLAM3 system, the pose estimation accuracy of the visual SLAM system of the present invention is improved by 84.04%. Compared with typical dynamic SLAM systems such as DS-SLAM, DVOSLAM, and DynaSLAM, the system of the present invention is improved in real time and accuracy.

[0151] In the description of the present invention, the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the number of the technical features indicated. Therefore, a feature specified as "first" or "second" may explicitly or implicitly include one or more of such features. "Multiple" means two or more, unless otherwise specifically defined.

[0152] In the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," "connect," "fixed," etc. should be understood broadly. For example, they may refer to fixed connection, detachable connection, or integration; mechanical connection or electrical connection; direct connection or indirect connection through an intermediate medium; internal communication between two components or interaction between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0153] In the present invention, unless otherwise expressly specified or limited, when a first feature is "above" or "below" a second feature, it may mean that the first and second features are in direct contact, or that the first and second features are in indirect contact through an intermediary. Furthermore, when a first feature is "above," "above," or "above" a second feature, it may mean that the first feature is directly above or diagonally above the second feature, or simply means that the first feature is at a higher level than the second feature. When a first feature is "below," "below," or "below" a second feature, it may mean that the first feature is directly below or diagonally below the second feature, or simply means that the first feature is at a lower level than the second feature.

[0154] In the description of this specification, the reference terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0155] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may be performed out of the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention pertain.

[0156] The logic and / or steps represented in the flowcharts or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing the logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (e.g., a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device). For purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include the following: an electrical connection having one or more wires (electronic devices), a portable computer disk cartridge (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disc read-only memory (CDROM). Furthermore, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing it in another suitable manner if necessary, and then storing it in a computer memory.

[0157] It should be understood that various parts of the present invention can be implemented using hardware, software, firmware, or a combination thereof. In the above-described embodiments, multiple steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0158] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0159] In addition, the functional units in the various embodiments of the present invention may be integrated into a single processing module, or each unit may exist physically separately, or two or more units may be integrated into a single module. The aforementioned integrated modules may be implemented in the form of hardware or in the form of software functional modules. If the integrated modules are implemented in the form of software functional modules and sold or used as independent products, they may also be stored in a computer-readable storage medium.

[0160] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. A lightweight visual SLAM system for dynamic scenes, characterized by: It includes visual sensor, tracking thread, target detection module, logical discrimination module, local mapping thread, loop closure thread and all BA threads, among which, The visual sensor adopts an RGBD camera, and the RGBD camera is used to collect scene images; The target detection module is an improved target detection network yolov5s, wherein the improved target detection network yolov5s includes an input end, a backbone network, a neck network and a head network. The backbone network adopts a lightweight network Ghostnet, and the lightweight network Ghostnet incorporates a CA attention mechanism. The target detection module is embedded in the tracking thread in the form of a thread, and the target detection module is used to detect object categories; The logic discrimination module is used to determine whether the object detected by the target detection module is a dynamic target or a static target, and to determine whether to remove the dynamic target or the static target; The tracking thread includes an ORB feature point extraction module, which extracts ORB feature points from the scene image captured by the RGBD camera through the ORB feature point extraction module. After removing the ORB feature points corresponding to the dynamic targets, the tracking thread performs feature matching on the remaining static target ORB feature points to restore the RGBD camera pose. After obtaining the initial pose, the local pose optimization is performed, and then the key frame is reviewed and generated. When the queue of key frames to be processed is not empty, the local mapping thread starts working. The local mapping thread processes the generated key frames, deletes the map points that do not meet the conditions, and creates new map points with matching relationships to supplement them. Then, the thread performs local BA optimization to optimize all map points and delete redundant key frames. The loop closure thread is used to detect large loops and correct the accumulated error by performing pose graph optimization. The loop closure thread starts the global BA thread after the pose graph optimization; The global BA thread executes global BA and updates the map; The specific method for detecting and eliminating dynamic target ORB feature points is as follows: The improved object detection network yolov5s generates a prediction box after reasoning on the RGB-D scene image frame. The prediction box consists of six parameters: position parameters x, y, w, h, as well as confidence and classification results. Among them, x and y are the relative values ​​of the center of the prediction box and the RGB-D scene image, w and h represent the relative values ​​of the length and width of the prediction box and the RGB-D scene image, and confidence represents the credibility of the prediction box containing an object and the accuracy of the prediction box position. The classification result is determined by the object category contained in the dataset used during training. The parameters output by the improved target detection network yolov5s are converted into the formula for the coordinates of the prediction box in the RGB-D scene image. The formula is: Among them, l is the width of the RGB-D scene image; d is the height of the RGB-D scene image, the ORB feature points contained in the prediction box are all undetermined dynamic feature points based on prior knowledge, and the features outside the prediction box are all static feature points. If the set of all ORB feature points is P = {P1, P2, ..., P n }, the set of undetermined dynamic feature points R = {R1, R2, ..., R n }, the static feature point set is O={O1,O2,…,O n }, P = R∪O, all ORB feature points in the set R will participate in the screening of dynamic feature points, and the coordinate information (X, Y) of each ORB feature point is calculated by the ORB-SLAM2 system front end; (X A1 ,Y A1 ) and (X A2 ,Y A2 ) are the coordinates of the upper left vertex and the lower right vertex of the RGB-D scene image respectively; The logic discrimination module removes dynamic feature points through the dynamic feature point removal algorithm. The dynamic feature point removal algorithm is: The object targets are divided into three states: high dynamic, medium dynamic and low dynamic, among which low dynamic is directly considered as static; If the ORB feature point (x, y) belongs to the low dynamic feature category, it is determined to be a static feature point, otherwise the ORB feature point (x, y) is in the high dynamic feature range; If the ORB feature point (x, y) belongs to the low dynamic feature category, it is determined to be a static feature point. Otherwise, the ORB feature point (x, y) is determined to be a dynamic feature point. The dynamic ORB feature point is eliminated and the static feature point is matched between frames.

2. A lightweight VSLAM method in dynamic scenes, which is applied to the lightweight visual SLAM system in dynamic scenes according to claim 1, characterized in that: The following steps are involved: S1: Get the RGB-D scene image frame captured by the RGBD camera; S2: Extraction of ORB feature points from RGB-D scene image frames and detection and removal of dynamic target feature points; The specific steps include: S21: The tracking thread extracts ORB feature points of the current RGB-D scene image frame through the ORB feature point extraction module; S22: Detecting the object category on the RGB-D scene image frame captured by the RGBD camera through the improved target detection network yolov5s, and determining whether the object is a dynamic target or a static target through the logic discrimination module. If the object is a dynamic target, all ORB feature points corresponding to the dynamic target are eliminated, and the ORB feature points corresponding to the static target are retained; S3: Perform feature matching based on the ORB feature points of the static target to restore the RGBD camera pose. After obtaining the initial pose, perform local pose optimization, and then review and generate key frames. S4: Generate key frames; S5: The local mapping thread processes the generated keyframes, deletes map points that do not meet the conditions, and creates new map points with matching relationships to supplement them. Then, it performs local BA optimization to optimize all map points and delete redundant keyframes. S6: The loop closure thread detects large loops and corrects the accumulated error by performing pose graph optimization; S7: After the pose graph is optimized, the loopback thread starts the global BA thread to perform global BA and update the map.

3. Lightweight VSLAM method under dynamic scene according to claim 2, is characterized in that, S3 specifically includes the following steps: S31: RGBD camera initialization; S32: initial pose estimation; After successful initialization, each time a new scene image is passed in, the initial pose is estimated first, then a local map is generated, and then BA is performed on the map points in the local map and the current pose. The initial pose is estimated using a constant velocity motion model, a reference frame pose estimation, or a relocalization pose estimation. S33: tracking local map; After successfully estimating the initial pose of the current RGB-D scene image frame, the local map is updated based on the current pose and the pose of the current RGB-D scene image frame is optimized.

4. Lightweight VSLAM method under dynamic scene according to claim 3, is characterized in that, S33 tracking local map includes the following steps: S331: Update local map; S332: Projecting the local map points onto the feature points of the current RGB-D scene image frame; S333: BA optimizes the current frame pose; S334: Update the observed values ​​of the map points and count the number of inliers; S335: Determine whether tracking is successful based on the number of inliers.

Images