Mobile robot positioning and mapping method based on dynamic environment

Abstract

The invention discloses a mobile robot positioning and mapping method based on a dynamic environment. The method comprises the following steps: shooting an RGB image and a corresponding depth image of a scene to be detected in real time, and obtaining feature points in the image; constructing a lightweight YOLOv8s model, and obtaining dynamic target feature points, potential moving target feature points and static target feature points; performing accurate classification on the dynamic feature points and the static feature points in the target detection frame by using a sparse optical flow method; static feature points are reserved; generating a key frame; and establishing a global point cloud map by using the key frame and combining the depth image and the static feature points. Compared with the prior art, the method has the following advantages that the deep learning method and the traditional visual SLAM system are combined according to the requirements of the mobile robot for accurate positioning and map construction in a dynamic environment, so that the mobile robot can be better positioned in a dynamic scene, and a global point cloud map is constructed, so that subsequent work can be completed conveniently.

Description

Technical Field

[0001] The present invention belongs to the technical field of visual simultaneous localization and mapping, and in particular relates to a mobile robot localization and mapping method based on a dynamic environment. Background Art

[0002] The visual SLAM system is a synchronous positioning and mapping technology based on cameras as sensors. In unknown environments, it tracks the extracted feature points to solve the position and three-dimensional space map between camera frames. It has been widely used in platforms such as unmanned driving, mobile robots and drones.

[0003] Traditional visual SLAM systems are all operated under the assumption of static scenes. However, in the actual environment, there are a large number of constantly moving objects, such as people, moving vehicles and animals. These dynamic objects will cause large deviations in the robot's pose estimation, thereby causing the robot to fail in positioning. In addition, the traditional visual SLAM system constructs a sparse point cloud map, which cannot be used for subsequent navigation and path planning. Summary of the invention

[0004] In order to solve the above problems, the object of the present invention is to provide a mobile robot positioning and mapping method based on a dynamic environment.

[0005] In order to achieve the above object, the mobile robot positioning and mapping method based on a dynamic environment provided by the present invention comprises the following steps performed in sequence:

[0006] 1) Use the RGB-D camera installed on the mobile robot to capture the RGB image and corresponding depth image of the scene to be detected in real time, and transmit them to the ORB-SLAM3 system on the mobile robot for ORB feature point extraction to obtain feature points in the image;

[0007] 2) Build a lightweight YOLOv8s model and embed it into the above-mentioned ORB-SLAM3 system, then use the lightweight YOLOv8s model to obtain the target detection frame in the current RGB image, and then preliminarily divide the prior objects in the target detection frame into three categories: dynamic targets, potential moving targets, and static targets, and obtain dynamic target feature points, potential moving target feature points, and static target feature points;

[0008] 3) Using the sparse optical flow method, the optical flow vectors of the dynamic target feature points and potential moving target feature points and the average optical flow vector of the static target feature points are calculated, and then the dynamic feature points and static feature points in the target detection frame are accurately classified;

[0009] 4) Eliminate the above dynamic feature point set and only retain the static feature points in the static feature point set for feature matching and pose estimation, so as to achieve the purpose of positioning;

[0010] 5) The ORB-SLAM3 system automatically generates key frames, and uses the key frames in combination with the above-mentioned depth images and static feature points to establish a global point cloud map, thereby completing the mapping task.

[0011] In step 1), the RGB image and the corresponding depth image of the scene to be detected are captured in real time using an RGB-D camera installed on the mobile robot, and are transmitted to the ORB-SLAM3 system on the mobile robot for ORB feature point extraction. The specific method for obtaining the feature points in the image is:

[0012] 1.1) Install an RGB-D camera on the mobile robot as a data acquisition device. The RGB-D camera is installed in a horizontal position, and the shooting direction of the lens is consistent with the forward direction of the mobile robot, so that the central axis of the RGB-D camera coincides with the central axis of the mobile robot;

[0013] 1.2) Use the RGB-D camera to obtain the RGB image frame and the corresponding depth image of the scene to be detected, and transmit the acquired image to the ORB-SLAM3 system on the mobile robot in real time to extract ORB feature points and obtain the feature points in the image.

[0014] In step 2), the lightweight YOLOv8s model is constructed and embedded in the above-mentioned ORB-SLAM3 system, and then the lightweight YOLOv8s model is used to obtain the target detection frame in the current RGB image, and then the prior objects in the target detection frame are preliminarily divided into three categories: dynamic targets, potential moving targets and static targets. The specific method for obtaining dynamic target feature points, potential moving target feature points and static target feature points is:

[0015] 2.1) Use FasterNet lightweight backbone network to replace the backbone network of the YOLOv8s model;

[0016] 2.2) Introduce GSConv module and VoV-GSCSP module in Neck layer;

[0017] 2.3) Set up a PConv detection head with shared parameters to build a lightweight YOLOv8s model;

[0018] 2.4) Use the above lightweight YOLOv8s model to detect the current RGB image in real time and obtain the target detection frame;

[0019] 2.5) Preliminary classification of objects in the above target detection frame is performed based on the prior knowledge of the lightweight YOLOv8s model. Frequently moving objects such as people, animals, and sweeping robots are classified as dynamic targets; objects such as books and water cups that may move due to human movement are classified as potential moving targets; and other objects are classified as static targets;

[0020] 2.6) At the same time, obtain the dynamic target feature point M d , potential moving target feature points M p and static target feature points.

[0021] In step 3), the specific method of using the sparse optical flow method to calculate the optical flow vectors of the dynamic target feature points and the potential moving target feature points and the average optical flow vector of the static target feature points, and then accurately classifying the dynamic feature points and the static feature points in the target detection frame is:

[0022] 3.1) Calculate the dynamic target feature points M using the sparse optical flow method d and potential moving target feature points M p The optical flow vector (u,v);

[0023] 3.2) Calculate the optical flow vector of the static target feature point, and then substitute it into the following formula to calculate the average optical flow vector (U, V) of the static target feature point;

[0024]

[0025] 3.3) Set a dynamic feature point set M and a static feature point set S; then based on the above optical flow vector (u, v) and the average optical flow vector (U, V), use the following formula to accurately classify all feature points in the target detection frame. If the distance change ΔX of the same feature point between adjacent frames is greater than the optical flow threshold L, it is proved that the feature point is a dynamic feature point and is placed in the dynamic feature point set M; if it is less than the optical flow threshold L, it is proved that the feature point is a static feature point and is placed in the static feature point set S;

[0026]

[0027] In step 5), the ORB-SLAM3 system automatically generates key frames, and uses the key frames and the above-mentioned depth image and static feature points to establish a global point cloud map, thereby completing the specific method of the mapping task as follows:

[0028] 5.1) The ORB-SLAM3 system automatically generates key frames, and then uses the PCL point cloud library to convert each pixel in the key frame into a coordinate point in the three-dimensional space with its corresponding depth image;

[0029] 5.2) Using the internal and external parameters of the RGB-D camera, the two-dimensional pixel coordinates of the static feature points in the key frame are converted into three-dimensional space coordinates, and the point cloud of the current key frame is merged with the existing historical key frame point cloud;

[0030] 5.3) Using the pose information of the key frames, the point cloud of each key frame is spliced ​​according to its actual position in the world coordinate system to form a global point cloud map containing all key frame information, and BA optimization is performed globally.

[0031] The mobile robot positioning and mapping method based on dynamic environment provided by the present invention has the following advantages:

[0032] Aiming at the needs of mobile robots for precise positioning and map construction in dynamic environments, the present invention combines deep learning methods with traditional visual SLAM systems, so that mobile robots can better position themselves in dynamic scenes and build global point cloud maps to facilitate subsequent work. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 A flow chart of the mobile robot positioning and mapping method based on a dynamic environment provided by the present invention;

[0034] Figure 2 This is a diagram of the lightweight YOLOv8s model in the present invention;

[0035] Figure 3 (a) and (b) are the absolute pose differences of ORB-SLAM3 and the proposed method on the TUM dataset, respectively;

[0036] Figure 4 (a) and (b) are respectively the effect of removing dynamic feature points from the TUM dataset and the mapping effect of constructing a global point cloud map using the method of the present invention. DETAILED DESCRIPTION

[0037] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0038] like Figure 1 As shown, the mobile robot positioning and mapping method based on a dynamic environment provided by the present invention includes the following steps performed in sequence:

[0039] 1) Use the RGB-D camera installed on the mobile robot to capture the RGB image and corresponding depth image of the scene to be detected in real time, and transmit them to the ORB-SLAM3 system on the mobile robot for ORB feature point extraction to obtain feature points in the image;

[0040] The specific method is:

[0041] 1.1) Install an RGB-D camera on the mobile robot as a data acquisition device. The RGB-D camera is installed in a horizontal position, and the shooting direction of the lens is consistent with the forward direction of the mobile robot, so that the central axis of the RGB-D camera coincides with the central axis of the mobile robot;

[0042] 1.2) Use the RGB-D camera to obtain the RGB image frame and the corresponding depth image of the scene to be detected, and transmit the acquired image to the ORB-SLAM3 system on the mobile robot in real time to extract ORB feature points and obtain the feature points in the image.

[0043] 2) Build Figure 2 The lightweight YOLOv8s model shown in the figure is embedded in the above-mentioned ORB-SLAM3 system, and then the lightweight YOLOv8s model is used to obtain the target detection frame in the current RGB image, and then the prior objects in the target detection frame are preliminarily divided into three categories: dynamic targets, potential moving targets and static targets, and the dynamic target feature points, potential moving target feature points and static target feature points are obtained at the same time;

[0044] The specific method is:

[0045] YOLOv8s is the latest YOLO model. It is based on the iterative construction between the YOLO series and integrates many improvements, which greatly improves the detection accuracy and speed. It can be used for object detection, image classification and instance segmentation tasks. Based on the real-time requirements of the visual SLAM system, the present invention is used for object detection and image classification tasks.

[0046] 2.1) FasterNet lightweight backbone network is used to replace the backbone network of the YOLOv8s model. FasterNet combines partial convolution (PConv) and point-by-point convolution to reduce redundant calculations. The amount of PConv calculation is lower than that of conventional convolution, but higher than that of depthwise separable convolution and group convolution. PConv can make full use of computing resources and is also effective in extracting spatial features.

[0047] 2.2) Introducing the GSConv module and the VoV-GSCSP module in the Neck layer, the GSConv module is used to process the spliced ​​feature map just right, with less redundant information, no need for compression, and better compatibility with the attention module to optimize feature fusion and attention effects; the VoV-GSCSP module is used to reduce the computational and network complexity, achieving the goal of lightweight while maintaining model accuracy;

[0048] 2.3) Based on the fact that the computational complexity of the Head part of YOLOv8s increases a lot, the present invention refers to the shared parameter detection head idea of ​​RetinaNet and sets a shared parameter PConv detection head to achieve the purpose of lightweight; thus, a lightweight YOLOv8s model is constructed;

[0049] 2.4) Use the above lightweight YOLOv8s model to detect the current RGB image in real time and obtain the target detection frame;

[0050] 2.5) Preliminary classification of objects in the above target detection frame is performed based on the prior knowledge of the lightweight YOLOv8s model. Frequently moving objects such as people, animals, and sweeping robots are classified as dynamic targets; objects such as books and water cups that may move due to human movement are classified as potential moving targets; and other objects are classified as static targets;

[0051] 2.6) At the same time, obtain the dynamic target feature point M d , potential moving target feature points M p and static target feature points.

[0052] 3) Using the sparse (LK) optical flow method, the optical flow vectors of the dynamic target feature points and potential moving target feature points and the average optical flow vector of the static target feature points are calculated, and then the dynamic feature points and static feature points in the target detection frame are accurately classified;

[0053] In order to ensure the real-time requirements of the visual SLAM system, the present invention uses a sparse optical flow method with a small amount of calculation to screen the feature points:

[0054] The sparse optical flow method is based on the following three assumptions:

[0055] Assumption 1: Grayscale is constant: It is assumed that the grayscale value of a pixel on the same object will not change due to other factors;

[0056] Assumption 2: Spatial consistency: It is assumed that the motions of adjacent pixels in the image are similar and the distances between their projections onto the image are also close;

[0057] Assumption 3: Time continuity: It is assumed that in each calculation, the target position will not change dramatically due to the change of time, and the change of the corresponding position of the pixel points of the moving target between adjacent frames is relatively small;

[0058] Assume that at time t, the coordinates of the feature point in the image are (x, y) and the grayscale value is I(x, y, t);

[0059] From assumption 1, we can get:

[0060] I(x,y,t)=I(x+dx,y+dy,t+dt) (1)

[0061] In the formula, I(x,y,t) represents the grayscale value of the feature point at the coordinate (x,y) at time t, and I(x+dx,y+dy,t+dt) represents the grayscale value of the feature point at the coordinate (x+dx,y+dy) at time t+dt;

[0062] According to Assumption 3, the Taylor series expansion is performed on the right side of equation (1), retaining the first-order term, and we get:

[0063]

[0064] Add equation (1) and equation (2) together and divide both sides by dt to obtain:

[0065]

[0066] In the formula, and I x and I y , respectively represent the grayscale gradient of the feature point in the x-axis and y-axis directions, and divide both sides by dt to get:

[0067]

[0068] In the formula, dx / dt and dy / dt are denoted as u and v, which represent the speed of the feature point in the x-axis and y-axis directions respectively. For I t , represents the change of feature points over time, then:

[0069]

[0070] To calculate the optical flow vector of the feature point at the coordinate (x, y), a 5×5 pixel window is selected with the feature point as the center. According to Assumption 2, there are 25 pixels with the same motion in the window, thus converting Equation (5) into an overdetermined equation about u and v, as shown in Equation (6):

[0071]

[0072] remember:

[0073]

[0074] Where A is the coefficient matrix;

[0075] Then formula (6) can be rewritten as formula (8), and we get:

[0076]

[0077] When the coefficient matrix A is overdetermined, the least squares method is used to calculate u and v, and the results are as follows:

[0078] [uv] T =-(A T A) -1 A T b (9)

[0079] Therefore, the sparse optical flow method can be used to obtain the optical flow vector of the feature point at the coordinate (x, y) at time t, which is used to track and estimate the position of a feature point in the image.

[0080] The specific method is:

[0081] 3.1) Calculate the dynamic target feature points M using the sparse optical flow method d and potential moving target feature points M p The optical flow vector (u,v);

[0082] 3.2) Calculate the optical flow vector of the static target feature point, and then substitute it into the following formula to calculate the average optical flow vector (U, V) of the static target feature point;

[0083]

[0084] 3.3) Set a dynamic feature point set M and a static feature point set S; then based on the above optical flow vector (u, v) and the average optical flow vector (U, V), use the following formula to accurately classify all feature points in the target detection frame. If the distance change ΔX of the same feature point between adjacent frames is greater than the optical flow threshold L, it is proved that the feature point is a dynamic feature point and is placed in the dynamic feature point set M; if it is less than the optical flow threshold L, it is proved that the feature point is a static feature point and is placed in the static feature point set S;

[0085]

[0086] 4) Eliminate the above dynamic feature point set M and only retain the static feature points in the static feature point set S for feature matching and pose estimation, so as to achieve the purpose of positioning;

[0087] 5) The ORB-SLAM3 system automatically generates key frames, and uses the key frames in combination with the above-mentioned depth images and static feature points to establish a global point cloud map, thereby completing the mapping task.

[0088] The key frame is the most representative frame of images that contains more image information selected from several consecutive frames of images processed by the ORB-SLAM3 system. It has the characteristics of reducing information redundancy, reducing calculation burden and error accumulation, and ensuring image information quality.

[0089] The specific method is:

[0090] 5.1) The ORB-SLAM3 system automatically generates key frames, and then uses the PCL point cloud library to convert each pixel in the key frame into a coordinate point in the three-dimensional space with its corresponding depth image;

[0091] 5.2) Using the internal and external parameters of the RGB-D camera, the two-dimensional pixel coordinates of the static feature points in the key frame are converted into three-dimensional space coordinates, and the point cloud of the current key frame is merged with the existing historical key frame point cloud;

[0092] 5.3) Using the pose information of the key frames, the point cloud of each key frame is spliced ​​according to its actual position in the world coordinate system to form a global point cloud map containing all key frame information, and BA optimization is performed globally.

[0093] Simulation experiment

[0094] The simulation experiment of the mobile robot positioning and mapping method based on dynamic environment provided by the present invention is: Ubuntu20.04 operating system, CPU processor is AMD R7 5800H, and display adapter is NVIDIA GeFore RTX 3060GPU. The public data set TUM RGB-D data set is selected for experiment, and the freiburg3_walking_xyz sequence is selected for evaluation.

[0095] In order to verify the performance of the method of the present invention in a dynamic environment, ORB-SLAM3 and the method of the present invention are used to conduct experimental comparisons in the data set to further verify the effect of the method:

[0096] Figure 3 is the absolute pose error comparison in the TUM dataset, where Figure 3 (a) represents the root mean square error of the absolute pose error of ORB-SLAM3; Figure 3 (b) represents the root mean square difference of the absolute pose error of the method of the present invention; Table 1 compares the root mean square difference of the absolute pose error of the two methods in a dynamic environment in the form of data. Experiments show that the method of the present invention improves the positioning accuracy of the visual SLAM system in a dynamic environment.

[0097] Table 1 Root mean square error (RMSE) of absolute pose error between ORB-SLAM3 and the method of the present invention

[0098] ORB-SLAM3 Method of the present invention fr3_walking_xyz 0.597946 0.132817

[0099] It can be seen from this that the present invention combines the lightweight YOLOv8s model with the visual ORB-SLAM3 system, and uses the LK optical flow method to remove dynamic feature points while retaining the static information in the image as much as possible, thereby effectively improving the positioning accuracy of the visual SLAM system in a dynamic environment.

[0100] Figure 4 (a) and (b) are respectively the effect of removing dynamic feature points from the TUM dataset and the mapping effect of constructing a global point cloud map using the method of the present invention.

Claims

1. A mobile robot positioning and mapping method based on a dynamic environment, characterized by: The method comprises the following steps performed in sequence: 1) Use the RGB-D camera installed on the mobile robot to capture the RGB image and corresponding depth image of the scene to be detected in real time, and transmit them to the ORB-SLAM3 system on the mobile robot for ORB feature point extraction to obtain feature points in the image; 2) Build a lightweight YOLOv8s model and embed it into the above-mentioned ORB-SLAM3 system, then use the lightweight YOLOv8s model to obtain the target detection frame in the current RGB image, and then preliminarily divide the prior objects in the target detection frame into three categories: dynamic targets, potential moving targets, and static targets, and obtain dynamic target feature points, potential moving target feature points, and static target feature points; 3) Using the sparse optical flow method, the optical flow vectors of the dynamic target feature points and potential moving target feature points and the average optical flow vector of the static target feature points are calculated, and then the dynamic feature points and static feature points in the target detection frame are accurately classified; 4) Eliminate the above dynamic feature point set and only retain the static feature points in the static feature point set for feature matching and pose estimation, so as to achieve the purpose of positioning; 5) The ORB-SLAM3 system automatically generates key frames, and uses the key frames in combination with the above-mentioned depth images and static feature points to establish a global point cloud map, thereby completing the mapping task.

2. The method for positioning and mapping a mobile robot based on a dynamic environment according to claim 1, characterized in that: In step 1), the RGB image and the corresponding depth image of the scene to be detected are captured in real time using an RGB-D camera installed on the mobile robot, and are transmitted to the ORB-SLAM3 system on the mobile robot for ORB feature point extraction. The specific method for obtaining the feature points in the image is: 1.1) Install an RGB-D camera on the mobile robot as a data acquisition device. The RGB-D camera is installed in a horizontal position, and the shooting direction of the lens is consistent with the forward direction of the mobile robot, so that the central axis of the RGB-D camera coincides with the central axis of the mobile robot; 1.2) Use the RGB-D camera to obtain the RGB image frame and the corresponding depth image of the scene to be detected, and transmit the acquired image to the ORB-SLAM3 system on the mobile robot in real time to extract ORB feature points and obtain the feature points in the image.

3. The method for positioning and mapping a mobile robot based on a dynamic environment according to claim 1, characterized in that: In step 2), the lightweight YOLOv8s model is constructed and embedded in the above-mentioned ORB-SLAM3 system, and then the lightweight YOLOv8s model is used to obtain the target detection frame in the current RGB image, and then the prior objects in the target detection frame are preliminarily divided into three categories: dynamic targets, potential moving targets and static targets. The specific method for obtaining dynamic target feature points, potential moving target feature points and static target feature points is: 2.1) Use FasterNet lightweight backbone network to replace the backbone network of the YOLOv8s model; 2.2) Introduce GSConv module and VoV-GSCSP module in Neck layer; 2.3) Set up a PConv detection head with shared parameters to build a lightweight YOLOv8s model; 2.4) Use the above lightweight YOLOv8s model to detect the current RGB image in real time and obtain the target detection frame; 2.5) Preliminary classification of objects in the above target detection frame is performed based on the prior knowledge of the lightweight YOLOv8s model. Frequently moving objects such as people, animals, and sweeping robots are classified as dynamic targets; objects such as books and water cups that may move due to human movement are classified as potential moving targets; and other objects are classified as static targets; 2.6) At the same time, obtain the dynamic target feature point M d , potential moving target feature points M p and static target feature points.

4. The method for positioning and mapping a mobile robot based on a dynamic environment according to claim 1, characterized in that: In step 3), the specific method of using the sparse optical flow method to calculate the optical flow vectors of the dynamic target feature points and the potential moving target feature points and the average optical flow vector of the static target feature points, and then accurately classifying the dynamic feature points and the static feature points in the target detection frame is: 3.1) Calculate the dynamic target feature points M using the sparse optical flow method d and potential moving target feature points M p The optical flow vector (u,v); 3.2) Calculate the optical flow vector of the static target feature point, and then substitute it into the following formula to calculate the average optical flow vector (U, V) of the static target feature point; 3.3) Set a dynamic feature point set M and a static feature point set S; then based on the above optical flow vector (u, v) and the average optical flow vector (U, V), use the following formula to accurately classify all feature points in the target detection frame. If the distance change ΔX of the same feature point between adjacent frames is greater than the optical flow threshold L, it is proved that the feature point is a dynamic feature point and is placed in the dynamic feature point set M; if it is less than the optical flow threshold L, it is proved that the feature point is a static feature point and is placed in the static feature point set S; 5. The method for positioning and mapping a mobile robot based on a dynamic environment according to claim 1, characterized in that: In step 5), the ORB-SLAM3 system automatically generates key frames, and uses the key frames and the above-mentioned depth image and static feature points to establish a global point cloud map, thereby completing the specific method of the mapping task as follows: 5.1) The ORB-SLAM3 system automatically generates key frames, and then uses the PCL point cloud library to convert each pixel in the key frame into a coordinate point in the three-dimensional space with its corresponding depth image; 5.2) Using the internal and external parameters of the RGB-D camera, the two-dimensional pixel coordinates of the static feature points in the key frame are converted into three-dimensional space coordinates, and the point cloud of the current key frame is merged with the existing historical key frame point cloud; 5.3) Using the pose information of the key frames, the point cloud of each key frame is spliced ​​according to its actual position in the world coordinate system to form a global point cloud map containing all key frame information, and BA optimization is performed globally.

Images