A method and system for constructing a dense point cloud map of a humanoid robot

Abstract

The present invention relates to the technical field of point cloud map construction, in particular to a method and system for constructing a dense point cloud map of a humanoid robot, including: acquiring in real time the video images collected when the humanoid robot walks; judging the gait phase of the humanoid robot in the current frame image of the video images; estimating the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image; 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 frames to obtain a global dense point cloud map. The present invention effectively improves the situation of fuzzy and jittery data collection during the mapping of the humanoid robot, can obtain a more accurate environmental map, improves the operation accuracy when the robot executes tasks, and reduces the risk of collision during the movement of the humanoid robot.

Description

Technical Field

[0001] 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 of a humanoid robot. Background Technique

[0002] In the current labor market, the aging trend is increasing year by year and the wages of employed people are rising rapidly, which has become the main reason for robots to replace humans, driving the rapid growth of domestic demand for humanoid robots. At the same time, with the rapid progress of technology, humanoid robots are gradually moving from the laboratory to the production line and becoming the new favorites of intelligent manufacturing in various industries, such as education, catering, handling and other industries.

[0003] For humanoid robots, Simultaneous Localization and Mapping (SLAM) is the first step to achieve autonomous exploration and execute various tasks. The core idea of the SLAM algorithm is to use sensor data such as lidar and cameras on the robot in the environment to build an environmental map in real time and calculate the position of the robot. The SLAM algorithm can be divided into two sub-problems: one is map building, and the other is robot position localization. Compared with mobile robots, the vertical movement and bumps caused by the human-like walking of humanoid robots amplify the motion effect during the sensor acquisition process, which is likely to cause data ambiguity. Therefore, one of the typical tasks is to generate an accurate and high-fidelity environmental map.

[0004] Currently, the main methods for optimizing the mapping of humanoid robots include adjusting RGB frame tracking, camera pose tracking, multi-sensor fusion and other methods. For example, Oliver et al. successfully implemented a humanoid real-time 3D SLAM system for the first time. They demonstrated loop closure in an indoor environment using the HRP-2 robot by tightly coupling a pattern generator, robot odometry, 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 the EF framework. It combines frame-to-model visual tracking of elastic fusion with the motion prior provided by a low-drift motion inertial state estimator to achieve local loop closure in a micro-dynamic environment. Compared with the motion inertial state estimator and the visual tracking of elastic fusion, this fusion method achieves a lower drift rate and improves the mapping accuracy.

[0005] In the existing methods for humanoid robot mapping, visual mapping has a lower cost and can obtain rich texture and color information, which helps to perform more accurate environmental modeling. The RGB-D dense mapping method is a visual mapping method that uses the color images and depth information obtained by an RGB-D sensor to construct a dense three-dimensional map of the scene. During the mapping process, the oscillation of the center of mass of the humanoid robot caused by human-like walking results in a large camera displacement, and the ground reaction force caused by the impact of the legs on the ground will affect the acquisition of images, causing data blurring. Generally speaking, the performance of the current RGB-D dense mapping method depends on the smoothness of camera movement and good texture of the scene. Therefore, for humanoid robots, the accuracy and robustness of the RGB-D dense mapping method will both decrease.

[0006] Currently, there are generally two types of strategies to improve the robustness and accuracy of RGB-D dense mapping. One is to integrate inertial information to compensate for the lack of visual information, thereby improving the performance of dense mapping when moving quickly or in a textureless environment. However, due to the inevitable drift error in pose estimation, which increases over time, the performance of dense mapping will be affected by time. The other is to design many special optimization algorithms in the backend process of dense mapping to minimize the drift error. However, these optimization strategies are different for different scenarios, and it is still a challenging problem to improve the robustness and precision of dense mapping in general scenarios.

[0007] In summary, due to the human-like walking of humanoid robots, the vertical movement and bumps caused thereby amplify the motion effect during the sensor acquisition process. The sensor shaking makes the acquired data blurred, affecting the quality of map construction, resulting in inaccurate positioning of humanoid robots, reducing the operation precision when performing tasks, and at the same time increasing the risk of collisions during the movement of humanoid robots. 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 during the walking of humanoid robots makes the acquired data blurred and affects the quality of map construction.

[0009] To solve the above technical problem, the present invention provides a method for constructing a dense point cloud map of a humanoid robot, including:

[0010] Real-time acquiring video images collected when the humanoid robot is walking;

[0011] Judging the gait phase of the humanoid robot in the current frame image of the video image, including:

[0012] Obtain the sensor information of the humanoid robot during walking; according to the sensor information, using the gait phase model, classify 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 the left / right foot single-support phase and the double-support phase respectively;

[0013] Estimate the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image, including:

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

[0015] If the gait phase is the left / right foot single-support phase of rotational motion, use the brute force matching model 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, judge the gait phase of the humanoid robot in the next frame image in the video image;

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

[0018] Perform local mapping and loop detection according to the key frame 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 joint encoder data of the humanoid robot during walking; among them, the IMU data of the humanoid robot during walking includes the IMU data of the center of mass of the humanoid robot and the IMU data of the two feet of the humanoid robot.

[0020] Preferably, after obtaining the sensor information of the humanoid robot during walking, preprocess the sensor information, including:

[0021] Use the smooth function to smooth the sensor information, and then use the principal component analysis method for dimensionality reduction.

[0022] Preferably, according to the sensor information, using the gait phase model, classify 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 the left / right foot single-support phase and the double-support phase respectively, including:

[0023] According to the IMU data of the center of mass of the humanoid robot and the joint encoder data, obtain the angular velocity of the center of mass in the z-axis direction; calculate the average value and standard deviation of the angular velocity of the center of mass in the z-axis direction within the time window, and input them into the trained support vector machine to classify the gait of the humanoid robot in the current frame image into translational motion and rotational motion;

[0024] Obtain the vertical acceleration of the feet based on the IMU data of the two feet of the humanoid robot; obtain the ground reaction force and torque based on the force-sensitive resistor data; based on the vertical acceleration of the feet and the ground reaction force and torque, use the Gaussian mixture model to cluster the translational motion and rotational motion into the left / right foot single-support phase and the double-support phase respectively.

[0025] Preferably, if the gait phase is the left / right foot single-support phase of the rotational motion, then use the brute-force matching model 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 descriptors of the ORB feature points of the current frame image as the query descriptors;

[0027] Obtain the ORB feature points of the previous frame image, and calculate the descriptors of the ORB feature points of the previous frame image as the matching descriptors;

[0028] For each query descriptor , obtain the matching descriptor with the smallest Hamming distance to it as the matching result of the current query descriptor;

[0029] Estimate the camera pose of the current frame image according to the matching result.

[0030] Preferably, after obtaining the matching descriptor with the smallest Hamming distance to the query descriptor as the matching result of the current query descriptor, calculate the Hamming distance between this matching descriptor and all query descriptors; if the Hamming distance between the current query descriptor and this matching descriptor is also the smallest, then retain this matching result, otherwise discard this matching result.

[0031] Preferably, after retaining the matching result of the current query descriptor and the matching descriptor , obtain the matching descriptor with the second smallest Hamming distance to the current query descriptor , and calculate the Hamming distance between the current query descriptor and the matching descriptor and the Hamming distance between the current query descriptor and the matching descriptor ; judge the size relationship between the ratio and the preset distance ratio; if the ratio is less than the preset distance ratio, then retain this matching result, otherwise discard this matching result.

[0032] Select a location and determine whether the current frame image is a key frame based on the camera pose of the current frame image, including:

[0033] If the number of inliers in the current frame image exceeds a preset minimum inlier threshold and meets any of the following conditions, it is a key frame; where the first condition is that the frame difference between the current frame image and the previous key frame is greater than a preset maximum number of frames; the second condition is that the frame difference between the current frame image and the previous key frame is greater than a preset minimum number of frames and the local mapping thread is in an idle state; the third condition is that the number of key frames in the key frame queue of the local mapping thread does not exceed three.

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

[0035] Solve the translation matrix and rotation matrix between the current frame image and the previous frame image by constructing a least squares problem. The formula is:

[0036] ;

[0037] Among them, 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 number index, represents the rotation matrix, represents the translation matrix;

[0038] Obtain the transformation matrix based on the translation matrix and rotation matrix between the current frame image and the previous frame image ;

[0039] If the norm of the transformation matrix is less than a preset value, determine whether the current frame image is a key frame based on the camera pose of the current frame image.

[0040] The present invention also provides a dense point cloud map construction system for a humanoid robot, including:

[0041] A video acquisition module for real-time acquisition of video images collected when the humanoid robot walks;

[0042] A gait judgment module for judging the gait phase of the humanoid robot in the current frame image of the video image, including: obtaining the sensor information when the humanoid robot walks; according to the sensor information, using the gait phase model, classifying 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 the left / right foot single support phase and the double support phase respectively;

[0043] The camera pose estimation module is used to estimate the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image, including: if the gait phase is the left / right foot single support stage of translational motion, the constant velocity motion model is used to estimate the camera pose of the current frame image; if the gait phase is the left / right foot single support stage of rotational motion, the brute force matching model is used to estimate the camera pose of the current frame image; if the gait phase is the double support stage of translational motion or the double support stage of rotational motion, the gait phase of the humanoid robot in the next frame image in the video image is judged.

[0044] The key frame judgment module is used to judge whether the current frame image is a key frame based on the camera pose of the current frame image.

[0045] The map construction module is used to perform local mapping and loop detection according to the key frames to obtain a global dense point cloud map.

[0046] The above technical solution of the present invention has the following beneficial effects compared with the prior art:

[0047] The method for constructing a dense point cloud map of a humanoid robot according to the present invention, on the basis of the original ORBSLAM2 algorithm, first judges the gait phase of the humanoid robot when selecting key frames, and selects the image frames in the more stable left / right foot single support stage for subsequent processing. It can more accurately track the camera pose in the case of sensor bumps and jitters caused by gait, minimize the influence of gait on the camera, and obtain more stable image frames for subsequent processing; and only uses the computationally intensive brute force matching model to estimate the camera pose of the current frame image in the rotational motion with large speed changes and severe camera jitters. On the premise of consuming a small amount of computing resources, it can make full use of the global information in the image, better capture the overall change relationship between images, so as to more accurately estimate the camera pose and improve the quality of point cloud map construction. The present invention effectively improves the fuzzy and jittery situation of data collection during map construction of humanoid robots, can obtain a more accurate environmental map, improves the operation accuracy of the robot when performing tasks, and reduces the risk of collision during the movement of the humanoid robot.

[0048] Furthermore, the present invention judges the norm of the transformation matrix of two adjacent frame images, and selects the image frame with the norm less than the preset threshold as the key frame to avoid the image frames with high overlap and large influence of gait being selected as key frames, reducing the drift and overlap during map construction, and further improving the quality of the map constructed by the humanoid robot. Description of the Drawings

[0049] In order to make the content of the present invention easier to be clearly understood, the following further details the present invention according to the specific embodiments of the present invention and in combination with the drawings, where:

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

[0051] Figure 2 is an example diagram of the mapping scene of a humanoid robot;

[0052] Figure 3 is the relationship diagram between translational and rotational motion and Laplacian variance and centroid three-dimensional velocity, where Figure 3 in (a) is the relationship diagram between translational and rotational motion and Laplacian variance, Figure 3 in (b) is the relationship diagram between translational and rotational motion and centroid three-dimensional velocity;

[0053] Figure 4 is the relationship diagram between gait phase and Laplacian variance and centroid three-dimensional velocity, where Figure 4 in (a) is the relationship diagram between gait phase and Laplacian variance, Figure 4 in (b) is the relationship diagram between gait phase and centroid three-dimensional velocity;

[0054] Figure 5 is the relationship diagram between gait phase and vertical ground reaction force and acceleration of the foot, where Figure 5 in (a) is the relationship diagram between gait phase and vertical ground reaction force of the foot, Figure 5 in (b) is the relationship diagram between gait phase and vertical acceleration of the foot;

[0055] Figure 6 is the comparison diagram of mapping effects before and after optimization of the present invention, where Figure 6 in (a) row is the mapping effect diagram before optimization, Figure 6 in (b) row is the mapping effect diagram after optimization. Specific implementation manner

[0056] 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 the present invention and be able to implement it, but the examples given are not intended to limit the present invention. Embodiment 1

[0057] The ORBSLAM2 algorithm is a dense point cloud map construction algorithm applicable to wheeled robots. However, the wheeled robot walks relatively smoothly, while the camera on the humanoid robot shakes during walking. Directly applying the ORBSLAM2 algorithm to the humanoid robot will cause the problem of blurred collected data and reduced map construction quality. Therefore, the present invention improves the ORBSLAM2 algorithm by analyzing the gait of the humanoid robot walking, making it applicable to the construction of a dense point cloud map of the humanoid robot.

[0058] Refer to Figure 1As shown in the figure, the present invention provides a method for constructing a dense point cloud map of a humanoid robot, including:

[0059] S1: Real-time acquire the video images collected when the humanoid robot walks;

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

[0061] Acquire the sensor information when the humanoid robot walks and perform preprocessing; according to the preprocessed 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 the left / right foot single support phase and the double support phase respectively;

[0062] S3: Estimate the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image, including:

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

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

[0065] If the gait phase is the double support phase of translational motion or the double support phase of rotational motion, judge the gait phase of the humanoid robot in the next frame image of the video image;

[0066] S4: Judge whether the current frame image is a key frame based on the camera pose of the current frame image; if not, return to judge the gait phase of the humanoid robot in the next frame image of the video image;

[0067] S5: Perform local mapping and loop detection according to the key frames to obtain a global dense point cloud map.

[0068] The scenario used for simulation in this embodiment is an indoor closed scenario. Walls, obstacles, etc. are added and visualized through the simulation software vrep. The indoor scene example of this embodiment is referred to Figure 2 as shown.

[0069] The video images in S1 need to collect color image frames and depth image frames.

[0070] In this embodiment, when the humanoid robot walks, the head camera acquires depth images and color images, and uses the rosbag tool to record them.

[0071] S2 is used to predict the gait phase of the humanoid robot in real time during walking. To optimize the mapping process of the humanoid robot, the present invention establishes a gait phase model of the humanoid robot, uses it to identify the gait phase during the walking of the humanoid robot, and optimizes the mapping according to different gait phases to minimize the external motion interference caused by walking.

[0072] Designing the gait phase model of the humanoid robot includes the following steps:

[0073] S21: To design the gait phase model, the present invention first studies the centroid dynamics of the humanoid robot during walking. These dynamics are described by the Newton-Euler equations:

[0074] ;

[0075] ;

[0076] where and are the position and acceleration of the centroid respectively, is the angular momentum rate around the centroid, and are the ground reaction force and torque of the k-th contact point, is the k-th contact point between the robot and the ground, is the gravity vector, is the mass of the robot.

[0077] S22: From the centroid dynamics, it can be known that the key indicators of the robot gait phase are the position and acceleration of the centroid, the ground reaction force and torque. Therefore, the present invention collects the sensor information of the humanoid robot during walking, including: IMU data, force-sensitive resistor data, and joint encoder data during the walking of the humanoid robot; among them, the IMU data during the walking of the humanoid robot includes the IMU data of the centroid of the humanoid robot and the IMU data of the two feet of the humanoid robot. And the vertical acceleration of the centroid and the angular velocity of the centroid in the z-axis direction are obtained according to the IMU data of the robot centroid and the joint encoder data, the vertical acceleration of the foot is obtained according to the IMU data of the robot's two feet, and the ground reaction force and torque are obtained according to the force-sensitive resistor data.

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

[0079] In this embodiment, the data information in the bag file is read, and the sensor information collected during the walking of the humanoid robot is preprocessed, including:

[0080] The sensor information is smoothed using a smooth function to filter out the mutated data caused by the front - back and left - right swaying of the robot due to instability.

[0081] Then, the principal component analysis method is used for dimensionality reduction, mapping the data into a two - dimensional space, effectively capturing the latent dynamic information.

[0082] S23: Based on the above - pre - processed sensor information, the present invention constructs a gait phase model for gait phase estimation. The gait phase model includes a support vector machine and a Gaussian mixture model. The support vector machine is used to first classify the gait of the humanoid robot during walking into rotational motion and translational motion, and the Gaussian mixture model is used to further divide the gait phase into the left / right foot single - support phase and the double - support phase. It specifically includes the following steps:

[0083] S231: First, the angular velocity of the centroid in the z - axis direction collected is used to perform a preliminary classification of the gait. Calculate the average value and standard deviation of the angular velocity of the centroid in the z - axis direction within the time window:

[0084] ;

[0085] ;

[0086] Among them, is the angular velocity at the th moment, is the number of data within the time window, and represent the average value and standard deviation of the angular velocity respectively.

[0087] In the training stage, thresholds are set for the average value and standard deviation of the angular velocity and used as the input of the support vector machine. Rotational motion and translational motion are used as different labels as the output. Through learning, the gait is successfully classified into rotational motion and translational motion.

[0088] In the inference stage, 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, and the gait of the current - frame humanoid robot is classified into translational motion and rotational motion.

[0089] S232: After using the support vector machine to classify the gait of the current - frame humanoid robot into translational motion and rotational motion, the Gaussian mixture model is used to cluster the rotational motion and translational motion respectively. Each class outputs three groups of data. The Gaussian mixture model classifies by fitting the Gaussian distributions of each class and according to the probability of each data point, clustering the translational motion and rotational motion into the left / right foot single - support phase and the double - support phase respectively.

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

[0091] The present invention analyzes the stages of regular and irregular movements of the robot during walking through different gait phases, and designs a mapping optimization method.

[0092] In S3, in order to evaluate the influence of rotational motion and translational motion on the acquired image frames, the present invention uses the Laplacian variance for evaluation. The Laplacian variance evaluates the clarity of an image by calculating the degree of gray-scale change in the image. First, the color image is converted into a grayscale image:

[0093] ;

[0094] where R, G, and B are the pixel values of the red, green, and blue channels of the image respectively, is the grayscale value of the image.

[0095] Convolve the image with the Laplacian operator kernel to obtain the Laplacian transform result of the image, capture the region of gray-scale change in the image, and calculate the variance:

[0096] ;

[0097] where, is the variance of the Laplacian value, is the Laplacian value of the u-th pixel point in the image, is the average value of all Laplacian values, is the total number of pixel points in the image.

[0098] The larger the calculated variance, the more obvious the image edges and the clearer the image; the smaller the calculated variance, the smoother the image edge changes, the lack of obvious edges or details, and the more blurred the image. The present invention has found through calculation and comparison that rotational motion is more likely to cause image blurring than translational motion.

[0099] To evaluate the influence of rotational motion and translational motion on the camera motion speed, the present invention evaluates it through the three-dimensional centroid speed estimated by the State Estimation Robot Walking (SEROW) framework. The robot walking framework calculates the centroid speed by using the previously acquired sensor information. It has been found through comparison that the centroid speed of rotational motion usually experiences larger fluctuations and instability than that of translational motion.

[0100] Specifically refer to Figure 3 as shown, Figure 3In (a), it is a graph of the relationship between translational and rotational motion and the Laplacian variance, where the black line is the variance of the Laplacian determinant; Figure 3 In (b), it is a graph of the relationship between translational and rotational motion and the three-dimensional velocity of the centroid, where the blue, red, and green lines are the centroid velocities on the x, y, and z axes respectively. Figure 3 It can be seen that the Laplacian variances during rotational motion are all smaller than those during translational motion. Therefore, rotational motion is more likely to cause image blurring compared to translational motion. At the same time, the centroid velocity during rotational motion usually experiences larger fluctuations and instability compared to translational motion.

[0101] To evaluate the influence of each gait phase on the acquired image frames, the Laplacian variance and the three-dimensional centroid velocity are also used for evaluation. Through comparison, it is found that most of the peaks in the Laplacian variance graph are located within the left / right foot single-support phase, and the image clarity is relatively high. At the same time, most of the zero-crossings of the three-dimensional centroid velocity occur at the beginning of the left and right foot single-support phases. At this moment, the minimum motion is propagated to the camera, so the probability of capturing high-quality images is high.

[0102] Specifically refer to Figure 4 as shown in Figure 4 In (a), it is a graph of the relationship between gait phase and the Laplacian variance, where the black line is the variance of the Laplacian determinant, and the red dots represent local maxima; Figure 4 In (b), it is a graph of the relationship between gait phase and the three-dimensional centroid velocity, where the blue, red, and green lines are the centroid velocities on the x, y, and z axes, and the red dots are the corresponding zero-crossing points. Figure 4 It can be seen that most of the peaks in the Laplacian variance graph are located within the left / right foot single-support phase, and at this time the image clarity is relatively high. Most of the zero-crossings of the three-dimensional centroid velocity occur at the beginning of the left and right foot single-support phases, and at this time the minimum motion is propagated to the camera, and the image quality is relatively high.

[0103] In addition, the present invention also compares the influence of each gait phase on the acquired image frames through sensor data. From the vertical accelerations of the left and right feet, it can be seen that after the left / right foot single-support phase, the impact accelerations of the left and right legs stably approach zero on one side, and only a small amount is propagated to the image frames of the robot. Similarly, after entering the left / right foot single-support phase, the ground reaction forces of both feet are stabilized. Therefore, the image frames obtained in the left / right foot single-support phase are clearer and more stable.

[0104] Specifically refer to Figure 5 as shown in Figure 5 In (a), it is a graph of the relationship between gait phase and the vertical ground reaction force of the feet when the humanoid robot is walking, where the red and blue lines are the ground reaction forces of the left and right feet respectively; Figure 5In (b), it is a graph showing the relationship between gait phase and vertical foot acceleration, where the green and yellow lines represent the vertical accelerations of the left and right feet respectively. From Figure 5 it can be seen that the impact acceleration of the left and right feet stabilizes and approaches zero after the left / right foot single-support phase, and only a small amount propagates to the robot's image frame. Similarly, after entering the left / right foot single-support phase, the ground reaction forces of both feet become stable. Therefore, the image frames obtained during the left / right foot single-support phase are clearer and more stable.

[0105] Therefore, through comparison, it is found that rotational motion is more unstable than translational motion and the image is more likely to be blurred. The original pose estimation module in ORBSLAM2 is a constant-velocity motion model, and its core idea is: assuming that the object is in a uniform motion state within a short period (adjacent frames). However, for a humanoid robot during rotational motion, due to the irregular speed changes caused by gait and camera jitter, the motion model often fails to provide good pose estimation. Moreover, the brute-force matching model has disadvantages such as large computational complexity and poor scalability. Therefore, the brute-force matching model is only used in rotational motion with large speed changes and severe camera jitter.

[0106] Since it is found through comparison that the images during the left / right foot single-support phase are more stable and least affected by gait, the key frames in mapping are selected in this gait phase of the left / right foot single-support phase, and stable key frames are selected based on the translational and rotational changes between image frames.

[0107] Specifically, the constant-velocity motion model is tracking and matching, that is, re-projecting the map points onto the current frame for matching, and then optimizing according to the Bundle Adjustment (BA), which is equivalent to the classic Perspective-n-Point (PnP).

[0108] The following specifically introduces the situation of using the brute-force matching model to estimate the camera pose of the current frame image during the left / right foot single-support phase of rotational motion. The steps are as follows:

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

[0110] Assume , are the previous frame image and the current frame image respectively. The feature points extracted from the current frame image are , and the feature points extracted from the previous frame image are ; is the total number of feature points.

[0111] Calculate the descriptor for each feature point. The descriptors of the feature points in the current frame image are query descriptors, and the descriptors of the feature points in the previous frame image are matching descriptors; is the total number of descriptors. Match the descriptors of each frame image with the descriptor map of the previous frame. If the match is successful, estimate the initial pose of the camera.

[0112] S32: For each query descriptor , obtain the matching descriptor with the smallest Hamming distance to it as the matching result of the current query descriptor.

[0113] For each query descriptor in the current frame , find the most similar descriptor by calculating the Hamming distance to the matching descriptors in the previous frame . The calculation formula is as follows:

[0114] ;

[0115] where and are the values of the v-th bit in the query descriptor and the matching descriptor respectively, and is the total length of the descriptor.

[0116] For each query descriptor , calculate the Hamming distance to all the matching descriptors in the previous frame and select the descriptor with the smallest distance as the matching result:

[0117] .

[0118] S33: To ensure the quality of the match, use the condition of mutual best match. When and only when is the feature that is the best match for in the previous frame image, and at the same time is the feature that is the best match for in the current frame image, and are valid matches.

[0119] After obtaining the matching descriptor with the smallest Hamming distance to the query descriptor as the matching result of the current query descriptor, calculate this matching descriptor​​​​ The Hamming distance from all query descriptors; if the current query descriptor and the matching descriptor also have the smallest Hamming distance, then keep the matching result; otherwise, discard the matching result.

[0120] S34: After keeping the matching result of the current query descriptor and the matching descriptor , obtain the matching descriptor with the second smallest Hamming distance from the current query descriptor , and calculate the Hamming distance between the current query descriptor and the matching descriptor , as well as the Hamming distance between the current query descriptor and the matching descriptor . Determine the magnitude relationship between the ratio of the two Hamming distances and a preset distance ratio , which is expressed by the formula: ; If the ratio is less than the preset distance ratio, then keep the matching result; otherwise, discard the matching result.

[0121] ;

[0122] If the ratio is less than the preset distance ratio, then keep the matching result; otherwise, discard the matching result.

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

[0124] By this method, the quality and reliability of the matching can be improved under rotation and jitter conditions, thereby reducing the probability of tracking failure.

[0125] S36: Track the local map based on the estimated initial pose and further calculate and optimize the initial camera pose according to bundle adjustment. When the camera pose estimation is reliable, ORBSLAM2 will select key frames to add through S4.

[0126] In S4, it is determined whether the current frame image is a key frame based on the camera pose of the current frame image, including:

[0127] The number of inliers in the current frame image exceeds a preset minimum inlier threshold and satisfies any of the following conditions:

[0128] (1) The frame difference between the current frame image and the previous key frame is greater than a preset maximum number of frames;

[0129] (2) The frame difference between the current frame image and the previous key frame is greater than a preset minimum number of frames, and the local mapping thread is in an idle state;

[0130] (3) The number of key frames in the key frame queue of the local mapping thread does not exceed three.

[0131] The inlier points refer to the feature points in the current frame image that match successfully with the previous frame image and are obtained based on the camera pose of the current frame image.

[0132] Since the camera jitter caused by the gait of the humanoid robot during walking often results in large displacements and rotations between image frames, the present invention adds a relative motion screening condition to the original key frame screening conditions in ORBSLAM2. By judging the overlap degree of two adjacent frame images, stable key frames can be obtained.

[0133] Preferably, before judging whether the current frame image is a key frame based on the camera pose of the current frame image, it further includes:

[0134] By constructing a least squares problem to solve the translation matrix and rotation matrix between the current frame image and the previous frame image, the formula is:

[0135] ;

[0136] Among them, 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 number index, represents the rotation matrix, represents the translation matrix;

[0137] To simplify the problem, by subtracting the centroid of each feature point from its position, new feature points and of the current frame image and the previous frame image are obtained, eliminating the influence of translation and only retaining the position change of rotation:

[0138] ;

[0139] Expand and simplify the above formula:

[0140] ;

[0141] Perform SVD decomposition on the summation term in the formula. When the summation term matrix is full rank, the rotation matrix can be determined. Then use the calculated to solve the translation matrix :

[0142] ;

[0143] Obtain the transformation matrix based on the translation matrix and rotation matrix between the current frame image and the previous frame image;

[0144] If the norm of the transformation matrix is less than a preset value, determine whether the current frame image is a key frame based on the camera pose of the current frame image. When the displacement and rotation matrix are small, it indicates that the overlap degree between two adjacent frame images is small, the influence of gait is small, and the image is stable.

[0145] In S5, it includes:

[0146] Taking the current key frame as the center, construct a local map. Add the feature points matching the current key frame to the local map, and calculate the spatial relationship between the feature points according to the camera pose and the three-dimensional coordinates of the feature points to construct the topological structure of the local map; through loop detection and global optimization, connect the local maps to construct a global map. The global map is a sparse map composed of multiple key frames and their corresponding feature points, which can reflect the general structure of the entire scene and the movement trajectory of the camera.

[0147] For the image frames of non-key frames, ordinary frame local tracking is performed.

[0148] Figure 6 For the comparison of the mapping effects before and after optimization, where Figure 6 the row (a) in it is the mapping effect diagram before optimization, Figure 6 the row (b) in it is the mapping effect diagram after optimization. It can be seen from Figure 6 that the optimization results of the present invention have improved the loop detection, drift and overlap conditions during the mapping of humanoid robots.

[0149] In summary, the method for constructing a dense point cloud map of a humanoid robot according to the present invention, based on the original ORBSLAM2 algorithm, first judges the gait phase of the humanoid robot when selecting key frames, and selects the image frames in the more stable left / right foot single support stage for subsequent processing, which can more accurately track the camera pose in the case of sensor bumps and jitters caused by gait, minimize the influence of gait on the camera, and obtain more stable image frames for subsequent processing; and only uses the computationally intensive brute-force matching model to estimate the camera pose of the current frame image during rotational motions with large speed changes and severe camera jitters, and can make full use of the global information in the image on the premise of consuming a small amount of computing resources, better capture the overall change relationship between images, thereby more accurately estimate the camera pose and improve the quality of point cloud map construction. The present invention effectively improves the blurring and jitter of the data collected during the mapping of humanoid robots, can obtain a more accurate environmental map, improves the operation accuracy when the robot performs tasks, and reduces the risk of collision during the movement of the humanoid robot.

[0150] Further, the present invention determines the norm of the transformation matrix of two adjacent frames of images, and selects the image frame with the norm less than a preset threshold as the key frame, so as to avoid the image frames with high overlap and large influence of gait from being selected as key frames, reduce the drift and overlap during map building, and further improve the quality of the map built by the humanoid robot. Embodiment 2

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

[0152] A video acquisition module, configured to acquire the video images collected when the humanoid robot walks in real time;

[0153] A gait judgment module, configured to judge the gait phase of the humanoid robot in the current frame image of the video image, including: acquiring the sensor information when the humanoid robot walks; according to the sensor information, using the gait phase model, classifying 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 the left / right foot single support phase and the double support phase respectively;

[0154] A camera pose estimation module, configured to estimate the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image, including: if the gait phase is the left / right foot single support phase of translational motion, using a constant velocity motion model to estimate the camera pose of the current frame image; if the gait phase is the left / right foot single support phase of rotational motion, using a brute force matching model to estimate the camera pose of the current frame image; if the gait phase is the double support phase of translational motion or the double support phase of rotational motion, judging the gait phase of the humanoid robot in the next frame image of the video image;

[0155] A key frame judgment module, configured to judge whether the current frame image is a key frame based on the camera pose of the current frame image;

[0156] A map construction module, configured to perform local map building and loop detection according to the key frames to obtain a global dense point cloud map.

[0157] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can adopt the form of a computer program product implemented 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.

[0158] This application is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, and the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in the process Figure 1 one process or multiple processes and / or blocks Figure 1 or means for implementing the functions specified in multiple blocks.

[0159] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including instruction means, and the instruction means implement the functions specified in the process Figure 1 one process or multiple processes and / or blocks Figure 1 or means for implementing the functions specified in multiple blocks.

[0160] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in the process Figure 1 one process or multiple processes and / or blocks Figure 1 or means for implementing the functions specified in multiple blocks.

[0161] Obviously, the above embodiments are only examples for clear illustration and are not limitations on the implementation manners. For those of ordinary skill in the art, other different forms of changes or variations can be made based on the above description. It is not necessary and impossible to enumerate all implementation manners here. And the obvious changes or variations derived therefrom are still within the protection scope of the present invention.

Claims

1. A method for constructing a dense point cloud map of a humanoid robot, characterized in that, Including: Real-time acquisition of video images collected when a humanoid robot walks; Judging the gait phase of the humanoid robot in the current frame image of the video image, including: Obtaining the sensor information of the humanoid robot when walking; according to the sensor information, using the gait phase model, classifying 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 the left / right foot single support phase and the double support phase respectively; Estimating the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image, including: If the gait phase is the left / right foot single support phase of translational motion, use the constant velocity motion model to estimate the camera pose of the current frame image; If the gait phase is the left / right foot single support phase of rotational motion, use the brute force matching model to estimate the camera pose of the current frame image; If the gait phase is the double support phase of translational motion or the double support phase of rotational motion, judge the gait phase of the humanoid robot in the next frame image of the video image; Judging whether the current frame image is a key frame based on the camera pose of the current frame image, including: if the number of inliers in the current frame image exceeds the preset minimum threshold of inliers and meets any of the following conditions, it is a key frame; where the first condition is that the frame difference between the current frame image and the previous key frame is greater than the preset maximum number of frames; the second condition is that the frame difference between the current frame image and the previous key frame is greater than the preset minimum number of frames and the local mapping thread is in an idle state; the third condition is that the number of key frames in the key frame queue of the local mapping thread does not exceed three; Performing local mapping and loop detection based on the key frames to obtain a global dense point cloud map.

2. The method for constructing a dense point cloud map of a humanoid robot according to claim 1, wherein The sensor information of the humanoid robot when walking includes: IMU data, force-sensitive resistor data, and joint encoder data of the humanoid robot when walking; where the IMU data of the humanoid robot when walking includes the IMU data of the center of mass of the humanoid robot and the IMU data of the two feet of the humanoid robot.

3. A method for constructing a dense point cloud map of a humanoid robot according to claim 2, characterized in that, After obtaining the sensor information of the humanoid robot when walking, preprocessing the sensor information, including: Using the smooth function to smooth the sensor information, and then using the principal component analysis method for dimensionality reduction processing.

4. A method for constructing a dense point cloud map of a humanoid robot according to claim 2, characterized in that, According to the sensor information, using the gait phase model, classifying 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 the left / right foot single support phase and the double support phase respectively, including: According to the IMU data of the center of mass of the humanoid robot and the joint encoder data, obtaining the angular velocity of the center of mass in the z-axis direction; calculating the average value and standard deviation of the angular velocity of the center of mass in the z-axis direction within the time window, and inputting them into the trained support vector machine to classify the gait of the humanoid robot in the current frame image into translational motion and rotational motion; Obtaining the vertical acceleration of the feet according to the IMU data of the two feet of the humanoid robot; obtaining the ground reaction force and torque according to the force-sensitive resistor data; based on the vertical acceleration of the feet and the ground reaction force and torque, using the Gaussian mixture model to cluster the translational motion and rotational motion into the left / right foot single support phase and the double support phase respectively.

5. A method for constructing a dense point cloud map of a humanoid robot according to claim 1, characterized in that, If the gait phase is the left / right foot single-support phase of rotational motion, a brute-force matching model is used to estimate the camera pose of the current frame image, including: Extract the ORB feature points of the current frame image, and calculate the descriptors of the ORB feature points of the current frame image as query descriptors; Obtain the ORB feature points of the previous frame image, and calculate the descriptors of the ORB feature points of the previous frame image as matching descriptors; For each query descriptor , obtain the matching descriptor with the smallest Hamming distance from it as the matching result of the current query descriptor; Estimate the camera pose of the current frame image according to the matching result.

6. The method for constructing a dense point cloud map of a humanoid robot according to claim 5, wherein Obtain query descriptors The matching descriptor with the smallest Hamming distance After using it as the matching result of the current query descriptor, calculate this matching descriptor The Hamming distances from all query descriptors; if the current query descriptor also has the smallest Hamming distance from this matching descriptor , then retain this matching result, otherwise discard this matching result.

7. A method for constructing a dense point cloud map of a humanoid robot according to claim 6, characterized in that, Save the current query descriptor Matching descriptors After the matching results are obtained, get the descriptor related to the current query The matching descriptor with the second smallest Hamming distance , and calculate the current query descriptor Matching descriptors The Hamming distance between With the current query descriptor Matching descriptor The Hamming distance between ratio, and determine the size relationship between the ratio and a preset distance ratio; if the ratio is smaller than the preset distance ratio, retain the matching result, otherwise discard the matching result.

8. A method for constructing a dense point cloud map of a humanoid robot according to claim 1, characterized in that, Before determining whether the current frame image is a key frame based on the camera pose of the current frame image, it also includes: Solve the translation matrix and rotation matrix between the current frame image and the previous frame image by constructing a least-squares problem. The formula is: ； Among them, 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 number index, represents the rotation matrix, represents the translation matrix; Obtain the transformation matrix based on the translation matrix and 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, determine whether the current frame image is a key frame based on the camera pose of the current frame image.

9. A dense point cloud map construction system for a humanoid robot, characterized in that, It includes: A video acquisition module for real-time acquisition of video images collected when the humanoid robot walks; A gait judgment module for judging the gait phase of the humanoid robot in the current frame image of the video image, including: obtaining the sensor information when the humanoid robot walks; according to the sensor information, using the gait phase model, classifying 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 the left / right foot single-support phase and the double-support phase respectively; A camera pose estimation module for estimating the camera pose of the current frame image based on the gait phase of the humanoid robot in the current frame image, including: if the gait phase is the left / right foot single-support phase of translational motion, use a constant velocity motion model to estimate the camera pose of the current frame image; if the gait phase is the left / right foot single-support phase of rotational motion, use a brute-force matching model to estimate the camera pose of the current frame image; otherwise, judge the gait phase of the humanoid robot in the next frame image of the video image; A key frame judgment module for judging whether the current frame image is a key frame based on the camera pose of the current frame image, including: if the number of inliers in the current frame image exceeds a preset minimum inlier threshold and satisfies any of the following conditions, it is a key frame; where the first condition is that the frame difference between the current frame image and the previous key frame is greater than a preset maximum number of frames; the second condition is that the frame difference between the current frame image and the previous key frame is greater than a preset minimum number of frames and the local mapping thread is in an idle state; the third condition is that the number of key frames in the key frame queue of the local mapping thread does not exceed three; A map construction module for performing local mapping and loop detection according to the key frames to obtain a global dense point cloud map.

Images