A Collaborative Path Planning Method and System for Three Robotic Arms Based on Spatial Obstacle Avoidance

Abstract

This invention provides a three-arm cooperative path planning method and system based on spatial obstacle avoidance, belonging to the field of robotic arm cooperative control technology. It analyzes intrusion monitoring results from a multi-level infrared sensing array to predict intrusion paths; starting from the real-time displacement points of the three robotic arms, it aligns the reference paths of the three robotic arms and the predicted intrusion paths spatiotemporally based on the reference displacement velocities of the three robotic arms to locate the spatiotemporal conflict points; starting from the spatiotemporal conflict points, it performs obstacle avoidance path backtracking and fitting, outputting the obstacle avoidance paths of the three robotic arms, and then performs coffee-making cooperative compensation to output the spatial cooperative path of the robotic arms. This achieves the technical effect of improving the safety of human-robot collaboration in open environments, effectively preventing the risk of robotic arm collision interruption caused by sudden intrusion, and ensuring the reliability of human-robot collaborative operation in open environments.

Description

Technical Field

[0001] This invention relates to the field of robotic arm collaborative control technology, and in particular to a three-robotic arm collaborative path planning method and system based on spatial obstacle avoidance. Background Technology

[0002] With the intelligent development of catering services, coffee robots are gradually being applied to open consumption scenarios, but their human-machine collaborative operation faces significant challenges.

[0003] Traditional coffee robots rely on fixed protection mechanisms in enclosed environments, making it difficult to adapt to the dynamic safety risks brought about by random approaching people in open scenarios.

[0004] The basic obstacle avoidance schemes used in existing technologies have the following limitations:

[0005] First, the deployment of a single sensor makes it difficult to accurately predict the intrusion trajectory, causing the robotic arm to frequently trigger the emergency stop mechanism and interrupt the production process. Second, the lack of spatiotemporal coordination in the path planning of multiple robotic arms leads to misalignment of the liquid injection and packaging processes due to deviations in the timing of process connections, affecting the consistency of beverage quality. Third, the obstacle avoidance path generation does not fully consider the motion stability of the liquid carrier, which can easily cause coffee to spill during high-speed obstacle avoidance.

[0006] These problems lead to reduced equipment operating efficiency and increased raw material loss, hindering the practical application of coffee robots in open environments. Summary of the Invention

[0007] This invention provides a three-robotic arm collaborative path planning method and system based on spatial obstacle avoidance, which is used to address the technical problem that the existing technology has insufficient safety of human-robot collaborative operation in open environments, which leads to frequent sudden stops of the robotic arms, interruption of the production process, and reduced coffee production efficiency and quality control stability.

[0008] In view of the above problems, the present invention provides a three-manipulator cooperative path planning method and system based on spatial obstacle avoidance.

[0009] The first aspect of the present invention provides a three-arm cooperative path planning method based on spatial obstacle avoidance. The method includes: deploying a K-level infrared sensing array in K annular intrusion monitoring areas of a coffee robot; activating a K-1 level infrared sensing array when the first level infrared sensing array detects a person intruding into the first annular intrusion monitoring area; initiating the K-1 level infrared sensing array mapping to perform intrusion monitoring on the K-1 environmental intrusion monitoring areas, and analyzing and outputting an intrusion prediction path based on the monitoring results; interacting with the coffee robot to obtain the real-time displacement points, reference paths, and reference displacement velocities of the three robotic arms; using the real-time displacement points of the three robotic arms as starting points, aligning the reference paths and intrusion prediction paths of the three robotic arms spatiotemporally based on the reference displacement velocities of the three robotic arms, and locating the spatiotemporal conflict points of the three robotic arms; performing obstacle avoidance path backtracking fitting starting from the spatiotemporal conflict points of the three robotic arms, and outputting the obstacle avoidance path of the three robotic arms; and performing coffee-making cooperative compensation on the obstacle avoidance path of the three robotic arms, and outputting the spatial cooperative path of the robotic arms.

[0010] In one implementation, a K-level infrared sensing array is deployed in K annular intrusion detection areas of the coffee robot, and the following processing is also performed:

[0011] By simulating the displacement of the robotic arm of the coffee robot, the working area of ​​the robotic arm is located; starting from the working area of ​​the robotic arm, the monitoring area is expanded in a ring to define K ring-shaped intrusion monitoring areas; the K-level infrared sensing array is deployed over the K ring-shaped intrusion monitoring areas.

[0012] In one implementation, the K-1 level infrared sensing array mapping is activated to perform intrusion monitoring on K-1 environmental intrusion monitoring zones, and the intrusion prediction path is analyzed and output based on the monitoring results. The following processing is also performed:

[0013] Intrusion monitoring is performed on K-1 environmental intrusion monitoring zones by mapping the K-1 level infrared sensing array, and K intrusion behavior data are output. Intrusion behavior features are collected from the K intrusion behavior data, and K intrusion behavior features are output. The intrusion behavior features include intrusion displacement velocity, intrusion displacement direction, and intrusion spatial location. Inertial analysis is performed on the K intrusion behavior features to output the intrusion prediction path.

[0014] In one embodiment, the coffee robot comprises a cup-retrieving robotic arm, a liquid-pipette robotic arm, and a packaging robotic arm.

[0015] In one implementation, starting from the real-time displacement point of the three robotic arms, the reference path of the three robotic arms and the intrusion prediction path are spatiotemporally aligned based on the reference displacement velocity of the three robotic arms to locate the spatiotemporal conflict point of the three robotic arms, and the following processing is also performed:

[0016] The cup-retrieving robot arm's real-time displacement point, reference path, and reference displacement velocity are extracted from the three robotic arms' real-time displacement points, reference paths, and reference displacement velocities. Starting from the cup-retrieving real-time displacement point, the reference path is decomposed into trajectory spatiotemporal discretization nodes based on the reference displacement velocity. The cup-retrieving spatiotemporal conflict point is located by spatiotemporally aligning the intrusion prediction path and the trajectory spatiotemporal discretization nodes. Similarly, the liquid handling spatiotemporal conflict point and the packaging spatiotemporal conflict point are located, where the cup-retrieving spatiotemporal conflict point, liquid handling spatiotemporal conflict point, and packaging spatiotemporal conflict point constitute the three robotic arms' spatiotemporal conflict points.

[0017] In one implementation, obstacle avoidance path backtracking and fitting is performed starting from the spatiotemporal conflict point of the three robotic arms to output the obstacle avoidance path of the three robotic arms, and the following processing is also performed:

[0018] Starting from the spatiotemporal conflict point of cup retrieval, locate the cup retrieval obstacle avoidance start point and cup retrieval obstacle avoidance end point along the cup retrieval reference path; construct multiple obstacle avoidance candidate paths between the cup retrieval obstacle avoidance start point and cup retrieval obstacle avoidance end point; using the cup retrieval reference displacement velocity as a constraint, perform a spill prevention simulation on the multiple obstacle avoidance candidate paths to filter out the target obstacle avoidance path; based on the cup retrieval obstacle avoidance start point and cup retrieval obstacle avoidance end point, replace the target obstacle avoidance path with the cup retrieval reference path to obtain the cup retrieval obstacle avoidance path; and so on, perform obstacle avoidance path backtracking fitting starting from the spatiotemporal conflict point of the three robotic arms to output the obstacle avoidance path of the three robotic arms, wherein the obstacle avoidance path of the three robotic arms includes a cup retrieval obstacle avoidance path, a liquid handling obstacle avoidance path, and a packaging obstacle avoidance path.

[0019] In one implementation, the obstacle avoidance path of the three robotic arms is compensated for collaboratively in coffee making, and a collaborative spatial path for the robotic arms is output. The following processing is also performed:

[0020] The error of the coffee making process is calculated for the cup-picking obstacle avoidance path, liquid-pipette obstacle avoidance path, and packaging obstacle avoidance path. Based on the calculation results, the robot arm displacement is collaboratively compensated, and the cup-picking compensation path, liquid-pipette compensation path, and packaging compensation path are output. The complexity of the robot arm displacement control is evaluated for the cup-picking compensation path, liquid-pipette compensation path, and packaging compensation path to select the target compensation path. The target compensation path is used to locally replace the three robot arm obstacle avoidance paths, and the robot arm spatial collaborative path is output.

[0021] A second aspect of the present invention provides a three-robotic arm collaborative path planning system based on spatial obstacle avoidance. The system includes: a perception deployment unit for deploying a K-level infrared sensing array in K annular intrusion detection areas of a coffee robot; a perception activation unit for activating a K-1 level infrared sensing array when the first level infrared sensing array detects a person intruding into the first annular intrusion detection area; an intrusion detection unit for initiating the K-1 level infrared sensing array mapping to perform intrusion detection on the K-1 environmental intrusion detection areas, and analyzing and outputting an intrusion prediction path based on the monitoring results; and a parameter calling unit for... The coffee robot obtains the real-time displacement points, reference paths, and reference displacement velocities of its three robotic arms. A conflict localization unit, starting from the real-time displacement points, aligns the reference paths and intrusion prediction paths of the three robotic arms in time and space based on their reference displacement velocities to locate the spatiotemporal conflict points. An obstacle avoidance planning unit, starting from the spatiotemporal conflict points, performs obstacle avoidance path backtracking and fitting, outputting the obstacle avoidance paths of the three robotic arms. A collaborative compensation unit performs coffee-making collaborative compensation on the obstacle avoidance paths of the three robotic arms, outputting the spatial collaborative paths of the robotic arms.

[0022] The technical solution provided in this invention has at least the following technical effects or advantages:

[0023] The method provided in this invention deploys a K-level infrared sensing array in K annular intrusion monitoring areas of a coffee robot. When the first-level infrared sensing array detects a person intruding into the first annular intrusion monitoring area, a K-1 level infrared sensing array is activated. The K-1 level infrared sensing array is then used to map and monitor the K-1 environmental intrusion monitoring areas, and an intrusion prediction path is output based on the monitoring results. The coffee robot interacts with the sensor to obtain the real-time displacement points, reference paths, and reference displacement velocities of its three robotic arms. Starting from the real-time displacement points, the reference paths and intrusion prediction paths of the three robotic arms are spatiotemporally aligned based on their reference displacement velocities to locate the spatiotemporal conflict points. Obstacle avoidance path backtracking and fitting are performed starting from these conflict points to output the obstacle avoidance paths of the three robotic arms. Collaborative compensation for coffee making is applied to the obstacle avoidance paths of the three robotic arms to output the spatial collaborative paths of the robotic arms. This method achieves the technical effect of improving the safety of human-robot collaboration in open environments, effectively preventing the risk of robotic arm collision interruption caused by sudden intrusion, and ensuring the reliability of human-robot collaborative operation in open environments. Attached Figure Description

[0024] Figure 1 A schematic diagram of the three-manipulator cooperative path planning method based on spatial obstacle avoidance provided by the present invention is shown.

[0025] Figure 2A schematic diagram of the structure of the three robotic arm cooperative path planning system based on spatial obstacle avoidance provided by the present invention is shown.

[0026] Explanation of reference numerals in the attached diagram: Perception deployment unit 1, Perception activation unit 2, Intrusion detection unit 3, Parameter calling unit 4, Conflict location unit 5, Obstacle avoidance planning unit 6, Cooperative compensation unit 7. Detailed Implementation

[0027] This invention provides a three-robotic arm collaborative path planning method and system based on spatial obstacle avoidance, which is used to address the technical problem in the prior art where insufficient safety of human-robot collaborative operation in open environments leads to frequent sudden stops of the robotic arms, causing interruptions in the production process and resulting in reduced coffee production efficiency and quality control stability.

[0028] 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.

[0029] Example 1: A flowchart of the three-manipulator cooperative path planning method based on spatial obstacle avoidance provided in this embodiment of the invention is shown below. Figure 1 The method includes:

[0030] Step S100: Deploy a K-level infrared sensing array in the K annular intrusion detection areas of the coffee robot.

[0031] In one embodiment, a K-level infrared sensing array is deployed in K annular intrusion detection areas of the coffee robot. Step S100 provided by the present invention further includes:

[0032] Step S110: The working area of ​​the robotic arm is located by simulating the displacement of the robotic arm of the coffee robot.

[0033] Step S120: Starting from the working area of ​​the robotic arm, expand the monitoring area in a ring to define K ring-shaped intrusion monitoring areas.

[0034] Step S130: Deploy the K-layer infrared sensing array covering the K annular intrusion monitoring areas.

[0035] In this embodiment, based on the robot's forward kinematics algorithm, the motion combinations of each joint of the robotic arm within its maximum stroke range are simulated to accurately calculate the limit poses achievable by the end effector in three-dimensional space. Through repeated iterative optimization, the effective working envelope of the robotic arm when completing core processes such as cup picking, liquid injection, and packaging is finally determined, forming a three-dimensional model of the robotic arm's working area that includes safety margins. The definition of the robotic arm's working area provides a spatial reference for subsequent monitoring range division.

[0036] Centered on the origin of the robotic arm's base coordinate system, and following international safety standards for human-machine collaboration safety distances, a ring-shaped monitoring zone extends outwards in layers. The innermost monitoring zone is flush with the boundary of the robotic arm's working area, primarily preventing direct contact risks during operation. The middle monitoring zone covers the human-machine interaction buffer zone, used to detect potential threats approaching. The outermost monitoring zone extends to the public area surrounding the equipment, enabling early intrusion warnings. The radius of each ring-shaped zone is dynamically adjusted according to the robotic arm's movement speed to ensure consistency in the risk response time window.

[0037] The K-layer infrared sensing array is deployed across the K ring-shaped intrusion monitoring areas. The sensors at each layer achieve data fusion through a time synchronization protocol, constructing a multi-level defense system from near to far and from precise to wide coverage. At the same time, the sensor deployment density follows the principle of equal coverage intensity to ensure that there are no blind spots in the monitoring area.

[0038] Step S200: When the first-level infrared sensing array detects personnel intrusion into the first ring-shaped intrusion monitoring area, the K-1 level infrared sensing array is activated.

[0039] Specifically, when the first-level infrared sensing array deployed on the outermost layer detects a human body entering the first ring-shaped intrusion monitoring zone (ring-shaped monitoring boundary), it immediately activates the hierarchical interlocking response mechanism and sends an activation command to the adjacent inner K-1 level sensor array.

[0040] This hierarchical wake-up strategy, from the outside in, can reduce energy consumption during routine monitoring and ensure rapid improvement in internal monitoring accuracy when potential threats are detected.

[0041] Meanwhile, the activation delay of sensors at each level is limited to within 50 milliseconds to ensure continuous tracking of intrusion targets from the periphery to the core area.

[0042] Step S300: Start the K-1 level infrared sensing array mapping to perform intrusion monitoring on K-1 environmental intrusion monitoring areas, and analyze and output the intrusion prediction path based on the monitoring results.

[0043] In one embodiment, the K-1 level infrared sensing array mapping is activated to perform intrusion monitoring on K-1 environmental intrusion monitoring zones, and the intrusion prediction path is analyzed and output based on the monitoring results. Step S300 of the method provided by this invention further includes:

[0044] Step S310: Intrusion monitoring is performed on K-1 environmental intrusion monitoring zones through the K-1 level infrared sensing array mapping, and K intrusion behavior data are output.

[0045] Step S320: Collect intrusion behavior features from the K intrusion behavior data and output K intrusion behavior features, wherein the intrusion behavior features include intrusion displacement velocity, intrusion displacement direction and intrusion spatial location.

[0046] Step S330: Perform motion inertial analysis on the K intrusion behavior features and output the intrusion prediction path.

[0047] Specifically, in the collaborative working mode of the K-1 level infrared sensing array, K-1 groups of infrared sensing devices in K-1 environmental intrusion monitoring zones synchronously collect environmental data, and then convert the discrete sensor raw data into a standardized dataset through timestamp alignment and spatial coordinate correction to obtain the K intrusion behavior data.

[0048] Each intrusion behavior data includes core information such as target ID, spatial coordinates, and time stamp, forming a traceable intrusion behavior data chain. The data chain is updated every 10 milliseconds to ensure high-frequency sampling of fast-moving targets.

[0049] Using a velocity vector decomposition algorithm, intrusion behavior characteristics are calculated from the K intrusion behavior data to obtain the instantaneous movement speed and direction angle of the intrusion target in three-dimensional space. Furthermore, combined with historical trajectory data, the spatial distribution characteristics of the intrusion target are extracted to identify the regularity of its movement patterns, ultimately outputting K intrusion behavior characteristics, which consist of intrusion displacement velocity, intrusion displacement direction, and intrusion spatial location.

[0050] By spatiotemporally connecting the K intrusion behavior features, a motion vector field of the intrusion target in three-dimensional space is constructed. Then, based on the principle of human kinematics, the acceleration change law of the target between adjacent monitoring zones is analyzed, the continuous effect of the intrusion target's motion inertia is deduced, and an intrusion prediction path reflecting the natural movement trend of the intrusion target is generated. Each path point in the intrusion prediction path has an intrusion prediction time identifier.

[0051] This embodiment achieves high-precision prediction of the future movement trajectory of an intrusion target through the collaborative monitoring of multi-level sensors and human motion inertial modeling, providing a reliable predictive basis for the dynamic path planning of the robotic arm and improving the technical effect of enhancing the safety of human-machine collaborative operation in open environments.

[0052] Step S400: Interact with the coffee robot to obtain the real-time displacement points of the three robotic arms, the reference paths of the three robotic arms, and the reference displacement speeds of the three robotic arms.

[0053] In one embodiment, the coffee robot comprises a cup-retrieving robotic arm, a liquid-pipette robotic arm, and a packaging robotic arm.

[0054] Specifically, the coffee robot consists of three robotic arms: a cup-retrieving robotic arm, a liquid-piping robotic arm, and a packaging robotic arm. Correspondingly, the real-time displacement points of the three robotic arms obtained through interaction with the coffee robot specifically include the real-time displacement point for cup retrieval, the real-time displacement point for liquid-piping, and the real-time displacement point for packaging. The real-time displacement point refers to the real-time spatial position of the robotic arm gripper.

[0055] Similarly, the three robotic arm reference paths specifically include a cup-picking reference path, a liquid-piping reference path, and a packaging reference path. The reference path is the standard movement path of the robotic arm when performing a task.

[0056] Similarly, the reference displacement speeds of the three robotic arms include the cup-picking reference displacement speed, the liquid-pipette reference displacement speed, and the packaging reference displacement speed. The reference displacement speed is the standard moving speed of the robotic arm when it moves along the reference path. This speed can effectively prevent coffee from spilling.

[0057] Step S500: Starting from the real-time displacement point of the three robotic arms, align the reference path of the three robotic arms and the intrusion prediction path in time and space according to the reference displacement velocity of the three robotic arms, and locate the time and space conflict point of the three robotic arms.

[0058] In one embodiment, taking the real-time displacement point of the three robotic arms as the starting point, and aligning the reference path of the three robotic arms and the intrusion prediction path in time and space according to the reference displacement velocity of the three robotic arms, the time and space conflict point of the three robotic arms is located. The method step S500 provided by the present invention further includes:

[0059] Step S510: Extract the cup-picking real-time displacement point, cup-picking reference path, and cup-picking reference displacement speed of the cup-picking robot arm from the real-time displacement points, reference paths, and reference displacement speeds of the three robotic arms.

[0060] Step S520: Taking the real-time displacement point of cup retrieval as the starting point, decompose the cup retrieval reference path into trajectory spatiotemporal discretization nodes according to the cup retrieval reference displacement velocity.

[0061] Step S530: Locate the spatiotemporal conflict point of cup taking by aligning the spatiotemporal discretized nodes of the intrusion prediction path and trajectory in spatiotemporal space.

[0062] Step S540: By analogy, locate the spatiotemporal conflict points of pipetting and packaging, wherein the spatiotemporal conflict points of cup picking, pipetting, and packaging constitute the spatiotemporal conflict points of the three robotic arms.

[0063] Specifically, the cup-picking robot arm's real-time displacement point, cup-picking reference path, and cup-picking reference displacement speed are extracted from the three robotic arms' real-time displacement points, reference paths, and reference displacement speeds.

[0064] Starting from the current actual position of the cup-retrieving robotic arm, the preset cup-retrieving reference path is discretized into a sequence of time-marked trajectory points according to the cup-retrieving reference displacement speed, which is the trajectory spatiotemporal discretization node.

[0065] In the trajectory spatiotemporal discretization nodes, each discrete node contains precise spatial coordinates and a corresponding expected arrival time, forming a theoretical motion trajectory chain of the robotic arm in four-dimensional spatiotemporal space.

[0066] This discretization process transforms a continuous path into a time-space coupled set of nodes, enabling the motion trajectories of the robotic arm and the intruding target to be compared and analyzed within a unified spatiotemporal framework.

[0067] The spatiotemporal mapping engine performs a four-dimensional matching analysis between the intrusion prediction path of the intrusion target and the spatiotemporal discretized nodes of the cup-retrieving robotic arm's trajectory. Specifically, it calculates the spatial distance between the two in each time slice. When it is detected that the distance between the end of the robotic arm and the intrusion target is lower than the dynamic safety threshold at a certain moment, the spatiotemporal node is marked as a potential spatiotemporal conflict point for cup retrieval.

[0068] The collision point determination comprehensively considers the braking distance margin caused by the inertia of the robotic arm and the uncertainty tolerance of human motion prediction to ensure the reliability of the recognition results.

[0069] Similarly, using the same spatiotemporal mapping and conflict detection logic, the standard paths of the pipetting robot and the packaging robot are analyzed frame by frame to locate the spatiotemporal conflict points of pipetting and packaging. The spatiotemporal conflict points of cup picking, pipetting, and packaging constitute the spatiotemporal conflict points of the three robots.

[0070] This embodiment uses spatiotemporal alignment and discretized path analysis to accurately locate the four-dimensional conflict points between the three robotic arms and the intrusion path, construct a global risk topology map, realize a collaborative benchmark for dynamic obstacle avoidance of multiple processes, and ensure the safety of human-machine collaboration and the continuity of the coffee making process in open scenarios.

[0071] Step S600: Starting from the spatiotemporal conflict point of the three robotic arms, perform obstacle avoidance path backtracking fitting and output the obstacle avoidance path of the three robotic arms.

[0072] In one embodiment, obstacle avoidance path backtracking and fitting are performed starting from the spatiotemporal conflict point of the three robotic arms, and the obstacle avoidance path of the three robotic arms is output. Step S600 of the method provided by this invention further includes:

[0073] Step S610: Starting from the spatiotemporal conflict point of cup retrieval, locate the cup retrieval obstacle avoidance start point and cup retrieval obstacle avoidance end point along the cup retrieval reference path.

[0074] Step S620: Construct multiple alternative obstacle avoidance paths between the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point.

[0075] Step S630: Using the cup-taking reference displacement velocity as a constraint, perform a tipping and spill prevention simulation on the multiple obstacle avoidance candidate paths to select the target obstacle avoidance path.

[0076] Step S640: Based on the cup-collecting obstacle avoidance starting point and cup-collecting obstacle avoidance ending point, replace the target obstacle avoidance path with the cup-collecting reference path to obtain the cup-collecting obstacle avoidance path.

[0077] Step S650: Similarly, starting from the spatiotemporal conflict point of the three robotic arms, perform obstacle avoidance path backtracking and fitting, and output the obstacle avoidance path of the three robotic arms, wherein the obstacle avoidance path of the three robotic arms includes a cup picking obstacle avoidance path, a liquid pipetting obstacle avoidance path, and a packaging obstacle avoidance path.

[0078] Specifically, for the spatiotemporal conflict point of the cup-retrieving robotic arm, the nearest feasible path bifurcation point is searched forward along its original cup-retrieving baseline path as the obstacle avoidance starting position, and at the same time, it is extended backward to the first safe connection point that allows path return as the obstacle avoidance termination position.

[0079] The starting and ending points of the cup-picking obstacle avoidance system constitute the replacement interval of the local obstacle avoidance path. The determination of this interval must ensure a smooth transition of the robotic arm's joint angular velocity, avoiding vibration issues caused by sudden stops and starts. The distance between the starting and ending points is dynamically adjusted based on the safety margin around the conflict point, ensuring sufficient detour space along the obstacle avoidance path segment.

[0080] Within the obstacle avoidance interval formed by the cup-picking obstacle avoidance start point and the cup-picking obstacle avoidance end point, multiple obstacle avoidance candidate paths are generated based on the fast random tree algorithm. These multiple obstacle avoidance candidate paths exhibit different avoidance curve shapes in three-dimensional space, each corresponding to a different obstacle avoidance strategy:

[0081] For example, the high-level lifting path crosses the risk zone by increasing the height of the cup retrieval, the lateral offset path uses the redundant degrees of freedom of the robotic arm to avoid collisions laterally, and the time-delay path uses speed control to stagger the time-space conflict window.

[0082] Each obstacle avoidance alternative path is accompanied by kinematic parameter labels, including joint rotation sequence, terminal acceleration curve, estimated energy consumption and other characteristic dimensions.

[0083] Using the reference displacement velocity for cup retrieval as a constraint, dynamic simulations were performed on candidate paths to evaluate the stability of the liquid during the retrieval process. A model of liquid surface sloshing within the cup was constructed to simulate the liquid trajectory under different acceleration curves, calculating the maximum sloshing amplitude and overflow risk coefficient. Simultaneously, the vibration spectrum of the robotic arm executing each path was detected to eliminate motion modes that might cause resonance. The screening criteria required that the liquid sloshing height of the candidate path not exceed 5% of the cup height, and the end-effector vibration amplitude be controlled within ±1 mm to ensure that the beverage quality is not affected by obstacle avoidance actions. Finally, the target obstacle avoidance path was selected.

[0084] After selecting the optimal obstacle avoidance path, the velocity and acceleration of the old and new path segments are continuously connected through a fifth-order polynomial interpolation algorithm. Then, based on the cup-picking obstacle avoidance start point and cup-picking obstacle avoidance end point, the original path point sequence of the replacement interval is replaced with the optimized obstacle avoidance path point set. At the same time, the path points of adjacent segments are verified by inverse kinematics calculation to ensure the smoothness of joint motion of the entire cup-picking path, thus obtaining the replaced cup-picking obstacle avoidance path.

[0085] The cup-retrieving obstacle avoidance path envelopes the look-ahead buffer of the cup-retrieving robotic arm controller, enabling seamless switching of motion commands and avoiding servo motor overload caused by sudden path changes.

[0086] Similarly, starting from the spatiotemporal conflict point of the three robotic arms, the obstacle avoidance path is backtracked and fitted to output the obstacle avoidance path of the three robotic arms. The obstacle avoidance path of the three robotic arms includes the cup-picking obstacle avoidance path, the liquid-piping obstacle avoidance path, and the packaging obstacle avoidance path.

[0087] This embodiment generates an optimized path that balances obstacle avoidance safety and liquid spill prevention through path optimization and dynamic simulation verification under multiple constraints, achieving adaptive adjustment of dynamic safety margin and smooth path transition.

[0088] Step S700: Perform coffee-making collaborative compensation on the obstacle avoidance path of the three robotic arms, and output the spatial collaborative path of the robotic arms.

[0089] In one embodiment, the obstacle avoidance path of the three robotic arms is compensated for collaboratively in coffee making, and a collaborative spatial path for the robotic arms is output. Step S700 of the method provided by this invention further includes:

[0090] Step S710: Calculate the coffee making process error for the cup-picking obstacle avoidance path, liquid-pipette obstacle avoidance path, and packaging obstacle avoidance path, and perform robotic arm displacement collaborative compensation based on the calculation results, outputting the cup-picking compensation path, liquid-pipette compensation path, and packaging compensation path.

[0091] Step S720: Evaluate the complexity of the robotic arm displacement control for the cup-picking compensation path, the liquid-pipette compensation path, and the packaging compensation path to select target compensation paths.

[0092] Step S730: Replace the obstacle avoidance path of the three robotic arms with the target compensation path locally, and output the spatial cooperative path of the robotic arms.

[0093] Specifically, the obstacle avoidance path of each robotic arm is compensated and adjusted independently to ensure that the predetermined obstacle avoidance paths of other robotic arms are not changed when optimizing a single robotic arm. When compensating for the cup-retrieving robotic arm, the original obstacle avoidance paths of the pipetting and sealing robotic arms remain unchanged, and only the cup-retrieving path is locally modified.

[0094] By employing a reverse-engineering algorithm to trace the source of process errors, critical segments in the cup-retrieving path that cause timing deviations are accurately identified. Speed ​​adjustment points or trajectory fine-tuning points are then inserted within these segments to minimize the scope of path modifications. The compensated cup-retrieving path maintains timing errors within ±50 milliseconds compared to the original process at key nodes such as entering and exiting the ice zone and cup transfer, while ensuring that the liquid injection start-up waiting time of the pipetting robot arm remains unaffected. The resulting compensated path modifies only necessary motion segments, maximizing the maintenance of the overall system's collaborative baseline.

[0095] Similarly, without changing the other two obstacle avoidance paths, a pipetting compensation path and a packaging compensation path are generated, and they match the original process sequence.

[0096] Independent controllability assessments are performed on the compensation paths generated by individual robotic arms to avoid the surge in complexity caused by cross-arm parameter coupling. The assessment focuses on the characteristics of the compensation path itself: the joint motion spectrum is analyzed using Discrete Fourier Transform to eliminate high-frequency vibration components. The path curvature change rate is calculated to ensure it does not exceed the servo system's tracking capability. The spatial deviation between the end effector trajectory and the process reference path is measured and controlled within a ±2 mm process tolerance band.

[0097] While ensuring the quality of individual paths, the compensation scheme with the smallest modification is preferred. For example, for the cup-retrieving robotic arm, a micro-compensation scheme that only adjusts 3 path points is preferred, rather than overall path reconstruction. This reduces control complexity by 62%, and its complexity is lower than other compensation paths. Therefore, the cup-retrieving compensation path is selected as the target compensation path.

[0098] Based on the robotic arm to which the target compensation path belongs, the obstacle avoidance path is locally replaced in the obstacle avoidance path of the three robotic arms, and the spatial cooperative path of the robotic arms is output.

[0099] After receiving the spatial collaborative path of the robotic arms, the control module of the coffee robot synchronously coordinates the movement trajectories of the three robotic arms according to the spatial collaborative path. During obstacle avoidance, it maintains the stable posture of the cup, avoids liquid spillage, ensures a safe distance and smooth operation of human-machine collaboration in an open environment, and maintains the standardized quality and stable output efficiency of coffee making.

[0100] This embodiment achieves the technical effect of improving the safety of human-machine collaboration in open environments, effectively preventing the risk of mechanical arm collision interruption caused by sudden intrusion, and ensuring the reliability of human-machine collaborative operation in open environments.

[0101] Example 2, based on the same inventive concept as the three-robotic arm cooperative path planning method based on spatial obstacle avoidance in the previous examples, such as... Figure 2 As shown, this invention provides a three-manipulator cooperative path planning system based on spatial obstacle avoidance, wherein the system includes:

[0102] The perception deployment unit 1 is used to deploy a K-level infrared perception array in the K annular intrusion detection areas of the coffee robot.

[0103] The sensing activation unit 2 is used to activate the K-1 level infrared sensing array when the first-level infrared sensing array detects personnel intrusion into the first ring intrusion monitoring area.

[0104] The intrusion monitoring unit 3 is used to initiate the K-1 level infrared sensing array mapping to perform intrusion monitoring on K-1 environmental intrusion monitoring areas, and analyze and output the intrusion prediction path based on the monitoring results.

[0105] The parameter calling unit 4 is used to interact with the coffee robot to obtain the real-time displacement points of the three robotic arms, the reference path of the three robotic arms, and the reference displacement speed of the three robotic arms.

[0106] The conflict localization unit 5 is used to locate the spatiotemporal conflict point of the three robotic arms by taking the real-time displacement point of the three robotic arms as the starting point and aligning the reference path and intrusion prediction path of the three robotic arms in time and space according to the reference displacement velocity of the three robotic arms.

[0107] The obstacle avoidance planning unit 6 is used to perform obstacle avoidance path backtracking fitting starting from the spatiotemporal conflict point of the three robotic arms, and output the obstacle avoidance path of the three robotic arms.

[0108] The collaborative compensation unit 7 is used to perform collaborative compensation on the obstacle avoidance path of the three robotic arms and output the spatial collaborative path of the robotic arms.

[0109] In one embodiment, the sensing deployment unit 1 is further configured to:

[0110] By simulating the displacement of the robotic arm of the coffee robot, the working area of ​​the robotic arm is located; starting from the working area of ​​the robotic arm, the monitoring area is expanded in a ring to define K ring-shaped intrusion monitoring areas; the K-level infrared sensing array is deployed over the K ring-shaped intrusion monitoring areas.

[0111] In one embodiment, the intrusion detection unit 3 is further configured to:

[0112] Intrusion monitoring is performed on K-1 environmental intrusion monitoring zones by mapping the K-1 level infrared sensing array, and K intrusion behavior data are output. Intrusion behavior features are collected from the K intrusion behavior data, and K intrusion behavior features are output. The intrusion behavior features include intrusion displacement velocity, intrusion displacement direction, and intrusion spatial location. Inertial analysis is performed on the K intrusion behavior features to output the intrusion prediction path.

[0113] In one embodiment, the coffee robot comprises a cup-retrieving robotic arm, a liquid-pipette robotic arm, and a packaging robotic arm.

[0114] In one embodiment, the conflict localization unit 5 is further configured to:

[0115] The cup-retrieving robot arm's real-time displacement point, reference path, and reference displacement velocity are extracted from the three robotic arms' real-time displacement points, reference paths, and reference displacement velocities. Starting from the cup-retrieving real-time displacement point, the reference path is decomposed into trajectory spatiotemporal discretization nodes based on the reference displacement velocity. The cup-retrieving spatiotemporal conflict point is located by spatiotemporally aligning the intrusion prediction path and the trajectory spatiotemporal discretization nodes. Similarly, the liquid handling spatiotemporal conflict point and the packaging spatiotemporal conflict point are located, where the cup-retrieving spatiotemporal conflict point, liquid handling spatiotemporal conflict point, and packaging spatiotemporal conflict point constitute the three robotic arms' spatiotemporal conflict points.

[0116] In one embodiment, the obstacle avoidance planning unit 6 is further configured to:

[0117] Starting from the spatiotemporal conflict point of cup retrieval, locate the cup retrieval obstacle avoidance start point and cup retrieval obstacle avoidance end point along the cup retrieval reference path; construct multiple obstacle avoidance candidate paths between the cup retrieval obstacle avoidance start point and cup retrieval obstacle avoidance end point; using the cup retrieval reference displacement velocity as a constraint, perform a spill prevention simulation on the multiple obstacle avoidance candidate paths to filter out the target obstacle avoidance path; based on the cup retrieval obstacle avoidance start point and cup retrieval obstacle avoidance end point, replace the target obstacle avoidance path with the cup retrieval reference path to obtain the cup retrieval obstacle avoidance path; and so on, perform obstacle avoidance path backtracking fitting starting from the spatiotemporal conflict point of the three robotic arms to output the obstacle avoidance path of the three robotic arms, wherein the obstacle avoidance path of the three robotic arms includes a cup retrieval obstacle avoidance path, a liquid handling obstacle avoidance path, and a packaging obstacle avoidance path.

[0118] In one embodiment, the collaborative compensation unit 7 is further configured to:

[0119] The error of the coffee making process is calculated for the cup-picking obstacle avoidance path, liquid-pipette obstacle avoidance path, and packaging obstacle avoidance path. Based on the calculation results, the robot arm displacement is collaboratively compensated, and the cup-picking compensation path, liquid-pipette compensation path, and packaging compensation path are output. The complexity of the robot arm displacement control is evaluated for the cup-picking compensation path, liquid-pipette compensation path, and packaging compensation path to select the target compensation path. The target compensation path is used to locally replace the three robot arm obstacle avoidance paths, and the robot arm spatial collaborative path is output.

[0120] 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 method for collaborative path planning of three robots based on spatial obstacle avoidance, characterized in that, Comprise: Deploy K-level infrared sensing arrays in K annular intrusion monitoring area of coffee robot; When the first level infrared sensing array monitors the personnel intrusion into the first annular intrusion monitoring area, activate K-1 level infrared sensing array; Start the K-1 level infrared sensing array mapping to monitor K-1 environmental intrusion monitoring area, and output the intrusion prediction path according to the monitoring result analysis; Interact the coffee robot to obtain three mechanical arm real-time displacement points, three mechanical arm reference path and three mechanical arm reference displacement speed; With the three mechanical arm real-time displacement point as the starting point, according to the three mechanical arm reference displacement speed, space-time alignment of the three mechanical arm reference path and the intrusion prediction path, positioning three mechanical arm space-time conflict point; With the three mechanical arm space-time conflict point as the starting point, obstacle avoidance path backtracking fitting is carried out, and three mechanical arm obstacle avoidance path is output; The coffee making collaborative compensation is carried out on the three mechanical arm obstacle avoidance path, and the mechanical arm space collaborative path is output; Wherein, the three mechanical arm of the coffee robot comprises a cup taking mechanical arm, a pipetting mechanical arm and an encapsulation mechanical arm; With the three mechanical arm real-time displacement point as the starting point, according to the three mechanical arm reference displacement speed, space-time alignment of the three mechanical arm reference path and the intrusion prediction path, positioning three mechanical arm space-time conflict point, comprising: Extract the cup taking real-time displacement point, cup taking reference path and cup taking reference displacement speed of the cup taking mechanical arm from the three mechanical arm real-time displacement point, three mechanical arm reference path and three mechanical arm reference displacement speed; With the cup taking real-time displacement point as the starting point, according to the cup taking reference displacement speed, the cup taking reference path is decomposed into trajectory space-time discrete nodes; Through space-time alignment of the intrusion prediction path and the trajectory space-time discrete nodes, the cup taking space-time conflict point is positioned; By analogy, the pipetting space-time conflict point and the encapsulation space-time conflict point are positioned, wherein the cup taking space-time conflict point, the pipetting space-time conflict point and the encapsulation space-time conflict point constitute the three mechanical arm space-time conflict point.

2. The method of claim 1, wherein, Deploy K-level infrared sensing arrays in K annular intrusion monitoring area of coffee robot, comprising: Through mechanical arm displacement simulation of coffee robot, the mechanical arm working area is positioned; With the mechanical arm working area as the starting point, the annular expansion of the monitoring area is carried out, and K annular intrusion monitoring area is framed; The K level infrared sensing array is deployed in the K annular intrusion monitoring area.

3. The method of claim 1, wherein, Start the K-1 level infrared sensing array mapping to monitor K-1 environmental intrusion monitoring area, and output the intrusion prediction path according to the monitoring result analysis, comprising: Through the K-1 level infrared sensing array mapping, K-1 environmental intrusion monitoring area is monitored, and K intrusion behavior data is output; Intrusion behavior feature collection is carried out on the K intrusion behavior data, and K intrusion behavior feature is output, wherein the intrusion behavior feature comprises intrusion displacement speed, intrusion displacement direction and intrusion space position; Mobile inertia analysis is carried out on the K intrusion behavior feature, and the intrusion prediction path is output.

4. The method of claim 1, wherein, With the three mechanical arm space-time conflict point as the starting point, obstacle avoidance path backtracking fitting is carried out, and three mechanical arm obstacle avoidance path is output, comprising: A cup-taking obstacle avoidance starting point and a cup-taking obstacle avoidance ending point are located along the cup-taking reference path starting from the cup-taking time-space conflict point; A plurality of obstacle avoidance alternative paths are constructed between the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point; A target obstacle avoidance path is screened out by simulating spilling prevention of pouring with the cup-taking reference displacement speed as a constraint; The target obstacle avoidance path is replaced into the cup-taking reference path according to the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point, and a cup-taking obstacle avoidance path is obtained; By analogy, obstacle avoidance path backtracking fitting is performed starting from the three-robot time-space conflict point, and the three-robot obstacle avoidance path is output, wherein the three-robot obstacle avoidance path includes a cup-taking obstacle avoidance path, a pipetting obstacle avoidance path, and a packaging obstacle avoidance path.

5. The three-robot collaborative path planning method based on space obstacle avoidance according to claim 4, wherein, The three-robot obstacle avoidance path is subjected to coffee making collaborative compensation, and a robot spatial collaborative path is output, including: The cup-taking obstacle avoidance path, the pipetting obstacle avoidance path, and the packaging obstacle avoidance path are subjected to coffee making process error calculation, and robot displacement collaborative compensation is performed according to the calculation results, and a cup-taking compensation path, a pipetting compensation path, and a packaging compensation path are output; The cup-taking compensation path, the pipetting compensation path, and the packaging compensation path are subjected to robot displacement control complexity evaluation to screen a target compensation path; The target compensation path is locally replaced into the three-robot obstacle avoidance path, and the robot spatial collaborative path is output.

6. A three-robot collaborative path planning system based on spatial obstacle avoidance, characterized in that, Steps for implementing the method of any one of claims 1 to 5, including: A perception deployment unit is configured to deploy K-level infrared perception arrays in K annular intrusion monitoring areas of a coffee robot; A perception activation unit is configured to activate K-1 level infrared perception arrays when the first level infrared perception array monitors a person intruding into the first annular intrusion monitoring area; An intrusion monitoring unit is configured to start the K-1 level infrared perception array to map and monitor K-1 environmental intrusion monitoring areas, and output an intrusion prediction path according to the monitoring result; A parameter calling unit is configured to interact with the coffee robot to obtain a three-robot real-time displacement point, a three-robot reference path, and a three-robot reference displacement speed; A conflict positioning unit is configured to position a three-robot time-space conflict point by starting from the three-robot real-time displacement point and aligning the three-robot reference path and the intrusion prediction path according to the three-robot reference displacement speed; An obstacle avoidance planning unit is configured to output a three-robot obstacle avoidance path by performing obstacle avoidance path backtracking fitting starting from the three-robot time-space conflict point; A collaborative compensation unit is configured to output a robot spatial collaborative path by performing coffee making collaborative compensation on the three-robot obstacle avoidance path.

Images