Three-mechanical-arm linkage control method and system applied to coffee beverage robot

Abstract

This invention discloses a three-arm linkage control method and system for coffee and beverage robots, belonging to the field of cooperative control technology. The method includes: when arm A deviates from its obstacle avoidance envelope, calling the output multi-torque timing record of arm A, and calculating the deviation velocity characteristics of the multi-torque nodes deviating from the multi-torque control sequence of arm A to predict the output updated displacement trajectory; fitting the cooperative fault-tolerant trajectory of arms B-C based on the coordinates of the end gripper of the updated displacement trajectory, and outputting cooperative fault-tolerant control parameters for three-arm fault-tolerant control and order processing. This solves the technical problem in existing technologies where real-time arm faults in the collaborative operation of multiple arms in coffee and beverage robots cannot be effectively self-corrected, leading to weak stability in the coffee and beverage production process. It achieves the technical effect of improving trajectory self-correction and dynamic cooperative capabilities under fault conditions, ensuring the continuous stability of the beverage production process and the accuracy of spatial motion coordination.

Description

Technical Field

[0001] This invention relates to the field of collaborative control technology, and in particular to a three-arm linkage control method and system for coffee beverage robots. Background Technology

[0002] Current multi-arm collaborative control technology for coffee and beverage robots has significant shortcomings: in dynamic operation scenarios, when the robotic arm suddenly deviates or malfunctions, the existing system lacks the ability to make real-time adaptive corrections.

[0003] Traditional solutions rely on preset fixed obstacle avoidance envelopes and rigid coordination parameters. When a single arm's trajectory is abnormal, it cannot quickly reconstruct the safe activity boundaries of other robotic arms, leading to a surge in collision risks.

[0004] Existing trajectory correction methods are based on offline replanning mechanisms, resulting in response delays exceeding 800ms, which is insufficient to meet the requirements of continuous beverage preparation sequence. Furthermore, torque anomaly detection often employs threshold alarm modes, failing to incorporate multi-joint dynamic characteristics for trajectory prediction, causing fault-tolerant actions to lag behind actual offset development.

[0005] In summary, existing technologies suffer from the technical problem that real-time robotic arm malfunctions during the collaborative operation of multiple robotic arms in coffee beverage robots cannot be effectively self-corrected, resulting in a relatively weak stability of the coffee beverage production process. Summary of the Invention

[0006] This invention provides a three-arm linkage control method and system for coffee and beverage robots, which addresses the technical problem in the prior art where real-time robotic arm malfunctions in the multi-arm collaborative operation process of coffee and beverage robots cannot be effectively self-corrected, resulting in weak stability of the coffee and beverage production process.

[0007] In view of the above problems, the present invention provides a three-arm linkage control method and system for coffee beverage robots.

[0008] In a first aspect, the present invention provides a three-arm linkage control method for a coffee beverage robot, the method comprising:

[0009] Based on the user's order information, target collaborative control constraints and target collaborative control parameters are retrieved from the robotic arm collaboration library. The target collaborative control constraints include obstacle avoidance envelopes A, B, and C corresponding to robotic arms A, B, and C, as well as A, B, and C multi-torque control sequences. During the collaborative coffee-making control of robotic arms A, B, and C using the target collaborative control parameters, the obstacle avoidance envelopes A, B, and C are used for robotic arm operation conflict monitoring. When robotic arm A... When the arm deviates from the obstacle avoidance envelope A, the torque log of the A robotic arm is invoked, and the A multi-torque timing record is output. Based on the deviation speed characteristics of the multi-torque nodes of the A multi-torque timing record from the A multi-torque control sequence, the displacement trajectory of the A robotic arm is predicted, and the updated displacement trajectory is output. Based on the end gripper coordinates of the updated displacement trajectory, the B and C robotic arms are fitted with a collaborative fault-tolerant trajectory, and collaborative fault-tolerant control parameters are output. After the collaborative fault-tolerant control parameters are used to partially cover the target collaborative control parameters, three-arm fault-tolerant control and order processing are performed.

[0010] In one implementation, based on the coordinates of the end gripper of the updated displacement trajectory, the B robotic arm and the C robotic arm are fitted with a cooperative fault-tolerant trajectory, and cooperative fault-tolerant control parameters are output. The following processing is also performed:

[0011] Using the updated displacement trajectory as the clipping criterion, geometric constraint reconstruction is performed on the obstacle avoidance envelopes B and C respectively to generate the B-limited envelope and the C-limited envelope; using the B-limited envelope and the C-limited envelope as displacement space constraints, and based on the end gripper coordinates of the updated displacement trajectory, the B-arm and the C-arm are fitted with a collaborative fault-tolerant trajectory, and the collaborative fault-tolerant control parameters are output.

[0012] In one implementation, geometric constraint reconstruction is performed on the obstacle avoidance envelope B and obstacle avoidance envelope C respectively using the updated displacement trajectory as the clipping criterion to generate B-limited envelope and C-limited envelope, and the following processing is also performed:

[0013] A preset envelope safety interval is established; the envelope safety interval is expanded spatially with the updated displacement trajectory as the center to generate an updated displacement envelope; based on the spatial overlap characteristics of the updated displacement envelope and the B obstacle avoidance envelope, the B obstacle avoidance envelope is narrowed to the B narrowed envelope; based on the spatial overlap characteristics of the updated displacement envelope and the C obstacle avoidance envelope, the C obstacle avoidance envelope is narrowed to the C narrowed envelope.

[0014] In one implementation, the B-bound and C-bound constraints are used as displacement space constraints. Based on the end gripper coordinates of the updated displacement trajectory, the B-arm and C-arm are fitted with a cooperative fault-tolerant trajectory, and the cooperative fault-tolerant control parameters are output. The following processing is also performed:

[0015] Using the end effector coordinates as the endpoint of the robot arm's displacement, the fault-tolerant trajectory of the B-arm is coupled within the B-bounding space to output a first cooperative fault-tolerant trajectory; using the end effector coordinates as the endpoint of the robot arm's displacement, the fault-tolerant trajectory of the C-arm is coupled within the C-bounding space to output a second cooperative fault-tolerant trajectory; a first cooperative fault-tolerant control sequence and a second cooperative fault-tolerant control sequence corresponding to the first and second cooperative fault-tolerant trajectories are matched and output, wherein the first and second cooperative fault-tolerant control sequences constitute the cooperative fault-tolerant control parameters.

[0016] In one implementation, when robotic arm A deviates from the obstacle avoidance envelope A, the torque log of robotic arm A is invoked, outputting A multi-torque timing records, and the following processing is also performed:

[0017] During the operation of the robotic arm A, multiple real-time joint position sequences of multiple a joints in the robotic arm A are collected through pose sensing; an A-envelope twin space is constructed based on the A obstacle avoidance envelope; the multiple real-time joint position sequences are synchronized to the A-envelope twin space for trajectory deviation monitoring; when the deviation regression time of the multiple real-time joint position sequences from the A-envelope twin space exceeds a preset time window, it is determined that the robotic arm A has deviated from the A obstacle avoidance envelope.

[0018] In one implementation, the following processing is also performed:

[0019] Calculate the spatial extreme values ​​of the deviation of the multiple real-time joint position sequences from the twin space of envelope A; if the spatial extreme values ​​exceed a preset spatial deviation scale, then abruptly stop the A robotic arm and call the emergency stop end coordinates; after extending and expanding the B obstacle avoidance envelope and C obstacle avoidance envelope according to the emergency stop end coordinates, perform cooperative fault-tolerant trajectory fitting on the B robotic arm and the C robotic arm, and output the cooperative fault-tolerant control parameters.

[0020] In one implementation, based on the deviation speed characteristics of the multi-torque nodes from the A multi-torque control sequence recorded in the A multi-torque time series, the displacement trajectory of the A robotic arm is predicted, an updated displacement trajectory is output, and the following processing is also performed:

[0021] The A-multiple torque timing record is used to traverse and compare the A-multiple torque control sequence to obtain multiple torque deviation sequences corresponding to the multiple a joints; the torque deviation gradient is calculated for the multiple torque deviation sequences to output the torque deviation speed features of multiple joints; a torque deviation timing matrix is ​​constructed based on the torque deviation speed features of multiple joints; the displacement trajectory of the A-manipulator is fitted according to the torque deviation timing matrix to output the updated displacement trajectory.

[0022] In a second aspect, the present invention provides a three-arm linkage control system for a coffee beverage robot, the system comprising:

[0023] The information retrieval unit is used to retrieve target collaborative control constraints and target collaborative control parameters from the robotic arm collaboration library based on user order information. The target collaborative control constraints include obstacle avoidance envelopes A, B, and C corresponding to robotic arms A, B, and C, as well as A multi-torque control sequences, B multi-torque control sequences, and C multi-torque control sequences. The conflict monitoring unit is used to monitor robotic arm operation conflicts using the obstacle avoidance envelopes A, B, and C during the collaborative coffee-making control of robotic arms A, B, and C using the target collaborative control parameters. The log retrieval unit is used when robotic arm A... When deviating from the obstacle avoidance envelope A, the torque log of robotic arm A is invoked, and A multi-torque time sequence record is output; the trajectory prediction unit is used to predict the displacement trajectory of robotic arm A based on the deviation speed characteristics of the multi-torque nodes of the A multi-torque control sequence from the A multi-torque time sequence record, and output the updated displacement trajectory; the fault-tolerant trajectory fitting unit is used to perform collaborative fault-tolerant trajectory fitting for robotic arms B and C based on the end gripper coordinates of the updated displacement trajectory, and output collaborative fault-tolerant control parameters; the order creation unit is used to perform fault-tolerant control and order creation for the three robotic arms after partially covering the target collaborative control parameters with the collaborative fault-tolerant control parameters.

[0024] One or more technical solutions provided in this invention have at least the following technical effects or advantages:

[0025] The method provided in this invention achieves high-safety obstacle avoidance control for three robotic arms (A, B, and C) during coffee making using target collaborative control parameters. It employs obstacle avoidance envelopes A, B, and C for monitoring robotic arm movement conflicts. When robotic arm A deviates from its obstacle avoidance envelope, its torque log is retrieved, outputting a multi-torque timing record. Based on the deviation speed characteristics of multiple torque nodes from the multi-torque control sequence, the displacement trajectory of robotic arm A is predicted, and an updated displacement trajectory is output. Based on the end-gripper coordinates of the updated displacement trajectory, collaborative fault-tolerant trajectory fitting is performed on robotic arms B and C, outputting collaborative fault-tolerant control parameters. After partially overriding the target collaborative control parameters with these parameters, fault-tolerant control and order processing for the three robotic arms are implemented. This method achieves high-safety obstacle avoidance control for multi-robotic arm collaborative operations, enhancing trajectory self-correction and dynamic collaboration capabilities under fault conditions, and ensuring continuous stability and spatial motion coordination accuracy in the beverage making process. Attached Figure Description

[0026] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0027] Figure 1 This invention provides a schematic flowchart of a three-arm linkage control method for coffee beverage robots.

[0028] Figure 2 A schematic diagram of the three-arm linkage control system for coffee beverage robots provided by the present invention is shown.

[0029] Explanation of the attached diagram labels: Information retrieval unit 1, Conflict monitoring unit 2, Log retrieval unit 3, Trajectory prediction unit 4, Fault-tolerant trajectory fitting unit 5, Order creation unit 6. Detailed Implementation

[0030] This invention provides a three-arm linkage control method and system for coffee and beverage robots, which addresses the technical problem in the prior art where real-time robotic arm malfunctions in the multi-arm collaborative operation process of coffee and beverage robots cannot be effectively self-corrected, resulting in weak stability of the coffee and beverage production process.

[0031] The technical solutions of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. It should be understood that the present invention is not limited to the exemplary embodiments described herein. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention. It should also be noted that, for ease of description, only the parts related to the present invention are shown in the accompanying drawings, not all of them.

[0032] Example 1: A flowchart of the three-arm linkage control method for a coffee beverage robot provided in this embodiment of the invention is shown below. Figure 1 The method includes:

[0033] Step A100: Based on the user's order information, retrieve the target collaborative control constraints and target collaborative control parameters from the robotic arm collaborative library. The target collaborative control constraints include obstacle avoidance envelopes A, B, and C corresponding to robotic arms A, B, and C, as well as multi-torque control sequences A, B, and C.

[0034] Specifically, this embodiment parses the user's order information to obtain the type of beverage selected by the user. Then, based on the beverage type, it retrieves and loads a matching collaborative operation configuration scheme from a pre-established robotic arm motion collaboration database (robotic arm collaboration library). This scheme includes two core elements: The first is the target collaborative control constraint, which characterizes the collaborative control constraints. Specifically, it refers to the three-dimensional operational safety areas (obstacle avoidance envelopes, specifically A, B, and C obstacle avoidance envelopes) of each robotic arm: the coffee machine operating robotic arm (A), the milk frothing robotic arm (B), and the finished product transfer robotic arm (C). These areas are defined by mathematical modeling to define the activity boundaries of each robotic arm in space to prevent collisions. The second is the target collaborative control parameter, which characterizes the robotic arm collaborative control parameters when making this type of coffee beverage. This includes a sequence of torque output values ​​(multi-torque control sequences, specifically A, B, and C multi-torque control sequences) of each robotic arm joint drive device at preset process time nodes. This sequence precisely controls the torque output of each joint motor to achieve precise motion coordination.

[0035] Step A200: During the collaborative control of coffee making by robotic arms A, B, and C using the target collaborative control parameters, obstacle avoidance envelopes A, B, and C are used to monitor collisions during robotic arm operation.

[0036] Specifically, during the coffee-making process, the robotic arm integrated control center drives the three arms A, B, and C to work together according to the preset target collaborative control parameters (torque control sequence of each robotic arm).

[0037] During this process, the current positions of the ends and links of robotic arms A, B, and C are compared in real time with the boundaries of the preset obstacle avoidance envelopes A, B, and C to detect any risk of exceeding the limits.

[0038] For example, if robotic arm B deviates from its defined obstacle avoidance envelope B while moving the milk frother, and gets too close to robotic arm A's obstacle avoidance envelope (such as the coffee extraction area), a collision warning will be triggered, thereby adjusting the movement trajectory to avoid a collision.

[0039] Such monitoring is typically achieved by combining spatial geometry detection algorithms (such as GJK collision detection) or path planning optimization techniques.

[0040] Step A300: When the A robotic arm deviates from the A obstacle avoidance envelope, the torque log of the A robotic arm is invoked, and the A multi-torque timing record is output.

[0041] In one embodiment, when the A robotic arm deviates from the A obstacle avoidance envelope, a torque log is invoked for the A robotic arm, and an A multi-torque timing record is output. The method step A300 provided by this invention includes:

[0042] Step A310: During the operation of the robotic arm A, the real-time joint position sequence of multiple a joints in the robotic arm A is collected by pose sensing.

[0043] Step A320: Construct an A-envelope twin space based on the A-obstacle avoidance envelope.

[0044] Step A330: Synchronize the multiple real-time joint position sequences to the A envelope twin space for trajectory deviation monitoring.

[0045] Step A340: When the deviation regression time of the multiple real-time joint position sequences from the A envelope twin space exceeds a preset time window, it is determined that the A robotic arm has deviated from the A obstacle avoidance envelope.

[0046] Since the fault-tolerant collaborative processing of other robotic arms is similar when each robotic arm fails, this embodiment will use robotic arm A as an example to illustrate the technical solution.

[0047] Specifically, during the operation of the robotic arm A, pose sensors (such as encoders or inertial measurement units) installed on each joint of the robotic arm A continuously collect real-time joint angle and position data of multiple joints a in the robotic arm A, forming a time-sequential sequence of multiple real-time joint positions. For example, the rotation angle change curve of joint 1 during coffee extraction and the displacement data of joint 2 during translation are collected to ensure comprehensive monitoring of the robotic arm's movements.

[0048] Based on the geometric parameters (such as geometric shape and size range) of the obstacle avoidance envelope of the robotic arm A, a corresponding digital twin space (A envelope twin space) is constructed. The multiple real-time joint position sequences are synchronized to the A envelope twin space, and the Euclidean distance difference between the actual trajectory and the theoretical safety boundary is calculated to determine whether there is a continuous deviation.

[0049] If the trajectory of robotic arm A deviates from the preset time window (e.g., 500 milliseconds) and fails to return to the obstacle avoidance envelope, robotic arm A is determined to have entered an abnormal state. For example, if a coffee cup gets stuck and causes the end effector of robotic arm A to continuously deviate from the preset path for more than a threshold time, an emergency response mechanism is triggered, such as pausing the action or switching to fault-tolerant mode.

[0050] When robotic arm A deviates from the obstacle avoidance envelope, the timing torque log of the deviation phase is invoked for robotic arm A, and the timing record of multiple torques of A is output for subsequent deviation feature identification and fault-tolerant control solution of the other two robotic arms B and C.

[0051] Step A400: Based on the deviation speed characteristics of the multi-torque nodes from the A multi-torque control sequence recorded by the A multi-torque time series, predict the displacement trajectory of the A robotic arm and output the updated displacement trajectory.

[0052] In one embodiment, based on the deviation speed characteristics of the multi-torque nodes deviating from the A multi-torque control sequence in the A multi-torque time series record, the displacement trajectory of the A robotic arm is predicted, and an updated displacement trajectory is output. Step A400 of the method provided by this invention includes:

[0053] Step A410: Use the A-multi-torque timing record to traverse and compare the A-multi-torque control sequence to obtain multiple torque deviation sequences corresponding to the multiple a joints.

[0054] Step A420: Calculate the torque deviation gradient for the multiple torque deviation sequences and output the torque deviation speed characteristics of multiple joints.

[0055] Step A430: Construct a torque deviation time series matrix based on the torque deviation speed characteristics of the multiple joints.

[0056] Step A440: Fit the displacement trajectory of the robotic arm A according to the torque deviation time series matrix, and output the updated displacement trajectory.

[0057] Specifically, by comparing the torque timing data (A-multi-torque timing record) recorded during the actual operation of robotic arm A with the preset ideal torque sequence (A-multi-torque control sequence) at each joint's time node, the torque deviation of each joint at different times is identified, forming an independent torque deviation sequence for each joint, thus obtaining multiple torque deviation sequences corresponding to the multiple a joints.

[0058] Mathematical differentiation (gradient calculation) is performed on the torque deviation sequence for each joint to obtain the torque deviation rate characteristics for multiple joints. This characteristic represents the drastic degree of torque change; for example, if the torque of joint 3 drops by 30% from its rated value within 0.2 seconds, its deviation rate characteristic is -150% per second. Such characteristics can reflect abnormal load changes in the robotic arm joints, such as motor stall or external collisions.

[0059] The torque deviation velocity characteristics of each joint are integrated into a matrix structure (torque deviation time series matrix) along the time dimension. The rows of the matrix represent time nodes, and the columns represent the deviation velocity values ​​of different joints. For example, the data in the 5th row and 3rd column indicates that at the timestamp of 0.5 seconds, the torque deviation velocity of joint 3 is -120% per second. This matrix serves as the input to the trajectory prediction model to capture multi-joint cooperative anomaly patterns.

[0060] Based on the torque deviation time series matrix, combined with the kinematic model of the robotic arm (such as the DH parameter model) and dynamic equations (such as the Newton-Euler equations), the displacement changes of each joint in the future are estimated through numerical integration methods, generating a corrected trajectory (updated displacement trajectory). For example, if joint 1 continues to have a positive torque deviation, it is predicted that it will accelerate, and the displacement increment needs to be reduced in advance in the updated trajectory to avoid exceeding the limit.

[0061] This embodiment predicts the displacement trajectory of the robotic arm by analyzing torque deviation characteristics and dynamically updates the path, providing a basis for trajectory optimization analysis for the fault-tolerant control of the other two robotic arms that will subsequently coordinate coffee making.

[0062] Step A500: Based on the coordinates of the end gripper of the updated displacement trajectory, perform collaborative fault-tolerant trajectory fitting on robotic arms B and C, and output collaborative fault-tolerant control parameters.

[0063] In one embodiment, based on the coordinates of the end gripper of the updated displacement trajectory, the B robotic arm and the C robotic arm are fitted with a collaborative fault-tolerant trajectory, and collaborative fault-tolerant control parameters are output. Step A500 of the method provided by this invention includes:

[0064] Step A510: Using the updated displacement trajectory as the clipping criterion, perform geometric constraint reconstruction on the obstacle avoidance envelope B and obstacle avoidance envelope C respectively to generate the B-limited envelope and the C-limited envelope.

[0065] Step A520: Using the B-bound and C-bound envelopes as displacement space constraints, and based on the end gripper coordinates of the updated displacement trajectory, perform collaborative fault-tolerant trajectory fitting on the B-arm and C-arm, and output the collaborative fault-tolerant control parameters.

[0066] In one embodiment, geometric constraint reconstruction is performed on the obstacle avoidance envelope B and obstacle avoidance envelope C respectively using the updated displacement trajectory as the clipping reference to generate B-limited envelope and C-limited envelope. The method step A510 provided by the present invention includes:

[0067] Step A511: Preset envelope safety interval.

[0068] Step A512: Expand the envelope safety interval with the updated displacement trajectory as the center space to generate the updated displacement envelope.

[0069] Step A513: Based on the spatial overlap characteristics of the updated displacement envelope and the obstacle avoidance envelope B, the obstacle avoidance envelope B is narrowed to the narrowed B envelope.

[0070] Step A514: Based on the spatial overlap characteristics of the updated displacement envelope and the obstacle avoidance envelope, the obstacle avoidance envelope is narrowed to the narrowed obstacle avoidance envelope.

[0071] In one embodiment, the B-bound and C-bound constraints are used as displacement space constraints. Based on the coordinates of the end gripper of the updated displacement trajectory, the B-arm and C-arm are fitted with a cooperative fault-tolerant trajectory, and the cooperative fault-tolerant control parameters are output. Step A520 of the method provided by this invention includes:

[0072] Step A521: Using the coordinates of the end gripper as the displacement endpoint of the robotic arm, perform fault-tolerant trajectory coupling of the B robotic arm within the B-limited envelope, and output the first cooperative fault-tolerant trajectory.

[0073] Step A522: Using the coordinates of the end gripper as the displacement endpoint of the robotic arm, perform fault-tolerant trajectory coupling of the C robotic arm within the C-limited envelope, and output the second cooperative fault-tolerant trajectory.

[0074] Step A523: Match and output the first collaborative fault-tolerant control sequence and the second collaborative fault-tolerant control sequence corresponding to the first collaborative fault-tolerant trajectory and the second collaborative fault-tolerant trajectory, wherein the first collaborative fault-tolerant control sequence and the second collaborative fault-tolerant control sequence constitute the collaborative fault-tolerant control parameters.

[0075] This embodiment uses the updated displacement trajectory as the central reference base and reshapes the original safe activity areas (B obstacle avoidance envelope, C obstacle avoidance envelope) of robotic arms B and C to generate a smaller new safe area (B restricted envelope, C restricted envelope). For example, if the updated trajectory is close to the edge of the original activity area of ​​robotic arm B, this area needs to be reduced to avoid conflict with the new path of robotic arm A.

[0076] Specifically, a safety interval for the envelope is preset. This safety interval is the minimum safe distance between the robotic arm's trajectory and surrounding obstacles. Using each path point on the updated displacement trajectory as the center, a spherical spatial region is generated according to the preset safety interval radius. These spheres are then merged into a continuous safety buffer space using Minkowski summation operations, serving as the updated displacement envelope. For example, if the updated trajectory is arc-shaped, the generated envelope will appear as a tubular three-dimensional region.

[0077] The overlapping region between the updated displacement envelope and the original obstacle avoidance envelope of the B robotic arm is calculated using geometric Boolean operations. This overlapping region is then removed from the original envelope, and the remaining portion becomes the B-limited envelope. For example, if the updated trajectory crosses the original working area of ​​the B robotic arm, the intersecting portion is removed to avoid path interference.

[0078] Similarly, the same operation is performed on the original obstacle avoidance envelope of the C-arm: the spatial intersection of the displacement envelope is detected and updated, and the overlapping portion is removed to form the C-bounded envelope. For example, the range of motion of the finished product transfer arm may be limited to a specific quadrant of the control panel due to the path update of the A-arm.

[0079] After envelope compression, the coordinates of the gripper at the end of the updated trajectory are used as the target endpoint of robotic arm B. Path search and smoothing are performed within its compressed safe area (B's compressed envelope) to generate a fault-tolerant motion trajectory for robotic arm B (the first cooperative fault-tolerant trajectory). For example, the milk frothing robotic arm needs to bypass the newly added safe buffer area of ​​robotic arm A, forming a circuitous but safe movement path.

[0080] Similarly, using the coordinates of the end effector gripper as the target endpoint of robotic arm C, its motion path is planned within the constricted envelope of C, and the fault-tolerant trajectory of robotic arm C (the second cooperative fault-tolerant trajectory) is output. For example, the finished product transfer robotic arm needs to be lowered to avoid intersecting with the path of the corrected robotic arm A, while ensuring that the beverage is placed stably.

[0081] The first and second collaborative fault-tolerant trajectories are converted into joint motion control command sequences for the corresponding robotic arms: the torque-time curves of each joint of robotic arm B (first collaborative fault-tolerant control sequence) and the torque-time curves of each joint of robotic arm C (second collaborative fault-tolerant control sequence) are generated, and the two together constitute complete collaborative fault-tolerant control parameters.

[0082] This embodiment achieves the technical effect of eliminating trajectory conflicts with malfunctioning robotic arms, ensuring the continuity of beverage production, and maintaining a precise work rhythm by dynamically reconstructing the safety areas of robotic arms B and C and planning collaborative paths.

[0083] Step A600: After partially overriding the target collaborative control parameters with the aforementioned collaborative fault-tolerant control parameters, perform three-arm fault-tolerant control and order processing.

[0084] Specifically, in this embodiment, after completing the fault-tolerant trajectory planning for robotic arms B and C, the newly generated collaborative fault-tolerant control parameters (including the corrected joint torque sequence and motion trajectory) selectively replace the affected parameters in the original preset target collaborative control parameters (partial coverage). Based on the updated hybrid control parameter set, the three robotic arms A, B, and C are driven to work collaboratively. While maintaining the continuity of the current order beverage production process, safe fault-tolerant control is achieved in fault conditions by dynamically adjusting the timing and spatial path of each robotic arm's movements.

[0085] This embodiment achieves highly safe obstacle avoidance control for multi-robotic arm collaborative operation, and achieves the technical effect of improving trajectory self-correction and dynamic coordination capabilities under fault conditions, ensuring the continuous stability of the beverage production process and the accuracy of spatial motion coordination.

[0086] In one embodiment, the method step A300 provided by the present invention includes:

[0087] Step A311: Calculate the deviation spatial extreme values ​​of the multiple real-time joint position sequences from the A envelope twin space.

[0088] Step A312: If the deviation from the spatial extreme value exceeds the preset spatial deviation scale, then the A robotic arm is stopped immediately and the emergency stop end coordinates are called.

[0089] Step A313: After extending the obstacle avoidance envelopes B and C based on the coordinates of the emergency stop end point, perform cooperative fault-tolerant trajectory fitting on the B robotic arm and the C robotic arm, and output the cooperative fault-tolerant control parameters.

[0090] Specifically, this embodiment calculates the maximum deviation distance (deviation space extreme value) between the real-time joint position sequence of robotic arm A (angle and displacement data of each joint at consecutive time points) and the envelope twin space of robotic arm A (a pre-established virtual safety area model). For example, if joint 3 of robotic arm A deviates from the theoretical safety boundary by 12 mm within 0.5 seconds, this value is recorded as the deviation space extreme value of the current monitoring window.

[0091] When the deviation from the spatial extreme value exceeds a preset threshold (such as 10 mm or 5° angular deviation), the safety emergency stop protocol is immediately triggered: the power supply to robotic arm A is forcibly cut off, the mechanical brake is activated to lock the joint movement, and the end effector coordinates at the moment of emergency stop (emergency stop end effector coordinates) are recorded. For example, if robotic arm A's end effector shifts by 15 mm due to a collision, exceeding the safety threshold, the stopping operation is completed within 5 milliseconds and the current position information is saved.

[0092] Based on the end-effector coordinates of the emergency stop, the original obstacle avoidance envelopes of robotic arms B and C are spatially expanded: the safety buffer area is expanded with the emergency stop coordinates as the center (e.g., the radius is increased by 200 mm), forming new expanded obstacle avoidance envelopes for B and C. Within this expanded safety constraint area, the cooperative motion trajectory (cooperative fault-tolerant trajectory) of robotic arms B and C is replanned, and the corresponding set of control parameters (cooperative fault-tolerant control parameters) is generated.

[0093] This embodiment achieves the technical effect of collaborative fault-tolerant control of the remaining robotic arms in the event of a severe robotic arm failure, ensuring that a single robotic arm failure does not affect the normal coffee beverage production process.

[0094] Example 2, based on the same inventive concept as the three-arm linkage control method applied to the coffee beverage robot in the previous examples, such as... Figure 2 As shown, this invention provides a three-arm linkage control system for coffee beverage robots, wherein the system includes:

[0095] Information retrieval unit 1 is used to retrieve target collaborative control constraints and target collaborative control parameters from the robotic arm collaborative library according to the user's order information. The target collaborative control constraints include obstacle avoidance envelopes A, B, and C corresponding to robotic arms A, B, and C, as well as multi-torque control sequences A, B, and C.

[0096] The conflict monitoring unit 2 is used to monitor the conflict during the collaborative control of coffee making by robotic arms A, B, and C using the obstacle avoidance envelopes A, B, and C.

[0097] Log call unit 3 is used to call the torque log of the A robotic arm when the A robotic arm deviates from the A obstacle avoidance envelope, and output the A multi-torque timing record.

[0098] The trajectory prediction unit 4 is used to predict the displacement trajectory of the A robotic arm based on the deviation speed characteristics of the multi-torque nodes deviating from the A multi-torque control sequence in the A multi-torque time series record, and output the updated displacement trajectory.

[0099] The fault-tolerant trajectory fitting unit 5 is used to perform collaborative fault-tolerant trajectory fitting on the B robotic arm and the C robotic arm based on the coordinates of the end gripper of the updated displacement trajectory, and output collaborative fault-tolerant control parameters.

[0100] The order processing unit 6 is used to perform three-arm fault-tolerant control and order processing after partially covering the target collaborative control parameters with the collaborative fault-tolerant control parameters.

[0101] In one embodiment, the fault-tolerant trajectory fitting unit 5 is further configured to:

[0102] Using the updated displacement trajectory as the clipping criterion, geometric constraint reconstruction is performed on the obstacle avoidance envelopes B and C respectively to generate the B-limited envelope and the C-limited envelope; using the B-limited envelope and the C-limited envelope as displacement space constraints, and based on the end gripper coordinates of the updated displacement trajectory, the B-arm and the C-arm are fitted with a collaborative fault-tolerant trajectory, and the collaborative fault-tolerant control parameters are output.

[0103] In one embodiment, the fault-tolerant trajectory fitting unit 5 is further configured to:

[0104] A preset envelope safety interval is established; the envelope safety interval is expanded spatially with the updated displacement trajectory as the center to generate an updated displacement envelope; based on the spatial overlap characteristics of the updated displacement envelope and the B obstacle avoidance envelope, the B obstacle avoidance envelope is narrowed to the B narrowed envelope; based on the spatial overlap characteristics of the updated displacement envelope and the C obstacle avoidance envelope, the C obstacle avoidance envelope is narrowed to the C narrowed envelope.

[0105] In one embodiment, the fault-tolerant trajectory fitting unit 5 is further configured to:

[0106] Using the end effector coordinates as the endpoint of the robot arm's displacement, the fault-tolerant trajectory of the B-arm is coupled within the B-bounding space to output a first cooperative fault-tolerant trajectory; using the end effector coordinates as the endpoint of the robot arm's displacement, the fault-tolerant trajectory of the C-arm is coupled within the C-bounding space to output a second cooperative fault-tolerant trajectory; a first cooperative fault-tolerant control sequence and a second cooperative fault-tolerant control sequence corresponding to the first and second cooperative fault-tolerant trajectories are matched and output, wherein the first and second cooperative fault-tolerant control sequences constitute the cooperative fault-tolerant control parameters.

[0107] In one embodiment, the log calling unit 3 is further configured to:

[0108] During the operation of the robotic arm A, multiple real-time joint position sequences of multiple a joints in the robotic arm A are collected through pose sensing; an A-envelope twin space is constructed based on the A obstacle avoidance envelope; the multiple real-time joint position sequences are synchronized to the A-envelope twin space for trajectory deviation monitoring; when the deviation regression time of the multiple real-time joint position sequences from the A-envelope twin space exceeds a preset time window, it is determined that the robotic arm A has deviated from the A obstacle avoidance envelope.

[0109] In one embodiment, the log calling unit 3 is further configured to:

[0110] Calculate the spatial extreme values ​​of the deviation of the multiple real-time joint position sequences from the twin space of envelope A; if the spatial extreme values ​​exceed a preset spatial deviation scale, then abruptly stop the A robotic arm and call the emergency stop end coordinates; after extending and expanding the B obstacle avoidance envelope and C obstacle avoidance envelope according to the emergency stop end coordinates, perform cooperative fault-tolerant trajectory fitting on the B robotic arm and the C robotic arm, and output the cooperative fault-tolerant control parameters.

[0111] In one embodiment, the trajectory prediction unit 4 is further configured to:

[0112] The A-multiple torque timing record is used to traverse and compare the A-multiple torque control sequence to obtain multiple torque deviation sequences corresponding to the multiple a joints; the torque deviation gradient is calculated for the multiple torque deviation sequences to output the torque deviation speed features of multiple joints; a torque deviation timing matrix is ​​constructed based on the torque deviation speed features of multiple joints; the displacement trajectory of the A-manipulator is fitted according to the torque deviation timing matrix to output the updated displacement trajectory.

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

Claims

1. A three-arm linkage control method applied to a coffee beverage robot, characterized in that, include: Based on the user's order information, the target collaborative control constraints and target collaborative control parameters are retrieved from the robotic arm collaborative library. The target collaborative control constraints include obstacle avoidance envelopes A, B, and C corresponding to robotic arms A, B, and C, as well as multi-torque control sequences A, B, and C. In the process of coordinating coffee making control of robotic arms A, B, and C using the target cooperative control parameters, the obstacle avoidance envelopes A, B, and C are used to monitor robotic arm operation conflicts. When the A robotic arm deviates from the A obstacle avoidance envelope, the torque log of the A robotic arm is invoked and the A multi-torque timing record is output; Based on the deviation speed characteristics of the multi-torque nodes from the A multi-torque control sequence in the A multi-torque time series record, the displacement trajectory of the A robotic arm is predicted, and the updated displacement trajectory is output. Based on the coordinates of the end gripper of the updated displacement trajectory, perform collaborative fault-tolerant trajectory fitting on robotic arms B and C, and output collaborative fault-tolerant control parameters. After the cooperative fault-tolerant control parameters are partially overridden with the target cooperative control parameters, fault-tolerant control and order processing of the three robotic arms are performed.

2. The three-arm linkage control method for coffee beverage robots as described in claim 1, characterized in that, Based on the end gripper coordinates of the updated displacement trajectory, collaborative fault-tolerant trajectory fitting is performed on robotic arms B and C, and collaborative fault-tolerant control parameters are output, including: Using the updated displacement trajectory as the clipping criterion, geometric constraint reconstruction is performed on the obstacle avoidance envelope B and obstacle avoidance envelope C respectively to generate the B-limited envelope and the C-limited envelope; Using the B-bound and C-bound envelopes as displacement space constraints, and based on the end gripper coordinates of the updated displacement trajectory, the B-arm and C-arm are fitted with a collaborative fault-tolerant trajectory, and the collaborative fault-tolerant control parameters are output.

3. The three-arm linkage control method for coffee beverage robots as described in claim 2, characterized in that, Using the updated displacement trajectory as the clipping criterion, geometric constraint reconstruction is performed on the obstacle avoidance envelopes B and C respectively to generate B-limited envelopes and C-limited envelopes, including: Preset envelope safety interval; The envelope safety interval is expanded in space with the updated displacement trajectory as the center to generate the updated displacement envelope; Based on the spatial overlap characteristics between the updated displacement envelope and the obstacle avoidance envelope B, the obstacle avoidance envelope B is narrowed to the narrowed B envelope. Based on the spatial overlap characteristics between the updated displacement envelope and the obstacle avoidance envelope, the obstacle avoidance envelope is narrowed to the narrowed C envelope.

4. The three-arm linkage control method for coffee beverage robots as described in claim 3, characterized in that, Using the B-bound and C-bound envelopes as displacement space constraints, and based on the end gripper coordinates of the updated displacement trajectory, the B-arm and C-arm are fitted with a collaborative fault-tolerant trajectory, and the collaborative fault-tolerant control parameters are output, including: Using the coordinates of the end gripper as the displacement endpoint of the robotic arm, the fault-tolerant trajectory of the B robotic arm is coupled within the B-constriction envelope to output the first cooperative fault-tolerant trajectory. Using the coordinates of the end gripper as the displacement endpoint of the robotic arm, the fault-tolerant trajectory of the C robotic arm is coupled within the C-limited envelope to output a second cooperative fault-tolerant trajectory. The matching output corresponds to the first collaborative fault-tolerant trajectory and the second collaborative fault-tolerant trajectory, which are first collaborative fault-tolerant control sequences and second collaborative fault-tolerant control sequences, wherein the first collaborative fault-tolerant control sequence and the second collaborative fault-tolerant control sequence constitute the collaborative fault-tolerant control parameters.

5. The three-arm linkage control method for coffee beverage robots as described in claim 1, characterized in that, When robotic arm A deviates from the obstacle avoidance envelope, the torque log of robotic arm A is invoked, and A multi-torque timing record is output, including: During the operation of the robotic arm A, multiple real-time joint position sequences of multiple a joints in the robotic arm A are collected through pose sensing. Construct an A-envelope twin space based on the aforementioned A-obstacle avoidance envelope; The multiple real-time joint position sequences are synchronized to the A-envelope twin space for trajectory deviation monitoring; When the deviation regression time of the multiple real-time joint position sequences from the A envelope twin space exceeds a preset time window, it is determined that the A robotic arm has deviated from the A obstacle avoidance envelope.

6. The three-arm linkage control method for coffee beverage robots as described in claim 5, characterized in that, Also includes: Calculate the deviation spatial extreme values ​​of the multiple real-time joint position sequences from the A-envelope twin space; If the deviation from the spatial extreme value exceeds the preset spatial deviation scale, then the A robotic arm is brought to an emergency stop and the emergency stop end coordinates are called. After extending the obstacle avoidance envelopes B and C based on the emergency stop end coordinates, the B robotic arm and the C robotic arm are fitted with a cooperative fault-tolerant trajectory, and the cooperative fault-tolerant control parameters are output.

7. The three-arm linkage control method for coffee beverage robots as described in claim 5, characterized in that, Based on the deviation speed characteristics of the multi-torque nodes from the A multi-torque control sequence in the A multi-torque time series record, the displacement trajectory of the A robotic arm is predicted, and the updated displacement trajectory is output, including: By traversing and comparing the A-multiple torque timing records, multiple torque deviation sequences corresponding to the multiple a joints are obtained; Torque deviation gradients are calculated for the multiple torque deviation sequences, and multiple joint torque deviation speed characteristics are output. A torque deviation time series matrix is ​​constructed based on the torque deviation speed characteristics of the multiple joints; The displacement trajectory of robotic arm A is fitted based on the torque deviation time series matrix, and the updated displacement trajectory is output.

8. A three-robotic arm linkage control system applied to a coffee beverage robot, characterized in that, The steps for implementing the method according to any one of claims 1 to 7 include: The information retrieval unit is used to retrieve target collaborative control constraints and target collaborative control parameters from the robotic arm collaboration library according to the user's order information. The target collaborative control constraints include obstacle avoidance envelopes A, B, and C corresponding to robotic arms A, B, and C, and multi-torque control sequences A, B, and C. The conflict monitoring unit is used to monitor the conflict during the collaborative control of coffee making by robotic arms A, B, and C using the obstacle avoidance envelopes A, B, and C. The log call unit is used to call the torque log of the robotic arm A when the robotic arm A deviates from the obstacle avoidance envelope of the robotic arm A, and output the multi-torque timing record of the robotic arm A. The trajectory prediction unit is used to predict the displacement trajectory of the A robotic arm based on the deviation speed characteristics of the multi-torque nodes deviating from the A multi-torque control sequence in the A multi-torque time series record, and output the updated displacement trajectory. The fault-tolerant trajectory fitting unit is used to perform collaborative fault-tolerant trajectory fitting on the B robotic arm and the C robotic arm based on the coordinates of the end gripper of the updated displacement trajectory, and output collaborative fault-tolerant control parameters. The order processing unit is used to perform three-arm fault-tolerant control and order processing after partially covering the target collaborative control parameters with the collaborative fault-tolerant control parameters.

Images