Robot synchronous positioning and mapping method, system and device in dynamic environment and medium

Abstract

The invention discloses a robot synchronous positioning and mapping method, system and device in a dynamic environment and a medium, and relates to the field of robot positioning and mapping, and the method comprises the steps: obtaining an image sequence of the surrounding environment of a robot; performing feature extraction on the color image, and determining coordinate information, color information and depth information of each feature point in combination with the depth image; target identification is carried out on the image sequence, and a static target and a potential dynamic target in the surrounding environment of the robot are determined; removing feature points of potential dynamic targets whose motion trails do not conform to an expected motion trail, and combining the feature points of the static targets to obtain static feature points; generating a dense point cloud map according to the depth information, the coordinate information and the 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 execute a navigation task. According to the invention, the positioning precision of the robot and the readability of the map can be guaranteed in a dynamic environment.

Description

Technical Field

[0001] The present application relates to the technical field of robot positioning and map building, and in particular to a method, system, device and medium for synchronous positioning and map building of a robot in a dynamic environment. Background Art

[0002] Mobile robots are a combination of multiple disciplines, mainly involving professional fields such as control theory, computer technology, information technology, sensor technology, and artificial intelligence. Therefore, it has basic characteristics such as perception, decision-making, and execution. The emergence of mobile robots is to assist or even replace humans in completing work in complex environments or dangerous situations, and to improve work efficiency and quality. It is worth noting that if robots want to successfully complete their tasks, the most important thing is to solve the problems of real-time positioning, map construction, and path planning of robots.

[0003] Simultaneous Localization and Mapping (SLAM) technology is an important method for robots to perform autonomous positioning and environmental mapping. It has gradually become one of the key technologies for robots to perform complex and advanced tasks, and is widely used in many fields such as unmanned driving, augmented reality, and drones. SLAM technology enables robots to obtain information about the surrounding environment in unknown environments without human help, relying only on their own sensors, estimate their posture, and incrementally build environmental maps to provide positioning and map information for the robot. In addition, it can also improve the autonomous navigation capabilities of mobile robots and their adaptability to different application environments, and help achieve real-time obstacle avoidance and dynamic path planning.

[0004] However, traditional SLAM algorithms rely on static environment assumptions to ensure the robustness and efficiency of positioning, but actual unstructured scenes usually contain dynamic targets such as pedestrians, vehicles, and animals. Therefore, traditional SLAM algorithms cannot guarantee positioning 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 simultaneous positioning and map construction of a robot in a dynamic environment, which can ensure the positioning accuracy of the robot and the readability of the map in a dynamic environment.

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

[0007] In a first aspect, the present application provides a method for synchronous positioning and map construction of a robot in a dynamic environment, comprising:

[0008] Acquire an image sequence of the robot's surrounding environment; the image sequence includes a continuous multi-frame color image and a continuous multi-frame depth image;

[0009] Extracting features from the color images in the image sequence to obtain a plurality of feature points, coordinate information and color information of each feature point, and determining depth information of each feature point according to the depth image in the image sequence;

[0010] Performing target recognition on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment;

[0011] Determine the motion trajectory of the feature points of the potential dynamic target according to the color image in the image sequence, remove the feature points whose motion trajectory does not conform to the expected motion trajectory, and obtain static feature points by combining the feature points of the static target;

[0012] Generate a dense point cloud map according to the depth information, coordinate information and color information of the static feature points;

[0013] The dense point cloud map is discretized into a two-dimensional grid and a grid map is constructed to guide the robot to perform a navigation task.

[0014] In a second aspect, the present application provides a robot synchronous positioning and map building system in a dynamic environment, comprising:

[0015] An image acquisition module is used to acquire an image sequence of the robot's surrounding environment; the image sequence includes a continuous multi-frame color image and a continuous multi-frame depth image;

[0016] A feature extraction module, used to extract features from the color image 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 according to the depth image in the image sequence;

[0017] A 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] A feature point determination module is used to determine the motion trajectory of the feature points of the potential dynamic target according to the color image in the image sequence, remove the feature points whose motion trajectory does not conform to the expected motion trajectory, and obtain static feature points in combination with the feature points of the static target;

[0019] A point cloud map generation module, used to generate a dense point cloud map according to 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 to perform navigation tasks.

[0021] In a third aspect, the present 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-mentioned method for synchronous positioning and mapping of a robot in a dynamic environment.

[0022] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned method for synchronous positioning and mapping of a robot in a dynamic environment.

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

[0024] The present application provides a method, system, device and medium for synchronous positioning and mapping of a robot in a dynamic environment. By performing target recognition on color images in an image sequence, static targets and potential dynamic targets in the robot's surrounding environment are determined, and the feature points of potential dynamic targets whose motion trajectories do not conform to the expected motion trajectories are further eliminated, and static feature points are obtained by combining the feature points of static targets, thereby reducing the impact of dynamic targets on positioning accuracy and stability and improving the performance of the robot in a dynamic environment. A dense point cloud map is generated based on the depth information, coordinate information and color information of the static feature points, and the dense point cloud map is discretized into a two-dimensional grid to construct a grid map. The gridding technology can provide a clear occupancy status representation while maintaining environmental information, thereby improving the readability of the map, and thus improving the positioning accuracy, stability and mapping performance of the robot in a dynamic environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0026] Figure 1 A schematic diagram of a flow chart of a method for synchronous positioning and map construction of a robot in a dynamic environment provided by an embodiment of the present application;

[0027] Figure 2 A block diagram of a method for synchronous positioning and mapping of a robot in a dynamic environment provided by an embodiment of the present application;

[0028] Figure 3 This is a schematic diagram of a process for preprocessing an image in an embodiment of the present application;

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

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

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

[0032] Figure 7 This is a flowchart of constructing a dense point cloud map in one embodiment of the present application;

[0033] Figure 8 A flowchart of a key frame screening and updating strategy in one embodiment of the present application;

[0034] Fig. 9 A schematic diagram of a camera model in an embodiment of the present application;

[0035] Fig.10 A flowchart of constructing a grid map in an embodiment of the present application;

[0036] Fig.11 A schematic diagram of functional modules of a robot synchronous positioning and map building system in a dynamic environment provided by an embodiment of the present application;

[0037] Fig.12 A schematic diagram of the hardware structure of a robot synchronous positioning and map building system in a dynamic environment provided in one embodiment of the present application. DETAILED DESCRIPTION

[0038] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

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

[0040] (1) Traditional SLAM algorithms lack the ability to identify 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 will be disturbed by dynamic targets, resulting in feature matching failure and error accumulation, which will cause a large deviation between the camera pose trajectory estimated by the SLAM algorithm and the ground truth trajectory, thereby causing the positioning accuracy and system robustness of the SLAM algorithm to be seriously reduced.

[0042] (3) Dynamic targets can also cause the SLAM algorithm to fail in map construction and cause ghosting to appear in the map.

[0043] (4) The sparse maps constructed by traditional SLAM algorithms can neither meet the requirements of robot task execution 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, the static feature points will be eliminated, and when the static object is moving, the dynamic feature points will be used, resulting in the problem of retaining the true dynamic feature points and eliminating the static feature points.

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

[0046] Therefore, the present application provides a method for simultaneous positioning and mapping of a robot in a dynamic environment, which can extract true dynamic target information from an image sequence and eliminate the influence of these areas, thereby improving positioning accuracy and map readability.

[0047] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

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

[0049] Step 101, obtaining an image sequence of the robot's surrounding environment, wherein the image sequence includes a plurality of consecutive color images and a plurality of consecutive depth images.

[0050] In an exemplary embodiment, an RGB-D camera is used to collect continuous multi-frame color images and continuous multi-frame depth images of the robot's surrounding environment. Figure 3 As shown, each frame of the color image is processed by denoising and image enhancement in turn, and each frame of the depth image is processed by denoising and filling in turn to obtain an image sequence of the robot's surrounding environment.

[0051] Since the RGB-D camera is disturbed by the environment, there will be errors in the measurement, and there will be missing values ​​in the depth image due to sensor viewing angle, occlusion or reflection. Therefore, the depth image is denoised and filled to improve the quality of the depth information. By denoising and enhancing the color image, the image quality is improved, the details are highlighted, the discernibility of the image features is enhanced, and the interference of noise is reduced.

[0052] Among them, the median filter method is used to remove the noise in the color image. The median filter method is a nonlinear smoothing technology that sets the gray value of each pixel point to the median of the gray values ​​of all pixels in a certain neighborhood window of the point. The median filter method mainly replaces the pixel value with the gray median in the pixel neighborhood to eliminate isolated noise points, which can be expressed as:

[0053]

[0054] in, is the restored image, I(r,c) is the noise image, (r,c) and (x,y) represent the 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 the color image. Histogram equalization adjusts the grayscale distribution of the image to make the histogram of the color image more uniform, thereby enhancing the details and visibility of the color image. In order to perform histogram equalization, it is first necessary to calculate the cumulative distribution function of the color image. The cumulative distribution function represents the proportion of pixels less than or equal to a certain grayscale value, which can be expressed as:

[0056]

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

[0058] Then, the grayscale value of the original color image is mapped to the new grayscale value using the cumulative distribution function. The following formula is used for mapping:

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

[0060] Among them, g(x,y) is the grayscale value of the color image after equalization, f(x,y) is the grayscale 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 grayscale value of the original color image.

[0061] The median filtering method is used to remove the salt and pepper noise in the depth image and keep the edge information.

[0062] The depth image is filled using bilinear interpolation. Bilinear interpolation provides a smoother and more natural image by taking into account the values ​​of the four surrounding known pixels to calculate the value of the new pixel. Assuming that 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 is the scaling factor. Then the coordinates of the four neighboring pixels can be expressed as: upper left corner: Top right corner: Bottom left: Bottom right corner:

[0063] First, perform horizontal interpolation, and the interpolation I1 of the upper left corner and the upper right corner is:

[0064]

[0065] The interpolation I2 of the lower left corner and the lower right corner 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 , extracting features from the color image in the image sequence to obtain a plurality of feature points, coordinate information and color information of each feature point, and determining depth information of each feature point according to the depth image in the image sequence.

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

[0072] Among them, the oriented features from accelerated segment test (OFAST) algorithm is used to detect the feature points and obtain the description direction of the corresponding feature points. Assume that the moment m of the image block B is pq The definition is as follows:

[0073]

[0074] Among them, the values ​​of p and q are 0 or 1, (x, y) represents the pixel position; I'(x, y) represents the grayscale value at this pixel position, and x p Represents the weighted result of the offset of the pixel of the image block relative to the centroid on the x-axis coordinate, y q The weighted result representing the offset of the pixels of the image block relative to the centroid on the y-axis coordinate.

[0075] In the circular image area with a radius of R, the image moments along the two coordinate axes are: Among them, m 10 Represents the image moment along the x-axis, m 01 Represents the image moment along the y-axis.

[0076] The sum of the gray values ​​of all pixels in the circular image area m 00 for:

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

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

[0079] The corresponding descriptor is obtained according to the OFAST corner point value in the detected color image, that is, at the position (x i ,y i ) and generate the feature descriptor f after randomly sampling n1 pairs of points n1 (p) is: Among them, τ(p; x i ,y i ) represents the feature vector.

[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, in view of the problem that the traditional SLAM algorithm lacks the ability to identify targets in the environment and that dynamic targets in a dynamic environment interfere with the feature matching and pose estimation of the traditional SLAM algorithm, a dynamic target recognition model is used to process the color image, identify potential dynamic targets in the environment, and obtain semantic information corresponding to the targets, divide the targets into static targets and potential dynamic targets, and further divide the feature points on the targets into static feature points and dynamic feature points. Step 103 includes the following steps 301 and 302.

[0082] Step 301: for any 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 a bounding box, a label and a mask. The label is static or dynamic.

[0083] The dynamic target recognition model is obtained by pre-training the YOLOv8n-seg network using a training sample set, wherein 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 computational requirements. YOLOv8n-seg combines object detection and semantic segmentation, allowing objects to be identified in images and providing segmentation masks. Figure 4 As shown, YOLOv8n-seg mainly consists of four parts: input, backbone network, neck and output.

[0085] The input part of the YOLOv8n-seg network mainly preprocesses the color image and uses the adaptive scaling method to uniformly scale the color image to the same width and height scale, that is, 640*640. The adaptive scaling method can be expressed as: Among them, r1 is the scaling ratio; 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 width and height of the scaled image can be expressed as: Wherein, w′ is the width of the scaled image, and h′ is the height of the scaled image.

[0087] The backbone network of the YOLOv8n-seg network is mainly responsible for extracting multi-scale features from the input image. It mainly includes three parts: the Cross-Stage Parial Network (CBS) convolution module, the Cross-Stage Partial Network with Cross mini-attention (C2f) module, and the Spatial Pyramid Pooling Fast (SPPF) module. The main function of the CBS convolution module is to introduce short connections between each stage so that features can be transferred between different stages, thereby improving the expressiveness of features and the efficiency of information transfer. The C2f module enables the model to better capture the complex features in the image, thereby achieving better results in the target detection task. In addition, the C2f module also has good scalability, which can further improve the performance of the model without significantly increasing the computational cost. The SPPF module is used for pooling operations of different scales, splicing feature maps of different scales together to improve the detection ability of targets of different sizes.

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

[0089] The output of the YOLOv8n-seg network includes two task branches, namely the image segmentation branch and the object detection branch. The object detection branch performs bounding box prediction and category prediction, 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. The two branches are then linearly combined to calculate and output the instance mask to generate the prototype mask.

[0090] Step 302: According to the label of each target, multiple targets are divided into static targets and potential dynamic targets. Feature points can be further divided into static feature points and potential dynamic feature points.

[0091] Step 104 , determining the motion trajectory of the feature points of the potential dynamic target based on the color image in the image sequence, removing the feature points whose motion trajectory does not conform to the expected motion trajectory, and combining the feature points of the static target to obtain static feature points.

[0092] In an exemplary embodiment, in view of the fact that common dynamic SLAM algorithms always remove feature points on predefined target objects without judgment, the present application uses a motion consistency detection model to judge potential dynamic targets according to the motion trajectory of feature points, further distinguish dynamic targets from static targets, remove feature points that do not match expected motion, and only retain static feature points to improve positioning accuracy and stability. Step 104 includes the following steps 401 to 404.

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

[0094] Specifically, Figure 5 As shown, the dense optical flow method Farneback is used to calculate the optical flow in the potential dynamic target area to obtain the 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, we build an image model and regard the image as a function of a two-dimensional signal, then: 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 the 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 there exists: Here, Δb(X) represents the intermediate quantity.

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

[0101]

[0102] Among them, ω(ΔX) is the weight coefficient, and the image sequence is recorded as I(x, y, t).

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

[0104]

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

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

[0107] This application uses the potential dynamic target as the template image and the color image as the source image, and uses similarity measurement to compare the areas in the template image and the source image to obtain similar areas. The similarity measurement matching algorithm can be described as:

[0108] Calculate the mean of the template image: in, 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 the local area of ​​the target image: in, is the average value of the window corresponding to the target image at position (u1, v1), and I(x+u1,y+v1) is the pixel value of the target image at position (u1, v1).

[0110] In order to calculate the correlation, remove the mean of the image, then we have:

[0111] To measure the similarity between two images, perform cross-correlation:

[0112] To measure the intensity variation of the image, calculate its standard deviation:

[0113]

[0114] Among them, σ T represents the standard deviation of the template image, Represents the standard deviation of the local area of ​​the target image.

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

[0116]

[0117] Among them, R(u1,v1) is the similarity between the source image and the template image, which is a value between -1 and 1, 1 means a perfect match, -1 means a complete mismatch, and 0 means no correlation.

[0118] Step 402, according to the position of each feature point of the potential dynamic target in each frame of the color image, a dynamic time warping method is used to determine the motion trajectory of each feature point of the potential dynamic target.

[0119] Dynamic time warping is a method to measure the similarity between two time series. After completing the tracking and matching of the target in the environment (i.e., completing the optical flow tracking and template matching), the motion trajectory of the target is formed by recording the position of the target area in each frame (mainly the bounding box information). The motion trajectory of the target can be used to determine whether the target is moving. After determining the motion state of the target, it is also necessary to determine the motion state of the feature points in the target area. Therefore, in order to determine the motion trajectory of the feature points and facilitate the subsequent elimination process, the motion trajectory of the feature points in the obtained dynamic target area is analyzed using time dynamic warping.

[0120] The motion trajectory of the feature point can be expressed as: T k ={(x k1 ,y k1 ),(x k2 ,y k2 ),…,(x kn ,y kn )}; where T k Represents the feature point P k The trajectory of (x kn ,y kn ) indicates P k The coordinates in the nth frame image.

[0121] Define the expected motion trajectory as: Texpected ={(x e1 ,y e1 ),(x e2 ,y e2 ),...,(x en ,y en )}; where 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 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 feature point and the expected motion trajectory is as follows:

[0124] (1) Calculate the distance between the motion trajectory and the expected motion trajectory. In particular, for any two points (x k1 ,y k1 ) and (x e1 ,y e1 ), using Euclidean distance it can be expressed as: Among them, 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 in 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 the similarity by the minimum cumulative distance: D TW (T k ,T expected )=D[n][m]; where D TW (T k ,T expected ) is the dynamic time warping distance, and D[n][m] is the element in the distance matrix.

[0127] (4) Normalize the dynamic time warping distance to calculate the similarity score: Among them, 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 whose similarity is less than a set similarity threshold, and combine the feature points of the static target to obtain static feature points.

[0129] If the similarity of a feature point is less than the set similarity threshold, it means that its motion trajectory does not match the expected motion trajectory, and it is marked as a dynamic feature point. Combined with the obtained mask information, the dynamic feature points in the mask area on the dynamic target are removed. For feature points whose similarity is greater than or equal to the set similarity threshold, it means that its motion trajectory matches the expected motion trajectory and is a static feature point, which is added to the 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] The map constructed by the traditional SLAM algorithm is a sparse feature point map. It only uses the geometric information of the space, so it can neither meet the needs of the robot to perform tasks nor fully reflect the true original appearance of the environment. At the same time, the map constructed by the traditional SLAM algorithm in a dynamic environment will be interfered by the dynamic targets in the environment, resulting in a large number of overlapping map points on the map, and ultimately leading to ghosting problems. The present application removes the interference of dynamic targets in the environment by screening and updating key frames, combining the screened and updated key frames with the semantic information obtained by the dynamic target recognition model to generate a static point cloud, and then splicing and filtering the point cloud to generate a dense point cloud map of the environment. The specific process is as follows: Figure 7 As shown in the figure, the dense point cloud map can reflect the real information of the environment in detail, reduce the ghosting and offset of the map, and improve the readability of the map.

[0132] Step 105 includes the following 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 a reference frame image, wherein the reference frame image is a frame image preceding the current frame image.

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

[0135] Since too many keyframes are inserted, information redundancy will occur, which will eventually lead to accumulation and stratification of point clouds in the constructed map. Therefore, in order to improve the quality of the map and ensure that as few new keyframes are inserted as possible, this application uses a keyframe screening and updating strategy to process the input keyframes.

[0136] like Figure 8As shown in the figure, for the input key frame, 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 key frame is updated. The key frame screening and updating 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] Wherein, 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. represents the variance of the reference frame image, Represents the variance of the current frame image, σ 12 is the covariance of the two frames, C1 and C2 are constants. According to the similarity calculation results, the key frames with higher similarity are discarded and the image sequence is updated.

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

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

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

[0142] The camera model is Fig. 9 As shown, the correspondence between a spatial point [x”, y”, z”] and its pixel coordinates [u”, v”, d”] in the image is:

[0143]

[0144] Among them, d" is the depth information, f x is the focal length of the camera on the x-axis, f y is the focal length of the camera on the y axis, c x is the aperture center of the camera on the x-axis, c y is the aperture center of the camera on the y-axis, and s is the scaling factor of the depth map.

[0145] On the contrary, we have:

[0146]

[0147] The single-frame point cloud is obtained by using the camera internal parameters. The main coordinate calculation formula is:

[0148]

[0149] Among them, C” is the intrinsic parameter matrix of the camera, R” is the rotation matrix, and t” is the displacement vector.

[0150] Fig. 9 In the above equation, P represents the spatial coordinate point in the real world; p1 and q1 represent the pixel coordinate points projected by P on the image plane; Q represents the spatial coordinate point in the real world; Z O Represents the z-axis of the real-world coordinate system.

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

[0152] Specifically, the generated single-frame point clouds are spliced ​​to obtain local point clouds. Since the local point clouds have abnormal points and noise points, they need to be filtered to remove outliers and similar points in overlapping areas, reduce the overlap of point clouds, and improve the quality of point clouds.

[0153] Among them, a statistical filter is used to remove outliers in the point cloud, and a voxel filter is used to filter out similar points in the overlapping part of the point cloud to ensure that there is only one point in a voxel. The main expression formula is:

[0154]

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

[0156] Step 106, discretize the dense point cloud map into a two-dimensional grid and construct a grid map to guide the robot to perform the navigation task.

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

[0158] In an exemplary embodiment, step 106 includes the following steps 601 to 604 .

[0159] Step 601: extract three-dimensional 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 ) is the coordinate of the point cloud data.

[0160] Step 602, determine the grid size.

[0161] Step 603, traverse the three-dimensional point cloud data, and determine the boundary of the grid map according to the minimum coordinate and the maximum coordinate in the three-dimensional point cloud data:

[0162] Step 604: construct a grid map according to the three-dimensional point cloud data, the grid size and the boundary of the grid map.

[0163] Specifically, according to the grid map boundary and grid size, a two-dimensional array is initialized to represent the grid map. The initial value of each element in the array is 0, indicating that the area is not occupied. The three-dimensional point cloud data is traversed and its position in the grid map is calculated to fill the map. The corresponding grid cell value is updated to 1, indicating that the area is occupied. The construction process of the grid map is as follows: Fig.10 shown.

[0164] For each point (x a ,y a ,z a ) calculates its index in the grid map:

[0165]

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

[0167] Preferably, the method for synchronous positioning and mapping of a robot in a dynamic environment further comprises: step 107, performing an erosion operation on the grid map to clean the grid map and improve the quality.

[0168] The erosion operation can make the map smoother. The principle is to use a small matrix called a structural element, slide on the image and check whether the corresponding position of the pixel and the structural element meets a certain condition (usually an "and" operation). If all pixel values ​​meet the conditions, the center pixel value remains unchanged, otherwise it is eroded to 0. This process can be used to eliminate small noise, separate touching objects, and shrink objects. The erosion operation can be expressed as:

[0169]

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

[0171] This application uses a dynamic target recognition model to detect static targets and potential dynamic targets in the environment, as well as the semantic information corresponding to the targets, and distinguishes the feature points on the targets into static feature points and potential dynamic feature points, so that the SLAM algorithm has the ability to identify targets in the environment; uses a motion consistency detection model to further judge potential dynamic targets, and removes feature points that do not match the expected motion based on the motion trajectory of the feature points, thereby reducing the impact of dynamic targets on positioning accuracy and stability and improving the performance of the robot in a dynamic environment; combines the screened and updated key frames with the target semantic information obtained by the dynamic target recognition model to generate a single-frame point cloud, and constructs a dense point cloud map by filtering and splicing the point cloud; discretizes the three-dimensional point cloud data in the dense point cloud map into a two-dimensional grid, and uses rasterization technology to provide a clear occupancy status representation while maintaining environmental information, thereby constructing a raster map.

[0172] This application can eliminate the interference of dynamic targets on the synchronous positioning and map construction method in actual dynamic scenes, realize the real-time positioning of the robot, and improve the readability of the map. It improves the positioning accuracy, stability and mapping performance of the robot in a dynamic environment. It provides basic technical support for applications such as robot navigation, path planning and environmental perception.

[0173] Based on the same inventive concept, the embodiment of the present application also provides a system for synchronous positioning and mapping of robots in dynamic environments for implementing the above-mentioned method for synchronous positioning and mapping of robots in dynamic environments. The implementation scheme for solving the problem provided by the system is similar to the implementation scheme recorded in the above-mentioned method, so the specific limitations in one or more embodiments of the system for synchronous positioning and mapping of robots in dynamic environments provided below can be found in the above-mentioned limitations on the method for synchronous positioning and mapping of robots in dynamic environments, and will not be repeated here.

[0174] In an exemplary embodiment, Fig.11 As shown, the robot synchronous positioning and mapping 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 construction module 206.

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

[0176] The feature extraction module 202 is used to 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 according to 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, and 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 according to the color image in the image sequence, remove the feature points whose motion trajectory does not conform to the expected motion trajectory, and obtain static feature points in combination with the feature points of the static target.

[0179] The point cloud map generation module 205 is used to generate a dense point cloud map according to 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 and construct a grid map to guide the robot to perform a navigation task.

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

[0182] Among them, the operating system of Raspberry Pi is Ubuntu 18.04, and Raspberry Pi includes a processor and a memory. The memory mainly stores executable program code and saves temporary data required by the processor to execute executable code. The processor includes a graphics processing unit (GPU) and a central processing unit (CPU). The GPU performs parallel computing tasks to speed up the reasoning speed of the dynamic target recognition model. The CPU is responsible for tasks such as basic instruction sending and receiving, controlling the overall process, and communicating with external devices.

[0183] In an exemplary embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps in the above-mentioned method embodiments when executing the computer program.

[0184] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program, and when the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[0185] In an exemplary embodiment, a computer program product is provided, including a computer program, and when the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[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, stored data, displayed data, 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 relevant data must comply with relevant regulations.

[0187] In this application, all actions to obtain signals, information or data are carried out in compliance with the relevant data protection laws and policies of the country where they are located and with the authorization given by the owner of the corresponding device.

[0188] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0189] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, 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 article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A method for synchronous positioning and mapping of a robot in a dynamic environment, characterized in that: The method for synchronous positioning and map construction of a robot in a dynamic environment comprises: Acquire an image sequence of the robot's surrounding environment; the image sequence includes a continuous multi-frame color image and a continuous multi-frame depth image; Extracting features from the color images in the image sequence to obtain a plurality of feature points, coordinate information and color information of each feature point, and determining depth information of each feature point according to the depth image in the image sequence; Performing target recognition on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment; Determine the motion trajectory of the feature points of the potential dynamic target according to the color image in the image sequence, remove the feature points whose motion trajectory does not conform to the expected motion trajectory, and obtain static feature points by combining the feature points of the static target; Generate a dense point cloud map according to the depth information, coordinate information and color information of the static feature points; The dense point cloud map is discretized into a two-dimensional grid and a grid map is constructed to guide the robot to perform a navigation task.

2. The method for synchronous positioning and mapping of robots in a dynamic environment according to claim 1, characterized in that: Get an image sequence of the robot's surroundings, including: An RGB-D camera is used to collect continuous multi-frame color images and continuous multi-frame depth images of the robot's surrounding environment; Each frame of color image is processed by denoising and image enhancement in turn, and each frame of depth image is processed by denoising and filling in turn to obtain an image sequence of the robot's surrounding environment.

3. The method for synchronous positioning and mapping of robots in a dynamic environment according to claim 1, characterized in that: Performing target recognition on the color images in the image sequence to determine static targets and potential dynamic targets in the robot's surrounding environment, specifically includes: For any color image frame 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 a bounding box, a label and a mask; the label is static or dynamic; the dynamic target recognition model is obtained by pre-training a 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; According to the label of each target, multiple targets are divided into static targets and potential dynamic targets.

4. The method for synchronous positioning and mapping of robots in a dynamic environment according to claim 1, characterized in that: Determining the motion trajectory of the feature points of the potential dynamic target according to the color image in the image sequence, removing the feature points whose motion trajectory does not conform to the expected motion trajectory, and combining the feature points of the static target to obtain the static feature points specifically includes: According to the image sequence, dense optical flow method and template matching technology are used to track and match the potential dynamic target to obtain the position of each feature point of the potential dynamic target in each frame of color image; According to the position of each feature point of the potential dynamic target in each frame of the color image, a dynamic time warping method is used to determine the motion trajectory of each feature point of the potential dynamic target; Calculate the similarity between the motion trajectory of each feature point of the potential dynamic target and the expected motion trajectory; Feature points with similarity less than a set similarity threshold are removed, and the static feature points are obtained by combining the feature points of the static target.

5. The method for synchronous positioning and mapping of robots in a dynamic environment according to claim 1, characterized in that: Generating a dense point cloud map according to the depth information, coordinate information and color information of the static feature points, specifically including: For any frame image in the image sequence, a structural similarity algorithm is used to calculate the similarity between the current frame image and a reference frame image; the reference frame image is a frame image previous to the current frame image; The current frame image whose similarity is greater than a set threshold is removed from the image sequence to obtain a processed image sequence; the processed image sequence includes multiple frames of key color images and multiple frames of key depth images; For any key color image in the processed image sequence, determine the coordinate information, color information and depth information of the static feature point according to the key color image and the corresponding key depth image; Determine a single-frame point cloud according to the coordinate information, color information and depth information of the static feature points; All single-frame point clouds are stitched together to obtain a dense point cloud map.

6. The method for synchronous positioning and mapping of robots in a dynamic environment according to claim 1, characterized in that: Discretizing the dense point cloud map into a two-dimensional grid and constructing a grid map specifically includes: Extracting three-dimensional point cloud data from the dense point cloud map; Determine the grid size; Traversing the three-dimensional point cloud data, and determining the boundary of the grid map according to the minimum coordinate and the maximum coordinate in the three-dimensional point cloud data; A grid map is constructed according to the three-dimensional point cloud data, the grid size and the boundary of the grid map.

7. The method for synchronous positioning and mapping of robots in a dynamic environment according to claim 1, characterized in that: The method for synchronous positioning and mapping of a robot in a dynamic environment also includes: An erosion operation is performed on the grid map.

8. A system for synchronous positioning and mapping of a robot in a dynamic environment, applied to the method for synchronous positioning and mapping of a robot in a dynamic environment as claimed in any one of claims 1 to 7, characterized in that: The robot synchronous positioning and map building system in a dynamic environment includes: An image acquisition module is used to acquire an image sequence of the robot's surrounding environment; the image sequence includes a continuous multi-frame color image and a continuous multi-frame depth image; A feature extraction module, used to extract features from the color image 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 according to the depth image in the image sequence; A 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; A feature point determination module is used to determine the motion trajectory of the feature points of the potential dynamic target according to the color image in the image sequence, remove the feature points whose motion trajectory does not conform to the expected motion trajectory, and obtain static feature points in combination with the feature points of the static target; A point cloud map generation module, used to generate a dense point cloud map according to 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 to perform navigation tasks.

9. A computer device comprising: 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 method for synchronous positioning and mapping of a robot in a dynamic environment as described in any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method for synchronous positioning and mapping of a robot in a dynamic environment according to any one of claims 1 to 7 is implemented.

Images