Robot real-time positioning method based on ground texture

Abstract

The invention belongs to the technical field of visual SLAM, and discloses a robot real-time positioning method based on ground texture. Acquiring a ground image through a monocular camera at the bottom of the robot, and performing contrast-limited adaptive histogram equalization processing; detecting key points for the first frame, initializing the system, otherwise, tracking the optical flow; matching the current frame feature point tracked by the optical flow with the previous frame feature point, estimating the relative pose transformation of the image, calculating the pose transformation error, and adding the pose transformation error into the factor graph as an odometer constraint; dynamically setting a key frame, and carrying out loopback detection on the key frame; estimating relative pose transformation and errors of the current key frame and the loopback frame, and adding the relative pose transformation and errors into the factor graph as loopback constraints; and performing incremental optimization through the factor graph to obtain an optimized pose. The loopback detection strategy for the ground texture image is provided, loopback detection correctness and scale control are effectively improved, accumulated drift is reduced, and system precision is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of visual SLAM, and particularly to a real-time positioning method for a robot based on ground texture. Background Art

[0002] The technology of unmanned logistics transportation has been gradually applied to the manufacturing industry. Some enterprises have started to use unmanned logistics equipment such as AGV (Automated Guided Vehicle) and AMR (Autonomous Mobile Robot) for logistics transportation. AMR is an unmanned logistics device that operates based on environmental perception. It usually senses the environment through sensors such as lidar and cameras, autonomously plans paths, and realizes the automation of logistics transportation. Different from AGV, AMR does not require a pre-set guiding path, can autonomously sense the environment and obstacles, and autonomously plan paths, with higher flexibility and adaptability. When exploring unknown areas, the robot ground system often relies on the Simultaneous Localization and Mapping (SLAM) technology to map the surrounding environment and track its position in the environment. Although a variety of sensors can be used, from lidar to vision, the monocular camera has become a popular choice due to its low cost and rich information content. These monocular SLAM systems often look for significant features in the world for navigation.

[0003] However, some environments, such as flat open spaces, lack enough features for reliable navigation, and cameras are also sensitive to lighting changes in the environment, such as glare at sunset. In addition, in many production sites, there are many mobile robots or forklifts with dynamic operations. Although using a front camera or lidar for positioning can achieve accurate positioning in some indoor applications, it may be blocked in some scenarios, resulting in positioning errors. In these scenarios, the only consistent feature source comes from the ground on which the robot travels.

[0004] The ground texture positioning method based on downward vision (towards the ground) is not affected by occlusion and lighting, and can also achieve a reliable positioning effect. For suitable ground texture types, such as concrete, asphalt, or carpet, which may seem random and uniform, but all contain fiber cracks, scratches, or unique arrangements, and are common in most fields of autonomous mobile robot applications. This type of method can maintain sufficient stability. Therefore, the solution based on ground texture has the potential to achieve high-precision positioning without infrastructure, can also achieve positioning in an environment without static landmarks, and helps to avoid privacy issues of domestic robots.

[0005] The literature "High-precision localization using ground texture. (IEEE ICRA), 2019" extracts SIFT feature points from ground texture images and proposes a precise voting strategy to make the peaks significantly different from other points in the background, enabling offline construction of a map database first and then online localization. Although the proposed method can achieve high precision in some ground scenarios, its method relying on a prior map database is complex to use. Creating a complete map in the initial setup takes a lot of time, and it cannot explore in previously unknown environments or cope with changing ground environments. The literature "Monocular simultaneous localization and mapping using ground textures (IEEE ICRA), 2023" is a monocular SLAM algorithm implemented using ground texture features. It utilizes the known depth of ground texture images to optimize when estimating the transformation between overlapping images and identifying loop closures. Although the proposed real-time ground texture SLAM method gets rid of the dependence on a prior map, its accuracy is average, and it runs in a single thread and can only give the globally optimized pose after the system runs. Moreover, it lacks a key frame strategy and has a large amount of redundant calculation in the loop detection part, affecting the real-time performance of the system. Summary of the Invention

[0006] The object of the present invention is to provide a real-time robot localization method based on ground texture. Through a monocular camera installed at the bottom of the robot, the optical flow method is used to implement the front-end odometer. The cumulative error is reduced through loop detection, and an online ground texture SLAM system running in multiple threads is implemented using an incremental optimization method.

[0007] The technical solution of the present invention is as follows: A real-time robot localization method based on ground texture, comprising the following steps:

[0008] Step 1: Obtain a ground image through a monocular camera at the bottom of the robot and perform limited contrast adaptive histogram equalization processing; detect key points for the first frame and initialize the system, otherwise perform optical flow tracking;

[0009] Step 2: Match the feature points of the current frame tracked by the optical flow with the feature points of the previous frame, estimate the relative pose transformation of the image and calculate the pose transformation error, and add it to the factor graph as an odometer constraint;

[0010] Step 3: Dynamically set keyframes and perform loop detection on the keyframes; pre-train a large number of ground texture images through DBoW3 to generate a visual word bag model of the ground texture; insert each keyframe into the visually constructed word bag database in real time, and detect loop frames for the images in the visually word bag database through the visual word bag model of the ground texture, estimate the relative pose transformation and error between the current keyframe and the loop frame, and add it as a loop constraint to the factor graph;

[0011] Step 4: Perform incremental optimization through the factor graph composed of odometry constraints and loop constraints. When optimizing the pose, incrementally optimize the newly added measurement images and constraints to obtain the optimized pose.

[0012] The specific limitation of the contrast-limited adaptive histogram equalization process in Step 1 is as follows:

[0013] Divide a ground texture image into n sub-blocks of the same size. Assume the sub-block size is B×B and the histogram is h k is the number of pixels with gray level i in the k-th sub-block. The normalized cumulative distribution function of each sub-block is:

[0014]

[0015] Define the contrast limit threshold clipLimit. For each sub-block, if the number of pixels at a certain gray level exceeds clipLimit, clip it to clipLimit and evenly distribute the excess pixels to other gray levels:

[0016]

[0017] where h′ k (i) is the histogram of the k-th sub-block after limiting the contrast, and excess is the number of pixels exceeding clipLimit;

[0018] Then the mapping function of the k-th sub-block is s k :

[0019]

[0020] Adopt the method of bilinear interpolation to calculate the new gray value of the pixel point according to the mapping functions of the surrounding four sub-blocks. For P(x, y, i) in the non-boundary case, (x, y) is its pixel coordinate and i is the pixel value. Find its adjacent 4 sub-blocks. Let the mapping value of the histogram of the upper left sub-block to point P be s′ lt , similarly, the mapping value of the histogram of the upper right sub-block to point P is s′ rt , the mapping value of the histogram of the lower left sub-block to point P is s′ ld , the mapping value s′ of the histogram of the upper left sub-block to point Prd ; Let the distance of point P from the center point of the upper - left sub - block be (Δx, Δy), then the normalized distance is:

[0021]

[0022] For non - boundary cases:

[0023] s′=(1 - Δy′)((1 - Δx′)s′ lt +Δx′s′ rt )+Δy′((1 - Δx′)s′ ld +Δx′s′ rd )

[0024] For boundary cases, only consider its nearest one or two sub - blocks.

[0025] In the first - frame image, key points are detected using Shi - Tomasi corner detection. Different key points satisfy the minimum distance threshold, and the first frame is set as the key frame; the pose of the first frame is [0, 0, 0] T is set as the initial pose.

[0026] The specific steps of step 2 are as follows:

[0027] After performing limited - contrast adaptive histogram equalization on the original ground texture image, use the key points in the previous - frame image to perform optical - flow tracking in the current frame to obtain the corresponding key points in adjacent frames; through bidirectional optical flow, RANSAC processing, and de - distortion, calculate the pose transformation of the current frame relative to the previous frame, and estimate it frame by frame in sequence to obtain continuous motion estimation;

[0028] After converting the feature - point sets of the de - distorted previous frame and the current frame from the pixel coordinate system to the camera coordinate system, the obtained point sets are P and Q respectively;

[0029] Take the pose estimation of the previous moment as the initial estimate, calculate the pose transformation matrix and the error function between the two; for outliers, if the estimated rotation angle exceeds the set threshold, it is considered invalid, replace the current - moment value with the pose - estimation value of the previous moment, and apply an amplification factor to the error function to increase the uncertainty of the current - pose estimation;

[0030] For the feature - point sets P = {p1, p2, …, p n} and Q = {q1, q2, …, q n} of two - frame images, their relative pose is T=(R, t). For each pair of matching points, define the projection error function as e i :

[0031] e i (T)=q i -(Rp i+t)

[0032] The projection error function represents the residual between the point p of the previous frame transformed to the current frame coordinate system through the relative pose T and the actual point q i after transformation; the error function follows a Gaussian distribution, e i ~N(0,Σ), where Σ is the covariance matrix; i ~N(0,Σ), where Σ is the covariance matrix;

[0033] The pose constraint is to minimize the sum of the squares of the projection errors of all matching point pairs:

[0034]

[0035] The optical flow tracking is realized based on the assumption of brightness constancy; the assumption of brightness constancy is that the pixel brightness of the same spatial point in consecutive frames remains unchanged, and the position change of feature points in the image is tracked by minimizing the brightness error; using the previous frame image, the sparse feature point set of the previous frame image and the next frame image, a point set with the same features in the next frame image is obtained, and the optical flow between the sparse feature point sets of the two frame images is judged; to ensure the correctness and consistency of the optical flow tracking, an inverse optical flow tracking is performed again. If the matching points meet the threshold conditions, it is considered that both the forward and backward tracking are correct. At this time, only these matching points are retained, and then the feature points with inconsistent tracking are removed according to the homography matrix; after calculating the pose transformation using this set of matching points, the current frame is supplemented with the same number of Shi-Tomas corner points as the detected and removed ones for the optical flow tracking of the next frame.

[0036] The key frames are set at fixed frame intervals, and the current frame in which the number of well-matched feature points between adjacent frames is lower than a certain threshold; each key frame and its features are inserted into the loop detection database for loop detection.

[0037] The loop detection is specifically as follows: when loop detection is performed on the key frame image, ORB key points are extracted and the BEBLID descriptors of each input key frame are calculated, and the key frame image and its feature descriptors are added to the loop detection database together for querying candidate loop frames;

[0038] Only key frames are input into the loop closure detection, and the loop closure detection is not initially performed. A detection window parameter is set. When the loop closure detection database reaches the set scale, candidate loop closure frames are queried, and only candidate loop closure frames outside this detection window are queried. When querying candidate loop closure frames, only the results with the 4 largest index values in the past N frames will be queried subsequently. For these 4 candidate frames, the visual bag-of-words score must exceed the set threshold to perform descriptor matching. If the number of well-matched descriptors exceeds the threshold, the relative motion and covariance matrix between the current frame and the candidate loop closure frame are calculated. According to the covariance matrix, the maximum uncertainty measure is estimated. Only when the maximum uncertainty measure value is lower than the threshold, the loop closure constraint is added to the factor graph for update.

[0039] The composition of the factor graph includes the relative pose constraint between frames, i.e., the odometry constraint, and the relative pose constraint between the current frame and the loop closure frame calculated by the loop closure detection, i.e., the loop closure constraint. As the odometry constraint and the loop closure constraint are added to the factor graph, the pose optimization is performed using the ISAM2 method at regular intervals to obtain the updated pose.

[0040] The factor graph consists of nodes and edges. The nodes include variable nodes and factor nodes. The variable nodes are the IDs of each frame of the image, and the factor nodes represent the pose estimation and error between different variables. The edges connect the variable nodes and the factor nodes, representing the constraint relationship between the variables.

[0041] The beneficial effects of the present invention: Aiming at the problems existing in the previous robot positioning method based on ground texture, such as relying on the prior map database, low single-thread operation efficiency, and low pose optimization efficiency, the present invention proposes a real-time robot positioning method based on ground texture. A loop closure detection strategy for ground texture images is proposed, which effectively improves the correctness and scale control of loop closure detection, reduces the cumulative drift, and improves the system accuracy. An incremental optimization strategy is integrated for ground texture to update and optimize the pose, which effectively improves the pose estimation accuracy and the pose optimization speed. Brief Description of the Drawings

[0042] Figure 1 It is a flowchart of a real-time robot positioning method based on ground texture;

[0043] Figure 2 It is an example of a factor graph. Detailed Embodiment

[0044] Figure 1 It is the main flowchart of the technical solution of the present invention. As Figure 1 shown, the present invention proposes a real-time robot positioning method based on ground texture, including the following steps:

[0045] Step 1: Obtain the ground image through the monocular camera at the bottom of the robot and perform Contrast Limited Adaptive Histogram Equalization (CLAHE); detect key points for the first frame to initialize the system, otherwise perform optical flow tracking;

[0046] Step 2: Match the feature points of the current frame tracked by optical flow with those of the previous frame, estimate the relative pose transformation of the image and calculate the pose transformation error, and add it to the factor graph as an odometry constraint;

[0047] Step 3: Dynamically set key frames and perform loop detection on the key frames; pre-train a large number of ground texture images through DBoW3 to generate a visual word bag model for ground texture; insert each key frame into the visually constructed word bag database in real time, and detect loop frames in the images of the visual word bag database through the visual word bag model of ground texture, estimate the relative pose transformation and error between the current key frame and the loop frame, and add it to the factor graph as a loop constraint;

[0048] Step 4: Perform incremental optimization through the factor graph composed of odometry constraints and loop constraints. When optimizing the pose, incrementally optimize the newly added measurement images and constraints to obtain the optimized pose;

[0049] The following is a specific embodiment, and the specific steps are as follows:

[0050] Step 1: Obtain the ground image through the monocular camera at the bottom of the robot and perform Contrast Limited Adaptive Histogram Equalization (CLAHE); detect key points for the first frame to initialize the system, otherwise perform optical flow tracking;

[0051] CLAHE needs to be performed on each frame of the image. Divide a ground texture image into n sub-blocks of the same size. Assume the sub-block size is B×B, and the histogram h k is the number of pixels with gray level i in the k-th sub-block. The normalized cumulative distribution function of each sub-block is:

[0052]

[0053] Define the contrast limit threshold clipLimit. For each sub-block, if the number of pixels at a certain gray level exceeds clipLimit, clip it to clipLimit and evenly distribute the excess pixels to other gray levels:

[0054]

[0055] where h′ k (i) is the histogram of the k-th sub-block after limiting the contrast, and excess is the number of pixels exceeding clipLimit;

[0056] Then the mapping function of the k-th sub-block is s k :

[0057]

[0058] Using the method of bilinear interpolation, the new gray value of the pixel is calculated according to the mapping functions of the surrounding four sub-blocks. For P(x, y, i) in the non-boundary case, (x, y) is its pixel coordinate and i is the pixel value. Find its adjacent 4 sub-blocks. Let the mapping value of the histogram of the upper-left sub-block to point P be s′ lt , similarly, the mapping value of the histogram of the upper-right sub-block to point P is s′ rt , the mapping value of the histogram of the lower-left sub-block to point P is s′ ld , the mapping value of the histogram of the upper-left sub-block to point P is s′ rd ; Let the distance of point P relative to the center point of the upper-left sub-block be (Δx, Δy), then the normalized distance is:

[0059]

[0060] For the non-boundary case:

[0061] s′ = (1 - Δy′)((1 - Δx′)s′ lt + Δx′s′ rt ) + Δy′((1 - Δx′)s′ ld + Δx′s′ rd )

[0062] For the boundary case, only consider its nearest one or two sub-blocks.

[0063] After the first frame of image is processed by contrast-limited adaptive histogram equalization, the Shi Tomasi corner detection is used to detect key points. Different key points satisfy the minimum distance threshold, and the first frame is set as the key frame; the pose of the first frame [0, 0, 0] T is set as the initial pose.

[0064] Step 2: In this example, corresponding Figure 1The odometer module uses the Lucas-Kanade optical flow method. Based on the assumption of brightness constancy, optical flow tracking is implemented based on the assumption of brightness constancy. The brightness constancy assumption is that the pixel brightness at the same spatial point in consecutive frames remains unchanged. The position change of feature points in the image is tracked by minimizing the brightness error. Using the previous frame image, the sparse feature point set of the previous frame image, and the subsequent frame image, a point set with the same features in the subsequent frame image is obtained, and the optical flow between the sparse feature point sets of the two frame images is judged. When a camera moves, the image also changes accordingly. Therefore, the image pixel value (gray value) can be regarded as a function I(x, y, t) of pixel coordinates and time. Then for a certain spatial point, the pixel coordinates at time t are (x, y). Due to the movement of the camera, its image coordinates are constantly changing. If we assume that the gray value of the pixel corresponding to this spatial point remains gray during the movement, that is, it satisfies the gray invariance assumption:

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

[0066]

[0067] Then according to the gray invariance assumption:

[0068]

[0069] I x dx + I y dy = -I t

[0070] The left side of equation (8) and are the gradients of gray in the x and y directions respectively, denoted as I x and I y . According to the assumption of Lucas-Kanade optical flow, all pixels share the same motion dx and dy, denoted as u and v. The right side of equation (8) is the change amount of gray with respect to time, denoted as I t .

[0071]

[0072] Among them, the two quantities I x and I y can be obtained by calculating the change of pixel points before and after, and are known quantities. And the value of I t can also be obtained by calculating the gray change amount between the previous and subsequent frames.

[0073] Assume that the pixels within a w*w window have the same motion, and thus w 2 equations can be listed:

[0074]

[0075] Then, the pixel motion is obtained by solving according to the least squares method:

[0076]

[0077] After performing limited contrast adaptive histogram equalization on the original ground texture image, corresponding key points in the adjacent frames are obtained through optical flow tracking in the current frame using the key points in the previous frame. To ensure the correctness and consistency of the optical flow tracking, a reverse optical flow tracking is performed again. If the matching points meet the threshold conditions, it is considered that both the forward and reverse tracking are correct. At this time, only these matching points are retained, and then the inconsistent feature points are removed using RANSAC based on the homography matrix.

[0078] After bidirectional optical flow, RANSAC processing, and distortion removal, the pose transformation of the current frame relative to the previous frame is calculated, and the estimation is carried out for each frame in turn to obtain continuous motion estimation.

[0079] After the feature point sets of the undistorted previous frame and the current frame are transformed from the pixel coordinate system to the camera coordinate system, the obtained point sets are P and Q respectively.

[0080] Taking the pose estimation of the previous moment as the initial estimation, the pose transformation matrix and the error function between the two are calculated. For outliers, if the estimated rotation angle exceeds the set threshold, it is considered invalid, and the pose estimation value of the previous moment is used to replace the current moment, and a magnification factor is applied to the error function to increase the uncertainty of the current pose estimation.

[0081] For the feature point sets P = {p1, p2, …, p n} and Q = {q1, q2, …, q n} of two frames of images, their relative pose is T = (R, t). For each group of matching points, the projection error function is defined as e i :

[0082] e i (T) = q i - (Rp i + t)

[0083] The projection error function represents the residual between the point p i of the previous frame transformed to the current frame coordinate system through the relative pose T and the actual point q i . The error function follows a Gaussian distribution, e i ~ N(0, Σ), where Σ is the covariance matrix.

[0084] The pose constraint is to minimize the sum of the squares of the projection errors of all matching point pairs:

[0085]

[0086] The sum of squared projection errors is optimized using the Levenberg-Marquardt (LM) optimization algorithm to calculate the relative pose and error. After calculating the pose transformation using the matching point sets of each group of images, the current frame is supplemented with the same number of key points detected and removed for use in the optical flow tracking of the next frame.

[0087] Step 3: Dynamically set key frames and perform loop detection on the key frames; a large number of ground texture images are pre-generated through DBoW3 training to generate a visual word bag model for ground textures; each key frame is inserted into the visually constructed word bag database in real time, and the loop frames are detected in the images of the visual word bag database through the visual word bag model of the ground texture, estimating the relative pose transformation and error between the current key frame and the loop frame, and adding them as loop constraints to the factor graph;

[0088] In this example, corresponding to Figure 1 Loop detection module. Each key frame is inserted into the visually constructed word bag database in real time, and the loop frames are detected in the images of the visual word bag database through the visual word bag model of the ground texture. After the key frame image is input into the loop detection thread, ORB key points are extracted and the BEBLID descriptors of each input frame are calculated, and the key frame image and its feature descriptors are added to the loop detection database together for querying candidate loop frames;

[0089] Only key frames are input into the loop detection, and initially the loop detection is not performed; a detection window parameter is set, and when the loop detection database reaches the set scale, candidate loop frames are queried, and only the candidate loop frames outside this detection window are queried; when querying candidate loop frames, only the results with the 4 largest index values in the past N frames will be queried subsequently; for these 4 candidate frames, the visual word bag score must exceed the set threshold to perform descriptor matching; if the number of well-matched descriptors exceeds the threshold, calculate the relative motion and covariance matrix between the current frame and the candidate loop frame; according to the covariance matrix, estimate the maximum uncertainty measure, and only when the maximum uncertainty measure value is lower than the threshold, add the loop constraint to the factor graph for updating.

[0090] When a loop frame matching the current key frame is detected, calculate the relative pose transformation and error between the current key frame and the loop frame, and add them as loop constraints to the factor graph;

[0091] Step 4: Perform incremental optimization through the factor graph composed of odometry constraints and loop constraints. When optimizing the pose, incrementally optimize the newly added measurement images and constraints to obtain the optimized pose;

[0092] The factor graph consists of nodes and edges. The nodes include variable nodes and factor nodes. The variable nodes are the IDs of each frame of the image, and the factor nodes represent the pose estimation and error between different variables. The edges connect the variable nodes and the factor nodes, representing the constraint relationships between the variables;

[0093] The composition of the factor graph in this system includes the relative pose constraint between frames, i.e., the odometry constraint, and the relative pose constraint between the current frame and the loop closure frame calculated by loop closure detection, i.e., the loop closure constraint. As the odometry constraint and the loop closure constraint are added to the factor graph, the pose optimization is performed using the ISAM2 method at regular intervals;

[0094] Corresponding Figure 2 For the factor graph example, the composition of the factor graph mainly has two parts. One is the relative pose and error between frames estimated by odometry, and the other is the relative pose and error between the current frame and the loop closure frame calculated by loop closure detection. The black connecting lines are the odometry constraints, and the red connecting lines are the loop closure constraints. As the odometry constraint and the loop closure constraint are added to the factor graph, the pose optimization is performed using the ISAM2 method at regular intervals to obtain the updated pose;

[0095] ISAM2 optimization process:

[0096] First, the factor graph is converted into a Bayesian tree through variable elimination. When new factors enter the factor graph and are updated, since the newly added factors may destroy the consistency of the original tree structure, the Bayesian tree needs to be adjusted. Find all the variable nodes in the factor graph that are associated with the new factors. Then, in the Bayesian tree, the subtrees corresponding to these variables are marked as the "affected regions". First, pruning is performed to remove the nodes of the affected subtrees and restore them to factor graph segments. For the pruned factor graph segments, elimination is performed again in the original elimination order to generate new subtrees. Then, the new subtrees are inserted and merged into the original Bayesian tree to keep the overall tree structure consistent. Then, incremental linearization and solution are performed. Only the newly added factors are linearized, and their Jacobian matrix J new and residual r new .

[0097] Update the information matrix H new and vector b new Use incremental QR decomposition to only decompose the affected rows and columns, avoiding global matrix operations.

[0098]

[0099] Through the structure of the Bayesian tree, back substitution is performed from the leaf nodes to the root nodes to calculate the variable increment Δθ and update the variable estimate:

[0100] θ new = θ old + Δθ

[0101] To control the computational scale, ISAM2 periodically marginalizes old variables, removing variables that are no longer needed, such as historical poses far from the current pose, from the optimization problem. When the residual error exceeds a threshold or a fixed time interval, all factors are re-linearized and the Bayesian tree is reconstructed.

[0102] Compared with the state-of-the-art ground texture localization algorithm in the current open-source feature point-based methods, the monocular simultaneous localization and mapping using ground textures (ICRA, 2023), the experimental results on the GeoTracking dataset show that the root mean square error is reduced by an average of 14.9%, and can even be reduced by 67.4% on specific ground; the overall accuracy is also improved by 12.9% after adding loop detection. For real-time performance, the front-end thread includes an average of 0.48 ms for image preprocessing and 24.82 ms for the odometry module, and the back-end optimization thread includes an average of 36.45 ms for the loop detection module. It should be noted here that the subsequent pose optimization is an incremental optimization and takes less time, and the time consumption has been included in the front-end thread when counting the time, meeting the requirements of real-time operation.

[0103] Table 1 shows the performance of the root mean square error (unit: m) of pose estimation on the GeoTracking dataset

[0104]

[0105] Table 2 shows the root mean square error of the complete SLAM system with loop optimization on the loop sequence

[0106]

Claims

1. A real-time robot positioning method based on ground texture, characterized in that It includes the following steps: Step 1: Obtain a ground image through a monocular camera at the bottom of the robot and perform limited contrast adaptive histogram equalization processing; Detect key points for the first frame to initialize the system, otherwise perform optical flow tracking; Step 2: Match the feature points of the current frame tracked by optical flow with the feature points of the previous frame, estimate the relative pose transformation of the image and calculate the pose transformation error, and add it to the factor graph as an odometry constraint; Step 3: Dynamically set key frames and perform loop detection on the key frames; Previously, a large number of ground texture images are generated through DBoW3 training to generate a visual word bag model of the ground texture; each key frame is inserted into the visual word bag database constructed in real time, and the loop frames are detected in the images of the visual word bag database through the visual word bag model of the ground texture, estimate the relative pose transformation and error between the current key frame and the loop frame, and add it to the factor graph as a loop constraint; Step 4: Perform incremental optimization through the factor graph composed of odometry constraints and loop constraints. When optimizing the pose, incrementally optimize the newly added measurement images and constraints to obtain the optimized pose.

2. The real-time positioning method of the robot based on ground texture according to claim 1, wherein The limited contrast adaptive histogram equalization processing in Step 1 is specifically as follows: Divide a ground texture image into n sub-blocks of the same size. Assume the sub-block size is B×B and the histogram is h k is the number of pixels with gray level i in the k-th sub-block. The normalized cumulative distribution function of each sub-block is: Define the contrast limit threshold clipLimit. For each sub-block, if the number of pixels of a certain gray level exceeds clipLimit, clip it to clipLimit and evenly distribute the excess pixels to other gray levels: where h′ k (i) is the histogram of the k-th sub-block after limiting the contrast, and excess is the number of pixels exceeding clipLimit; Then the mapping function of the k-th sub-block is s k : Using the method of bilinear interpolation, the new gray value of the pixel is calculated according to the mapping functions of the four surrounding sub-blocks. For P(x, y, i) in the non-boundary case, (x, y) is its pixel coordinate and i is the pixel value. Find its four adjacent sub-blocks. Let the mapping value of the histogram of the upper-left sub-block to point P be s′ lt , similarly, the mapping value of the histogram of the upper-right sub-block to point P is s′ rt , the mapping value of the histogram of the lower-left sub-block to point P is s′ ld , the mapping value of the histogram of the upper-left sub-block to point P is s′ rd ; Let the distance of point P from the center point of the upper-left sub-block be (Δx, Δy), then the normalized distance is: For non-boundary cases: s′ = (1 - Δy′)((1 - Δx′)s′ lt + Δx′s′ rt ) + Δy′((1 - Δx′)s′ ld + Δx′s′ rd ) For boundary cases, only consider its nearest one or two sub-blocks.

3. The real-time positioning method of the robot based on ground texture according to claim 1, characterized in that Key points are detected using the Shi Tomasi corner detection method in the first frame image. Different key points satisfy the minimum distance threshold, and the first frame is set as the key frame; the pose of the first frame is [0, 0, 0] T It is set as the initial pose.

4. The real-time positioning method of the robot based on ground texture according to claim 1, characterized in that Step 2 is specifically as follows: After performing limited contrast adaptive histogram equalization on the original ground texture image, perform optical flow tracking in the current frame through the key points in the previous frame to obtain the corresponding key points in adjacent frames; calculate the pose transformation of the current frame relative to the previous frame through bidirectional optical flow, RANSAC processing, and distortion removal, and estimate it sequentially for each frame of image to obtain continuous motion estimation; After converting the feature point sets of the undistorted previous frame and the current frame from the pixel coordinate system to the camera coordinate system, the obtained point sets are P and Q respectively; Take the pose estimate of the previous moment as the initial estimate, calculate the pose transformation matrix and error function between the two; for outliers, if the estimated rotation angle exceeds the set threshold, it is considered invalid, replace the current moment with the pose estimate value of the previous moment, and apply a magnification factor to the error function to increase the uncertainty of the current pose estimate; For two sets of feature points of images \(P=\{p_1,p_2,\cdots,p\) n \}\) and \(Q = \{q_1,q_2,\cdots,q\) n \}\), their relative pose is \(T=(R,t)\). For each set of matching points, define the projection error function as \(e\) i :\) e i (T) = q i -(Rp i + t) The projection error function represents the residual between the point p of the previous frame transformed to the current frame coordinate system by the relative pose T i and the actual point q i ; The error function follows a Gaussian distribution, e i ~ N(0, Σ), where Σ is the covariance matrix; The pose constraint is to minimize the sum of the squared projection errors of all matching point pairs:

5. The real-time positioning method of the robot based on ground texture according to claim 4, characterized in that The optical flow tracking is implemented based on the brightness constancy assumption; the brightness constancy assumption is that the pixel brightness of the same spatial point in consecutive frames remains unchanged, and the position change of feature points in the image is tracked by minimizing the brightness error; using the previous frame image, the sparse feature point set of the previous frame image, and the next frame image, a point set with the same features in the next frame image is obtained, and the optical flow between the sparse feature point sets of the two frame images is judged; to ensure the correctness and consistency of the optical flow tracking, an inverse optical flow tracking is performed again. If the matching points meet the threshold conditions, it is considered that both the forward and backward tracking are correct. At this time, only these matching points are retained, and then the feature points with inconsistent tracking are removed according to the homography matrix; after calculating the pose transformation using this set of matching points, the current frame is supplemented with the same number of Shi-Tomas corner points as the detected and removed ones for the optical flow tracking of the next frame.

6. The real-time positioning method of the robot based on ground texture according to claim 1, characterized in that The key frames are set at fixed frame intervals, and the current frame in which the number of well-matched feature points after screening between adjacent frames is lower than a certain threshold; each key frame and its features are inserted into the loop closure detection database for loop closure detection.

7. The real-time positioning method of the robot based on ground texture according to claim 6, characterized in that The loop closure detection is specifically as follows: when the loop closure detection of the key frame image is performed, ORB key points are extracted and the BEBLID descriptors of each input key frame are calculated, and the key frame image and its feature descriptors are added to the loop closure detection database together for querying candidate loop closure frames; Only key frames are input into the loop closure detection, and the loop closure detection is not initially performed; a detection window parameter is set. When the loop closure detection database reaches the set scale, candidate loop closure frames will be queried, and only the candidate loop closure frames outside this detection window will be queried; when querying candidate loop closure frames, only the results with the 4 largest index values in the past N frames will be queried subsequently; for these 4 candidate frames, the visual bag-of-words score must exceed the set threshold to perform descriptor matching; if the number of well-matched descriptors exceeds the threshold, the relative motion and covariance matrix between the current frame and the candidate loop closure frame are calculated; According to the covariance matrix, the maximum uncertainty measure is estimated. Only when the maximum uncertainty measure value is lower than the threshold, the loop closure constraint is added to the factor graph for updating.

8. The real-time robot positioning method based on ground texture according to claim 1, characterized in that, The composition of the factor graph includes the relative pose constraint between frames, i.e., the odometry constraint, and the relative pose constraint between the current frame and the loop closure frame calculated by the loop closure detection, i.e., the loop closure constraint; as the odometry constraint and the loop closure constraint are added to the factor graph, the pose optimization is performed once every certain time using the ISAM2 method to obtain the updated pose; The factor graph consists of nodes and edges. The nodes include variable nodes and factor nodes. The variable nodes are the IDs of each frame image, and the factor nodes represent the pose estimation and error between different variables; The edges connect the variable nodes and the factor nodes, representing the constraint relationships between the variables.

Images