Humanoid robot method, device, equipment and medium based on visual servoing for pouring water

Abstract

The application discloses a humanoid robot pouring method and device based on visual servo, and relates to the technical field of industrial vision intelligence. The method comprises the following steps: acquiring first posture information of a target object in a working environment; determining a pouring target position above a water cup based on a preset height and a water cup position; iteratively adjusting the end posture of the mechanical arm of the humanoid robot to reduce the position deviation between the spout position and the pouring target position by using a first visual servo control algorithm; when the position deviation meets a first preset condition, controlling the mechanical arm to perform a pouring action; during the execution of the pouring action, acquiring a real-time updated spout position, and compensating and adjusting the end posture of the mechanical arm based on the real-time deviation between the real-time updated spout position and the pouring target position by using a second visual servo control algorithm. The application improves the accurate alignment and real-time control of the robot pouring action, and has stronger stability and adaptability.

Description

Technical Field

[0001] This application relates to the field of industrial vision and intelligence technology, and in particular to a method, apparatus, equipment and medium for pouring water using a humanoid robot based on visual servoing. Background Technology

[0002] With the continuous development of robotics technology, humanoid robots are increasingly widely used in scenarios such as home service and catering assistance. Among these applications, completing tasks requiring fine motor skills, such as pouring water, has become a significant indicator of a robot's intelligence level. While seemingly simple, pouring water actually demands that the robot possess precise environmental perception, stable object grasping, accurate motion planning, and real-time control over the execution process. This task involves the deep integration of multiple technical fields, including robot visual measurement and perception, motion planning and control, and represents a current hot topic and challenging issue in robotic intelligence research.

[0003] The key to successfully pouring water lies in keeping the spout aligned with the center of the cup. However, the kinematic geometry from the dexterous hand to the spout is unknown, and this relationship dynamically changes during the grasping and pouring process due to the rotation of the handle and the shaking of the kettle. Existing VLA (Vision-Language-Action) methods struggle to accurately model and compensate for this uncertainty online, easily leading to alignment deviations and thus a higher risk of water spillage.

[0004] Therefore, improving the precision and real-time control of humanoid robots' water-pouring actions in dynamic environments is a problem that urgently needs to be solved. Summary of the Invention

[0005] The main purpose of this application is to provide a method, apparatus, device and medium for humanoid robots to pour water based on visual servoing, which aims to solve the technical problem of how to improve the accuracy and real-time control of humanoid robots' water-pouring actions in dynamic environments.

[0006] To achieve the above objectives, this application proposes a method for pouring water for a humanoid robot based on visual servoing. The method is applied to a humanoid robot and includes:

[0007] Acquire the first pose information of a target object in the working environment. The first pose information of the target object includes at least the position of the spout of the kettle in the robot coordinate system and the position of the cup in the robot coordinate system.

[0008] Based on the preset height and the position of the water cup, determine the target position for pouring water above the water cup;

[0009] Based on the positional deviation between the spout position and the water pouring target position, the pose of the end effector of the humanoid robot arm is iteratively adjusted using a first visual servo control algorithm to reduce the positional deviation.

[0010] When the positional deviation meets the first preset condition, the robotic arm is controlled to perform a tilting action;

[0011] During the pouring action, the updated spout position is acquired in real time, and the pose of the robotic arm end effector is compensated and adjusted based on the real-time deviation between the updated spout position and the target pouring position using a second visual servo control algorithm.

[0012] In one embodiment, the first pose information of the target object further includes the handle position of the kettle handle in the robot coordinate system, and the step of obtaining the first pose information of the target object in the working environment includes:

[0013] Acquire RGB and depth images;

[0014] The RGB image is segmented to obtain a target region segmentation mask, which includes a segmentation mask for the kettle spout region, a segmentation mask for the water cup region, and a segmentation mask for the kettle handle.

[0015] The surface point cloud of the target object is reconstructed based on the depth image to obtain the target surface point cloud matrix, which includes a first surface point cloud matrix, a second surface point cloud matrix, and a third surface point cloud matrix.

[0016] Based on the segmentation mask of the kettle spout region and the first surface point cloud matrix, the three-dimensional coordinates of the kettle spout in the camera coordinate system are calculated, and the position of the kettle spout in the robot coordinate system is obtained by coordinate transformation.

[0017] Based on the segmentation mask of the water cup region and the second surface point cloud matrix, the 6D pose of the water cup in the robot coordinate system is estimated, and the position of the water cup in the robot coordinate system is extracted from the 6D pose of the water cup in the robot coordinate system.

[0018] Based on the segmentation mask of the kettle handle and the third surface point cloud matrix, the 6D pose of the kettle handle in the robot coordinate system is estimated, and the handle position in the robot coordinate system is extracted from the 6D pose of the kettle handle in the robot coordinate system.

[0019] In one embodiment, before the step of iteratively adjusting the end-effector pose of the humanoid robot to reduce the positional deviation based on the positional deviation between the spout position and the target pouring position using a first visual servo control algorithm, the method further includes:

[0020] Obtain the handle position of the kettle handle in the robot coordinate system and the 6D pose of the kettle handle in the robot coordinate system;

[0021] Based on the handle position of the kettle in the robot coordinate system and the 6D posture of the kettle handle in the robot coordinate system, the grasping pose is planned, and the robotic arm is controlled to move to the handle position to perform the grasping action.

[0022] In one embodiment, the step of calculating the three-dimensional coordinates of the kettle spout in the camera coordinate system based on the segmentation mask of the kettle spout region and the first surface point cloud matrix, and obtaining the spout position in the robot coordinate system through coordinate transformation, includes:

[0023] Extract the pixel coordinate set from the segmentation mask of the kettle spout region;

[0024] The corresponding depth value set is obtained from the first surface point cloud matrix based on the pixel coordinate set;

[0025] The depth value set is sorted and truncated by filtering to obtain the denoised representative depth values;

[0026] Based on the statistical center value of the pixel coordinate set, camera intrinsic parameters, and the representative depth value, the three-dimensional coordinates of the kettle spout in the camera coordinate system are calculated.

[0027] By using a pre-calibrated transformation matrix from camera to robot coordinate system, the three-dimensional coordinates of the kettle spout in the camera coordinate system are converted into the spout position in the robot coordinate system.

[0028] In one embodiment, the step of iteratively adjusting the end-effector pose of the humanoid robot to reduce the positional deviation based on the positional deviation between the spout position and the target pouring position using a first visual servo control algorithm includes:

[0029] Obtain the spout position and the target water pouring position at the current iteration time;

[0030] Calculate the current position deviation based on the spout position and the target water pouring position at the current iteration time;

[0031] Based on the current position deviation, calculate the expected position of the robotic arm's end effector at the next iteration time;

[0032] The first constraint is that the end-effector of the robotic arm remains unchanged, and the second constraint is that the end-effector of the robotic arm moves to the desired position. The corresponding target joint angles of the robotic arm are then solved.

[0033] Control the robotic arm to move to the pose corresponding to the target joint angle, determine whether the current position deviation meets the first preset condition, if not, return to the step of obtaining the spout position and the water pouring target position at the current iteration time, until the current position deviation meets the first preset condition.

[0034] In one embodiment, the step of solving for the corresponding target joint angle of the robotic arm, with the first constraint being that the end-effector posture remains unchanged and the second constraint being that the end-effector position moves to the desired position, includes:

[0035] The first optimization objective is to keep the end-effector posture unchanged, and the second optimization objective is to move the end-effector position to the desired position. An inverse motion solution objective function is constructed. The objective function includes a forward kinematics operator for the robotic arm and a diagonal weight matrix. The diagonal weight matrix is ​​used to assign different weights to the deviations of different dimensions of the end-effector posture rotation matrix.

[0036] The objective function is solved by optimization algorithm to obtain the target joint angle, which corresponds to the joint angle corresponding to the minimum value of the objective function.

[0037] In one embodiment, the step of acquiring the updated spout position in real time during the pouring action, and adjusting the pose of the robotic arm end effector using a second visual servo control algorithm based on the real-time deviation between the updated spout position and the target pouring position, includes:

[0038] The tilting action is planned as a step-by-step rotation around a preset axis. After each preset angle of rotation, the continuous execution of the tilting action is paused, and a compensation step is performed.

[0039] The compensation steps include:

[0040] Obtain the current updated spout position and calculate the deviation between the current updated spout position and the target pouring position;

[0041] Determine whether the deviation between the current updated spout position and the target pouring position exceeds the allowable tolerance range;

[0042] If the deviation between the current updated spout position and the target water pouring position exceeds the allowable tolerance range, the first visual servo control algorithm is triggered to iteratively adjust the end-effector pose of the humanoid robot until the deviation between the current updated spout position and the target water pouring position is within the allowable tolerance range.

[0043] After the compensation step, the next step rotation is performed.

[0044] In addition, to achieve the above objectives, this application also proposes a visual servoing-based humanoid robot water-pouring system, which includes: a water cup, a kettle, a storage platform, and a humanoid robot.

[0045] The water cup and the water bottle are placed on the storage platform;

[0046] The humanoid robot stands on one side of the placement platform to perform the visual servo-based humanoid robot water-pouring method described above.

[0047] In one embodiment, the humanoid robot includes a robot head camera, a dexterous hand, a robotic arm, and a control unit, and the water bottle is provided with a water bottle handle and a water bottle spout;

[0048] The robot head camera is used to acquire the first pose information of the target object in the working environment. The first pose information of the target object includes at least the position of the spout of the kettle in the robot coordinate system and the position of the cup in the robot coordinate system.

[0049] The control unit is used to control the dexterous hand to grasp the handle of the kettle;

[0050] The control unit is also used to control the robotic arm to perform a water-pouring action after the grasping action is completed. The water-pouring action includes aligning the spout of the kettle with the target position for water pouring.

[0051] In one embodiment, the control unit is further configured to, after the grasping action is completed, control the robotic arm to perform the water-pouring action, and then control the robotic arm and the dexterous hand to return to the default zero position in sequence.

[0052] One or more technical solutions proposed in this application have at least the following technical effects:

[0053] The system acquires the first pose information of the target object in the working environment. This first pose information includes at least the position of the kettle spout and the position of the cup in the robot coordinate system. Based on a preset height and the cup position, a pouring target position is determined above the cup. Based on the positional deviation between the spout position and the pouring target position, a first visual servo control algorithm iteratively adjusts the end effector pose of the humanoid robot's arm to reduce the positional deviation. When the positional deviation meets a first preset condition, the robot arm is controlled to perform a pouring action. This system employs visual servo-based pose estimation and motion control technology, solving the problems of inaccurate alignment and liquid spillage caused by the uncertain kinematic relationship between the spout and the end effector in traditional robot water pouring methods. By acquiring the six-degree-of-freedom pose of the target object in real time and calculating the pouring target position, precise spout positioning is achieved. The position-based visual servo algorithm iteratively adjusts the robot arm, overcoming geometric changes during motion and ensuring the stability and reliability of the alignment.

[0054] In this embodiment, during the pouring action, the updated spout position is acquired in real time. Based on the real-time deviation between the updated spout position and the target pouring position, the pose of the robotic arm's end effector is compensated and adjusted using a second visual servo control algorithm. The iterative adjustment of the robotic arm using a position-based visual servo algorithm overcomes geometric changes during movement, ensuring the stability and reliability of alignment. A displacement compensation mechanism is introduced into the pouring action, dynamically correcting the spout position through visual feedback, effectively preventing water flow deviation. This method reduces reliance on external data, improves the robot system's generalization ability and operational accuracy, and exhibits stronger stability and adaptability. Attached Figure Description

[0055] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0056] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0057] Figure 1 This is a flowchart illustrating the first embodiment of the visual servoing-based humanoid robot water-pouring method of this application;

[0058] Figure 2 This is a schematic diagram of the first visual servo control algorithm according to an embodiment of this application;

[0059] Figure 3This is a schematic diagram of the second visual servo control algorithm according to an embodiment of this application;

[0060] Figure 4 This is a schematic diagram of the first structure of the humanoid robot water-pouring system based on visual servoing in this application;

[0061] Figure 5 This is a schematic diagram of the second structure of the humanoid robot water-pouring system based on visual servoing in this application.

[0062] The purpose, features, and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0063] The technical solutions in the embodiments of this application will be clearly and completely described below with reference to the embodiments of this application. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0064] This application provides a visual servoing-based method for pouring water into a humanoid robot. Specifically, refer to... Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the visual servoing-based humanoid robot water-pouring method of this application. In this embodiment, the visual servoing-based humanoid robot water-pouring method includes steps S10-S50:

[0065] Step S10: Obtain the first pose information of the target object in the working environment. The first pose information of the target object includes at least the position of the spout of the kettle in the robot coordinate system and the position of the cup in the robot coordinate system.

[0066] It should be noted that the first pose information of the target object refers to the six-degree-of-freedom (6DOF) pose data of the kettle and cup acquired by the robot's head camera. This 6DOF pose data can include position and orientation. The spout position represents the three-dimensional coordinates of the kettle spout in the robot coordinate system, and the cup position represents the three-dimensional coordinates of the cup in the robot coordinate system. For example, the first pose information of the target object can be obtained using a 6D cube bag pose estimation algorithm.

[0067] Step S20: Based on the preset height and the position of the water cup, determine the target position for pouring water above the water cup.

[0068] It should be noted that the target position for pouring water refers to the target point calculated at a preset height above the center of the water cup. This is used to guide the spout to be aligned and ensure that the water flows accurately into the water cup when pouring.

[0069] For example, suppose the 6D pose of the cup in the robot coordinate system has been obtained using the cube bag 6D pose estimation algorithm. Then the 3D coordinates of points on the surface of the cup It can be represented as:

[0070]

[0071] Set the distance of the spout above the cup Start pouring water from the top of the cup, then the point where the water is poured should be... for:

[0072]

[0073] in, Indicates the preset height. Indicates the position of the water cup. Indicates the target location for pouring water.

[0074] Step S30: Based on the positional deviation between the spout position and the target water pouring position, the pose of the end effector of the humanoid robot arm is iteratively adjusted using the first visual servo control algorithm to reduce the positional deviation.

[0075] It should be noted that the first visual servo control algorithm is a position-based visual servoing method. It can dynamically adjust the end pose of the robotic arm by iteratively comparing the deviation between the spout position and the target water pouring position to achieve precise alignment.

[0076] Step S40: When the position deviation meets the first preset condition, control the robotic arm to perform the tilting action.

[0077] For example, the first preset condition refers to the state where the position deviation is less than a set threshold, indicating that the spout is close enough to the target position for pouring water and the pouring action can be safely started.

[0078] In step S50, during the pouring action, the updated spout position is acquired in real time, and the pose of the robotic arm end effector is compensated and adjusted based on the real-time deviation between the updated spout position and the target pouring position using a second visual servo control algorithm.

[0079] It should be noted that the second visual servo control algorithm is a displacement compensation method based on visual feedback. During the pouring process, it monitors the spout position changes in real time and adjusts the robotic arm posture through inverse kinematics to compensate for deviations caused by rotation or vibration. Real-time deviation refers to the dynamic error between the current position of the spout and the target position during pouring, used to trigger the compensation mechanism. Here, to distinguish between the first and second visual servo control algorithms, the concepts of position deviation and real-time deviation are introduced. In the actual working environment, both position deviation and real-time deviation can be understood as the deviation between the current spout position and the target position.

[0080] In this embodiment, a vision-servoing-based attitude estimation and motion control technique is employed to solve the problems of inaccurate alignment and liquid spillage caused by the uncertain kinematic relationship between the spout and the end effector of the robotic arm in traditional robot water-pouring methods. By acquiring the six-degree-of-freedom attitude of the target object in real time and calculating the target position for water pouring, precise positioning of the spout is achieved. The robotic arm is iteratively adjusted using a position-based vision-servoing algorithm to overcome geometric changes during movement, ensuring the stability and reliability of alignment. A displacement compensation mechanism is introduced into the water-pouring action, dynamically correcting the spout position through visual feedback, effectively preventing water flow deviation. The method in this embodiment reduces dependence on external data, improves the generalization ability and operational accuracy of the robot system, and possesses stronger stability and adaptability.

[0081] In one implementation, the first pose information of the target object also includes the handle position of the kettle handle in the robot coordinate system. Therefore, step S10 includes:

[0082] Step S101: Obtain the RGB image and the depth image.

[0083] It should be noted that RGB images refer to color images captured by the robot's head camera, containing red, green, and blue channel information, used to identify the color and texture features of target objects. Depth images refer to depth information images acquired synchronously with RGB images, where each pixel value represents the distance from the corresponding point to the camera, used for 3D spatial reconstruction.

[0084] Step S102: Perform instance segmentation on the RGB image to obtain the target region segmentation mask. The target region segmentation mask includes the segmentation mask of the kettle spout region, the segmentation mask of the water cup region, and the segmentation mask of the kettle handle.

[0085] It should be noted that a pre-trained instance segmentation model can perform pixel-level segmentation of RGB images, outputting a target region segmentation mask for each object. This mask is a binary image mask, where white pixels represent the target object region and black pixels represent the background. It can be divided into independent region masks for the kettle spout, cup, and handle. Images of the kettle and cup can be acquired under different angles and lighting conditions. Annotation software is used to segment and annotate the handle, spout, and cup. Finally, the training data is exported and input into the YOLOv8n_seg model for training, resulting in a trained instance segmentation model. This trained instance segmentation model can then be embedded into a humanoid robot. After acquiring images, the robot can directly use the trained instance segmentation model to perform instance segmentation on RGB images.

[0086] Step S103: Reconstruct the surface point cloud of the target object based on the depth image to obtain the target surface point cloud matrix, which includes a first surface point cloud matrix, a second surface point cloud matrix, and a third surface point cloud matrix.

[0087] It should be noted that the surface point cloud of the target object refers to the set of three-dimensional points reconstructed from the depth image and camera intrinsic parameters, representing the geometry of the target object's surface. The reconstruction process involves extracting pixel coordinates from the depth image and segmentation mask, and mapping them to the camera coordinate system to generate a three-dimensional point cloud. The first surface point cloud matrix corresponds to the point cloud matrix of the kettle spout. The second surface point cloud matrix corresponds to the point cloud matrix of the water cup. The third surface point cloud matrix corresponds to the point cloud matrix of the kettle handle.

[0088] For example, the reconstruction process of the point cloud S of the target object surface is as follows:

[0089] Assuming the binary mask of the target object is known, we can obtain the set vectors U and V of the target object's pixel coordinates. Using Python, U and V can be obtained by calling the `numpy.where()` function.

[0090]

[0091] Let the length of vector U be N, then the corresponding pixel point The depth of the location is ,in Representing vectors index ( Based on the mapping relationship between the camera coordinate system and the image coordinate system, the point cloud of the target object's surface can be calculated using the following formula:

[0092]

[0093] in, This represents element-wise vector dot product. , , and This represents the camera intrinsic parameters. If vectors X, Y, and Z are set as N-row, column vectors, then a target surface point cloud matrix with dimensions N rows and 3 columns can be constructed. .

[0094] Step S104: Based on the segmentation mask of the kettle spout region and the first surface point cloud matrix, calculate the three-dimensional coordinates of the kettle spout in the camera coordinate system, and obtain the spout position in the robot coordinate system through coordinate transformation.

[0095] It should be noted that the 3D coordinates in the camera coordinate system refer to the position of the point cloud in the camera's local coordinate system, calculated using depth values ​​and camera intrinsic parameters. Coordinate transformation refers to the homogeneous transformation matrix from the camera coordinate system to the robot coordinate system, used to transform the point cloud from the camera coordinate system to the robot coordinate system.

[0096] Step S105: Based on the segmentation mask of the water cup region and the second surface point cloud matrix, estimate the 6D pose of the water cup in the robot coordinate system, and extract the position of the water cup in the robot coordinate system from the 6D pose of the water cup in the robot coordinate system.

[0097] The 6D pose of the water cup in the robot coordinate system can be represented as follows: SE(3) is an abbreviation for Special Euclidean Group, which represents the set of all rigid body motions in three-dimensional space, where rigid body motions can include rotation and translation. 6D pose is a six-degree-of-freedom pose, including three-dimensional position and three-dimensional orientation. For example, the three-dimensional translation vector extracted from the 6D pose of the water cup represents the position of the center point of the water cup in the robot coordinate system.

[0098] Step S106: Based on the segmentation mask of the kettle handle and the third surface point cloud matrix, estimate the 6D pose of the kettle handle in the robot coordinate system, and extract the handle position of the kettle handle in the robot coordinate system from the 6D pose of the kettle handle in the robot coordinate system.

[0099] For example, the handle position and orientation information obtained by point cloud fitting can be used for robotic arm grasping planning.

[0100] In this implementation, the notification directly uses a pre-trained instance segmentation model, avoiding the additional computational resource consumption of the humanoid robot. Instance segmentation is achieved by simultaneously acquiring RGB and depth images, enabling efficient segmentation of the kettle spout, cup, and handle. Point cloud reconstruction and coordinate transformation overcome the limitations of lacking depth information in two-dimensional images, accurately calculating the position and pose of the target object in three-dimensional space. Based on 6D pose estimation, the influence of uncertainties in kinematic geometry is eliminated, providing stable input for subsequent visual servo control and laying a reliable foundation for the precise execution of the water-pouring action.

[0101] It is understandable that before step S30, the process also includes: obtaining the handle position of the kettle handle in the robot coordinate system and the 6D pose of the kettle handle in the robot coordinate system; planning the grasping pose based on the handle position of the kettle handle in the robot coordinate system and the 6D pose of the kettle handle in the robot coordinate system, and controlling the robotic arm to move to the handle position before performing the grasping action.

[0102] It should be noted that the grasping pose refers to the target pose of the dexterous hand when grasping the kettle handle, including the end effector position and orientation. This pose can be calculated based on the 6D pose of the handle and a preset grasping strategy, ensuring the stability and alignment accuracy of the grasping action. For example, an RGB image (img) can be input into a trained instance segmentation model to infer the kettle handle mask (mask_handle), and the 3D surface point cloud S of the target object can be reconstructed based on the depth image (depth). Finally, the 6D pose of the kettle handle is calculated using a 6D pose estimation algorithm based on cubic bag fitting. Furthermore, inverse kinematics is used to drive the joint angle changes of the robotic arm and dexterous hand. For example, the dexterous hand can be moved to the handle position by the joint movement of the robotic arm, and then the joint angle changes of the dexterous hand can be controlled to achieve grasping. This embodiment achieves automated and high-precision control of the grasping action by obtaining the precise position and orientation of the handle in real time and combining it with inverse kinematics to plan the grasping posture.

[0103] In one embodiment, step S104 includes: extracting a set of pixel coordinates from the segmentation mask of the kettle spout region; obtaining a corresponding set of depth values ​​from the first surface point cloud matrix based on the set of pixel coordinates; sorting and truncating the set of depth values ​​to obtain a representative depth value after denoising; calculating the three-dimensional coordinates of the kettle spout in the camera coordinate system based on the statistical center value of the set of pixel coordinates, camera intrinsic parameters, and the representative depth value; and converting the three-dimensional coordinates of the kettle spout in the camera coordinate system into the spout position in the robot coordinate system through a pre-calibrated transformation matrix from camera to robot coordinate system.

[0104] For example, assuming the spout binary mask is mask_s, the spout pixel coordinate point set vectors U_s and V_s are obtained through formula (3). Since the spout segmentation mask and depth may contain noise, denoising processing is required. For example, for the depth vector... Perform sorting in ascending or descending order to obtain the initial sorting result, let Then remove the first 25% and the last 25% to get Finally, take the average of the remaining points, that is... Similarly, the average of the pixel coordinates is used to obtain the spout row coordinates. The column coordinates of the spout are vs= .mean(V_s). The spout coordinates in the camera coordinate system are obtained according to formulas (4) and (5):

[0105]

[0106] The coordinates of the spout in the robot coordinate system are:

[0107]

[0108] in, This is represented as a rotation and translation transformation from the camera coordinate system to the robot coordinate system. Indicates the position of the spout. This represents the spout coordinates in the camera coordinate system.

[0109] In this embodiment, instance segmentation and pixel coordinate extraction techniques are employed to solve the problems of inaccurate target region positioning and noise sensitivity in traditional methods, achieving precise isolation of the spout region. By sorting and truncating the depth value set, interference from outliers and noise in the depth image is overcome, improving the reliability of the depth data. The calculation of the statistical center value and camera intrinsic parameters simplifies the 3D coordinate solution process and reduces random errors. By using a pre-calibrated coordinate transformation matrix, deviations between multiple coordinate systems are eliminated, ensuring the uniformity and accuracy of the spout position in the robot coordinate system, laying a stable foundation for subsequent visual servo control, thereby significantly improving the accuracy and adaptability of the water-pouring action.

[0110] In one implementation, step S30 includes:

[0111] Step S301: Obtain the spout position and the target position for pouring water at the current iteration time.

[0112] For example, suppose the robot arm has 6 degrees of freedom, and the angle of the i-th joint is... (1≤i≤6), then let the joint angle vector be... .make Represents the joint angle vector of the robotic arm The final pose can be a 4x4 homogeneous transformation matrix, consisting of a rotation matrix and a translation vector, where... This represents the forward kinematics operator for the robotic arm. Let at some moment... The position of the spout is The end effector posture of the robotic arm is End position = The point where water is poured from the top of the cup. .

[0113] Step S302: Calculate the current position deviation based on the spout position and the target water pouring position at the current iteration time.

[0114] For example, the current position deviation can be expressed as: .

[0115] Step S303: Calculate the expected position of the robotic arm end effector at the next iteration time based on the current position deviation.

[0116] For example, the formula for calculating the desired position of the robotic arm's end effector at the next iteration is as follows:

[0117]

[0118] in, This represents the iteration step size. It can be proven that when... At that time, if the spout position is sensed If formula (8) has no fluctuation error, the iterative formula (2) will converge to the point where the spout position coincides with the water pouring position above the cup. Considering the trade-off between convergence speed and perceived noise, we can... Set to 0.2.

[0119] Step S304: Using the first constraint that the end-effector posture remains unchanged and the second constraint that the end-effector position moves to the desired position, solve for the corresponding target joint angle of the robotic arm.

[0120] Step S304 includes: taking the robotic arm end-effector posture remaining unchanged as the first optimization objective and taking the end-effector position moving to the desired position as the second optimization objective; constructing an inverse kinematics objective function, which includes a robotic arm forward kinematics operator and a diagonal weight matrix, the diagonal weight matrix being used to assign different weights to the deviations of different dimensions of the end-effector posture rotation matrix; solving the objective function through an optimization algorithm to obtain the target joint angle, the target joint angle corresponding to the joint angle corresponding to the minimum value of the objective function.

[0121] For example, according to End-of-time attitude We expect Maintain the grasping posture at all times, and move the end position to Therefore, the objective function for solving the inverse motion is:

[0122]

[0123] in, This represents the forward kinematics operator of the robotic arm. Let represent the diagonal weight matrix, and represent the end-effector attitude fitting weight vector; thus, we can obtain The gradient vectors for the end effector's attitude and position are:

[0124]

[0125] Based on the chain rule and the inverse kinematics of robotic arms You can get For joint angle The gradient vector is:

[0126]

[0127] It should be noted that the optimization algorithm can be a Sequential Least Squares Quadratic Programming (SLSQP) algorithm. Therefore, the objective function can be solved using SLSQP based on the gradient vector. Joint vector corresponding to the minimum value .

[0128] Step S305: Control the robotic arm to move to the pose corresponding to the target joint angle, and determine whether the current position deviation meets the first preset condition. If not, return to step S301 until the current position deviation meets the first preset condition.

[0129] For example, such as Figure 2 As shown, assume the initial end-effector attitude is Water is poured when the distance from the spout to the target position is less than d. The entire spout alignment algorithm process is as follows:

[0130] (1) Let t=0, and use formula (6) to calculate the spout position. Formula (8) is used to calculate the water pouring position. ;

[0131] (2) while ;

[0132] (3) Calculation Using formula (9) and SLSQP to solve the loss formula (10), the new joint vector is obtained. ;

[0133] (4) Move to the new joint angle position using the moveit library in ros2. ;

[0134] (5) t=t+1, calculate the position of the spout. Calculate the water pouring position ;

[0135] (6) end while.

[0136] In this embodiment, position-based visual servo control iteratively acquires the positions of the spout and the target and calculates the deviation, thus solving the problem of low alignment accuracy caused by the uncertainty of the kinematic relationship between the spout and the end effector of the robotic arm. By using deviation-driven desired position calculation and inverse kinematics solution, the robotic arm achieves automated and precise adjustment, avoiding the complexity of traditional path planning. By constructing an objective function and weight matrix, position and posture constraints are balanced, ensuring the stability and alignment reliability of the grasping process during water pouring. Combined with optimization algorithms and a loop judgment mechanism, the convergence and robustness of the system are improved.

[0137] In one implementation, step S50 includes:

[0138] Step S501: The tilting action is planned as a step-by-step rotation around a preset axis. After each preset angle of rotation, the continuous execution of the tilting action is paused, and a compensation step is performed.

[0139] Specifically, the compensation steps include: obtaining the current updated spout position and calculating the deviation between the current updated spout position and the target water pouring position; determining whether the deviation between the current updated spout position and the target water pouring position exceeds the allowable tolerance range; if the deviation between the current updated spout position and the target water pouring position exceeds the allowable tolerance range, triggering the first visual servo control algorithm to iteratively adjust the end-effector pose of the humanoid robot until the deviation between the current updated spout position and the target water pouring position is within the allowable tolerance range.

[0140] It should be noted that the allowable tolerance range refers to a preset deviation threshold, which is used to determine whether the spout position deviates too much from the target pouring position; if the deviation exceeds this range, a compensation mechanism is triggered.

[0141] For example, water begins to pour when the spout moves to the target position above the cup. The total rotation angle is set. The rotation angle is 100° each time. Therefore, the total number of rotations is Assume the terminal attitude at time t is... So first rotate around the arm. The angle, i.e., the attitude at time t+1, is:

[0142]

[0143] Similarly, the target loss function can be constructed as follows:

[0144]

[0145] in, A weight vector is fitted to the end effector attitude. Then, the objective function is solved using SLSQP. The new joint vector is obtained. The system then moves to the new joint vector position using the `moveit` function. After the movement is complete, the spout position can be detected. If the current spout position exceeds the target positioning range, displacement compensation is performed using a position-based visual servoing algorithm. For example, the compensation algorithm flow is as follows: Figure 3 As shown, the specific steps are as follows:

[0146] (1) Set the total rotation angle The rotation angle is 100° each time. Calculate the number of rotations Obtain the initial pose ;

[0147] (2) For t in range(r):

[0148] (a) Update the end pose according to formula (10) Solve for the joint vector that minimizes (11). And use moveit to move to the joint vector position. ;

[0149] (b) Initiate the position-based visual servo spout movement alignment algorithm;

[0150] (c) Get and update the current end position ;

[0151] (4) end for.

[0152] Step S502: After the compensation step, perform the next step rotation action.

[0153] It is understandable that after the compensation step, the tilting action can continue. For example, after rotating around a preset axis by a preset angle, the tilting action is paused again, and the above compensation step is performed.

[0154] In this embodiment, a step-by-step rotation and real-time compensation mechanism is employed to solve the problem of water spillage caused by changes in spout displacement during pouring. By decomposing the action into preset axes and angles, the instability of continuous rotation is avoided, improving the precision of control. Visual feedback and deviation judgment in the compensation step are used to achieve dynamic correction of positional offset. Iterative servo control reduces dependence on external models, enhances the system's adaptability and reliability, and ensures high precision and robustness of the pouring action.

[0155] Based on the same inventive concept, such as Figure 4 As shown, this application also provides a visual servoing-based humanoid robot water-pouring system, which includes: a water cup 10, a water jug ​​20, a storage platform 30, and a humanoid robot 40. The water cup 10 and water jug ​​20 are placed on the storage platform 30; the humanoid robot 40 stands on one side of the storage platform 30 to perform the aforementioned visual servoing-based humanoid robot water-pouring method.

[0156] Specifically, such as Figure 5 As shown, the humanoid robot 40 includes a robot head camera 401, a dexterous hand 402, a robotic arm 403, and a control unit 404. The kettle 20 is equipped with a kettle handle 201 and a kettle spout 202. The robot head camera 401 is used to acquire the first posture information of the target object in the working environment. The first posture information of the target object includes at least the position of the kettle spout 202 in the robot coordinate system and the position of the water cup 10 in the robot coordinate system. The control unit 404 is used to control the dexterous hand 402 to grasp the kettle handle 201. After the grasping action is completed, the control unit 404 is also used to control the robotic arm 403 to perform a water pouring action, which includes aligning the kettle spout 202 with the target position.

[0157] The control unit 404 is also used to control the robotic arm 403 and the dexterous hand 402 to return to the default zero position in sequence after the robotic arm 403 performs the water pouring action.

[0158] It should be noted that the humanoid robot water-pouring system based on visual servoing provided in this application embodiment and the humanoid robot water-pouring method based on visual servoing provided in this application embodiment are based on the same application concept. Therefore, the specific implementation of this embodiment can refer to the implementation of the aforementioned humanoid robot water-pouring method based on visual servoing, and the repeated parts will not be described again.

[0159] In some embodiments, the control unit 404 includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the above-described visual servoing-based humanoid robot water-pouring method.

[0160] Specifically, the processor may include, for example, a general-purpose microprocessor, an instruction set processor and / or an associated chipset and / or a special-purpose microprocessor (e.g., an application-specific integrated circuit (ASIC)), etc. The processor may also include onboard memory for caching purposes. The processor may be a single processing unit or multiple processing units for performing different actions of the method flow according to embodiments of this application.

[0161] Memory can be any medium capable of containing, storing, transmitting, propagating, or transmitting instructions. For example, memory can include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, instruments, or propagation media. Specific examples of memory include: magnetic storage devices such as magnetic tape or hard disk drives (HDDs); optical storage devices such as optical discs (CD-ROMs); and also random access memory (RAM) or flash memory; and / or wired / wireless communication links.

[0162] Those skilled in the art will understand that the features described in the various embodiments of this application can be combined and / or combined in various ways, even if such combinations or combinations are not explicitly described in this application. In particular, the features described in the various embodiments of this application can be combined and / or combined in various ways without departing from the spirit and teachings of this application. All such combinations and / or combinations fall within the scope of this application. Therefore, the scope of this application should not be limited to the above embodiments. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A method for pouring water using a humanoid robot based on visual servoing, characterized in that, The visual servoing-based humanoid robot water-pouring method is applied to a humanoid robot, and the method includes: Acquire the first pose information of a target object in the working environment. The first pose information of the target object includes at least the position of the spout of the kettle in the robot coordinate system and the position of the cup in the robot coordinate system. Based on the preset height and the position of the water cup, determine the target position for pouring water above the water cup; Based on the positional deviation between the spout position and the target water-pouring position, a first visual servo control algorithm is used to iteratively adjust the end-effector pose of the humanoid robot to reduce the positional deviation. This includes: acquiring the spout position and the target water-pouring position at the current iteration time; calculating the current positional deviation based on the spout position and the target water-pouring position at the current iteration time; calculating the desired position of the end-effector at the next iteration time based on the current positional deviation; using the end-effector pose remaining unchanged as a first constraint and the end-effector moving to the desired position as a second constraint, solving for the corresponding target joint angle of the end-effector; controlling the end-effector to move to the pose corresponding to the target joint angle; determining whether the current positional deviation meets a first preset condition; if not, returning to the step of acquiring the spout position and the target water-pouring position at the current iteration time, until the current positional deviation meets the first preset condition. When the positional deviation meets the first preset condition, the robotic arm is controlled to perform a tilting action; During the pouring action, the updated spout position is acquired in real time, and based on the real-time deviation between the updated spout position and the target pouring position, the pose of the robotic arm end effector is compensated and adjusted through a second visual servo control algorithm. This includes: planning the pouring action as a step-by-step rotation around a preset axis, pausing the continuous execution of the pouring action after each preset angle rotation, and performing a compensation step. The compensation step includes: obtaining the current updated spout position and calculating the deviation between the current updated spout position and the target pouring position; determining whether the deviation between the current updated spout position and the target pouring position exceeds the allowable tolerance range; if the deviation between the current updated spout position and the target pouring position exceeds the allowable tolerance range, triggering the first visual servo control algorithm to iteratively adjust the end-effector pose of the humanoid robot until the deviation between the current updated spout position and the target pouring position is within the allowable tolerance range; after the compensation step, executing the next step rotation action.

2. The method as described in claim 1, characterized in that, The first pose information of the target object also includes the handle position of the kettle handle in the robot coordinate system. The step of obtaining the first pose information of the target object in the working environment includes: Acquire RGB and depth images; The RGB image is segmented to obtain a target region segmentation mask, which includes a segmentation mask for the kettle spout region, a segmentation mask for the water cup region, and a segmentation mask for the kettle handle. The surface point cloud of the target object is reconstructed based on the depth image to obtain the target surface point cloud matrix, which includes a first surface point cloud matrix, a second surface point cloud matrix, and a third surface point cloud matrix. Based on the segmentation mask of the kettle spout region and the first surface point cloud matrix, the three-dimensional coordinates of the kettle spout in the camera coordinate system are calculated, and the position of the kettle spout in the robot coordinate system is obtained by coordinate transformation. Based on the segmentation mask of the water cup region and the second surface point cloud matrix, the 6D pose of the water cup in the robot coordinate system is estimated, and the position of the water cup in the robot coordinate system is extracted from the 6D pose of the water cup in the robot coordinate system. Based on the segmentation mask of the kettle handle and the third surface point cloud matrix, the 6D pose of the kettle handle in the robot coordinate system is estimated, and the handle position in the robot coordinate system is extracted from the 6D pose of the kettle handle in the robot coordinate system.

3. The method as described in claim 2, characterized in that, Before the step of iteratively adjusting the end-effector pose of the humanoid robot to reduce the positional deviation based on the positional deviation between the spout position and the target water-pouring position using a first visual servo control algorithm, the method further includes: Obtain the handle position of the kettle handle in the robot coordinate system and the 6D pose of the kettle handle in the robot coordinate system; Based on the handle position of the kettle in the robot coordinate system and the 6D posture of the kettle handle in the robot coordinate system, the grasping pose is planned, and the robotic arm is controlled to move to the handle position to perform the grasping action.

4. The method as described in claim 2, characterized in that, The step of calculating the three-dimensional coordinates of the kettle spout in the camera coordinate system based on the segmentation mask of the kettle spout region and the first surface point cloud matrix, and obtaining the spout position in the robot coordinate system through coordinate transformation, includes: Extract the pixel coordinate set from the segmentation mask of the kettle spout region; The corresponding depth value set is obtained from the first surface point cloud matrix based on the pixel coordinate set; The depth value set is sorted and truncated by filtering to obtain the denoised representative depth values; Based on the statistical center value of the pixel coordinate set, camera intrinsic parameters, and the representative depth value, the three-dimensional coordinates of the kettle spout in the camera coordinate system are calculated. By using a pre-calibrated transformation matrix from camera to robot coordinate system, the three-dimensional coordinates of the kettle spout in the camera coordinate system are converted into the spout position in the robot coordinate system.

5. The method as described in claim 1, characterized in that, The steps of determining the target joint angle of the robotic arm by using the constraint of keeping the end-effector's posture unchanged as the first constraint and the constraint of moving the end-effector's position to the desired position as the second constraint include: The first optimization objective is to keep the end-effector posture unchanged, and the second optimization objective is to move the end-effector position to the desired position. An inverse motion solution objective function is constructed. The objective function includes a forward kinematics operator for the robotic arm and a diagonal weight matrix. The diagonal weight matrix is ​​used to assign different weights to the deviations of different dimensions of the end-effector posture rotation matrix. The objective function is solved by optimization algorithm to obtain the target joint angle, which corresponds to the joint angle corresponding to the minimum value of the objective function.

6. A humanoid robot water-pouring system based on visual servoing, characterized in that, The visual servo-based humanoid robot water-pouring system includes: a water cup, a kettle, a storage platform, and a humanoid robot. The water cup and the water bottle are placed on the storage platform; The humanoid robot stands on one side of the placement platform and is used to perform the visual servo-based humanoid robot water-pouring method as described in any one of claims 1 to 5.

7. The system as described in claim 6, characterized in that, The humanoid robot includes a robot head camera, a dexterous hand, a robotic arm, and a control unit; the water bottle is equipped with a handle and a spout. The robot head camera is used to acquire the first pose information of the target object in the working environment. The first pose information of the target object includes at least the position of the spout of the kettle in the robot coordinate system and the position of the cup in the robot coordinate system. The control unit is used to control the dexterous hand to grasp the handle of the kettle; The control unit is also used to control the robotic arm to perform a water-pouring action after the grasping action is completed. The water-pouring action includes aligning the spout of the kettle with the target position for water pouring.

8. The system as described in claim 7, characterized in that, The control unit is also used to control the robotic arm and the dexterous hand to return to the default zero position in sequence after the robotic arm performs the water-pouring action.

Images