Visual SLAM positioning and mapping method in dynamic environment

Abstract

The invention discloses a visual SLAM (Simultaneous Localization and Mapping) positioning and mapping method in a dynamic environment, which comprises a traditional visual SLAM basic process, and further comprises a dynamic object detection and filtering mechanism and / or a hierarchical adaptive map updating mechanism based on time-space consistency. The space-time consistency-based dynamic object detection and filtering mechanism is an additional introduction mechanism for feature extraction and description steps, and the hierarchical adaptive map updating mechanism is an additional introduction mechanism for map updating steps. The dynamic object detection and filtering mechanism process based on space-time consistency comprises the steps of multi-scale motion consistency detection, feature point dynamic probability evaluation and dynamic map point filtering. The hierarchical self-adaptive map updating mechanism process comprises the steps of multi-layer map structure design, self-adaptive map updating strategy and map merging and optimizing. Misjudgment is reduced, dynamic and shielding environments of different degrees are processed, scene adaptability and generalization are good, and a map is updated in real time to reflect environment changes.

Description

Technical Field

[0001] The present invention relates to a visual SLAM positioning and mapping technology, and in particular to a visual SLAM positioning and mapping method in a dynamic environment. Background Art

[0002] Visual SLAM (Simultaneous Localization and Mapping) is a technology that enables a robot or other device to determine its own position through a visual sensor (such as a camera) in an unknown environment and simultaneously construct a map of the surrounding environment. For example, when a mobile robot moves in an indoor environment, it uses a camera installed on itself to obtain environmental image information. The visual SLAM technology can calculate the position and pose (position and orientation) of the robot in space based on these image data and generate a map describing the structure of the indoor environment.

[0003] Since the environment where our robot is deployed is a production workshop, there are a large number of walking pedestrians and large mobile devices. Because traditional visual SLAM methods are based on the assumption of static scenes and do not distinguish between static and dynamic objects, the dynamic objects in these environments will affect the performance of the visual SLAM algorithm. Specifically, it is manifested in: (1) The quality of the map in the mapping process decreases: Feature points on dynamic objects are wrongly added to the map, which will cause the map to contain a large number of unstable feature points. These unstable feature points will make the map sparse and inaccurate, reducing the quality of the map. The presence of dynamic objects may affect the accuracy of loop detection because loop detection depends on the matching of feature points. If dynamic objects cause incorrect feature point matching, the loop detection of the map may fail, which will also reduce the mapping quality; (2) The positioning accuracy decreases: Feature points on dynamic objects may be misidentified as feature points in the static environment, resulting in incorrect feature point matching. These incorrect feature points will introduce noise and affect the accuracy of pose estimation, thereby affecting the positioning accuracy.

[0004] In order to improve the robustness and accuracy of the system and cope with the influence of dynamic objects in SLAM, usually an additional mechanism is introduced, such as dynamic object detection and elimination. Deep learning (such as Mask RCNN) or other computer vision technologies are used to detect dynamic objects, then perform semantic segmentation on them, and exclude them from the feature point extraction and matching process. However, traditional computer vision technologies have low accuracy, and although deep learning has high accuracy, it consumes a large amount of computing resources. In addition, MaskRCNN only relies on single-frame semantic segmentation, is easily affected by occlusion, and the edges of dynamic objects are blurred. Summary of the Invention

[0005] The present invention provides a visual SLAM positioning and mapping method in a dynamic environment, which can distinguish the features of static and dynamic objects, improve the mapping quality, improve the map positioning accuracy, and does not require a large consumption of computing resources, aiming at solving the current situation that the existing visual SLAM positioning and mapping assumes a static scene, does not distinguish static and dynamic objects, resulting in low mapping quality, low map positioning accuracy, or large consumption of computing resources.

[0006] The specific technical solution adopted by the present invention to solve the above technical problems is as follows: A visual SLAM positioning and mapping method in a dynamic environment includes image acquisition and processing, feature extraction and description, feature matching and screening, pose estimation, map update, and loop detection and optimization in the basic process. It is characterized in that: it also includes a dynamic object detection and filtering mechanism and / or a hierarchical adaptive map update mechanism based on spatio-temporal consistency. The dynamic object detection and filtering mechanism based on spatio-temporal consistency is an additional introduction mechanism for the feature extraction and description step in the basic process, and the hierarchical adaptive map update mechanism is an additional introduction mechanism for the map update step in the basic process. The process of the dynamic object detection and filtering mechanism based on spatio-temporal consistency includes multi-scale motion consistency detection, feature point dynamic probability evaluation, and dynamic map point filtering steps; the process of the hierarchical adaptive map update mechanism includes multi-layer map structure design and adaptive map update strategy and map merging and optimization steps.

[0007] Preferably, in the multi-scale motion consistency detection step, during the feature extraction stage, the motion consistency in the image is detected through multi-scale analysis, which helps to distinguish the static background and dynamic objects and reduce the interference of dynamic objects on feature point extraction; its calculation formula is as follows: Formula 1 Where F score is the multi-scale scoring result, w 1 、w 2 、w 3 are weight parameters, F optical is the optical flow consistency score, F geom is the geometric consistency score, F semantic is the semantic consistency score.

[0008] Preferably, in the feature point dynamic probability evaluation, during the feature point description stage, the dynamic probability of each feature point is evaluated, which can be achieved by analyzing the motion pattern of the feature point in consecutive frames. Feature points with a high dynamic probability will be marked as potential dynamic feature points; its calculation formula is as follows: Formula 2 Where p motion is the motion-based dynamic probability, p semanticis the dynamic probability based on semantics, p history is the historical dynamic probability, and the dynamic probability trend is obtained from the historical frame statistics. α, β, and γ are coefficients that adjust the influence of each factor.

[0009] Preferably, after the dynamic feature points are stably detected, in the feature matching stage, the above dynamic probability evaluation results are used to filter out the dynamic feature points, reducing the matching error rate and improving the matching accuracy. In the subsequent pose estimation stage, the dynamic probability evaluation results are further used to exclude the influence of the dynamic feature points, ensuring the accuracy of the pose estimation.

[0010] Preferably, there is a dynamic probability threshold p for filtering the dynamic map points threshold ; when the probability evaluation result calculated by the feature point is greater than the dynamic probability threshold, it is marked as a dynamic feature point and stored in the dynamic mask feature set; if the feature point probability evaluation result is lower than the dynamic probability threshold, it is marked as a static feature point and incorporated into the static map for update.

[0011] Preferably, the multi-layer map structure design includes a global static map layer design, a local dynamic map layer design, and a semantic instance map layer design. Among them, the global static map layer design stores the feature points of the static environment, providing a stable positioning and mapping basis for map update; the local dynamic map layer design stores the feature points of the dynamic environment, which is used for short-term positioning and mapping design for map update; the semantic instance map layer design stores semantic information, including object categories and object positions, providing a richer environmental description for map update.

[0012] Preferably, the adaptive map update strategy adaptively updates different levels of the map according to the environmental dynamics. When a dynamic object is detected, the local dynamic map layer is updated, while the global static map layer remains unchanged; its strategy formula is as follows: Formula 3 where t static is the static duration of the feature point, n observe is the number of observations of the feature point, q measure is the measurement performance of the feature point.

[0013] Preferably, the map merging and optimization is carried out in the loop detection stage, using the multi-layer map structure for loop detection; after the loop is detected, through map merging and optimization, global consistency is ensured; the feature points in the local dynamic map layer can be matched and optimized with the feature points in the global static map layer to improve the overall accuracy of the map; its formula is as follows: Formula 4 where m final is the final map, m staticis a static map, m dynamic is a dynamic map, m semantic is a semantic map.

[0014] Preferably, the number of observations of the feature points shall not be less than the minimum number of observations, which is 3 times.

[0015] Preferably, the dynamic probability threshold is set to 0.6, or an empirical value obtained through actual tests.

[0016] The beneficial effects of the present invention are as follows: It can distinguish the features of static and dynamic objects, improve the mapping quality, improve the map positioning accuracy, and does not require deep learning with large computational resource consumption. The present invention proposes a high-efficiency and robust visual SLAM algorithm for a dynamic environment. The present invention combines optical flow, geometric, and semantic multi-scale features, considers temporal information and historical states, and reduces misjudgments through a probability model, and can handle dynamic and occluded environments with different degrees, and has good scene adaptability and generalization. In addition, a map construction algorithm suitable for a dynamic environment is designed to accurately extract the object point cloud, construct a dense multi-level map, and can update the map in real time to reflect the changes in the environment. The present invention improves the reliability and computational speed of dynamic detection, strengthens the efficiency of map management at the same time, is easy to implement in engineering, and improves the practicality of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 is a schematic structural diagram of the flowchart of the visual SLAM positioning and map construction method in a dynamic environment of the present invention.

[0018] Figure 2 is a schematic structural diagram of the flowchart of the dynamic map point filtering flowchart in the visual SLAM positioning and map construction method in a dynamic environment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0019] The following further describes the present invention in detail with reference to the drawings and specific embodiments.

[0020] Figure 1 、 Figure 2 In the embodiment shown, wherein Figure 1 The left view is the basic process of traditional visual SLAM. A visual SLAM positioning and map construction method in a dynamic environment includes image acquisition and processing, feature extraction and description, feature matching and screening, pose estimation, map update, and loop detection and optimization of the basic process (see Figure 1Left view), the innovation of the present invention involves feature extraction and description as well as map update steps: specifically, it includes a dynamic object detection and filtering mechanism based on spatio-temporal consistency and / or a hierarchical adaptive map update mechanism. The dynamic object detection and filtering mechanism based on spatio-temporal consistency is an additional mechanism introduced to the feature extraction and description steps in the basic process, and the hierarchical adaptive map update mechanism is an additional mechanism introduced to the map update step in the basic process. The process of the dynamic object detection and filtering mechanism based on spatio-temporal consistency includes multi-scale motion consistency detection, feature point dynamic probability evaluation, and dynamic map point filtering steps; the process of the hierarchical adaptive map update mechanism includes multi-layer map structure design, adaptive map update strategy, and map merging and optimization steps.

[0021] In the multi-scale motion consistency detection step, during the feature extraction stage, motion consistency in the image is detected through multi-scale analysis, which helps to distinguish the static background and dynamic objects and reduce the interference of dynamic objects on feature point extraction; its calculation formula is as follows: Formula 1 Where F score is the multi-scale scoring result, w 1 , w 2 , w 3 are weight parameters, F optical is the optical flow consistency score, F geom is the geometric consistency score, F semantic is the semantic consistency score.

[0022] In the feature point dynamic probability evaluation, during the feature point description stage, the dynamic probability of each feature point is evaluated, which can be achieved by analyzing the motion pattern of the feature point in consecutive frames. Feature points with a high dynamic probability will be marked as potential dynamic feature points; its calculation formula is as follows: Formula 2 Where p motion is the motion-based dynamic probability, p semantic is the semantics-based dynamic probability, p history is the historical dynamic probability, and the dynamic probability trend is obtained from historical frame statistics. α, β, γ are coefficients that adjust the influence of each factor.

[0023] After the dynamic feature points are stably detected, in the feature matching stage, the above dynamic probability evaluation results are used to filter out the dynamic feature points, reducing the matching error rate and improving the matching accuracy. In the subsequent pose estimation stage, the dynamic probability evaluation results are further used to exclude the influence of dynamic feature points and ensure the accuracy of pose estimation.

[0024] When filtering out dynamic feature points from the dynamic probability evaluation results of feature points, there is a dynamic probability threshold p for dynamic map point filtering threshold ; The dynamic probability threshold is set to 0.6 (an empirical value obtained through actual testing). It is judged whether the dynamic probability evaluation result of the feature point is greater than the dynamic probability threshold. When the calculated probability evaluation result of the feature point is greater than the dynamic probability threshold, it is marked as a dynamic feature point and stored and updated into the dynamic mask feature set; if the feature point probability evaluation result (no) is lower than the dynamic probability threshold, it is marked as a static feature point and incorporated into the static map for update (see Figure 2 ).

[0025] The multi-layer map structure design includes the global static map layer design, the local dynamic map layer design, and the semantic instance map layer design. Among them, the global static map layer design stores the feature points of the static environment, providing a stable positioning and mapping foundation for map update; the local dynamic map layer design stores the feature points of the dynamic environment, used for short-term positioning and mapping design for map update; the semantic instance map layer design stores semantic information, including object categories and object positions, providing a richer environmental description for map update.

[0026] The adaptive map update strategy adaptively updates different levels of the map according to the environmental dynamics. When a dynamic object is detected, the local dynamic map layer is updated, while the global static map layer remains unchanged; its strategy formula is as follows: Formula 3 where t static is the static duration of the feature point, n observe is the number of observations of the feature point, q measure is the measurement performance of the feature point. The number of observations of the feature point shall not be less than the minimum number of observations, which is 3 times.

[0027] Map merging and optimization is carried out in the loop detection stage, using the multi-layer map structure for loop detection; after detecting a loop, through map merging and optimization, global consistency is ensured; the feature points in the local dynamic map layer can be matched and optimized with the feature points in the global static map layer to improve the overall accuracy of the map; its formula is as follows: Formula 4 where m final is the final map, m static is the static map, m dynamic is the dynamic map, m semantic is the semantic map.

[0028] The above content and structure describe the basic principle, main features and advantages of the product of the present invention, which should be understood by those skilled in the art. What is described in the above examples and specifications only illustrates the principle of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements all fall within the scope of the present invention claimed. The scope of protection claimed by the present invention is defined by the appended claims and their equivalents.

Claims

1. A visual SLAM positioning and map construction method in a dynamic environment, including the basic process of image acquisition and processing, feature extraction and description, feature matching and screening, pose estimation, map update and loop detection and optimization, characterized in that: It also includes the use of a dynamic object detection and filtering mechanism based on spatiotemporal consistency and / or a hierarchical adaptive map update mechanism, wherein the dynamic object detection and filtering mechanism based on spatiotemporal consistency is an additional introduction mechanism for the feature extraction and description steps in the basic process, and the hierarchical adaptive map update mechanism is an additional introduction mechanism for the map update step in the basic process. The dynamic object detection and filtering mechanism process based on spatiotemporal consistency includes multi-scale motion consistency detection, dynamic probability evaluation of feature points, and dynamic map point filtering steps; the hierarchical adaptive map update mechanism process includes multi-layer map structure design, adaptive map update strategy, and map merging and optimization steps.

2. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 1, characterized in that: The multi-scale motion consistency detection step detects the motion consistency in the image through multi-scale analysis in the feature extraction stage, which helps to distinguish static background and dynamic objects and reduce the interference of dynamic objects on feature point extraction; its calculation formula is as follows: Formula 1 where F score is the multi-scale scoring result, w1, w2, w3 are weight parameters, F optical is the optical flow consistency score, F geom is the geometric consistency score, F semantic is the semantic consistency score.

3. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 1, characterized in that: The feature point dynamic probability evaluation evaluates the dynamic probability of each feature point in the feature point description stage, which can be achieved by analyzing the motion pattern of the feature point in consecutive frames. The feature point with high dynamic probability will be marked as a potential dynamic feature point; its calculation formula is as follows: Formula 2 where p motion is the dynamic probability based on motion, p semantic is a dynamic probability based on semantics, p history is the historical dynamic probability, and the dynamic probability trend is obtained from the historical frame statistics. α, β, and γ are coefficients for adjusting the influence of various factors.

4. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 3, characterized in that: After the dynamic feature points are stably detected, in the feature matching stage, the dynamic probability evaluation results are used to filter out the dynamic feature points, reduce the matching error rate, and improve the matching accuracy. In the subsequent pose estimation stage, the dynamic probability evaluation results are further used to eliminate the influence of dynamic feature points and ensure the accuracy of pose estimation.

5. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 1, characterized in that: The dynamic map point filtering has a dynamic probability threshold p threshold ; When the probability evaluation result calculated by the feature point is greater than the dynamic probability threshold, it is marked as a dynamic feature point and stored in the dynamic mask feature set; if the probability evaluation result of the feature point is lower than the dynamic probability threshold, it is marked as a static feature point and included in the static map for updating.

6. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 1, characterized in that: The multi-layer map structure design includes a global static map layer design, a local dynamic map layer design and a semantic instance map layer design, wherein the global static map layer design stores feature points of a static environment, providing a stable positioning and mapping basis for map updates; the local dynamic map layer design stores feature points of a dynamic environment, and is used to design short-term positioning and mapping for map updates; The semantic instance map layer is designed to store semantic information, including object categories and object locations, to provide a richer description of the environment for map updates.

7. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 1, characterized in that: The adaptive map update strategy adaptively updates maps of different levels according to the dynamics of the environment. When a dynamic object is detected, the local dynamic map layer is updated, while the global static map layer remains unchanged. The strategy formula is as follows: Formula 3 where t static is the static duration of the feature point, n observe is the number of feature point observations, q measure is the feature point measurement performance.

8. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 1, characterized in that: The map merging and optimization described above is to use a multi-layer map structure to perform loop detection during the loop detection stage; after the loop is detected, the map is merged and optimized to ensure global consistency; the feature points in the local dynamic map layer can be matched and optimized with the feature points in the global static map layer to improve the overall accuracy of the map; The formula is as follows: Equation 4 Where m final is the final map, m static It is a static map. dynamic It is a dynamic map. semantic It is a semantic map.

9. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 7, characterized in that: The number of feature point observations shall not be less than the minimum number of observations of 3 times.

10. The method for visual SLAM positioning and mapping in a dynamic environment according to claim 5, characterized in that: The dynamic probability threshold is set to 0.6, or an empirical value obtained through actual testing.

Images