A vision-based mobile robot positioning method

Abstract

The present invention provides a mobile robot positioning method based on vision, comprising the following steps: S1) collecting image information of the surrounding environment of the mobile robot; S2) obtaining the motion trajectory of the mobile robot based on the optical flow method, and screening key frames at the same time; S3) constructing a local map based on the motion trajectory; S4) obtaining the next key frame of the real-time environment through the local map, and extracting the features of the key frame; S5) projecting the three-dimensional map points of the local map into the current key frame, and calculating the reprojection error; S6) judging whether the reprojection error is greater than a threshold, if so, performing bundle optimization on the current key frame, correcting the position and posture of the key frame, updating the local map according to the key frame, and entering step S7, if not, entering step S7; S7) judging whether to continue positioning, if so, returning to step S2, otherwise ending positioning. The present invention not only improves the calculation speed but also has a certain robustness.

Description

Technical Field

[0001] The invention relates to the field of mobile robot positioning, and in particular to a vision-based mobile robot positioning method. Background Art

[0002] With the continuous development of mobile robots, the demand for mobile robots in various industries is constantly increasing, such as epidemic prevention robots, cleaning robots, and inspection robots. Among them, the research on autonomous positioning of mobile robots has received widespread attention, and accurate positioning can further meet the needs of subsequent tasks, such as path planning and local obstacle avoidance. At present, there are two mainstream methods for positioning mobile robots, namely vision-based positioning methods and lidar-based positioning methods. Due to the high price of lidar and the advantages of cameras such as low cost and easy installation, vision-based positioning methods have become a research hotspot.

[0003] Vision-based positioning methods can be divided into feature point-based methods and optical flow methods. The optical flow method uses the assumption of grayscale invariance and uses the optical flow tracking method for positioning. This positioning method is fast and works well in low-texture areas, but grayscale invariance is a strong assumption and is easily affected by exposure and blur. The feature point method estimates the pose by extracting features and matching image frames. Although this method does not need to consider grayscale changes, it consumes a lot of time in feature point extraction and matching. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a vision-based mobile robot positioning method, which not only improves the calculation speed but also has a certain robustness.

[0005] The present invention achieves the above technical objectives through the following technical means.

[0006] A vision-based mobile robot positioning method comprises the following steps:

[0007] S1) collecting image information of the surrounding environment of the mobile robot;

[0008] S2) performing optical flow tracking on the input image frame to obtain the motion trajectory of the mobile robot based on the optical flow method, and performing key frame screening while the image frame is input;

[0009] S3) constructing a local map based on the motion trajectory of the mobile robot obtained in step S2;

[0010] S4) obtaining the next key frame of the real-time environment through the local map and extracting the features of the key frame;

[0011] S5) projecting the three-dimensional map points of the local map into the current key frame and calculating the reprojection error;

[0012] S6) determining whether the reprojection error is greater than a threshold value, if so, performing bundle optimization on the current key frame, correcting the position and posture of the key frame, updating the local map according to the key frame, and entering step S7, if not, directly entering step S7;

[0013] S7) Determine whether to continue positioning, if yes, return to step S2, if no, end positioning.

[0014] Preferably, in step S2, while the image frame is input, the key frame is screened, which specifically includes the following steps:

[0015] A1) At least 30 frames have passed since the last keyframe was inserted;

[0016] A2) At least 60 points are tracked in the current frame;

[0017] A3) The repetition ratio between the points observed in the current frame and the points observed in the previous key frame is less than 85%.

[0018] Preferably, extracting the key frame features in step S4 specifically includes the following steps:

[0019] B1) dividing the key frame into image grids;

[0020] B2) extracting ORB features from the current key frame;

[0021] B3) Determine the number of feature points. If it is less than 1000, lower the threshold for extraction. If it is more than 2000, perform quadtree homogenization.

[0022] Preferably, the step B2 specifically comprises the following steps:

[0023] B2.1) Use a 9*9 Gaussian kernel to smooth the image;

[0024] B2.2) Use FAST algorithm to detect feature points, detect the gray value of a circle around the candidate feature point, if there are at least 12 pixels around the candidate point whose gray value difference with the candidate point is greater than the threshold, then the candidate point is a feature point, where the mathematical expression for calculating the gray value difference is:

[0025]

[0026] Where: N represents the number of feature points, i represents the current candidate feature point, I(i) is the grayscale value of any point on the circumference, I(p) is the grayscale value of the feature point at the center of the circle, and d is the threshold.

[0027] B2.3) Make a circle with the feature point in step 4.2.2 as the center and d as the radius;

[0028] B2.4) Randomly select 132 pairs of points within the circle obtained in step S4.2.3. In the same pair of points, if the grayscale value of the previous point is greater than the grayscale value of the next point, then take 1, otherwise take 0, to form a 132-bit binary descriptor.

[0029] Preferably, the calculation formula for calculating the reprojection error in step S5 is:

[0030] e i,j＝ x i,j –ρ(T i,w ,P w,j )

[0031] Where: e i,j represents the reprojection error corresponding to the jth feature point in the i-th key frame, x i,j represents the jth feature point in the i-th frame, ρ represents the projection function, T i,w represents the camera external parameter matrix of the i-th frame, where w represents the world coordinate system, P w,j Represents the coordinates of the map point corresponding to the jth feature point in the world coordinate system.

[0032] Preferably, updating the local map according to the key frame in step S6 includes the following steps:

[0033] C1) adding feature points in the current key frame that do not appear in the local map to the local map;

[0034] C2) After a map point is created, if the map point is observed in less than three keyframes, the map point is deleted from the local map;

[0035] C3) The new feature points and map points are tested through positive depth test, parallax test or reprojection error test, and unqualified points are eliminated.

[0036] Beneficial effects of the present invention:

[0037] The vision-based mobile robot positioning method of the present invention adopts the optical flow method to estimate the pose of the mobile robot, extracts features from key frames, and further optimizes the pose estimation of the mobile robot by projecting map points to points in the current key frames and constraining the reprojection errors of feature points. This not only retains the speed of the optical flow method, but also uses the feature point method to optimize the initial pose estimated by the optical flow method, thereby improving the positioning accuracy of the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 The present invention is a flowchart of a vision-based mobile robot positioning method according to an embodiment of the present invention.

[0039] Figure 2 This is a feature extraction flow chart of an embodiment of the present invention. DETAILED DESCRIPTION

[0040] In order to make the above-mentioned purpose, features and advantages of the present invention more obvious and easy to understand, the specific implementation of the present invention is described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the present invention, so the present invention is not limited by the specific implementation disclosed below.

[0041] See also Figure 1 According to a vision-based mobile robot positioning method according to an embodiment of the present invention, the method specifically comprises the following steps:

[0042] Step S1) collecting image information of the surrounding environment of the mobile robot through a camera device and transmitting it to the industrial computer through a transmission interface;

[0043] Step S2) performing optical flow tracking on the input image frame to obtain the motion trajectory of the mobile robot based on the optical flow method, and performing key frame screening while the image frame is input;

[0044] Wherein, while the image frame is input, the key frame is screened, which specifically includes the following steps:

[0045] A1) At least 30 frames have passed since the last keyframe was inserted;

[0046] A2) At least 60 points are tracked in the current frame;

[0047] A3) The repetition ratio between the points observed in the current frame and the points observed in the previous key frame is less than 85%.

[0048] Step S3) constructing a local map based on the motion trajectory of the mobile robot obtained by the optical flow method;

[0049] Step S4) obtaining the next key frame of the real-time environment through the local map and extracting the features of the key frame;

[0050] Among them, Figure 2 As shown, the features of the key frame are extracted. The specific steps are as follows:

[0051] B1) dividing the key frame into image grids;

[0052] B2) Extracting ORB features from the current key frame, specifically including:

[0053] B2.1) Use a 9*9 Gaussian kernel to smooth the image;

[0054] B2.2) Use the FAST (features from accelerated segment test) algorithm to detect feature points. Detect the grayscale value of a circle around the candidate feature point. If there are at least 12 pixels around the candidate point whose grayscale value difference with the candidate point is greater than the threshold, then the candidate point is a feature point. The mathematical expression for calculating the grayscale value difference is:

[0055]

[0056] Where: N represents the number of feature points, i represents the current candidate feature point, I(i) is the grayscale value of any point on the circumference, I(p) is the grayscale value of the feature point at the center of the circle, and d is the threshold.

[0057] B2.3) Make a circle with the feature point in step B2.2 as the center and d as the radius;

[0058] B2.4) Randomly select 132 pairs of points within the circle obtained in step B2.3. In the same pair of points, if the grayscale value of the previous point is greater than the grayscale value of the next point, then it is taken as 1, otherwise it is taken as 0, forming a 132-bit binary descriptor.

[0059] B3) Determine the number of feature points. If it is less than 1000, lower the threshold for extraction. If it is more than 2000, perform quadtree homogenization.

[0060] Step S5) projecting the three-dimensional map points of the local map into the current key frame and calculating the reprojection error;

[0061] Among them, the calculation formula for calculating the reprojection error is:

[0062] e i,j＝ x i,j –ρ(T i,w ,P w,j )

[0063] Where: e i,j represents the reprojection error corresponding to the jth feature point in the i-th key frame, x i,j represents the jth feature point in the i-th frame, ρ represents the projection function, T i,w represents the camera external parameter matrix of the i-th frame, where w represents the world coordinate system, P w,j Represents the coordinates of the map point corresponding to the jth feature point in the world coordinate system.

[0064] Step S6) Determine whether the reprojection error is greater than a threshold value. If so, perform bundle optimization on the current key frame, correct the position and posture of the key frame, update the local map according to the key frame, and enter step S7. If not, enter step S7;

[0065] Wherein, updating the local map according to the key frame specifically includes the following steps:

[0066] C1) adding feature points in the current key frame that do not appear in the local map to the local map;

[0067] C2) After a map point is created, if the map point is observed in less than three keyframes, the map point is deleted from the local map;

[0068] C3) The new feature points and map points are tested through positive depth test, parallax test or reprojection error test, and unqualified points are eliminated.

[0069] Step S7) determines whether to continue positioning, if yes, returns to step 2), if no, ends positioning.

[0070] The present invention utilizes the optical flow method to realize the rapid posture tracking of the mobile robot. At the same time, under the constraint of minimizing the reprojection error, the feature point method is used to optimize the posture of the mobile robot solved by the optical flow method, which not only improves the calculation speed but also has a certain robustness.

[0071] The above-mentioned embodiments only express several implementation methods of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the patent of the present invention. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A vision-based mobile robot positioning method, characterized in that: The following steps are involved: S1) collecting image information of the surrounding environment of the mobile robot; S2) performing optical flow tracking on the input image frame to obtain the motion trajectory of the mobile robot based on the optical flow method, and performing key frame screening while the image frame is input, specifically comprising the following steps: A1) At least 30 frames have passed since the last keyframe was inserted; A2) At least 60 points are tracked in the current frame; A3) The repetition ratio between the points observed in the current frame and the points observed in the previous key frame is less than 85%; S3) constructing a local map based on the motion trajectory of the mobile robot obtained in step S2); S4) obtaining the next key frame of the real-time environment through the local map and extracting the features of the key frame, specifically comprising the following steps: B1) dividing the key frame into image grids; B2) extracting ORB features from the current key frame; B3) Determine the number of feature points. If it is less than 1000, lower the threshold for extraction. If it is more than 2000, perform quadtree homogenization. S5) projecting the three-dimensional map points of the local map into the current key frame and calculating the reprojection error; S6) determining whether the reprojection error is greater than a threshold value, if so, performing bundle optimization on the current key frame, correcting the position and posture of the key frame, updating the local map according to the key frame, and entering step S7); if not, entering step S7); S7) Determine whether to continue positioning, if yes, return to step S2), if no, end positioning.

2. The vision-based mobile robot positioning method according to claim 1, characterized in that: The step B2) specifically comprises the following steps: B2.1) Use a 9*9 Gaussian kernel to smooth the image; B2.2) Use FAST algorithm to detect feature points, detect the gray value of a circle around the candidate feature point, if there are at least 12 pixels around the candidate point whose gray value difference with the candidate point is greater than the threshold, then the candidate point is a feature point, where the mathematical expression for calculating the gray value difference is: Where: N represents the number of feature points, i represents the current candidate feature point, I(i) is the grayscale value of any point on the circumference, I(p) is the grayscale value of the feature point at the center of the circle, and d is the threshold; B2.3) Make a circle with the feature point in step B2.2 as the center and d as the radius; B2.4) Randomly select 132 pairs of points within the circle obtained in step B2.

3. In the same pair of points, if the grayscale value of the previous point is greater than the grayscale value of the next point, then it is taken as 1, otherwise it is taken as 0, forming a 132-bit binary descriptor.

3. The vision-based mobile robot positioning method according to claim 1, characterized in that: The calculation formula for calculating the reprojection error in step S5) is: and i,j＝ x i,j -ρ(T i,w ,P w,j ) Where: e i,j represents the reprojection error corresponding to the jth feature point in the i-th key frame, x i,j represents the jth feature point in the i-th frame, ρ represents the projection function, T i,w represents the camera external parameter matrix of the i-th frame, where w represents the world coordinate system, P w,j Represents the coordinates of the map point corresponding to the jth feature point in the world coordinate system.

4. The vision-based mobile robot positioning method according to claim 1, characterized in that: In step S6), the local map is updated according to the key frame, including the following steps: C1) adding feature points in the current key frame that do not appear in the local map to the local map; C2) After a map point is created, if the map point is observed in less than three keyframes, the map point is deleted from the local map; C3) The new feature points and map points are tested through positive depth test, parallax test or reprojection error test, and unqualified points are eliminated.

Images