Dynamic vision SLAM method based on extended Bayesian model

Abstract

A dynamic vision SLAM (Simultaneous Localization and Mapping) method based on an extended Bayesian model belongs to the technical field of autonomous robot navigation and computer vision crossing, and mainly comprises the following steps: performing prior dynamic object recognition on a current frame image by using an improved PWt-YOLO network, and outputting a binary mask and semantic probability distribution; oRB feature extraction is carried out by using a hierarchical quadtree algorithm and combining an adaptive threshold value; establishing an extended Bayesian probability model, fusing semantic prior, optical flow residual and epipolar geometric constraints of two-dimensional Gaussian distribution constructed based on dynamic object boundaries, realizing time sequence transmission of dynamic feature probabilities through a Markov chain, and executing feature point filtering according to joint dynamic probabilities; and robust pose estimation is realized through RANSAC-PnP, a sliding window and the like. According to the method, the problem of SLAM system positioning drift in a dynamic environment can be effectively solved by constructing a multi-modal fusion dynamic feature discrimination system.

Description

Technical Field

[0001] The present invention relates to the technical field of intersection of autonomous robot navigation and computer vision, and in particular to a dynamic visual SLAM (Simultaneous Localization and Mapping) method based on multimodal probability fusion. Background Art

[0002] Visual Simultaneous Localization and Mapping (SLAM) technology is the core foundation for autonomous robot navigation. It uses sensors such as cameras and IMUs to estimate the robot's motion in real time and build a map of the environment. As a key area in robotics, visual SLAM has a wide range of applications in real-world production, such as autonomous driving, smart homes, and industrial automation.

[0003] Traditional visual SLAM methods typically assume a static environment and fail to account for dynamic objects within it. However, in real-world scenarios, dynamic objects are often unavoidable (e.g., pedestrians, vehicles, and other moving objects). Feature points extracted from these dynamic objects can lead to cumulative pose estimation errors, which can even cause tracking failures and unreliable map structures. With the continuous advancement of robotics, robots must be able to accurately locate and move autonomously in complex dynamic scenes.

[0004] At present, commonly used dynamic visual SLAM methods are divided into two categories. One is a geometric constraint method based on multi-view geometry or optical flow consistency, but this method relies on the continuity assumption of motion and has difficulty in handling sudden dynamic interference; the other is a deep learning-based method that directly identifies dynamic areas by using semantic segmentation networks or target detection networks, but the existing models have high computational complexity and are unable to handle potential dynamic objects. Summary of the Invention

[0005] To solve the problems of inaccurate pose estimation and large predicted trajectory errors in existing visual SLAM in dynamic environments, this paper proposes a dynamic visual SLAM algorithm based on an extended Bayesian model that integrates multimodal perception, lightweight computing and probabilistic reasoning, to achieve positioning and mapping in complex dynamic environments, and promote the practical application of robot autonomous navigation technology.

[0006] This method constructs a multimodal fusion dynamic feature discrimination system, including: generating dynamic object semantic masks using an improved PWt-YOLO network. Based on YOLOv7-tiny, this network proposes a PWt_Block module that fuses partial convolution and wavelet transforms, integrating the WIoUv3 loss function, for real-time dynamic target extraction; designing a hierarchical quadtree ORB feature extraction algorithm with adaptive thresholding to achieve multi-scale uniform distribution; establishing an extended Bayesian probability model that integrates a semantic prior based on a two-dimensional Gaussian distribution constructed based on dynamic object boundaries, optical flow residuals, and epipolar geometry constraints; implementing temporal propagation of dynamic feature probabilities through a Markov chain, and performing feature point filtering based on the joint dynamic probabilities; and achieving robust pose estimation through RANSAC-PnP and sliding window algorithms. Experiments show that this method reduces the absolute trajectory error by an average of 95.8% compared to ORB-SLAM2 on the TUM RGB-D walking dynamic dataset, effectively addressing the positioning drift problem of SLAM systems in dynamic environments.

[0007] The specific steps include:

[0008] S1: Synchronously acquire depth image data through an RGB-D camera; pass the current frame RGB image captured by the RGBD camera into the improved PWt-YOLO network to obtain the target detection box information of the prior dynamic object, and fuse the depth image data to obtain the binary mask and semantic probability distribution of the prior dynamic object.

[0009] The improved PWt-YOLO network includes an improved PWt_block module and a WIoUv3 loss function. The specific construction method is as follows:

[0010] SLAM systems require real-time precise positioning and mapping. To quickly and accurately identify dynamic objects in the environment and provide binary masks for them, this paper improves the YOLOv7-tiny network structure. To address the issues of its large number of parameters and long inference time in the backbone network, a lightweight PWt_Block module is proposed to replace the ELAN-tiny module in the backbone network. The specific method is as follows:

[0011] By integrating the concepts of partial convolution and wavelet transform, the improved PWt-YOLO network is constructed by segmenting feature map channels of different resolutions. The feature maps of some channels are directly output, and the multi-frequency features of the remaining channels are extracted through wavelet transform. The two outputs are then concatenated and processed through 1x1 point-by-point convolution, batch normalization, and activation functions. This reduces redundant network parameters and enhances the ability to extract detailed features, resulting in the improved PWt-YOLO network. This design reduces the number of network parameters while improving inference efficiency and the ability to express dynamic object features.

[0012] The improved PWt-YOLO network was then trained using the COCO dataset, using the weighted intersection-over-union (WIoUv3) loss function. The WIoUv3 loss function features a dynamic non-monotonic focusing mechanism. While addressing the vanishing gradient problem of the loss function, it also assigns differentiated weights to real-world labeled data of varying quality, effectively reducing the impact of poor-quality labeled data on training results.

[0013] Furthermore, step S1 specifically includes:

[0014] S1.1, use the trained PWt-YOLO network to perform frame-by-frame detection on the image data collected by the SLAM system camera and output the detection frame coordinate information of the prior dynamic objects.

[0015] Specifically, the RGB image of the current frame is passed into the network model with a size of 480×640×3. Feature maps of different scales are extracted through the improved backbone network, and multi-scale feature fusion is performed using the neck network. Finally, the detection head is used to perform target detection on feature maps of different scales, identify dynamic objects in the image frame, and generate target detection frames.

[0016] S1.2, perform k-means clustering analysis on the depth image, set the number of clusters to 2, calculate the average depth value of the object in each detection box, and select the pixel points in the larger class as the region growing seed points.

[0017] In this step, the category information, position information and depth information of the target detection frame are fused, the point sampling method is used to sample the target detection frame coordinate area of the depth image, and the k-means method is used to cluster the sampling points to obtain the foreground depth information of the dynamic object. Specifically:

[0018] Taking the center of the prediction box as the origin, we sample k pixels in the horizontal and vertical directions until we stop beyond the edge of the prediction box. Then we can get the sampling depth value point set P of each prediction box, where p = {x, y, d}∈P. We cluster the point set P and divide it into two clusters: the foreground object point cluster P obj and background point cluster P scene , where P obj Inclusion ratio P scene For more points, for the sampling depth value point set P, set its objective function as:

[0019]

[0020] Where K is the number of clusters; P i is the point set of the i-th cluster, d is the depth value of each point p, μ i is the centroid of the ith cluster, ||d-μ i|| 2 Represents the depth d of point p and the cluster centroid μ i The square of the Euclidean distance between them. i The calculation formula is:

[0021]

[0022] S1.3, based on the depth of the target detection frame and the average depth of the object, calculate the maximum depth value of the four corner points of the detection frame, set a dynamic depth threshold, and trigger the region growth condition when the pixel point meets the depth threshold.

[0023] Specifically, suppose the depths of the four corner points of the K-th target detection frame in the image are The maximum depth of the corner point and the average depth of the target object Defined as:

[0024]

[0025]

[0026] Thus, the dynamic depth threshold of the K-th target detection box is obtained The formula is:

[0027]

[0028] Among them, ε1 and ε2 are scale coefficients; δ is the preset object size, that is, the depth range occupied by the dynamic object in the target detection frame.

[0029] S1.4, generates an accurate object mask based on the eight-neighborhood region growing algorithm, encodes the mask and detection box coordinate information into a binary matrix for storage, and maintains a 1:1 mapping relationship between the matrix row and column resolution and the RGB image.

[0030] Specifically, based on the dynamic depth threshold The eight-neighborhood region growing algorithm is used to segment the depth image. The growing rule is defined as: if the current pixel depth If the neighborhood satisfies the continuity constraint, it is marked as a dynamic area and a binary mask of the dynamic object is generated.

[0031] S1.5: Generate semantic Gaussian distribution based on the results of the dynamic object recognition algorithm and calculate semantic prior.

[0032] In order to solve the boundary fuzziness problem caused by holes and noise in the depth image of the binary mask, semantic probability is introduced into the bounding box to reduce the impact of inaccurate segmentation. This paper uses a two-dimensional Gaussian model to establish a semantic probability model for the target detection box generated by PWt-YOLO. For the target detection box, let σ semanticRepresents the standard deviation of the detection box and establishes a two-dimensional semantic probability Gaussian distribution P(O semantic ):

[0033]

[0034] S2 performs hierarchical gridding on the RGB image, divides the image into adjustable grid cells, performs grayscale value distribution statistics based on local variance analysis, optimizes feature distribution density with a quadtree structure, and extracts a spatially uniform ORB feature point set through an adaptive threshold.

[0035] In this step, a set of ORB feature points with uniform spatial distribution and robust scale is constructed through hierarchical grid analysis and quadtree distribution optimization. The specific process is as follows:

[0036] S2.1, for the input image, build an image pyramid and allocate the number of feature points to be extracted in layers.

[0037] Specifically, according to the preset total number of ORB feature points, an RGB image pyramid is constructed, and the resolution decreases layer by layer.

[0038] S2.2, divide each layer of the image into a grid of fixed size, calculate the mean and standard deviation of the pixel gradients in the grid, and set the adaptive threshold.

[0039] Specifically, let the size of each layer image be (h, w), divide it into a fixed-size grid of k×k, set the average grayscale value in the jth grid of the i-th layer image, and calculate the adaptive threshold T in each layer image grid i as follows:

[0040]

[0041] Among them, α is the proportional coefficient, the value range is (0,1), S j is the jth grid area, I(x,y) is the grayscale value of the selected pixel, and the threshold dynamically adapts to the local grayscale distribution to avoid the problem of feature points being too sparse or too dense due to a fixed threshold.

[0042] S2.3 uses the dynamic FAST corner detector and uses an adaptive threshold to perform the grayscale centroid method on the detected corners to calculate the rotation angle and scale information of the corners.

[0043] Specifically, the adaptive threshold in step S2.2 is used to extract FAST feature points in each grid, and the rotation angle and scale information of the feature points are calculated to ensure that the feature points have rotation invariance and scale consistency.

[0044] S2.4: Use the quadtree algorithm to optimize the spatial distribution of feature points.

[0045] In this step, for the feature points extracted in step S2.3, a quadtree is constructed in each layer of the image pyramid to achieve uniform distribution of the feature points. Specifically,

[0046] S2.4.1: Recursively divide the current layer image into quadtree sub-regions until the number of sub-regions reaches a preset threshold;

[0047] S2.4.2: In each sub-region, retain the feature points with the highest response value and eliminate redundant points to achieve a uniform spatial distribution of feature points.

[0048] S2.5, generate the improved rBRIEF descriptor with a descriptor dimension of 128 bits, and apply bilinear interpolation to maintain rotation invariance during calculation.

[0049] Specifically, for the evenly distributed ORB feature points, a fixed seed is used to calculate the binary descriptor to ensure the repeatability of feature point extraction and matching.

[0050] S3: Use the pyramid LK optical flow method to track the motion trajectory of feature points, combine the epipolar geometry constraints to calculate the reprojection residual, and analyze the dynamic observation probability of feature points.

[0051] In this disclosure, a method for determining the probability of dynamic feature points based on optical flow residuals and epipolar geometry constraints is further proposed. Specifically, the current frame RGB image captured by the camera is grayscale processed, and the obtained grayscale image is used to extract evenly distributed ORB feature points using the adaptive threshold quadtree method in step S2. The feature points of the current frame are matched with the feature points of the previous frame, and the pyramid LK optical flow method is combined with the epipolar geometry residual to analyze the dynamic observation probability of the feature points.

[0052] Furthermore, the specific process is as follows:

[0053] S3.1: Use pyramid sparse optical flow to match feature points of two consecutive frames.

[0054] After obtaining the optical flow calculated at time t and t+1, define the optical flow residual O flow for:

[0055] O flow =||v obs -v pred ||2

[0056] Where v obs The displacement vector of the feature point tracked by the pyramid optical flow method, v pred is the theoretical displacement vector estimated by the camera motion model, ||v obs -v pred||2 is the Euclidean distance between the actual observed displacement and the model-predicted displacement. Due to different motion characteristics, the residuals of static feature points and dynamic feature points follow different probability distributions. The optical flow residuals of static feature points are mainly caused by sensor noise, feature matching errors, and camera motion estimation errors. In this disclosure, it is assumed to follow a zero-mean Gaussian distribution:

[0057]

[0058] Where S represents the static feature point hypothesis, P(O flow |S) represents the residual probability distribution of optical flow of static feature points, σ flow represents the standard deviation of the optical flow residual under the static assumption.

[0059] The optical flow residual of dynamic feature points mainly comes from their own motion and shows obvious discrete characteristics. Due to the unpredictability of their motion state, this paper assumes that the optical flow residual of dynamic feature points conforms to a wide Gaussian distribution with zero mean:

[0060]

[0061] Where D represents the dynamic feature point hypothesis, P(O flow |D) represents the residual probability distribution of optical flow of dynamic feature points, σ flow,d is the residual standard deviation of the dynamic point.

[0062] S3.2: Use epipolar geometry constraints to obtain the distance between the matching feature points and the epipolar line.

[0063] For the normalized pixel coordinates x1 and x2 of the feature points in the previous and next frames, define the epipolar line l2 from x1 to x2 = [A, B, C] T The distance d is:

[0064]

[0065] Consider d as the epipolar geometric error O epi , then:

[0066]

[0067] This can characterize the degree of deviation of feature point matching from geometric constraints, in pixels. If the epipolar geometric error under the static feature point assumption conforms to the Gaussian distribution and the epipolar geometric error under the dynamic feature point assumption conforms to the wide Gaussian distribution, we can obtain:

[0068]

[0069] Where S and D represent the static and dynamic assumptions respectively, P(O epi|S) represents the probability distribution of epipolar geometric errors of static feature points, σ epi represents the standard deviation of epipolar geometry error under static assumption, P(O epi,d |D) represents the probability distribution of epipolar geometric errors of dynamic feature points, σ epi,d represents the standard deviation of epipolar geometry error under the dynamic assumption, σ epi,d >>σ epi .

[0070] S4: The temporal prior is transferred through the Markov chain, and an extended Bayesian model is constructed by combining semantic, optical flow and geometric observations. The posterior probability of the dynamic / static state of the feature points is calculated using the Bayesian theorem.

[0071] This paper further proposes an online calculation method for dynamic feature point probabilities based on an extended Bayesian model. By integrating temporal priors, semantic probabilities, and geometric multi-observation constraints, it achieves robust determination of dynamic / static states. The specific steps are as follows:

[0072] S4.1: Construct the temporal transfer dynamic probability P of the feature points of the current frame based on the Markov chain m (·|·),

[0073] Specifically: the current time prior state K of the feature point t (dynamic / static) depends only on the actual state K at the previous moment t-1 ,Right now:

[0074] P m (K t |K t-1 ,K t-2 ,...,K0)=P m (K t |K t-1 )

[0075] Assume p d2d and p s2s They represent the preset dynamic-to-dynamic transition probability and static-to-static transition probability, S t and D t Represent the static and dynamic states at time t, S t-1 and D t-1 Representing the static and dynamic states at time t-1 respectively, the state transition probability P from the previous frame to the current frame can be defined m for:

[0076]

[0077] The four formulas in the above formula respectively represent the time prior probabilities of the feature point transferring from dynamic to dynamic, dynamic to static, static to static, and static to dynamic from time t-1 to time t.

[0078] S4.2, integrates semantic prior, temporal prior of the previous frame, optical flow residual observation and epipolar geometric error observation to generate an extended Bayesian model and calculate the dynamic probability of feature points.

[0079] In this step, an extended Bayesian model is constructed by fusing multi-source information. Specifically:

[0080] Combining the temporal prior, the semantic prior probability in S1, the optical flow residual observation likelihood in S3.1, and the epipolar geometric error observation likelihood in S3.2, we can obtain the extended Bayesian probability model:

[0081]

[0082] Where α and β are weight variables, the dynamic probability results based on the Bayesian model can be obtained as follows:

[0083]

[0084] S4.3: Determine the dynamic and static states of the feature points based on the threshold.

[0085] Specifically, for the probability P(D t =1|·), set τ as the dynamic probability threshold, if P(D t =1|·)>τ, it is a dynamic point, otherwise it is a static point. After calculating the dynamic probability of the feature point, the state K of the current feature point is assigned t , only static feature points are used for SLAM pose estimation and map construction.

[0086] The filtered static feature points are input into the tracking thread, local mapping thread and loop detection thread of the SLAM system to perform subsequent pose optimization, local map update and global consistency maintenance.

[0087] S5, for static feature points, the RANSAC-PnP algorithm is used to execute the tracking thread, and the bag-of-words model is used to calculate the image similarity and filter the key frames.

[0088] In this step, the SLAM backend processing flow is further adopted to achieve high-precision positioning and mapping in dynamic environments through robust pose estimation, keyframe management, and map optimization. The specific steps are as follows:

[0089] S5.1, pose estimation: After obtaining the matching static feature points of two adjacent frames, the camera pose is estimated according to the PnP algorithm. The RANSAC algorithm is used to remove incorrectly matched feature points. When optimizing the reprojection error of spatial points and pixel points, only the static feature points are optimized. This makes the camera unaffected by the movement of dynamic objects and obtains a robust camera trajectory.

[0090] Specifically, a brute force matching algorithm is used to associate the static ORB feature points of adjacent frames; the camera pose is estimated based on the PnP algorithm, and the RANSAC mechanism is introduced to dynamically eliminate mismatched points to improve the robustness of pose estimation.

[0091] S5.2, tracking local map: By optimizing the reprojection error, the local map association update is established, combining the information of the current frame with the features in the local map, so as to achieve fast response to camera movement.

[0092] Specifically, S5.1 is used to obtain the pose trajectory under static conditions, triangulation operations are performed on the matched feature point pairs, the three-dimensional coordinates of the map points are restored, and the pose information is further optimized through relative motion constraints between key frames.

[0093] In this step, the tracking state is managed by a state machine; the state machine has four states: initialization, tracking, loss, and relocalization, which include: (1) initialization to build the initial map and pose; (2) tracking to continuously update the pose based on static feature points; (3) loss will trigger the relocalization mechanism and restore the pose by matching historical key frames through the bag-of-words model (BoW); (4) relocalization uses the map key frame features to quickly regain the camera position.

[0094] S5.3, key frame screening: Select images that can provide new feature information or have distinguishing characteristics as key frames, and screen and match feature points based on the bag-of-words model to improve matching accuracy and efficiency.

[0095] Specifically, frames containing new features or significant scene changes are selected as key frames; key frame features are quickly matched based on BoW to improve screening efficiency; at the same time, redundant key frames are eliminated through common view relationship and information entropy analysis to reduce computational overhead.

[0096] S6 constructs a sliding window, performs local bundle adjustment to optimize the camera pose and map point 3D coordinates, performs loop detection based on similarity measurement, and eliminates accumulated errors through global pose graph optimization.

[0097] In this disclosure, after the tracking thread is completed, the system continues to execute the local mapping thread and loop detection thread to complete the system's precise positioning and map construction. Specifically, it includes:

[0098] S6.1 Local mapping thread: performs sliding window bundle adjustment to optimize the camera pose and map point 3D coordinates.

[0099] The local mapping thread dynamically constructs a sliding window based on the principle of spatiotemporal proximity, selects the common view key frame of the current frame and the multiple frames closest to the time series to form a local optimization window, prioritizes image frames that can introduce new scene information or have significant discrimination as key frames, and uses BoW to efficiently match and filter feature points, thereby improving the accuracy and computational efficiency of feature association.

[0100] In the selected sliding window, a BA (Bundle Adjustment) model with reprojection error as the target is constructed. The poses of all key frames and map point coordinates in the window are jointly optimized. By minimizing the reprojection objective function, the optimal poses of the key frames and the optimal positions of the map points in the window are obtained.

[0101] On this basis, the thread performs continuous map maintenance, including removing redundant map points within the sliding window whose observation counts fall below a threshold or whose projection errors are excessively large, and removing historical keyframes within the window with low relevance to the current scene. This controls the map's size and maintains its geometric consistency. Furthermore, a prediction-feedback control mechanism dynamically adjusts the map update frequency to adapt to dynamic scene changes and environmental uncertainties in real time.

[0102] After each sliding window BA, the thread synchronizes the optimized pose and map points to the global map to ensure global consistency of positioning and mapping.

[0103] S6.2 Loop closure detection thread: Implement loop closure verification based on similarity measurement.

[0104] The loop detection thread uses the bag-of-words model to retrieve historical key frames that are visually similar to the current frame as candidate loops, and uses the RANSAC-based essential matrix geometry verification method to confirm the reliability of the loop relationship.

[0105] S6.3 eliminates accumulated errors through global pose graph optimization.

[0106] If the verification of step S6.2 is passed, the system starts the pose graph optimization process, uses the pose graph optimizer represented by the Lie group, and globally corrects the cumulative pose error through the graph optimization method. Its objective function is to minimize the pose difference before and after loop closure, and the optimized residual function includes the inertial navigation data constraint term.

[0107] Finally, the verified loop keyframes are fused with the current map to eliminate scale drift and path deviation, completing the closed-loop correction.

[0108] This paper designs a visual SLAM system based on an extended Bayesian model and implements innovative improvements to multiple modules to achieve robot positioning and mapping in highly dynamic environments. Compared with the existing technology, the benefits of this paper are:

[0109] 1) Based on the lightweight PWt-YOLO target detection network, the PWt_block module is proposed by integrating partial convolution and wavelet transform ideas, and WIoUv3 is used as the loss function for network training, which improves the speed of dynamic object detection while reducing the number of model parameters; combined with deep clustering and region growing algorithms, high-precision dynamic object masks are generated to achieve fast segmentation of dynamic areas; 2) An ORB feature extraction method based on hierarchical grid grayscale analysis and quadtree distribution optimization is proposed, and the distribution of feature points is made more uniform and stable by adapting different texture areas through dynamic thresholds; 3) An extended Bayesian model is constructed, which integrates temporal Markov priors, semantic probability, optical flow residuals and epipolar geometry constraints to achieve online probabilistic discrimination of dynamic / static features; 4) The proposed lightweight PWt-YOLO target detection network has an 8.2% decrease in parameters compared to YOLOv7-tiny, a 12.3% increase in inference speed, lower requirements on hardware equipment, and higher real-time performance of the algorithm; the proposed dynamic visual SLAM algorithm is used in TUM The absolute trajectory error of the high-dynamic walking sequence in the RGBD-Dynamic dataset is reduced by more than 92% compared to ORB-SLAM2, and the pose estimation is more accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0110] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

[0111] Figure 1 is an exemplary dynamic visual SLAM system structure and flow chart according to the present disclosure;

[0112] Figure 2 Flowchart of the algorithm for dynamic object mask generation based on semantic and depth information

[0113] Figure 3 is a diagram of an exemplary PWt-YOLO network structure according to the present disclosure;

[0114] Figure 4 is a structural diagram of an exemplary PWt_Block module according to the present disclosure;

[0115] Figure 5 is a flowchart of an exemplary dynamic point filtering module execution according to the present disclosure;

[0116] Figure 6 Trajectory and error map estimated using ORB-SLAM2 for the walking sequence;

[0117] Figure 7 Trajectory and error plots using the method described in this disclosure for a walking sequence. DETAILED DESCRIPTION

[0118] The preferred embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although preferred embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided to make the present disclosure more thorough and complete, and to fully convey the scope of the present disclosure to those skilled in the art.

[0119] The present disclosure provides a dynamic SLAM solution that integrates multimodal perception, lightweight computing, and probabilistic reasoning to break through the bottleneck of existing technologies, promote the practical application of robot autonomous navigation technology, and expand robot application scenarios.

[0120] The flowchart of the exemplary embodiment according to the present disclosure is as shown in the attached figure. Figure 1 As shown, it mainly includes the following steps:

[0121] S1, synchronously acquires depth image data through an RGB-D camera, uses an improved PWt-YOLO network to identify a priori dynamic objects, and outputs a binary mask and a semantic probability distribution map. The network includes an improved PWt_block module and a WIoUv3 loss function;

[0122] S2, for the input image, constructs an image pyramid and hierarchically allocates the number of feature points to be extracted, performs hierarchical gridding on the RGB image, divides the image into adjustable grid cells, performs grayscale value distribution statistics based on local variance analysis, optimizes feature distribution density with a quadtree structure, and extracts a spatially uniform ORB feature point set through adaptive thresholding;

[0123] S3, construct an image pyramid, use the LK optical flow method to track the motion trajectory of feature points, calculate the reprojection residuals combined with epipolar geometry constraints, and analyze the dynamic observation probability of feature points;

[0124] S4, through the Markov chain to transfer the time prior, combined with semantic, optical flow and geometric observations to build an extended Bayesian model, and calculate the dynamic / static state posterior probability of the feature points through the Bayesian theorem;

[0125] S5, filter static feature points, use RANSAC-PnP algorithm to execute tracking thread, and simultaneously calculate image similarity through bag-of-words model to filter key frames;

[0126] S6 constructs a sliding window, performs local bundle adjustment to optimize the camera pose and map point 3D coordinates, performs loop detection based on similarity measurement, and eliminates accumulated errors through global pose graph optimization.

[0127] Application Examples

[0128] Experimental platform: The experimental running environment is Ubuntu 18.04 operating system, the CPU is Intel i7-13400, the GPU is NVIDIA GeForce RTX 4080, and the Cuda version is 11.3 for evaluation.

[0129] The overall steps are as follows Figure 1 As shown:

[0130] S101, Semantic and temporal dynamic prior: Using the improved PWt-YOLO network, the dynamic object recognition algorithm is used to detect and segment RGB image frames, separate the prior dynamic objects and static scenes, and obtain the binary mask map and semantic probability of the prior dynamic objects. Figure 2 .

[0131] (1) This embodiment uses the PWt-YOLO network to detect targets on input image frames. The network structure is shown in Figure 3 and Figure 4 The core improvement is the design of the PWt_Block module, including:

[0132] ① The idea of partial convolution is used to divide the input feature map into two parts along the channel dimension, where 75% of the channels directly retain the original features through identity mapping (Identify) to reduce computational redundancy.

[0133] ② Using the idea of wavelet transform, Haar wavelet transform is performed on the remaining 25% of the channels, decomposing them into high-frequency and low-frequency horizontal and vertical features, thereby enhancing the ability to capture the edges and textures of dynamic objects. At the same time, the receptive field is expanded through multi-level wavelet transform, thereby improving the perception of a wide range of features with a small increase in parameters.

[0134] ③Using common dynamic object categories in the COCO dataset as training data, the WIoUv3 loss function is adopted, and the weight of low-quality annotation boxes is reduced through the dynamic focusing mechanism. The training cycle is 100 epochs, the initial learning rate is 0.001, and the batch size is 16. After the input image frame passes through the network, the coordinate information, category and confidence of the target detection box where the dynamic object is located will be obtained.

[0135] (2) In this embodiment, the target detection frame is divided into 9 equal points for depth sampling, and the k-means clustering method is used to iterate the sampling points multiple times to obtain the optimal foreground object depth point set;

[0136] (3) The dynamic depth threshold is then calculated based on the maximum depth of the detection frame corner points and the average depth of the foreground points, thereby avoiding the regional confusion problem caused by unstable boundary depth information while considering the depth of the object body and effectively suppressing background noise.

[0137] (4) Based on the dynamic threshold, the growth rule is generated and the eight-neighborhood region growing algorithm is used to obtain the binary mask of the dynamic object. Then, a 3×3 core binary image expansion process is used to solve the image noise and hole problems.

[0138] In this embodiment, the two-dimensional Gaussian model parameter σ is set x =w / 4,σ y =h / 4, where w and h are the width and height of the detection box respectively, and the semantic probability of dynamic objects in target detection is generated.

[0139] S102 , extracting ORB feature points from the input RGB image frame to obtain a required number of evenly distributed ORB feature point sets.

[0140] To adapt to multi-scale scene features, this embodiment sets the number of image pyramid layers to 8, the scaling factor to 1 / 1.2, and the bottom layer resolution to 640×480, thereby constructing an image pyramid. The scale of the ORB feature points in each pyramid layer is proportional to the number of layers it is in, ensuring scale invariance of feature matching.

[0141] In this embodiment, each layer of the image is divided into a 30×30 grid, the grayscale average and adaptive threshold are automatically calculated in each grid, the FAST corner points are extracted in each grid, and their direction (based on the grayscale centroid method) and scale (corresponding to the number of pyramid layers) are calculated.

[0142] This embodiment uses a quadtree to solve the problem of feature point aggregation. Specifically, starting from the entire layer image, it is recursively divided into four sub-regions until the sub-region size is less than or equal to 30×30 pixels; in each sub-region, only the feature point with the highest response value is retained, and the remaining points are discarded; in each subdivided region, only the feature point with the highest response value is retained, thereby obtaining a uniformly distributed ORB feature point set.

[0143] S103, Geometry-based dynamic observation:

[0144] This embodiment performs optical flow matching and epipolar geometry constraint verification on the feature points of the current frame, respectively, and establishes an observation likelihood function of the optical flow residual and the epipolar geometry error.

[0145] S104, perform dynamic probability estimation on the ORB feature point set of the current frame and separate static feature points from dynamic feature points. Figure 5 shown.

[0146] This embodiment constructs an extended Bayesian model, in which: the state transition probability p is constructed by the Markov chain d2d =0.9,p s2s=0.95; the semantic prior, optical flow residual and epipolar geometric error are integrated to calculate the dynamic probability; the dynamic depth threshold τ = 0.75 is set, and the point is determined to be dynamic if the probability is higher than the threshold.

[0147] S105: Input the static feature points into the ORB-SLAM2 network for subsequent pose estimation, specifically:

[0148] A brute force matching algorithm is used to associate static feature points in adjacent frames, and RANSAC is used to remove mismatched feature points. The PnP algorithm is used to solve the camera pose. The rotation matrix R and translation vector t of the current frame relative to the reference frame are obtained, and iterative optimization is performed until the reprojection error converges.

[0149] S106, local mapping, performing sliding window bundle adjustment optimization:

[0150] The current frame and the first N key frames with the strongest co-viewing relationship are selected to form the initial optimized sliding window, and a sparse graph model containing the key frame poses and associated map points in all windows is constructed;

[0151] Perform quality maintenance on map points within the window, merge redundant points and remove invalid points;

[0152] Construct a reprojection error function for the keyframes in the sliding window, use the g2o library to execute the LM algorithm, and iteratively obtain the optimal keyframe pose and map point position;

[0153] By judging the degree of common view, new keyframes are inserted and old keyframes are removed. The keyframes of a removed window are subjected to Schur marginalization, and their information is converted into constraints on the remaining variables to avoid the destruction of optimization sparsity.

[0154] Update local map.

[0155] S107, loop detection and global optimization:

[0156] The DBoW2 library is used to retrieve candidate keyframes with a similarity greater than 0.8. The essential matrix is calculated through RANSAC, with a reprojection error threshold of 1.5 pixels and a minimum number of inliers of 50. A loop constraint is added to the pose graph, and the nodes of the pose graph are set as keyframe poses, and the edges of the pose graph are set as relative pose observations. The pose graph is then optimized to obtain a global trajectory, thereby improving the accuracy of positioning and mapping.

[0157] This embodiment uses an improved PWt-YOLO network to generate semantic masks of dynamic objects. Based on YOLOv7-tiny, this network proposes a PWt_Block module that fuses partial convolution and wavelet transform ideas, integrates the WIoUv3 loss function, and extracts dynamic targets in real time. A hierarchical quadtree ORB feature extraction algorithm is designed, combined with an adaptive threshold to achieve multi-scale uniform distribution. An extended Bayesian probability model is established, integrating semantic priors based on a two-dimensional Gaussian distribution constructed based on the boundaries of dynamic objects, optical flow residuals, and epipolar geometry constraints. The temporal transmission of dynamic feature probabilities is achieved through a Markov chain, and feature point filtering is performed based on the joint dynamic probability. Robust pose estimation is achieved through RANSAC-PnP and sliding windows.

[0158] This example uses two indicators, absolute trajectory error (ATE) and relative pose error (RPE), for evaluation, and also gives the performance improvement of this example method over ORB-SLAM2.

[0159] The comparison between this embodiment and ORB-SLAM2 is shown in the following table:

[0160] Table 1 ATE comparison between this embodiment and ORB-SLAM2 (m)

[0161]

[0162] Table 2 RPE comparison between this embodiment and ORB-SLAM2 (m)

[0163]

[0164]

[0165] Experimental results show that in the high-dynamic scene of the walking sequence, the algorithm of this embodiment has been significantly improved compared with ORB-SLAM2, and the ATE accuracy has been improved by more than 92%. This is because the algorithm of this embodiment can effectively identify dynamic feature points in the environment and filter them, thereby ensuring the use of stable static feature points to participate in the subsequent positioning and mapping of SLAM, thereby improving the robustness of SLAM in dynamic environments.

[0166] In the low-dynamic scene of the sitting sequence, the ATE and RPE accuracy of the sitting_halfsphere and sitting_static sequences are higher than ORB-SLAM2, while the accuracy of the sitting_rpy and sitting_xyz sequences decreases. This is mainly because the semantic mask used in this embodiment directly regards the person in the sitting sequence as a dynamic object, filtering out most of the dynamic points on their body, resulting in the loss of some static feature points that can be used for pose estimation and optimization, thereby affecting the positioning accuracy of the system. Especially in the stting_rpy sequence, while the person maintains low-dynamic motion, the camera itself moves more violently, occasionally moving to weak texture areas, resulting in a lack of sufficient static feature points to participate in subsequent processes, causing a decrease in accuracy.

[0167] Figure 6 and Figure 7 The trajectories and error diagrams of ORB-SLAM2 and the method described in this disclosure under the high-dynamic sequence fr3_walking_xyz are shown in the figure. (a) to (d) are different sequences in walking. The black dotted line in the left figure of each sub-figure represents the true trajectory, and the light solid line represents the trajectory estimated by different algorithms (ORB-SLAM2, the method described in this disclosure). The higher the degree of overlap between the light solid line and the black dotted line, the smaller the error of the estimated trajectory relative to the true trajectory. The right figure shows the RPE error. Comparison Figure 6 and Figure 7 As can be seen from the left figure in sub-figures (a) to (d) in the figure, the trajectory estimated by the method described in the present disclosure is closer to the actual trajectory of the black dotted line than the light solid line of ORB-SLAM2. Figure 6 The right image of (a)(b)(d) shows that the RPE error of ORB-SLAM2 exceeds 1.0 meters. Figure 6 In the right figure of (c), the RPE error of ORB-SLAM2 is also close to 0.8 meters. Figure 7 As can be seen from the right figures of (a)(c)(d), the RPE error of the method described in this disclosure is controlled within 0.08 meters. Figure 7 (b) The RPE error extreme value shown in the right figure is also less than 0.16 meters. Figure 6 and Figure 7 From the comparison, it can be seen that the trajectory error of the method proposed in this disclosure is significantly reduced compared with ORB SLAM2.

[0168] It can be seen that the absolute trajectory error of this method on the TUM RGB-D walking dynamic dataset is reduced by 95.8% on average compared with ORB-SLAM2, effectively solving the positioning drift problem of the SLAM system in dynamic environments.

[0169] The above technical solutions are only exemplary embodiments of the present invention. For those skilled in the art, it is easy to make various types of improvements or modifications based on the application methods and principles disclosed in the present invention, and are not limited to the methods described in the above specific embodiments of the present invention. Therefore, the methods described above are only preferred and do not have a restrictive meaning.

Claims

1. A dynamic visual SLAM method based on an extended Bayesian model, comprising the following steps: S0, obtain depth image data; S1, passes the current frame RGB image into the improved PWt-YOLO network, identifies the prior dynamic objects, and outputs the binary mask and semantic probability distribution; S2, for the input image, constructs an image pyramid and hierarchically allocates the number of feature points to be extracted, performs hierarchical gridding on the image, divides the image into adjustable grid cells, performs grayscale value distribution statistics based on local variance analysis, optimizes feature distribution density with a quadtree structure, and extracts a spatially uniform ORB feature point set based on an adaptive threshold; S3, uses the pyramid LK optical flow method to track the motion trajectory of the feature points, combines the epipolar geometric constraints to calculate the reprojection residual, and calculates the geometric observation likelihood function of the feature points; S4, through the Markov chain to transfer the time prior, combined with semantic, optical flow and geometric observations to build an extended Bayesian model, and calculate the dynamic / static state posterior probability of the feature points through the Bayesian theorem; S5, screening static feature points: RANSAC-PnP algorithm is used to execute the tracking thread, and the bag-of-words model is used to calculate the image similarity and screen the key frames; S6 constructs a sliding window, performs local bundle adjustment to optimize the camera pose and map point 3D coordinates, performs loop detection based on similarity measurement, and eliminates accumulated errors through global pose graph optimization.

2. The method according to claim 1, characterized in that In step S1, the improved PWt-YOLO network includes an improved PWt_block module and a WIoUv3 loss function, and the specific construction method includes: The basic network adopts the YOLOv7-tiny architecture, the input resolution is set to 640×640, and the output feature map scale is [20×20, 40×40, 80×80]; Design an image feature extraction module PWt_Block composed of a structure including partial convolution layer and wavelet convolution layer, which is composed of a serialized layer structure; Replace the ELAN-tiny module of the YOLOv7-tiny backbone network with PWt_Block to enhance multi-scale feature extraction; The weighted intersection-over-union loss function WIoUv3 is used for training.

3. The method according to claim 1 or 2, characterized in that The step S1 specifically includes: S1.2, using the improved PWt-YOLO network to detect RGB images frame by frame and output the detection box coordinate information of the prior dynamic objects; S1.2, perform k-means clustering analysis on the depth image, set the number of clusters to 2, calculate the average depth value of the object in each detection box, and select the pixel points in the larger cluster as the region growing seed points; S1.3 calculates the maximum depth value of the four corner points of the detection frame, sets an adaptive depth threshold, and triggers the region growth condition when the pixel point meets the depth threshold; S1.4 generates an accurate object mask based on the eight-neighborhood region growing algorithm, encoding the mask and detection frame coordinate information into a binary matrix for storage. The matrix row and column resolutions maintain a 1:1 mapping relationship with the RGB image. S1.5, generate semantic Gaussian distribution based on the results of the dynamic object recognition algorithm and calculate semantic prior.

4. The method according to claim 1, wherein The step S2 specifically includes: S2.1, for the input image, construct an image pyramid and allocate the number of feature points extracted in layers; S2.2, divide each layer of the image into a grid of fixed size, calculate the mean and standard deviation of the pixel gradients within the grid, and set an adaptive threshold; S2.3 uses the dynamic FAST corner detector and uses an adaptive threshold to perform the grayscale centroid method on the detected corners to calculate the rotation angle and scale information of the corners; S2.4, using the quadtree algorithm to optimize the spatial distribution of feature points; S2.5, generate the improved rBRIEF descriptor with a descriptor dimension of 128 bits, and apply bilinear interpolation to maintain rotation invariance during calculation.

5. The method according to claim 1, wherein The step S4 specifically includes: S4.1, construct the temporal transition dynamic probability of the feature points of the current frame based on the Markov chain; S4.3 fuses semantic priors, temporal transition dynamic probability of the previous frame, optical flow residual observations, and epipolar geometric error observations to generate an extended Bayesian model and calculate the dynamic probability of feature points; S4.4 determines the dynamic and static states of the feature points based on the threshold.

6. The method according to claim 1, characterized in that Step S5 specifically includes: S5.1, pose estimation: After obtaining the matching static feature points of two adjacent frames, the camera pose is estimated using the PnP algorithm. The RANSAC algorithm is used to remove incorrectly matched feature points. When optimizing the reprojection error of spatial points and pixel points, only the static feature points are optimized. This makes the camera unaffected by the movement of dynamic objects and obtains a robust camera trajectory. S5.2, tracking local map: by optimizing the reprojection error to establish a local map association update, the information of the current frame is combined with the features in the local map to achieve fast response to camera movement; S5.3, key frame screening: Select images that can provide new feature information or have distinguishing characteristics as key frames, and screen and match feature points based on the bag-of-words model to improve matching accuracy and efficiency.

7. The method according to claim 1, characterized in that The step S6 specifically includes: S6.1, local mapping thread: performs sliding window bundle adjustment to optimize camera pose and map point 3D coordinates; S6.2, loop closure detection thread: implement loop closure verification based on similarity measurement; S6.3, eliminate accumulated errors through global pose graph optimization.

Images