A multi-robot cooperative control method for industrial control

Abstract

The application discloses a kind of multi-robot cooperative control methods for industrial control, belong to industrial control technical field.The method receives the global time reference of synchronization signal generation on industrial control bus, and the local clock of each robot controller is compensated calibration to obtain synchronous clock;Based on the task execution dependency graph of subtask sequence and multiple dimensions collaborative task decomposition into based on synchronous clock;According to the token passing mechanism based on task priority of dependency graph between each robot controller, control robot executes subtask and real-time acquisition end pose feedback data;Actual trajectory is reconstructed using pose feedback data, and phase comparison is carried out between reconstructed trajectory and theoretical trajectory, and synchronization adjustment instruction is generated when phase deviation exceeds the allowed range, and token passing is dynamically adjusted.The method realizes distributed task coordination and closed-loop synchronization compensation based on pose feedback, and improves the phase consistency of multi-robot cooperative motion.

Description

Technical Field

[0001] This invention relates to the field of industrial control technology, specifically to a multi-robot collaborative control method for industrial control. Background Technology

[0002] In multi-robot collaborative industrial operations, multiple robots need to coordinate precisely in time and space to complete production processes with stringent timing requirements, such as collaborative handling, synchronous welding, or flexible assembly. Most existing multi-robot collaborative control solutions employ a centralized master-slave synchronous architecture, where a central control node issues commands, and each robot controller executes a predetermined motion trajectory according to a unified clock cycle. However, the complex industrial environment makes it difficult to eliminate communication link jitter and frequency drift of the local crystal oscillators of each controller, resulting in a continuous deviation between the global clock and the local clock. When executing highly dynamic, variable-speed trajectories, this minute clock-level offset accumulates over several motion cycles, manifesting as a significant phase misalignment between the robot's end effectors, directly reducing process quality.

[0003] Some improved solutions introduce a periodic synchronization method based on global timestamps, enabling nodes to align clock counts at discrete time points. However, this type of synchronization only achieves consistency in time information and does not establish execution order constraints related to the semantics of collaborative actions at the task scheduling level. In actual collaborative tasks, there are strict sequential dependencies and spatial connection requirements between subtasks. Once a robot experiences motion delay due to local disturbances or load changes, the system cannot dynamically adjust the execution rhythm of other robots based on the inherent logic of the task, and can only passively wait or reduce the speed of the entire system, resulting in an overall slowdown in production rhythm. At the same time, when the robot's end-effector trajectory has developed phase deviation, most existing methods rely on offline analysis or replanning of the entire trajectory segment for correction, lacking a mechanism for real-time synchronization compensation for individual robots during operation, and failing to quickly restore phase consistency between trajectories without interrupting other robots.

[0004] To address these challenges, it is necessary to solve how to achieve strict adherence to the execution sequence and relationships between tasks among distributed robot controllers, preventing the overall collaborative order from being disrupted by delays in any node. Furthermore, it is also necessary to solve how to utilize real-time feedback of end-effector pose information to detect the phase shift of each robot relative to its theoretical trajectory, and to dynamically correct the collaborative control parameters without replanning the global motion, thereby continuously suppressing the accumulation of phase deviations between multiple robot trajectories. Summary of the Invention

[0005] The purpose of this invention is to provide a multi-robot cooperative control method for industrial control, which solves the problems of low cooperative accuracy caused by the disconnect between task-dependent scheduling and clock synchronization in the prior art, and the difficulty in maintaining end-point trajectory synchronization due to the lack of a real-time phase closed-loop compensation mechanism.

[0006] To achieve the above objectives, the present invention provides the following technical solution: The present invention provides a multi-robot collaborative control method for industrial control, comprising: receiving a synchronization signal on an industrial control bus and generating a global time reference; compensating and calibrating the local clocks of each robot controller according to the global time reference to obtain a synchronized clock; decomposing the multi-dimensional collaborative task to be executed according to preset rules based on the synchronized clock, generating a sub-task sequence and a task execution dependency graph for each robot; establishing a token passing mechanism based on task priority among the robot controllers according to the task execution dependency graph, controlling each robot to execute the sub-task sequence through the token passing mechanism, and collecting pose feedback data of each robot end effector in real time; reconstructing the actual motion trajectory of each robot end effector using the pose feedback data to obtain a reconstructed trajectory; comparing the phase of the reconstructed trajectory with the corresponding theoretical trajectory in the sub-task sequence, and generating a synchronization adjustment command when the comparison result indicates that the phase deviation exceeds the allowable range. This method eliminates time inconsistencies between multiple controllers through global clock synchronization, achieves orderly collaboration through task dependency and token passing, and forms a closed-loop synchronization correction by combining trajectory reconstruction and phase comparison, significantly improving the accuracy and real-time performance of multi-robot collaborative control.

[0007] Preferably, receiving the synchronization signal and generating a global time reference specifically includes: monitoring periodically broadcast synchronization messages in the industrial Ethernet bus, the synchronization messages containing timestamp information; extracting the timestamp information and recording the local reception time, calculating the signal transmission delay, and using the transmission delay to correct the timestamp information to obtain the global time reference, thereby eliminating network transmission errors and ensuring the accuracy of the time reference.

[0008] Preferably, the process of compensating and calibrating the local clock to obtain a synchronized clock is as follows: obtain the current count value of the local clock of each robot controller, calculate the difference between the global time reference and the current count value of the local clock as the clock deviation value, input the clock deviation value as a compensation amount into the clock synchronization module, and the clock synchronization module adjusts the oscillation frequency of the local clock. When the difference between the adjusted local clock and the global time reference is less than a preset synchronization threshold, it is determined as the synchronized clock of the robot controller; through closed-loop frequency adjustment, high-precision clock synchronization of each controller is achieved.

[0009] Preferably, the process of generating the sub-task sequence and task execution dependency graph includes: obtaining the overall target description of the multi-dimensional collaborative task, which includes a task space coordinate sequence and an attitude angle sequence; sampling the overall target description according to a preset time slice length based on the synchronization clock to obtain sub-target points corresponding to multiple time slices; allocating the sub-target points of each time slice to the corresponding robots based on the kinematic and dynamic constraints of each robot controller to form the sub-task sequence; and establishing a task execution dependency graph based on the spatial continuity and logical order between the sub-target points, where nodes represent sub-tasks and directed edges represent the execution order constraints between sub-tasks. This decomposition method fully considers the differences in individual robot capabilities and the spatiotemporal connection requirements of tasks, ensuring the feasibility and coherence of the global collaborative task.

[0010] Preferably, the process of establishing a token passing mechanism based on task priority and collecting pose feedback data includes: performing topological sorting on the task execution dependency graph and assigning a task priority number to each subtask; establishing a virtual token ring among the robot controllers and passing the tokens between nodes in descending order of priority; when a robot controller acquires a token, it extracts the corresponding subtask from the subtask sequence and controls the robot to execute it, collecting pose feedback data of the robot end effector in real time through joint encoders and vision sensors during execution. Further, during token passing, the current holder node is initialized to the robot controller corresponding to the highest priority number; when this node completes the current subtask, a task completion identifier is generated and attached to the token, the priority number of the next subtask adjacent to the current subtask in the task execution dependency graph is queried, and the token is passed to the target robot controller node corresponding to that priority number. Through the token flow driven by the dependency graph, conflict-free execution and strict order maintenance of subtasks are achieved.

[0011] Preferably, reconstructing the actual motion trajectory of the robot's end effector includes: sorting the pose feedback data according to the sampling time to obtain a discrete pose sequence; transforming the coordinate system of each discrete pose to unify it into a preset world coordinate system to obtain a unified coordinate system pose sequence; and fitting the unified coordinate system pose sequence using an interpolation algorithm to generate a continuous and smooth trajectory curve as the reconstructed trajectory. More preferably, the interpolation fitting process first calculates the Cartesian spatial distance and attitude angle change between adjacent pose points to determine the motion time interval, and then uses a fifth-order polynomial interpolation algorithm to fit and generate a trajectory curve with continuous curvature, accurately restoring the actual motion of the robot's end effector and providing a reliable basis for synchronization deviation analysis.

[0012] Preferably, the process of generating synchronization adjustment instructions includes: extracting the theoretical pose sequence corresponding to the current reconstructed trajectory from the subtask sequence and generating a theoretical trajectory curve; aligning the reconstructed trajectory and the theoretical trajectory curve to the same time axis, and calculating the position deviation and phase deviation between them at each moment; determining whether the position deviation and the phase deviation exceed their respective allowable deviation ranges; when either deviation exceeds the allowable range, generating a synchronization adjustment instruction, which includes the target robot identifier to be adjusted and the adjustment amount determined according to the deviation; sending the synchronization adjustment instruction to the virtual token ring, which modifies the dwell time of the token at the target robot controller node according to the adjustment amount and adjusts the subsequent transmission order of the token, thereby correcting the motion rhythm of each robot in real time. Further, when the position deviation exceeds the allowable range, calculating the integral value of the position deviation and converting it into a position compensation vector as the adjustment amount; when the phase deviation exceeds the allowable range, calculating the phase difference change rate and determining the phase compensation amount as the adjustment amount; and simultaneously identifying the identifier corresponding to the robot controller that caused the deviation as the target robot identifier. This closed-loop adjustment strategy can quickly suppress synchronization errors and enhance the collaborative consistency and anti-interference capability of multi-robot systems in dynamic environments.

[0013] The technical effects and advantages provided by the present invention in the above technical solution are as follows: The multi-dimensional collaborative tasks to be executed are sampled in time slices according to the global synchronization clock, and sub-task sequences and task execution dependency graphs are generated by combining the kinematic and dynamic constraints of each robot. A token passing mechanism based on task priority is built among the robot controllers. The token carries a task completion identifier and is passed in a directed manner according to the priority number of adjacent sub-tasks in the dependency graph. Only the robot controller that obtains the token can trigger the execution action of the corresponding sub-task. This distributed token coordination method directly maps the logical dependencies between collaborative tasks to the directed flow path of communication tokens, ensuring that each robot acts strictly according to the process requirements, eliminating single-point bottlenecks and communication bottlenecks in centralized scheduling. The token passing path can be dynamically updated according to the dependency graph. When a robot experiences a brief delay due to external reasons, the token will not be passed to subsequent nodes that depend on its completion in advance, avoiding conflicts and deadlocks. At the same time, unaffected parallel branches can continue to execute, effectively improving the system's concurrency and disturbance resistance.

[0014] Using real-time end-effector pose feedback data acquired by robot joint encoders and vision sensors, a continuous and smooth actual motion trajectory is reconstructed through coordinate system transformation and fifth-order polynomial interpolation. The reconstructed trajectory is aligned with the theoretical trajectory generated from the theoretical pose sequence extracted from the subtask sequence onto the same time axis, and position and phase deviations are continuously calculated at each moment. When the position or phase deviation exceeds the allowable range, a synchronization adjustment command is generated. This command carries the target robot identifier to be adjusted and the position compensation vector calculated based on the integral value of the position deviation or the phase compensation amount calculated based on the rate of change of phase difference. The synchronization adjustment command is injected into a virtual token ring, adjusting the dwell time of the token at the target robot controller node and modifying the subsequent token transmission order. This closed-loop compensation mechanism directly acts on the timing of coordinated execution, enabling directional motion compensation for individual robots with deviations without stopping the system or replanning the motion trajectories of other robots, quickly aligning the phase relationship between end-effector trajectories. The compensation amounts for position and phase deviations are calculated based on integrals and rates of change, respectively, enabling the compensation process to suppress overshoot and smooth transitions, thereby achieving precise suppression of dynamic deviations and maintaining the long-term synchronous operation accuracy of the multi-robot system. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.

[0016] Figure 1 This is a flowchart of a multi-robot collaborative control method; Figure 2 This is a flowchart of a robot controller clock synchronization method based on the IEEE 1588 precise time protocol; Figure 3 It is a flowchart of multi-dimensional collaborative task decomposition and dependency relationship construction; Figure 4 This is a flowchart of multi-robot synchronous control based on trajectory deviation detection and token adjustment. Detailed Implementation

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

[0018] See Figure 1 This invention provides a multi-robot collaborative control method for industrial control, comprising: receiving a synchronization signal on an industrial control bus and generating a global time reference; compensating and calibrating the local clocks of each robot controller according to the global time reference to obtain a synchronization clock; decomposing the multi-dimensional collaborative task to be executed according to preset rules based on the synchronization clock to generate a sub-task sequence and a task execution dependency graph for each robot; establishing a token passing mechanism based on task priority among the robot controllers according to the task execution dependency graph, controlling each robot to execute the sub-task sequence through the token passing mechanism, and collecting pose feedback data of each robot end effector in real time; reconstructing the actual motion trajectory of each robot end effector using the pose feedback data to obtain a reconstructed trajectory; comparing the phase of the reconstructed trajectory with the corresponding theoretical trajectory in the sub-task sequence, and generating a synchronization adjustment command when the comparison result indicates that the phase deviation exceeds the allowable range.

[0019] Example 1; see Figure 2 The system monitors periodically broadcast synchronization messages on the industrial Ethernet bus, and these synchronization messages contain timestamp information. In a specific implementation, the master node on the industrial Ethernet bus broadcasts synchronization messages at fixed intervals. The synchronization messages adopt the message format specified by the IEEE 1588 Precision Time Protocol and carry the timestamp information of the master node's transmission time.

[0020] The timestamp information is extracted, and the local reception time is recorded. In a specific implementation, each robot controller is connected to an industrial Ethernet bus. When the robot controller's network interface card detects the preamble of the synchronization message at the physical layer, it records the local reception time. The timestamp information is parsed from the data field of the synchronization message; the timestamp information is the transmission time value under the master node's clock domain.

[0021] The signal transmission delay is calculated based on the timestamp information and the local reception time. In a specific implementation, the signal transmission delay is calculated as follows: the robot controller sends a delay request message to the master node, recording the local transmission time of the delay request message. After receiving the delay request message, the master node records its reception time and returns it to the robot controller via a delay response message. The robot controller extracts the master node reception time from the delay response message and, combined with the local transmission time, timestamp information, and local reception time, calculates the signal transmission delay. The signal transmission delay includes line propagation delay and protocol stack processing delay.

[0022] The timestamp information is corrected using the signal transmission delay to obtain the global time reference. In a specific implementation, the correction method is to add the timestamp information to the signal transmission delay to obtain the corrected master node transmission time, and use the corrected master node transmission time as the global time reference.

[0023] Obtain the current local clock count value for each robot controller. In practice, the current local clock count value is the value of the internal timer counter of the robot controller, and the counting frequency of the timer counter is determined by the local crystal oscillator.

[0024] The difference between the global time base and the current count value of the local clock is calculated as the clock deviation value for each robot controller. In a specific implementation, the expression for calculating the clock deviation value is: ,in, Indicates the clock offset value. Represents the global time base. This represents the current count value of the local clock. Both the global time base and the current count value of the local clock are converted to the same time unit.

[0025] The clock deviation value is used as a compensation amount and input to the clock synchronization module of each robot controller, which adjusts the oscillation frequency of the local clock. In a specific implementation, the clock synchronization module includes a digital phase-locked loop (PLL), which comprises a phase detector, a loop filter, and a numerically controlled oscillator (CNC). The phase detector receives the clock deviation value as a phase error signal, and the loop filter performs proportional-integral (PI) processing on the clock deviation value to obtain a frequency control word. The frequency control word is input to the CNC oscillator, which adjusts the oscillation frequency of the output clock according to the frequency control word. The proportional and integral coefficients are set based on the short-term stability characteristics of the local clock and the synchronization period of the industrial Ethernet bus. The proportional coefficient ranges from 0.5 to 2.0, and the integral coefficient ranges from 0.01 to 0.1. Setting the proportional coefficient to 1.0 and the integral coefficient to 0.05 allows the loop bandwidth to suppress network jitter while quickly tracking changes in the global time base.

[0026] When the difference between the adjusted local clock and the global time reference is less than a preset synchronization threshold, the adjusted local clock is determined as the synchronization clock of the robot controller. In specific implementations, the preset synchronization threshold is set to 1 microsecond. The clock synchronization module continuously monitors the absolute value of the difference between the adjusted local clock and the global time reference. When the absolute value of the difference is less than 1 microsecond for multiple consecutive synchronization cycles, it is determined that the local clock is synchronized, and the current local clock is used as the synchronization clock. The robot controller then executes subsequent task scheduling based on the synchronization clock.

[0027] Example 2: See Figure 3 The overall objective description of the multi-dimensional collaborative task is obtained, which includes a task space coordinate sequence and an attitude angle sequence. In specific implementation, the multi-dimensional collaborative task is issued by the upper-level production management system. The task description adopts a structured data format, which contains a position array and an attitude array. Each element in the position array is a three-dimensional Cartesian coordinate, and each element in the attitude array is represented by a quaternion or Euler angle. The position array and attitude array are arranged in chronological order to form the task space coordinate sequence and attitude angle sequence.

[0028] Based on the synchronization clock, the overall target description is sampled according to a preset time slice length to obtain multiple sub-target points corresponding to time slices. In specific implementation, the preset time slice length is a fixed constant, determined based on the communication cycle of the industrial control bus and the interpolation cycle of the robot controller; the preset time slice length is set to 2 milliseconds. Starting from the current moment of the synchronization clock, a position value and an attitude value are extracted from the task space coordinate sequence and attitude angle sequence every 2 milliseconds to form a sub-target point. The sub-target point contains two fields: position coordinates and attitude angle. All sub-target points form a sub-target point sequence, and the length of the sub-target point sequence is equal to the total task duration divided by the time slice length.

[0029] Based on the kinematic and dynamic constraints of each robot controller, the sub-target points corresponding to each time slice are assigned to the corresponding robots, generating the sub-task sequence. In specific implementations, kinematic constraints include robot workspace boundaries, joint angle limits, and joint velocity limits; dynamic constraints include joint torque limits and end-effector load capacity limits. The assignment process is as follows: collision detection and reachability analysis are performed between the geometric model of the workpiece to be processed and the workspaces of all robots to determine the set of feasible robots corresponding to each sub-target point. When multiple robots exist in the set of feasible robots, the joint torque required for each feasible robot to execute the sub-target point is further calculated. The robot with the smallest joint torque is selected as the executing robot, and the sub-target point is assigned to the executing robot. All sub-target points assigned to the same robot are arranged in chronological order to form a sub-task sequence for each robot. Each element in the sub-task sequence contains the sub-target point and its corresponding synchronization clock timestamp.

[0030] Based on the spatial continuity and logical sequence between the sub-target points, a task execution dependency graph is established. The nodes of the task execution dependency graph represent the sub-tasks, and the directed edges represent the execution order constraints between sub-tasks. In specific implementation, spatial continuity refers to the Cartesian distance between two consecutive sub-target points, and logical sequence refers to the sequential relationship of processes as specified by the technological requirements. The task execution dependency graph is established as follows: each sub-task of each robot is treated as a node, and the node identifier consists of the robot identifier and the sub-task sequence number. All adjacent time slices of sub-target points are traversed. When two sub-target points are assigned to different robots, the sub-task node corresponding to the preceding sub-target point is pointed to the sub-task node corresponding to the following sub-target point, establishing a directed edge. The weight of the directed edge is the Cartesian distance between the preceding and following sub-target points. When there is a technological logical relationship between two layers of sub-target points that is completed through the same workstation or the same transfer area, a directed edge is also established, with the weight set to the sum of the distances between the spatial center of the transfer area and the preceding and following sub-target points. The task execution dependency graph is stored as an adjacency list structure. Each node in the adjacency list structure records its set of predecessor nodes and set of successor nodes.

[0031] Example 3: Perform topological sorting on the task execution dependency graph and assign a task priority number to each subtask. In specific implementation, the Kahn algorithm is used to perform topological sorting on the task execution dependency graph. A queue of nodes with an in-degree of zero is maintained. All nodes with an in-degree of zero in the task execution dependency graph are added to the queue. Nodes are removed from the queue and output. The in-degree of the successor nodes pointed to by all directed edges corresponding to the removed node is decremented by one. When the in-degree of a successor node becomes zero, it is added to the queue. This process is repeated until the queue is empty. The output order of the topological sorting is the valid order of subtask execution. Task priority numbers are assigned incrementally starting from the number 1 according to the output order. The smaller the number, the higher the task priority. The subtask with a task priority number of 1 is the highest priority subtask.

[0032] A virtual token ring is established among the robot controllers, with each node of the virtual token ring corresponding to one robot controller. In a specific implementation, the virtual token ring is established through multicast communication based on the User Datagram Protocol (UDP) in an industrial Ethernet bus. Each robot controller has a unique ring address in the virtual token ring, which is the last two bytes of the robot controller's Media Access Control (MAC) address. A node ring sequence table is maintained in the virtual token ring, storing the logical order of the ring addresses of all robot controllers. The initial order of the node ring sequence table is determined according to the power-on order of the robot controllers. The token in the virtual token ring is a data structure, and the token content includes a current holder ring address field, a task completion identifier field, a target subtask identifier field, and an additional data field.

[0033] According to the task priority number, the tokens in the virtual token ring are passed between nodes in descending order of priority; when a robot controller obtains the token, the robot controller extracts the corresponding subtask from the subtask sequence and controls the robot to execute it; during the execution, the robot controller collects the pose feedback data of the robot end in real time through joint encoders and vision sensors. In specific implementation, the token transfer process is as follows: The current holder node of the virtual token ring is initialized to the robot controller corresponding to the highest priority number, which is 1. The robot controller assigned to the subtask with priority number 1 is queried. The current holder ring address field in the token is set to the ring address of that robot controller, and the target subtask identifier field is set to the identifier of that subtask. The token is sent to the virtual token ring via multicast. After receiving the token from the multicast message, the robot controller checks whether the current holder ring address field in the token matches the ring address of its own robot controller. If they match, the token is considered acquired. When the robot controller that acquired the token completes the current subtask, it generates a task completion identifier, which includes the successful execution status code and completion timestamp of the current subtask. This identifier is written into the task completion identifier field of the token. The robot controller queries all successor nodes adjacent to the current subtask node according to the task execution dependency graph, reads the task priority number of the subtask corresponding to each successor node, and selects the successor subtask with the smallest task priority number as the next subtask. The current holder ring address field in the token is modified to the ring address of the robot controller to which the next subtask is assigned, and the target subtask identifier field is modified to the identifier of the next subtask. The token is then transmitted to the virtual token ring via multicast. The robot controller extracts the corresponding subtask from the subtask sequence by searching for the corresponding subtask data based on the subtask identifier in the locally stored subtask sequence. The subtask data includes the sub-target point position coordinates, attitude angles, and timestamps. The process of controlling the robot to execute the subtask involves inputting the sub-target point pose into the motion planner to generate joint space trajectories, and driving the robot's joints to move to complete the sub-target point through position closed-loop control. During execution, the joint encoder collects the angle values ​​of each joint at a fixed sampling frequency of 2 kHz, and the vision sensor collects the three-dimensional spatial coordinates of the robot's end-effector marker at a fixed frame rate of 100 Hz. The joint angle values ​​are used to calculate the first end-effector pose through the robot's forward kinematics model. The first end-effector pose is then fused with the three-dimensional spatial coordinates collected by the vision sensor using Kalman filtering to obtain the robot's end-effector pose feedback data, which includes position coordinates and attitude quaternions.

[0034] After the robot corresponding to the current holder node completes the current subtask, the node generates a task completion identifier and attaches it to the token. The current holder node then queries the priority number of the next subtask adjacent to the current subtask based on the task execution dependency graph. The current holder node then passes the token to the target robot controller node corresponding to the priority number of the next subtask. In a specific implementation, the method for querying the priority number of the next subtask adjacent to the current subtask is as follows: starting from the current subtask node in the task execution dependency graph, traverse all successor nodes pointed to by outgoing edges, obtain the subtask identifier and its task priority number corresponding to each successor node, compare the numerical values ​​of the task priority numbers of all successor nodes, and select the task priority number with the smallest value as the priority number of the next subtask. If the current subtask node has no successor node, it indicates that the current subtask is the end point of the task execution dependency graph, and the token is passed to the pre-specified terminal node in the virtual token ring. The terminal node is the main robot controller responsible for reporting the task completion status.

[0035] Example 4: Using the pose feedback data, the actual motion trajectory of each robot end effector is reconstructed to obtain the reconstructed trajectory. This includes sorting the pose feedback data according to the sampling time to obtain a discrete pose sequence. In specific implementation, the pose feedback data is marked with a sampling timestamp provided by a synchronous clock during acquisition. The discrete pose sequence is sorted using a merge sort algorithm, arranging all pose feedback data in ascending order of sampling timestamps. Each element in the sorted discrete pose sequence contains a sampling timestamp, three-dimensional position coordinates, and an attitude quaternion.

[0036] Each pose in the discrete pose sequence undergoes a coordinate system transformation to unify it into a preset world coordinate system, resulting in a unified coordinate system pose sequence. In specific implementations, the preset world coordinate system is a global Cartesian coordinate system shared by all robot controllers. The coordinate system transformation process involves maintaining a homogeneous transformation matrix from the robot base coordinate system to the world coordinate system, and a homogeneous transformation matrix from the vision sensor coordinate system to the world coordinate system. Both homogeneous transformation matrices are obtained through offline hand-eye calibration and base calibration. For pose feedback data derived from joint encoders after forward kinematics calculation, a multiplicative transformation is performed using the homogeneous transformation matrix from the robot base coordinate system to the world coordinate system. For pose feedback data derived from the vision sensor, a multiplicative transformation is performed using the homogeneous transformation matrix from the vision sensor coordinate system to the world coordinate system. All transformed pose points are represented in the world coordinate system. All transformed pose points are arranged according to the sampling timestamp order to obtain the unified coordinate system pose sequence.

[0037] An interpolation algorithm is used to fit the pose sequence in the unified coordinate system to generate a continuous and smooth trajectory curve, which serves as the reconstructed trajectory. Specifically, the Cartesian spatial distance and attitude angle change between two adjacent pose points in the unified coordinate system pose sequence are calculated; the motion time interval between two adjacent pose points is determined based on the Cartesian spatial distance and the attitude angle change; and the trajectory curve is generated by fitting the coordinates of the pose points in the unified coordinate system pose sequence and the motion time interval using a fifth-order polynomial interpolation algorithm.

[0038] In practice, the Cartesian spatial distance between two adjacent pose points is the Euclidean distance. Let the position coordinates of two adjacent pose points be respectively... and Cartesian spatial distance The calculation method is as follows The change in attitude angle is the angle difference between two attitude quaternions, which is obtained by calculating the norm of the difference between the two quaternions and taking the arcsine. The motion time interval is determined by the Cartesian space distance and the change in attitude angle. The motion time interval is taken as the maximum value between the preset maximum allowable linear velocity divided by the Cartesian space distance and the preset maximum allowable angular velocity divided by the change in attitude angle. The preset maximum allowable linear velocity is set to 1.5 m / s and the preset maximum allowable angular velocity is set to 1.0 radians / s.

[0039] The trajectory curve is generated as follows: Each pair of adjacent pose points in the unified coordinate system pose sequence is used as the start and end points of a local trajectory segment. A fifth-order polynomial curve is constructed for each local trajectory segment, and the position trajectory and attitude trajectory are fitted separately. The fifth-order polynomial form of the position trajectory is: ,in, This represents the local time variable of the current segment's local trajectory. The domain is , This is the motion time interval corresponding to the current segment; Indicates local time The position coordinate vector at a given time. ; to This is a coefficient vector of a polynomial, where each coefficient vector contains three components corresponding to... , , The axis; the coefficient vector is solved through boundary conditions, which are: when hour, Equal to the coordinates of the starting position of the current segment. Equal to the starting velocity vector, Equal to the initial acceleration vector; when hour, Equals the coordinates of the current segment's end point. Equal to the final velocity vector, The initial and final velocity vectors are both set to zero; the initial and final acceleration vectors are also set to zero. Substituting the boundary conditions into the fifth-degree polynomial equation, we obtain the equation regarding... to The system of linear equations is solved to obtain the coefficient vector. The attitude trajectory is fitted to each component of the quaternion using a fifth-order polynomial of the same form. The velocity and acceleration in the boundary conditions correspond to the rate of change of the quaternion and its derivative with respect to the rate of change. The solution process is the same as that for the position trajectory. All local trajectory curves are stitched together to obtain a continuous and smooth reconstructed trajectory.

[0040] Example 5: See Figure 4 The theoretical pose sequence corresponding to the current reconstructed trajectory is extracted from the subtask sequence, and a theoretical trajectory curve is generated. In specific implementation, the time interval corresponding to the current reconstructed trajectory is determined by the starting and ending sampling timestamps of the reconstructed trajectory. Based on the time interval, a search is performed in the subtask sequence to extract all sub-target points whose sampling timestamps fall within the time interval of the current reconstructed trajectory. The extracted sub-target points include position coordinates and attitude angles, and are arranged in the order of sampling timestamps to form a theoretical pose sequence. The theoretical trajectory curve is generated using the same fifth-order polynomial interpolation algorithm as the one used to generate the reconstructed trajectory. The pose points in the theoretical pose sequence are used as control points, and the motion time interval between each two adjacent control points is used as the interpolation interval length to fit a continuous and smooth theoretical trajectory curve.

[0041] The reconstructed trajectory and the theoretical trajectory curve are aligned to the same time axis, and the positional and phase deviations between them at each moment are calculated. In specific implementations, time axis alignment uses absolute timestamp alignment, with the global time of the synchronized clock as the reference. Data points on both the reconstructed trajectory and the theoretical trajectory curve carry absolute timestamps. The resampling of both the reconstructed trajectory and the theoretical trajectory curve is performed at the same absolute timestamp, with a resampling frequency of 1 kHz. The positional deviation is calculated by taking the difference between the position coordinate vectors of the reconstructed trajectory and the theoretical trajectory curve at the same absolute timestamp, and calculating the Euclidean norm of the difference as the positional deviation value. The phase deviation is calculated by taking the arc length parameter corresponding to the position coordinate vector of the reconstructed trajectory and the arc length parameter corresponding to the position coordinate vector of the theoretical trajectory curve at the same absolute timestamp, and calculating the difference between the two arc length parameters as the phase deviation value. The arc length parameter is the cumulative path length traveled along the trajectory curve from the starting point.

[0042] The system determines whether the positional deviation and the phase deviation exceed their respective allowable deviation ranges. In a specific implementation, the allowable deviation range for the positional deviation is set to ±0.5 mm; when the absolute value is greater than 0.5 mm, the positional deviation is determined to exceed the allowable deviation range. The allowable deviation range for the phase deviation is set to ±1.0 mm; when the absolute value is greater than 1.0 mm, the phase deviation is determined to exceed the allowable deviation range. The allowable deviation range for the positional deviation is determined based on the robot's repeatability accuracy index, and the allowable deviation range for the phase deviation is determined based on the process tolerance of the end effector contact or assembly in the collaborative task.

[0043] When the position deviation or phase deviation exceeds the allowable deviation range, a synchronization adjustment command is generated. This command includes the target robot identifier to be adjusted and the adjustment amount. In a specific implementation, when the position deviation exceeds the allowable deviation range, the integral value of the position deviation between the reconstructed trajectory and the theoretical trajectory is calculated. The integral value is the integral of the position deviation value with respect to time from the moment the position deviation first exceeds the allowable deviation range to the current moment. A position compensation vector is determined based on the integral value. The direction of the position compensation vector is opposite to the direction of the position deviation vector. The magnitude of the position compensation vector is obtained by multiplying the integral value of the position deviation by a proportional gain coefficient, which is set to 0.8. The proportional gain coefficient is set so that the single compensation amount does not exceed the integral value of the position deviation itself. The position compensation vector is used as the adjustment amount. When the phase deviation exceeds the allowable deviation range, the rate of change of the phase difference between the reconstructed trajectory and the theoretical trajectory is calculated. The rate of change of the phase difference is the first derivative of the phase deviation value with respect to time, and the time step for the difference calculation is 1 millisecond. The phase compensation amount is determined based on the rate of change of the phase difference. The expression for calculating the phase compensation amount is: ,in, This indicates the phase compensation amount, expressed in meters. This represents the rate of change of phase difference, that is, the first derivative of the phase deviation value with respect to time, and its unit is meters per second; This represents the acceleration due to the change in phase difference, i.e., the second derivative of the phase deviation with respect to time, and is expressed in meters per second. 2 ; This represents the first-order gain coefficient, with a value ranging from 0.01 to 0.05. It is obtained through offline parameter tuning, with the tuning target being a phase deviation response time of less than 10 control cycles. This represents the second-order gain coefficient, ranging from 0.001 to 0.01, obtained through offline parameter tuning. The tuning objective is to suppress overshoot oscillations during phase deviation adjustment. The phase compensation amount is used as the adjustment value. The robot identifier corresponding to the robot controller that caused the position or phase deviation is determined. Specifically, the source field of the reconstructed trajectory is read. This source field records the unique identifier of the robot controller corresponding to the pose feedback data used to acquire the reconstructed trajectory. This unique identifier of the robot controller is used as the target robot identifier.

[0044] The synchronization adjustment command is sent to the virtual token ring corresponding to the token passing mechanism. The virtual token ring modifies the dwell time of the token at the target robot controller node and adjusts the subsequent passing order of the token according to the adjustment amount. In specific implementation, the synchronization adjustment command is sent to all nodes of the virtual token ring via multicast message, which includes a target robot identifier field and an adjustment amount field. After receiving the synchronization adjustment command, the target robot controller node parses the adjustment amount into a time compensation value and an order adjustment value. The time compensation value is determined as follows: the magnitude of the position compensation vector is divided by the current actual linear velocity of the robot end effector to obtain the duration corresponding to the position compensation; the phase compensation amount is divided by the current actual linear velocity of the robot end effector to obtain the duration corresponding to the phase compensation; the maximum value between the duration corresponding to the position compensation and the duration corresponding to the phase compensation is taken as the time compensation value. The order adjustment value is determined as follows: when the position or phase deviation indicates that the target robot is lagging behind the theoretical trajectory, the order adjustment value is set to -1, meaning the target robot controller node's position in the subsequent token transmission order will be moved forward one position; when the position or phase deviation indicates that the target robot is ahead of the theoretical trajectory, the order adjustment value is set to +1, meaning the target robot controller node's position in the subsequent token transmission order will be moved backward one position. The dwell time of the token at the target robot controller node is increased or decreased by a time compensation value. The subsequent token transmission order is rearranged according to the order adjustment value, and subsequent token transmissions are executed according to the updated node ring order.

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

Claims

1. A multi-robot cooperative control method for industrial control, characterized in that, The method includes: Receive synchronization signals from the industrial control bus and generate a global time base; Based on the global time reference, the local clocks of each robot controller are compensated and calibrated to obtain a synchronized clock; Based on the synchronization clock, the multi-dimensional collaborative tasks to be executed are decomposed according to preset rules to generate sub-task sequences and task execution dependency graphs for each robot. Based on the task execution dependency graph, a token passing mechanism based on task priority is established among the robot controllers. The token passing mechanism controls each robot to execute the sub-task sequence and collects pose feedback data of each robot end in real time. Using the pose feedback data, the actual motion trajectory of each robot end effector is reconstructed to obtain the reconstructed trajectory; The reconstructed trajectory is compared with the corresponding theoretical trajectory in the subtask sequence. When the comparison result indicates that the phase deviation exceeds the allowable range, a synchronization adjustment command is generated.

2. The multi-robot cooperative control method for industrial control according to claim 1, characterized in that, The process of receiving synchronization signals from the industrial control bus and generating a global time base specifically includes: Monitor periodically broadcast synchronization messages on the industrial Ethernet bus, the synchronization messages containing timestamp information; Extract the timestamp information and record the local reception time; Calculate the signal transmission delay based on the timestamp information and the local reception time; The timestamp information is corrected using the signal transmission delay to obtain the global time base.

3. The multi-robot cooperative control method for industrial control according to claim 1, characterized in that, Based on the global time base, the local clocks of each robot controller are compensated and calibrated to obtain a synchronized clock, specifically including: Get the current local clock count value for each robot controller; The difference between the global time base and the current count value of the local clock is calculated and used as the clock deviation value for each robot controller; The clock deviation value is used as a compensation amount and input into the clock synchronization module of each robot controller, so that the clock synchronization module can adjust the oscillation frequency of the local clock. When the difference between the adjusted local clock and the global time reference is less than a preset synchronization threshold, the adjusted local clock is determined as the synchronization clock of the robot controller.

4. A multi-robot cooperative control method for industrial control according to claim 1, characterized in that, The generation of sub-task sequences and task execution dependency graphs for each robot specifically includes: Obtain the overall objective description of the multi-dimensional collaborative task, which includes a task space coordinate sequence and an attitude angle sequence; Based on the synchronization clock, the overall target description is sampled according to a preset time slice length to obtain multiple sub-target points corresponding to time slices; Based on the kinematic and dynamic constraints of each robot controller, the sub-target points corresponding to each time slice are assigned to the corresponding robots to generate the sub-task sequence. Based on the spatial continuity and logical order of the sub-target points, a task execution dependency graph is established. The nodes of the task execution dependency graph are the sub-tasks, and the directed edges of the task execution dependency graph represent the execution order constraints between the sub-tasks.

5. A multi-robot cooperative control method for industrial control according to claim 1, characterized in that, Based on the task execution dependency graph, a token passing mechanism based on task priority is established among the robot controllers. This mechanism controls each robot to execute the sub-task sequence and collects pose feedback data from each robot's end effector in real time. Specifically, this includes: Perform topological sorting on the task execution dependency graph and assign a task priority number to each subtask; A virtual token ring is established among the robot controllers, and each node of the virtual token ring corresponds to one robot controller; Based on the task priority number, the tokens in the virtual token ring are passed between the nodes in descending order of priority. When a robot controller obtains the token, it extracts the corresponding subtask from the subtask sequence and controls the robot to execute it. During execution, the robot controller collects pose feedback data of the robot end effector in real time through joint encoders and vision sensors.

6. A multi-robot cooperative control method for industrial control according to claim 5, characterized in that, Based on the task priority number, the tokens in the virtual token ring are passed between nodes in descending order of priority, specifically including: The current holder node of the virtual token ring is initialized to the robot controller corresponding to the highest priority number; When the robot corresponding to the current holder node completes the current sub-task, the node generates a task completion identifier and attaches the task completion identifier to the token; The current holder node queries the priority number of the next subtask adjacent to the current subtask based on the task execution dependency graph. The current holder node passes the token to the target robot controller node corresponding to the priority number of the next subtask.

7. A multi-robot cooperative control method for industrial control according to claim 1, characterized in that, Using the pose feedback data, the actual motion trajectory of each robot end effector is reconstructed to obtain the reconstructed trajectory, specifically including: The pose feedback data is sorted according to the sampling time to obtain a discrete pose sequence; Each pose in the discrete pose sequence is transformed to a preset world coordinate system to obtain a unified coordinate system pose sequence. An interpolation algorithm is used to fit the pose sequence of the unified coordinate system to generate a continuous and smooth trajectory curve, which serves as the reconstructed trajectory.

8. A multi-robot cooperative control method for industrial control according to claim 7, characterized in that, The step of fitting the pose sequence in the unified coordinate system using an interpolation algorithm to generate a continuous and smooth trajectory curve as the reconstructed trajectory specifically includes: Calculate the Cartesian spatial distance and attitude angle change between two adjacent pose points in the unified coordinate system pose sequence; The motion time interval between two adjacent pose points is determined based on the Cartesian spatial distance and the change in attitude angle. The trajectory curve is generated by fitting a fifth-order polynomial interpolation algorithm based on the pose point coordinates in the pose sequence of the unified coordinate system and the motion time interval.

9. A multi-robot cooperative control method for industrial control according to claim 1, characterized in that, The generation of synchronization adjustment instructions specifically includes: Extract the theoretical pose sequence corresponding to the current reconstructed trajectory from the subtask sequence, and generate the theoretical trajectory curve; Align the reconstructed trajectory with the theoretical trajectory curve on the same time axis, and calculate the positional and phase deviations between the reconstructed trajectory and the theoretical trajectory at each moment. Determine whether the position deviation and the phase deviation exceed their respective allowable deviation ranges; When the position deviation or the phase deviation exceeds the allowable deviation range, the synchronization adjustment instruction is generated, which includes the target robot identifier to be adjusted and the adjustment amount. The synchronization adjustment command is sent to the virtual token ring corresponding to the token passing mechanism. The virtual token ring modifies the dwell time of the token at the target robot controller node and adjusts the subsequent passing order of the token according to the adjustment amount.

10. A multi-robot cooperative control method for industrial control according to claim 9, characterized in that, When the position deviation or the phase deviation exceeds the allowable deviation range, the synchronization adjustment command is generated. The synchronization adjustment command includes the target robot identifier to be adjusted and the adjustment amount, specifically including: When the position deviation exceeds the allowable deviation range, the integral value of the position deviation between the reconstructed trajectory and the theoretical trajectory is calculated, the position compensation vector is determined based on the integral value of the position deviation, and the position compensation vector is used as the adjustment amount; When the phase deviation exceeds the allowable deviation range, the rate of change of the phase difference between the reconstructed trajectory and the theoretical trajectory is calculated, the phase compensation amount is determined based on the rate of change of the phase difference, and the phase compensation amount is used as the adjustment amount. The robot identifier corresponding to the robot controller that caused the positional deviation or the phase deviation is determined, and the robot identifier is used as the target robot identifier.

Images