Double-arm cooperative control method for double-skin milk production

Abstract

The application relates to the technical field of mechanical arm control, and particularly discloses a double-arm cooperative control method for double-skin milk production, which comprises the following steps: obtaining a predicted three-dimensional coordinate of a bowl-gripping mechanical arm gripper; obtaining an actual position coordinate of the bowl-gripping mechanical arm gripper; calculating a position deviation vector according to the predicted three-dimensional coordinate; generating a trajectory correction instruction according to the position deviation vector and updating the actual position coordinate; driving a discharging machine to discharge according to the position relationship between the actual position coordinate and a second three-dimensional coordinate of a discharge port; obtaining an opening and closing angle of a steaming barrel cover; if the opening and closing angle is smaller than a preset lower limit angle threshold and a discharging completion state signal is obtained, driving an opening cover mechanical arm to act; and if the opening and closing angle is greater than a preset upper limit angle threshold, driving the bowl to move into the steaming barrel. The double-arm cooperative control method for double-skin milk production breaks the execution barriers between the mechanical arms and between the mechanical arms and surrounding equipment, realizes seamless connection in time and accurate alignment in space of multiple components.

Description

Technical Field

[0001] This invention relates to the field of robotic arm control technology, and more specifically, to a method for coordinated control of two arms in the making of double-skin milk pudding. Background Technology

[0002] In the automated production process of double-skin milk pudding, the smooth connection of multiple processes directly determines the quality of the final product and production efficiency.

[0003] However, due to the lack of close temporal coordination between the various components, the robotic arm cannot dynamically correct its trajectory based on the actual position of the discharge port or the opening of the cooking tank during movement. It can only rely on preset fixed coordinates for blind positioning. This blindness in spatial positioning further leads to a lack of strict status confirmation mechanisms between devices, making conflicts between various action links highly likely. For example, in the actual feeding and cooking process, when the robotic arm responsible for picking up the bowl moves the empty bowl under the discharge machine, if there is a slight deviation in position that cannot be corrected in time, the discharge machine will still spray raw materials according to the fixed program, causing a large-scale spillage of milk. At the same time, if the lid of the cooking tank has not been fully opened by another robotic arm, and the robotic arm picking up the bowl forcibly sends the bowl full of raw materials into the tank, it will directly hit the lid, causing serious mechanical collision and equipment damage. Summary of the Invention

[0004] In order to overcome the defects of the existing technology, the present invention provides a dual-arm coordinated control method for making double-skin milk, which aims to solve the problems in the above-mentioned existing technology.

[0005] The technical solution adopted by this invention to solve its technical problem is: a method for coordinated control of two arms in the making of double-skin milk pudding, comprising the following steps: S1: Obtain the initial image at the start of double-skin milk making, and get the first three-dimensional coordinates of the mechanical arm that grips the bowl and the second three-dimensional coordinates of the discharge port; S2: Based on the first three-dimensional coordinates, the Kalman filter algorithm is used to predict the trajectory and obtain the predicted three-dimensional coordinates of the gripper of the robotic arm that picks up the bowl at the current moment; S3: Obtain the current actual position coordinates of the gripper of the robotic arm that picks up the bowl, and calculate the position deviation vector based on the predicted three-dimensional coordinates at the current moment; S4: Generate trajectory correction instructions based on the position deviation vector, and update the actual position coordinates of the gripper of the robotic arm that picks up the bowl; S5: Generate an injection trigger signal based on the positional relationship between the actual position coordinates and the second three-dimensional coordinates; S6: Drive the discharge machine to inject material into the bowl through the discharge port according to the material injection trigger signal, and obtain the material injection completion status signal; S7: Obtain the opening angle of the steaming bucket lid. If the opening angle is less than the preset lower limit angle threshold and the filling completion status signal is obtained, drive the lid opening robot arm to open the steaming bucket lid. If the opening angle is greater than the preset upper limit angle threshold, drive the bowl gripping robot arm to move into the steaming bucket.

[0006] It is worth noting that, in step S1, the steps of obtaining the first three-dimensional coordinates of the robotic arm that grips the bowl and the second three-dimensional coordinates of the discharge port include: Obtain the video stream of double-skin milk pudding from the start of its production, as well as the pre-calibrated camera intrinsic parameter matrix; Each frame in the video stream is separated into a color image frame stream and a depth image frame stream; The YOLOv8 algorithm is used to process all color image frame streams to obtain the first bounding box diagram of the gripper of the bowl-grabbing robotic arm corresponding to each frame image. The YOLOv8 algorithm is used to process the second bounding box diagram of the outlet of the color image frame stream corresponding to the first frame image. Extract the first center point of the first boundary map and the second center point of the second boundary map to obtain the first pixel coordinates and the second pixel coordinates; Based on the first pixel coordinates and the second pixel coordinates, extract the first depth value corresponding to the first pixel coordinates and the second depth value corresponding to the second pixel coordinates from the depth image frame stream corresponding to the color image frame stream; Extract the focal length parameter and optical center coordinate parameter from the camera intrinsic parameter matrix; After performing spatial coordinate system transformation on the first pixel coordinates based on the focal length parameter and the optical center coordinate parameter, the first three-dimensional coordinates of the gripper of the bowl-grabbing robotic arm corresponding to each frame image are obtained by combining the first depth value. After performing spatial coordinate system transformation on the second pixel coordinates based on the focal length parameter and the optical center coordinate parameter, the second three-dimensional coordinates of the discharge port are obtained by combining the second depth value.

[0007] Preferably, in step S2, the step of obtaining the predicted three-dimensional coordinates of the gripper of the robotic arm for picking up the bowl at the current moment includes: Obtain the first three-dimensional coordinates corresponding to all frames before the current moment; combine the first three-dimensional coordinates in chronological order to construct a historical coordinate sequence; calculate the difference between adjacent first three-dimensional coordinates for the historical coordinate sequence to obtain the initial velocity vector; construct the initial state vector based on the last first three-dimensional coordinate in the historical coordinate sequence and the corresponding initial velocity vector. Obtain a pre-set state transition matrix, and perform matrix multiplication on the initial state vector according to the state transition matrix to obtain the predicted state vector; A first intermediate covariance matrix is ​​obtained by performing matrix multiplication on the state transition matrix, the transpose of the state transition matrix, and the initial error covariance matrix. A second intermediate covariance matrix is ​​obtained by performing matrix multiplication on the prediction error covariance matrix, the observation matrix, and the transpose of the observation matrix. A third intermediate covariance matrix is ​​obtained by performing matrix addition on the second intermediate covariance matrix and the observation noise covariance matrix. Finally, the inverse of the third intermediate covariance matrix is ​​obtained. The predicted state vector is obtained by performing matrix multiplication on the observation matrix; the second three-dimensional coordinates corresponding to the frame image of the previous moment are obtained as the observation vector; the observation vector and the predicted observation vector are subtracted by matrix to obtain the residual vector. The Kalman gain matrix is ​​obtained by performing matrix multiplication on the prediction error covariance matrix, the transpose of the observation matrix, and the inverse matrix; the state correction vector is obtained by performing matrix multiplication on the residual vector based on the Kalman gain matrix. The predicted state vector and the corrected state vector are added together to obtain the updated state vector; the position component is extracted from the updated state vector to obtain the predicted three-dimensional coordinates at the current time.

[0008] Optionally, in step S3, the step of calculating the position deviation vector includes: Obtain the current actual position coordinates of the gripper of the robotic arm that grips the bowl. If the Euclidean distance between the actual position coordinates and the predicted 3D coordinates at the current moment is greater than a set first distance threshold, then add the predicted 3D coordinates as the first 3D coordinates to the historical coordinate sequence, and calculate the coordinate differences between the actual position coordinates and the predicted 3D coordinates on the x-axis, y-axis and z-axis respectively to obtain the position deviation vector.

[0009] Specifically, in step S4, the step of updating the actual position coordinates of the gripper of the robotic arm that picks up the bowl includes: Input the position deviation vector into the PID controller; The PID controller uses the proportional control gain, integral control gain and derivative control gain to perform proportional-integral-derivative summation on the position deviation vector to generate trajectory correction instructions. The trajectory correction command is sent to the motor driver; the motor driver controls the servo motor of the bowl-gripping robotic arm to run according to the trajectory correction command, and then obtains the actual position coordinates of the gripper of the bowl-gripping robotic arm at the next time node.

[0010] It is worth noting that in step S5, the step of generating the injection trigger signal includes: Obtain the actual position coordinates of the gripper of the robotic arm that picks up the bowl; obtain the second three-dimensional coordinates of the discharge port; calculate the relative distance between the actual position coordinates and the second three-dimensional coordinates using the Euclidean distance formula; Determine whether the relative distance is less than or equal to a preset second distance threshold; if the relative distance is less than the preset distance threshold, generate an injection trigger signal.

[0011] Preferably, in step S6, the step of obtaining the injection completion status signal includes: A valve opening command is generated based on the injection trigger signal; The discharge valve at the discharge port is opened by the valve opening command. After the discharge valve is opened, a countdown begins, and a discharge completion status signal is generated after the countdown ends.

[0012] Optionally, in step S7, the step of obtaining the opening angle of the cooking tank lid includes: Rotary pulse data is collected by a rotary encoder at the rotating shaft of the cooking pot lid. The opening and closing angle is obtained by multiplying the rotary pulse data with a preset pulse angle conversion coefficient.

[0013] The beneficial effects of this invention are as follows: In the dual-arm collaborative control method for making double-skin milk pudding, addressing the issues of trajectory deviation during the automated production process leading to inaccurate filling, and the lack of coordination between the filling and the opening / closing state of the steaming bucket lid causing poor pot entry, this solution obtains the three-dimensional coordinates of the robotic arm holding the bowl and the outlet by acquiring an initial image. It then uses a Kalman filter algorithm to predict the trajectory and calculates the deviation based on the actual position, generating a correction command to accurately update the robotic arm position. Subsequently, filling is triggered based on the relationship between the corrected position and the outlet. After acquiring the filling completion signal, the opening robotic arm is controlled to perform either opening or pot entry actions based on the real-time opening / closing angle of the steaming bucket lid. This solution effectively improves the accuracy of robotic arm movement and filling, achieving intelligent collaborative cooperation between the two arms and significantly enhancing the continuity and reliability of automated double-skin milk pudding production. Attached Figure Description

[0014] Fig. 1 A flowchart of a dual-arm coordinated control method for double-skin milk pudding.

[0015] Fig. 2 This is a flowchart of the steps in step S2.

[0016] Fig. 3 This is a data flow diagram for steps S2 to S5. Detailed Implementation

[0017] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings. It should be noted that these descriptions are for the purpose of aiding understanding the present invention, but do not constitute a limitation thereof. Furthermore, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0018] Combination Figs. 1 to 3 The illustrated method for coordinated control of both arms in the making of double-skin milk includes the following steps: S1: Obtain the initial image at the start of double-skin milk making, and get the first three-dimensional coordinates of the mechanical arm that grips the bowl and the second three-dimensional coordinates of the discharge port; S2: Based on the first three-dimensional coordinates, the Kalman filter algorithm is used to predict the trajectory and obtain the predicted three-dimensional coordinates of the gripper of the robotic arm that picks up the bowl at the current moment; S3: Obtain the current actual position coordinates of the gripper of the robotic arm that picks up the bowl, and calculate the position deviation vector based on the predicted three-dimensional coordinates at the current moment; S4: Generate trajectory correction instructions based on the position deviation vector, and update the actual position coordinates of the gripper of the robotic arm that picks up the bowl; S5: Generate an injection trigger signal based on the positional relationship between the actual position coordinates and the second three-dimensional coordinates; S6: Drive the discharge machine to inject material into the bowl through the discharge port according to the material injection trigger signal, and obtain the material injection completion status signal; S7: Obtain the opening angle of the steaming bucket lid. If the opening angle is less than the preset lower limit angle threshold and the filling completion status signal is obtained, drive the lid opening robot arm to open the steaming bucket lid. If the opening angle is greater than the preset upper limit angle threshold, drive the bowl gripping robot arm to move into the steaming bucket.

[0019] In the dual-arm collaborative control method for making double-skin milk pudding, this solution addresses the issues of trajectory deviations during the robotic arm's movement while gripping the bowl, leading to inaccurate filling, and the lack of coordination between the filling and the opening / closing of the steaming bucket lid, resulting in poor cooking. The solution obtains the three-dimensional coordinates of the robotic arm gripping the bowl and the outlet by acquiring an initial image. It then uses a Kalman filter algorithm to predict the trajectory and calculates the deviation based on the actual position, generating a correction command to accurately update the robotic arm's position. Subsequently, filling is triggered based on the relationship between the corrected position and the outlet. After receiving the filling completion signal, the solution coordinates with the real-time opening / closing angle of the steaming bucket lid to control the opening robotic arm to perform either opening the lid or cooking the food. This solution effectively improves the accuracy of the robotic arm's movement and filling, achieving intelligent collaborative cooperation between the two arms and significantly enhancing the continuity and reliability of automated double-skin milk pudding production.

[0020] It is worth noting that, in step S1, the steps of obtaining the first three-dimensional coordinates of the robotic arm that grips the bowl and the second three-dimensional coordinates of the discharge port include: Obtain the video stream of double-skin milk pudding from the start of its production, as well as the pre-calibrated camera intrinsic parameter matrix; Each frame in the video stream is separated into a color image frame stream and a depth image frame stream; The YOLOv8 algorithm is used to process all color image frame streams to obtain the first bounding box diagram of the gripper of the bowl-grabbing robotic arm corresponding to each frame image. The YOLOv8 algorithm is used to process the second bounding box diagram of the outlet of the color image frame stream corresponding to the first frame image. Extract the first center point of the first boundary map and the second center point of the second boundary map to obtain the first pixel coordinates and the second pixel coordinates; Based on the first pixel coordinates and the second pixel coordinates, extract the first depth value corresponding to the first pixel coordinates and the second depth value corresponding to the second pixel coordinates from the depth image frame stream corresponding to the color image frame stream; Extract the focal length parameter and optical center coordinate parameter from the camera intrinsic parameter matrix; After performing spatial coordinate system transformation on the first pixel coordinates based on the focal length parameter and the optical center coordinate parameter, the first three-dimensional coordinates of the gripper of the bowl-grabbing robotic arm corresponding to each frame image are obtained by combining the first depth value. After performing spatial coordinate system transformation on the second pixel coordinates based on the focal length parameter and the optical center coordinate parameter, the second three-dimensional coordinates of the discharge port are obtained by combining the second depth value.

[0021] For example, a video stream is acquired using an RGBD device, such as a binocular camera, fixed to the double-skin milk making table, and the video stream is separated frame by frame into a color image frame stream and a depth image frame stream. In this embodiment, the RGBD device can simultaneously capture RGB color images and D depth images, so during separation, the RGB color images can be directly acquired as the color image frame stream, and the D depth images can be directly acquired as the depth image frame stream.

[0022] It should be noted that the YOLOv8 algorithm is a deep learning-based object detection model that extracts features and outputs the target location through a convolutional neural network. In the process of making double-skin milk pudding, the color image frame stream is input into the YOLOv8 model. The model identifies the gripper of the robotic arm that holds the bowl and generates a first bounding box map. Since the outlet position is fixed, only the first frame image is processed to obtain the second bounding box map of the outlet to reduce computational load. Subsequently, the center of the bounding box is extracted as the first pixel coordinate and the second pixel coordinate, and the corresponding first depth value and second depth value are extracted from the depth image frame stream.

[0023] Understandably, the camera intrinsic parameter matrix describes the internal optical characteristics of the RGBD device, including focal length and optical center coordinates. The focal length reflects the magnification, while the optical center coordinates represent the image center offset. The system then performs a spatial coordinate system transformation, subtracting the optical center coordinates from the pixel coordinates to eliminate the offset, dividing by the focal length to convert to physical scale, and using these as the X and Y coordinates. Finally, the depth value is used as the corresponding Z coordinate to determine the target's three-dimensional spatial coordinates in the real physical world. Through this process, the system accurately obtains the first three-dimensional coordinates of the gripper and the second three-dimensional coordinates of the discharge port.

[0024] Preferably, in step S2, the step of obtaining the predicted three-dimensional coordinates of the gripper of the robotic arm for picking up the bowl at the current moment includes: Obtain the first three-dimensional coordinates corresponding to all frames before the current moment; combine the first three-dimensional coordinates in chronological order to construct a historical coordinate sequence; calculate the difference between adjacent first three-dimensional coordinates for the historical coordinate sequence to obtain the initial velocity vector; construct the initial state vector based on the last first three-dimensional coordinate in the historical coordinate sequence and the corresponding initial velocity vector. Obtain a pre-set state transition matrix, and perform matrix multiplication on the initial state vector according to the state transition matrix to obtain the predicted state vector; A first intermediate covariance matrix is ​​obtained by performing matrix multiplication on the state transition matrix, the transpose of the state transition matrix, and the initial error covariance matrix. A second intermediate covariance matrix is ​​obtained by performing matrix multiplication on the prediction error covariance matrix, the observation matrix, and the transpose of the observation matrix. A third intermediate covariance matrix is ​​obtained by performing matrix addition on the second intermediate covariance matrix and the observation noise covariance matrix. Finally, the inverse of the third intermediate covariance matrix is ​​obtained. The predicted state vector is obtained by performing matrix multiplication on the observation matrix; the second three-dimensional coordinates corresponding to the frame image of the previous moment are obtained as the observation vector; the observation vector and the predicted observation vector are subtracted by matrix to obtain the residual vector. The Kalman gain matrix is ​​obtained by performing matrix multiplication on the prediction error covariance matrix, the transpose of the observation matrix, and the inverse matrix; the state correction vector is obtained by performing matrix multiplication on the residual vector based on the Kalman gain matrix. The predicted state vector and the corrected state vector are added together to obtain the updated state vector; the position component is extracted from the updated state vector to obtain the predicted three-dimensional coordinates at the current time.

[0025] For example, in the process of making double-skin milk pudding, the system extracts the first three-dimensional coordinates of the gripper corresponding to multiple consecutive frames of images before the current moment from the first three-dimensional coordinates obtained in step S1. It should be noted that the system combines the above-mentioned first three-dimensional coordinates in chronological order to construct a historical coordinate sequence and calculates the difference between adjacent coordinates to obtain an initial velocity vector. Then, it combines the coordinates at the end of the sequence with the initial velocity vector to construct an initial state vector containing position and velocity information.

[0026] Understandably, the state transition matrix describes the physical laws governing the movement of a robotic arm in the physical world from its current velocity and position to the next moment. This parameter is typically obtained from a pre-set database. The system uses this state transition matrix to multiply the initial state vector and deduce the predicted state vector.

[0027] Simultaneously, the system multiplies the state transition matrix and its transpose with the initial error covariance matrix to obtain the first intermediate covariance matrix, and then superimposes it with the process noise covariance matrix, representing the internal uncertainties of the system, to obtain the prediction error covariance matrix. The initial error covariance matrix represents the uncertainty matrix of the system state estimate at the initial moment, and it is used to determine the initial convergence speed of the filter, affect the Kalman gain K, and determine the disturbance rejection and stability of the filter. Specifically, the initial error covariance matrix and the process noise covariance matrix are also obtained from a preset database.

[0028] The system then introduces an observation matrix to map the predicted state to the observation space, obtaining a second intermediate covariance matrix. This is then superimposed with the observation noise covariance matrix, representing the sensor measurement error, to obtain a third intermediate covariance matrix. Specifically, the observation matrix is ​​obtained from a pre-set database. For this scheme, only the observation positions and the observation matrix are needed. The system then inverts the third intermediate covariance matrix and calculates the Kalman gain matrix by combining the prediction error covariance matrix and the transpose of the observation matrix. The Kalman gain matrix is ​​essentially a dynamic weighting that balances the reliability of model predictions with the reliability of sensor observations.

[0029] For example, the system calculates the residual vector between the actual observation vector and the predicted observation vector. This residual is then corrected using the Kalman gain matrix and superimposed onto the predicted state vector. Finally, the position component is extracted to obtain the predicted 3D coordinates of the gripper at the current moment. In this embodiment, the updated state vector is... , where x, y and z are the predicted three-dimensional coordinates of the gripper at the current moment.

[0030] Optionally, in step S3, the step of calculating the position deviation vector includes: Obtain the current actual position coordinates of the gripper of the robotic arm that grips the bowl. If the Euclidean distance between the actual position coordinates and the predicted 3D coordinates at the current moment is greater than a set first distance threshold, then add the predicted 3D coordinates as the first 3D coordinates to the historical coordinate sequence, and calculate the coordinate differences between the actual position coordinates and the predicted 3D coordinates on the x-axis, y-axis and z-axis respectively to obtain the position deviation vector.

[0031] In this embodiment, the method of obtaining the current actual position coordinates of the gripper of the bowl-grabbing robotic arm is the same as in step S1. Specifically, after a preset time after the start of double-skin milk making, which is before the gripper of the bowl-grabbing robotic arm reaches the discharge port, the color image frame stream and depth image frame stream are obtained by an RGBD device fixed to the double-skin milk making table, and the current actual position coordinates of the gripper of the bowl-grabbing robotic arm are obtained according to the color image frame stream, depth image frame stream and pre-calibrated camera intrinsic parameter matrix.

[0032] If the Euclidean distance between the actual position coordinates and the predicted three-dimensional coordinates at the current time is greater than the set first distance threshold, it means that the gripper of the bowl-picking robot arm deviates from the preset direction and may deviate from the second three-dimensional coordinates when it reaches the destination. At this time, on the one hand, the historical coordinate sequence in step S2 is updated by predicting the three-dimensional coordinates, and then the predicted three-dimensional coordinates of the next time node are calculated. On the other hand, the position deviation vector is calculated and then step S4 is executed.

[0033] If the Euclidean distance between the actual position coordinates and the predicted three-dimensional coordinates at the current moment is less than or equal to the set first distance threshold, it means that the gripper of the bowl-grabbing robotic arm has not deviated from the preset direction. On the one hand, the current actual position coordinates of the gripper of the bowl-grabbing robotic arm are used as the first three-dimensional coordinates to update the historical coordinate sequence in step S2, and then the predicted three-dimensional coordinates of the next time node are calculated to prepare for the processing in the subsequent step S5 if the gripper of the bowl-grabbing robotic arm has not yet reached the discharge port. On the other hand, step S4 is skipped and step S5 is executed.

[0034] Specifically, when calculating the position deviation vector, the system decomposes the actual position coordinates into actual components in three orthogonal directions: the x-axis, y-axis, and z-axis, and similarly decomposes the predicted three-dimensional coordinates into predicted components in the corresponding axes. The system subtracts the corresponding predicted components from the actual components of the x-axis, y-axis, and z-axis, respectively, to calculate the coordinate difference in each dimension. Then, the calculated coordinate differences of the x-axis, y-axis, and z-axis are combined to obtain the position deviation vector representing the current spatial displacement error.

[0035] Specifically, in step S4, the step of updating the actual position coordinates of the gripper of the robotic arm that picks up the bowl includes: Input the position deviation vector into the PID controller; The PID controller uses the proportional control gain, integral control gain and derivative control gain to perform proportional-integral-derivative summation on the position deviation vector to generate trajectory correction instructions. The trajectory correction command is sent to the motor driver; the motor driver controls the servo motor of the bowl-gripping robotic arm to run according to the trajectory correction command, and then obtains the actual position coordinates of the gripper of the bowl-gripping robotic arm at the next time node.

[0036] For example, in the automated production scenario of double-skin milk pudding, the system inputs the calculated position deviation vector to a preset PID controller for processing. Specifically, the PID controller is a linear controller based on proportional, integral, and derivative terms. Its core lies in using the proportional control gain, integral control gain, and derivative control gain to perform proportional-integral-derivative summation on the position deviation vector.

[0037] During the process of grasping the double-skin milk bowl, the proportional control gain is used to linearly amplify the current position deviation vector. The larger the deviation, the stronger the proportional control effect, thereby quickly reducing the distance between the current position of the gripper and the target position. The integral control gain calculates the cumulative amount of the position deviation vector over time. By integrating and summing historical position deviation vectors, the system can eliminate steady-state errors caused by gravity or friction, ensuring that the gripper is ultimately precisely aligned with the double-skin milk bowl. The derivative control gain focuses on the rate of change of the position deviation vector. The system predicts the future trend of the gripper position by calculating the derivative of the position deviation vector at adjacent time points, thereby applying damping in advance to suppress overshoot and oscillations that may occur when the robotic arm moves rapidly. The system linearly sums the calculation results of the proportional, integral, and derivative terms to generate a trajectory correction command for adjusting the robotic arm's motion posture. The generated trajectory correction command is then sent to the motor driver, which controls the joint servo motors of the gripping robotic arm according to the command.

[0038] The system then continues to acquire the actual position coordinates of the gripper at the next time node, and then acquires the predicted three-dimensional coordinates of the next time node output in step S2 before executing step S3 to determine the Euclidean distance between the actual position coordinates and the predicted three-dimensional coordinates at the current time, until the Euclidean distance between the actual position coordinates and the predicted three-dimensional coordinates at the current time is less than or equal to the set first distance threshold.

[0039] It is worth noting that in step S5, the step of generating the injection trigger signal includes: Obtain the actual position coordinates of the gripper of the robotic arm that picks up the bowl; obtain the second three-dimensional coordinates of the discharge port; calculate the relative distance between the actual position coordinates and the second three-dimensional coordinates using the Euclidean distance formula; Determine whether the relative distance is less than or equal to a preset second distance threshold; if the relative distance is less than the preset distance threshold, generate an injection trigger signal.

[0040] In this embodiment, if the relative distance is less than or equal to a preset second distance threshold, it means that the bowl held by the gripper of the bowl-grabbing robot arm has been aligned with the discharge port, and thus a feeding trigger signal can be generated to drive the discharge machine.

[0041] If the relative distance is greater than the preset second distance threshold, it means that the bowl held by the gripper of the bowl-grabbing robot arm has not yet been aligned with the discharge port. In this case, it is necessary to continue to wait for the gripper of the bowl-grabbing robot arm to move. At this time, the system will obtain the predicted three-dimensional coordinates of the next time node output in step S2, and then execute step S3 until the relative distance is less than or equal to the preset second distance threshold.

[0042] Preferably, in step S6, the step of obtaining the injection completion status signal includes: A valve opening command is generated based on the injection trigger signal; The discharge valve at the discharge port is opened by the valve opening command. After the discharge valve is opened, a countdown begins, and a discharge completion status signal is generated after the countdown ends.

[0043] For example, upon receiving the injection trigger signal from the upstream stage, the system immediately parses the signal and converts it into a valve opening command recognizable by the underlying hardware. Specifically, the system drives the discharge valve at the outlet to open via the valve opening command and simultaneously initiates a countdown strategy. The discharge valve typically employs a high-precision electromagnetic proportional valve, and the command contains a specific voltage pulse sequence for precisely controlling the valve core's opening angle.

[0044] The moment the discharge valve opens, the system's built-in timer module begins a countdown. Understandably, the countdown ends when the timer's value decreases to zero, at which point the system determines that the volume of the double-skin milk pudding in the bowl has reached the required standard, and then generates a completion signal for the filling process.

[0045] Optionally, in step S7, the step of obtaining the opening angle of the cooking tank lid includes: Rotary pulse data is collected by a rotary encoder at the rotating shaft of the cooking pot lid. The opening and closing angle is obtained by multiplying the rotary pulse data with a preset pulse angle conversion coefficient.

[0046] Specifically, the system collects rotational pulse data via a rotary encoder at the rotating shaft of the cooking tank lid. The rotary encoder converts mechanical rotational displacement into digital pulse signals; as the shaft rotates, the grating disk rotates accordingly, outputting a sequence of electrical pulses. The system calculates the opening and closing angle by multiplying the total number of pulses by a preset pulse angle conversion coefficient. This coefficient, a parameter calibrated based on the encoder resolution, represents the actual mechanical rotation angle corresponding to a single pulse. By multiplying the number of pulses by this coefficient, the system accurately calculates the opening and closing angle.

[0047] If the opening angle is less than the preset lower limit angle threshold and the filling completion status signal is obtained, it means that the steaming bucket lid is in the closed state and the lid needs to be opened next. At this time, the lid opening robot arm lifts the lid according to the preset path. If the opening angle is greater than the preset upper limit angle threshold, it means that the steaming bucket lid is in the fully open state. At this time, the bowl-grabbing robot arm runs smoothly according to the preset action path and places the bowl containing the double-skin milk into the steaming bucket.

[0048] The embodiments of the present invention have been described in detail above with reference to the accompanying drawings, but the present invention is not limited to the described embodiments. For those skilled in the art, various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention, and these variations still fall within the protection scope of the present invention.

Claims

1. A method for coordinated control of two arms in the making of double-skin milk pudding, characterized in that, Includes the following steps: S1: Obtain the initial image at the start of double-skin milk making, and get the first three-dimensional coordinates of the mechanical arm that grips the bowl and the second three-dimensional coordinates of the discharge port; S2: Based on the first three-dimensional coordinates, the Kalman filter algorithm is used to predict the trajectory and obtain the predicted three-dimensional coordinates of the gripper of the robotic arm that picks up the bowl at the current moment; S3: Obtain the current actual position coordinates of the gripper of the robotic arm that picks up the bowl, and calculate the position deviation vector based on the predicted three-dimensional coordinates at the current moment; S4: Generate trajectory correction instructions based on the position deviation vector, and update the actual position coordinates of the gripper of the robotic arm that picks up the bowl; S5: Generate an injection trigger signal based on the positional relationship between the actual position coordinates and the second three-dimensional coordinates; S6: Drive the discharge machine to inject material into the bowl through the discharge port according to the material injection trigger signal, and obtain the material injection completion status signal; S7: Obtain the opening angle of the steaming bucket lid. If the opening angle is less than the preset lower limit angle threshold and the filling completion status signal is obtained, drive the lid opening robot arm to open the steaming bucket lid. If the opening angle is greater than the preset upper limit angle threshold, drive the bowl gripping robot arm to move into the steaming bucket.

2. The method for coordinated control of two arms in making double-skin milk pudding according to claim 1, characterized in that: In step S1, the steps of obtaining the first three-dimensional coordinates of the robotic arm that grips the bowl and the second three-dimensional coordinates of the discharge port include: Obtain the video stream of double-skin milk pudding from the start of its production, as well as the pre-calibrated camera intrinsic parameter matrix; Each frame in the video stream is separated into a color image frame stream and a depth image frame stream; The YOLOv8 algorithm is used to process all color image frame streams to obtain the first bounding box diagram of the gripper of the bowl-grabbing robotic arm corresponding to each frame image. The YOLOv8 algorithm is used to process the second bounding box diagram of the outlet of the color image frame stream corresponding to the first frame image. Extract the first center point of the first boundary map and the second center point of the second boundary map to obtain the first pixel coordinates and the second pixel coordinates; Based on the first pixel coordinates and the second pixel coordinates, extract the first depth value corresponding to the first pixel coordinates and the second depth value corresponding to the second pixel coordinates from the depth image frame stream corresponding to the color image frame stream; Extract the focal length parameter and optical center coordinate parameter from the camera intrinsic parameter matrix; After performing spatial coordinate system transformation on the first pixel coordinates based on the focal length parameter and the optical center coordinate parameter, the first three-dimensional coordinates of the gripper of the bowl-grabbing robotic arm corresponding to each frame image are obtained by combining the first depth value. After performing spatial coordinate system transformation on the second pixel coordinates based on the focal length parameter and the optical center coordinate parameter, the second three-dimensional coordinates of the discharge port are obtained by combining the second depth value.

3. The method for coordinated control of two arms in making double-skin milk pudding according to claim 1, characterized in that: In step S2, the step of obtaining the predicted three-dimensional coordinates of the gripper of the robotic arm for picking up bowls at the current moment includes: Obtain the first three-dimensional coordinates corresponding to all frames before the current moment; combine the first three-dimensional coordinates in chronological order to construct a historical coordinate sequence; calculate the difference between adjacent first three-dimensional coordinates for the historical coordinate sequence to obtain the initial velocity vector; construct the initial state vector based on the last first three-dimensional coordinate in the historical coordinate sequence and the corresponding initial velocity vector. Obtain a pre-set state transition matrix, and perform matrix multiplication on the initial state vector according to the state transition matrix to obtain the predicted state vector; A first intermediate covariance matrix is ​​obtained by performing matrix multiplication on the state transition matrix, the transpose of the state transition matrix, and the initial error covariance matrix. A second intermediate covariance matrix is ​​obtained by performing matrix multiplication on the prediction error covariance matrix, the observation matrix, and the transpose of the observation matrix. A third intermediate covariance matrix is ​​obtained by performing matrix addition on the second intermediate covariance matrix and the observation noise covariance matrix. Finally, the inverse of the third intermediate covariance matrix is ​​obtained. The predicted state vector is obtained by performing matrix multiplication on the observation matrix; the second three-dimensional coordinates corresponding to the frame image of the previous moment are obtained as the observation vector; the observation vector and the predicted observation vector are subtracted by matrix to obtain the residual vector. The Kalman gain matrix is ​​obtained by performing matrix multiplication on the prediction error covariance matrix, the transpose of the observation matrix, and the inverse matrix; the state correction vector is obtained by performing matrix multiplication on the residual vector based on the Kalman gain matrix. The predicted state vector and the corrected state vector are added together to obtain the updated state vector; the position component is extracted from the updated state vector to obtain the predicted three-dimensional coordinates at the current time.

4. The method for coordinated control of two arms in making double-skin milk pudding according to claim 3, characterized in that: In step S3, the step of calculating the position deviation vector includes: Obtain the current actual position coordinates of the gripper of the robotic arm that grips the bowl. If the Euclidean distance between the actual position coordinates and the predicted 3D coordinates at the current moment is greater than a set first distance threshold, then add the predicted 3D coordinates as the first 3D coordinates to the historical coordinate sequence, and calculate the coordinate differences between the actual position coordinates and the predicted 3D coordinates on the x-axis, y-axis and z-axis respectively to obtain the position deviation vector.

5. The method for coordinated control of two arms in making double-skin milk pudding according to claim 4, characterized in that: In step S4, the step of updating the actual position coordinates of the gripper of the robotic arm that picks up the bowl includes: Input the position deviation vector into the PID controller; The PID controller uses the proportional control gain, integral control gain and derivative control gain to perform proportional-integral-derivative summation on the position deviation vector to generate trajectory correction instructions. The trajectory correction command is sent to the motor driver; the motor driver controls the servo motor of the bowl-gripping robotic arm to run according to the trajectory correction command, and then obtains the actual position coordinates of the gripper of the bowl-gripping robotic arm at the next time node.

6. The method for coordinated control of two arms in making double-skin milk pudding according to claim 5, characterized in that: In step S5, the step of generating the injection trigger signal includes: Obtain the actual position coordinates of the gripper of the robotic arm that picks up the bowl; obtain the second three-dimensional coordinates of the discharge port; calculate the relative distance between the actual position coordinates and the second three-dimensional coordinates using the Euclidean distance formula; Determine whether the relative distance is less than or equal to a preset second distance threshold; if the relative distance is less than the preset distance threshold, generate an injection trigger signal.

7. The method for coordinated control of two arms in making double-skin milk pudding according to claim 1, characterized in that: In step S6, the step of obtaining the injection completion status signal includes: A valve opening command is generated based on the injection trigger signal; The discharge valve at the discharge port is opened by the valve opening command. After the discharge valve is opened, a countdown begins, and a discharge completion status signal is generated after the countdown ends.

8. The method for coordinated control of two arms in making double-skin milk pudding according to claim 1, characterized in that: In step S7, the step of obtaining the opening angle of the cooking tank lid includes: Rotary pulse data is collected by a rotary encoder at the rotating shaft of the cooking pot lid. The opening and closing angle is obtained by multiplying the rotary pulse data with a preset pulse angle conversion coefficient.

Images