A robot simultaneous localization and mapping method, system, device and medium in a dynamic environment

Abstract

The application discloses a robot simultaneous localization and mapping method, system, device and medium in a dynamic environment, and relates to the field of robot localization and map construction. The method comprises the following steps: acquiring an image sequence of the environment around the robot; extracting features from the color image, and determining the coordinate information, color information and depth information of each feature point in combination with the depth image; performing target identification on the image sequence, and determining the static targets and potential dynamic targets in the environment around the robot; removing the feature points of the potential dynamic targets whose motion trajectories do not conform to the expected motion trajectories, and obtaining the static feature points in combination with the feature points of the static targets; generating a dense point cloud map according to the depth information, coordinate information and color information of the static feature points; discretizing the dense point cloud map into a two-dimensional grid, and constructing a grid map to guide the robot to perform a navigation task. The application can guarantee the positioning accuracy of the robot and the readability of the map in a dynamic environment.

Description

Technical Field

[0001] This application relates to the field of robot localization and mapping technology, and in particular to a method, system, device and medium for robot synchronous localization and mapping in a dynamic environment. Background Technology

[0002] Mobile robots are a multidisciplinary field, primarily involving control theory, computer technology, information technology, sensor technology, and artificial intelligence. Therefore, they possess fundamental characteristics such as perception, decision-making, and execution. The emergence of mobile robots aims to assist or even replace humans in performing tasks in complex or dangerous environments, thereby improving work efficiency and quality. It is worth noting that for a robot to successfully complete its tasks, the most crucial aspects are solving the problems of real-time robot localization, map building, and path planning.

[0003] Simultaneous Localization and Mapping (SLAM) is a crucial method for robots to autonomously locate themselves and build maps of their environment. It has gradually become a key technology for robots to perform complex and advanced tasks and is widely used in fields such as autonomous driving, augmented reality, and drones. SLAM enables robots to acquire information about their surroundings in unknown environments without human assistance, relying solely on their onboard sensors to estimate their pose and incrementally build environmental maps, providing them with localization and mapping information. Furthermore, it can improve the autonomous navigation capabilities of mobile robots and their adaptability to different application environments, facilitating real-time obstacle avoidance and dynamic path planning.

[0004] However, traditional SLAM algorithms rely on static environment assumptions to ensure robustness and efficiency in localization. Real-world unstructured scenarios often contain dynamic targets such as pedestrians, vehicles, and animals. Therefore, traditional SLAM algorithms cannot guarantee localization accuracy and map readability in dynamic environments. Summary of the Invention

[0005] The purpose of this application is to provide a method, system, device and medium for robot synchronous localization and map building in dynamic environments, which can ensure the positioning accuracy of the robot and the readability of the map in dynamic environments.

[0006] To achieve the above objectives, this application provides the following solution:

[0007] Firstly, this application provides a method for simultaneous localization and mapping of a robot in a dynamic environment, including:

[0008] Acquire an image sequence of the robot's surrounding environment; the image sequence includes multiple consecutive color images and multiple consecutive depth images;

[0009] Feature extraction is performed on the color images in the image sequence to obtain multiple feature points, coordinate information and color information of each feature point, and the depth information of each feature point is determined based on the depth image in the image sequence;

[0010] Target recognition is performed on the color images in the image sequence to identify static targets and potential dynamic targets in the robot's surrounding environment;

[0011] The motion trajectory of the feature points of the potential dynamic target is determined based on the color image in the image sequence. Feature points whose motion trajectories do not conform to the expected motion trajectory are removed, and static feature points are obtained by combining them with the feature points of the static target.

[0012] A dense point cloud map is generated based on the depth, coordinate, and color information of the static feature points.

[0013] The dense point cloud map is discretized into a two-dimensional grid to construct a raster map to guide the robot in performing navigation tasks.

[0014] Secondly, this application provides a robot synchronous localization and mapping system for a dynamic environment, including:

[0015] The image acquisition module is used to acquire an image sequence of the robot's surrounding environment; the image sequence includes multiple consecutive color images and multiple consecutive depth images;

[0016] The feature extraction module is used to extract features from the color images in the image sequence, obtain multiple feature points, coordinate information and color information of each feature point, and determine the depth information of each feature point based on the depth image in the image sequence.

[0017] The target recognition module is used to perform target recognition on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment.

[0018] The feature point determination module is used to determine the motion trajectory of the feature points of the potential dynamic target based on the color image in the image sequence, remove feature points whose motion trajectory does not conform to the expected motion trajectory, and combine them with the feature points of the static target to obtain static feature points.

[0019] The point cloud map generation module is used to generate a dense point cloud map based on the depth information, coordinate information and color information of the static feature points.

[0020] The grid map construction module is used to discretize the dense point cloud map into a two-dimensional grid and construct a grid map to guide the robot in performing navigation tasks.

[0021] Thirdly, this application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the above-described method for robot synchronous localization and map building in a dynamic environment.

[0022] Fourthly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method for robot synchronous localization and map building in a dynamic environment.

[0023] According to the specific embodiments provided in this application, this application has the following technical effects:

[0024] This application provides a method, system, device, and medium for robot synchronous localization and mapping in dynamic environments. By performing target recognition on color images in an image sequence, static targets and potential dynamic targets in the robot's surrounding environment are identified. Furthermore, feature points of potential dynamic targets whose trajectories do not conform to the expected trajectories are eliminated, and static feature points are obtained by combining these with the feature points of static targets. This reduces the impact of dynamic targets on localization accuracy and stability, improving the robot's performance in dynamic environments. A dense point cloud map is generated based on the depth, coordinate, and color information of the static feature points. This dense point cloud map is then discretized into a two-dimensional grid to construct a raster map. The rasterization technique preserves environmental information while providing a clear representation of occupancy status, improving map readability and thus enhancing the robot's localization accuracy, stability, and mapping performance in dynamic environments. Attached Figure Description

[0025] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0026] Figure 1 A flowchart illustrating a method for simultaneous localization and mapping of a robot in a dynamic environment, provided as an embodiment of this application;

[0027] Figure 2 A block diagram illustrating a method for simultaneous localization and mapping of a robot in a dynamic environment, provided in an embodiment of this application;

[0028] Figure 3 This is a schematic diagram of the image preprocessing process in one embodiment of this application;

[0029] Figure 4This is a schematic diagram of the dynamic target recognition process in one embodiment of this application;

[0030] Figure 5 This is a schematic diagram of the motion consistency detection process in one embodiment of this application;

[0031] Figure 6 This is a schematic diagram of the template matching technology in one embodiment of this application;

[0032] Figure 7 This is a flowchart illustrating the construction process of a dense point cloud map in one embodiment of this application;

[0033] Figure 8 This is a flowchart illustrating the keyframe filtering and updating strategy in one embodiment of this application;

[0034] Figure 9 This is a schematic diagram of a camera model in one embodiment of this application;

[0035] Figure 10 This is a flowchart illustrating the construction process of a raster map in one embodiment of this application;

[0036] Figure 11 This application provides a schematic diagram of the functional modules of a robot synchronous localization and mapping system in a dynamic environment, as an embodiment of the present application.

[0037] Figure 12 This is a schematic diagram of the hardware structure of a robot synchronous localization and mapping system in a dynamic environment, provided in an embodiment of this application. Detailed Implementation

[0038] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0039] Traditional SLAM algorithms suffer from the following problems in dynamic environments:

[0040] (1) Traditional SLAM algorithms lack the ability to distinguish targets in the environment and still assume that the environment is in an ideal state of stillness.

[0041] (2) In a dynamic environment, the SLAM algorithm is affected by the interference of dynamic targets, which leads to the failure of feature matching and the continuous accumulation of errors. As a result, the deviation between the camera pose trajectory estimated by the SLAM algorithm and the ground true trajectory is large, which leads to a serious decrease in the positioning accuracy and system robustness of the SLAM algorithm.

[0042] (3) Dynamic targets can also cause map building failure in SLAM algorithm and cause ghosting in the map.

[0043] (4) The sparse maps constructed by the traditional SLAM algorithm can neither meet the needs of the robot to perform tasks nor fully reflect the true appearance of the environment.

[0044] (5) Common dynamic SLAM algorithms always remove feature points on predefined target objects without judgment. That is, if the movable object is static, it will eliminate static feature points. When the static object is moving, it will use dynamic feature points, thus resulting in the problem of retaining the real dynamic feature points and eliminating static feature points.

[0045] (6) The problem that dense point cloud maps cannot be used for robot navigation.

[0046] Therefore, this application provides a method for robot synchronous localization and map construction in dynamic environments, which can extract real dynamic target information from image sequences and eliminate the influence of these areas, thereby improving positioning accuracy and map readability.

[0047] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0048] In one exemplary embodiment, such as Figure 1 and Figure 2 As shown, a method for synchronous localization and map building of a robot in a dynamic environment is provided. This method is executed by a computer device, which can be executed by a computer device such as a terminal or a server alone, or by a terminal and a server together. It includes the following steps 101 to 106.

[0049] Step 101: Acquire an image sequence of the robot's surrounding environment. The image sequence includes multiple consecutive color images and multiple consecutive depth images.

[0050] In one exemplary embodiment, an RGB-D camera is used to acquire consecutive multi-frame color images and consecutive multi-frame depth images of the robot's surrounding environment. For example... Figure 3 As shown, each frame of color image is sequentially denoised and image enhanced, and each frame of depth image is sequentially denoised and filled to obtain an image sequence of the robot's surrounding environment.

[0051] Because RGB-D cameras are susceptible to environmental interference, resulting in measurement errors, and depth images may contain missing values ​​due to sensor viewing angle, occlusion, or reflection, denoising and padding are performed on the depth images to improve the quality of depth information. Denoising and image enhancement of color images improve image quality, highlight details, enhance the discernibility of image features, and reduce noise interference.

[0052] Median filtering is used to remove noise from color images. Median filtering is a non-linear smoothing technique that sets the gray value of each pixel to the median of the gray values ​​of all pixels within a neighborhood window. Median filtering primarily replaces the pixel's value with the median gray value in its neighborhood, thereby eliminating isolated noise points. It can be expressed as:

[0053]

[0054] in, For the restored image, I(r,c) is the noisy image, (r,c) and (x,y) represent pixel positions, w is the neighborhood window of the center pixel I(x,y), and median represents median filtering.

[0055] Histogram equalization is used to enhance color images. Histogram equalization adjusts the grayscale distribution of an image, making its histogram more uniform, thereby enhancing the detail and visibility of the color image. To perform histogram equalization, the cumulative distribution function of the color image must first be calculated. The cumulative distribution function represents the proportion of pixels with a grayscale value less than or equal to a certain value, and can be expressed as:

[0056]

[0057] Where C(i) is the cumulative distribution function value, P(j) is the normalized histogram, and i represents the gray value.

[0058] Then, the cumulative distribution function is used to map the gray values ​​of the original color image to new gray values. Specifically, the mapping is performed using the following formula:

[0059] g(x,y)=round(C(f(x,y))×(L-1));

[0060] Where g(x,y) is the gray value of the equalized color image, f(x,y) is the gray value of the original color image, L is the total number of gray levels (usually 256), and C(f(x,y)) is the cumulative distribution function of the gray values ​​of the original color image.

[0061] Median filtering is used to remove salt-and-pepper noise from depth images while preserving edge information.

[0062] Bilinear interpolation is used to fill in the depth image. Bilinear interpolation calculates the value of the new pixel by considering the values ​​of the four surrounding known pixels, thus providing a smoother and more natural image effect. Assuming the new pixel position (x′, y′) corresponds to the pixel position (x, y) in the original image, then: Among them, S x and S y Let be the scaling factor. Then the coordinates of the four neighboring pixels can be represented as: Top left corner: Top right corner: Bottom left corner: Bottom right corner:

[0063] First, perform horizontal interpolation. The interpolation values ​​I1 for the top left and top right corners are:

[0064]

[0065] The interpolation I2 for the bottom left and bottom right corners is:

[0066]

[0067] Then, vertical interpolation is performed to combine I1 and I2 as follows:

[0068]

[0069] Where I′(x′,y′) represents the new pixel value calculated using the bilinear interpolation formula.

[0070] Step 102: Extract features from the color images in the image sequence to obtain multiple feature points, coordinate information and color information of each feature point, and determine the depth information of each feature point based on the depth image in the image sequence.

[0071] In one exemplary embodiment, an Oriented Features from Accelerated Segment Test and Rotated BRIEF (ORB) algorithm is used to extract features from a color image.

[0072] Specifically, the Oriented Features from Accelerated Segment Test (OFAST) algorithm is used to detect feature points and obtain the corresponding feature point description directions. Assume the moment m of image block B... pq The definition is as follows:

[0073]

[0074] Where p and q have values ​​of 0 or 1, (x,y) represents the pixel position; I'(x,y) represents the grayscale value at this pixel position, x p The weighted result of the offset of the pixels of an image patch relative to its centroid on the x-axis, y q This represents the weighted result of the offset of the pixels of an image patch relative to its centroid on the y-axis.

[0075] In a circular image region of radius R, the image moments along the two coordinate axes are as follows: Where, m 10 The image moment along the x-axis, m 01 This represents the image moment along the y-axis.

[0076] The sum of gray values ​​of all pixels within a circular image region, m 00 for:

[0077] Then the centroid C of the image moments is:

[0078] Then the direction θ of the feature point is:

[0079] Based on the detected OFAST corner values ​​in the color image, the corresponding descriptor is obtained, i.e., at position (x i ,y i The feature descriptor f generated after randomly sampling n1 pairs of points at position ) n1 (p) is: Wherein, τ(p;x) i ,y i ) represents the eigenvector.

[0080] Step 103: Perform target recognition on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment.

[0081] In an exemplary embodiment, to address the shortcomings of traditional SLAM algorithms in identifying targets in the environment and the interference of dynamic targets in dynamic environments on feature matching and pose estimation, a dynamic target recognition model is used to process the color image, identify potential dynamic targets in the environment, obtain the semantic information corresponding to the targets, classify the targets into static targets and potential dynamic targets, and further classify the feature points on the targets into static feature points and dynamic feature points. Step 103 includes steps 301 and 302.

[0082] Step 301: For any frame of color image in the image sequence, a dynamic target recognition model is used to perform target recognition on the color image to obtain semantic information of multiple targets. The semantic information includes bounding boxes, labels, and masks. The labels can be static or dynamic.

[0083] The dynamic target recognition model is obtained by pre-training the YOLOv8n-seg network using a training sample set. The training sample set includes multiple sample images and semantic information of the target in each sample image.

[0084] The YOLOv8n-seg network has fewer parameters and lower computational requirements. YOLOv8n-seg combines object detection and semantic segmentation, allowing for object recognition in images and providing segmentation masks. For example... Figure 4 As shown, YOLOv8n-seg mainly consists of four parts: input, backbone network, neck, and output.

[0085] The input portion of the YOLOv8n-seg network primarily preprocesses the color image and uses an adaptive scaling method to uniformly scale the color image to the same width and height scale, i.e., 640*640. The adaptive scaling method can be expressed as: Where r1 is the scaling factor; w1 is the width of the input color image; h1 is the height of the input color image; and S is the network size of the input color image.

[0086] The scaled image dimensions can then be represented as: Where w′ is the width of the scaled image and h′ is the height of the scaled image.

[0087] The backbone of the YOLOv8n-seg network is primarily responsible for extracting multi-scale features from the input image. It mainly consists of three parts: a Cross-Stage Partial Network (CBS) convolutional module, a Cross-Stage Partial Network with Cross mini-attention (C2f) module, and a Spatial Pyramid Pooling Fast (SPPF) module. The CBS convolutional module introduces short connections between each stage, allowing features to be transferred between different stages, thereby improving feature expressiveness and information transfer efficiency. The C2f module enables the model to better capture complex features in the image, resulting in better performance in object detection tasks. Furthermore, the C2f module has good scalability, allowing for further performance improvements without significantly increasing computational costs. The SPPF module is used for pooling operations at different scales, concatenating feature maps of different scales to improve the detection capability for targets of different sizes.

[0088] The neck region of the YOLOv8n-seg network is mainly used for multi-scale feature fusion, which enhances feature representation capabilities by fusing feature maps from different stages of the backbone network.

[0089] The YOLOv8n-seg network outputs two task branches: an image segmentation branch and an object detection branch. The object detection branch predicts bounding boxes and class, and adds a set of mask coefficients to each bounding box to encode the instance. The image segmentation branch uses a fully convolutional network to generate a set of prototype masks. Then, the two branches are linearly combined to compute and output the instance masks to generate the prototype masks.

[0090] Step 302: Based on the label of each target, the multiple targets are divided into static targets and potential dynamic targets. Furthermore, feature points can be divided into static feature points and potential dynamic feature points.

[0091] Step 104: Determine the motion trajectory of the feature points of the potential dynamic target based on the color images in the image sequence, remove feature points whose motion trajectories do not conform to the expected motion trajectory, and combine them with the feature points of the static target to obtain static feature points.

[0092] In an exemplary embodiment, common dynamic SLAM algorithms indiscriminately remove feature points from predefined target objects. This application uses a motion consistency detection model to judge potential dynamic targets based on the motion trajectory of feature points, further distinguishing between dynamic and static targets, eliminating feature points that do not conform to the expected motion, and retaining only static feature points to improve positioning accuracy and stability. Step 104 includes steps 401 to 404.

[0093] Step 401: Based on the image sequence, the dense optical flow method and template matching technology are used to track and match the potential dynamic target in order to obtain the position of each feature point of the potential dynamic target in each frame of color image.

[0094] Specifically, such as Figure 5 As shown, the dense optical flow method Farneback is used to calculate the optical flow within a potential dynamic target region, obtaining pixel-level motion information of the potential dynamic target in adjacent frames. Farneback is a gradient-based method that assumes that the image gradient is constant and that the local optical flow is constant.

[0095] First, construct an image model, treating the image as a function of a two-dimensional signal, then we have: f1(x,y)=X T AX+b T X+c'; where the variable is the two-dimensional coordinate position X=(x,y). T A is a 2×2 symmetric matrix, b is a 2×1 matrix vector, and c' is a scalar.

[0096] Then the displacement is estimated, assuming the original position is: f1(X) = X T A1X+b1 T X+c1; where the variable is the two-dimensional coordinate position X=(x,y). T A1 is a 2×2 symmetric matrix; b1 is a 2×1 matrix vector; c1 is a scalar.

[0097] After a pixel moves, there is a global displacement.

[0098] in, Then the global displacement d can be obtained as:

[0099] To approximate the true value, let: Then it exists: Here, Δb(X) represents an intermediate quantity.

[0100] Using the neighborhood of the interest pixel, a weighted objective function is constructed for optimization to obtain the displacement e(X):

[0101]

[0102] Where ω(ΔX) is the weighting coefficient, and the image sequence is denoted as I(x,y,t).

[0103] The dense optical flow method can solve for the motion parameters u and v by minimizing the cost function in the optical flow constraint:

[0104]

[0105] Among them, I t It is the gradient of the image at time t, I x I represents the gradient of the image along the x-axis. y The gradient of the image along the y-axis, where u and v are the components of the optical flow along the x-axis and y-axis, respectively.

[0106] To further confirm the precise location of dynamic targets and enhance tracking robustness, template matching technology is used to further identify and track potential dynamic targets based on the dense optical flow method. Template matching technology is used to find regions in an image that are similar to a given template (or reference image). It determines the best matching location by comparing the template with sub-regions in the image. A schematic diagram of the template matching technology is shown below. Figure 6 As shown. Assume there exists an image of frame i' with width and height T. w and T h The initial template image is generated. In the i'+1th frame, a template matching search is performed from left to right and from top to bottom, and the most similar rectangular region is found after moving Δu and Δv.

[0107] This application uses a potential dynamic target as a template image and a color image as a source image. A similarity metric is used to compare regions in the template image and the source image to obtain similar regions. The similarity metric matching algorithm can be described as follows:

[0108] Calculate the mean of the template image: in, T(x,y) is the average value of the template image, N is the total number of pixels in the template image, and T(x,y) is the pixel value of the template image.

[0109] Calculate the mean of a local region in the target image: in, I(x+u1,y+v1) is the average value of the target image at position (u1,v1) within the corresponding window, and I(x+u1,y+v1) is the pixel value of the target image at position (u1,v1).

[0110] To calculate the correlation, the image is mean-trimmed, resulting in:

[0111] To measure the similarity between two images, cross-correlation is performed on them:

[0112] To measure the intensity variation of an image, its standard deviation is calculated:

[0113]

[0114] Where, σ T The standard deviation of the template image is represented. This represents the standard deviation of a local region in the target image.

[0115] Therefore, the formula for template matching can be expressed as:

[0116]

[0117] R(u1,v1) represents the similarity between the source image and the template image. It is a value between -1 and 1, where 1 indicates a perfect match, -1 indicates a complete mismatch, and 0 indicates no relevance.

[0118] Step 402: Based on the position of each feature point of the potential dynamic target in each frame of color image, the motion trajectory of each feature point of the potential dynamic target is determined by the dynamic time warping method.

[0119] Dynamic temporal warping is a method for measuring the similarity between two time series. After tracking and matching targets in the environment (i.e., after optical flow tracking and template matching), the target's motion trajectory is formed by recording the position of the target region (mainly bounding box information) in each frame of the image. The target's motion trajectory can be used to determine whether the target is moving. After determining the target's motion state, it is also necessary to determine the motion state of feature points within the target region. Therefore, to determine the motion trajectory of feature points and facilitate subsequent removal processing, dynamic temporal warping is used to analyze the motion trajectory of feature points within the obtained dynamic target region.

[0120] The trajectory of the feature point can be represented as: T k ={(x k1 ,y k1 ),(x k2 ,y k2 ),…,(x kn ,y kn )};wherein, T k Representing feature point P k The trajectory, (x kn ,y kn ) represents P k The coordinates in the nth frame of the image.

[0121] Define the expected trajectory as: Texpected ={(x e1 ,y e1 ),(x e2 ,y e2 ),...,(x en ,y en )};wherein, T expected Represents the expected trajectory of the feature points, (x en ,y en ) represents the coordinates of the feature point in the nth frame of the image.

[0122] Step 403: Calculate the similarity between the motion trajectory of each feature point of the potential dynamic target and the expected motion trajectory.

[0123] Specifically, the process of calculating the similarity between the motion trajectory of a certain feature point and the expected motion trajectory is as follows:

[0124] (1) Calculate the distance between the trajectory and the expected trajectory. Specifically, for any two points (x... k1 ,y k1 ) and (x e1 ,y e1 Using Euclidean distance, it can be expressed as: Where, d((x) k1 ,y k1 ),(x e1 ,y e1 )) represents the Euclidean distance between two points.

[0125] (2) Construct a distance matrix D, where the elements of the distance matrix are the distances between the motion trajectory and the expected motion trajectory.

[0126] (3) Dynamically program to fill the distance matrix and evaluate similarity by minimizing the cumulative distance: D TW (T k ,T expected ) = D[n][m]; where D TW (T k ,T expected ) represents the dynamic time-warped distance, and D[n][m] represents the elements in the distance matrix.

[0127] (4) Normalize the dynamic time-warped distance to calculate the similarity score: Where Q is the similarity, and max(n,m) represents the maximum length of the feature point trajectory and the expected trajectory.

[0128] Step 404: Remove feature points with similarity less than the set similarity threshold, and combine them with the feature points of the static target to obtain static feature points.

[0129] If the similarity of a feature point is less than a set similarity threshold, it indicates that its motion trajectory does not match the expected motion trajectory, and it is marked as a dynamic feature point. Based on the obtained mask information, dynamic feature points in the masked region on the dynamic target are removed. For feature points with a similarity greater than or equal to the set similarity threshold, it indicates that their motion trajectory matches the expected motion trajectory, and they are considered static feature points, which are then added to the list of static feature points.

[0130] Step 105: Generate a dense point cloud map based on the depth information, coordinate information and color information of the static feature points.

[0131] Traditional SLAM algorithms construct sparse point maps. They only utilize spatial geometric information, thus failing to meet the needs of robot task execution or fully reflect the true state of the environment. Furthermore, in dynamic environments, maps constructed by traditional SLAM algorithms are affected by dynamic targets, leading to significant point overlap and ghosting. This application, after removing interference from dynamic targets, generates a static point cloud by selecting and updating keyframes and combining these keyframes with semantic information obtained from a dynamic target recognition model. The point cloud is then stitched and filtered to generate a dense point cloud map of the environment. The specific process is as follows: Figure 7 As shown, dense point cloud maps can reflect the true information of the environment in detail, reduce map ghosting and offset phenomena, and improve map readability.

[0132] Step 105 includes steps 501 to 505.

[0133] Step 501: For any frame image in the image sequence, a structural similarity algorithm is used to calculate the similarity between the current frame image and the reference frame image. The reference frame image is the frame image preceding the current frame image.

[0134] Step 502: Remove current frame images with a similarity greater than a set threshold from the image sequence to obtain a processed image sequence. The processed image sequence includes multiple key color images and multiple key depth images.

[0135] Inserting too many keyframes can lead to information redundancy, resulting in point cloud stacking and layering in the constructed map. Therefore, to improve map quality and minimize the insertion of new keyframes, this application utilizes a keyframe filtering and updating strategy to process the input keyframes.

[0136] like Figure 8As shown, for each input keyframe, the current frame image and the reference frame image are compared for similarity. If the current frame image and the reference frame image are similar, the current frame image is discarded; otherwise, the current frame image is inserted into the image sequence, and the keyframe is updated. The keyframe selection and update strategy mainly uses the Structural Similarity Index Metric (SSIM) algorithm to compare the similarity between the current frame image and the reference frame image. The main formula can be expressed as:

[0137]

[0138] Where J1 represents the reference frame image, J2 represents the current frame image, μ1 represents the average value of the reference frame image, and μ2 represents the average value of the current frame image. This represents the variance of the reference frame image. σ represents the variance of the current frame image. 12 C1 and C2 are the covariances of two image frames, respectively, and are constants. Based on the similarity calculation results, keyframes with high similarity are discarded, and the image sequence is updated.

[0139] Step 503: For any frame of key color image in the processed image sequence, determine the coordinate information, color information and depth information of static feature points based on the key color image and the corresponding key depth image.

[0140] Step 504: Determine the single-frame point cloud based on the coordinate information, color information, and depth information of the static feature points.

[0141] Specifically, based on the coordinate, color, and depth information of static feature points, the camera intrinsic parameters are used to calculate the transformation of pixel points on the static target from two-dimensional to three-dimensional coordinates, thereby obtaining a single-frame point cloud.

[0142] Camera model such as Figure 9 As shown, the correspondence between a spatial point [x”,y”,z”] and its pixel coordinates [u”,v”,d”] in the image is as follows:

[0143]

[0144] Where d” represents depth information, f x f is the focal length of the camera on the x-axis. y Let c be the focal length of the camera on the y-axis. x c is the center of the camera's aperture on the x-axis. y Let be the aperture center of the camera on the y-axis, and s be the scaling factor of the depth map.

[0145] Conversely, we have:

[0146]

[0147] The point cloud of a single frame is obtained by calculating the camera's intrinsic parameters. The main coordinate calculation formula is as follows:

[0148]

[0149] Where C” is the camera's intrinsic parameter matrix, R” is the rotation matrix, and t” is the displacement vector.

[0150] Figure 9 In the image, P represents a real-world spatial coordinate point; p1 and q1 represent the pixel coordinate points projected from P onto the image plane; Q represents a real-world spatial coordinate point; Z... O The z-axis represents the coordinate system of the real world.

[0151] Step 505: Stitch together all the single-frame point clouds to obtain a dense point cloud map.

[0152] Specifically, the generated single-frame point clouds are stitched together to obtain local point clouds. Since local point clouds contain outliers and noise points, they need to be filtered to remove outliers and similar points in overlapping areas, thereby reducing the overlap of the point clouds and improving their quality.

[0153] Specifically, a statistical filter is used to remove outliers from the point cloud, and a voxel filter is used to filter out similar points in overlapping areas of the point cloud, ensuring that there is only one point within a voxel. The main formula is as follows:

[0154]

[0155] Where "m" represents a local point cloud, and T k "Indicates the position of the RGB-D camera, C" k This represents a single frame of point cloud.

[0156] Step 106: Discretize the dense point cloud map into a two-dimensional grid to construct a grid map to guide the robot in performing navigation tasks.

[0157] Since dense point cloud maps do not contain information about the presence of obstacles, they cannot assist robots in completing navigation tasks. Therefore, this application further constructs a grid map based on dense point cloud maps, providing fundamental support for applications such as robot navigation, path planning, and environmental perception.

[0158] In one exemplary embodiment, step 106 includes steps 601 to 604.

[0159] Step 601, extract 3D point cloud data P from the dense point cloud map: P = {(x a ,y a ,z a)a=1,2,...,A'};where A' is the number of point clouds, (x a ,y a ,z a ) represents the coordinates of the point cloud data.

[0160] Step 602: Determine the grid size.

[0161] Step 603: Traverse the 3D point cloud data and determine the boundary of the raster map based on the minimum and maximum coordinates in the 3D point cloud data.

[0162] Step 604: Construct a grid map based on the 3D point cloud data, the grid size, and the boundary of the grid map.

[0163] Specifically, based on the boundaries and grid size of the raster map, a two-dimensional array is initialized to represent the raster map. Each element in the array is initially set to 0, indicating that the area is unoccupied. The 3D point cloud data is traversed, and its position within the raster map is calculated to fill the map, updating the corresponding grid cell value to 1, indicating that the area is occupied. The raster map construction process is as follows: Figure 10 As shown.

[0164] For each point (x) in the point cloud a ,y a ,z a Calculate its index in the raster map:

[0165]

[0166] Among them, G row G represents the row index. col Indicates column index.

[0167] Preferably, the method for robot synchronous localization and map building in a dynamic environment further includes: step 107, performing an erosion operation on the grid map to clean the grid map and improve its quality.

[0168] Erosion can smooth out maps. It works by using a small matrix called a structuring element, sliding across the image and checking if corresponding pixels satisfy a certain condition (usually an AND operation). If all pixel values ​​meet the condition, the center pixel value remains unchanged; otherwise, it is eroded to 0. This process can be used to eliminate small noises, separate contacting objects, and shrink objects. The erosion operation can be represented as:

[0169]

[0170] Where I” is the input image, SE is the structuring element, and (α,β) is the coordinate offset of the structuring element.

[0171] This application utilizes a dynamic target recognition model to detect static and potential dynamic targets in the environment, as well as the corresponding semantic information of the targets. It distinguishes feature points on the targets into static and potential dynamic feature points, enabling the SLAM algorithm to recognize targets in the environment. A motion consistency detection model is used to further judge potential dynamic targets. Based on the motion trajectory of feature points, feature points that do not conform to the expected motion are eliminated, reducing the impact of dynamic targets on positioning accuracy and stability, and improving the robot's performance in dynamic environments. The filtered and updated keyframes are combined with the target semantic information obtained from the dynamic target recognition model to generate a single-frame point cloud. A dense point cloud map is constructed by filtering and stitching the point cloud. The 3D point cloud data in the dense point cloud map is discretized into a 2D grid. Rasterization technology is used to provide a clear representation of occupancy status while maintaining environmental information, thereby constructing a raster map.

[0172] This application can eliminate the interference of dynamic targets on synchronous localization and map building methods in real-world dynamic scenarios, enabling real-time robot localization and improving map readability. It enhances the robot's localization accuracy, stability, and mapping performance in dynamic environments, providing fundamental technical support for applications such as robot navigation, path planning, and environmental perception.

[0173] Based on the same inventive concept, this application also provides a system for synchronous localization and mapping (SMR) of robots in dynamic environments, used to implement the above-described method for simultaneous localization and mapping (SLAM) of robots in dynamic environments. The solution provided by this system is similar to the solution described in the above method. Therefore, the specific limitations of one or more embodiments of the system for synchronous localization and mapping of robots in dynamic environments provided below can be found in the limitations of the method for synchronous localization and mapping of robots in dynamic environments described above, and will not be repeated here.

[0174] In one exemplary embodiment, such as Figure 11 As shown, the robot synchronous localization and map building system in a dynamic environment includes: an image acquisition module 201, a feature extraction module 202, a target recognition module 203, a feature point determination module 204, a point cloud map generation module 205, and a grid map building module 206.

[0175] The image acquisition module 201 is used to acquire an image sequence of the robot's surrounding environment. The image sequence includes multiple consecutive color images and multiple consecutive depth images.

[0176] The feature extraction module 202 is used to extract features from the color images in the image sequence, obtain multiple feature points, coordinate information and color information of each feature point, and determine the depth information of each feature point based on the depth image in the image sequence.

[0177] The target recognition module 203 is used to perform target recognition on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment.

[0178] The feature point determination module 204 is used to determine the motion trajectory of the feature points of the potential dynamic target based on the color image in the image sequence, remove feature points whose motion trajectory does not conform to the expected motion trajectory, and combine them with the feature points of the static target to obtain static feature points.

[0179] The point cloud map generation module 205 is used to generate a dense point cloud map based on the depth information, coordinate information and color information of the static feature points.

[0180] The grid map construction module 206 is used to discretize the dense point cloud map into a two-dimensional grid to construct a grid map to guide the robot in performing navigation tasks.

[0181] like Figure 12 As shown, the robot synchronous localization and mapping system in a dynamic environment uses a Raspberry Pi as the core processing unit, mainly connected to an RGB-D camera and a monitor. The RGB-D camera is used to acquire and transmit images of the surrounding environment. The monitor is used to display the constructed dense point cloud map and raster map.

[0182] The Raspberry Pi runs on Ubuntu 18.04 and consists of a processor and memory. The memory primarily stores executable program code and temporary data needed by the processor to execute the code. The processor includes a Graphics Processing Unit (GPU) and a Central Processing Unit (CPU). The GPU performs parallel computing tasks, accelerating the inference speed of the dynamic target recognition model. The CPU is responsible for basic instruction sending and receiving, controlling the overall process, and communicating with external devices.

[0183] In one exemplary embodiment, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps in the above-described method embodiments.

[0184] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0185] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0186] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data must comply with relevant regulations.

[0187] In this application, all actions to acquire signals, information, or data are carried out in compliance with the relevant data protection laws and policies of the country where the location is situated, and with the authorization granted by the owner of the relevant device.

[0188] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0189] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0190] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for simultaneous localization and mapping of a robot in a dynamic environment, characterized in that, The method for robot synchronous localization and mapping in dynamic environments includes: Acquire an image sequence of the robot's surrounding environment; the image sequence includes multiple consecutive color images and multiple consecutive depth images; Feature extraction is performed on the color images in the image sequence to obtain multiple feature points, coordinate information and color information of each feature point, and the depth information of each feature point is determined based on the depth image in the image sequence; Target recognition is performed on the color images in the image sequence to identify static targets and potential dynamic targets in the robot's surrounding environment; Based on the image sequence, dense optical flow and template matching techniques are used to track and match potential dynamic targets in order to obtain the position of each feature point of the potential dynamic target in each frame of color image; Based on the position of each feature point of the potential dynamic target in each frame of color image, the motion trajectory of each feature point of the potential dynamic target is determined by the dynamic time warping method. Calculate the similarity between the motion trajectory and the expected motion trajectory of each feature point of a potential dynamic target; specifically, this includes: calculating the distance between the motion trajectory and the expected motion trajectory; constructing a distance matrix D, where each element represents the distance between the motion trajectory and the expected motion trajectory; dynamically programming to fill the distance matrix; and evaluating the similarity by minimizing the cumulative distance: D TW (T k ,T expected ) = D[n][m]; where D TW (T k ,T expected ) represents the dynamic time-warped distance, D[n][m] represents the elements in the distance matrix, and T k Representing feature point P k The trajectory of T expected The expected trajectory of the feature points is represented; the dynamic time-normalized distance is normalized to calculate the similarity score. Where Q is the similarity, and max(n,m) represents the maximum length between the feature point trajectory and the expected trajectory; Feature points with similarity less than a set similarity threshold are marked as dynamic feature points. Combined with the obtained mask information, dynamic feature points in the masked region on the dynamic target are removed. Conversely, static feature points are obtained by combining the feature points of the static target. Based on the depth, coordinate, and color information of the static feature points, a dense point cloud map is generated. Specifically, this includes: for any frame image in the image sequence, calculating the similarity between the current frame image and a reference frame image using a structural similarity algorithm; the reference frame image is the frame image preceding the current frame image; removing current frame images with similarity greater than a set threshold from the image sequence to obtain a processed image sequence; the processed image sequence includes multiple key color images and multiple key depth images; for any key color image in the processed image sequence, determining the coordinate, color, and depth information of the static feature points based on the key color image and the corresponding key depth image; determining a single-frame point cloud based on the coordinate, color, and depth information of the static feature points; and stitching together all the single-frame point clouds to obtain a dense point cloud map. The dense point cloud map is discretized into a two-dimensional grid to construct a grid map to guide the robot in performing navigation tasks; The raster map is subjected to an erosion operation.

2. The method for robot synchronous localization and map building in a dynamic environment according to claim 1, characterized in that, Acquire image sequences of the robot's surrounding environment, specifically including: An RGB-D camera is used to acquire multiple consecutive frames of color images and multiple consecutive frames of depth images of the robot's surrounding environment; Each frame of color image is sequentially denoised and image enhanced, and each frame of depth image is sequentially denoised and filled to obtain an image sequence of the robot's surrounding environment.

3. The method for robot synchronous localization and map building in a dynamic environment according to claim 1, characterized in that, Target recognition is performed on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment, specifically including: For any frame of color image in the image sequence, a dynamic target recognition model is used to identify targets in the color image to obtain semantic information of multiple targets; the semantic information includes bounding boxes, labels, and masks; the labels are static or dynamic; the dynamic target recognition model is obtained by pre-training the YOLOv8n-seg network with a training sample set; the training sample set includes multiple sample images and semantic information of targets in each sample image; Based on the label of each target, multiple targets are divided into static targets and potential dynamic targets.

4. The method for robot synchronous localization and map building in a dynamic environment according to claim 1, characterized in that, Discretizing the dense point cloud map into a two-dimensional grid to construct a raster map specifically includes: Extract 3D point cloud data from the dense point cloud map; Determine the grid size; Traverse the three-dimensional point cloud data and determine the boundary of the raster map based on the minimum and maximum coordinates in the three-dimensional point cloud data; A raster map is constructed based on the 3D point cloud data, the raster size, and the boundaries of the raster map.

5. A robot synchronous localization and mapping system for a dynamic environment, applied to the robot synchronous localization and mapping method for a dynamic environment as described in any one of claims 1-4, characterized in that, The robot synchronous localization and mapping system in the dynamic environment includes: The image acquisition module is used to acquire an image sequence of the robot's surrounding environment; the image sequence includes multiple consecutive color images and multiple consecutive depth images; The feature extraction module is used to extract features from the color images in the image sequence, obtain multiple feature points, coordinate information and color information of each feature point, and determine the depth information of each feature point based on the depth image in the image sequence. The target recognition module is used to perform target recognition on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment. The feature point determination module is used to determine the motion trajectory of the feature points of the potential dynamic target based on the color image in the image sequence, remove feature points whose motion trajectory does not conform to the expected motion trajectory, and combine them with the feature points of the static target to obtain static feature points. The point cloud map generation module is used to generate a dense point cloud map based on the depth information, coordinate information and color information of the static feature points. The grid map construction module is used to discretize the dense point cloud map into a two-dimensional grid and construct a grid map to guide the robot in performing navigation tasks.

6. A computer device, comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor executes the computer program to implement the robot synchronous localization and mapping method in a dynamic environment as described in any one of claims 1-4.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by a processor, the computer program implements the robot synchronous localization and mapping method in a dynamic environment as described in any one of claims 1-4.

Images