Three-mechanical-arm cooperative path planning method and system based on space obstacle avoidance

Abstract

The invention provides a three-mechanical-arm cooperative path planning method and system based on space obstacle avoidance, and relates to the technical field of mechanical arm cooperative control. An intrusion prediction path is analyzed according to an intrusion monitoring result of a multi-level infrared sensing array; the three-mechanical-arm real-time displacement point serves as a starting point, the three-mechanical-arm reference path and the intrusion prediction path are aligned in a space-time mode according to the three-mechanical-arm reference displacement speed, and three-mechanical-arm space-time conflict points are positioned; and obstacle avoidance path backtracking fitting is carried out with the three-mechanical-arm space-time conflict point as a starting point, coffee making cooperative compensation is carried out after the three-mechanical-arm obstacle avoidance path is output, and a mechanical arm space cooperative path is output. The technical effects that the man-machine cooperation safety of the coffee robot in the open environment is improved, the mechanical arm collision interruption risk caused by sudden invasion is effectively prevented, and the man-machine cooperation operation reliability in the open environment is guaranteed are achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of collaborative control of robotic arms, and in particular to a method and system for collaborative path planning of three robotic arms based on spatial obstacle avoidance. Background Art

[0002] With the development of intelligent catering services, coffee robots are gradually being used in open consumption scenarios, and their human-machine collaborative operations face significant challenges.

[0003] Traditional coffee robots rely on fixed protection mechanisms in closed environments and are difficult to adapt to the dynamic safety risks brought about by the random approach of people in open scenarios.

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

[0005] First, a single sensor deployment makes it difficult to accurately predict the intrusion trajectory, causing the robotic arm to frequently trigger the emergency stop mechanism, interrupting the production process; second, the path planning of multiple robotic arms lacks spatiotemporal coordination, and the timing deviation of the process connection causes the injection and packaging processes to be misaligned, 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 when avoiding obstacles at high speed.

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

[0007] The present invention provides a three-manipulator collaborative path planning method and system based on spatial obstacle avoidance, which is used to solve the technical problem in the prior art that the human-machine collaborative operation of the coffee robot in an open environment is insufficiently safe, resulting in frequent emergency stops of the manipulator 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 collaborative path planning method and system based on spatial obstacle avoidance.

[0009] The first aspect of the present invention provides a three-manipulator collaborative path planning method based on spatial obstacle avoidance, the method comprising: deploying K-level infrared sensing arrays in K circular intrusion monitoring areas of the coffee robot; activating the K-1-level infrared sensing array when the first-level infrared sensing array detects a person intruding into the first circular intrusion monitoring area; starting the K-1-level infrared sensing array mapping to perform intrusion monitoring on the K-1 environmental intrusion monitoring areas, and outputting an intrusion prediction path based on the monitoring results analysis; interacting with the coffee robot to obtain the real-time displacement points, three-manipulator reference paths and three-manipulator reference displacement speeds of the three manipulators; taking the real-time displacement points of the three manipulators as the starting point, aligning the three-manipulator reference paths and the intrusion prediction paths in time and space according to the three-manipulator reference displacement speeds, and locating the three-manipulator spatiotemporal conflict points; performing back-fitting of the obstacle avoidance path with the three-manipulator spatiotemporal conflict points as the starting point, and outputting the three-manipulator obstacle avoidance path; performing coffee-making collaborative compensation on the three-manipulator obstacle avoidance path, and outputting the manipulator spatial collaborative path.

[0010] In one embodiment, K-level infrared sensing arrays are deployed in K circular intrusion monitoring areas of the coffee robot, and the following processing is also performed:

[0011] By simulating the displacement of the coffee robot's robotic arm, the robotic arm's working area is located; the monitoring area is expanded in a circular manner with the robotic arm's working area as the starting point, and K circular intrusion monitoring areas are framed; and the K-level infrared sensing array is deployed to cover the K circular intrusion monitoring areas.

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

[0013] Through the K-1 level infrared sensing array mapping, K-1 environmental intrusion monitoring areas are monitored for intrusion, and K intrusion behavior data are output; the K intrusion behavior data are subjected to intrusion behavior feature collection, and K intrusion behavior features are output, wherein the intrusion behavior features include intrusion displacement speed, intrusion displacement direction and intrusion spatial position; the K intrusion behavior features are subjected to mobile inertia analysis, and the intrusion prediction path is output.

[0014] In one embodiment, the three robotic arms of the coffee robot include a cup-taking robotic arm, a pipetting robotic arm, and a packaging robotic arm.

[0015] In one embodiment, the three robotic arms' real-time displacement points are used as starting points, and the three robotic arms' reference paths and intrusion prediction paths are spatiotemporally aligned based on the three robotic arms' reference displacement speeds to locate the three robotic arms' spatiotemporal conflict points. The following processing is also performed:

[0016] The real-time displacement points, reference paths and reference displacement speeds of the cup-taking robot arm are extracted from the real-time displacement points, reference paths and reference displacement speeds of the three robot arms; with the real-time displacement points of the cup-taking robot arm as the starting point, the reference path of the cup-taking robot arm is decomposed into trajectory time-space discretization nodes according to the reference displacement speed of the cup-taking robot arm; the time-space conflict point of cup-taking is located by time-space alignment of the intrusion prediction path and the trajectory time-space discretization nodes; and by analogy, the time-space conflict point of pipetting and the time-space conflict point of encapsulation are located, wherein the time-space conflict point of cup-taking, the time-space conflict point of pipetting and the time-space conflict point of encapsulation constitute the time-space conflict point of the three robot arms.

[0017] In one embodiment, an obstacle avoidance path is back-fitted with the spatiotemporal conflict point of the three robotic arms as a starting point, and an obstacle avoidance path of the three robotic arms is output. The following processing is also performed:

[0018] Taking the cup-picking spatiotemporal conflict point as the starting point, the cup-picking obstacle avoidance starting point and the cup-picking obstacle avoidance ending point are located along the cup-picking reference path; multiple obstacle avoidance alternative paths are constructed between the cup-picking obstacle avoidance starting point and the cup-picking obstacle avoidance ending point; taking the cup-picking reference displacement speed as a constraint, the multiple obstacle avoidance alternative paths are subjected to dumping and anti-spill simulation to screen out a target obstacle avoidance path; according to the cup-picking obstacle avoidance starting point and the cup-picking obstacle avoidance ending point, the target obstacle avoidance path is replaced with the cup-picking reference path to obtain the cup-picking obstacle avoidance path; and so on, taking the three-arm spatiotemporal conflict point as the starting point, the obstacle avoidance path is back-fitted, and the three-arm obstacle avoidance path is output, wherein the three-arm obstacle avoidance path includes a cup-picking obstacle avoidance path, a pipetting obstacle avoidance path, and a packaging obstacle avoidance path.

[0019] In one embodiment, coffee making collaborative compensation is performed on the obstacle avoidance paths of the three robotic arms, and a spatial collaborative path of the robotic arms is output. The following processing is also performed:

[0020] The coffee making process errors are calculated for the cup-taking obstacle avoidance path, the pipetting obstacle avoidance path, and the packaging obstacle avoidance path, and the robot arm displacement collaborative compensation is performed based on the calculation results, and the cup-taking compensation path, the pipetting compensation path, and the packaging compensation path are output; the robot arm displacement control complexity is evaluated for the cup-taking compensation path, the pipetting compensation path, and the packaging compensation path to screen 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] The second aspect of the present invention provides a three-manipulator collaborative path planning system based on spatial obstacle avoidance, the system comprising: a perception deployment unit for deploying K-level infrared perception arrays in the K annular intrusion monitoring areas of the coffee robot; a perception activation unit for activating the K-1-level infrared perception array when the first-level infrared perception array detects a person intruding into the first annular intrusion monitoring area; an intrusion monitoring unit for starting the K-1-level infrared perception array mapping to perform intrusion monitoring on the K-1 environmental intrusion monitoring areas, and outputting an intrusion prediction path based on the monitoring results; a parameter calling unit for exchanging The coffee robot obtains the real-time displacement points of the three robotic arms, the three-arm reference paths and the three-arm reference displacement speeds; a conflict positioning unit is used to take the real-time displacement points of the three robotic arms as the starting point, and spatially align the three-arm reference paths and the intrusion prediction paths according to the three-arm reference displacement speeds, so as to locate the spatiotemporal conflict points of the three robotic arms; an obstacle avoidance planning unit is used to perform back-fitting of the obstacle avoidance path with the three-arm spatiotemporal conflict points as the starting point, and output the three-arm obstacle avoidance path; a collaborative compensation unit is used to perform coffee-making collaborative compensation on the three-arm obstacle avoidance path, and output the robotic arm spatial collaborative path.

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

[0023] The method provided by an embodiment of the present invention deploys K layers of infrared sensing arrays in K circular intrusion monitoring areas of a coffee robot. When the first layer of infrared sensing array detects a person intruding into the first circular intrusion monitoring area, it activates the K-1 layer of infrared sensing arrays. The K-1 layer of infrared sensing arrays is then activated to map the K-1 environmental intrusion monitoring areas for intrusion monitoring, and an intrusion prediction path is output based on the monitoring results. The coffee robot then interacts 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, the reference paths and the predicted intrusion paths are spatially aligned based on the reference displacement velocities of the three robotic arms to locate the spatiotemporal conflict points of the three robotic arms. Obstacle avoidance paths are back-fitted using the spatiotemporal conflict points of the three robotic arms to output the obstacle avoidance paths of the three robotic arms. Coffee-making collaborative compensation is then performed on the obstacle avoidance paths of the three robotic arms to output the spatial collaborative paths of the robotic arms. This method improves the safety of human-robot collaboration in an open environment, effectively prevents the risk of robotic arm collision interruption caused by sudden intrusions, and ensures the reliability of human-robot collaborative operations in open environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 The figure shows a flow chart of a three-manipulator collaborative path planning method based on spatial obstacle avoidance provided by the present invention;

[0025] Figure 2The schematic diagram shows the structure of the three-manipulator collaborative path planning system based on spatial obstacle avoidance provided by the present invention.

[0026] Explanation of the accompanying symbols: perception deployment unit 1, perception activation unit 2, intrusion monitoring unit 3, parameter calling unit 4, conflict positioning unit 5, obstacle avoidance planning unit 6, collaborative compensation unit 7. DETAILED DESCRIPTION

[0027] The present invention provides a three-manipulator collaborative path planning method and system based on spatial obstacle avoidance, which is used to solve the technical problem in the prior art that the human-machine collaborative operation of coffee robots in an open environment is insufficiently safe, resulting in frequent emergency stops of the manipulator arms, interruption of the production process, and reduced coffee production efficiency and quality control stability.

[0028] Below, the technical solutions of the present invention will be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments of the present invention. It should be understood that the present invention is not limited to the example embodiments described herein. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. It should also be noted that, for the convenience of description, only the parts related to the present invention, rather than all, are shown in the accompanying drawings.

[0029] Example 1, a flow chart of a three-manipulator collaborative path planning method based on spatial obstacle avoidance provided by an embodiment of the present invention, see Figure 1 , the method comprising:

[0030] Step S100: Deploy K-level infrared sensing arrays in the K circular intrusion monitoring areas of the coffee robot.

[0031] In one embodiment, K-level infrared sensing arrays are deployed in K circular intrusion monitoring areas of the coffee robot. Step S100 provided by the present invention further includes:

[0032] Step S110: Position the working area of ​​the robotic arm by simulating the displacement of the coffee robot's robotic arm.

[0033] Step S120: Expand the monitoring area in a circular manner starting from the working area of ​​the robotic arm to define K circular intrusion monitoring areas.

[0034] Step S130: Deploy the K-level infrared sensing arrays to cover the K annular intrusion monitoring areas.

[0035] This embodiment, based on a robot forward kinematics algorithm, simulates the combined motion of each joint within its maximum travel range and accurately calculates the end effector's achievable extreme positions in three-dimensional space. Through repeated iterative optimization, the effective working envelope of the robot arm during core processes such as cup removal, liquid injection, and packaging is ultimately determined. A three-dimensional model of the robot arm's working area, including a safety margin, is then formed as the working area. The definition of this working area provides a spatial reference for subsequent monitoring range demarcation.

[0036] Centered on the origin of the robot arm's base coordinate system, circular monitoring zones extend outward layer by layer, following the safe distance for human-robot collaboration specified by international safety standards. The innermost zone closely follows the perimeter of the robot arm's workspace, primarily preventing direct contact risks during operation. The middle zone encompasses the human-robot interaction buffer zone, detecting potential threats approaching. The outer zone extends to the public area surrounding the equipment, providing early warning of intrusions. The radius of each ring zone dynamically adjusts based on the robot arm's movement speed to ensure consistent risk response time windows.

[0037] The K-level infrared sensing arrays are deployed to cover the K annular intrusion monitoring areas. Sensors at each layer achieve data fusion through a time synchronization protocol to build a multi-level defense system from near to far and from precise to broad. At the same time, the density of sensor deployment 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 a person intruding into the first annular intrusion monitoring area, the K-1-level infrared sensing array is activated.

[0039] Specifically, when the first-level infrared sensing array deployed in the outermost layer detects that a human body enters the first circular intrusion monitoring area (circular monitoring boundary), the hierarchical interlocking response mechanism is immediately activated and an activation instruction is sent to the K-1 level sensor array in the adjacent inner layer.

[0040] This hierarchical wake-up strategy from the outside to the inside can not only reduce the energy consumption of normal monitoring, but also ensure that the internal monitoring accuracy is quickly improved when potential threats are detected.

[0041] At the same time, the activation delay of sensors at each level is limited to within 50 milliseconds to ensure the continuity of 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 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 areas, and an intrusion prediction path is output based on the monitoring results. The method step S300 provided by the present invention further includes:

[0044] Step S310: Perform intrusion monitoring on K-1 environmental intrusion monitoring areas through the K-1 level infrared sensing array mapping, and output K intrusion behavior data.

[0045] Step S320: collecting intrusion behavior features from the K intrusion behavior data, and outputting K intrusion behavior features, wherein the intrusion behavior features include intrusion displacement speed, intrusion displacement direction, and intrusion spatial position.

[0046] Step S330: Perform mobile inertia analysis on the K intrusion behavior features and output the predicted intrusion path.

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

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

[0049] Using a velocity vector decomposition algorithm, the K intrusion behavior data are used to calculate intrusion behavior characteristics, obtaining the instantaneous movement rate and motion direction angle of the intruder in three-dimensional space. Furthermore, combined with historical trajectory data, the spatial position distribution characteristics of the intruder are extracted, and the regularity of its movement pattern is identified. Ultimately, K intrusion behavior characteristics are output, consisting of the intrusion displacement speed, intrusion displacement direction, and intrusion spatial position.

[0050] The K intrusion behavior features are connected in time and space to construct a motion vector field of the intrusion target in three-dimensional space. Then, based on the principles 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, wherein 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 the intruder target through collaborative monitoring of multi-level sensors and inertial modeling of human motion, providing a reliable predictive basis for the dynamic path planning of the robotic arm, and improving the technical effect of the safety of human-machine collaborative operations 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 three robotic arms of the coffee robot include a cup-taking robotic arm, a pipetting robotic arm, and a packaging robotic arm.

[0054] Specifically, the three robotic arms of the coffee robot include a cup-taking robotic arm, a pipetting robotic arm and a packaging robotic arm. Correspondingly, the real-time displacement points of the three robotic arms obtained by the interaction of the coffee robot specifically include the cup-taking real-time displacement point, the pipetting real-time displacement point and the packaging real-time displacement point. The real-time displacement point refers to the real-time spatial position of the robotic arm gripper.

[0055] By analogy, the three robotic arm reference paths specifically include a cupping reference path, a pipetting reference path, and a packaging reference path. The reference path is a standard movement path of the robotic arm when performing a task.

[0056] By analogy, the three robotic arm reference displacement speeds include the cup-taking reference displacement speed, the pipetting reference displacement speed, and the packaging reference displacement speed. The reference displacement speed is the standard movement speed of the robotic arm when it moves along the reference path. This speed can effectively prevent coffee liquid from spilling.

[0057] Step S500: Taking the real-time displacement point of the three robotic arms as the starting point, the three robotic arms reference paths and the intrusion prediction path are spatially and temporally aligned according to the reference displacement speeds of the three robotic arms, and the spatial and temporal conflict points of the three robotic arms are located.

[0058] In one embodiment, taking the real-time displacement points of the three robotic arms as starting points, the three robotic arm reference paths and the predicted intrusion paths are spatiotemporally aligned according to the three robotic arm reference displacement speeds to locate the spatiotemporal conflict points of the three robotic arms. Step S500 of the method provided by the present invention further includes:

[0059] Step S510: extracting 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 three robot arm real-time displacement points, three robot arm reference paths and three robot arm reference displacement speeds.

[0060] Step S520: Taking the cup-taking real-time displacement point as the starting point, decompose the cup-taking reference path into trajectory time-space discretization nodes according to the cup-taking reference displacement speed.

[0061] Step S530: aligning the intrusion prediction path and the spatiotemporal discretization nodes of the trajectory in time and space to locate the spatiotemporal conflict point.

[0062] Step S540: Similarly, locate the pipetting time-space conflict point and the encapsulation time-space conflict point, wherein the cupping time-space conflict point, pipetting time-space conflict point and encapsulation time-space conflict point constitute the three-manipulator arm time-space conflict point.

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

[0064] Taking the current actual position of the cup-picking robot arm as the starting point, the preset cup-picking reference path is discretized into a sequence of trajectory points with time marks according to the cup-picking reference displacement speed, that is, the trajectory time-space discretization nodes.

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

[0066] This discretization process converts the continuous path into a set of time-space coupled nodes, so that the motion trajectories of the robotic arm and the intrusion target can be compared and analyzed in a unified space-time framework.

[0067] The predicted intrusion path of the intrusion target and the spatiotemporal discretization nodes of the cup-retrieving robot's trajectory are matched in four dimensions through a spatiotemporal mapping engine. Specifically, the spatial distance between the two within each time slice is calculated. When it is detected that the distance between the end of the robot arm and the intrusion target at a certain moment is lower than the dynamic safety threshold, the spatiotemporal node is marked as a potential spatiotemporal conflict point for cup-retrieving.

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

[0069] By analogy, the same spatiotemporal mapping and conflict detection logic is used to analyze the standard paths of the pipetting robot arm and the packaging robot arm frame by frame, and the pipetting spatiotemporal conflict points and the packaging spatiotemporal conflict points are located. The cupping spatiotemporal conflict points, pipetting spatiotemporal conflict points, and packaging spatiotemporal conflict points constitute the spatiotemporal conflict points of the three robotic arms.

[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, and achieve a collaborative benchmark for dynamic obstacle avoidance in multiple processes, ensuring the safety of human-machine collaboration and the continuity of the coffee-making process in open scenarios.

[0071] Step S600: performing backtracking fitting of the obstacle avoidance path with the spatiotemporal conflict point of the three robotic arms as the starting point, and outputting the obstacle avoidance path of the three robotic arms.

[0072] In one embodiment, an obstacle avoidance path is back-fitted with the spatiotemporal conflict point of the three robotic arms as a starting point, and an obstacle avoidance path of the three robotic arms is output. The method step S600 provided by the present invention further includes:

[0073] Step S610: Taking the cup-taking time-space conflict point as the starting point, locate the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point along the cup-taking reference path.

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

[0075] Step S630: Using the cup-picking reference displacement speed as a constraint, perform a dumping and spilling prevention simulation on the multiple obstacle avoidance alternative paths to screen out a target obstacle avoidance path.

[0076] Step S640: According to the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point, the target obstacle avoidance path is replaced with the cup-taking reference path to obtain the cup-taking obstacle avoidance path.

[0077] Step S650: Similarly, taking the spatiotemporal conflict point of the three robotic arms as the starting point, back-fitting of the obstacle avoidance path is performed, and the obstacle avoidance path of the three robotic arms is output, wherein the obstacle avoidance path of the three robotic arms includes a cup-taking obstacle avoidance path, a pipetting obstacle avoidance path, and a packaging obstacle avoidance path.

[0078] Specifically, for the spatiotemporal conflict point of the cup-picking robot arm, it traces back along its original cup-picking reference path to find the nearest feasible path bifurcation point as the starting position for obstacle avoidance, and at the same time extends backward to the first safe connection point that allows path regression as the end position for obstacle avoidance.

[0079] The two key points of the cup-picking obstacle avoidance starting and ending points constitute the replacement interval of the local obstacle avoidance path. The determination process must ensure smooth transitions in the robot arm's joint angular velocity to avoid vibration 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 to ensure sufficient detour space in the obstacle avoidance path segment.

[0080] Within the obstacle avoidance interval formed by the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point, multiple obstacle avoidance alternative paths are generated based on the fast random tree algorithm. The multiple obstacle avoidance alternative paths present different avoidance curve shapes in three-dimensional space, corresponding to different obstacle avoidance strategies respectively:

[0081] For example, the high-position lifting path crosses the risk area by raising the cup-taking height, the lateral offset path uses the redundant degrees of freedom of the robotic arm for lateral avoidance, and the time delay path staggers the time and space conflict window through speed control.

[0082] Each alternative obstacle avoidance path is labeled with kinematic parameters, including joint angle sequence, terminal acceleration curve, estimated energy consumption, and other characteristic dimensions.

[0083] Using the reference displacement velocity for cup removal as a constraint, dynamic simulations were performed on candidate paths, focusing on evaluating the liquid's stability during cup removal. A sloshing model of the liquid surface within the cup was constructed, simulating the liquid's motion trajectory under different acceleration curves. The maximum sloshing amplitude and overflow risk factor were calculated. The vibration spectrum of the robotic arm during each path was also measured to eliminate motion patterns that could cause resonance. The screening criteria required that the liquid sloshing height of the candidate path not exceed 5% of the cup's height, and that the terminal vibration amplitude be controlled within a range of ±1 mm, ensuring that the drink's quality was not affected by the obstacle avoidance action. Ultimately, the target obstacle avoidance path was selected.

[0084] After selecting the optimal obstacle avoidance path, the fifth-order polynomial interpolation algorithm is used to achieve the continuity of speed and acceleration of the new and old path segments. Then, based on the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending 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 sections are verified by inverse kinematics solution to ensure the smoothness of the joint motion of the entire cup-taking path, and the replaced cup-taking obstacle avoidance path is obtained.

[0085] The cup-picking obstacle avoidance path envelops the look-ahead buffer of the cup-picking robot arm controller, thereby achieving seamless switching of motion instructions and avoiding overload of the servo motor caused by sudden path changes.

[0086] Similarly, the obstacle avoidance path is back-fitted with the spatiotemporal conflict point of the three robotic arms as the starting point, and the obstacle avoidance path of the three robotic arms is output, wherein the obstacle avoidance path of the three robotic arms includes the cupping obstacle avoidance path, the pipetting obstacle avoidance path and the packaging obstacle avoidance path.

[0087] This embodiment generates an optimized path that takes both obstacle avoidance safety and liquid spill prevention into account 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 paths of the three robotic arms and output the spatial collaborative path of the robotic arms.

[0089] In one embodiment, coffee making collaborative compensation is performed on the obstacle avoidance paths of the three robotic arms to output a spatial collaborative path of the robotic arms. The method step S700 provided by the present invention further includes:

[0090] Step S710: Calculate the coffee making process errors for the cupping obstacle avoidance path, the pipetting obstacle avoidance path, and the packaging obstacle avoidance path, perform coordinated compensation for the robot arm displacement based on the calculation results, and output the cupping compensation path, the pipetting compensation path, and the packaging compensation path.

[0091] Step S720: performing a robot arm displacement control complexity evaluation on the cup-taking compensation path, the pipetting compensation path, and the packaging compensation path to select a target compensation path.

[0092] Step S730: Use the target compensation path to locally replace the three-manipulator obstacle avoidance path and output the manipulator spatial collaborative path.

[0093] Specifically, compensation adjustments are made to each robot's obstacle avoidance path independently, ensuring that optimization of a single robot does not alter the established obstacle avoidance paths of other robots. When compensating for the cupping robot, the pipetting and packaging robots maintain their original obstacle avoidance paths, with only local corrections made to the cupping path.

[0094] A process error reverse tracing algorithm accurately identifies critical sections of the cup retrieval path that cause process timing deviations. Speed ​​adjustments or trajectory fine-tuning points are then inserted within these sections to minimize path changes. The compensated cup retrieval path maintains timing errors within ±50 milliseconds relative to the original process at key nodes, such as entering and exiting the ice area and cup transfer, while also ensuring that the pipetting robot's initial wait time for liquid injection is unaffected. The resulting compensated path modifies only essential motion segments, maximizing the overall system coordination baseline.

[0095] Similarly, a pipetting compensation path and a packaging compensation path are generated under the condition that the other two obstacle avoidance paths are not changed, and they match the original process sequence.

[0096] Independently evaluate the controllability of the compensation path generated by each robot arm to avoid the increased complexity caused by cross-arm parameter coupling. Evaluate the characteristics of the focused compensation path: Analyze the joint motion spectrum using discrete Fourier transform to eliminate high-frequency vibration components. Calculate the rate of change of the path curvature to ensure it does not exceed the servo system's tracking capabilities. Measure the spatial deviation between the end-point trajectory and the process reference path and control it within the ±2 mm process tolerance band.

[0097] While ensuring the quality of individual paths, the compensation scheme with the smallest change is prioritized. For example, for the cup-picking robot, a micro-compensation scheme that adjusts only three path points, rather than reconstructing the entire path, is preferred. This reduces control complexity by 62%, making it less complex than other compensation paths. Therefore, the cup-picking compensation path is selected as the target compensation path.

[0098] According to 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 robotic arm spatial collaborative path is output.

[0099] After receiving the spatial collaborative path of the robotic arms, the control module of the coffee robot synchronously coordinates the motion trajectories of the three robotic arms based on the spatial collaborative path of the robotic arms, maintains the stable posture of the cup body during obstacle avoidance, prevents liquid spillage, ensures the 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 of coffee robots in open environments, effectively preventing the risk of collision and interruption of robotic arms caused by sudden intrusions, and ensuring the reliability of human-machine collaborative operations in open environments.

[0101] Embodiment 2 is based on the same inventive concept as the three-manipulator collaborative path planning method based on spatial obstacle avoidance in the above embodiment. Figure 2 As shown, the present invention provides a three-manipulator collaborative path planning system based on spatial obstacle avoidance, wherein the system includes:

[0102] The perception deployment unit 1 is used to deploy K-level infrared perception arrays in K circular intrusion monitoring 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 a person intruding into the first annular intrusion monitoring area.

[0104] The intrusion monitoring unit 3 is used to start the K-1 level infrared sensing array mapping to perform intrusion monitoring on K-1 environmental intrusion monitoring areas, 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 three robotic arm reference paths and the three robotic arm reference displacement speeds.

[0106] The conflict positioning unit 5 is used to take the real-time displacement point of the three robotic arms as the starting point, align the three robotic arm reference paths and the intrusion prediction path in time and space according to the three robotic arm reference displacement speeds, and locate the three robotic arm time and space conflict points.

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

[0108] The collaborative compensation unit 7 is used to perform coffee making collaborative compensation on the obstacle avoidance paths of the three robotic arms and output a 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 coffee robot's robotic arm, the robotic arm's working area is located; the monitoring area is expanded in a circular manner with the robotic arm's working area as the starting point, and K circular intrusion monitoring areas are framed; and the K-level infrared sensing array is deployed to cover the K circular intrusion monitoring areas.

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

[0112] Through the K-1 level infrared sensing array mapping, K-1 environmental intrusion monitoring areas are monitored for intrusion, and K intrusion behavior data are output; the K intrusion behavior data are subjected to intrusion behavior feature collection, and K intrusion behavior features are output, wherein the intrusion behavior features include intrusion displacement speed, intrusion displacement direction and intrusion spatial position; the K intrusion behavior features are subjected to mobile inertia analysis, and the intrusion prediction path is output.

[0113] In one embodiment, the three robotic arms of the coffee robot include a cup-taking robotic arm, a pipetting robotic arm, and a packaging robotic arm.

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

[0115] The real-time displacement points, reference paths and reference displacement speeds of the cup-taking robot arm are extracted from the real-time displacement points, reference paths and reference displacement speeds of the three robot arms; with the real-time displacement points of the cup-taking robot arm as the starting point, the reference path of the cup-taking robot arm is decomposed into trajectory time-space discretization nodes according to the reference displacement speed of the cup-taking robot arm; the time-space conflict point of cup-taking is located by time-space alignment of the intrusion prediction path and the trajectory time-space discretization nodes; and by analogy, the time-space conflict point of pipetting and the time-space conflict point of encapsulation are located, wherein the time-space conflict point of cup-taking, the time-space conflict point of pipetting and the time-space conflict point of encapsulation constitute the time-space conflict point of the three robot arms.

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

[0117] Taking the cup-picking spatiotemporal conflict point as the starting point, the cup-picking obstacle avoidance starting point and the cup-picking obstacle avoidance ending point are located along the cup-picking reference path; multiple obstacle avoidance alternative paths are constructed between the cup-picking obstacle avoidance starting point and the cup-picking obstacle avoidance ending point; taking the cup-picking reference displacement speed as a constraint, the multiple obstacle avoidance alternative paths are subjected to dumping and anti-spill simulation to screen out a target obstacle avoidance path; according to the cup-picking obstacle avoidance starting point and the cup-picking obstacle avoidance ending point, the target obstacle avoidance path is replaced with the cup-picking reference path to obtain the cup-picking obstacle avoidance path; and so on, taking the three-arm spatiotemporal conflict point as the starting point, the obstacle avoidance path is back-fitted, and the three-arm obstacle avoidance path is output, wherein the three-arm obstacle avoidance path includes a cup-picking obstacle avoidance path, a pipetting obstacle avoidance path, and a packaging obstacle avoidance path.

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

[0119] The coffee making process errors are calculated for the cup-taking obstacle avoidance path, the pipetting obstacle avoidance path, and the packaging obstacle avoidance path, and the robot arm displacement collaborative compensation is performed based on the calculation results, and the cup-taking compensation path, the pipetting compensation path, and the packaging compensation path are output; the robot arm displacement control complexity is evaluated for the cup-taking compensation path, the pipetting compensation path, and the packaging compensation path to screen 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 modifications or substitutions that can be easily conceived by a person 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 based on the scope of protection of the claims.

Claims

1. A three-manipulator collaborative path planning method based on spatial obstacle avoidance, characterized in that: include: Deploy K-level infrared sensing arrays in the coffee robot's K circular intrusion monitoring areas; When the first-level infrared sensing array detects a person intruding into the first ring-shaped intrusion monitoring area, the K-1-level infrared sensing array is activated; Activate the K-1 level infrared sensing array mapping to perform intrusion monitoring on K-1 environmental intrusion monitoring areas, and output intrusion prediction paths based on the monitoring results; Interacting with the coffee robot to obtain the real-time displacement points, reference paths, and reference displacement speeds of the three robotic arms; Taking the real-time displacement point of the three robotic arms as the starting point, the three robotic arms reference path and the intrusion prediction path are temporally and spatially aligned according to the three robotic arms reference displacement speed, and the temporal and spatial conflict point of the three robotic arms is located; Perform obstacle avoidance path backtracking fitting with the spatiotemporal conflict point of the three robotic arms as the starting point, and output the obstacle avoidance path of the three robotic arms; The three robotic arms' obstacle avoidance paths are subjected to coffee making collaborative compensation, and a spatial collaborative path of the robotic arms is output.

2. The three-manipulator collaborative path planning method based on spatial obstacle avoidance according to claim 1, characterized in that: Deploy a K-level infrared sensing array in the coffee robot's K circular intrusion monitoring areas, including: By simulating the displacement of the coffee robot's robotic arm, the robotic arm's working area is located; The monitoring area is expanded in a circular manner starting from the working area of ​​the robotic arm to define K circular intrusion monitoring areas; The K-level infrared sensing arrays are deployed to cover the K annular intrusion monitoring areas.

3. The three-manipulator collaborative path planning method based on spatial obstacle avoidance according to claim 1, characterized in that: The K-1 level infrared sensing array mapping is activated to perform intrusion monitoring on the K-1 environmental intrusion monitoring areas, and an intrusion prediction path is output based on the monitoring results, including: Perform intrusion monitoring on K-1 environmental intrusion monitoring areas through the K-1 level infrared sensing array mapping, and output K intrusion behavior data; Collecting intrusion behavior features of the K intrusion behavior data and outputting K intrusion behavior features, wherein the intrusion behavior features include intrusion displacement speed, intrusion displacement direction, and intrusion spatial position; Perform movement inertia analysis on the K intrusion behavior features and output the intrusion prediction path.

4. The three-manipulator collaborative path planning method based on spatial obstacle avoidance according to claim 1, characterized in that: The three robotic arms of the coffee robot include a cup-taking robotic arm, a liquid transfer robotic arm, and a packaging robotic arm.

5. The three-manipulator collaborative path planning method based on spatial obstacle avoidance according to claim 4 is characterized in that: Taking the real-time displacement point of the three robotic arms as the starting point, aligning the three robotic arm reference paths and the intrusion prediction path in time and space according to the three robotic arm reference displacement speeds, and locating the three robotic arm time and space conflict points, including: Extracting the cup-taking real-time displacement point, cup-taking reference path and cup-taking reference displacement speed of the cup-taking robot arm from the three real-time displacement points, three reference paths and three reference displacement speeds of the robot arms; Taking the cup-taking real-time displacement point as the starting point, the cup-taking reference path is decomposed into trajectory time-space discretization nodes according to the cup-taking reference displacement speed; Positioning the spatiotemporal conflict point of the cupping by aligning the spatiotemporal discretization nodes of the invasion prediction path and trajectory in spatiotemporal order; By analogy, the pipetting time-space conflict point and the encapsulation time-space conflict point are located, wherein the cupping time-space conflict point, the pipetting time-space conflict point and the encapsulation time-space conflict point constitute the three-manipulator arm time-space conflict point.

6. The three-manipulator collaborative path planning method based on spatial obstacle avoidance according to claim 5, characterized in that: Taking the spatiotemporal conflict point of the three manipulators as the starting point, a backtracking fitting of the obstacle avoidance path is performed to output the obstacle avoidance path of the three manipulators, including: Taking the cup-taking time-space conflict point as the starting point, locating the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point along the cup-taking reference path; Constructing a plurality of alternative obstacle avoidance paths between the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point; Using the cup-picking reference displacement speed as a constraint, performing a dumping and spilling prevention simulation on the multiple obstacle avoidance alternative paths to screen out a target obstacle avoidance path; According to the cup-taking obstacle avoidance starting point and the cup-taking obstacle avoidance ending point, the target obstacle avoidance path is replaced with the cup-taking reference path to obtain the cup-taking obstacle avoidance path; Similarly, the obstacle avoidance path is back-fitted with the spatiotemporal conflict point of the three robotic arms as the starting point, and the obstacle avoidance path of the three robotic arms is output, wherein the obstacle avoidance path of the three robotic arms includes the cupping obstacle avoidance path, the pipetting obstacle avoidance path and the packaging obstacle avoidance path.

7. The three-manipulator collaborative path planning method based on spatial obstacle avoidance according to claim 6, characterized in that: Performing coffee making collaborative compensation on the obstacle avoidance paths of the three robotic arms and outputting a spatial collaborative path of the robotic arms includes: Calculating coffee making process errors for the cupping obstacle avoidance path, the pipetting obstacle avoidance path, and the packaging obstacle avoidance path, performing coordinated compensation for the robot arm displacement based on the calculation results, and outputting the cupping compensation path, the pipetting compensation path, and the packaging compensation path; Performing a robotic arm displacement control complexity evaluation on the cupping compensation path, the pipetting compensation path, and the packaging compensation path to screen a target compensation path; The target compensation path is used to locally replace the three-manipulator obstacle avoidance path, and the manipulator spatial collaborative path is output.

8. A three-manipulator collaborative path planning system based on spatial obstacle avoidance, characterized by: The steps for implementing the method according to any one of claims 1 to 7 include: A perception deployment unit is used to deploy K-level infrared perception arrays in K circular intrusion monitoring areas of the coffee robot; A sensing activation unit is used to activate the K-1 level infrared sensing array when the first level infrared sensing array detects a person intruding into the first annular intrusion monitoring area; An intrusion monitoring unit is used to activate the K-1 level infrared sensing array mapping to perform intrusion monitoring on K-1 environmental intrusion monitoring areas, and output an intrusion prediction path based on the monitoring results; A parameter calling unit, configured to interact with the coffee robot to obtain the real-time displacement points, reference paths, and reference displacement speeds of the three robotic arms; a conflict positioning unit, configured to use the real-time displacement point of the three robotic arms as a starting point, spatially and temporally align the three robotic arm reference paths and the intrusion prediction path according to the reference displacement speeds of the three robotic arms, and locate the spatial and temporal conflict point of the three robotic arms; An obstacle avoidance planning unit is used to perform backtracking fitting of an obstacle avoidance path with the spatiotemporal conflict point of the three robotic arms as a starting point, and output an obstacle avoidance path for the three robotic arms; The collaborative compensation unit is used to perform coffee making collaborative compensation on the obstacle avoidance paths of the three robotic arms and output a spatial collaborative path of the robotic arms.

Images