Dynamic scene visual positioning method for quadruped robot

Abstract

The invention relates to a quadruped robot dynamic scene visual positioning method comprising the following steps: identifying a rectangular detection frame of a dynamic object in a scene image through a target detection algorithm YOLOv8, and obtaining static feature points outside the frame; synchronizing the rectangular detection frame to the depth image, connecting areas with similar depth values of the depth image together by using a connected domain algorithm, and obtaining an instance segmentation mask of the dynamic object; using an LK local optical flow algorithm to identify in-frame preliminary static feature points inside the rectangular detection frame and outside the instance segmentation mask; dividing the preliminary static feature points in the frame into different clusters according to depth values by using a density clustering algorithm, and secondarily identifying the accurate static feature points in the frame which do not fall into the instance segmentation mask; and merging the out-frame static feature points and the in-frame accurate static feature points to obtain total static feature points. The problem of inaccurate visual positioning in the prior art is solved.

Description

Technical Field

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

[0002] With the rapid progress and development of technology in today's robotics industry, autonomous robots are being used more and more widely in various fields. Unlike wheeled robots that have a stable chassis and often do not move in the height direction, quadruped robots usually have 12 degrees of freedom. For quadruped robots, in order to achieve autonomous movement in complex environments, the function of visual perception and positioning is particularly critical and important. When a quadruped robot is equipped with a depth camera, 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 uses the tracking model to calculate the camera's position after performing inter-frame matching operations.

[0003] However, in the human-machine collaboration scenario, pedestrians and other objects in constant motion in the scene will cause the feature points extracted on them at the previous moment to be different from the feature points extracted at the next moment in the three-dimensional space. This situation will make the inter-frame matching invalid, thus greatly interfering with the normal operation of the robot positioning system. Under such interference, the positioning system cannot give a relatively accurate position and posture of the camera. Summary of the invention

[0004] The present invention provides a dynamic scene visual positioning method for a quadruped robot, aiming to obtain instance segmentation masks of dynamic objects in real time through target detection algorithms and depth images, and to amplify feature points using the optical flow method and to secondarily determine static feature points using clustering and geometric constraints, so as to solve the problem of inaccurate visual positioning in the prior art.

[0005] The purpose of the present invention can be achieved through the following technical solutions:

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

[0007] 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;

[0008] 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;

[0009] 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;

[0010] 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;

[0011] 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;

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

[0013] Furthermore, 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.

[0014] Furthermore, 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.

[0015] Furthermore, in step S3, the depth value threshold is selected by the formula:

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

[0017] Where Vdepth is the depth value threshold; m is the median of the effective depths of all pixels after sorting; t is the tertiary of the effective depths of all pixels after sorting; α is an adjustable factor that is 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 pixels.

[0018] Furthermore, in step S4, the use of the LK local optical flow algorithm to identify preliminary static feature points within the rectangular detection frame and outside the instance segmentation mask includes the following steps:

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

[0020] 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.

[0021] If the motion vector is smaller than a preset value, the pixel point is marked as a preliminary static feature point in the frame.

[0022] Furthermore, 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:

[0023] 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;

[0024] S52, traversing feature points: calculating the distance between each feature point and all other feature points;

[0025] 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;

[0026] 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;

[0027] 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.

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

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

[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 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;

[0031] 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.

[0032] 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;

[0033] 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 position to eliminate accumulated errors and further improve positioning accuracy and location. Figure 1 consistency;

[0034] S65, real-time update and output: As the robot moves, the camera pose and map are continuously updated to adapt to environmental changes.

[0035] Beneficial effects of the present invention:

[0036] The rectangular detection frame of the dynamic object in the scene image is identified by the target detection algorithm YOLOv8, and the static feature points outside the frame are obtained; the rectangular detection frame is synchronized to the depth image, and the areas with similar depth values ​​in the depth image are connected together using the connected domain algorithm 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 in the frame that are inside the rectangular detection frame and outside the instance segmentation mask; the density clustering algorithm is used to divide the preliminary static feature points in the frame into different clusters according to the depth value, and the precise static feature points in the frame that do not fall on the instance segmentation mask are identified twice; the static feature points outside the frame and the precise static feature points in the frame are merged to obtain the total static feature points. The present invention obtains the instance segmentation mask of the dynamic object in real time through the target detection algorithm and the depth image, and uses the optical flow method to amplify the feature points and uses clustering and geometric constraints to determine the static feature points twice, so as to solve the problem of inaccurate visual positioning in the prior art. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] In order to facilitate understanding by those skilled in the art, the present invention is further described below with reference to the accompanying drawings.

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

[0039] Figure 2 This is a flowchart of the LK local optical flow algorithm for identifying preliminary static feature points within a frame in one embodiment of the present invention.

[0040] Figure 3 This is a flowchart of using the density clustering algorithm DBSCAN to divide preliminary static feature points in a frame into different clusters according to depth values ​​in one embodiment of the present invention.

[0041] Figure 4 Schematic diagram of the epipolar geometry method in one embodiment of the present invention. DETAILED DESCRIPTION

[0042] In order to further explain the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the specific implementation mode, structure, characteristics and effects of the present invention are described in detail below in combination with the accompanying drawings and preferred embodiments.

[0043] See also Figure 1-Figure 4 The present application provides a method for visual positioning of a quadruped robot in a dynamic scene, comprising the following steps:

[0044] 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;

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

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

[0047] Secondly, the ORB algorithm is used to extract feature points from these scene images. The ORB algorithm first uses the improved FAST corner detection algorithm to quickly search for possible feature points in the image. It compares the grayscale difference between a pixel in the image and the surrounding neighboring pixels. When the difference reaches a certain threshold, the pixel is marked as a potential feature point.

[0048] Then, in order to make the feature points rotationally invariant, the algorithm calculates the grayscale centroid of the image. By determining the grayscale centroid of the pixels around the feature point, a direction vector is obtained, thus giving each feature point a specific direction.

[0049] Finally, the improved BRIEF descriptor is used to generate binary descriptors of feature points. These descriptors are concise and efficient, and can quickly match and identify feature points. They capture the local texture information around the feature points, so that the same feature points can be accurately identified even under different lighting conditions and viewing angles.

[0050] 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;

[0051] In this embodiment, first, the scene image captured by the depth camera is processed using the target detection algorithm YOLOv8. YOLOv8 is an efficient and accurate target 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. By performing multiple convolution and pooling operations on the image, the algorithm can extract high-level features in the image and use these features for target detection. When YOLOv8 identifies dynamic objects in the scene image, rectangular detection frames of these objects are given in the image. These detection frames accurately define the position and range of dynamic objects.

[0052] Secondly, based on the obtained rectangular detection frame of the dynamic object, the feature points located in these frames are preliminarily eliminated. This is because the feature points located on the dynamic object may change with the movement of the object, thereby interfering with the visual positioning of the quadruped robot. By eliminating these feature points, the impact of dynamic objects on the positioning system can be reduced. The specific elimination process can be performed by traversing all the extracted feature points to determine whether each feature point is located in the rectangular detection frame of any dynamic object. If the feature point is in the detection frame, 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 located on the static object are retained. By using YOLOv8 to identify dynamic objects in the scene image and preliminarily eliminating the feature points located in the rectangular detection frame, 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, 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;

[0054] In this embodiment, after the rectangular detection frames of the dynamic objects in the scene image are marked, these rectangular detection frames are synchronized 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, it is ensured that the positions of the rectangular detection frames in the scene image and the depth image correspond accurately, which can be achieved by coordinate transformation through the camera's intrinsic and extrinsic matrix.

[0055] Then, the depth image is processed using the connected domain algorithm. The purpose of the connected domain algorithm is to connect regions with similar depth values ​​in the depth image. The algorithm traverses each pixel in the depth image and checks the difference in depth value 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 regions with similar depth values, the region that meets the preset depth value threshold is finally taken 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. Usually, the depth value of a dynamic object fluctuates within a certain range, while the depth value of the static background is relatively stable. By comparing the depth values ​​of each connected region with the preset depth value threshold, it is possible to determine which regions may belong to dynamic objects. The regions that meet the threshold conditions are marked as instance segmentation masks of dynamic objects, which can accurately outline the shape and position of dynamic objects in the depth image. The dynamic object instance segmentation mask obtained in this way can be further used to eliminate feature points that may interfere with the visual positioning of the quadruped robot. By comparing the mask with the position of the feature points, it is possible to determine 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 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.

[0057] In this embodiment, the depth image is consistent in size with 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 stored in each pixel position is the distance between the camera and the object at the corresponding position of the pixel. This distance value can be obtained through the measurement principle of the depth camera. Depth cameras usually use structured light, time of flight and other technologies to measure the distance between the object and the camera, and store these distance values ​​in the form of pixels in the depth image.

[0058] Furthermore, 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 each pixel 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.

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

[0060] Furthermore, the depth value threshold is selected by the formula:

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

[0062] Where Vdepth is the depth value threshold; m is the median of the effective depths of all pixels after sorting; t is the tertiary of the effective depths of all pixels after sorting; α is an adjustable factor that is 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 pixels.

[0063] S4. Feature point expansion: Use the LK local optical flow algorithm to identify preliminary static feature points within the rectangular detection box 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 motion of pixels in an image. It is based on the assumption that the brightness of the image remains constant, that is, the brightness of the object in the image remains constant for a short period of time. When applying the LK local optical flow algorithm, the focus is on the area inside the rectangular detection box and outside the instance segmentation mask. The rectangular detection box is the range of the dynamic object in the scene image identified by the target detection algorithm YOLOv8, while the instance segmentation mask is a more accurate dynamic object area determined by the connected domain algorithm and a preset depth value threshold.

[0065] For pixels that are 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 pixels in adjacent frame images, the algorithm can determine the displacement direction and size of the pixels. The purpose of identifying these feature points is to further determine 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 recognition process can be achieved through the following steps: First, traverse all pixels inside the rectangular detection box to determine whether each point is outside the instance segmentation mask. For points that meet the conditions, apply the LK local optical flow algorithm to calculate their motion vectors. If the motion vector is large, it means that the point may be affected by a dynamic object and requires further analysis and processing, such as marking it as a potential interfering feature point so that it can be eliminated or weighted in subsequent positioning calculations. By using the LK local optical flow algorithm to identify static feature points that are inside the rectangular detection box and outside the instance segmentation mask, the impact of dynamic objects on visual positioning can be more comprehensively considered, thereby improving the positioning accuracy and stability of the quadruped robot in dynamic scenes.

[0067] 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;

[0068] In this embodiment, first, the density clustering algorithm DBSCAN is used to process the preliminary static feature points located in the rectangular detection frame. The DBSCAN algorithm is a density-based spatial clustering algorithm that can cluster points with similar density to form different clusters. In this process, the algorithm will divide according to the depth value of the feature point. Since the depth value reflects the distance between the camera corresponding to the pixel point 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 in the frame can be divided into different clusters according to the similarity of the depth value. Then, the clusters falling 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 to be the range of dynamic objects, and the feature points falling within this range are likely to change with the movement of dynamic objects. 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 precise static feature points in the frame. These feature points are located in the rectangular detection frame, but not in the instance segmentation mask, and are therefore considered to be relatively stable static feature points.

[0069] Finally, 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 static feature points outside the frame are the static feature points remaining after the feature points inside the rectangular detection frame are initially eliminated, while the precise static feature points inside the frame are the feature points determined by the DBSCAN algorithm that are inside the rectangular detection frame and not inside the instance segmentation mask. By merging these two parts of static feature points, a more complete and accurate total static feature point set can be obtained.

[0070] Furthermore, 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:

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

[0072] S52, traverse feature points: for each feature point, calculate the distance between it and all other feature points. The distance comprehensively considers the difference in depth value and position of the feature point in the image, and common distance measurement methods such as Euclidean distance or Manhattan distance can be used. By calculating the distance, the foundation is laid for the subsequent determination of the neighborhood relationship between feature points.

[0073] S53, marking core points and boundary points: Classify each feature point according to the set neighborhood radius and minimum sample number. Points with feature points in the neighborhood 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.

[0074] S54, forming clusters: starting from a core point that has not been visited, traverse all points in its neighborhood (including core points and boundary points), 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 have been visited or all points are marked as a cluster or noise points.

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

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

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

[0078] If the correspondence between different image frames of a high motion probability feature point meets the constraints of epipolar geometry, then it can be considered that it does have a high probability of motion. On the contrary, if the feature point does not meet the constraints of epipolar geometry, then it may be necessary to re-evaluate its mobility or adjust it 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 the high motion probability feature point can be determined more accurately, thereby improving the reliability and accuracy of the visual positioning of the quadruped robot in dynamic scenes.

[0079] Specifically, Figure 2As shown, the epipolar geometric constraint relationship between two adjacent frames of images is shown. P is a map point, P' is the position of the map point after movement in space, and E is the distance from the matched feature point to the camera epipolar line. Let the homogeneous coordinates of the feature matching points p1 and p2 between the previous and next frames be expressed as p1 = [u1, v1, 1], p2 = [u2, v2, 1], where u and v are the pixel coordinate values ​​of the feature points. Let the camera's basic matrix be expressed as F, which only depends on the camera's internal and external parameters and can be obtained through camera calibration using general procedures. The epipolar geometric constraints are: The corresponding epipolar line of the feature point p1 in the first frame in the second frame is l2, and Here XYZ is the calculated intermediate value. Figure 2 The distance E in is the distance from the feature point p2 to the second frame epipolar line l2:

[0080]

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

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

[0083] In the present embodiment, ORB-SLAM3 is an advanced simultaneous positioning and mapping (SLAM) algorithm, which can use the feature points in the image to determine the position and posture of the camera and build a map of the environment. For a quadruped robot, accurate camera pose estimation is the key to achieving autonomous navigation and environmental perception. First, the total static feature points are input into the feature tracking module of ORB-SLAM3. This module tracks the movement of feature points between continuous image frames and determines the relative movement of the camera by matching the feature points in different frames. Then, the camera pose is optimized and estimated using these matched feature points. ORB-SLAM3 usually uses optimization algorithms such as Bundle Adjustment to adjust the pose parameters of the camera by minimizing the reprojection error, so that the projection position of the feature points in the image is as close as possible to the actual observation position. While estimating the camera pose, ORB-SLAM3 also builds 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] Further, in step S6, the subsequent operation of ORB-SLAM3 on the total static feature points includes the following steps:

[0085] S61, Feature extraction and matching: After the total static feature points are input into the ORB-SLAM3 system, the system will further process these feature points and extract more advanced feature descriptors. These descriptors are then used to match feature points between consecutive image frames, and similar point pairs are found by comparing the descriptors of feature points in different frames, and their corresponding relationships in different frames are determined, 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 projection position of the feature point in the image is as close as possible to the actual observation position, and the optimal camera pose estimation result is obtained through continuous iteration.

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

[0088] S64, closed loop detection and optimization: To improve positioning accuracy and stability, ORB-SLAM3 performs closed loop detection, that is, detects whether the robot returns to a previously visited location during movement. Once a closed loop is detected, global optimization is performed to adjust the structure of the entire map and the camera position to eliminate accumulated errors, further improving positioning accuracy and stability. Figure 1 Consistency.

[0089] S65, Real-time update and output: ORB-SLAM3 continuously receives new image frames and total static feature points and updates them in real time. As the robot moves, the camera pose and 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 autonomous navigation, obstacle avoidance and other tasks.

[0090] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technical personnel in this field can make some changes or modify the technical contents disclosed above into equivalent embodiments without departing from the scope of the technical solution of the present invention. However, any brief modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention are still 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 ORB-SLAM3 operations for real-time estimation of the camera pose of the quadruped robot.

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: Vdepth= 0 . 5× ( m+t ) +α · SD Where Vdepth is the depth value threshold; m is the median of the effective depths of all pixels after sorting; t is the tertiary of the effective depths of all pixels after sorting; α is an adjustable factor that is adjusted according to the usage scenario; SD is the standard deviation of the effective depths of all pixels.

5. A quadruped robot dynamic scene visual positioning method according to claim 1, characterized in that: 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.

6. 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.

7. 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.

8. 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