Node arrangement control method for double-arm pattern sequence process

Abstract

The application discloses a node arrangement control method for a double-arm pattern drawing action sequence process, relates to the technical field of robot cooperative control, and is used for solving the problems that when a double-arm robot executes a pattern drawing pattern, a plurality of state couplings cause a deviation source to be difficult to identify, and a trajectory point that has not been executed is difficult to be safely and accurately corrected; in the pattern drawing, when a line position or width exceeds a permitted range, a deviation starting moment is determined, actual data is compared with a trajectory point record value, an adjustment object and a trajectory point adjustment mode are determined according to a change moment, an order and a direction, a correction amount is determined according to a change amount, a trajectory point that has not been executed is modified after expected change and constraint checking, and the pattern drawing correction accuracy and execution stability are improved.

Description

Technical Field

[0001] This invention relates to the field of robot collaborative control technology, and more specifically, to a node arrangement control method for a double-arm latte art sequence process. Background Technology

[0002] With the development of robotics and machine vision technologies, dual-arm robotic arms can grip a base cup and a milk container, completing latte art actions such as fusion injection, pattern laying, pattern convergence, and tangenting according to node sequence and trajectory segments. Current control methods execute trajectories based on calibrated cup shape, milk foam state, and motion parameters, maintaining dual-arm coordination through time synchronization. However, during pattern laying, the position of the base cup, its tilt angle, the spout's movement position, the milk container's tilt angle, the pouring flow rate, the vertical distance between the spout and the liquid surface of the base cup, and the arrival time difference between the two arms may change simultaneously or sequentially. The same pattern line position shift or pattern line width change may be caused by different states, and directly modifying a single motion parameter may also cause relative pose deviation in latte art, robotic arm interference, pattern line interruption, or discontinuity in subsequent trajectories. Existing technologies struggle to accurately identify the source of deviation during action execution while simultaneously ensuring pattern correction, dual-arm coordination, and motion safety.

[0003] To address the above problems, this invention proposes a solution. Summary of the Invention

[0004] In order to overcome the above-mentioned defects of the prior art, embodiments of the present invention provide a node arrangement control method for the double-arm latte art motion sequence process, so as to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: In a preferred embodiment, it includes: Read the position and width of the pattern lines during the pattern laying process, and determine the starting time of the deviation when either exceeds the corresponding allowable range in the pattern laying action record; Read the actual data from the start time of the deviation to the current acquisition time during the pattern laying process, and compare it with the corresponding trajectory point record value at the same acquisition time to determine the change time and direction of each actual data; determine the change sequence according to the change time, and determine the adjustment object according to the change sequence and change direction to generate the trajectory point adjustment method; Read the change amount of the adjustment object and the change amount of the pattern line, determine the correction amount according to the correspondence between the change amount and the change amount of the pattern line, determine the expected change direction of the adjustment method of each trajectory point and perform constraint check; Select a trajectory point adjustment method that simultaneously changes the position and width of the pattern lines within the corresponding allowable range and passes the constraint check, and modify the trajectory points that have not yet been executed according to the correction amount.

[0006] In a preferred embodiment, trajectory segments of pattern laying nodes are generated for the base cup arm and the milk cylinder arm, respectively. The trajectory segments are multiple trajectory points arranged in sequence when the robotic arms execute the pattern laying nodes. The trajectory points of the two robotic arms are recorded using a common time reference, which is the same timing start point and the same time unit used by the motion controllers of the two robotic arms. The relative pose of the latte art is determined jointly based on the trajectory points corresponding to the same arrival time under the common time reference, and the relative pose of the latte art is recorded to the corresponding trajectory points.

[0007] In a preferred embodiment, based on the base positions of the two robotic arms, the positional relationship between the robotic arm end effector and the gripper, and the positional relationship between the gripper and the base cup or milk container, the target positions and orientations of the base cup and milk container in the pattern laying nodes are converted into the target positions and orientations of the corresponding robotic arm end effectors. Inverse kinematics calculations are performed on the target positions and orientations of the robotic arm end effectors. Joint positions that exceed the allowable range, where the robotic arm collides with a fixed component, or where the two robotic arms interfere with each other are deleted. Among the remaining joint positions, joint positions with smaller differences from the current joint position are selected. The selected joint positions are connected according to the action sequence, and trajectory points are generated using cubic B-spline interpolation.

[0008] In a preferred embodiment, the shortest time required for the base cup arm and milk cylinder arm to execute the pattern laying node is calculated separately, and the longer of the two times is taken as the node duration. Keeping the geometry of the motion path unchanged, the arrival time of the trajectory point of the arm with the shorter execution time is extended according to the node duration, so that the two robotic arms start the pattern laying node simultaneously under a common time reference and reach the node end position within the allowable difference in arrival time. The distances between the two robotic arm bodies, fixtures, base cups, milk cylinders, and fixed components are checked point by point, and the relative pose of the latte art is recalculated according to the trajectory points. If the joint movement restrictions, minimum allowable distances, and allowable deviations of the relative pose of the latte art cannot be met simultaneously, the pattern laying node is marked as an execution failure.

[0009] In a preferred embodiment, during the pattern laying node execution, the camera continuously acquires images of the liquid surface in the base cup according to the acquisition time, determines the range of the liquid surface in the base cup based on the cup rim contour, and determines the formed pattern lines based on the color difference between the coffee liquid and the milk foam; the position and width of the pattern lines are determined along the spout movement direction corresponding to the pattern laying, and the acquisition time of the camera, the acquisition time of the milk container weighing sensor, and the control cycle of the two robotic arms are aligned according to a common time reference, so that the same acquisition time corresponds to the actual data and the actual trajectory points reached by the two robotic arms.

[0010] In a preferred embodiment, after the position or width of the pattern line first exceeds the corresponding allowable range, subsequent continuously acquired images of the liquid surface of the substrate cup are read. When the time covered by the liquid surface images of the substrate cup is not less than the pattern forming time, and the position or width of the pattern line remains outside the corresponding allowable range, the acquisition time at which the first excess of the corresponding allowable range is acquired is determined as the deviation start time. The actual data is compared with the corresponding trajectory point record value at the same acquisition time, and the acquisition time at which the difference first exceeds the corresponding allowable range is determined as the change time. The change time is uncertain only when the actual data exceeds the corresponding allowable range at a single acquisition time and re-enters the corresponding allowable range at the next acquisition time. The change order is determined based on the time difference between each change time and the allowable time difference, or the changes occur simultaneously. When the acquisition time intervals are different, the change order is determined after conversion to the same control cycle according to a common time reference.

[0011] In a preferred embodiment, based on the order and direction of change, the trajectory points that the base cup arm has not yet executed or the trajectory points that the milk cylinder arm has not yet executed are determined as position adjustment objects, the tilt angle of the milk cylinder is determined as tilting adjustment objects, the movement position of the spout is determined as height adjustment objects, and the movement speed of the milk cylinder arm is determined as width adjustment objects. If the time difference between the two robotic arms actually reaching the same trajectory point occurs before the relative pose of the latte art changes, the arrival time of the trajectory point that the two robotic arms have not yet executed is adjusted; if the time difference and the relative pose of the latte art change simultaneously, it is determined whether to adjust the arrival time of the trajectory point first or the movement path of the corresponding robotic arm first, based on the actual movement path of the two robotic arms. When multiple data points identified as adjustment targets change simultaneously, trajectory point adjustment methods are generated, one for adjusting only each data point and the other for adjusting multiple data points sequentially according to different adjustment orders. When multiple data points identified as adjustment targets change sequentially, trajectory point adjustment methods are generated, one for adjusting only one data point and the other for adjusting multiple data points sequentially according to the actual occurrence order.

[0012] In a preferred embodiment, the changes in the state of the object to be adjusted relative to the recorded value of the corresponding trajectory point and the changes in the pattern lines are read; a collection period in which the state changes continuously while other states that may affect the same pattern line are all within the corresponding allowable range is selected, and a correction amount is determined based on the correspondence between the state change and the pattern line change within the collection period; if the collection period does not exist, the correction amount is determined by reading the correspondence under the same cup type, the same milk foam consistency, and the quality of adjacent milk containers from the pre-designed latte art calibration data, and the correction amount is limited according to the maximum allowable correction amount per instance; when adjusting multiple states sequentially according to the actual occurrence order, a first correction amount and its corresponding expected change in the pattern line are determined based on the earliest state that changed, the expected change in the pattern line is subtracted from the pattern line change amount that needs to be corrected this time, and the correction amount for the next state is determined based on the result after subtraction.

[0013] In a preferred embodiment, starting from the first trajectory point that has not yet been executed, the trajectory point up to the next point where the spout movement direction changes is determined as the initial adjustment range. For each trajectory point adjustment method, without driving the two robotic arms, the expected pattern line position and expected pattern line width corresponding to the end position of the initial adjustment range are determined. Trajectory point adjustment methods that would cause any robotic arm joint position to exceed the allowable range, the distance between the two robotic arms to be less than the minimum allowable distance, the relative pose of the latte art to deviate further, or the current pouring flow change direction to be opposite to the required change direction of the pattern line width are deleted. Among the remaining trajectory point adjustment methods, the trajectory point adjustment method is selected in sequence based on whether the pattern line position and pattern line width can be changed to the corresponding allowable range simultaneously, the number of execution objects to be changed, the number of modified trajectory points, and the increase in the remaining execution time of the node. The unexecuted trajectory points are modified according to the selected trajectory point adjustment method. After each modified trajectory point is executed, the pattern line position and pattern line width are re-determined. When the change direction is opposite to the expected change direction, the execution of the unfinished adjustment amount is stopped, and a new trajectory point adjustment method is generated.

[0014] The technical effects and advantages of the node arrangement control method of the present invention for the double-arm latte art motion sequence process are as follows: This invention unifies multi-source data to a time base, allowing changes in pattern lines to correspond to the state of trajectory points. Adjustments are differentiated based on the time, direction, and sequence of these changes, reducing misadjustments caused by direct corrections. The correction amount is determined based on the correspondence between the changes in the adjusted object and the changes in the pattern lines, adapting to state changes. Constraint checks are performed using the distance between the two arms, the relative posture of the latte art, and the direction of the pouring flow to prevent collisions, trajectory exceeding limits, and further pattern deviation. During execution, the results are verified based on the trajectory points. If the results do not meet expectations, adjustments are stopped and reassessed to bring the pattern line position and width back within acceptable ranges, maintaining the continuity of subsequent trajectories and improving the quality of latte art formation and the reliability of the two-arm coordination. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the node arrangement and control method for the double-arm latte art sequence process of the present invention.

[0016] Figure 2 This is a flowchart illustrating the trajectory point adjustment method and dynamic correction process of the node arrangement control method for the double-arm latte art sequence process of the present invention. Detailed Implementation

[0017] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] In this embodiment, the present invention discloses a node arrangement control method for a double-arm latte art motion sequence process, including: In step one, the pattern number and cup type number from the order instruction are read. Based on the pattern number, the corresponding latte art action record is retrieved from the pre-stored latte art pattern library. The latte art action record records the movement sequence of each pattern position, including spout position, spout direction, movement speed, milk pitcher tilt angle, pouring flow rate, vertical distance between the spout and the base cup liquid surface, pattern line position, pattern line width, conditions for starting and stopping pouring, and the allowable range for each data point. The latte art action record controls the base cup and milk pitcher to complete the latte art test according to preset actions, simultaneously recording the base cup position, base cup tilt angle, spout position, milk pitcher tilt angle, pouring flow rate, and liquid surface position within the cup. Simultaneously, the position and width of the formed pattern lines are detected by a camera. For latte art tests where the finished pattern detection result is within the corresponding allowable deviation, the corresponding data is written into the latte art action record according to the acquisition time and movement sequence. For latte art tests where the finished pattern detection result exceeds the corresponding allowable deviation, the corresponding data is not used as the initial data for the latte art action record. The cup diameter, height, and allowable tilt range are read from the cup type calibration table based on the cup type number. The cup type calibration table is obtained by measuring different cup types after they are installed in the equipment and stored according to the cup type number. The extraction volume recorded during this coffee making process is read, and an image of the base cup is captured by a camera set above the latte art area. The position of the base cup, the orientation of the rim, and the liquid level inside the cup are determined based on the camera calibration parameters.

[0019] The system reads the milk frothing duration and milk foam temperature recorded by the steam milk frothing system, and simultaneously reads the mass of the milk container from the milk container weighing sensor. Both robotic arms have grippers equipped with weighing detection components capable of detecting changes in the mass of the gripped object. When either robotic arm is designated as the milk container arm, the weighing detection component on its gripper acts as the milk container weighing sensor. The milk container weighing sensor is located at the position where the gripper contacts the milk container and bears its mass. Zero-point calibration is performed when the milk container is not gripped. After gripping an empty milk container, its own mass is recorded. After gripping a milk container filled with milk foam, the mass of the milk container is subtracted from the measured mass to obtain the mass of the milk foam.

[0020] During equipment installation, the robotic arm holding a milk cylinder of known mass is controlled to move at different joint positions, speeds, directions, and tilt angles. The difference between the detection result of the milk cylinder weighing sensor and the known mass is recorded, establishing a correspondence between the robotic arm's motion state and the weighing detection difference. During latte art, based on the current joint position, speed, tilt angle, and direction of movement of the milk cylinder arm, the corresponding weighing detection difference is read from the aforementioned correspondence. This difference is then subtracted from the detection result of the milk cylinder weighing sensor, and the subtracted result is taken as the mass of the milk cylinder.

[0021] The consistency of the milk foam used for this latte art was determined based on the correspondence between the milk frothing duration, milk foam temperature, milk pitcher mass, and a pre-defined milk foam consistency. This correspondence was obtained by creating milk foam using different milk frothing durations and temperatures, recording the corresponding milk pitcher mass, and detecting the milk foam flow state. When detecting the milk foam flow state, the milk pitcher containing the milk foam was held in a pre-defined detection position, ensuring the spout maintained the same spout orientation and detection tilt angle. The amount of milk pitcher mass reduction and the spread of the milk foam in the detection container after leaving the spout were recorded within the same detection time. A larger amount of milk pitcher mass reduction and a larger spread indicate a higher degree of milk foam flow; a smaller amount of milk pitcher mass reduction and a smaller spread indicate a lower degree of milk foam flow. The milk frothing duration, milk foam temperature, milk pitcher mass, milk pitcher mass reduction, and spread were all mapped to different milk foam consistencies, and the allowable range of milk foam consistency was divided in order from high to low flowability.

[0022] The consistency of the milk foam used in this latte art was determined by comparing the actual frothing duration, milk foam temperature, and milk pitcher weight with pre-calibrated data. For each milk foam consistency, the allowable ranges for frothing duration, milk foam temperature, and milk pitcher weight were read. When the actual frothing duration, milk foam temperature, and milk pitcher weight all fell within the allowable ranges corresponding to the same milk foam consistency, that milk foam consistency was determined as the consistency of the milk foam used in this latte art.

[0023] When the actual frothing duration, milk foam temperature, and milk pitcher mass are within the data range corresponding to two adjacent milk foam consistency values, calculate the difference between the actual frothing duration, milk foam temperature, and milk pitcher mass and the nearest boundary of the allowable range corresponding to each adjacent milk foam consistency value. Divide each difference by the width of the corresponding allowable range, and sum the calculation results for the same milk foam consistency value. Select the milk foam consistency with the smaller sum. If the sums are the same, first select the milk foam consistency with the smaller difference between the actual milk foam temperature and the corresponding allowable range. If they are still the same, select the milk foam consistency with the smaller difference between the actual milk pitcher mass and the corresponding allowable range. If they still cannot be distinguished, the milk foam status is determined to be unsuccessful. If the actual frothing duration, milk foam temperature, or milk pitcher mass is not within the allowable range corresponding to any milk foam consistency value, and is not within the data range corresponding to two adjacent milk foam consistency values, the milk foam status is determined to be unsuccessful.

[0024] The location of the milk cup and the orientation of the spout are determined by capturing images of the milk cup with a camera; the location of the base cup, the liquid level in the cup, the location of the milk cup, and the orientation of the spout are then converted into the coordinate system of the latte art work area.

[0025] The system reads the current joint position, joint speed, and operating status from the motion controllers of the two six-axis collaborative robotic arms, and the clamping position and clamping force detected by the force sensor from the clamp controller. It also reads the positions of the two robotic arm bases, the positional relationship between the robotic arm end effector and the clamp, the positional relationship between the clamp and the held base cup or milk container, and the positions of the coffee machine casing, operating table, and other fixed components within the latte art work area, all stored during equipment installation. The system records the acquisition time for each data point; if the acquisition time difference between camera data, weighing data, temperature data, and robotic arm status data exceeds the allowable time difference set by the controller, the corresponding data is re-acquired. If the calibration table version is inconsistent with the current installation configuration, the sensor status is abnormal, or the robotic arm is not in a permitted movement state, the current data reading is stopped and an error handling process is initiated. Figure 1 As shown.

[0026] This embodiment does not separately specify the allowable range and allowable deviation of the determination method. All methods are pre-calibrated using the same robotic arm, fixture, base cup, milk container, and camera after the equipment is installed. During calibration, the remaining data are kept within the corresponding allowable range, and the data to be calibrated is changed step by step, with at least one latte art action performed at each data level. The data range in which the robotic arm can complete the corresponding action without collision or slippage, and the position and width of the pattern lines are within the corresponding allowable deviation, is determined as the allowable range of the data.

[0027] Continue changing the data until the pattern line position, pattern line width, latte art relative pose, current pouring flow, or robotic arm movement state first exceed the corresponding requirements. The data before this first exceedance is taken as the boundary of the allowable range. When calibrating the same data multiple times, select the common data range that satisfies the corresponding requirements in each calibration as the final allowable range.

[0028] In this embodiment, any distance approaching the minimum permissible distance means that the distance is still greater than or equal to the minimum permissible distance, but the difference between the distance and the minimum permissible distance is not greater than the distance required for the corresponding robotic arm to reduce its speed to the permissible safe speed at the current moving speed. The distance required to reduce to the permissible safe speed is determined based on the robotic arm's current moving speed, maximum permissible deceleration, and control cycle.

[0029] The clamping force is close to the upper limit of the clamping force, which means that the current clamping force has not exceeded the upper limit of the clamping force, but the difference between the current clamping force and the upper limit of the clamping force is not greater than the change in clamping force corresponding to the clamping controller performing a minimum clamping closure adjustment.

[0030] Continuous sliding of the base cup or milk container means that the camera detects that the position of the base cup or milk container relative to the corresponding fixture exceeds the allowable range, and in a series of images covering the time required for the corresponding robotic arm to stop moving, the position does not return to the allowable range.

[0031] Milk foam entering the coffee liquid without forming a clear pattern on the surface means that after the milk foam enters the coffee liquid, the camera does not detect a continuous milk foam area at the corresponding position whose width reaches the lower limit of the allowable range of the line width of the pattern.

[0032] Adjacent milk container mass refers to the two milk container masses located on either side of the current milk container mass in the pre-spreading calibration data and having the smallest difference from the current milk container mass; when the current milk container mass is the same as one of the milk container masses in the pre-spreading calibration data, the data corresponding to that milk container mass is read directly.

[0033] The occurrence of the same change multiple times means that, under the same cup type, the same milk foam consistency, and the mass of adjacent milk containers, at least three consecutive latte art designs were detected with the same direction of change, and the time of each change had a common range.

[0034] A large-scale shift in the main body of a pattern means that the edge of the main body of the pattern exceeds the corresponding allowable deviation, and the center positions of the pattern lines corresponding to more than half of the pattern positions all exceed the corresponding allowable deviation in the same direction. If only one or a few adjacent pattern positions exceed the corresponding allowable deviation, it is not certain that the main body of the pattern has shifted significantly.

[0035] Based on the current joint positions of the two robotic arms, calculate the movement distances when the first robotic arm grasps the base cup, the second robotic arm grasps the milk container, and the first robotic arm grasps the milk container and the second robotic arm grasps the base cup. For both allocation methods, check whether the robotic arm joints exceed the allowable range, whether the robotic arms and grippers will collide with fixed components, whether the two robotic arms interfere with each other within the latte art area, and whether the gripping position needs to be readjusted midway after grasping. Delete allocation methods where joints exceed limits or collisions cannot be avoided; among the remaining allocation methods, first select the allocation method with the shorter expected movement time; if the expected movement times are the same, select the allocation method with the larger minimum distance between the two robotic arms; if they are still the same, select the allocation method where the joint position is far from the joint limit and no re-gripping is required. The robotic arm responsible for grasping and adjusting the position and tilt angle of the base cup is called the base cup arm, and the robotic arm responsible for grasping the milk pitcher and controlling the position of the spout, the tilt angle of the milk pitcher, and the pouring action is called the milk pitcher arm. The base cup arm and the milk pitcher arm represent the roles performed in this latte art, and do not correspond to the robotic arms on the left or right side of the equipment.

[0036] The estimated motion time is determined based on the motion paths from the current joint position of the robotic arm to the gripping position, from the gripping position to the lapping area, and from the lapping area to the reset position. The change in joint position for each joint in each motion path segment is calculated, and the time required for each segment is determined according to the maximum joint velocity and maximum joint acceleration of the corresponding joint. The times required for each segment of the same robotic arm's motion path are added together to obtain the estimated motion time for that robotic arm. The longer of the estimated motion times for two robotic arms under the same allocation method is taken as the estimated motion time for that allocation method.

[0037] When checking whether the robotic arm and gripper will collide with the fixed component and whether there is mutual interference between the two robotic arms, additional check positions are added between the current joint position of the robotic arm, the gripping position, the corresponding position of the latte art area, and the reset position. The distances between the two robotic arm bodies, grippers, base cup, milk container, and fixed component are calculated at each check position. If any distance is less than the minimum allowable distance and cannot be avoided by changing the movement path between the check positions, the corresponding allocation method is determined as an allocation method where collision cannot be avoided.

[0038] If only one of the two allocation methods is not deleted, the allocation method that is not deleted is used directly. If both allocation methods have joint over-limit issues or are both determined to be unable to avoid collisions, the base cup arm and milk cylinder arm are not determined, the dual-arm role allocation is stopped, the dual-arm role allocation node is marked as execution failure, and the process is transferred to the exception handling node. After transferring to the exception handling node, the two robotic arms maintain their current joint positions, the fixture maintains its current opening and closing positions, and the joint over-limit positions, the check positions where insufficient distance occurs, and the minimum distance between the two robotic arms are recorded for both allocation methods.

[0039] Following the movement sequence recorded in the latte art action log, the latte art actions are divided into four parts: blending and pouring, pattern laying, pattern closing, and tangent. Blending and pouring refers to the action of the spout being above the liquid surface and mixing the milk foam with the coffee liquid; pattern laying refers to the action of the spout approaching the liquid surface and moving along the pattern trajectory to form the main lines; pattern closing refers to the action of reducing the pouring volume and merging the main lines; and tangent refers to the action of the spout crossing the formed main lines and stopping pouring. For each action, the corresponding spout position, movement direction, movement speed, and milk pitcher tilt angle are read, and the position recorded in the pattern library with the cup rim as the reference is converted to the position on the liquid surface of the base cup in this case.

[0040] Establish a node diagram according to the actual execution sequence. The node diagram includes, in sequence, the start check, dual-arm role assignment, base cup gripping, milk pitcher gripping, base cup status confirmation, milk foam status confirmation, latte art preparation synchronization, dual-arm preparatory posture, fusion injection, pattern laying, pattern convergence, tangent line, stop pouring, base cup reset, milk pitcher reset, finished pattern inspection, and anomaly handling. Base cup gripping and milk pitcher gripping are performed by the base cup arm and milk pitcher arm respectively after dual-arm role assignment, and can start simultaneously; base cup status confirmation is performed after base cup gripping, checking whether the base cup position, cup opening orientation, and liquid level are within the allowable range for the corresponding cup type; milk foam status confirmation is performed after milk pitcher gripping, checking whether the milk pitcher quality, milk foam temperature, and milk foam consistency are within the allowable range for the corresponding beverage formula. Latte art preparation synchronization is only allowed to be executed after both base cup status confirmation and milk foam status confirmation have passed, and then sequentially connects dual-arm preparatory posture, fusion injection, pattern laying, pattern convergence, tangent line, and stop pouring.

[0041] For each node in the node diagram, record the robotic arm executing that node, the previous node, the data read, the action completion conditions, the allowed execution time, the occupied base cup, milk pitcher, or latte art area, and the node to which the action is transferred if it fails. For example, the action completion conditions for the base cup gripping node are that the clamp is closed and reaches the gripping position, the gripping force is within the corresponding allowable range, and the camera detects that the position of the base cup relative to the clamp has not changed; the action completion conditions for the milk pitcher gripping node are that the clamp is closed and reaches the gripping position, the gripping force is within the corresponding allowable range, and the milk pitcher mass detected by the milk pitcher weighing sensor is within the allowable range of the corresponding beverage formula.

[0042] The completion conditions for the base cup status confirmation node are that the position of the base cup, the orientation of the cup opening, and the position of the liquid level inside the cup are all within the allowable range for the corresponding cup type; the completion conditions for the milk foam status confirmation node are that the quality of the milk pitcher, the temperature of the milk foam, and the consistency of the milk foam are all within the allowable range for the corresponding beverage recipe; the completion conditions for the latte art preparation synchronization node are that both the base cup status confirmation node and the milk foam status confirmation node have been completed, and the base cup arm, milk pitcher arm, base cup, milk pitcher, and latte art area are not occupied by other nodes.

[0043] The completion conditions for the injection, pattern laying, pattern convergence, tangent, and stop-pouring nodes are determined based on the base cup position, base cup tilt angle, spout movement position, milk pitcher tilt angle, current pouring flow rate, and relative pose of the latte art at the corresponding node's end position. When all the data are within the allowable range recorded for the corresponding node, and the time difference between the actual arrival times of the two robotic arms at the node's end position does not exceed the allowable arrival time difference, the corresponding node is marked as complete.

[0044] When a node is marked as allowed to execute, the robotic arm, base cup, milk tank, or latte art area recorded by that node is marked as occupied; when the node completes, fails to execute, or is transferred to an exception handling node, the occupied marking of the robotic arm, base cup, milk tank, or latte art area that is no longer in use is removed.

[0045] Nodes using the same robotic arm, the same object, or the same latte art area that may interfere with each other must not be executed simultaneously. If the current node has completed, all required data for the current node has passed verification, and the corresponding robotic arm, object, and latte art area are not occupied by other nodes, the current node is marked as allowed to execute. If the grasping fails, return to the corresponding grasping node to re-grab; if the position or milk foam state does not meet the allowed range, re-detect; if it still does not meet the allowed range after re-detection or there is a risk of robotic arm collision, stop both robotic arms and proceed to the exception handling node.

[0046] In step two, the execution objects, previous nodes, latte art action records, action completion conditions, allowed execution time, and resource usage for each node in the node diagram—including the dual-arm preparatory pose, fusion injection, pattern laying, pattern convergence, tangent, and stop pouring—are read. Trajectory segments for each node are then generated for the base cup arm and milk cylinder arm. These trajectory segments are multiple trajectory points arranged sequentially when the robotic arm executes a node. Each trajectory point records the joint position the robotic arm should reach, the time it takes to reach that joint position, and the corresponding base cup position, base cup tilt angle, spout movement position, vertical distance between the spout and the base cup liquid surface, milk cylinder tilt angle, spout movement direction, relative latte art pose, and pouring flow rate. The trajectory points corresponding to the base cup arm record the base cup position and base cup tilt angle, while the trajectory points corresponding to the milk cylinder arm record the spout movement position, milk cylinder tilt angle, spout movement direction, and pouring flow rate. Based on the trajectory points corresponding to the base cup arm and milk cylinder arm at the same arrival time, the relative latte art pose corresponding to that arrival time is determined. The trajectory points of the two robotic arms are recorded using a common time reference, which is the same timing starting point and the same time unit used by the motion controllers of the two robotic arms.

[0047] After the start-up check node is completed, the two robotic arm motion controllers receive the same start-time command and use the moment of receiving the start-time command as the starting point of the common time reference. The controllers send time correction commands to the two robotic arm motion controllers, the camera, the milk bottle weighing sensor, and the temperature sensor at fixed time intervals. Each device corrects its recorded acquisition time according to the time correction command.

[0048] When a time correction command is sent, the controller writes the elapsed time since the starting point of the common time reference into the time correction command. The motion controllers of the two robotic arms, the camera, the milk bottle weighing sensor, and the temperature sensor each read the elapsed time recorded from the aforementioned starting point and calculate the difference between their recorded elapsed time and the elapsed time in the time correction command.

[0049] When the difference does not exceed the allowable time difference, the difference is allocated to each acquisition time or control cycle before the next time correction command arrives, so that the corrected acquisition time changes continuously. When the difference exceeds the allowable time difference, the data acquired by the corresponding device between the previous time correction command and the current time correction command is not used, and the corresponding data is reacquired. When correcting the acquisition time, the sensor detection results and the robotic arm status data are not changed; only the recording time of the data relative to a common time reference is changed.

[0050] The camera acquisition time, the milk container weighing sensor acquisition time, and the control cycles of the two robotic arms are converted into the elapsed time from the start of the self-timer. When the acquisition time of a certain camera does not coincide with the control cycle of a robotic arm, data from the adjacent control cycles before and after that acquisition time are read, and the corresponding data is determined based on the position of that camera acquisition time between the two control cycles. When adjacent data can change continuously, the conversion is performed according to the amount of change between two adjacent data. When adjacent data cannot change continuously, the data from the control cycle closer to that camera acquisition time is used.

[0051] When the difference between the acquisition time of the camera, milk bottle weighing sensor, or any robotic arm motion controller and the common time reference exceeds the allowable time difference, the corresponding data will not be used for comparison of the state change sequence, and the corresponding data will be reacquired.

[0052] Using a pre-marked reference point on the latte art worktable as the origin of the latte art work area coordinate system, and determining the three coordinate axes of the latte art work area coordinate system according to the equipment installation direction. Based on the base cup position, cup opening orientation, and liquid surface position determined in step one, the spout movement positions corresponding to fusion injection, pattern laying, pattern convergence, and tangent are converted into actual positions in the latte art work area coordinate system. For each actual position, the relative position and relative orientation of the milk pitcher spout relative to the base cup liquid surface are determined, and these relative positions and relative orientations are recorded as the latte art relative pose. The latte art relative pose includes the corresponding position of the spout on the base cup liquid surface, the vertical distance between the spout and the base cup liquid surface, the milk pitcher tilt angle, and the spout movement direction.

[0053] Based on the relative pose of the latte art corresponding to the fusion pouring, the base cup arm is first determined to move the base cup to the latte art area and maintain the target position and target tilt angle of the base cup within the allowable tilt range of the cup shape. Then, the milk pitcher arm is determined to move the milk pitcher above the liquid surface of the base cup and make the spout face the target position and target orientation of the initial pouring position. During the fusion pouring process, the milk pitcher arm is controlled to approach the liquid surface of the base cup according to the spout movement speed and milk pitcher tilt angle recorded in the latte art action record, and the initial vertical distance between the spout and the liquid surface of the base cup is determined according to the liquid surface position in the cup and the consistency of the milk foam. The initial vertical distance is obtained by pre-calibrating the latte art using the same cup shape and different milk foam consistencies, and recording the spout height when the milk foam can enter the coffee liquid without forming obvious patterns on the liquid surface.

[0054] After the spout movement corresponding to the fusion injection is completed, the position of the base cup, the tilt angle of the base cup, the position of the milk cylinder, and the tilt angle of the milk cylinder at the end of that node are used to generate the end-effector motion path corresponding to the pattern laying. The end-effector of the robotic arm represents the position of the fixture mounted on the robotic arm, and the end-effector pose represents the position and orientation of the fixture in the latte art work area coordinate system. During the pattern laying process, the base cup arm adjusts the position and tilt angle of the base cup according to the latte art action record, and the milk cylinder arm moves the spout sequentially according to the converted pattern position, and reduces the vertical distance between the spout and the liquid surface of the base cup. The initial milk cylinder tilt angle corresponding to each pattern position is determined based on the milk cylinder mass, milk foam consistency, and latte art action record, so that the pouring flow rate matches the width of the corresponding pattern line. The pouring flow rate is the amount of milk cylinder mass reduction detected by the weighing sensor per unit time, and the correspondence between the initial milk cylinder tilt angle and the pouring flow rate is obtained by calibrating the same milk cylinder at different tilt angles.

[0055] After the pattern is laid out, the last trajectory point of the pattern is used as the starting trajectory point for the pattern termination. Following the latte art recording, the pitcher tilt angle is gradually decreased while the vertical distance between the spout and the liquid surface in the base cup is increased. Simultaneously, the base cup arm is controlled to decrease the tilt angle of the base cup, gradually restoring it to an upward-facing position. Upon reaching the spout position corresponding to the pattern termination, this position is used as the starting point of the tangent. The pitcher arm is controlled to move across the already formed pattern body in the direction corresponding to the tangent, while continuing to decrease the pitcher tilt angle. After the tangent is completed, a trajectory segment corresponding to stopping pouring is generated. The pitcher arm is then controlled to restore the pitcher to the angle where milk foam stops flowing, and the spout is moved outside the range of the base cup rim.

[0056] Based on the base positions of the two robotic arms, the positional relationship between the robotic arm end effector and the gripper, and the positional relationship between the gripper and the base cup or milk container, the target positions and orientations of the base cup and milk container at each node are converted into the corresponding target positions and orientations of the robotic arm end effectors. Inverse kinematics calculations are performed on the target positions and orientations of each robotic arm end effector. These inverse kinematics calculations are the process of determining the positions of each joint of the robotic arm based on the position and orientation that the robotic arm end effector should achieve. When the same target position and orientation correspond to multiple sets of joint positions, joint positions that exceed the allowable range, where the robotic arm collides with a fixed component, or where the two robotic arms interfere with each other are deleted. From the remaining joint positions, a set of joint positions with the smallest difference from the current joint position is selected. Joint positions where a slight movement of the robotic arm end effector causes a sharp increase in the velocity of one or more joints are also deleted.

[0057] When determining whether a joint speed increases sharply, the change between the target position and target orientation of two adjacent robotic arm ends is used as the robotic arm end-effector movement, and the change in joint position between the corresponding two sets of joint positions is calculated. If the robotic arm end-effector movement is less than the pre-calibrated small end-effector movement range, and the change in any joint position exceeds the joint position change that the joint can complete at the maximum joint speed within one control cycle, then the joint position is determined to cause a sharp increase in joint speed.

[0058] For the deleted joint position, the next joint position with the smallest difference from the current joint position is selected in sequence; if none of the other joint positions meet the requirements, the corresponding target position is moved forward or backward along the original end effector motion path of the robotic arm, and the inverse kinematics calculation is performed again. If the relative pose of the latte art cannot be kept within the corresponding allowable deviation after moving the target position, the corresponding node is marked as execution failure.

[0059] The selected joint positions are connected according to the sequence of actions in each node, and cubic B-spline interpolation is used to supplement continuous joint positions between adjacent joint positions. The cubic B-spline interpolation method generates a smooth curve using the starting joint position, the joint position where the action direction changes, and the ending joint position. The joint positions used to determine the shape of the smooth curve are called control points. The starting and ending joint positions of each node are used as fixed control points, and the joint positions corresponding to changes in the spout movement direction, the base cup tilt direction, and the milk cylinder tilt angle are used as intermediate control points, ensuring continuous variation in joint position, joint velocity, and joint acceleration between adjacent trajectory points.

[0060] When performing cubic B-spline interpolation, the position of each joint is interpolated separately. The starting joint position, intermediate control points, and ending joint position are arranged according to the sequence of actions in the corresponding nodes, and the time interval between each control point is determined based on the arrival time of each control point. The starting and ending joint positions remain unchanged, while intermediate control points are used to change the shape of the joint position curve between adjacent control points.

[0061] Starting from the node start time, each sampling time is determined sequentially according to the control cycle of the robotic arm motion controller. At each sampling time, the joint position corresponding to the B-spline curve is read three times, and the read joint position is used as the trajectory point corresponding to that sampling time. The joint velocity is determined based on the change in joint position and the time difference of arrival of adjacent trajectory points, and the joint acceleration is determined based on the change in velocity and the time difference of arrival of adjacent joint points.

[0062] The first trajectory point after interpolation uses the starting joint position of the node, and the last trajectory point uses the ending joint position of the node. When the change in the spout movement direction, the change in the base cup tilt direction, or the change in the milk container tilt angle does not fall within the sampling time corresponding to a control cycle, the sampling time closest to the change is added, and the joint position corresponding to the change is written into the added trajectory point. After adjusting or adding intermediate control points, all affected trajectory points are regenerated according to the control cycle of the robotic arm motion controller.

[0063] Based on the joint position range, maximum joint velocity, and maximum joint acceleration recorded in the robotic arm motion controller, each interpolated trajectory point is checked. The maximum permissible change in joint acceleration per unit time is determined based on the robotic arm servo control calibration results, and trajectory segments exceeding this maximum permissible change are deleted. The permissible execution time of the deleted trajectory segments is extended, and the arrival time of adjacent trajectory points is recalculated. If the extended time still exceeds the limits for joint velocity, joint acceleration, or change in joint acceleration, the corresponding intermediate control points are adjusted, and cubic B-spline interpolation is performed again.

[0064] When adjusting intermediate control points, first identify the trajectory point that exceeds the limits for joint velocity, joint acceleration, or change in joint acceleration, and then read the intermediate control points before and after that trajectory point. When the direction of the joint position change exceeding the limit is consistent with the direction of change from the previous intermediate control point to the next intermediate control point, move the corresponding intermediate control point in the opposite direction to reduce the change in joint position between that intermediate control point and the preceding and following intermediate control points. When the position exceeding the limit is located between two intermediate control points, add a joint position between the two intermediate control points and use the added joint position as the new intermediate control point.

[0065] After each adjustment of the intermediate control points, B-spline interpolation is performed three times, and the joint position, joint velocity, joint acceleration, and the change in joint acceleration per unit time are rechecked. When adjusting the intermediate control points causes the end effector of the robotic arm to deviate from the corresponding spout movement position or base cup position, adjacent intermediate control points are adjusted synchronously according to the deviation amount to keep the recalculated latte art relative pose within the corresponding allowable deviation.

[0066] Calculate the shortest time required for the base cup arm and milk cylinder arm to execute the same node, and take the longer time as the node duration from the start of execution to the fulfillment of the action completion conditions. Keeping the geometry of the motion path unchanged, extend the arrival time of the trajectory point of the arm with the shorter execution time according to the node duration, so that both robotic arms start the corresponding node simultaneously under a common time reference and arrive at the node's end position simultaneously within a specified allowable arrival time difference. The allowable arrival time difference is the maximum time difference allowed when the two robotic arms reach the same synchronous position, determined by controlling the two robotic arms to repeatedly execute coordinated actions of the same duration after equipment installation, based on the actual arrival time deviation and pattern forming condition. Independent start and end times are set for fusion injection, pattern laying, pattern convergence, tangenting, and stop tilting. The next node is only allowed to start after both robotic arms of the previous node have met the action completion conditions.

[0067] For each synchronized trajectory segment, the distances between the two robotic arms, grippers, base cup, milk pitcher, coffee machine casing, and latte art worktable are checked point by point. The minimum allowable distance is determined based on the equipment installation dimensions, robotic arm repeatability error, camera position detection error, and the distance required for the robotic arm to stop moving. When the distance corresponding to any trajectory point is less than the minimum allowable distance, the arrival time before and after that trajectory point is first extended; if extending the arrival time cannot eliminate the insufficient distance, the intermediate control point of the base cup arm or milk pitcher arm is changed, and the corresponding joint position and trajectory point are recalculated. The milk pitcher spout approaching the liquid surface of the base cup is a required approach movement for latte art. It is not judged according to the minimum allowable distance between the milk pitcher and the base cup, but rather by checking whether the spout is within the cup opening range, whether the spout is higher than the liquid surface of the base cup, and whether other parts of the milk pitcher are in contact with the base cup.

[0068] After completing the distance check, the relative pose of the latte art is recalculated according to the base cup position, base cup tilt angle, spout position, and milk pitcher tilt angle corresponding to the trajectory point. The recalculated relative pose is then compared with the relative pose of the latte art action record. If the spout position, the vertical distance between the spout and the liquid surface of the base cup, or the milk pitcher tilt angle exceeds the corresponding allowable deviation, the intermediate control point of the milk pitcher arm is adjusted first. If adjusting the milk pitcher arm would result in insufficient distance between the two robotic arms, the intermediate control point of the base cup arm is adjusted or the node duration is extended. Nodes that still cannot simultaneously meet the joint motion restrictions, minimum allowable distance, and allowable deviation of the relative pose of the latte art after recalculation are marked as execution failures and transferred to the exception handling node in the node graph. Trajectory points that meet all conditions are written into the trajectory segment of the corresponding node according to a common time reference.

[0069] In step three, when the dual-arm pre-pose node is marked as executable, the base cup arm and the milk cylinder arm read the trajectory segment corresponding to that node and simultaneously execute their respective trajectory points according to a common time reference. The motion controllers of the two robotic arms read the current joint position, current joint speed, and trajectory point arrival time according to a control cycle, which is a fixed time interval for the motion controllers to continuously update the robotic arm motion commands and sensor data. After the base cup arm reaches the target position and target tilt angle of the base cup, and the milk cylinder arm reaches the target position and target orientation of the milk cylinder, images of the base cup and milk cylinder are re-acquired by the camera. Based on the camera calibration parameters, the position of the base cup, the orientation of the cup opening, the position of the liquid surface inside the cup, the position of the milk cylinder, and the orientation of the spout are determined, and the relative pose of the latte art is recalculated. When the recalculated relative pose of the latte art is within the allowable deviation recorded by the dual-arm pre-pose node, and the time difference between the arrival of the two robotic arms to the corresponding trajectory points does not exceed the allowable time difference, the dual-arm pre-pose node is marked as complete, and the fusion injection node is marked as executable.

[0070] After the fusion injection node begins execution, the base cup arm maintains or adjusts the base cup position and tilt angle according to the trajectory segment corresponding to the fusion injection. The milk cylinder arm moves the spout above the liquid surface of the base cup according to the trajectory segment corresponding to the same node, and gradually increases the tilt angle of the milk cylinder. Each time the milk cylinder arm executes a control cycle, it calculates the current latte art relative pose based on the current joint position, the positional relationship between the robotic arm end effector and the gripper, the positional relationship between the gripper and the milk cylinder, and the position of the base cup and the liquid surface position detected by the camera. The current latte art relative pose is then compared with the latte art relative pose recorded at the current trajectory point. The comparison includes the corresponding position of the spout on the liquid surface of the base cup, the vertical distance between the spout and the liquid surface of the base cup, the tilt angle of the milk cylinder, and the direction of spout movement.

[0071] As the spout approaches the liquid surface of the base cup, the clamping force detected by the force sensor mounted on the milk cylinder arm clamp is read. This clamping force is the force applied by the clamp to the milk cylinder and detected by the force sensor. The lower limit of the clamping force is calibrated based on the force required to prevent slippage of milk cylinders containing different masses of milk foam during the movement of the robotic arm. The upper limit of the clamping force is calibrated based on the force that the milk cylinder material and the contact area with the clamp can withstand without deformation or damage. When the clamping force is below the lower limit, the clamp closing amount is gradually increased within the range not exceeding the upper limit. When the clamping force approaches the upper limit, the increase in the clamp closing amount is stopped, and the speed at which the milk cylinder arm approaches the liquid surface of the base cup is reduced. When the camera detects a change in the position of the milk cylinder relative to the clamp, the movement of the milk cylinder arm is paused, the clamping force is readjusted, and the position of the milk cylinder is detected. After the position of the milk cylinder returns to the allowable range, the fusion injection of the corresponding trajectory segment continues from the paused position.

[0072] The current pouring flow rate is obtained by continuously recording the milk cylinder mass from the milk cylinder weighing sensor according to the acquisition time. The difference in milk cylinder mass between adjacent acquisition times is divided by the corresponding time interval. Before calculating the current pouring flow rate, the weighing difference is read from the correspondence between the robotic arm motion state and the weighing difference established during equipment installation, based on the current joint position, current joint speed, milk cylinder tilt angle, and direction of movement of the milk cylinder arm. The milk cylinder mass between adjacent acquisition times is then corrected. The corrected difference in milk cylinder mass between adjacent acquisition times is then divided by the corresponding time interval to obtain the current pouring flow rate.

[0073] When the mass of the milk container increases at adjacent data collection times, or when the decrease in the mass of the milk container exceeds the pre-calibrated maximum mass decrease at the same milk container tilt angle, the corresponding milk container mass is identified as abnormal data. For a single abnormal data point, the corresponding milk container mass is determined by the direction and amount of change in the mass of the two normal milk containers before and after the abnormal data point. For consecutive abnormal data points, the adjustment of the milk container tilt angle based on the current pouring flow rate is paused, and the current milk container tilt angle is maintained until milk container masses that can change in the same direction are continuously obtained.

[0074] To avoid repeated changes in the tilt angle of the milk container caused by a single fluctuation in its mass, the current pouring flow rate obtained from multiple consecutive data collection points is compared. The tilt angle of the milk container is adjusted only when the current pouring flow rate is continuously below or continuously above the corresponding allowable range. The number of consecutive data collection points is determined based on the data collection time interval of the milk container weighing sensor and the pouring response time, ensuring that the consecutive data collection points cover at least one pouring response time.

[0075] The current pouring flow rate is compared with the pouring flow rate corresponding to the current trajectory point of the fusion injection. If the current pouring flow rate is lower than the allowable range, the milk pitcher tilt angle is increased, provided it does not exceed the corresponding allowable range. If the current pouring flow rate is higher than the allowable range, the milk pitcher tilt angle is decreased. After adjusting the milk pitcher tilt angle, the vertical distance between the spout and the liquid surface of the base cup is recalculated. If this vertical distance exceeds the allowable range corresponding to the fusion injection, the position of the milk pitcher arm is adjusted in a direction perpendicular to the liquid surface of the base cup to allow the milk foam to enter the coffee liquid and mix with it.

[0076] During the fusion injection process, the camera continuously monitors the liquid level in the cup according to the acquisition time. When the liquid level rises, the spout position in the remaining trajectory points of the fusion injection is adjusted away from the base cup liquid level according to the increase in the liquid level, so that the vertical distance between the spout and the base cup liquid level remains within the allowable range corresponding to the fusion injection. The adjusted spout position is converted into the joint positions of the milk cylinder arm through inverse kinematics calculation, and the corresponding trajectory points that have not yet been executed are replaced; the arrival time of the replaced trajectory points is still redistributed according to the original node end time. If the velocity or acceleration of any joint exceeds the allowable range recorded by the motion controller after redistribution, the remaining execution time of the fusion injection node is extended, and the arrival time of the trajectory points that have not yet been executed by the two robotic arms is recalculated according to the extended remaining execution time.

[0077] After the fusion injection ends, the base cup position, base cup tilt angle, milk cylinder position, milk cylinder tilt angle, liquid level in the cup, and pouring flow rate are read from the last control cycle of the fusion injection node. These data are used as the initial states of the two robotic arms in the pattern laying node. Once the pattern laying node begins execution, the base cup arm continuously adjusts the base cup position and tilt angle according to the trajectory segment corresponding to the pattern laying. The milk cylinder arm moves the milk cylinder according to the spout position and direction corresponding to the pattern laying, reducing the vertical distance between the spout and the liquid level in the base cup to the allowable range corresponding to the pattern laying.

[0078] It should be noted that when the pattern laying node reaches the point where the spout's movement direction changes, the base cup arm is changing the base cup's position and tilt angle according to the trajectory segment, while the milk cylinder arm simultaneously changes the spout's movement direction and the milk cylinder's tilt angle. As milk foam continues to flow out, the mass of the milk cylinder decreases, the liquid level in the cup rises, and the vertical distance between the spout and the liquid level in the base cup changes accordingly. Although the two robotic arms execute the trajectory segment according to a common time reference, their actual arrival times at the corresponding trajectory points may still be at different positions within the allowable time difference.

[0079] At this point, the camera detects that the formed pattern lines have both shifted in position and changed in width. For example, the pattern lines have shifted outward from the base cup and increased in width. This detection result cannot directly indicate which motion parameter should be adjusted, because the same change in pattern lines may correspond to the following different states: The actual change in the tilt angle of the base cup is slower than the actual change in the tilt angle of the milk pitcher, causing the spout to move to the corresponding position on the liquid surface of the base cup; the tilt angle of the milk pitcher and the mass of the milk pitcher change together, causing the current pouring flow rate to deviate from the pouring flow rate corresponding to the trajectory segment; the increase in the liquid level in the cup reduces the vertical distance between the spout and the liquid surface of the base cup; the consistency of the milk foam causes the same current pouring flow rate to form different pattern line widths; the time difference between the base cup arm and the milk pitcher arm reaching the corresponding trajectory point causes the spout's movement position to no longer correspond to the base cup position at the same moment.

[0080] The above situations may also occur simultaneously. Directly adjusting the spout position based on the pattern line position may bring the distance between the milk pitcher arm and the base cup arm close to the minimum allowable distance; directly reducing the milk pitcher tilt angle based on the pattern line width may interrupt subsequent pattern lines even if the current pouring flow rate itself does not exceed the allowable range; directly reducing the movement speed of one of the robotic arms may change the original time correspondence between the spout position, the base cup position, and the current pouring flow rate.

[0081] Therefore, in this embodiment, the camera continuously acquires images of the liquid surface in the base cup according to the acquisition time during the pattern laying process. The range of the liquid surface in the base cup is determined based on the cup rim contour, and the formed pattern lines are determined based on the color difference between the coffee liquid and the milk foam floating on the surface of the coffee liquid. Along the spout movement direction corresponding to the pattern laying, the position and width of the pattern lines formed at each acquisition time are determined respectively. The camera acquisition time is aligned with the acquisition time of the milk container weighing sensor and the control cycle of the two robotic arms according to a common time reference, so that the base cup position, base cup tilt angle, spout movement position, vertical distance between the spout and the liquid surface in the base cup, milk container tilt angle, current pouring flow rate, and the actual trajectory points reached by the two robotic arms are all recorded at the same acquisition time.

[0082] Before defining the established pattern lines, first determine the liquid surface area of ​​the base cup based on the cup rim outline, and then delete any image content outside this area. When the base cup is tilted, adjust the position and size of the liquid surface image based on the major and minor axes of the cup rim outline and the cup rim orientation, ensuring that the corrected cup rim outline corresponds to the cup rim facing upwards.

[0083] In the corrected base cup liquid surface image, image data of the coffee liquid area and milk foam area are read, and the boundary data is determined according to the color difference between the coffee liquid and milk foam in the pre-laundry calibration. The image area whose color difference with the coffee liquid reaches the boundary data and is continuous with the adjacent milk foam area is identified as the formed pattern line; the image area whose area is smaller than the area corresponding to the camera position detection error and is not continuous with the adjacent pattern line is deleted.

[0084] The extension direction of the pattern lines is determined by moving the spout along the direction corresponding to the pattern pattern, and the two edges of the pattern lines are read at each pattern position along a direction perpendicular to the extension direction. The midpoint between the two edges is determined as the pattern line position, and the distance between the two edges is determined as the pattern line width. When the spout obscures the corresponding pattern position, the last image of the liquid surface of the substrate cup that could completely detect the pattern line before the spout obscures it is read, and the detection is repeated after the spout leaves the corresponding pattern position. The pattern line position and pattern line width are not determined based on the image obscured by the spout.

[0085] When pre-calibrating the color difference between coffee liquid and milk foam in latte art, the same camera position, camera acquisition parameters, and lighting conditions of the latte art area are used as in actual latte art. Images of the base cup liquid surface containing only coffee liquid and images of the base cup liquid surface containing the milk foam area are acquired respectively. The image data of the coffee liquid area and the milk foam area are read, and the color difference that can separate the coffee liquid area and the milk foam area is selected as the boundary data.

[0086] During actual latte art production, if a continuous image region with adjacent milk foam areas cannot be obtained based on the boundary data, or if one side of the determined pattern line's edge is outside the base cup's liquid surface area, the pattern line position and width are not determined based on the base cup's liquid surface image. If the time covered by a continuous base cup liquid surface image for which the pattern line position and width cannot be determined is less than the pattern forming time, the current trajectory segment is maintained and base cup liquid surface images continue to be acquired; if the covered time reaches or exceeds the pattern forming time, the modification of trajectory points based on the pattern line position and width is paused, while the relative pose of the latte art, the current pouring flow rate, and the distance between the two robotic arms continue to be checked.

[0087] If the position and width of the pattern lines still cannot be determined after re-acquisition, reduce the tilt angle of the milk container until the current pouring flow rate is within the allowable range corresponding to stopping pouring. Stop both robotic arms from continuing to perform pattern laying and proceed to the anomaly handling node. When proceeding to the anomaly handling node, record the image of the liquid surface in the base cup where the position and width of the pattern lines cannot be determined, the camera acquisition time, the position of the base cup, the position of the liquid surface inside the cup, and the movement position of the spout.

[0088] The positions and widths of the formed pattern lines are compared with the corresponding line positions and widths in the current pattern position record. When either comparison result exceeds the corresponding allowable range, the executed trajectory points and corresponding base cup liquid surface images are sequentially read backward from the current acquisition time until the acquisition time when the pattern line position or pattern line width first exceeds the corresponding allowable range and remains in the excess state in subsequent continuously acquired base cup liquid surface images is found. This acquisition time is determined as the deviation start time. The time covered by the subsequent continuously acquired base cup liquid surface images is not less than the pattern forming time. When the pattern line position or pattern line width remains outside the corresponding allowable range within this time, the acquisition time when it first exceeds the corresponding allowable range is determined as the deviation start time; when the pattern line position or pattern line width in any base cup liquid surface image within the time re-enters the corresponding allowable range, the search continues to move backward to find the next acquisition time that exceeds the corresponding allowable range.

[0089] When the position and width of a pattern line first exceed their respective allowable ranges at different acquisition times, the start times for the deviations corresponding to the pattern line position and width are determined respectively. When generating trajectory point adjustment methods, states consistent with the direction of change of the pattern line position and states consistent with the direction of change of the pattern line width are read according to the corresponding deviation start times. When the position and width of a pattern line first exceed their respective allowable ranges within the allowable time difference, both are determined to have simultaneously exceeded their respective allowable ranges.

[0090] The data includes the base cup position, base cup tilt angle, spout movement position, vertical distance between the spout and the base cup liquid surface, milk cylinder tilt angle, milk cylinder mass, current pouring flow rate, liquid surface position in the cup, and the actual time taken for the two robotic arms to reach each trajectory point between the start of the deviation and the current acquisition time.

[0091] The above data are compared with the data recorded at the corresponding trajectory points at the same acquisition time. When the difference between the actual data and the data recorded at the corresponding trajectory points exceeds the corresponding allowable range, the acquisition time that first exceeds the corresponding allowable range is determined as the change time of the data; if the actual data only exceeds the corresponding allowable range at a single acquisition time and re-enters the corresponding allowable range at the next acquisition time, that single acquisition time is not determined as the change time.

[0092] If the time difference between the changes of two data points is greater than the allowable time difference, the data with the earlier acquisition time will change first; if the time difference between the changes of two data points does not exceed the allowable time difference, the two data points will be determined to have changed simultaneously. If the acquisition time intervals of camera data, milk container weighing sensor data, and robotic arm status data are different, making it impossible to directly compare two changes, the changes will be converted to the same control cycle according to a common time reference before determining the order of changes.

[0093] When determining whether the directions of change are consistent, comparisons are made between the increasing or decreasing direction of the actual data relative to the data recorded at the corresponding trajectory point, and between the moving direction of the pattern line position relative to the corresponding line position, or the increasing or decreasing direction of the pattern line width relative to the corresponding line width. If the correspondence between the direction of change of a certain state and the direction of change of the pattern line cannot be determined based on the pre-calibrated pattern, that state will not be used as the basis for this adjustment.

[0094] The order of changes in the base cup position, base cup tilt angle, spout movement position, and milk cylinder tilt angle relative to the corresponding trajectory point records before and after the deviation begins is compared. When the base cup position or base cup tilt angle changes in the same direction before the pattern line position, and the actual position of the spout in the latte art work area coordinate system is within the corresponding allowable range while the corresponding position of the spout on the base cup liquid surface exceeds the allowable range, the trajectory point of the base cup arm that has not yet been executed is taken as the position adjustment object; when both the base cup position and base cup tilt angle are within the corresponding allowable range, and the spout movement position changes in the same direction before the pattern line position, the trajectory point of the milk cylinder arm that has not yet been executed is taken as the position adjustment object.

[0095] When the current pouring flow rate changes before the pattern line width, and the vertical distance between the spout and the liquid surface of the base cup is within the corresponding allowable range, the tilt angle of the milk pitcher is used as the pouring adjustment target; when the current pouring flow rate is within the corresponding allowable range, but the liquid level in the cup rises, causing the vertical distance between the spout and the liquid surface of the base cup to decrease, and the pattern line width increases as the vertical distance decreases, the spout movement position is used as the height adjustment target; when both the current pouring flow rate and the vertical distance between the spout and the liquid surface of the base cup are within the corresponding allowable range, and the pattern line width changes with the actual moving speed of the milk pitcher arm along the pattern laying direction, the moving speed of the milk pitcher arm is used as the width adjustment target.

[0096] Calculate the time difference between the base cup arm and the milk pitcher arm when they actually reach the same trajectory point. If the time difference changes before the relative pose of the latte art exceeds the allowable deviation, and the actual movement paths of both robotic arms are within the corresponding allowable range, without changing the base cup position, base cup tilt angle, spout movement position, and milk pitcher tilt angle, adjust the arrival time of the trajectory point that the two robotic arms have not yet executed; if the time difference and the relative pose of the latte art change simultaneously, determine whether to adjust the trajectory point arrival time first or the movement path of the corresponding robotic arm first, based on the actual change order of the base cup position, base cup tilt angle, and spout movement position.

[0097] When the time difference and the relative pose of the latte art change simultaneously, first check the actual motion paths of the base cup arm and the milk cylinder arm. If the actual motion paths of both robotic arms are within their respective allowable ranges, but the time difference between the two robotic arms reaching the same trajectory point exceeds the allowable arrival time difference, first adjust the arrival time of the trajectory points that the two robotic arms have not yet executed; after adjustment, recalculate the relative pose of the latte art. If the relative pose of the latte art still exceeds the corresponding allowable deviation, then adjust the motion path of the corresponding robotic arm.

[0098] If any of the data related to the base cup position, base cup tilt angle, or spout movement position exceeds the corresponding allowable range before or simultaneously with the time difference mentioned above, the motion path of the corresponding robotic arm shall be adjusted first, and then the arrival time of the trajectory points that have not yet been executed by the two robotic arms shall be redistributed according to the adjusted motion path. No adjustment may cause the actual motion path of the other robotic arm, the joint position of the robotic arm, or the distance between the two robotic arms to exceed the corresponding requirements.

[0099] If the time difference between the changes of multiple states does not exceed the allowable time difference, the states are not considered to have a sequential order. Trajectory point adjustment methods that adjust only each state are generated, with each simultaneously changing state designated as the first to be adjusted, and the remaining simultaneously changing states designated as the next to be adjusted, generating trajectory point adjustment methods corresponding to different adjustment orders. Each trajectory point adjustment method undergoes predicted change and constraint checks; no trajectory point adjustment method is directly deleted based on an uncertain order of occurrence.

[0100] When both the position and width of the pattern line exceed their respective allowable ranges, the following changes are retained: the position of the base cup, the tilt angle of the base cup, and the movement position of the spout, all consistent with the direction of the pattern line position change. Additionally, the current pouring flow rate, the vertical distance between the spout and the liquid surface of the base cup, and the milk cylinder arm movement speed are retained, all consistent with the direction of the pattern line width change. States with inconsistent directions of change or those occurring later than the pattern line change are not considered for this adjustment. For cases where multiple states occur consecutively before the pattern line change and their directions all correspond to the pattern line change, trajectory point adjustment methods are generated, allowing for both adjusting only one state and adjusting multiple states sequentially according to their actual occurrence order.

[0101] When generating a trajectory point adjustment method that adjusts only one state, the change in that state relative to the corresponding trajectory point record value is read from the deviation start time to the current acquisition time, along with the corresponding change in the pattern line position or width. An acquisition time period is selected where the state changes continuously while other states that may affect the same pattern line are all within their corresponding allowable ranges. Based on the correspondence between the state change and the pattern line change within the acquisition time period, the correction amount required to move the pattern line position or width to the boundary of the corresponding allowable range is determined.

[0102] When there is no data collection period with only one continuous state change between the start of the deviation and the current data collection time, the correspondence between the change in state and the change in pattern lines under the same cup type, milk foam consistency, and adjacent milk container mass is read from the pre-laundry calibration data, and the correction amount is determined accordingly. The determined correction amount must not cause the base cup position, base cup tilt angle, spout movement position, milk container tilt angle, current pouring flow rate, or milk container arm movement speed to exceed the corresponding allowable range, and must not exceed the maximum allowable correction amount for a single instance obtained from the pre-laundry calibration.

[0103] When pre-calibrating the correspondence between changes in the state and changes in the pattern lines in the latte art, the same cup type, pattern position, and milk foam consistency are used. The base cup position, base cup tilt angle, spout movement position, vertical distance between the spout and the base cup liquid surface, milk pitcher tilt angle, current pouring flow rate, and milk pitcher arm movement speed (excluding the state to be calibrated) are all within the corresponding allowable ranges. The state to be calibrated is changed step by step. At each level, the pattern is laid out at the corresponding pattern position, and the change in that state relative to the recorded value of the corresponding trajectory point, as well as the change in the position or width of the corresponding pattern line relative to the recorded latte art action, are recorded.

[0104] The state change quantities and their corresponding pattern line changes are stored in ascending order of state change quantity. When the pattern line change quantity to be corrected is the same as a set of pre-set pattern line calibration data, the state change quantity corresponding to that set of pre-set pattern line calibration data is directly used as the correction quantity. When the pattern line change quantity to be corrected is located between two adjacent sets of pre-set pattern line calibration data, the corresponding state change quantity is determined based on the position of the pattern line change quantity to be corrected between the two adjacent sets of pattern line change quantities, and the determined state change quantity is used as the correction quantity.

[0105] When the change in the pattern lines that needs correction exceeds the coverage range of the pre-defined pattern calibration data, the correction amount is not determined further beyond the coverage range based on the aforementioned correspondence. Instead, the boundary state change amount, which is consistent with the direction of the pattern line change that needs correction, is adopted, and the correction amount is limited to the maximum allowable correction amount per instance. If the expected position or width of the pattern lines still cannot change to the corresponding allowable range after adopting the boundary state change amount, the corresponding trajectory point adjustment method is deleted.

[0106] When reading the pre-calibrated latte art data corresponding to the mass of adjacent milk containers, the correction amount is determined based on the correspondence between the state change and the pattern line change under the two adjacent milk container masses. Then, the final correction amount is determined based on the position of the current milk container mass between the two adjacent milk container masses. If the current milk container mass is the same as one of the milk container masses in the pre-calibrated latte art data, the pre-calibrated latte art data corresponding to that milk container mass is directly used.

[0107] When pre-calibrating the maximum permissible correction amount for a single latte art operation, starting from the latte art action record that has never been corrected, each time only one of the following states is changed: base cup position, base cup tilt angle, spout movement position, milk pitcher tilt angle, or milk pitcher arm movement speed, and the single change amount for that state is increased incrementally. For each level of single change amount, at least one pattern laying is performed at the corresponding pattern position, and the robotic arm joint position, relative latte art pose, current pouring flow, distance between the two robotic arms, pattern line position, and pattern line width are checked.

[0108] When a single change in quantity can cause the position or width of the pattern lines to change in the expected direction, and the robot arm joint position, the relative pose of the latte art, the current pouring flow rate, and the distance between the two robot arms all meet the corresponding requirements, the single change in quantity is retained as a usable single change in quantity. The single change in quantity continues to increase until the first occurrence of the following: the robot arm joint position exceeds the allowable range, the relative pose of the latte art exceeds the allowable deviation, the current pouring flow rate exceeds the allowable range, the distance between the two robot arms is less than the minimum allowable distance, or the position or width of the pattern lines does not change in the expected direction. The maximum usable single change in quantity before the first occurrence of these situations is determined as the maximum allowable single change in quantity for the corresponding state.

[0109] When calibrating the same state multiple times, the minimum value among the maximum permissible correction values ​​obtained from each calibration is selected as the final maximum permissible correction value for that state. Record the corresponding maximum permissible correction values ​​for different cup types, different milk foam consistency, different pattern positions, and different milk container weights.

[0110] When adjusting multiple states sequentially according to their actual occurrence order, first determine the first correction amount based on the state that changed earliest, and calculate the expected change in the position or width of the pattern lines after applying the first correction amount; then determine the correction amount for the next state based on the expected change amount that still does not fall within the corresponding allowable range, without repeatedly correcting multiple states according to the complete deviation amount.

[0111] After adopting the first correction amount, based on the correspondence between the earliest state change amount and the pattern line change amount, the expected change amount of the pattern line corresponding to the first correction amount is determined, and the expected change amount of the pattern line is subtracted from the pattern line change amount that needs to be corrected this time. The result after subtraction is used as the pattern line change amount that needs to be corrected in the next state.

[0112] If the result after subtraction is already within the corresponding allowable range, the next state will not be adjusted further; if the result after subtraction still exceeds the corresponding allowable range, the correction amount for the next state will be determined based on the correspondence between the change amount of the next state and the change amount of the pattern lines. If the expected change amount of the pattern lines corresponding to the first correction amount is greater than the change amount of the pattern lines that needs to be corrected this time, the first correction amount will be reduced until the expected position or width of the pattern lines reaches the boundary of the corresponding allowable range, and the next state will not be adjusted according to the original first correction amount.

[0113] For situations where three or more states are adjusted sequentially according to their actual occurrence order, after determining the correction amount for each state, the remaining pattern line changes that need to be corrected are updated in the same way until the expected pattern line position and the expected pattern line width both fall within the corresponding allowable range, or the correction amount calculation for all states has been completed.

[0114] Starting from the first trajectory point that has not yet been executed, the initial adjustment range is defined up to the next trajectory point where the spout's movement direction changes. For each trajectory point adjustment method, without driving the two robotic arms, the base cup position, base cup tilt angle, spout movement position, milk cylinder tilt angle, relative pose of the latte art, arrival time of the two robotic arms, and distance between the two robotic arms are calculated based on the adjusted trajectory points. Based on the correspondence between the change in spout movement position and the change in pattern line position from the start of the deviation to the current acquisition time, and the correspondence between the current pouring flow rate, the vertical distance between the spout and the liquid surface of the base cup, and the change in the milk cylinder arm movement speed and the pattern line width, the expected change direction of the pattern line position and pattern line width after adopting each trajectory point adjustment method is determined.

[0115] When determining the expected direction of change in the pattern line position, the change in the pattern line position caused by a unit data change is determined based on the changes in the base cup position, base cup tilt angle, and spout movement position (identified as adjustment objects) between the deviation start time and the current acquisition time, and the corresponding changes in the pattern line position. The correction amount in the trajectory point adjustment method is converted into the expected change in the pattern line position according to the aforementioned correspondence, and then added to the current pattern line position to obtain the expected pattern line position corresponding to the end position of the initial adjustment range.

[0116] When determining the expected direction of change in the pattern line width, the change in pattern line width caused by a unit data change is determined based on the changes in the data identified as adjustment targets among the current pouring flow rate, the vertical distance between the spout and the liquid surface of the base cup, and the milk cylinder arm movement speed between the deviation start time and the current acquisition time, as well as the corresponding change in pattern line width. The correction amount in the trajectory point adjustment method is converted into the expected change in pattern line width according to the aforementioned correspondence, and then added to the current pattern line width to obtain the expected pattern line width corresponding to the end position of the initial adjustment range.

[0117] For trajectory point adjustment methods that adjust multiple states sequentially according to their actual occurrence order, the expected pattern line positions and expected pattern line widths are updated sequentially according to the actual occurrence order of each state. When the correspondence cannot be determined from the data between the start time of the deviation and the current acquisition time, the expected direction of change is determined by reading data from the pre-calibrated latte art system for the same cup type, the same pattern position, the mass of adjacent milk containers, and the same milk foam consistency.

[0118] Delete trajectory point adjustment methods that would cause any robotic arm joint position to exceed the allowable range, the distance between two robotic arms to be less than the minimum allowable distance, the relative pose of the latte art to deviate further, or the current pouring flow change direction to be opposite to the required change direction of the pattern line width. Among the remaining trajectory point adjustment methods, first select the one that can simultaneously change both the pattern line position and pattern line width to the corresponding allowable range; if multiple trajectory point adjustment methods exist, select the one that changes the number of execution objects less; if the number of execution objects is the same, select the one that modifies the number of trajectory points less; if the number of modified trajectory points is still the same, select the one that increases the remaining execution time of the node less.

[0119] If the expected pattern line position or width corresponding to the end position of the initial adjustment range is still not within the corresponding allowable range, extend the adjustment range to the next trajectory point where the spout movement direction changes, and recheck the robotic arm joint position, the relative pose of the latte art, the current pouring flow, and the distance between the two robotic arms. Extend the trajectory range corresponding to only one spout movement direction at a time until both the expected pattern line position and the expected pattern line width are within the corresponding allowable range, or until all remaining trajectory points are included in the adjustment range.

[0120] Modify the unexecuted trajectory points according to the selected trajectory point adjustment method. When adjusting the unexecuted trajectory points of the base cup arm, simultaneously calculate the compensation amount for the movement position of the milk cylinder arm spout based on the correction amount of the base cup position or the base cup tilt angle, so that the adjusted latte art relative posture remains within the corresponding allowable deviation; when adjusting the unexecuted spout movement position of the milk cylinder arm, determine whether reverse compensation is needed for the unexecuted base cup position of the base cup arm based on the change in the distance between the spout movement position and the two robotic arms. The reverse compensation is that the base cup arm moves in a direction that increases the distance between the two robotic arms without changing the corresponding position of the spout on the liquid surface of the base cup.

[0121] When adjusting the milk container tilt angle, the correspondence between the tilt angle and the current pouring flow rate is redefined based on the change in the milk container's mass. Only the trajectory points where the current pouring flow rate has not yet been affected by the previous tilt angle adjustment are modified. The time required for the current pouring flow rate, detected by the milk container weighing sensor, to begin a stable change after the tilt angle changes is called the pouring response time. This response time is determined by comparing the moment the tilt angle changes with the moment the current pouring flow rate continuously changes. Before the pouring response time ends, the same milk container tilt angle is not adjusted again in the opposite direction.

[0122] After adjustments are made to the spout position, milk pitcher tilt angle, or milk pitcher arm movement speed, the time required for milk foam to flow from the spout and form a patterned line recognizable by the camera on the liquid surface of the base cup is called the pattern forming time. This pattern forming time is determined based on the timing of the control command change and the moment the camera detects a corresponding change in the position or width of the patterned line. Before the pattern forming time ends, the relative pose of the latte art, the current pouring flow rate, and the distance between the two robotic arms are continuously read, but the same adjustment object is not modified in reverse based on the incompletely formed patterned line.

[0123] When the current pouring flow rate begins to change steadily, at least one consecutive current pouring flow rate covering a pre-calibrated pouring response time should be read. When the consecutive current pouring flow rates all change in the direction corresponding to the adjustment of the milk container tilt angle, and the difference between adjacent current pouring flow rates does not change in opposite directions, the time of the first continuous change is determined as the time of continuous change of the current pouring flow rate.

[0124] When the position or width of the pattern lines begins to change, at least one continuous substrate cup liquid surface image covering a pre-calibrated pattern forming time is read. When the position or width of the pattern lines in the continuous substrate cup liquid surface images both change in the direction expected by the corresponding adjustment, the moment of acquiring the first image of the continuous change is determined as the moment when the position or width of the pattern lines begins to change accordingly.

[0125] If the actual pouring response time or pattern forming time determined during this latte art process exceeds the pre-calibrated range, the pre-calibrated data will not be immediately modified according to the current result. If the same change occurs repeatedly under the same cup type, the same milk foam consistency, and the mass of adjacent milk containers, the common range of the multiple change times will be used as the updated pouring response time or pattern forming time.

[0126] When multiple consecutive changes in time share a common range, the lower and upper boundaries of this common range are selected as the lower and upper boundaries of the updated pouring response time or pattern forming time range, respectively. When multiple consecutive changes in time do not share a common range, the pre-calibrated pouring response time or pattern forming time is not updated. Instead, each determined change time, along with the corresponding cup type, milk foam consistency, milk pitcher mass, milk pitcher tilt angle, and milk pitcher arm movement speed, is recorded and used in subsequent latte art projects.

[0127] The updated pouring response time or pattern forming time is only used for subsequent latte art under the same cup type, same milk foam consistency, and adjacent milk container mass; if the cup type, milk foam consistency, or milk container mass exceeds the corresponding range, the updated result is not used directly.

[0128] For each modified trajectory point, a new image of the liquid level in the base cup is acquired, and the base cup position, base cup tilt angle, spout movement position, liquid level in the cup, milk container tilt angle, current pouring flow rate, and the actual arrival time of the two robotic arms at the trajectory point are reread. When the direction of change of the pattern line position or pattern line width is consistent with the expected direction of change, the current trajectory point adjustment method is maintained, and the spout height in the unexecuted trajectory points is updated according to the latest liquid level in the cup. When the direction of change of the pattern line position or pattern line width is opposite to the expected direction of change, the execution of the unfinished adjustment amount is stopped, the sequence of state changes after the start of the deviation is re-determined, and the trajectory point adjustment method is regenerated.

[0129] When the pattern line position has entered the corresponding allowable range but the pattern line width still exceeds the corresponding allowable range, stop modifying the base cup position and the corresponding position of the spout on the base cup liquid surface. Only retain adjustments to the milk pitcher tilt angle, the vertical distance between the spout and the base cup liquid surface, or the milk pitcher arm movement speed. When the pattern line width has entered the corresponding allowable range but the pattern line position still exceeds the corresponding allowable range, keep the current pouring flow rate unchanged, and only adjust the base cup position, base cup tilt angle, or spout movement position. After both the pattern line position and pattern line width have entered the corresponding allowable range, recalculate the remaining trajectory points according to the state of the last adjusted trajectory point, so that the subsequent base cup position, base cup tilt angle, spout movement position, milk pitcher tilt angle, and arrival time change continuously, such as... Figure 2 As shown.

[0130] When all trajectory point adjustments cannot simultaneously satisfy the limitations of the robotic arm joint position, the allowable deviation of the latte art relative pose, the allowable range of the current pouring flow, and the minimum allowable distance, the moving speed of the base cup arm and the milk cylinder arm is reduced, while the current latte art relative pose remains unchanged, and the remaining trajectory points are recalculated. If a trajectory point that meets the conditions cannot be obtained after reducing the moving speed, the movement of the two robotic arms is stopped, the tilt angle of the milk cylinder is reduced until the milk foam stops flowing out, and the process is transferred to the abnormal handling node in the node diagram.

[0131] When transitioning to the anomaly handling node, record the reason for the deletion of each trajectory point adjustment method, the corresponding robotic arm joint position at the time of deletion, the distance between the two robotic arms, the relative pose of the latte art, the current pouring flow rate, the expected pattern line position, and the expected pattern line width. The starting state of the anomaly handling node is defined by the base cup position, base cup tilt angle, spout movement position, milk container tilt angle, liquid level in the cup, and milk container mass when the two robotic arms stop moving.

[0132] The exception handling node first checks the motion path from the initial state to the placement positions of the base cup and milk cylinder. If the motion path meets the joint position restrictions and minimum allowable distance of the robotic arms, it generates trajectory segments corresponding to the base cup arm and milk cylinder arm, and moves the base cup and milk cylinder to their corresponding placement positions. If the motion path does not meet the joint position restrictions or minimum allowable distance of the robotic arms, it keeps both robotic arms stopped and the gripper closed, outputs a manual handling prompt, and stops automatically generating motion trajectories.

[0133] After the last trajectory point of the pattern laying meets the action completion conditions, the position of the base cup, the tilt angle of the base cup, the position of the milk pitcher, the tilt angle of the milk pitcher, the liquid level in the cup, and the pouring flow rate corresponding to that trajectory point are used as the starting state of the pattern convergence node. During the execution of the pattern convergence node, the milk pitcher arm reduces the tilt angle of the milk pitcher and increases the vertical distance between the spout and the liquid surface of the base cup according to the trajectory segment corresponding to the pattern convergence. The base cup arm reduces the tilt angle of the base cup according to the trajectory segment corresponding to the same node. The camera continuously detects the position of the end of the formed pattern lines; if the end of the pattern line has not yet reached the position corresponding to the pattern convergence, the pouring continues and the current trajectory segment is executed. After the end of the pattern line reaches the position corresponding to the pattern convergence, the tilt angle of the milk pitcher is reduced according to the pouring flow rate corresponding to the pattern convergence.

[0134] When the spout position, milk pitcher tilt angle, and base cup tilt angle corresponding to the pattern convergence are all within the allowable deviation, the last trajectory point of the pattern convergence node is taken as the starting trajectory point of the tangent node. The milk pitcher arm crosses the already formed pattern body along the movement direction corresponding to the tangent, and continues to reduce the milk pitcher tilt angle during the movement; the base cup arm maintains the base cup position and continues to restore the base cup to the state of cup opening facing upwards. When the spout movement position is detected to deviate from the corresponding tangent position during the tangent process, the trajectory points of the milk pitcher arm that have not yet been executed are adjusted in the opposite direction of the deviation; when the already formed pattern body moves as a whole, the base cup arm position is adjusted according to the movement direction of the pattern body detected by the camera, and the spout movement position of the milk pitcher arm is adjusted simultaneously.

[0135] During the execution of fusion injection, pattern laying, pattern convergence, and tangent, the distances between the two robotic arm bodies, gripper, base cup, milk pitcher, coffee machine casing, and latte art station are checked in each control cycle. When any distance approaches the minimum allowable distance, the movement speed of the base cup arm and milk pitcher arm is reduced simultaneously, and the trajectory points that have not yet been executed are recalculated. When any distance is less than the minimum allowable distance, any robotic arm joint position exceeds the allowable range, the gripping force exceeds the allowable range, or the camera detects continuous slippage of the base cup or milk pitcher, the movement of the two robotic arms is stopped, the tilt angle of the milk pitcher is reduced until the milk foam stops flowing, and the process proceeds to the exception handling node in the node diagram.

[0136] After the milk pitcher arm reaches the tangent end position, the mass of the milk pitcher continuously collected by the milk pitcher weighing sensor is read and the current pouring flow rate is calculated. When the current pouring flow rate decreases to the allowable range corresponding to the stop pouring, the spout is outside the range of the base cup opening, and the tilt angle of the base cup is within the allowable range specified by the stop pouring node, the tangent node is marked as completed, and the stop pouring node is marked as allowed to execute.

[0137] In step four, after the stop-pouring node is marked as permissible, the milk cylinder arm reads the trajectory segment corresponding to the stop-pouring node, continues to decrease the milk cylinder tilt angle from the end of the tangent, and raises the spout position in a direction away from the base cup rim. The base cup arm maintains the base cup position according to the trajectory segment corresponding to the same node and adjusts the base cup tilt angle to the permissible range recorded by the stop-pouring node. The milk cylinder weighing sensor continuously records the milk cylinder mass according to the acquisition time, and divides the difference in milk cylinder mass between adjacent acquisition times by the corresponding time interval to obtain the current pouring flow rate. When the current pouring flow rate is still higher than the permissible range corresponding to the stop-pouring, the milk cylinder tilt angle continues to decrease, and the horizontal movement speed of the milk cylinder arm is reduced until milk foam stops flowing from the spout.

[0138] Once the current pouring flow rate is within the allowable range corresponding to the stop pouring, images of the spout and the base cup rim are captured by the camera. The spout position and the range of the base cup rim are determined based on the camera calibration parameters. When the spout is still within the range of the base cup rim, the milk cylinder arm maintains the milk cylinder tilt angle without increasing and continues to move the spout outside the range of the base cup rim. When the spout is already outside the range of the base cup rim, the current pouring flow rate is within the allowable range corresponding to the stop pouring, and the base cup tilt angle is within the allowable range recorded at the stop pouring node, the stop pouring node is marked as complete, and the base cup reset node and milk cylinder reset node are marked as allowed to execute.

[0139] After the base cup reset node begins execution, the base cup arm reads the base cup detection position calibrated during device installation. The base cup detection position is where the base cup rim faces upwards, the camera can completely capture the rim and internal pattern of the base cup, and the base cup will not contact the latte art worktable. Based on the current joint position of the base cup arm, the current position of the base cup, and the base cup detection position, a trajectory segment corresponding to the base cup reset node is generated. Following this trajectory segment, the tilt angle of the base cup is gradually reduced, while simultaneously moving the base cup to the base cup detection position. During the movement of the base cup arm, the clamping force detected by the force sensor on the clamp is read according to the control cycle. When the clamping force is below the lower limit, the clamp closure amount is increased; when the clamping force approaches the upper limit, the movement speed of the base cup arm is reduced until the base cup reaches the base cup detection position and maintains an upward-facing rim.

[0140] After the milk cylinder reset node begins execution, the milk cylinder arm reads the milk cylinder placement position calibrated during equipment installation. This placement position is one where the bottom of the milk cylinder can stably contact the latte art table, the spout is far from the base cup, and the milk cylinder arm can release the milk cylinder. Based on the current joint position of the milk cylinder arm, the current position of the milk cylinder, and the milk cylinder placement position, a trajectory segment corresponding to the milk cylinder reset node is generated. The milk cylinder is first restored to a spout-up position, and then moved to the milk cylinder placement position. When the bottom of the milk cylinder approaches the latte art table, the descent speed of the milk cylinder arm is reduced, and the force sensor on the milk cylinder arm clamp is read. When the force increases and the position of the milk cylinder no longer changes with the descent of the milk cylinder arm, it is determined that the bottom of the milk cylinder has contacted the latte art table, the descent of the milk cylinder arm is stopped, and the clamp remains closed.

[0141] During the execution of the base cup reset node and milk cylinder reset node, the distances between the base cup arm, milk cylinder arm, clamp, base cup, milk cylinder, coffee machine casing, and latte art station are checked cycle by cycle. If any distance approaches the minimum allowable distance, the movement speed of both the base cup arm and milk cylinder arm is reduced, and the unexecuted trajectory points are recalculated; if any distance is less than the minimum allowable distance, the movement of both robotic arms is stopped. If neither the base cup nor the milk cylinder slips and the recalculated trajectory segment can avoid collision, execution continues according to the recalculated trajectory segment; if the base cup or milk cylinder slips, the clamping force exceeds the allowable clamping force range, or the recalculation still cannot avoid collision, the clamp remains closed and the process transitions to the exception handling node.

[0142] After the base cup reaches the base cup detection position, images of the base cup are continuously acquired by the camera at the same acquisition time interval. The stability of the base cup and the liquid level inside the cup is determined based on the cup rim outline position and the position of the pattern inside the cup in adjacent base cup images. Stable liquid level means that the cup rim outline position remains unchanged in adjacent base cup images, and the movement distance of the pattern edge inside the cup is within the allowable range for finished pattern detection. If the liquid level inside the cup is not yet stable, the base cup arm maintains its current joint position and clamping force. After the liquid level inside the cup stabilizes, the base cup reset node is marked as complete, and the finished pattern detection node is marked as allowed to execute.

[0143] After the finished pattern detection node begins execution, a finished pattern image containing the complete cup rim and the pattern inside the cup is captured by a camera from above the base cup. The finished pattern image is captured after the base cup reaches the base cup detection position and the liquid level inside the cup remains stable. Based on the cup rim outline in the finished pattern image, the center, diameter, and orientation of the base cup rim are determined. Then, based on the cup shape data read in step one, the position and size of the finished pattern image are corrected to ensure that the center, diameter, and orientation of the cup rim in the finished pattern image are consistent with those used in the latte art recording.

[0144] Based on the color difference between the coffee liquid and the milk foam floating on its surface, the pattern lines formed by the milk foam in the finished design image are determined, and image content outside the cup rim area is deleted. The pattern position in the latte art recording is converted into the target pattern position in the finished design image according to the center and diameter of the base cup rim. Then, the formed pattern lines are compared with the target pattern position. The comparison includes the center position of the pattern lines, the width of the pattern lines, the continuity between adjacent pattern lines, the edge position of the main body of the pattern, the pattern termination position, and the tangent end position.

[0145] The center position of the pattern line is the middle position along the width direction of the pattern line; the width of the pattern line is the distance between the two edges of the milk foam coverage area at the same pattern position; continuity between adjacent pattern lines means that the end position of the previous pattern line connects with the start position of the next pattern line, or the distance between them is within the allowable range; the edge position of the main body of the pattern is the position of the outer contour of the pattern formed by multiple pattern lines. The allowable deviations corresponding to the center position of the pattern line, the width of the pattern line, the distance between adjacent pattern lines, the edge position of the main body of the pattern, the pattern termination position, and the tangent end position are obtained by conducting a pre-stitching test using the same cup shape, the same target pattern, and milk foam consistency within the allowable range, and then measuring the image of the completed pattern.

[0146] When the center position of the pattern lines, the width of the pattern lines, the spacing between adjacent pattern lines, the edge position of the main body of the pattern, the pattern termination position, and the tangent end position are all within their respective allowable deviations, the finished pattern inspection node is marked as complete. The milk cylinder arm gradually reduces the clamp closure amount until the milk cylinder is completely supported by the latte art operating table, and then moves the milk cylinder arm to the initial position of the milk cylinder arm calibrated during equipment installation; the base cup arm moves the base cup from the base cup inspection position to the base cup placement position calibrated during equipment installation, and after confirming that the bottom of the base cup has contacted the base cup placement position, gradually reduces the clamp closure amount, and moves the base cup arm to the initial position of the base cup arm calibrated during equipment installation.

[0147] When the finished pattern inspection result exceeds the corresponding allowable deviation, the corresponding latte art action record is determined based on the pattern position that exceeds the allowable deviation. When the center position of the pattern lines shifts as a whole in the same direction, the corresponding spout movement position in the latte art action record is corrected according to the shift direction and shift distance; when the width of the pattern lines is greater than the corresponding allowable range, the milk pitcher tilt angle recorded at the corresponding pattern position is reduced or the milk pitcher arm movement speed is increased; when the width of the pattern lines is less than the corresponding allowable range, the milk pitcher tilt angle recorded at the corresponding pattern position is increased or the milk pitcher arm movement speed is decreased; when the pattern termination position exceeds the corresponding allowable deviation, the spout movement end position corresponding to the pattern termination is corrected according to the distance between the pattern termination position and the target pattern position; when the tangent end position exceeds the corresponding allowable deviation, the movement distance and the stop pouring start time corresponding to the tangent are corrected.

[0148] When correcting the pitcher tilt angle and pitcher arm movement speed, the current pouring flow rate and the width of the formed pattern lines recorded during the pattern laying process are read. If the current pouring flow rate is within the corresponding allowable range but the pattern line width still exceeds the corresponding allowable deviation, the pitcher arm movement speed is corrected; if the current pouring flow rate exceeds the corresponding allowable range and the pattern line width exceeds the corresponding allowable deviation in the same direction, the pitcher tilt angle is corrected. Each correction amount does not exceed the maximum allowable correction amount obtained from the pre-calibrated latte art. The corrected spout movement position, pitcher tilt angle, pitcher arm movement speed, the end position of the spout movement corresponding to the pattern termination or the movement distance corresponding to the tangent, and the start time of stopping pouring are written into the latte art action record corresponding to the pattern number, cup type number, and milk foam consistency.

[0149] When writing the corrected data, retain the original latte art action record and record the corrected data together with the correction time, finished pattern inspection results, milk container quality, milk foam temperature, milk foam consistency, and liquid level in the cup. The corrected latte art action record will first be used as the latte art action record to be verified, and will be used for subsequent latte art under the same pattern number, cup type number, and milk foam consistency.

[0150] If the finished pattern of the subsequent latte art is within the corresponding allowable deviation, the corrected latte art action record will be marked as verified. If the finished pattern detection result exceeds the corresponding allowable deviation further along the deviation direction before correction, the corrected latte art action record will be stopped and the original latte art action record will be restored. If the finished pattern detection result still exceeds the corresponding allowable deviation but does not deviate further, the corrected latte art action record and the corresponding detection result will be retained, and the same correction direction will not be continuously written after the same latte art is completed.

[0151] When the finished pattern only has missing pattern lines or the spacing between adjacent pattern lines exceeds the allowable range, the pattern laying or pattern termination portion corresponding to the missing position is determined. Missing pattern lines refer to the presence of pattern lines at the target pattern position, but no milk foam coverage area is detected at the corresponding position in the finished pattern image; local restoration means keeping other existing pattern lines unchanged and only adding milk foam at the missing pattern line position or between adjacent pattern lines. The milk container's mass, milk foam temperature, and milk foam consistency are read from the milk container weighing sensor, and the base cup is checked to see if there is still remaining space to add milk foam based on the liquid level position inside the cup.

[0152] Based on the location of the missing pattern lines, the width of the pattern lines, and the movement path of the spout corresponding to the partial restoration, the corresponding pouring flow rate and movement time are read from the latte art action record. Multiplying the pouring flow rate by the movement time yields the estimated reduction in milk container mass required for partial restoration. Adding this estimated reduction in milk container mass to the maximum detection error of the milk container weighing sensor gives the minimum milk container mass required to complete the partial restoration.

[0153] When the milk container mass is not lower than the minimum milk container mass, the milk foam temperature and consistency are within the corresponding allowable ranges, the liquid level in the cup does not reach the highest allowable liquid level position recorded in the cup shape data, and the distance between the missing pattern line position and the edge of the cup rim is greater than the allowable distance for local recovery edge, a trajectory segment corresponding to local recovery is generated within the anomaly handling node. The allowable distance for local recovery edge is the minimum distance required for the spout to be located within the rim of the base cup and for the supplemented pattern line not to extend beyond the rim area during local recovery. The allowable distance for local recovery edge is determined based on the spout's external dimensions, camera position detection error, the width of the pattern line corresponding to local recovery, and the distance required for the spout to stop moving, and is calibrated separately from the minimum allowable distance used when inspecting for collisions with the robotic arm.

[0154] The trajectory segment corresponding to the partial restoration uses the start and end positions of the missing pattern lines as the spout movement path. The pouring flow rate is determined by the difference in pattern line width obtained from the current finished pattern detection. Furthermore, based on the adjustment relationship between the milk pitcher tilt angle, milk pitcher arm movement speed, and pattern line width in step three, the corresponding milk pitcher tilt angle and milk pitcher arm movement speed for the partial restoration are determined.

[0155] During partial restoration, the base cup arm keeps the base cup at the base cup detection position, while the milk cylinder arm re-grabs the milk cylinder from its placement position and moves the spout to the location where the pattern line is missing, following the trajectory segment corresponding to the partial restoration. The milk cylinder arm gradually increases the tilt angle of the milk cylinder until the current pouring flow reaches the allowable range corresponding to the partial restoration, and then moves the spout from the start to the end position of the missing pattern line. The camera continuously detects the position and width of the supplemented pattern line; when the supplemented pattern line reaches the target pattern position and the pattern line width is within the corresponding allowable deviation, the tilt angle of the milk cylinder is immediately reduced and the spout is moved out of the base cup opening range. Then, the process of stopping pouring, resetting the milk cylinder, and detecting the finished pattern is repeated. If partial restoration has already been performed but the finished pattern detection result still exceeds the corresponding allowable deviation, partial restoration will not be repeated.

[0156] When the main body of the pattern shifts significantly, existing pattern lines need to be removed, the liquid level in the cup reaches the maximum allowable level, the remaining milk container mass is insufficient, the milk foam temperature or consistency exceeds the corresponding allowable range, or the finished pattern detection result still exceeds the corresponding allowable deviation after partial restoration, a safe exit is executed. Safe exit means stopping further pouring and pattern correction, moving the base cup and milk container to a safe placement position, and stopping both robotic arms from continuing the latte art action. If the current trajectory can still avoid collisions, the milk container arm moves the milk container to its placement position and releases the clamp, and the base cup arm moves the base cup to its placement position and releases the clamp; if the current trajectory cannot avoid collisions, the base cup or milk container slides, or any clamping force exceeds the allowable clamping force range, both robotic arms stop moving while keeping the clamp closed, the finished pattern detection node is marked as a failure, and the base cup position, milk container position, joint positions of the two robotic arms, clamping force, and finished pattern detection result at the time of the anomaly are recorded.

[0157] As a specific implementation example, the control cycle of the two robotic arm motion controllers is set to ten milliseconds. The camera acquires an image of the liquid level in the base cup every twenty milliseconds, and the milk container weighing sensor acquires the mass of the milk container every ten milliseconds. The allowable difference in arrival time between the two robotic arms at their corresponding trajectory points is set to twenty milliseconds. After receiving the same start-time command, the camera, milk container weighing sensor, and two robotic arm motion controllers record data according to the elapsed time from their self-timed start point. When the camera acquisition time falls between two adjacent control cycles, the data at the corresponding acquisition time is determined based on the base cup position, base cup tilt angle, and spout movement position in the two adjacent control cycles.

[0158] During the execution of the pattern laying node corresponding to this order instruction, the camera continuously detected that the position of the pattern lines moved outward from the base cup, and the width of the pattern lines was greater than the width of the pattern lines recorded in the latte art action. After reading the liquid surface image of the base cup, it was determined that the position of the pattern lines exceeded the corresponding allowable range for multiple consecutive acquisition moments, and the acquisition moment of the first consecutive exceedance was determined as the deviation start moment corresponding to the position of the pattern lines; the width of the pattern lines began to exceed the corresponding allowable range for the next acquisition moment, and that next acquisition moment was determined as the deviation start moment corresponding to the width of the pattern lines.

[0159] By reading data before and after the start of the two deviations, it is determined that the tilt angle of the base cup changes in the direction that causes the pattern lines to shift outward before the position of the pattern lines. Since the actual position of the spout in the latte art work area coordinate system is within the corresponding allowable range, the trajectory points of the base cup arm that have not yet been executed are used as the target for position adjustment. Simultaneously, since the current pouring flow rate is within the corresponding allowable range, the increase in the liquid level inside the cup reduces the vertical distance between the spout and the liquid surface of the base cup, and the width of the pattern lines increases as the vertical distance decreases. Therefore, the spout movement position is used as the target for height adjustment.

[0160] Adjustment methods are generated for three scenarios: adjusting only the unexecuted trajectory points of the base cup arm, adjusting only the spout movement position of the milk cylinder arm, and adjusting the unexecuted trajectory points of the base cup arm before adjusting the spout movement position. Starting from the first unexecuted trajectory point, the initial adjustment range is defined up to the next trajectory point where the spout movement direction changes. The correction amount for the base cup tilt angle is determined based on the correspondence between the change in the base cup tilt angle and the change in the pattern line position from the start of the deviation to the current acquisition time. The correction amount for the spout movement position is determined based on the correspondence between the change in the vertical distance between the spout and the liquid surface of the base cup and the change in the pattern line width.

[0161] Without driving both robotic arms, the joint positions of the robotic arms, the relative pose of the latte art, the distance between the two robotic arms, the position of the expected pattern lines, and the width of the expected pattern lines were calculated for each of the three trajectory point adjustment methods. Adjusting only the unexecuted trajectory points of the base cup arm could not bring the expected pattern line width into the corresponding allowable range, so it was deleted. Adjusting only the spout position of the milk pitcher arm could not bring the expected pattern line position into the corresponding allowable range, so it was deleted. Adjusting the unexecuted trajectory points of the base cup arm first, and then adjusting the spout position of the milk pitcher arm, could make both the expected pattern line position and the expected pattern line width change into the corresponding allowable range, and the joint positions of the robotic arms, the relative pose of the latte art, and the distance between the two robotic arms all met the requirements. Therefore, this trajectory point adjustment method was selected.

[0162] After modifying the unexecuted trajectory points according to the selected trajectory point adjustment method, the liquid surface image of the base cup is re-acquired after each modified trajectory point is executed. When the pattern line position first enters the corresponding allowable range, the modification of the base cup position and the corresponding position of the spout on the base cup liquid surface is stopped, and only the vertical distance between the spout and the base cup liquid surface continues to be adjusted according to the latest liquid surface position in the cup; after the pattern line width also enters the corresponding allowable range, the remaining trajectory points in the pattern laying nodes are recalculated according to the state of the last adjusted trajectory point, so that the base cup position, base cup tilt angle, spout movement position, milk pitcher tilt angle, and arrival time change continuously.

[0163] The above formulas are all dimensionless calculations. The formulas are derived from software simulations based on a large amount of collected data to obtain the most recent real-world results. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0164] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, in the form of a computer program product.

[0165] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and inventive constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0166] In addition, the functional modules in the various embodiments of this application can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0167] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0168] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A node arrangement control method for a double-arm latte art motion sequence process, characterized in that, include: Read the position and width of the pattern lines during the pattern laying process, and determine the starting time of the deviation when either exceeds the corresponding allowable range in the pattern laying action record; The system reads actual data from the start of the deviation during the pattern laying process to the current acquisition time, including the base cup position, base cup tilt angle, spout movement position, vertical distance between the spout and the liquid surface of the base cup, milk cylinder tilt angle, milk cylinder mass, current pouring flow rate, liquid surface position in the cup, and the actual arrival time of each trajectory point by the two robotic arms. This data is then compared with the corresponding trajectory point records at the same acquisition time to determine the change time and direction of each actual data. The change sequence is determined based on the change time. States with inconsistent change directions or that occur later than the change of the pattern lines are not used as the basis for this adjustment. When the base cup position or base cup tilt angle changes in the same direction before the pattern line position, and the actual position of the spout in the latte art work area coordinate system is within the corresponding allowable range while the corresponding position of the spout on the liquid surface of the base cup exceeds the allowable range, the trajectory points that the base cup arm has not yet executed are used as the objects for position adjustment. When the position of the base cup and the tilt angle of the base cup are both within the corresponding allowable range, and the position of the spout changes in the same direction before the position of the pattern line, the trajectory point that the milk cup arm has not yet executed is taken as the position adjustment object. When the current pouring flow rate changes before the width of the pattern line, and the vertical distance between the spout and the liquid surface of the base cup is within the corresponding allowable range, the tilt angle of the milk pitcher is used as the object of pouring adjustment. When the current pouring flow rate is within the corresponding allowable range, and the liquid level in the cup rises, reducing the vertical distance between the spout and the liquid level in the base cup, and the width of the pattern lines increases as the vertical distance decreases, the spout movement position is used as the height adjustment target. When the current pouring flow rate and the vertical distance between the spout and the liquid surface of the base cup are both within the corresponding allowable range, and the width of the pattern lines changes with the actual moving speed of the milk cylinder arm along the pattern laying direction, the moving speed of the milk cylinder arm is used as the width adjustment object to generate the trajectory point adjustment method. Read the change in the state of the object to be adjusted relative to the recorded value of the corresponding trajectory point and the change in the position or width of the corresponding pattern line relative to the record of the latte art action. Determine the correction amount based on the correspondence between the change amount and the change amount of the pattern line. Determine the expected change direction of the adjustment method of each trajectory point and perform constraint checks. Select a trajectory point adjustment method that simultaneously changes the position and width of the pattern lines within the corresponding allowable range and passes the constraint check, and modify the trajectory points that have not yet been executed according to the correction amount.

2. The node arrangement control method for the double-arm latte art motion sequence process according to claim 1, characterized in that: The trajectory segments for generating pattern laying nodes for the base cup arm and the milk cylinder arm are respectively. The trajectory segments are multiple trajectory points arranged in sequence when the robotic arms execute the pattern laying nodes. The trajectory points of the two robotic arms are recorded using a common time reference. The common time reference is the same timing start point and the same time unit used by the motion controllers of the two robotic arms. The relative pose of the latte art is determined jointly based on the trajectory points corresponding to the same arrival time under the common time reference, and the relative pose of the latte art is recorded to the corresponding trajectory points.

3. The node arrangement control method for the double-arm latte art motion sequence process according to claim 2, characterized in that: Based on the base positions of the two robotic arms, the positional relationship between the robotic arm ends and the gripper, and the positional relationship between the gripper and the base cup or milk container, the target positions and target orientations of the base cup and milk container in the pattern laying nodes are converted into the target positions and target orientations of the corresponding robotic arm ends. Inverse kinematics calculations are performed on the target position and target orientation at the end of the robotic arm. Joint positions that are out of the allowable range, where the robotic arm collides with a fixed component, or where two robotic arms interfere with each other are deleted. Among the remaining joint positions, joint positions with smaller differences from the current joint position are selected. The selected joint positions are connected according to the action sequence, and trajectory points are generated using cubic B-spline interpolation.

4. The node arrangement control method for the double-arm latte art sequence process according to claim 3, characterized in that: Calculate the shortest time required for the base cup arm and milk cylinder arm to execute the pattern laying node, and take the longer time as the node duration. Keep the geometry of the motion path unchanged, and extend the arrival time of the trajectory point of the arm with the shorter execution time according to the node duration, so that the two robotic arms start the pattern laying node simultaneously under a common time reference and reach the node end position within the allowable difference in arrival time. Check the distance between the two robotic arm bodies, fixtures, base cups, milk cylinders, and fixed parts point by point, and recalculate the relative pose of the pattern laying according to the trajectory points. If the joint movement restrictions, minimum allowable distance, and allowable deviation of the relative pose of the pattern laying cannot be met at the same time, the pattern laying node is marked as execution failure.

5. The node arrangement control method for the double-arm latte art motion sequence process according to claim 4, characterized in that: During the pattern laying process, the camera continuously captures images of the liquid surface in the base cup according to the acquisition time, determines the range of the liquid surface in the base cup based on the outline of the cup mouth, and determines the pattern lines that have been formed based on the color difference between the coffee liquid and the milk foam. The position and width of the pattern lines are determined by moving the spout along the direction corresponding to the pattern pattern. The camera acquisition time, the acquisition time of the milk bottle weighing sensor, and the control cycle of the two robotic arms are aligned according to a common time reference so that the same acquisition time corresponds to the actual data and the actual trajectory points reached by the two robotic arms.

6. The node arrangement control method for the double-arm latte art motion sequence process according to claim 1, characterized in that: Once the position or width of the pattern line exceeds the corresponding allowable range for the first time, read the subsequently acquired liquid surface images of the substrate cup; When the time covered by the liquid surface image of the base cup is not less than the pattern forming time, and the position or width of the pattern lines both exceed the corresponding allowable range, the acquisition time at which the first excess of the corresponding allowable range is acquired is determined as the deviation start time; the actual data is compared with the corresponding trajectory point record value at the same acquisition time, and the acquisition time at which the difference first exceeds the corresponding allowable range is determined as the change time. The change time is uncertain only when the actual data exceeds the corresponding allowable range at a single acquisition time and re-enters the corresponding allowable range at the next acquisition time; the change order is determined according to the time difference between each change time and the allowable time difference, or the changes occur simultaneously. When the acquisition time intervals are different, the change order is determined after conversion to the same control cycle according to a common time reference.

7. The node arrangement control method for the double-arm latte art motion sequence process according to claim 6, characterized in that: If the time difference between the two robotic arms actually reaching the same trajectory point occurs before the relative pose of the latte art changes, the arrival time of the trajectory point that the two robotic arms have not yet executed is adjusted; if the time difference and the relative pose of the latte art change simultaneously, it is determined whether to adjust the arrival time of the trajectory point first or the movement path of the corresponding robotic arm first, based on the actual movement path of the two robotic arms. When multiple data points identified as adjustment targets change simultaneously, trajectory point adjustment methods are generated, one for adjusting only each data point and the other for adjusting multiple data points sequentially according to different adjustment orders. When multiple data points identified as adjustment targets change sequentially, trajectory point adjustment methods are generated, one for adjusting only one data point and the other for adjusting multiple data points sequentially according to the actual occurrence order.

8. The node arrangement control method for the double-arm latte art motion sequence process according to claim 7, characterized in that: Read the change in the state of the object to be adjusted relative to the recorded value of the corresponding trajectory point and the change in the pattern line; select a collection time period in which the state changes continuously and other states that may affect the same pattern line are all within the corresponding allowable range, and determine the correction amount according to the correspondence between the state change amount and the pattern line change amount within the collection time period; if the collection time period does not exist, read the correspondence under the same cup type, the same milk foam consistency and the quality of adjacent milk containers from the pre-paste flower calibration data to determine the correction amount, and limit the correction amount according to the maximum allowable correction amount for a single time; When adjusting multiple states sequentially according to their actual occurrence order, the first correction amount and its corresponding expected change amount of the pattern lines are determined based on the earliest state that changed. The expected change amount of the pattern lines is then subtracted from the change amount of the pattern lines that need to be corrected this time, and the correction amount of the next state is determined based on the result after subtraction.

9. The node arrangement control method for the double-arm latte art motion sequence process according to claim 8, characterized in that: Starting from the first trajectory point that has not yet been executed, the trajectory point up to the next point where the spout movement direction changes is determined as the initial adjustment range. For each trajectory point adjustment method, without driving the two robotic arms, the expected pattern line position and expected pattern line width corresponding to the end position of the initial adjustment range are determined. Trajectory point adjustment methods that would cause any robotic arm joint position to exceed the allowable range, the distance between the two robotic arms to be less than the minimum allowable distance, the relative pose of the latte art to deviate further, or the current pouring flow change direction to be opposite to the required change direction of the pattern line width are deleted. Among the remaining trajectory point adjustment methods, the trajectory point adjustment method is selected in sequence based on whether the pattern line position and pattern line width can be changed to the corresponding allowable range simultaneously, the number of execution objects to be changed, the number of modified trajectory points, and the increase in the remaining execution time of the node. The unexecuted trajectory points are modified according to the selected trajectory point adjustment method. After each modified trajectory point is executed, the pattern line position and pattern line width are redefined. When the change direction is opposite to the expected change direction, the execution of the unfinished adjustment amount is stopped, and a new trajectory point adjustment method is generated.

Images