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

Abstract

The invention relates to the technical field of point cloud map construction, in particular to a humanoid robot dense point cloud map construction method and system, and the method comprises the steps: obtaining a video image collected when a humanoid robot walks in real time; the gait phase of the humanoid robot in the current frame image in the video image is judged; estimating the camera pose of the current frame image based on the gait phase of the humanoid robot of the current frame image; judging whether the current frame image is a key frame or not based on the camera pose of the current frame image; and performing local mapping and loopback detection according to the key frame to obtain a global dense point cloud map. According to the method, the fuzzy and jittering conditions of the acquired data during mapping of the humanoid robot are effectively improved, a more accurate environment map can be obtained, the operation precision during task execution of the robot is improved, and the collision risk in the advancing process of the humanoid robot is reduced.

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 for a humanoid robot. Background Art

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

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

[0004] At present, the main methods for optimizing humanoid robot mapping 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 indoor environments using the HRP-2 robot by tightly coupling the 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, which 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 the motion inertial state estimator and elastically fused visual tracking, this fusion method achieves a lower drift rate and improves mapping accuracy.

[0005] Among the existing humanoid robot mapping methods, visual mapping has lower cost and can obtain rich texture and color information, which is helpful for more accurate environmental modeling. The RGB-D dense mapping method is a visual mapping method that uses color images and depth information obtained by RGB-D sensors to build 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 leads to a large camera displacement, and the ground reaction force caused by the impact of the legs on the ground will affect the acquisition of the image and cause data blur. Generally speaking, the performance of the current RGB-D dense mapping method depends on the smoothness of the camera motion and the good texture of the scene. Therefore, for humanoid robots, the accuracy and robustness of the RGB-D dense mapping method will be reduced.

[0006] There are generally two types of strategies to improve the robustness and accuracy of RGB-D dense mapping. One is to improve the performance of dense mapping in fast movement or without texture by integrating inertial information to compensate for the lack of visual information. However, since the drift error of pose estimation is inevitable and will increase over time, the performance of dense mapping will be affected by time. The second is to design many special optimization algorithms to minimize the drift error in the backend process of dense mapping. 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 the humanoid robot, the vertical movement and bumps caused thereby amplify the motion effect in the sensor collection process. The sensor shaking makes the collected data blurred, affecting the quality of map construction, causing inaccurate positioning of the humanoid robot, reducing the accuracy of its operation when performing tasks, and also increasing the risk of collision during the movement of the humanoid robot. Summary of the invention

[0008] To this end, the technical problem to be solved by the present invention is to overcome the problem in the prior art that when the humanoid robot walks, the sensor shakes, causing the collected data to be blurred, thereby affecting the quality of map construction.

[0009] In order to solve the above technical problems, the present invention provides a method for constructing a dense point cloud map of a humanoid robot, comprising: Real-time acquisition of video images collected when the humanoid robot walks; Determine the gait phase of the humanoid robot in the current frame image in the video image, including: Acquire sensor information when the humanoid robot is 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 movement and rotational movement, and cluster the translational movement and rotational movement into the left / right foot single support stage and double support stage respectively; The camera pose of the current frame image is estimated 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, 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 phase of rotational movement, 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 phase of translational motion or the double support phase of rotational motion, then determining the gait phase of the humanoid robot in the next frame of the video image; Determine whether the current frame image is a key frame based on the camera pose of the current frame image; Local mapping and loop detection are performed based on the key frames to obtain a global dense point cloud map.

[0010] Preferably, the sensor information when the humanoid robot walks includes: IMU data, force-sensitive resistor data and joint encoder data when the humanoid robot walks; wherein the IMU data when the humanoid robot walks includes IMU data of the center of mass of the humanoid robot and IMU data of both feet of the humanoid robot.

[0011] Preferably, after obtaining the sensor information of the humanoid robot when walking, preprocessing the sensor information includes: The smooth function is used to smooth the sensor information, and then the principal component analysis method is used to reduce the dimension.

[0012] Preferably, according to the sensor information, the gait phase model is used to divide the gait of the humanoid robot in the current frame image into translational movement and rotational movement, and the translational movement and rotational movement are clustered into a left / right foot single support phase and a double support phase, respectively, including: According to the IMU data and joint encoder data of the center of mass of the humanoid robot, 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 in 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; The vertical acceleration of the feet is obtained according to the IMU data of the humanoid robot's feet; the ground reaction force and torque are obtained according to the force-sensitive resistor data; based on the vertical acceleration of the feet and the ground reaction force and torque, the Gaussian mixture model is used to cluster the translational motion and the rotational motion into the left / right foot single support stage and the double support stage respectively.

[0013] Preferably, 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 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; For each query descriptor , get the matching descriptor with the smallest Hamming distance As the matching result of the current query descriptor; Estimate the camera pose of the current frame image based on the matching results.

[0014] Preferably, obtain and query descriptors The matching descriptor with the smallest Hamming distance As the matching result of the current query descriptor, calculate the matching descriptor The Hamming distance to all query descriptors; if the current query descriptor Matches this descriptor When the Hamming distance of is also the smallest, the matching result is retained, otherwise the matching result is discarded.

[0015] Preferably, keep the current query descriptor Matching descriptor 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.

[0016] Optionally, judging whether the current frame image is a key frame based on the camera pose of the current frame image includes: 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; the first condition is that the frame difference between the current frame image and the previous key frame is greater than the preset maximum frame number; the second condition is that the frame difference between the current frame image and the previous key frame is greater than the preset minimum frame number, 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 in the local mapping thread does not exceed three.

[0017] Preferably, before judging whether the current frame image is a key frame based on the camera posture of the current frame image, the method further includes: The translation matrix and rotation matrix between the current frame image and the previous frame image are solved by constructing a least squares problem. The formula is: ; in, 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, represents the rotation matrix, represents the translation matrix; Get 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, it is determined whether the current frame image is a key frame based on the camera posture of the current frame image.

[0018] The present invention also provides a humanoid robot dense point cloud map construction system, comprising: A video acquisition module is used to obtain video images collected when the humanoid robot walks in real time; The gait judgment module is used to judge the gait phase of the humanoid robot in the current frame image in the video image, including: obtaining sensor information when the humanoid robot walks; according to the sensor information, using the gait phase model, the gait of the humanoid robot in the current frame image is divided into translational movement and rotational movement, and the translational movement and rotational movement are clustered into the left / right foot single support stage and double support stage respectively; 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, then the camera pose of the current frame image is estimated by using a constant speed motion model; if the gait phase is the left / right foot single support stage of rotational motion, then the camera pose of the current frame image is estimated by using a brute force matching model; if the gait phase is the double support stage of translational motion or the double support stage of rotational motion, then the gait phase of the humanoid robot in the next frame image in the video image is determined; A key frame judgment module is used to judge whether the current frame image is a key frame based on the camera posture of the current frame image; The map construction module is used to perform local mapping and loop detection based on key frames to obtain a global dense point cloud map.

[0019] The above technical solution of the present invention has the following beneficial effects compared with the prior art: The method for constructing a dense point cloud map of a humanoid robot described in the present invention, based on the original ORBSLAM2 algorithm, first judges the gait phase of the humanoid robot when selecting key frames, selects more stable image frames of the left / right foot single support stage for subsequent processing, can more accurately track the camera posture under the condition of sensor bumps and jitters caused by gait, minimizes the impact of gait on the camera, and obtains more stable image frames for subsequent processing; and only in the rotational motion with large speed changes and severe camera jitter, a violent matching model with large computational complexity is used to estimate the camera posture of the current frame image, and the global information in the image can be fully utilized under the premise of consuming a small amount of computing resources, and the overall change relationship between images can be better captured, so as to more accurately estimate the camera posture and improve the quality of point cloud map construction. The present invention effectively improves the blurring and jittering of the collected data when the humanoid robot is building a map, can obtain a more accurate environmental map, improves the accuracy of the robot's operation when performing tasks, and reduces the risk of collision during the movement of the humanoid robot.

[0020] Furthermore, the present invention judges the norm of the transformation matrix of two adjacent frames of images and selects image frames whose norm is less than a preset threshold as key frames, so as to avoid image frames with high overlap and greater impact of gait from being selected as key frames, thereby reducing drift and overlap during map construction and further improving the quality of map construction by humanoid robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to make the content of the present invention more clearly understood, the present invention is further described in detail below according to specific embodiments of the present invention in conjunction with the accompanying drawings, wherein: Figure 1 It is a flow chart of a method for constructing a dense point cloud map of a humanoid robot according to the present invention; Figure 2 This is an example of a humanoid robot mapping scene; Figure 3 is a graph of the relationship between translational and rotational motion and the Laplace variance and the three-dimensional velocity of the center of mass, where Figure 3 (a) is the relationship between translation and rotation motion and Laplace variance. Figure 3 (b) is a diagram showing the relationship between translational and rotational motion and the three-dimensional velocity of the center of mass; Figure 4 is a plot of the gait phase versus the Laplace variance and the 3D velocity of the center of mass, where Figure 4 (a) is the relationship between gait phase and Laplace variance. Figure 4 (b) is the relationship between gait phase and three-dimensional velocity of center of mass; Figure 5 is a graph of the relationship between the gait phase and the vertical ground reaction force and acceleration of the foot, where Figure 5(a) is the relationship between the gait phase and the vertical ground reaction force of the foot. Figure 5 (b) is the relationship between gait phase and foot vertical acceleration; Figure 6 is a comparison chart of the mapping effects before and after the optimization of the present invention, where Figure 6 The (a) row in the figure is the image of the building effect before optimization. Figure 6 The (b) row in the figure shows the image construction effect after optimization. DETAILED DESCRIPTION

[0022] The present invention is further described below in conjunction with the accompanying drawings and specific embodiments so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention. Embodiment 1

[0023] The ORBSLAM2 algorithm is a dense point cloud map construction algorithm suitable for wheeled robots, but wheeled robots walk relatively smoothly, while the camera of a humanoid robot shakes when walking. Directly applying the ORBSLAM2 algorithm to a humanoid robot will cause the collected data to be blurred, resulting in a problem of reduced map construction quality. Therefore, the present invention improves the ORBSLAM2 algorithm by analyzing the walking gait of a humanoid robot, making it suitable for dense point cloud map construction of a humanoid robot.

[0024] Reference Figure 1 As shown, the present invention provides a method for constructing a dense point cloud map of a humanoid robot, comprising: S1: Real-time acquisition of video images collected when the humanoid robot walks; S2: Determine the gait phase of the humanoid robot in the current frame image in the video image, including: The sensor information of the humanoid robot when walking is obtained and preprocessed; according to the preprocessed sensor information, the gait of the humanoid robot in the current frame image is divided into translational movement and rotational movement by using the gait phase model, and the translational movement and rotational movement are clustered into the left / right foot single support stage and the double support stage respectively; S3: 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, 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 phase of rotational movement, 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 phase of translational motion or the double support phase of rotational motion, then determining the gait phase of the humanoid robot in the next frame of the video image; S4: judging whether the current frame image is a key frame based on the camera posture of the current frame image; if not, returning to judging the gait phase of the humanoid robot in the next frame image in the video image; S5: Perform local mapping and loop detection based on key frames to obtain a global dense point cloud map.

[0025] The scene used for simulation in this embodiment is an indoor closed scene, and walls, obstacles, etc. are added and visualized through the simulation software vrep. Figure 2 shown.

[0026] The video image of S1 needs to collect color image frames and depth image frames.

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

[0028] The S2 is used to predict the gait phase of the humanoid robot in real time when it walks. In order to optimize the mapping process of the humanoid robot, the present invention establishes a gait phase model of the humanoid robot, which is used to identify the gait phase of the humanoid robot during walking, and performs mapping optimization according to different gait phases to minimize the external motion interference caused by walking.

[0029] Designing a gait phase model for a humanoid robot involves the following steps: S21: In order to design the gait phase model, the present invention first studies the center of mass dynamics of the humanoid robot when walking. These dynamics are described by the Newton-Euler equations: ; ; in and are the position and acceleration of the center of mass, is the angular momentum rate around the center of mass, and are the ground reaction force and moment at the kth contact point, is the kth contact point between the robot and the ground, is the gravity vector, The quality of the robot.

[0030] S22: From the center of mass dynamics, it can be known that the key indicators of the robot gait stage are the position and acceleration of the center of mass, the ground reaction force and torque. Therefore, the sensor information collected by the present invention when the humanoid robot walks includes: IMU data, force-sensitive resistor data and joint encoder data when the humanoid robot walks; wherein the IMU data when the humanoid robot walks includes the IMU data of the center of mass of the humanoid robot and the IMU data of the feet of the humanoid robot. And according to the IMU data of the center of mass of the robot and the joint encoder data, the vertical acceleration of the center of mass and the angular velocity of the center of mass in the z-axis direction are obtained, the vertical acceleration of the foot is obtained according to the IMU data of the robot's feet, and the ground reaction force and torque are obtained according to the force-sensitive resistor data.

[0031] In this embodiment, in the VREP simulation environment in the ROS system, a 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 when the humanoid robot walks.

[0032] This embodiment reads the data information in the bag file and pre-processes the sensor information collected when the humanoid robot walks, including: The smooth function is used to smooth the sensor information and filter out the data that suddenly changes due to the instability of the robot, causing the robot to sway back and forth or left and right. The principal component analysis method is then used to reduce the dimensionality and map the data into a two-dimensional space, effectively capturing the dynamic information of the underlying layer.

[0033] S23: Based on the above preprocessed sensor information, the present invention constructs a gait phase model to estimate the gait phase. The gait phase model includes a support vector machine and a Gaussian mixture model, wherein the support vector machine is used to first divide the gait of the humanoid robot into rotational motion and translational motion when walking, and the Gaussian mixture model is used to further divide the gait phase into the left / right foot single support stage and the double support stage. Specifically, it includes the following steps: S231: First, the gait is preliminarily classified using the collected angular velocity of the center of mass in the z-axis direction, and 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: ; ; in, For the The angular velocity at time, is the number of data in the time window, and represent the mean and standard deviation of the angular velocity, respectively.

[0034] During the training phase, thresholds were set for the mean and standard deviation of the angular velocity and used as the input of the support vector machine. Rotational movement and translational movement were used as different labels as outputs. Through learning, the gait was successfully divided into rotational movement and translational movement.

[0035] In the inference stage, the mean and standard deviation of the angular velocity of the center of mass in the z-axis direction within the time window are input into the trained support vector machine, and the gait of the humanoid robot in the current frame is divided into translational motion and rotational motion.

[0036] S232. After using a support vector machine to classify the gait of the humanoid robot in the current frame into translational motion and rotational motion, a Gaussian mixture model is used to cluster the rotational motion and translational motion respectively, and each category is output as three sets of data. The Gaussian mixture model fits the Gaussian distribution of each category and classifies according to the probability of each data point, and clusters the translational motion and rotational motion into the left / right foot single support stage and the double support stage respectively.

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

[0038] The present invention analyzes the motion regularity and irregularity stages of the robot when walking through the obtained different gait phases, and designs a mapping optimization method.

[0039] In S3, in order to evaluate the influence of rotational motion and translational motion on the acquired image frame, the present invention uses Laplace variance for evaluation. Laplace variance evaluates the clarity of the image by calculating the degree of grayscale change in the image. First, the color image is converted into a grayscale image: ; Among them, R, G and B are the pixel values ​​of the red, green and blue channels of the image respectively. is the gray value of the image.

[0040] Convolve the Laplace operator kernel with the image to obtain the Laplace transform result of the image, capture the area of ​​grayscale change in the image, and calculate the variance: ; in, is the variance of the Laplace value, is the Laplace value of the u-th pixel in the image, is the average of all Laplace values, is the total number of pixels in the image.

[0041] The larger the calculated variance, the more obvious the image edge is, and the clearer the image is; the smaller the calculated variance, the more gradual the image edge change is, the lack of obvious edges or details, and the image is more blurred. The present invention has found through calculation and comparison that rotational motion is more likely to produce blurred images than translational motion.

[0042] In order to evaluate the impact of rotational and translational motion on the camera motion speed, the present invention evaluates the three-dimensional center of mass speed estimated by the State Estimation Robot Walking (SEROW) framework. The robot walking framework uses the previously acquired sensor information to infer the center of mass speed. After comparison, it is found that the center of mass speed of rotational motion usually experiences greater fluctuations and instability than that of translational motion.

[0043] Specific reference Figure 3 As shown, Figure 3 (a) is the relationship between translation and rotation motion and Laplace variance, where the black line is the variance of the Laplace determinant; Figure 3 (b) is a graph showing the relationship between translational and rotational motion and the three-dimensional velocity of the center of mass, where the blue, red, and green lines are the velocity of the center of mass on the x, y, and z axes, respectively. Figure 3 It can be seen that the Laplace variance during rotational motion is smaller than that during translational motion. Therefore, rotational motion is more likely to produce blurred images than translational motion. At the same time, the center of mass velocity of rotational motion usually experiences greater fluctuations and instability than that of translational motion.

[0044] In order to evaluate the impact of each gait phase on the acquired image frame, Laplace variance and 3D center of mass velocity are also used for evaluation. By comparison, it is found that most of the peaks in the Laplace variance graph are located in the left / right foot single support phase, and the image clarity is high. At the same time, most of the zero crossings of the 3D center of mass velocity occur at the beginning of the left and right foot single support phase. At this moment, the smallest movement is propagated to the camera, so the probability of capturing high-quality images is high.

[0045] Specific reference Figure 4 As shown, Figure 4 (a) is the relationship between gait phase and Laplace variance, where the black line is the variance of the Laplace determinant and the red dot indicates the local maximum value; Figure 4 (b) in the figure is a graph showing the relationship between the gait phase and the three-dimensional velocity of the center of mass, where the blue, red, and green lines are the velocity of the center of mass 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 Laplace variance map are located in the left / right foot single support phase, when the image clarity is high. Most of the zero crossings of the 3D center of mass velocity occur at the beginning of the left and right foot single support phase, when the smallest motion is propagated to the camera and the image quality is high.

[0046] In addition, the present invention also uses sensor data to compare the impact of each gait phase on the acquired image frames. It can be seen from the vertical acceleration of the left and right feet that the impact acceleration of the left and right legs stabilizes and approaches zero after the left / right foot single support phase stage, and only a small amount is transmitted to the image frame of the robot. Similarly, after entering the left / right foot single support phase stage, the ground reaction force of both feet is stabilized, so the image frames obtained in the left / right foot single support phase are clearer and more stable.

[0047] Specific reference Figure 5 As shown, Figure 5 (a) is a diagram showing the relationship between the gait phase and the vertical ground reaction force of the foot when the humanoid robot walks, where the red and blue lines are the ground reaction forces of the left and right feet, respectively; Figure 5 (b) in the figure is the relationship between gait phase and foot vertical acceleration, where the green and yellow lines are the vertical accelerations of the left and right feet, respectively. 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 is transmitted to the robot's image frame. Similarly, after entering the left / right foot single support phase, the ground reaction force of both feet is stabilized, so the image frame obtained in the left / right foot single support phase is clearer and more stable.

[0048] Therefore, the present invention concludes through comparison that the speed of rotational motion is more unstable and the image is more easily blurred than that of translational motion, and the original pose estimation module in ORBSLAM2 is a constant-speed motion model. The core idea is: assuming that the object is in a uniform motion state in a short period of time (adjacent frames), and the humanoid robot is in rotational motion due to irregular speed changes caused by gait and camera jitter, the motion model often cannot provide good pose estimation, and the brute force matching model has the disadvantages of 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.

[0049] Since the comparison shows that the image in the left / right foot single support stage is more stable and least affected by gait, the key frame in the map construction is selected in the gait phase of the left / right foot single support stage, and the stable key frame is selected through the translation and rotation changes between image frames.

[0050] Specifically, the constant speed motion model is tracking matching, that is, the map points are re-projected to the current frame for matching, and then optimized according to the bundle adjustment (BA), which is equivalent to the classic PnP (Perspective-n-Point).

[0051] The following specifically describes the situation where the gait phase is the left / right foot single support phase of rotational movement, and the brute force matching model is used to estimate the camera pose of the current frame image. The steps include: S31: extracting the ORB feature points of the current frame image, and calculating the descriptors of the ORB feature points of the current frame image as the query descriptors; obtaining the ORB feature points of the previous frame image, and calculating the descriptors of the ORB feature points of the previous frame image as the matching descriptors.

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

[0053] Calculate the descriptor of each feature point, the current frame image Descriptors of feature points To query the descriptor, the previous frame Descriptors of feature points is the matching descriptor; is the total number of descriptors. The descriptor of each frame is matched with the description of the previous frame. If the match is successful, the initial camera pose is estimated.

[0054] S32: For each query descriptor , get the matching descriptor with the smallest Hamming distance As the matching result of the current query descriptor.

[0055] For the current frame Each query descriptor of The Hamming distance of the matching descriptors in the search is used to find the most similar descriptor. The calculation formula is as follows: ; in, and Query Descriptor and match descriptors The value of the vth position in is the total length of the descriptor.

[0056] For each query descriptor , calculate all matching descriptors with the previous frame The Hamming distance , select the descriptor with the smallest distance As a result of the match: .

[0057] S33: To ensure the quality of the match, use the best matching conditions. yes The best matching feature in the previous frame, and yes When the best matching feature is found in the current frame image, and Only then can it be considered a valid match.

[0058] Get and query descriptors The matching descriptor with the smallest Hamming distance As the matching result of the current query descriptor, calculate the matching descriptor The Hamming distance to all query descriptors; if the current query descriptor Matches this descriptor When the Hamming distance of is also the smallest, the matching result is retained, otherwise the matching result is discarded.

[0059] S34: Keep the current query descriptor Matching descriptor After the matching results are obtained, get the current query descriptor The matching descriptor with the second smallest Hamming distance , and calculate the current query descriptor Matching descriptor The Hamming distance between With the current query descriptor Matching descriptor The Hamming distance between The ratio of the ratio and the preset distance ratio are determined. The size relationship between them is expressed as follows: ; If the ratio is smaller than the preset distance ratio, the matching result is retained, otherwise the matching result is discarded.

[0060] S35: Estimate the initial camera pose of the current frame image according to the matching result.

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

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

[0063] In S4, judging whether the current frame image is a key frame based on the camera posture of the current frame image includes: The number of inliers in the current frame image exceeds the preset minimum threshold of inliers and meets any of the following conditions: (1) The frame number difference between the current frame image and the previous key frame is greater than the preset maximum frame number; (2) The frame difference between the current frame image and the previous key frame is greater than the preset minimum frame number, and the local mapping thread is in an idle state; (3) The number of key frames in the key frame queue of the local mapping thread does not exceed three.

[0064] The inliers refer to feature points in the current frame image that are successfully matched with the previous frame image, and are obtained based on the camera pose of the current frame image.

[0065] When a humanoid robot walks, the camera shakes due to the gait, which often causes large displacement and rotation between image frames. Therefore, the present invention adds a relative motion filtering condition to the original key frame filtering condition in ORBSLAM2, and judges the overlap between two adjacent frames of images to obtain a stable key frame.

[0066] Preferably, before judging whether the current frame image is a key frame based on the camera posture of the current frame image, the method further includes: The translation matrix and rotation matrix between the current frame image and the previous frame image are solved by constructing a least squares problem. The formula is: ; in, 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, represents the rotation matrix, represents the translation matrix; To simplify the problem, the new feature points of the current frame image and the previous frame image are obtained by subtracting the centroid of each feature point from its position. and , eliminating the effect of translation and retaining only the position change of rotation: ; Expand and simplify the above formula: ; 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 : ; Get 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 the preset value, the current frame image is determined to be a key frame based on the camera posture of the current frame image. When the displacement and rotation matrices are small, it means that the overlap between two adjacent frames is small, the gait is less affected, and the image is stable.

[0067] The S5 includes: Construct a local map with the current keyframe as the center. Add the feature points matching the current keyframe to the local map, and calculate the spatial relationship between the feature points based on the camera pose and the three-dimensional coordinates of the feature points to construct the topological structure of the local map; connect the local maps through loop detection and global optimization to construct a global map. The global map is a sparse map composed of multiple keyframes and their corresponding feature points, which can reflect the general structure of the entire scene and the movement trajectory of the camera.

[0068] For non-keyframe image frames, ordinary frame local tracking is performed.

[0069] Figure 6 To compare the mapping effects before and after optimization, Figure 6 The (a) row in the figure is the image construction effect before optimization. Figure 6 The (b) line in the figure is the image of the optimized graph. Figure 6 It can be seen that the optimization results of the present invention have improved the loop detection, drift and overlap during the mapping of the humanoid robot.

[0070] In summary, the method for constructing a dense point cloud map of a humanoid robot described in the present invention, based on the original ORBSLAM2 algorithm, first judges the gait phase of the humanoid robot when selecting key frames, selects more stable image frames of the left / right foot single support stage for subsequent processing, can more accurately track the camera posture under the condition of sensor bumps and jitters caused by gait, minimizes the impact of gait on the camera, and obtains more stable image frames for subsequent processing; and only in the rotational motion with large speed changes and severe camera jitter, a violent matching model with large computational complexity is used to estimate the camera posture of the current frame image, and the global information in the image can be fully utilized under the premise of consuming a small amount of computing resources, and the overall change relationship between images can be better captured, so as to more accurately estimate the camera posture and improve the quality of point cloud map construction. The present invention effectively improves the blurring and jittering of the collected data when the humanoid robot is building a map, can obtain a more accurate environmental map, improves the accuracy of the robot's operation when performing tasks, and reduces the risk of collision during the movement of the humanoid robot.

[0071] Furthermore, the present invention judges the norm of the transformation matrix of two adjacent frames of images and selects image frames whose norm is less than a preset threshold as key frames, so as to avoid image frames with high overlap and greater impact of gait from being selected as key frames, thereby reducing drift and overlap during map construction and further improving the quality of map construction by humanoid robots. Embodiment 2

[0072] Based on the method for constructing a dense point cloud map of a humanoid robot described in the first embodiment, this embodiment further provides a system for constructing a dense point cloud map of a humanoid robot, including: A video acquisition module is used to obtain video images collected when the humanoid robot walks in real time; The gait judgment module is used to judge the gait phase of the humanoid robot in the current frame image in the video image, including: obtaining sensor information when the humanoid robot walks; according to the sensor information, using the gait phase model, the gait of the humanoid robot in the current frame image is divided into translational movement and rotational movement, and the translational movement and rotational movement are clustered into the left / right foot single support stage and double support stage respectively; 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, then the camera pose of the current frame image is estimated by using a constant speed motion model; if the gait phase is the left / right foot single support stage of rotational motion, then the camera pose of the current frame image is estimated by using a brute force matching model; if the gait phase is the double support stage of translational motion or the double support stage of rotational motion, then the gait phase of the humanoid robot in the next frame image in the video image is determined; A key frame judgment module is used to judge whether the current frame image is a key frame based on the camera posture of the current frame image; The map construction module is used to perform local mapping and loop detection based on key frames to obtain a global dense point cloud map.

[0073] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application may 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 codes.

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

[0075] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0076] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0077] Obviously, the above embodiments are merely examples for the purpose of clear explanation and are not intended to limit the implementation methods. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the implementation methods here. The obvious changes or modifications derived therefrom are still within the scope of protection of the present invention.

Claims

1. A method for constructing a dense point cloud map of a humanoid robot, characterized in that: include: Real-time acquisition of video images collected when the humanoid robot walks; Determine the gait phase of the humanoid robot in the current frame image in the video image, including: Acquire sensor information when the humanoid robot is 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 movement and rotational movement, and cluster the translational movement and rotational movement into the left / right foot single support stage and double support stage respectively; The camera pose of the current frame image is estimated 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, 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 phase of rotational movement, 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 phase of translational motion or the double support phase of rotational motion, then determining the gait phase of the humanoid robot in the next frame of the video image; Determine whether the current frame image is a key frame based on the camera pose of the current frame image; Local mapping and loop detection are performed based on the key frames to obtain a global dense point cloud map.

2. A method for constructing a dense point cloud map of a humanoid robot according to claim 1, characterized in that: The sensor information when the humanoid robot walks includes: IMU data, force-sensitive resistor data and joint encoder data when the humanoid robot walks; wherein the IMU data when the humanoid robot walks includes IMU data of the center of mass of the humanoid robot and IMU data of both 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 when the humanoid robot is walking, the sensor information is preprocessed, including: The smooth function is used to smooth the sensor information, and then the principal component analysis method is used to reduce the dimension.

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, the gait phase model is used to divide the gait of the humanoid robot in the current frame image into translational movement and rotational movement, and the translational movement and rotational movement are clustered into the left / right foot single support stage and double support stage respectively, including: According to the IMU data and joint encoder data of the center of mass of the humanoid robot, 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 in 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; The vertical acceleration of the feet is obtained according to the IMU data of the humanoid robot's feet; the ground reaction force and torque are obtained according to the force-sensitive resistor data; based on the vertical acceleration of the feet and the ground reaction force and torque, the Gaussian mixture model is used to cluster the translational motion and the rotational motion into the left / right foot single support stage and the double support stage respectively.

5. The 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, the 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 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; For each query descriptor , get the matching descriptor with the smallest Hamming distance As the matching result of the current query descriptor; Estimate the camera pose of the current frame image based on the matching results.

6. A method for constructing a dense point cloud map of a humanoid robot according to claim 5, characterized in that: Get and query descriptors The matching descriptor with the smallest Hamming distance As the matching result of the current query descriptor, calculate the matching descriptor The Hamming distance to all query descriptors; if the current query descriptor Matches this descriptor When the Hamming distance of is also the smallest, the matching result is retained, otherwise the matching result is discarded.

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 descriptor 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. The method for constructing a dense point cloud map of a humanoid robot according to claim 1, characterized in that: Based on the camera pose of the current frame image, whether the current frame image is a key frame is determined, 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; the first condition is that the frame difference between the current frame image and the previous key frame is greater than the preset maximum frame number; the second condition is that the frame difference between the current frame image and the previous key frame is greater than the preset minimum frame number, 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 in the local mapping thread does not exceed three.

9. A method for constructing a dense point cloud map of a humanoid robot according to claim 8, characterized in that: Before judging whether the current frame image is a key frame based on the camera pose of the current frame image, the following steps are also included: The translation matrix and rotation matrix between the current frame image and the previous frame image are solved by constructing a least squares problem. The formula is: ； in, 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, represents the rotation matrix, represents the translation matrix; Get 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, it is determined whether the current frame image is a key frame based on the camera posture of the current frame image.

10. A humanoid robot dense point cloud map construction system, characterized in that: include: A video acquisition module is used to obtain video images collected when the humanoid robot walks in real time; The gait judgment module is used to judge the gait phase of the humanoid robot in the current frame image in the video image, including: obtaining sensor information when the humanoid robot walks; according to the sensor information, using the gait phase model, the gait of the humanoid robot in the current frame image is divided into translational movement and rotational movement, and the translational movement and rotational movement are clustered into the left / right foot single support stage and double support stage respectively; 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, then the camera pose of the current frame image is estimated by using a constant speed motion model; if the gait phase is the left / right foot single support stage of rotational motion, then the camera pose of the current frame image is estimated by using a brute force matching model; if the gait phase is the double support stage of translational motion or the double support stage of rotational motion, then the gait phase of the humanoid robot in the next frame image in the video image is determined; A key frame judgment module is used to judge whether the current frame image is a key frame based on the camera posture of the current frame image; The map construction module is used to perform local mapping and loop detection based on key frames to obtain a global dense point cloud map.

Images