Method and system for constructing dense point cloud map for humanoid robot

Abstract

The present invention relates to the technical field of point cloud map construction, and in particular to a method and system for constructing a dense point cloud map for a humanoid robot. The method comprises: acquiring, in real time, video images captured when the humanoid robot walks; determining a gait phase of the humanoid robot in a current frame image of the video images; estimating a camera pose of the current frame image on the basis of the gait phase of the humanoid robot in the current frame image; on the basis of the camera pose of the current frame image, determining whether the current frame image is a key frame; and performing local mapping and loop closure detection on the basis of the key frame to obtain a global dense point cloud map. The present invention effectively ameliorates blurriness and jitter in data acquired during map construction by the humanoid robot, thereby obtaining a more accurate environmental map, improving the accuracy of operations when the robot executes tasks, and reducing the risk of collisions during locomotion of the humanoid robot.

Description

A method and system for constructing dense point cloud maps for humanoid robots Technical Field

[0001] This invention relates to the field of point cloud map construction technology, and in particular to a method and system for constructing dense point cloud maps for humanoid robots. Background Technology

[0002] Currently, the aging population and rapidly rising wages in the labor market are the primary reasons for robots replacing humans, driving the rapid growth in domestic demand for humanoid robots. Simultaneously, with the rapid advancement of technology, humanoid robots are gradually moving from laboratories to production lines, becoming a new favorite in intelligent manufacturing across various industries, such as education, catering, and material handling.

[0003] For humanoid robots, Simultaneous Localization and Mapping (SLAM) is the first step towards achieving autonomous exploration and performing various tasks. The core idea of ​​the SLAM algorithm is to utilize sensor data from the robot's environment, such as LiDAR and cameras, to build an environmental map in real time and calculate the robot's position. The SLAM algorithm can be divided into two sub-problems: map building and robot localization. Compared to mobile robots, the vertical motion and bumps caused by humanoid robots' walking amplify the motion effects during sensor data acquisition, easily leading to data ambiguity. Therefore, one typical task is to generate an accurate and high-fidelity environmental map.

[0004] Currently, methods for optimizing humanoid robot mapping mainly include adjusting RGB frame tracking, camera pose tracking, and multi-sensor fusion. For example, Oliver et al. successfully implemented a real-time 3D SLAM system for humanoid robots for the first time. They demonstrated loop closure using the HRP-2 robot in an indoor environment by tightly coupling a pattern generator, robot range finding, and inertial sensing into a standard extended Kalman filter framework. Scona et al. proposed a camera pose tracking method that fuses Valkyrie humanoid motion information into an EF framework. This method combines elastically fused frame-to-model visual tracking with motion priors provided by a low-drift motion inertial state estimator to achieve local loop closure in micro-dynamic environments. Compared with motion inertial state estimators and elastically fused visual tracking, this fusion method achieves a lower drift rate and improves mapping accuracy.

[0005] Among existing methods for humanoid robot mapping, visual mapping is less costly and can acquire rich texture and color information, which helps in more accurate environmental modeling. RGB-D dense mapping is a visual mapping method that uses color images and depth information acquired by RGB-D sensors to construct a dense 3D map of the scene. During the mapping process, the oscillation of the humanoid robot's center of mass caused by humanoid walking leads to significant camera displacement, and the ground reaction force caused by leg impacts also affects image acquisition, causing data blurring. Generally, the performance of current RGB-D dense mapping methods depends on the smoothness of camera motion and good scene texture; therefore, for humanoid robots, the accuracy and robustness of RGB-D dense mapping methods are reduced.

[0006] Currently, there are generally two types of strategies to improve the robustness and accuracy of RGB-D dense mapping. One is to compensate for the lack of visual information by integrating inertial information, thereby improving the performance of dense mapping during fast movement or without texture. However, since drift errors in pose estimation are unavoidable and increase over time, the performance of dense mapping will be time-dependent. The second approach involves designing many special optimization algorithms in the back-end processes of dense mapping to minimize drift errors. However, these optimization strategies differ for different scenarios, and improving the robustness and accuracy of dense mapping in general scenarios remains a challenging problem.

[0007] In summary, due to the humanoid robot's human-like walking motion, the vertical movement and bumps amplify the motion effects during sensor acquisition. Sensor shaking makes the acquired data blurry, affecting the quality of map construction, leading to inaccurate positioning of the humanoid robot, reducing the accuracy of its operation when performing tasks, and also increasing the risk of collisions during the humanoid robot's movement. Summary of the Invention

[0008] Therefore, the technical problem to be solved by the present invention is to overcome the problem in the prior art that the sensor shaking when the humanoid robot walks makes the collected data blurry and affects the quality of map construction.

[0009] To address the aforementioned technical problems, this invention provides a method for constructing dense point cloud maps for humanoid robots, comprising:

[0010] Real-time acquisition of video footage captured while a humanoid robot is walking;

[0011] Determining the gait phase of a humanoid robot in the current frame of a video image includes:

[0012] Acquire sensor information of the humanoid robot during walking; based on the sensor information, use the gait phase model to divide the gait of the humanoid robot in the current frame image into translational motion and rotational motion, and cluster the translational motion and rotational motion into single support phase on the left / right foot and double support phase on the right foot, respectively;

[0013] Gait phase estimation of the humanoid robot based on the current frame image to determine the camera pose of the current frame image, including:

[0014] If the gait phase is the single support phase of the left / right foot in translational motion, then the constant velocity motion model is used to estimate the camera pose of the current frame image;

[0015] If the gait phase is the single support phase of the left / right foot in rotational motion, then the brute-force matching model is used to estimate the camera pose of the current frame image;

[0016] If the gait phase is the double-support phase of translational motion or the double-support phase of rotational motion, then determine the gait phase of the humanoid robot in the next frame of the video image.

[0017] Determine whether the current frame image is a keyframe based on the camera pose of the current frame image;

[0018] Local mapping and loop closure detection are performed based on keyframes to obtain a global dense point cloud map.

[0019] Preferably, the sensor information of the humanoid robot during walking includes: IMU data, force-sensitive resistor data, and combined encoder data; wherein the IMU data of the humanoid robot during walking includes IMU data of the humanoid robot's center of mass and IMU data of the humanoid robot's two feet.

[0020] Preferably, after acquiring sensor information during the humanoid robot's walking, the sensor information is preprocessed, including:

[0021] The sensor information is smoothed using the smooth function, and then dimensionality reduction is performed using principal component analysis.

[0022] Preferably, based on sensor information and using a gait phase model, the gait of the humanoid robot in the current frame image is divided into translational motion and rotational motion, and the translational motion and rotational motion are clustered into a single-support phase on the left / right foot and a double-support phase, respectively, including:

[0023] Based on the IMU data of the humanoid robot's center of mass and the joint encoder data, the angular velocity of the center of mass in the z-axis direction is obtained; the average value and standard deviation of the angular velocity of the center of mass in the z-axis direction within the time window are calculated and input into the trained support vector machine to divide the gait of the humanoid robot in the current frame image into translational motion and rotational motion.

[0024] The vertical acceleration of the feet is obtained from the IMU data of the humanoid robot's two feet; the ground reaction force and torque are obtained from the force-sensitive resistor data; based on the vertical acceleration of the feet and the ground reaction force and torque, the translational motion and rotational motion are clustered into single-support stage and double-support stage of the left / right foot, respectively, using a Gaussian mixture model.

[0025] Preferably, if the gait phase is the single-foot support phase of rotational motion (left / right foot), then a brute-force matching model is used to estimate the camera pose of the current frame image, including:

[0026] Extract the ORB feature points of the current frame image and calculate the descriptor of the ORB feature points of the current frame image as the query descriptor;

[0027] Obtain the ORB feature points of the previous frame image and calculate the descriptor of the ORB feature points of the previous frame image as the matching descriptor.

[0028] For each query descriptor q, find the matching descriptor a with the smallest Hamming distance. best As the matching result of the current query descriptor;

[0029] The camera pose of the current frame image is estimated based on the matching results.

[0030] Preferably, the matching descriptor a with the smallest Hamming distance to the query descriptor q is obtained. best After matching the current query descriptor, calculate the matching descriptor 'a'. best Hamming distance to all query descriptors; if the current query descriptor q matches the matching descriptor a best If the Hamming distance is also minimized, then the matching result is retained; otherwise, the matching result is discarded.

[0031] Preferably, the current query descriptor q and the matching descriptor a are retained. best After obtaining the matching results, retrieve the current query descriptor q and its second smallest Hamming distance matching descriptor a. 2nd And calculate the match between the current query descriptor q and the matching descriptor a. best Hamming distance d between H (q,a best () with the current query descriptor q and the matching descriptor a 2nd Hamming distance d between H (q,a 2nd The ratio of the distance to the preset distance ratio is used to determine the relationship between the ratio and the preset distance ratio. If the ratio is less than the preset distance ratio, the matching result is retained; otherwise, the matching result is discarded.

[0032] Preferably, determining whether the current frame image is a keyframe based on the camera pose of the current frame image includes:

[0033] A frame is considered a keyframe if the number of inliers in the current frame exceeds a preset minimum threshold and meets any of the following conditions: the first condition is that the frame number difference between the current frame and the previous keyframe is greater than a preset maximum frame number; the second condition is that the frame number difference between the current frame and the previous keyframe is greater than a preset minimum frame number, and the local mapping thread is idle; the third condition is that the number of keyframes in the keyframe queue of the local mapping thread does not exceed three.

[0034] Preferably, before determining whether the current frame image is a keyframe based on the camera pose of the current frame image, the method further includes:

[0035] The translation and rotation matrices between the current frame and the previous frame are solved by constructing a least-squares problem, with the following formulas:

[0036] in, and Let represent the i-th ORB feature point of the current frame image and the previous frame image respectively, n represent the total number of feature points, t represent the frame index, R represent the rotation matrix, and t represent the translation matrix;

[0037] The transformation matrix is ​​obtained based on the translation and rotation matrices between the current frame image and the previous frame image.

[0038] If the norm of the transformation matrix is ​​less than a preset value, then the current frame image is determined to be a keyframe based on the camera pose of the current frame image.

[0039] This invention also provides a dense point cloud map construction system for humanoid robots, comprising:

[0040] The video acquisition module is used to acquire video footage of the humanoid robot walking in real time.

[0041] The gait determination module is used to determine the gait phase of the humanoid robot in the current frame of the video image, including: acquiring sensor information of the humanoid robot walking; based on the sensor information, using the gait phase model, dividing the gait of the humanoid robot in the current frame image into translational motion and rotational motion, and clustering the translational motion and rotational motion into single support phase on the left / right foot and double support phase on the right foot, respectively.

[0042] The camera pose estimation module is used to estimate the camera pose of the humanoid robot in the current frame image based on the gait phase of the current frame image. This includes: if the gait phase is a single support phase of the left / right foot in translational motion, then a constant velocity motion model is used to estimate the camera pose of the current frame image; if the gait phase is a single support phase of the left / right foot in rotational motion, then a brute-force matching model is used to estimate the camera pose of the current frame image; otherwise, the gait phase of the humanoid robot in the next frame image in the video image is determined.

[0043] The keyframe determination module is used to determine whether the current frame image is a keyframe based on the camera pose of the current frame image;

[0044] The map building module is used to perform local mapping and loop closure detection based on keyframes to obtain a global dense point cloud map.

[0045] Compared with the prior art, the above-described technical solution of the present invention has the following advantages:

[0046] This invention discloses a method for constructing dense point cloud maps for humanoid robots. Building upon the existing ORBSLAM2 algorithm, it first determines the gait phase of the humanoid robot when selecting keyframes, choosing image frames from the more stable left / right foot single-support phase for subsequent processing. This enables more accurate tracking of camera posture even under sensor jitter and shaking caused by gait, minimizing the impact of gait on the camera and obtaining more stable image frames for subsequent processing. Furthermore, it only employs a computationally intensive brute-force matching model to estimate the camera pose of the current frame image during rotational movements with significant speed changes and severe camera shaking. This method fully utilizes global information in the image while consuming minimal computational resources, better capturing the overall relationship between images, thus more accurately estimating the camera pose and improving the quality of point cloud map construction. This invention effectively improves the blurriness and jitter of data acquired during humanoid robot mapping, resulting in more accurate environmental maps, improving the operational accuracy of the robot during task execution, and reducing the risk of collisions during humanoid robot movement.

[0047] Furthermore, this invention determines the norm of the transformation matrix of two adjacent image frames and selects image frames with a norm less than a preset threshold as key frames. This avoids selecting image frames with high overlap or significant gait influence as key frames, reducing drift and overlap during mapping and further improving the quality of map construction by humanoid robots. Attached Figure Description

[0048] To make the content of this invention easier to understand, the invention will be further described in detail below with reference to specific embodiments and accompanying drawings, wherein:

[0049] Figure 1 is a flowchart of a method for constructing a dense point cloud map for a humanoid robot according to the present invention;

[0050] Figure 2 shows the relationship between translational and rotational motion and Laplace variance and the three-dimensional velocity of the center of mass. In Figure 2(a), the relationship between translational and rotational motion and Laplace variance is shown, and in Figure 2(b), the relationship between translational and rotational motion and the three-dimensional velocity of the center of mass is shown.

[0051] Figure 3 shows the relationship between gait phase, Laplace variance, and three-dimensional velocity of the center of mass. In Figure 3(a), the relationship between gait phase and Laplace variance is shown, and in Figure 3(b), the relationship between gait phase and three-dimensional velocity of the center of mass is shown.

[0052] Figure 4 shows the relationship between gait phase and vertical ground reaction force and acceleration of the foot. In Figure 4(a), the relationship between gait phase and vertical ground reaction force of the foot is shown, and in Figure 4(b), the relationship between gait phase and vertical acceleration of the foot is shown.

[0053] Figure 5 is a comparison of the mapping effect before and after optimization according to the present invention. In Figure 5, row (a) is the mapping effect before optimization, and row (b) is the mapping effect after optimization. Detailed Implementation

[0054] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0055] Example 1

[0056] The ORBSLAM2 algorithm is a dense point cloud map construction algorithm suitable for wheeled robots. However, wheeled robots walk relatively smoothly, while humanoid robots experience camera jolting and shaking during movement. Directly applying the ORBSLAM2 algorithm to humanoid robots can lead to blurred data and reduced map construction quality. Therefore, this invention improves the ORBSLAM2 algorithm by analyzing the gait of humanoid robots, making it suitable for dense point cloud map construction for humanoid robots.

[0057] Referring to Figure 1, the present invention provides a method for constructing a dense point cloud map for a humanoid robot, comprising:

[0058] S1: Real-time acquisition of video footage captured while the humanoid robot is walking;

[0059] S2: Determine the gait phase of the humanoid robot in the current frame of the video image, including:

[0060] The sensor information of the humanoid robot during walking is acquired and preprocessed. Based on the preprocessed sensor information, the gait of the humanoid robot in the current frame image is divided into translational motion and rotational motion using a gait phase model. The translational motion and rotational motion are then clustered into single support phase on the left / right foot and double support phase on the right foot, respectively.

[0061] S3: Estimating the camera pose of the humanoid robot's gait phase based on the current frame image, including:

[0062] If the gait phase is the single support phase of the left / right foot in translational motion, then the constant velocity motion model is used to estimate the camera pose of the current frame image;

[0063] If the gait phase is the single support phase of the left / right foot in rotational motion, then the brute-force matching model is used to estimate the camera pose of the current frame image;

[0064] If the gait phase is the double-support phase of translational motion or the double-support phase of rotational motion, then determine the gait phase of the humanoid robot in the next frame of the video image.

[0065] S4: Determine whether the current frame image is a keyframe based on the camera pose of the current frame image; if not, return to determine the gait phase of the humanoid robot in the next frame image of the video image.

[0066] S5: Perform local mapping and loop closure detection based on keyframes to obtain a global dense point cloud map.

[0067] This embodiment uses an indoor enclosed scene for simulation, and adds and visualizes walls, obstacles, etc. using the simulation software vrep.

[0068] The video imagery in S1 requires the acquisition of color image frames and depth image frames.

[0069] In this embodiment, when the humanoid robot is walking, the head camera acquires depth and color images, and the rosbag tool is used to record them.

[0070] S2 is used to predict the gait phase of the humanoid robot in real time. In order to optimize the mapping process of the humanoid robot, this invention establishes a gait phase model of the humanoid robot to identify the gait phase during the walking process, and optimizes the mapping based on different gait phases to minimize external motion interference caused by walking.

[0071] Designing a gait phase model for a humanoid robot includes the following steps:

[0072] S21: To design the gait phase model, this invention first studies the center-of-mass dynamics of a humanoid robot during walking. These dynamics are described by the Newton-Euler equations:

[0073] Where c and These are the position and acceleration of the center of mass, respectively. f is the angular momentum rate around the center of mass. k and τ k Let s be the ground reaction force and torque at the k-th contact point. k Let g be the k-th contact point between the robot and the ground, g be the gravity vector, and m be the mass of the robot.

[0074] S22: According to the dynamics of the center of mass, the key indicators for the robot's gait phase are the position and acceleration of the center of mass, and the ground reaction force and torque. Therefore, the sensor information collected by this invention during the humanoid robot's walking includes: IMU data, force-sensitive resistor data, and combined encoder data. The IMU data includes IMU data of the robot's center of mass and IMU data of both feet. Based on the IMU data of the robot's center of mass and the combined encoder data, the vertical acceleration and angular velocity of the center of mass in the z-axis direction are obtained; based on the IMU data of the robot's feet, the vertical acceleration of the feet is obtained; and based on the force-sensitive resistor data, the ground reaction force and torque are obtained.

[0075] In this embodiment, the humanoid robot is controlled to walk in the VREP simulation environment of the ROS system, and the rosbag tool is used to record the IMU data, force-sensitive resistor data and joint encoder data collected during the humanoid robot's walking in real time.

[0076] This embodiment reads data from the bag file and preprocesses the sensor information collected during the humanoid robot's walking, including:

[0077] The smooth function is used to smooth the sensor information and filter out data that changes abruptly due to the robot's instability, such as swaying back and forth or left and right.

[0078] Principal component analysis was then used for dimensionality reduction, mapping the data to a two-dimensional space, which effectively captured the dynamic information of the deep layer.

[0079] S23: Based on the preprocessed sensor information above, this invention constructs a gait phase model for gait phase estimation. The gait phase model includes a support vector machine (SVM) and a Gaussian mixture model (GMM). The SVM is used to first divide the gait of the humanoid robot into rotational and translational motions, while the GMM is used to further divide the gait phase into a single-foot (left / right) support phase and a double-foot support phase. Specifically, it includes the following steps:

[0080] S231: First, the gait is preliminarily classified using the collected angular velocity of the centroid in the z-axis direction, and the average and standard deviation of the angular velocity of the centroid in the z-axis direction within the time window are calculated:

[0081] Where, ω Z (t) represents the angular velocity at time t, and N represents the number of data points within the time window. and These represent the average value and standard deviation of the angular velocity, respectively.

[0082] During the training phase, thresholds were set for the mean and standard deviation of angular velocity and used as inputs to the support vector machine. Rotational and translational motions were used as different labels as outputs. Through successful learning, gait was classified into rotational and translational motions.

[0083] During the inference phase, the average value and standard deviation of the angular velocity of the centroid in the z-axis direction within the time window are input into the trained support vector machine to divide the gait of the humanoid robot in the current frame into translational motion and rotational motion.

[0084] S232. After using a support vector machine to divide the gait of the humanoid robot in the current frame into translational and rotational movements, a Gaussian mixture model is used to cluster the rotational and translational movements separately, with each category outputting three sets of data. The Gaussian mixture model fits the Gaussian distribution of each category and classifies the movements according to the probability of each data point, clustering the translational and rotational movements into single-support stages (left / right foot) and double-support stages, respectively.

[0085] Based on the gait phase model, the gait of the humanoid robot is divided into the single support phase of the left / right foot during translational motion, the double support phase during translational motion, the single support phase of the left / right foot during rotational motion, and the double support phase during rotational motion.

[0086] This invention uses the obtained asynchronous phase states to analyze the stages of regular and irregular motion during robot walking, and designs a mapping and optimization method.

[0087] In step S3, to evaluate the impact of rotational and translational motions on the acquired image frames, this invention uses Laplacian variance for evaluation. Laplacian variance assesses image sharpness by calculating the degree of grayscale variation in the image. First, the color image is converted to a grayscale image: gray = 0.2989R + 0.5870G + 0.1140B

[0088] Where R, G, and B are the pixel values ​​of the red, green, and blue channels of the image, respectively, and gray is the grayscale value of the image.

[0089] By convolving the image with a Laplacian kernel, the Laplacian transform of the image is obtained, capturing regions of grayscale variation in the image, and calculating the variance.

[0090] Among them, S 2 L is the variance of the Laplace value. u Let be the Laplacian value of the u-th pixel in the image, μ be the average of all Laplacian values, and N' be the total number of pixels in the image.

[0091] A larger calculated variance indicates more distinct image edges and a clearer image; a smaller calculated variance indicates smoother image edges, a lack of distinct edges or details, and a blurrier image. Through calculation and comparison, this invention has found that rotational motion is more likely to produce blurring than translational motion.

[0092] To evaluate the impact of rotational and translational motion on camera motion speed, this invention uses the 3D center-of-mass velocity estimated by the State Estimation Robot Walking (SEROW) framework. The SEROW framework calculates the center-of-mass velocity using previously acquired sensor information. Comparison reveals that rotational motion typically experiences greater fluctuations and instability in the center-of-mass velocity compared to translational motion.

[0093] Referring specifically to Figure 2, Figure 2(a) shows the relationship between translational and rotational motion and the Laplace variance, where the curve represents the variance of the Laplace determinant; Figure 2(b) shows the relationship between translational and rotational motion and the three-dimensional velocity of the center of mass, where lines labeled ①, ②, and ③ represent the velocities of the center of mass on the x, y, and z axes, respectively. As can be seen from Figure 2, the Laplace variance during rotational motion is smaller than that during translational motion. Therefore, images of rotational motion are more prone to blurring compared to translational motion. Furthermore, the velocity of the center of mass during rotational motion typically experiences greater fluctuations and instability compared to translational motion.

[0094] To evaluate the impact of each gait phase on the acquired image frames, Laplacian variance and 3D centroid velocity were used for assessment. Comparison revealed that most peaks in the Laplacian variance plot were located within the left / right foot single-support phase, indicating higher image sharpness. Furthermore, most zero-crossings of the 3D centroid velocity occurred at the beginning of the left / right foot single-support phase. At this instant, minimal motion is propagated to the camera, thus greatly increasing the probability of capturing high-quality images.

[0095] Referring specifically to Figure 3, Figure 3(a) shows the relationship between gait phase and Laplacian variance, where the curve represents the variance of the Laplacian determinant and the points represent local maxima. Figure 3(b) shows the relationship between gait phase and the three-dimensional velocity of the centroid, where lines labeled ①, ②, and ③ represent the centroid velocities on the x, y, and z axes, respectively, and the points are the corresponding zero-crossing points. Regions labeled (1), (2), and (3) represent the two-foot support phase, the left-foot single support phase, and the right-foot single support phase, respectively. As can be seen from Figure 3, most peaks in the Laplacian variance plot are located within the left / right-foot single support phase, at which point the image clarity is relatively high. Most zero-crossings of the three-dimensional centroid velocities occur at the beginning of the left and right-foot single support phases, where the smallest motion is propagated to the camera, resulting in higher image quality.

[0096] Furthermore, this invention also uses sensor data to compare the impact of each gait phase on the acquired image frames. As can be seen from the vertical acceleration of the left and right feet, the impact acceleration of the left and right legs stabilizes and approaches zero after the left / right foot single support phase phase, with only a small amount propagating to the robot's image frames. Similarly, after entering the left / right foot single support phase phase, the ground reaction force of both feet is stabilized. Therefore, the image frames obtained by the left / right foot single support phase are clearer and more stable.

[0097] Referring specifically to Figure 4, Figure 4(a) shows the relationship between the gait phase and the vertical ground reaction force of the feet when the humanoid robot is walking, where lines labeled ④ and ⑤ represent the ground reaction forces of the left and right feet, respectively. Figure 4(b) shows the relationship between the gait phase and the vertical acceleration of the feet, where lines labeled ⑥ and ⑦ represent the vertical accelerations of the left and right feet, respectively. As can be seen from Figure 4, the impact acceleration of the left and right feet stabilizes and approaches zero after the single-support phase of the left / right foot, with only a small amount propagating to the robot's image frames. Similarly, after entering the single-support phase of the left / right foot, the ground reaction forces of both feet stabilize, thus the image frames obtained in the single-support phase of the left / right foot are clearer and more stable.

[0098] Therefore, this invention concludes through comparison that rotational motion is more unstable in speed and more prone to image blurring than translational motion. The original pose estimation module in ORBSLAM2 is a constant-speed motion model, the core idea of ​​which is to assume that the object is in a uniform motion state for a short period of time (adjacent frames). However, when a humanoid robot rotates, the speed changes irregularly due to gait and camera shake, so the motion model often cannot provide good pose estimation. The brute-force matching model has disadvantages such as large computational load and poor scalability. Therefore, the brute-force matching model is only used in rotational motion with large speed changes and severe camera shake.

[0099] Since comparisons show that the images are more stable and least affected by gait during the single-foot support phase, the keyframes for mapping are selected during this gait phase, and stable keyframes are chosen through translation and rotation changes between image frames.

[0100] Specifically, the constant velocity motion model is a tracking matching, which means that the map points are reprojected onto the current frame for matching, and then optimized according to the Bundle Adjustment (BA) method, which is equivalent to the classic PnP (Perspective-n-Point).

[0101] The following details the process of estimating the camera pose of the current frame image using a brute-force matching model during the single-foot support phase of a gait where the gait phase is a rotational motion. The steps include:

[0102] S31: Extract the ORB feature points of the current frame image and calculate the descriptor of the ORB feature points of the current frame image as the query descriptor; obtain the ORB feature points of the previous frame image and calculate the descriptor of the ORB feature points of the previous frame image as the matching descriptor.

[0103] Assume I t I t+1 These are the previous frame image and the current frame image, respectively. Starting from the current frame image I... t+1 The extracted feature points are From the previous frame image I t The extracted feature points are n is the total number of feature points.

[0104] Calculate the descriptor for each feature point in the current frame image I. t+1 Feature point descriptors For query descriptors, the previous frame image I t Feature point descriptors For matching descriptors; m is the total number of descriptors. The descriptors of each frame are matched with the descriptor map of the previous frame; if a match is successful, the initial camera pose is estimated.

[0105] S32: For each query descriptor q, find the matching descriptor a with the smallest Hamming distance. best As the matching result of the current query descriptor.

[0106] For the current frame I t+1 Each query descriptor is calculated by comparing it with the previous frame I. t The Hamming distance between matching descriptors is used to find the most similar descriptor, and the calculation formula is as follows:

[0107] Where, q v and a v q and a are the values ​​of the v-th position in the query descriptor q and the matching descriptor a, respectively, and n' is the total length of the descriptors.

[0108] For each query descriptor q, calculate all matching descriptors a from the previous frame. j Hamming distance d H (q,a j Select the descriptor a with the smallest distance. best As a matching result:

[0109] S33: To ensure the quality of the match, the condition of a best-fit is used. This condition applies if and only if a best It is the feature that best matches q in the previous frame image, and q is a best When considering the best-matching features in the current frame image, q and a best Only then is it considered a valid match.

[0110] Find the matching descriptor a with the smallest Hamming distance to the query descriptor q. best After matching the current query descriptor, calculate the matching descriptor 'a'. best Hamming distance to all query descriptors; if the current query descriptor q matches the matching descriptor a best If the Hamming distance is also minimized, then the matching result is retained; otherwise, the matching result is discarded.

[0111] S34: To further improve the reliability of the matching, retain the current query descriptor q and the matching descriptor a. best After obtaining the matching results, retrieve the matching descriptor a with the second smallest Hamming distance to the current query descriptor q. 2nd And calculate the match between the current query descriptor q and the matching descriptor a. best Hamming distance d between H (q,a best () with the current query descriptor q and the matching descriptor a 2nd Hamming distance d between H (q,a 2nd The ratio of ) is used to determine the relationship between the ratio and the preset distance ratio r, expressed by the formula:

[0112] If the ratio is less than the preset distance ratio, the matching result is retained; otherwise, the matching result is discarded.

[0113] S35: Estimate the initial camera pose of the current frame image based on the matching results.

[0114] This method can improve the quality and reliability of matching under rotation and jitter, thereby reducing the chance of tracking failure.

[0115] S36: Track the local map based on the estimated initial pose and further calculate and optimize the initial camera pose using the bundle adjustment method. When the camera pose estimation is reliable, ORBSLAM2 will select keyframes to add via S4.

[0116] In step S4, determining whether the current frame image is a keyframe based on the camera pose of the current frame image includes:

[0117] The number of inliers in the current frame exceeds a preset minimum inlier threshold, and meets any of the following conditions:

[0118] (1) The frame number difference between the current frame image and the previous keyframe is greater than the preset maximum frame number;

[0119] (2) The frame number difference between the current frame image and the previous keyframe is greater than the preset minimum frame number, and the local mapping thread is in an idle state.

[0120] (3) The number of keyframes in the keyframe queue of the local mapping thread shall not exceed three.

[0121] The in-point refers to the feature point in the current frame image that is successfully matched with the previous frame image, and is obtained based on the camera pose of the current frame image.

[0122] Because the camera jolting caused by the gait of humanoid robots when they walk often results in large displacements and rotations between image frames, this invention adds a relative motion screening condition to the original keyframe screening conditions in ORBSLAM2. By judging the overlap between two adjacent image frames, stable keyframes can be obtained.

[0123] Preferably, before determining whether the current frame image is a keyframe based on the camera pose of the current frame image, the method further includes:

[0124] The translation and rotation matrices between the current frame and the previous frame are solved by constructing a least-squares problem, with the following formulas:

[0125] in, and Let represent the i-th ORB feature point of the current frame image and the previous frame image respectively, n represent the total number of feature points, t represent the frame index, R represent the rotation matrix, and t represent the translation matrix;

[0126] To simplify the problem, we obtain new feature points for the current frame and the previous frame by subtracting the centroid of each feature point from its position. and Eliminate the effects of translation and retain only the positional changes caused by rotation:

[0127] The above formula can be expanded and simplified as follows:

[0128] Performing SVD decomposition on the summation term in the equation, when the summation term matrix is ​​of full rank, the rotation matrix R can be determined. Then, use the calculated R to solve for the translation matrix t:

[0129] The transformation matrix is ​​obtained based on the translation and rotation matrices between the current frame image and the previous frame image.

[0130] If the norm of the transformation matrix is ​​less than a preset value, it is determined whether the current frame image is a keyframe based on the camera pose of the current frame image. When the displacement and rotation matrices are small, it indicates that the overlap between two adjacent frames is small, the influence of gait is small, and the image is stable.

[0131] S5 includes:

[0132] A local map is constructed centered on the current keyframe. Feature points matching the current keyframe are added to the local map, and the spatial relationships between feature points are calculated based on the camera pose and the 3D coordinates of the feature points to construct the topological structure of the local map. Through loop closure detection and global optimization, the local maps are connected to construct the global map. The global map is a sparse map composed of multiple keyframes and their corresponding feature points, reflecting the general structure of the entire scene and the camera's motion trajectory.

[0133] For image frames that are not key frames, local tracking of ordinary frames is performed.

[0134] Figure 5 shows a comparison of the mapping results before and after optimization. Row (a) in Figure 5 represents the mapping result before optimization, and row (b) represents the mapping result after optimization. As can be seen from Figure 5, the optimization results of this invention have improved the detection of loop closures, drift, and overlap during humanoid robot mapping.

[0135] In summary, the humanoid robot dense point cloud map construction method of this invention, based on the original ORBSLAM2 algorithm, first determines the gait phase of the humanoid robot when selecting keyframes, and selects image frames of the more stable left / right foot single-support phase for subsequent processing. This enables more accurate tracking of camera posture even under sensor jitter and shaking caused by gait, minimizing the impact of gait on the camera and obtaining more stable image frames for subsequent processing. Furthermore, it only uses a computationally intensive brute-force matching model to estimate the camera pose of the current frame image during rotational movements with large speed changes and severe camera shaking. This fully utilizes global information in the image while consuming minimal computational resources, better capturing the overall change relationship between images, thereby more accurately estimating the camera pose and improving the quality of point cloud map construction. This invention effectively improves the blurriness and jitter of data collected during humanoid robot mapping, resulting in a more accurate environmental map, improving the operational accuracy of the robot during task execution, and reducing the risk of collisions during humanoid robot movement.

[0136] Furthermore, this invention determines the norm of the transformation matrix of two adjacent image frames and selects image frames with a norm less than a preset threshold as key frames. This avoids selecting image frames with high overlap or significant gait influence as key frames, reducing drift and overlap during mapping and further improving the quality of map construction by humanoid robots.

[0137] Example 2

[0138] Based on the method for constructing a dense point cloud map for a humanoid robot described in Embodiment 1, this embodiment also provides a system for constructing a dense point cloud map for a humanoid robot, including:

[0139] The video acquisition module is used to acquire video footage of the humanoid robot walking in real time.

[0140] The gait determination module is used to determine the gait phase of the humanoid robot in the current frame of the video image, including: acquiring sensor information of the humanoid robot walking; based on the sensor information, using the gait phase model, dividing the gait of the humanoid robot in the current frame image into translational motion and rotational motion, and clustering the translational motion and rotational motion into single support phase on the left / right foot and double support phase on the right foot, respectively.

[0141] The camera pose estimation module is used to estimate the camera pose of the humanoid robot in the current frame image based on the gait phase of the current frame image. This includes: if the gait phase is a single support phase of the left / right foot in translational motion, then a constant velocity motion model is used to estimate the camera pose of the current frame image; if the gait phase is a single support phase of the left / right foot in rotational motion, then a brute-force matching model is used to estimate the camera pose of the current frame image; otherwise, the gait phase of the humanoid robot in the next frame image in the video image is determined.

[0142] The keyframe determination module is used to determine whether the current frame image is a keyframe based on the camera pose of the current frame image;

[0143] The map building module is used to perform local mapping and loop closure detection based on keyframes to obtain a global dense point cloud map.

[0144] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0145] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in one or more flowchart illustrations and / or one or more block diagrams.

[0146] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means that implement the functions specified in one or more flowcharts and / or one or more block diagrams.

[0147] These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, such that the instructions, which execute on the computer or other programmable apparatus, provide steps for implementing the functions specified in one or more flowcharts and / or one or more block diagrams.

[0148] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A humanoid robot dense point cloud map construction method, characterized by, The application relates to a method for estimating camera pose of a humanoid robot, comprising the following steps: real-time acquisition of video images collected when the humanoid robot walks; judging the gait phase of the humanoid robot in the current frame image in the video images, comprising the following steps: acquiring sensor information when the humanoid robot walks; according to the sensor information, the gait of the humanoid robot in the current frame image is divided into translational motion and rotational motion by using a gait phase model, and the translational motion and the rotational motion are respectively clustered into left / right single support phases and double support phases; estimating the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image, comprising the following steps: if the gait phase is the left / right single support phase of the translational motion, a constant speed motion model is used to estimate the camera pose of the current frame image; if the gait phase is the left / right single support phase of the rotational motion, a brute force matching model is used to estimate the camera pose of the current frame image; if the gait phase is the double support phase of the translational motion or the double support phase of the rotational motion, the gait phase of the humanoid robot in the next frame image in the video images is judged; judging whether the current frame image is a key frame based on the camera pose of the current frame image; performing local mapping and loop detection according to the key frame to obtain a global dense point cloud map.

2. The method of claim 1, wherein, The sensor information when the humanoid robot walks comprises IMU data, force-sensitive resistor data and joint encoder data of the humanoid robot when the humanoid robot walks; wherein the IMU data of the humanoid robot when the humanoid robot walks comprises IMU data of the centroid of the humanoid robot and IMU data of the two feet of the humanoid robot.

3. The method of claim 2, wherein, After the sensor information when the humanoid robot walks is acquired, the sensor information is preprocessed, comprising the following steps: smooth function is used to smooth the sensor information, and then principal component analysis is used for dimension reduction processing.

4. The method of claim 2, wherein, According to the sensor information, the gait of the humanoid robot in the current frame image is divided into translational motion and rotational motion by using a gait phase model, and the translational motion and the rotational motion are respectively clustered into left / right single support phases and double support phases, comprising the following steps: the angular velocity of the centroid in the z-axis direction is acquired according to the IMU data of the centroid of the humanoid robot and the joint encoder data; the average value and the standard deviation of the angular velocity of the centroid in the z-axis direction in a time window are calculated and input into a trained support vector machine, so that the gait of the humanoid robot in the current frame image is divided into translational motion and rotational motion; the vertical acceleration of the feet is acquired according to the IMU data of the two feet of the humanoid robot; the ground reaction force and torque are acquired according to the force-sensitive resistor data; based on the vertical acceleration of the feet and the ground reaction force and torque, the translational motion and the rotational motion are respectively clustered into left / right single support phases and double supporting phases by using a Gaussian mixture model.

5. The method of claim 1, wherein, if the gait phase is the left / right single support phase of the rotational motion, a brute-force matching model is used to estimate the camera pose of the current frame image, comprising the following steps: ORB feature points of the current frame image are extracted, and the descriptors of the ORB feature points of the current frame image are calculated as query descriptors; ORB feature points of the previous frame image are acquired, and the descriptors of the ORB feature points of the previous frame image are calculated as matching descriptors; For each query descriptor q, obtain the matching descriptor a with the smallest Hamming distance to it best as the result of the current query descriptor the camera pose of the current frame image is estimated according to the matching result.

6. The method of claim 5, wherein, Find the matching descriptor a with the smallest Hamming distance to the query descriptor q. best After matching the current query descriptor, calculate the matching descriptor 'a'. best Hamming distance to all query descriptors; if the current query descriptor q matches the matching descriptor a best If the Hamming distance is also minimized, then the matching result is retained; otherwise, the matching result is discarded.

7. The method of claim 6, wherein, reserve the matching result of the current query descriptor q and the matching descriptor a best , obtain the matching descriptor a 2nd with the second smallest Hamming distance from the current query descriptor q, and calculate the Hamming distance d best (q,a H ) between the current query descriptor q and the matching descriptor a best , and the ratio of the Hamming distance d 2nd (q,a H ) between the current query descriptor q and the matching descriptor a 2nd , determine the size relationship between the ratio and a preset distance ratio; if the ratio is smaller than the preset distance ratio, reserve the matching result, otherwise discard the matching result.

8. The method of claim 1, wherein, Determining whether the current frame is a keyframe based on the camera pose of the current frame includes: A frame is considered a keyframe if the number of inliers in the current frame exceeds a preset minimum threshold and meets any of the following conditions: the first condition is that the frame number difference between the current frame and the previous keyframe is greater than a preset maximum frame number; the second condition is that the frame number difference between the current frame and the previous keyframe is greater than a preset minimum frame number, and the local mapping thread is idle; the third condition is that the number of keyframes in the keyframe queue of the local mapping thread does not exceed three.

9. The method of claim 8, wherein, Before determining whether the current frame image is a keyframe based on the camera pose of the current frame image, the following steps are also included: The translation matrix and the rotation matrix between the current frame image and the previous frame image are solved by constructing a least square problem, and the formula is: wherein and respectively represent the i-th ORB feature point of the current frame image and the previous frame image, n represents the total number of feature points, t represents the frame index, R represents the rotation matrix, t represents the translation matrix; the transformation matrix is obtained based on the translation matrix and the rotation matrix between the current frame image and the previous frame image If the norm of the transformation matrix is ​​less than a preset value, then the current frame image is determined to be a keyframe based on the camera pose of the current frame image.

10. A humanoid robot dense point cloud map construction system, characterized by, include: The video acquisition module is used to acquire video footage of the humanoid robot walking in real time. The gait determination module is used to determine the gait phase of the humanoid robot in the current frame of the video image, including: acquiring sensor information of the humanoid robot walking; based on the sensor information, using the gait phase model, dividing the gait of the humanoid robot in the current frame image into translational motion and rotational motion, and clustering the translational motion and rotational motion into single support phase on the left / right foot and double support phase on the right foot, respectively. The camera pose estimation module is used to estimate the camera pose of the humanoid robot in the current frame image based on the gait phase of the current frame image. This includes: if the gait phase is a single support phase of the left / right foot in translational motion, then a constant velocity motion model is used to estimate the camera pose of the current frame image; if the gait phase is a single support phase of the left / right foot in rotational motion, then a brute-force matching model is used to estimate the camera pose of the current frame image; otherwise, the gait phase of the humanoid robot in the next frame image in the video image is determined. The keyframe determination module is used to determine whether the current frame image is a keyframe based on the camera pose of the current frame image; The map building module is used to perform local mapping and loop closure detection based on keyframes to obtain a global dense point cloud map.

Images