A Design Method for Mobile Robot Visual Odometry in a Dynamic Scene

Abstract

The present invention discloses a design method for a mobile robot visual odometer in a dynamic scenario, including: obtaining real-time image information through an Intel RealSense depth camera and preprocessing it; using an adaptive threshold method proposed based on the ORB algorithm to more comprehensively extract image feature points; using the MS_COCO dataset as a sample to train the YOLACT network and perform instance segmentation on the image to obtain image semantic information; combining the image semantic information and the L-K optical flow method to roughly filter out dynamic feature points; estimating the fundamental matrix F based on the progressive consistent sampling algorithm, and then calculating the epipolar distance and precisely filtering out dynamic feature points; finally, selecting the filtered key frames and using them as the input of the local mapping thread. The present invention combines environmental semantic information and geometric constraints to filter out dynamic feature points of key frames, thereby effectively avoiding the influence of dynamic objects in the surrounding environment and improving the accuracy of robot positioning and mapping in a dynamic environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot simultaneous localization and mapping (SLAM), and particularly relates to a method for designing a visual odometer of a mobile robot in a dynamic scenario. Background Technique

[0002] The market functional requirements for mobile robots are becoming increasingly rich, and related research has also become a hot issue in recent years, with broad development prospects. For mobile robots, the autonomous navigation ability is an important foundation for them to realize various advanced application functions and algorithm technologies. Moreover, the robot's positioning and cognitive ability of its own pose state and the exploration and perception ability of the unknown environment are the prerequisites for realizing autonomous navigation and path planning. Therefore, the simultaneous localization and mapping (SLAM) technology used to solve these two key problems has positive scientific research value and practical application significance.

[0003] With the improvement of the computing power of the hardware platform and the development of computer vision-related technologies, visual SLAM systems have overcome many difficulties, achieved certain research results with the efforts of many researchers, and formed a relatively mature system framework. However, there are still some problems that will affect the final positioning and mapping effect in the actual application process, which still need to be further optimized. Most of the currently proposed solutions are based on the premise assumption of a stable and static environment. However, there are many uncontrollable factors in the real environment, such as dynamic obstacles, light changes, and mutual overlap and occlusion of various objects, which will cause a large deviation in the final result, and the positioning and mapping accuracy and robustness of the robot in a complex environment are relatively poor. To eliminate the application scenario limitations of the system, the present invention optimizes and improves the currently relatively mature visual SLAM algorithm framework ORB-SLAM2, and proposes a visual odometer design method combining semantic information and geometric constraints for dynamic scenario problems. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings and deficiencies of the existing visual odometer solutions of SLAM systems, and propose a design method for a visual odometer of a mobile robot in a dynamic scenario that combines semantic information and geometric constraints, has high mapping accuracy, strong robustness, and wide adaptability.

[0005] The technical solution for achieving the purpose of the present invention is: a method for designing a visual odometer based on ORB-SLAM2 in a dynamic scenario, including the following steps:

[0006] Step S1: The Intel RealSense depth camera acquires real-time image information and performs preprocessing such as grayscale conversion;

[0007] Step S2: Extract the ORB feature points of the image using an adaptive threshold method based on grid division;

[0008] Step S3: Build a YOLACT network, and use the MS_COCO dataset as samples to complete training and then perform instance segmentation on the image to obtain the semantic information of the image;

[0009] Step S4: Combine the semantic information of the image and the L-K optical flow method to perform motion consistency detection, and roughly filter out dynamic feature points;

[0010] Step S5: Match the feature points of two frames of images based on the PROSAC method, and estimate the fundamental matrix F;

[0011] Step S6: Calculate the epipolar distance according to the fundamental matrix F, further refine and filter out dynamic feature points, estimate the initial pose of the robot based on the selected key frames, and optimize the result with Bundle Adjustment.

[0012] Further, step S1 includes the following steps:

[0013] Step S11: Select the Intel RealSense D415 depth camera for the visual sensor part, which can simultaneously obtain the depth information and color information of the image; to speed up the system operation speed, preprocess the acquired real-time image before extracting feature points, including removing image noise and grayscale conversion, etc.

[0014] Further, step S2 includes the following steps:

[0015] Step S21: To ensure the scale invariance of the feature points, so that the same image can still match the corresponding feature points after being scaled, first build a scale pyramid for the input image, and calculate the ORB feature points in images of different scales. The pyramid takes the original image acquired by the camera as the 0th layer, and gradually shrinks it by a scale factor layer by layer until the top layer of the pyramid.

[0016] Step S22: To ensure that the extracted feature points are more evenly and reasonably distributed, so as to obtain more comprehensive information, divide each layer of the pyramid image into a certain number of rows and columns of grids, and set the number of pre-extracted feature points in each grid and the initial FAST corner point extraction threshold;

[0017] Step S23: After the first pre-extraction of feature points is completed according to the initial threshold, if the actual number of feature points extracted in the grid is less than the set pre-extraction number, change the threshold and continue to extract, and repeat the above process until the adaptive extraction of feature points in the grid is completed.

[0018] Further, step S3 includes the following steps:

[0019] Step S31: Use the ResNet101 convolutional module to construct the backbone network part of YOLACT, which is mainly responsible for extracting the features of the image;

[0020] Step S32: Construct the Feature Pyramid Networks (FPN) network, the purpose of which is to generate multi-scale feature maps to ensure that objects of different sizes can be detected;

[0021] Step S33: Construct the Protonet branch to generate prototype masks and extract important parts of the image to be processed;

[0022] Step S34: Construct the Prediction Head branch to generate mask coefficients. A shared convolutional network is used to achieve better real-time segmentation. This step is carried out synchronously with S33;

[0023] Step S35: Use 18 common indoor household items in the MS_COCO (Microsoft Common Objects in Context) dataset as training samples to train the YOLACT network.

[0024] Furthermore, step S4 includes the following steps:

[0025] Step S41: The pixel velocity of dynamic objects in the middle can be calculated and tracked by the L-K optical flow method. Combine the calculation results with semantic information to analyze the dynamics of the object. If there is relative motion between the object and the background, the feature points contained in the object in the current frame are removed; if the object velocity vector is less than the threshold compared with the previous frame, the relevant feature points are retained. Thus, the rough filtering of dynamic feature points is completed.

[0026] Furthermore, step S5 includes the following steps:

[0027] Step S51: Calculate the minimum Euclidean distance and evaluation function value for each pair of feature points in two frames of images, and sort the feature points in descending order according to the evaluation function value. Calculate the sum of the quality of each group of eight feature points as a group and sort them. Select the 8 groups of matching points with the highest matching quality and calculate the fundamental matrix F;

[0028] Step S52: After removing the above 8 groups of matching points from the subset, calculate the corresponding projection points of the remaining feature points in the subset according to the fundamental matrix;

[0029] Step S53: Calculate the error between other feature points and the projection points. If it is less than the set value, it is identified as an inlier. After updating the number of inliers, recalculate the fundamental matrix F and obtain new inliers. If the number of iterations does not exceed the maximum value, return F and the set of inliers; otherwise, the model establishment fails;

[0030] Step S54: Calculate the epipolar distance. If it exceeds the set threshold, it indicates a large error and there are dynamic objects around that need to be removed.

[0031] Further, step S6 includes the following steps:

[0032] Step S61: Linearly and weightedly represent any punctuation point on the target object in the world coordinate system as coordinates in the camera coordinate system using four non-coplanar virtual control points, and then transform the problem to 3D-3D;

[0033] Step S62: Use the ICP (Iterative Closest Point) algorithm to solve the camera pose parameters, including the rotation parameter R and the translation parameter t.

[0034] Step S63: Due to the noise influence of the observation points, there may be errors in the estimation results. Therefore, consider using Bundle Adjustment (BA) to optimize the pose calculation results. BA optimization is a relatively common non-linear optimization method that optimizes both the camera pose and the spatial point positions at the same time. Its main idea is to construct a least squares problem after summing the errors and find the optimal camera pose to minimize the error term.

[0035] Compared with the prior art, the beneficial effects of the present invention are:

[0036] (1) The visual odometry scheme proposed by the present invention has better robustness. When the robot is in a complex environment, the visual odometry scheme proposed by the present invention can largely avoid the influence of dynamic objects, retain static feature points, and effectively improve the accuracy of robot positioning and mapping.

[0037] (2) In view of the situation that the results of the ORB feature extraction algorithm are prone to feature point concentration, the present invention proposes a grid-based adaptive feature extraction method. Through this method, more evenly distributed feature points can be obtained, and more comprehensive image information can be acquired.

[0038] (3) In view of the problem that the original system cannot obtain environmental semantic information, the present invention studies an instance segmentation processing method based on YOLACT. To obtain environmental semantic information, this paper proposes to add an instance segmentation thread to the original framework to segment the RGB image. YOLACT can achieve semantic annotation of common household items, which can be used not only for the visual odometry part but also for the construction of subsequent semantic maps. Description of the Drawings

[0039] Figure 1 It is the overall block diagram of the visual odometry designed for the present invention.

[0040] Figure 2 It is the flowchart of the tracking thread in the visual odometry designed for the present invention.

[0041] Figure 3 Schematic diagram of the grid adaptive threshold feature extraction method proposed by the present invention.

[0042] Figure 4 YOLACT network structure diagram constructed by the present invention. Specific implementation manner

[0043] To make the objectives, technical solutions and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and technical solutions.

[0044] The objective of the present invention is to overcome the disadvantages and deficiencies of the existing visual odometry solutions in SLAM systems, and a visual odometry algorithm for mobile robots based on ORB-SLAM2 in dynamic scenarios is proposed. Specifically, it includes the steps of: obtaining real-time image information through an Intel RealSense depth camera and performing preprocessing such as graying on it; using an adaptive threshold method proposed based on the ORB (Oriented FAST and Rotated BRIEF) algorithm to more comprehensively extract image feature points; using the MS_COCO dataset as a sample to train the YOLACT network and perform instance segmentation on the image to obtain image semantic information; combining the image semantic information and the L-K optical flow method to roughly filter out dynamic feature points; estimating the fundamental matrix F based on the Progressive Sample Consensus (PROSAC) algorithm, and then calculating the epipolar distance and precisely filtering out dynamic feature points; finally, selecting the filtered key frames and using them as the input of the local mapping thread. The key point of the present invention is to combine environmental semantic information and geometric constraints to filter out dynamic feature points of key frames, thereby effectively avoiding the influence of dynamic objects in the surrounding environment and improving the accuracy of robot positioning and mapping in dynamic environments.

[0045] The specific steps of this method will be described in detail below.

[0046] Combined with Figure 1 、 Figure 2 , a visual odometry algorithm for mobile robots based on ORB-SLAM2 in dynamic scenarios includes the following content:

[0047] Step S1: Collect and preprocess images, which specifically includes the following steps.

[0048] Considering the requirements of the SLAM system, an Intel RealSense D415 that can simultaneously obtain image depth information and color information is selected as the visual sensor. The Intel RealSense D415 depth camera is used to collect image information. To make the system operation more concise, the obtained real-time images are preprocessed before extracting feature points, including operations such as removing noise and graying.

[0049] Step S2: Extract image feature points, which specifically include the following steps.

[0050] Step S21: To ensure the scale invariance of feature points so that the same image can still match the corresponding feature points after being scaled proportionally, first construct a scale pyramid for the input image. Calculate ORB feature points in images of different scales. Take the original image obtained by the camera as the 0th layer, and gradually reduce it layer by layer according to the scale factor until the top layer of the pyramid. The total number of pyramid layers nlevels is set to 8, then the scaled image is:

[0051]

[0052] where, I k is the image size of the kth layer of the pyramid, I is the original image obtained by the camera, scaleFactor is the scaling factor, set to 1.2, k represents the pyramid layer number, and its value range is [1, nlevels - 1];

[0053] The number of feature points extracted on each layer of the image is:

[0054]

[0055] where, N is the total number of ORB feature points to be extracted, DesiredC i is the expected number of feature points to be extracted on the ith layer of the pyramid, the value range of i is [0, n - 1], n is the total number of pyramid layers, usually set to 8, and InvSF is the reciprocal of the scale factor scaleFactor;

[0056] Step S22: To ensure that the extracted feature points are more evenly and reasonably distributed, so as to obtain more comprehensive information, an adaptive threshold feature point extraction algorithm for dividing grids is proposed. The schematic diagram is as Figure 3 shown. Divide each layer of the pyramid image into grids, the number of grid columns is Cols i , and the number of rows is Rows i , and the calculation is as follows:

[0057]

[0058] where, t is the grid division coefficient. When t decreases, the total number of grids increases; IRat i represents the ratio of the number of rows and columns of the grids divided on the ith layer of the image, that is: IRat i = Rows / Cols.

[0059] And set the number of feature points cDesC i to be pre-extracted in each grid and the initial FAST corner point extraction threshold iniTh, and the calculation is as follows:

[0060]

[0061]

[0062] Among them, I(x) is the grayscale value of a certain pixel point in the image, κ is the average grayscale value of all pixel points, and sp represents the total number of pixel points included in the image.

[0063] Step S23: After the first pre - extraction of feature points is completed according to the initial threshold, if the actual number of extracted feature points in the grid is less than the set pre - extraction number, then change the threshold and continue the extraction. Repeat the above process until the adaptive extraction of feature points in the grid is completed.

[0064] Step S3: Obtain the image semantic information, which specifically includes the following steps.

[0065] Step S31: The overall structure diagram of the YOLACT network is as Figure 4 shown. The backbone network part uses ResNet101, which has a total of 5 convolutional modules, including conv1, conv2_x to conv5_x, corresponding to Figure 4 C1 to C5 in respectively, and is mainly responsible for completing the feature extraction of the image;

[0066] Step S32: Figure 4 P3 to P7 in are the FPN (Feature Pyramid Networks) network, which generates multi - scale feature maps to ensure that different - sized target objects can be detected. First, P5 is obtained by passing C5 through a convolutional layer, then the feature map of P5 is doubled by bilinear interpolation and added to the convolved C4 to obtain P4, and P3 is obtained through the same operation. In addition to the downward transformation, P6 is generated by convolving and downsampling P5, and P7 is obtained by performing the same operation on P6. Thus, the FPN network is completely established, generating feature maps of different scales, with richer features, which is more conducive to segmenting target objects of different sizes;

[0067] Step S33: Build the Protonet branch for generating prototype masks. The input of this branch is P3, which consists of several convolutional layers in the middle, and the final output has k channels. Each channel represents a prototype mask with a dimension of 138×138, and important parts of the image to be processed can be extracted through this mask;

[0068] Step S34: Build the Prediction Head branch for generating mask coefficients. To achieve better real - time performance of segmentation, a shared convolutional network is used, and this step is carried out synchronously with S33;

[0069] Step S35: Use the 18 common categories of indoor household objects in the MS_COCO (Microsoft Common Objects in Context) dataset as training samples to train the YOLACT network.

[0070] The MS_COCO dataset is an open source dataset released by Microsoft in 2014. It contains 91 types of objects and scenes that are common in daily life, and stores three types of annotations in the form of JSON files: object instances, object keypoints, and image captions. We customized 18 common indoor household objects as training samples.

[0071] Step S4: Perform motion consistency detection, which specifically includes the following steps.

[0072] Step S41: The pixel speed of the dynamic object can be tracked and calculated by the LK optical flow method, and the dynamics of the object is analyzed by combining the calculation results and semantic information. If there is relative motion between the target and the background, the feature points contained in the object in the current frame are eliminated; if the object velocity vector does not change much compared with the previous frame, the relevant feature points are retained, and the coarse filtering of dynamic feature points is completed.

[0073] Step S5: matching feature points, specifically including the following steps.

[0074] Step S51: Calculate the minimum Euclidean distance d between each feature point pair of two frames of images min , the next smallest Euclidean distance d min2 And the evaluation function value q(u x ), the evaluation function is calculated as follows:

[0075]

[0076] This means that if the set u N Contains N data points, then the data points in the set satisfy:

[0077]

[0078] Qualitatively speaking, the evaluation function value q(u x ) The data points with larger values ​​are ranked higher in the set. The feature points are arranged in descending order according to the value of the evaluation function. The quality of each group of eight feature points is calculated and sorted. The 8 groups of matching points with the highest matching quality are selected and the basic matrix F is calculated.

[0079] Step S52: after removing the above 8 groups of matching points from the subset, the corresponding projection points of the remaining feature points in the subset are calculated according to the basic matrix F;

[0080] Step S53: Calculate the error e between other feature points and the projection points. If it is less than the maximum error value ε, then this point is identified as an inlier. After updating the number of inliers, recalculate the fundamental matrix F and obtain new inliers. If the number of iterations does not exceed the maximum value, return the fundamental matrix F and the set of inliers. Otherwise, the model establishment fails;

[0081] Step S54: Calculate the epipolar distance according to the fundamental matrix F. The projection point on the camera imaging plane is p1, and its matching point on the current frame imaging plane is p2. Then the distance from the normalized coordinate x2 of p2 to the epipolar line l is:

[0082]

[0083] where x1 is the normalized coordinate of p1, and [A B C] T is the vector composed of the expression coefficients of the epipolar line l.

[0084] If the calculation result exceeds the set threshold θ, it indicates a large error, and there are dynamic objects around the corresponding 3D world point that need to be removed. The threshold θ is calculated as follows:

[0085]

[0086] Step S6: Estimate the initial pose of the robot, which specifically includes the following steps.

[0087] Step S61: Linearly and weightedly represent any punctuation mark on the target object in the world coordinate system as the coordinate in the camera coordinate system using four non-coplanar virtual control points, and then obtain a set of matching 3D points, thus transforming the problem into 3D-3D;

[0088] Step S62: Use the ICP (Iterative Closest Point) algorithm to solve the camera pose parameters, that is, the rotation parameter R and the translation parameter t that can minimize the sum of squared errors.

[0089] Step S63: Due to the noise influence of the observation points, there may be errors in the estimation results. Therefore, consider using Bundle Adjustment (BA) to optimize the pose calculation results. BA optimization is a relatively common non-linear optimization method that optimizes both the camera pose and the spatial point positions at the same time. Its main idea is to construct a least squares problem after summing the errors and find the optimal camera pose to minimize the error term.

[0090] The above-described embodiments are merely specific implementation manners of the present invention, used to illustrate the technical solutions of the present invention, rather than limiting it. The protection scope of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: Any person skilled in the art within the technical scope disclosed by the present invention can still modify the technical solutions recorded in the foregoing embodiments, or can easily think of changes, or perform equivalent replacements on some of the technical features; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A design method for a mobile robot visual odometer in a dynamic scenario, characterized in that, The following steps are involved: Step S1: acquiring real-time image information and performing preprocessing; Step S2: Extracting ORB feature points of the image using an adaptive threshold method based on grid division, specifically including the following steps: Step S21: construct a scale pyramid for the input image and calculate ORB feature points in images of different scales; Step S22: Divide each layer of the pyramid image into grids with corresponding numbers of columns and rows, and set the number of pre-extracted feature points in each grid and the initial FAST corner point extraction threshold; Step S23: After the first feature point pre-extraction is completed according to the initial threshold, if the number of feature points actually extracted in the grid is less than the set pre-extraction number, the threshold is changed to continue the extraction, and steps S21-S23 are repeated until the adaptive extraction of feature points in the grid is completed; Step S3: Build the YOLACT network, use the MS_COCO dataset as a sample to complete the training and perform instance segmentation on the image to obtain the image semantic information; specifically, it includes the following steps: Step S31: Use the ResNet101 convolution module to build the YOLACT backbone network, which is mainly responsible for completing the feature extraction of the image; Step S32: construct an FPN network and generate a multi-scale feature map to ensure that target objects of different sizes can be detected; Step S33: construct a Protonet branch to generate a prototype mask, through which the part of interest in the image to be processed is extracted; Step S34: construct a Prediction Head branch for generating mask coefficients, using a shared convolutional network. This step is performed simultaneously with S33. Step S35: Use 18 types of common indoor household objects in the MS_COCO dataset as training samples to train the YOLACT network; Step S4: Combine image semantic information and LK optical flow method to perform motion consistency detection and roughly filter out dynamic feature points; Step S5: Match the feature points of the two frames of images based on the PROSAC method and estimate the basic matrix; Step S6: Calculate the polar line distance according to the basic matrix, filter out the dynamic feature points, estimate the initial posture of the robot according to the selected key frame and perform Bundle Adjustment optimization on the result.

2. The design method of the mobile robot visual odometer in a dynamic scenario according to claim 1, characterized in that, In step S1, real-time image information is obtained through the Intel RealSense depth camera, and the preprocessing includes noise removal and grayscale operations.

3. The design method of the mobile robot visual odometer in a dynamic scenario according to claim 1, characterized in that, The MS_COCO dataset stores three types of annotations: target instances, target key points, and image descriptions in the form of JSON files.

4. The design method of the mobile robot visual odometer in a dynamic scenario according to claim 1, wherein Step S4 is specifically as follows: The pixel speed of the dynamic object can be tracked and calculated by the LK optical flow method, and the dynamics of the object can be analyzed by combining the calculation results and semantic information. If there is relative motion between the object and the background, the feature points of the object in the current frame will be removed. If the object velocity vector does not change more than the threshold value compared with the previous frame, the relevant feature points are retained, thus completing the coarse filtering of dynamic feature points.

5. The method for designing a mobile robot visual odometer in a dynamic scenario according to claim 1, wherein Step S5 comprises the following steps: Step S51: Calculate the minimum Euclidean distance and the evaluation function value for each pair of feature points in two frames of images, and sort the feature points in descending order according to the evaluation function value; calculate the sum of the quality of each group with every eight feature points as a group and sort them, select the 8 groups of matching points with the highest matching quality and calculate the fundamental matrix F; Step S52: After removing the above 8 groups of matching points from the subset, calculate the corresponding projection points of the remaining feature points in the subset according to the fundamental matrix; Step S53: Calculate the error between other feature points and the projection points, and if it is less than the set value, it is considered an inlier; After updating the number of inliers, recalculate the fundamental matrix F and obtain new inliers; if the number of iterations does not exceed the maximum value, return F and the set of inliers; Otherwise, the model establishment fails; Step S54: Calculate the epipolar distance, and if it exceeds the threshold, it means that the error is large and there are dynamic objects around that need to be removed.

6. The design method of the mobile robot visual odometer in a dynamic scenario according to claim 1, characterized in that, Step S6 includes the following steps: Step S61: Linearly and weightedly represent any punctuation on the target object in the world coordinate system as coordinates in the camera coordinate system using four non-coplanar virtual control points, and then transform the problem to 3D-3D; Step S62: Use the ICP algorithm to solve the camera pose parameters, including the rotation parameter R and the translation parameter t; Step S63: Use BA to optimize the pose calculation result.

7. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the mobile robot visual odometry design method in a dynamic scene as described in any one of claims 1-6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the mobile robot visual odometry design method in a dynamic scene as described in any one of claims 1-6.

Images