A Visual Localization Method for Quadruped Robots in Dynamic Scenes

Abstract

The present invention relates to a visual positioning method for a quadruped robot in a dynamic scenario. The rectangular detection frames of dynamic objects in the scene image are recognized by the target detection algorithm YOLOv8, and the static feature points outside the frames are obtained; the rectangular detection frames are synchronized to the depth image, and the connected component algorithm is used to connect the regions with similar depth values in the depth image together to obtain the instance segmentation mask of the dynamic object; the LK local optical flow algorithm is used to identify the preliminary static feature points inside the rectangular detection frame and outside the instance segmentation mask; the density clustering algorithm is used to divide the preliminary static feature points inside the frame into different clusters according to the depth values, and the precise static feature points inside the frame that do not fall within the instance segmentation mask are identified for the second time; the static feature points outside the frame and the precise static feature points inside the frame are merged to obtain the total static feature points. The present invention solves the problem of inaccurate visual positioning in the prior art.

Description

Technical Field

[0001] The present invention belongs to the technical field of visual positioning, and relates to a visual positioning method for a quadruped robot in a dynamic scene. Background Art

[0002] With the rapid progress and development of today's robot industry technology, the landing applications of autonomous robots have become more and more extensive in various fields. Different from those wheeled robots with stable chassis and often no movement in the height direction, quadruped robots usually have the ability to move with 12 degrees of freedom. For quadruped robots, in order to be able to move autonomously in complex environments, the function of visual perception and positioning is particularly crucial and important. When a depth camera is equipped on a quadruped robot, it can have the powerful function of positioning in a three-dimensional space environment. Specifically, the positioning system first extracts feature points from the image, and then after performing the inter-frame matching operation, it uses a tracking model to calculate the pose of the camera.

[0003] However, in the scenario of human-robot collaboration, objects in a constantly moving state in the scene, such as pedestrians, will cause the positions of the feature points extracted on them at the previous moment and the feature points extracted on them at the next moment to be different in the three-dimensional space. This situation will cause the inter-frame matching to fail, thus bringing great interference to the normal operation of the robot positioning system. Under this interference, the positioning system cannot give the relatively accurate pose of the camera. Summary of the Invention

[0004] The present invention provides a visual positioning method for a quadruped robot in a dynamic scene, aiming to obtain the instance segmentation mask of dynamic objects in real time through an object detection algorithm and a depth image, and use the optical flow method to amplify feature points and use clustering and geometric constraints to re-determine static feature points to solve the problem of inaccurate visual positioning in the prior art.

[0005] The object of the present invention can be achieved by the following technical solutions:

[0006] The present application provides a visual positioning method for a quadruped robot in a dynamic scene, including the following steps:

[0007] S1. Feature point acquisition: Install a depth camera on the quadruped robot to capture the front scene, and use the ORB algorithm to extract feature points from the scene image;

[0008] S2. Preliminary elimination of feature points: Use the object detection algorithm YOLOv8 to identify dynamic objects in the scene image, obtain the rectangular detection frames of the dynamic objects in the scene image, and preliminarily eliminate the feature points located within the rectangular detection frames to obtain static feature points outside the frames;

[0009] S3. Obtain the instance segmentation mask of the dynamic object: Synchronize the rectangular detection box to the depth image, use the connected component algorithm to connect the regions with similar depth values in the depth image together, and finally take the regions that meet the preset depth value threshold as the instance segmentation mask of the dynamic object;

[0010] S4. Feature point amplification: Use the LK local optical flow algorithm to identify the initial static feature points inside the rectangular detection box and outside the instance segmentation mask;

[0011] S5. Feature point selection and merging: Use the density clustering algorithm DBSCAN to divide the initial static feature points inside the box into different clusters according to the depth values, and mark the clusters that fall within the instance segmentation mask as high-motion probability feature points, and the remaining clusters are determined as the exact static feature points inside the box; Merge the static feature points outside the box and the exact static feature points inside the box to obtain the total static feature points;

[0012] S6. Perform subsequent operations of ORB-SLAM3 on the total static feature points for real-time estimation of the camera pose of the quadruped robot.

[0013] Further, in step S3, the size of the depth image is the same as that of the scene image, and the distance between the pixel point corresponding camera and the object being photographed is saved at each pixel position of the depth image.

[0014] Further, in step S3, for the connected component algorithm, all adjacent same-depth values are found and marked as connected after scanning the depth image twice. Specifically, the first scan starts from the upper left corner of the depth image, scans pixel by pixel from left to right and from top to bottom, and determines whether they belong to the same class according to whether the depth values of the current pixel and its left and upper adjacent pixels are the same, obtaining the class serial numbers of each pixel; The second scan updates the serial numbers in the classes obtained from the first scan to identify pixels that are actually in the same region but have different class serial numbers.

[0015] Further, in step S3, for the depth value threshold, the selection formula is:

[0016] Vdepth0.5×(m+t)+α·SD

[0017] In the formula, Vdepth is the depth value threshold; m is the median of the effective depths of all pixel points after sorting; t is the tripartite quantile of the effective depths of all pixel points after sorting; α is an adjustable factor, adjusted according to the usage scenario, and can be taken as 0.1 in general scenarios; SD is the standard deviation of the effective depths of all pixel points.

[0018] Further, in step S4, the use of the LK local optical flow algorithm to identify the initial static feature points inside the rectangular detection box and outside the instance segmentation mask includes the following steps:

[0019] Traverse all pixel points inside the rectangular detection box and determine whether each point is outside the instance segmentation mask;

[0020] For the points outside the instance segmentation mask, calculate the brightness change of the pixel points in the neighborhood in two consecutive frames of images. By comparing the brightness differences of the corresponding pixel points in the neighborhood of adjacent frames of images, the motion vector of the pixel points is obtained.

[0021] If the motion vector is less than the preset value, mark the pixel point as a preliminary static feature point inside the box.

[0022] Further, in step S5, using the density clustering algorithm DBSCAN to divide the preliminary static feature points inside the box into different clusters according to the depth value includes the following steps:

[0023] S51. Determine parameters: Set the neighborhood radius and the minimum number of samples of the DBSCAN algorithm; the neighborhood radius represents the neighborhood range of the feature points. When the distance between two feature points is less than the neighborhood radius, they are in each other's neighborhood; the minimum number of samples represents the minimum number of neighbors required around a specified core point. The feature points that meet this number are marked as core points;

[0024] S52. Traverse the feature points: Calculate the distance between each feature point and all other feature points.

[0025] S53. Mark core points and boundary points: Classify each feature point according to the set neighborhood radius and minimum number of samples; specifically: The points with the number of feature points in the neighborhood not less than the minimum number of samples are marked as core points; The points that are not core points but are in the neighborhood of core points are marked as boundary points; The points that are neither core points nor boundary points are marked as noise points;

[0026] S54. Form clusters: Start from an unvisited core point, traverse all the points in its neighborhood, and mark them as the same cluster; For each visited point, continue to traverse the points in its neighborhood until all reachable points are marked as the same cluster; Repeat the process of step S54 until all core points have been visited or all points have been marked as a certain cluster or noise point.

[0027] S55. Process noise points: For the feature points marked as noise points, take the noise points as a separate small cluster, or assign them to a suitable cluster according to needs in subsequent processing.

[0028] Further, in step S5, the epipolar geometry method is also used to secondarily evaluate the motility of high-probability motion feature points.

[0029] Further, in step S6, the subsequent operations of ORB-SLAM3 on the total static feature points include:

[0030] S61. Feature extraction and matching: Input the total static feature points into the ORB-SLAM3 system to extract feature descriptors, use the feature descriptors to perform feature point matching between consecutive image frames, find similar point pairs by comparing the feature descriptors of feature points in different frames, and determine the corresponding relationships of feature points in different frames;

[0031] S62. Camera pose estimation: Based on the well-matched feature point pairs, use geometric constraints and optimization algorithms such as bundle adjustment, and adjust the camera pose parameters by minimizing the reprojection error. Through continuous iteration, obtain the optimal camera pose estimation result;

[0032] S63. Map construction: While estimating the camera pose, integrate the total static feature points and the camera pose into the map together. Specifically, adopt a key-frame-based map construction or incremental map construction method to help the quadruped robot understand the surrounding environment;

[0033] S64. Loop detection and optimization: Detect whether the robot returns to a previously visited position during the movement of the robot. Once a loop is detected, perform global optimization, adjust the structure of the entire map and the camera pose, eliminate the cumulative error, and further improve the positioning accuracy and consistency; Figure 1 consistency;

[0034] S65. Real-time update and output: Continuously update the camera pose and the map as the robot moves to adapt to environmental changes.

[0035] Advantages of the present invention:

[0036] By using the object detection algorithm YOLOv8 to identify the rectangular detection frames of dynamic objects in the scene image, obtaining the static feature points outside the frames; synchronizing the rectangular detection frames to the depth image, using the connected component algorithm to connect the regions with similar depth values in the depth image together, obtaining the instance segmentation mask of the dynamic object; using the LK local optical flow algorithm to identify the preliminary static feature points inside the rectangular detection frame and outside the instance segmentation mask; using the density clustering algorithm to divide the preliminary static feature points inside the frame into different clusters according to the depth values, and secondarily identifying the accurate static feature points inside the frame that do not fall within the instance segmentation mask; merging the static feature points outside the frame and the accurate static feature points inside the frame to obtain the total static feature points. The present invention obtains the instance segmentation mask of the dynamic object in real time through the object detection algorithm and the depth image, and uses the optical flow method to augment the feature points and uses clustering and geometric constraints to secondarily determine the static feature points, so as to solve the problem of inaccurate visual positioning in the prior art. Description of the drawings

[0037] For the convenience of those skilled in the art to understand, the present invention will be further described below in conjunction with the accompanying drawings.

[0038] Figure 1 It is a flowchart of a visual positioning method for a quadruped robot in a dynamic scenario in the present invention.

[0039] Figure 2 It is a flowchart of initially identifying static feature points within the recognition frame by the LK local optical flow algorithm in an embodiment of the present invention.

[0040] Figure 3 It is a flowchart of using the density clustering algorithm DBSCAN to divide the initially identified static feature points within the frame into different clusters according to the depth value in an embodiment of the present invention.

[0041] Figure 4 It is a schematic diagram of the epipolar geometry method in an embodiment of the present invention. Specific Embodiments

[0042] To further elaborate on the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the following will, in conjunction with the accompanying drawings and preferred embodiments, describe in detail the specific embodiments, structures, features, and their effects of the present invention as follows.

[0043] Please refer to Figures 1 - 4 , this application provides a visual positioning method for a quadruped robot in a dynamic scenario, including the following steps:

[0044] S1. Feature point acquisition: Install a depth camera on the quadruped robot to capture the front scene, and use the ORB algorithm to extract feature points from the scene image;

[0045] In this embodiment, the depth camera is firmly installed at a specific position on the quadruped robot so that it can stably capture the front scene. The installation angle and position of the depth camera are carefully adjusted to ensure that the most comprehensive and clear front environment information can be obtained.

[0046] First, when the quadruped robot moves in different environments, the depth camera continuously captures the front scene images. These images contain rich environmental information, such as terrain, obstacles, the shape and texture of objects, etc.

[0047] Secondly, use the ORB algorithm to extract feature points from these scene images. The ORB algorithm first quickly searches for possible feature points in the image through an improved FAST corner detection algorithm. It compares the gray-scale difference between a pixel point in the image and the surrounding neighborhood pixel points, and when the difference reaches a certain threshold, the pixel point is marked as a potential feature point.

[0048] Then, to make the feature points rotation-invariant, the algorithm calculates the gray centroid of the image. By determining the gray center of gravity of the pixels around the feature points, a direction vector is obtained, thereby assigning a specific direction to each feature point.

[0049] Finally, an improved BRIEF descriptor is used to generate the binary descriptors of the feature points. These descriptors are concise and efficient, enabling rapid matching and recognition of feature points. They capture the local texture information around the feature points, allowing the same feature points to be accurately recognized even under different lighting conditions and viewpoints.

[0050] S2. Preliminary elimination of feature points: Use the object detection algorithm YOLOv8 to identify dynamic objects in the scene image, obtain the rectangular detection boxes of the dynamic objects in the scene image, preliminarily eliminate the feature points located within the rectangular detection boxes, and obtain the static feature points outside the boxes.

[0051] In this embodiment, first, the object detection algorithm YOLOv8 is used to process the scene image captured by the depth camera. YOLOv8 is an efficient and accurate object detection algorithm that can quickly identify various objects in the image. When analyzing the scene image, YOLOv8 divides the image into multiple grids and predicts the possible object categories and bounding boxes in each grid. Through multiple convolution and pooling operations on the image, the algorithm can extract the high-level features in the image and use these features for object detection. When YOLOv8 identifies the dynamic objects in the scene image, it will give the rectangular detection boxes of these objects in the image. These detection boxes accurately frame the positions and ranges of the dynamic objects.

[0052] Secondly, according to the obtained rectangular detection boxes of the dynamic objects, the feature points located within these boxes are preliminarily eliminated. This is because the feature points on the dynamic objects may change with the movement of the objects, thus interfering with the visual positioning of the quadruped robot. By eliminating these feature points, the influence of dynamic objects on the positioning system can be reduced. The specific elimination process can be achieved by traversing all the extracted feature points and determining whether each feature point is located within the rectangular detection box of any dynamic object. If the feature point is within the detection box, it is marked as a feature point to be eliminated. Finally, these marked feature points are deleted from the feature point set, and only the feature points on the static objects are retained. By using YOLOv8 to identify the dynamic objects in the scene image and preliminarily eliminating the feature points located within the rectangular detection boxes, the interference of dynamic objects on the visual positioning of the quadruped robot can be effectively reduced, and the accuracy and stability of the positioning system can be improved.

[0053] S3. Obtain the instance segmentation mask of the dynamic object: Synchronize the rectangular detection frame to the depth image, use the connected component algorithm to connect the regions with similar depth values in the depth image, and finally take the region that meets the preset depth value threshold as the instance segmentation mask of the dynamic object;

[0054] In this embodiment, after completing the annotation of the rectangular detection frames of the dynamic objects in the scene image, next, synchronize these rectangular detection frames to the depth image. The depth image provides the distance information between the objects in the scene and the camera, which is crucial for accurately identifying dynamic objects. During the synchronization process, ensure that the positions of the rectangular detection frames in the scene image and the depth image correspond accurately, which can be achieved through coordinate transformation using the camera's internal parameter matrix and external parameter matrix.

[0055] Then, use the connected component algorithm to process the depth image. The purpose of the connected component algorithm is to connect the regions with similar depth values in the depth image. The algorithm traverses each pixel in the depth image and checks the depth value difference between it and the surrounding pixels. If the difference is within a certain range, these pixels are marked as belonging to the same connected region. After connecting the regions with similar depth values, finally take the region that meets the preset depth value threshold as the instance segmentation mask of the dynamic object. The preset depth value threshold is determined according to the actual scene and requirements, and it is used to distinguish dynamic objects from static backgrounds. Generally, the depth values of dynamic objects will fluctuate within a certain range, while the depth values of static backgrounds are relatively stable. By comparing the depth values of each connected region with the preset depth value threshold, it can be determined which regions may belong to dynamic objects. Mark the regions that meet the threshold conditions as the instance segmentation mask of the dynamic object. This mask can accurately outline the shape and position of the dynamic object in the depth image. The obtained instance segmentation mask of the dynamic object can be further used to eliminate the feature points that may interfere with the visual positioning of the quadruped robot. By comparing the mask with the positions of the feature points, it can be determined which feature points are located on the dynamic object and eliminate them, thereby improving the accuracy and stability of the positioning system.

[0056] Furthermore, the size of the depth image is the same as that of the scene image, and the distance between the pixel point corresponding camera and the object being photographed is saved at each pixel position of the depth image.

[0057] In this embodiment, the depth image is the same size as the scene image. This means that for each pixel position in the scene image, there is a corresponding pixel position in the depth image. The uniqueness of the depth image is that the information saved at each pixel position is the distance between the camera corresponding to that pixel point and the object being photographed. This distance value can be obtained through the measurement principle of the depth camera. Depth cameras usually use technologies such as structured light and time-of-flight to measure the distance between the object and the camera, and store these distance values in the depth image in the form of pixels.

[0058] Further, in the connected region algorithm, all adjacent pixels with the same depth value are found and marked as connected after scanning the depth image twice. Specifically, the first scan starts from the upper left corner of the depth image and scans pixel by pixel from left to right and top to bottom. Whether the current pixel and its left and upper adjacent pixels belong to the same class is determined based on whether their depth values are the same, and the class serial numbers of each pixel are obtained. The second scan updates the serial numbers in the classes obtained from the first scan to identify pixels that are actually in the same region but have different class serial numbers.

[0059] The selection of the depth value threshold is determined based on the statistical distribution of the depth values, aiming to distinguish the foreground and background to the greatest extent.

[0060] Further, the formula for selecting the depth value threshold is:

[0061] Vdepth0.5×(m + t)+α·SD

[0062] In the formula, Vdepth is the depth value threshold; m is the median of the sorted valid depths of all pixel points; t is the tripartite quantile of the sorted valid depths of all pixel points; α is an adjustable factor, which is adjusted according to the usage scenario, and can be taken as 0.1 in general scenarios; SD is the standard deviation of the valid depths of all pixel points.

[0063] S4. Feature point amplification: Use the LK local optical flow algorithm to identify the initial static feature points inside the rectangular detection frame and outside the instance segmentation mask.

[0064] In this embodiment, the LK (Lucas-Kanade) local optical flow algorithm is used to analyze the image. The LK local optical flow algorithm is a method for calculating the movement of pixel points in an image. It is based on the assumption of brightness invariance of the image, that is, within a short period of time, the brightness of objects in the image remains unchanged. When applying the LK local optical flow algorithm, the focus is on the area inside the rectangular detection frame and outside the instance segmentation mask. The rectangular detection frame is the range of the dynamic object identified by the target detection algorithm YOLOv8 in the scene image, and the instance segmentation mask is a more accurate dynamic object area determined by the connected region algorithm and the preset depth value threshold.

[0065] For the pixel points located inside the rectangular detection box and outside the instance segmentation mask, the LK local optical flow algorithm calculates the motion vectors of these points between consecutive frame images. By comparing the brightness changes of corresponding pixel points in adjacent frame images, the algorithm can determine the displacement direction and magnitude of the pixel points. The purpose of identifying these feature points is to further determine the feature points that may be affected by dynamic objects but are not fully included in the instance segmentation mask. These feature points may interfere with the visual positioning of the quadruped robot, so they need to be identified and processed.

[0066] The specific identification process can be achieved through the following steps: First, traverse all pixel points inside the rectangular detection box and determine whether each point is outside the instance segmentation mask. For the points that meet the conditions, apply the LK local optical flow algorithm to calculate their motion vectors. If the motion vector is large, it indicates that the point may be affected by dynamic objects and needs further analysis and processing. For example, it can be marked as a potential interfering feature point for elimination or weighted processing in subsequent positioning calculations. By using the LK local optical flow algorithm to identify the static feature points located inside the rectangular detection box and outside the instance segmentation mask, the impact of dynamic objects on visual positioning can be considered more comprehensively, improving the positioning accuracy and stability of the quadruped robot in dynamic scenarios.

[0067] S5. Feature point selection and merging: Use the density clustering algorithm DBSCAN to divide the preliminary static feature points inside the box into different clusters according to the depth value, and mark the clusters that fall within the instance segmentation mask as high-motion-probability feature points, and determine the remaining clusters as the accurate static feature points inside the box; Merge the static feature points outside the box and the accurate static feature points inside the box to obtain the total static feature points;

[0068] In this embodiment, first, the density-based spatial clustering algorithm DBSCAN is used to process the preliminary static feature points within the rectangular detection frame. The DBSCAN algorithm is a density-based spatial clustering algorithm that can cluster points with similar densities together to form different clusters. During this process, the algorithm divides according to the depth values of the feature points. Since the depth value reflects the distance between the pixel point corresponding to the camera and the object being photographed, feature points with similar depth values are likely to belong to the same object or the same plane. Through the DBSCAN algorithm, the preliminary static feature points within the frame can be divided into different clusters according to the similarity of depth values. Then, the clusters that fall within the instance segmentation mask are marked as high-motion-probability feature points. This is because the area determined by the instance segmentation mask is considered the range of dynamic objects, and the feature points that fall within this range are likely to change as the dynamic objects move. Therefore, these feature points are marked as high-motion-probability feature points and need to be treated with caution in subsequent processing. For the remaining clusters, they are determined as the precise static feature points within the frame. These feature points are within the rectangular detection frame but not within the instance segmentation mask, so they are considered relatively stable static feature points.

[0069] Finally, the static feature points outside the frame and the precise static feature points within the frame are merged to obtain the total static feature points. The static feature points outside the frame are the remaining static feature points after initially removing the feature points within the rectangular detection frame, and the precise static feature points within the frame are the feature points that are within the rectangular detection frame and not within the instance segmentation mask determined by the DBSCAN algorithm. By merging these two parts of static feature points, a more complete and accurate set of total static feature points can be obtained.

[0070] Further, in step S5, using the density-based spatial clustering algorithm DBSCAN to divide the preliminary static feature points within the frame into different clusters according to the depth values includes the following steps:

[0071] S51. Determine parameters: Set the key parameters of the DBSCAN algorithm, the neighborhood radius (eps) and the minimum number of samples (min_samples). The neighborhood radius determines the neighborhood range of the feature points. If the distance between two feature points is less than this radius, they are within each other's neighborhoods. The minimum number of samples specifies the minimum number of neighbors required around a core point, and the feature points that meet this quantity are marked as core points.

[0072] S52. Traverse the feature points: Calculate the distance between each feature point and all other feature points. The distance comprehensively considers the difference in depth values of the feature points and the difference in their positions in the image. Common distance metrics such as Euclidean distance or Manhattan distance can be used. By calculating the distance, it lays the foundation for determining the neighborhood relationship between feature points in the subsequent process.

[0073] S53. Mark the core points and boundary points: Classify each feature point according to the set neighborhood radius and minimum number of samples. Points with the number of feature points in the neighborhood not less than the minimum number of samples are marked as core points; points that are not core points but are within the neighborhood of core points are marked as boundary points; points that are neither core points nor boundary points are marked as noise points.

[0074] S54. Form clusters: Start from an unvisited core point, traverse all the points (including core points and boundary points) in its neighborhood, and mark them as the same cluster. For each visited point, continue to traverse the points in its neighborhood until all reachable points are marked as the same cluster. Repeat this process until all core points are visited or all points are marked as a certain cluster or noise points.

[0075] S55. Process noise points: For the feature points marked as noise points, they can be further processed according to the actual situation. The noise points can be taken as a small separate cluster, or they can be assigned to a suitable cluster as needed in subsequent processing.

[0076] Furthermore, in step S5, the epipolar geometry method is also used to secondarily evaluate the motility of high-probability motion feature points.

[0077] In this embodiment, the epipolar geometry is a method based on binocular vision or multi-view geometry, which uses the correspondence relationship between images from different perspectives to infer the geometric structure of the scene and the motion state of objects. For high-motion-probability feature points, their motion consistency between different image frames can be further analyzed through the epipolar geometry method. Specifically, the epipolar geometry can calculate the fundamental matrix between two perspectives, and this matrix describes the geometric relationship of corresponding points in the two images.

[0078] If the correspondence relationship of a high-motion-probability feature point between different image frames conforms to the constraints of the epipolar geometry, then it can be considered that it indeed has a high probability of motion. On the contrary, if the feature point does not conform to the constraints of the epipolar geometry, then its motility may need to be re-evaluated, or it may be adjusted to a low-motion-probability feature point or even a static feature point. By using the epipolar geometry method for secondary evaluation, the motion state of high-motion-probability feature points can be determined more accurately, thereby improving the reliability and accuracy of visual positioning of the quadruped robot in a dynamic scene.

[0079] Specifically, as Figure 2 shown, it shows the epipolar geometry constraint relationship between two adjacent frames of images. P is a map point, P’ is the position of the map point after moving in space, and E is the distance from the matched feature point to the camera epipolar line. Let the feature matching points p 1 and p 2 in homogeneous coordinates be represented as p 1 =[u1 , v 1 , 1], p 2 = [u 2 , v 2 , 1], where u and v are the pixel coordinate values of the feature points. Let the fundamental matrix of the camera be denoted as F, which only depends on the internal and external parameters of the camera and can be obtained through general camera calibration procedures. The epipolar geometry constraint is as follows: The epipolar line l2 corresponding to the feature point p1 in the first frame in the second frame is, and Here XYZ is the intermediate value calculated. Figure 2 The distance E in 2 from the feature point p 2 to the epipolar line l

[0080]

[0081] For E exceeding the set threshold, the P point is considered a dynamic point in space, otherwise it is a static point.

[0082] S6. Perform subsequent operations of ORB-SLAM3 on the total static feature points for real-time estimation of the camera pose of the quadruped robot.

[0083] In this embodiment, ORB-SLAM3 is an advanced Simultaneous Localization and Mapping (SLAM) algorithm that can use the feature points in the image to determine the position and pose of the camera and construct a map of the environment. For a quadruped robot, accurate camera pose estimation is the key to achieving autonomous navigation and environmental perception. First, input the total static feature points into the feature tracking module of ORB-SLAM3. This module tracks the movement of feature points between consecutive image frames and determines the relative movement of the camera by matching feature points in different frames. Then, use these matched feature points for optimized estimation of the camera pose. ORB-SLAM3 usually adopts optimization algorithms such as Bundle Adjustment to adjust the camera pose parameters by minimizing the reprojection error, so that the projected position of the feature points in the image is as close as possible to the actual observed position. While estimating the camera pose, ORB-SLAM3 also constructs a map of the environment. The map can help the quadruped robot better understand the surrounding environment and provide a basis for navigation and path planning.

[0084] Furthermore, in step S6, the performing subsequent operations of ORB-SLAM3 on the total static feature points includes the following steps:

[0085] S61. Feature Extraction and Matching: After inputting the total static feature points into the ORB-SLAM3 system, the system will further process these feature points to extract more advanced feature descriptors. Then, these descriptors are used to perform feature point matching between consecutive image frames. By comparing the descriptors of feature points in different frames, similar point pairs are found to determine their corresponding relationships in different frames, providing a basis for subsequent camera pose estimation.

[0086] S62. Camera Pose Estimation: Based on the matched feature point pairs, ORB-SLAM3 starts to estimate the camera pose. Usually, geometric constraints and optimization algorithms such as bundle adjustment are used to adjust the camera pose parameters by minimizing the reprojection error, so that the projected position of the feature points in the image is as close as possible to the actual observation position. After continuous iteration, the optimal camera pose estimation result is obtained.

[0087] S63. Map Construction: While estimating the camera pose, ORB-SLAM3 begins to construct an environmental map. For the total static feature points, they will be integrated into the map together with the camera pose. Methods such as key-frame-based map construction or incremental map construction can be adopted. The map can help the quadruped robot better understand the surrounding environment and provide a basis for its navigation and path planning.

[0088] S64. Loop Closure Detection and Optimization: To improve the positioning accuracy and stability, ORB-SLAM3 performs loop closure detection, that is, detecting whether the robot returns to a previously visited position during its movement. Once a loop closure is detected, global optimization will be carried out to adjust the structure of the entire map and the camera pose, eliminate the accumulated error, and further improve the positioning accuracy and consistency. Figure 1 consistency.

[0089] S65. Real-time Update and Output: ORB-SLAM3 continuously receives new image frames and total static feature points and performs real-time updates. As the robot moves, the camera pose and the map are continuously updated to adapt to environmental changes. Finally, the real-time pose estimation of the camera and the constructed environmental map are output for the quadruped robot to use for tasks such as autonomous navigation and obstacle avoidance.

[0090] The above are only the preferred embodiments of the present invention and do not impose any formal restrictions on the present invention. Although the present invention has been disclosed above with preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or decorations equivalent to changes within the scope of the technical solution of the present invention. However, as long as it does not depart from the content of the technical solution of the present invention, any brief modifications, equivalent changes, and decorations made to the above embodiments based on the technical essence of the present invention still fall within the scope of the technical solution of the present invention.

Claims

1. A dynamic scene visual positioning method for a quadruped robot, characterized by: The following steps are involved: S1. Feature point acquisition: Install a depth camera on the quadruped robot to shoot the scene in front, and use the ORB algorithm to extract feature points from the scene image; S2. Preliminary elimination of feature points: Use the target detection algorithm YOLOv8 to identify dynamic objects in the scene image, obtain a rectangular detection frame of the dynamic object in the scene image, preliminarily eliminate feature points within the rectangular detection frame, and obtain static feature points outside the frame; S3, obtaining an instance segmentation mask of a dynamic object: synchronizing the rectangular detection frame to the depth image, connecting regions with similar depth values ​​in the depth image together using a connected domain algorithm, and finally taking regions that meet a preset depth value threshold as an instance segmentation mask of the dynamic object; S4, feature point expansion: using the LK local optical flow algorithm, identifying preliminary static feature points within the rectangular detection box and outside the instance segmentation mask; S5, feature point selection and merging: Use the density clustering algorithm DBSCAN to divide the preliminary static feature points in the frame into different clusters according to the depth value, and mark the clusters falling on the instance segmentation mask as high motion probability feature points, and determine the remaining clusters as precise static feature points in the frame; merge the static feature points outside the frame and the precise static feature points in the frame to obtain the total static feature points; S6, the total static feature points are subjected to subsequent operation of ORB-SLAM3 for real-time estimation of the camera pose of the quadruped robot; In step S4, the LK local optical flow algorithm is used to identify preliminary static feature points within the rectangular detection box and outside the instance segmentation mask, including the following steps: Traverse all pixels inside the rectangular detection box and determine whether each point is outside the instance segmentation mask; For points outside the instance segmentation mask, the brightness change of the pixels in the neighborhood is calculated in two consecutive frames, and the motion vector of the pixel is obtained by comparing the brightness difference of the pixels in the corresponding neighborhood in the adjacent frames. If the motion vector is smaller than a preset value, the pixel point is marked as a preliminary static feature point in the frame.

2. A quadruped robot dynamic scene visual positioning method according to claim 1, characterized in that: In step S3, the size of the depth image is consistent with the scene image, and each pixel position of the depth image stores the distance between the camera corresponding to the pixel and the object being photographed.

3. A quadruped robot dynamic scene visual positioning method according to claim 1, characterized in that: In step S3, the connected area algorithm finds all adjacent identical depth values ​​and marks them as connected after scanning the depth image twice. Specifically, the first scan starts from the upper left corner of the depth image, scans pixels one by one from left to right and from top to bottom, and determines whether the current pixel is classified into the same category based on whether the depth value of the left neighboring pixel and the upper neighboring pixel is the same, and obtains the category number of each pixel; the second scan updates the number in the category obtained by the first scan to identify pixels that are actually in the same area but have different category numbers.

4. A quadruped robot dynamic scene visual positioning method according to claim 1, characterized in that: In step S3, the depth value threshold is selected by the formula: ， In the formula, V depth is the depth value threshold; m It is the median of the sorted effective depths of all pixels; t It is the tertiary digit after sorting the effective depth of all pixels; α It is an adjustable factor, which is adjusted according to the usage scenario; SD is the standard deviation of the effective depth of all pixels.

5. A quadruped robot dynamic scene visual positioning method according to claim 1, characterized in that: In step S5, the use of the density clustering algorithm DBSCAN to divide the preliminary static feature points in the frame into different clusters according to the depth values ​​includes the following steps: S51, determine parameters: set the neighborhood radius and minimum number of samples of the DBSCAN algorithm; the neighborhood radius represents the neighborhood range of the feature point, and when the distance between two feature points is less than the neighborhood radius, they are in each other's neighborhood; the minimum number of samples represents the minimum number of neighbors required around the specified core point, and the feature points that meet this number are marked as core points; S52, traversing feature points: calculating the distance between each feature point and all other feature points; S53, marking core points and boundary points: classifying each feature point according to the set neighborhood radius and minimum sample number; specifically: points whose number of feature points in the neighborhood is not less than the minimum sample number are marked as core points; points that are not core points but in the neighborhood of core points are marked as boundary points; points that are neither core points nor boundary points are marked as noise points; S54, forming clusters: starting from a core point that has not been visited, traverse all points in its neighborhood and mark them as the same cluster; for each visited point, continue to traverse the points in its neighborhood until all reachable points are marked as the same cluster; repeat the process of step S54 until all core points have been visited or all points are marked as a cluster or noise points; S55, processing noise points: for feature points marked as noise points, the noise points are treated as a small cluster separately, or they are assigned to a suitable cluster as needed in subsequent processing.

6. A quadruped robot dynamic scene visual positioning method according to claim 1, characterized in that: In step S5, the epipolar geometry method is also used to secondarily evaluate the mobility of the high-probability motion feature points.

7. A quadruped robot dynamic scene visual positioning method according to claim 1, characterized in that: In step S6, the total static feature points are subjected to subsequent ORB-SLAM3 operations, including: S61, feature extraction and matching: input the total static feature points into the ORB-SLAM3 system to extract feature descriptors, use the feature descriptors to match feature points between consecutive image frames, find similar point pairs by comparing the feature descriptors of feature points in different frames, and determine the corresponding relationship between feature points in different frames; S62, Camera pose estimation: Based on the matched feature point pairs, use geometric constraints and optimization algorithms such as bundle adjustment, and adjust the camera pose parameters by minimizing the reprojection error. After continuous iteration, the optimal camera pose estimation result is obtained. S63, map construction: while estimating the camera pose, integrating the total static feature points together with the camera pose into the map, specifically using a keyframe-based map construction or incremental map construction method to help the quadruped robot understand the surrounding environment; S64, closed loop detection and optimization: During the robot's movement, it detects whether it has returned to a previously visited location. Once a closed loop is detected, global optimization is performed to adjust the structure of the entire map and the camera pose, eliminate cumulative errors, and further improve positioning accuracy and map consistency; S65, real-time update and output: As the robot moves, the camera pose and map are continuously updated to adapt to environmental changes.

Images