Multi-robot collaborative control system

Abstract

This application relates to the field of robotics technology and discloses a multi-manipulator cooperative control system, including a spatiotemporal elastic bundle construction module for generating spatiotemporal elastic bundles containing a central trajectory and non-uniform elastic bundles for each manipulator; a continuous cooperative deformation module for guiding bundle deformation through gradients weighted by the elastic modulus to eliminate spatiotemporal interference; a topological deadlock detection module for determining whether the system is trapped in a local optimum; a discrete topology negotiation module for generating and selecting the optimal macroscopic topology scheme, such as temporal rearrangement or spatial detour, when the system is trapped in a deadlock; and a constraint re-injection and reset module for converting the scheme into new constraints to guide subsequent planning. This invention, through a hybrid mechanism combining continuous planning and discrete decision-making, can proactively identify and break through local optima, significantly improving the success rate and globality of planning, while simultaneously considering computational efficiency and task execution accuracy.

Description

Technical Field

[0001] This invention relates to the field of robotics, specifically to a multi-robotic arm collaborative control system. Background Technology

[0002] In applications such as automated production and smart warehousing, deploying multiple robotic arms for collaborative operation is an important way to improve production efficiency and flexibility. Multi-robotic arm collaborative motion planning is the core technology to achieve this goal. Its purpose is to generate a conflict-free motion trajectory for each robotic arm that can complete the specified task.

[0003] Currently, a mainstream technical approach is based on continuous optimization planning methods. These methods typically transform the obstacle avoidance problem between robotic arms into a potential energy field optimization problem, guiding the trajectory to iteratively deform through gradient descent and other means until the conflict is eliminated. These methods have high computational efficiency; however, they are essentially local searches. When robotic arms form a specific confrontation posture, the system gradient may approach zero, but the actual physical interference still exists. This state is called a topological deadlock. In this state, the system cannot resolve the conflict by fine-tuning the trajectory, leading to planning failure.

[0004] To overcome the local optima problem, another approach employs discrete search methods, such as searching within a combinatorial or discretized state space, to find a globally feasible, conflict-free path. While these methods theoretically possess the ability to find global solutions, their computational complexity increases dramatically with the number of robotic arms and the complexity of the environment, resulting in a combinatorial explosion problem. This makes them unsuitable for real-time applications. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a multi-robotic arm collaborative control system, which solves the problem that existing multi-robotic arm collaborative planning methods have limitations in handling complex obstacle avoidance problems, where it is difficult to balance efficiency and global solution.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a multi-robotic arm collaborative control system, comprising: A spatiotemporal elastic bundle construction module is used to generate a spatiotemporal elastic bundle for each robotic arm based on the task-critical pose sequence of each robotic arm; the spatiotemporal elastic bundle includes a central trajectory, a bundle radius, and a non-uniform elastic modulus distributed along the central trajectory. A continuous cooperative deformation module is used to calculate the interference repulsion potential energy based on the spatiotemporal interference between the spatiotemporal elastic tubes, and guide the spatiotemporal elastic tubes to deform through a gradient weighted by the elastic modulus to eliminate spatiotemporal interference; A topological deadlock detection module is used to monitor the interference repulsion potential energy and the gradient during the continuous cooperative deformation process to determine whether the system is trapped in a topological deadlock. The discrete topology negotiation module is used to generate a set of topology candidates containing temporal rearrangement schemes or spatial detour schemes when the system is stuck in a topology deadlock, and select an optimal topology scheme from the set of topology candidates through a distributed consensus decision algorithm. The constraint re-injection and reset module is used to convert the optimal topology scheme into one or more specific mathematical or logical constraints, and reset the planning state of the affected robotic arm based on the mathematical or logical constraints, so as to re-enter the continuous cooperative deformation process. The trajectory execution module is used to convert the final center trajectory into a joint space trajectory that can be executed by the robot controller after the continuous cooperative deformation process converges, and then send it to the controller of each robotic arm.

[0007] The distribution characteristics of the non-uniform elastic modulus are as follows: At the moment corresponding to the key pose sequence of the mission, the elastic modulus is high, while in the transition section between two adjacent key poses, the elastic modulus is low.

[0008] The central trajectory is parameterized by a set of B-spline control points; The continuous cooperative deformation module achieves the deformation of the spatiotemporal elastic tube bundle by applying the gradient to the B-spline control point.

[0009] The topological deadlock detection module determines whether the system is trapped in a topological deadlock by judging whether the gradient is lower than a first preset threshold and the interference repulsion potential energy is higher than a second preset threshold simultaneously.

[0010] The discrete topology negotiation module is specifically used for: For each affected robotic arm, a multidimensional cost vector is calculated for each candidate solution in the topology candidate set; The multidimensional cost vector is converted into a scalar cost value through a cost function; The scalar cost of all affected robotic arms is aggregated to obtain the total cost of each candidate solution, and the candidate solution with the minimum total cost is selected as the optimal topology solution.

[0011] The multidimensional cost vector includes: At least one of time cost, energy cost, and path deviation cost.

[0012] The cost function is a weighted sum function, which obtains the scalar cost value by weighted summation of the costs of each dimension of the multidimensional cost vector.

[0013] When the optimal topology scheme is a temporal rearrangement scheme, the constraint re-injection and reset module converts the scheme into a temporal inequality constraint, which is used to define the order in which the affected robotic arms pass through the conflict area.

[0014] When the optimal topology scheme is a spatial detour scheme, the constraint re-injection and reset module converts the scheme into a forced path point and inserts the forced path point into the task-critical pose sequence of the affected robotic arm.

[0015] The trajectory execution module converts the central trajectory into the joint space trajectory by discretizing the central trajectory and calling the inverse kinematics solver for each sampling point.

[0016] This invention provides a multi-robotic arm collaborative control system. It has the following beneficial effects: 1. By setting up a topology deadlock detection module, this invention can identify local optimal traps that cannot be solved by continuous optimization alone in real time. Once a deadlock is detected, the system activates the discrete topology negotiation module to break the deadlock state from a macroscopic level, thereby avoiding planning failure and enabling the system to jump out of local optima and continue to converge toward a globally feasible solution.

[0017] 2. This invention introduces a spatiotemporal elastic tube model with non-uniform elastic modulus. By setting a high elastic modulus at key mission locations and a low elastic modulus in transitional path segments, and combining the gradient weighted by this modulus for trajectory deformation, the system ensures that trajectory adjustments for obstacle avoidance mainly occur in transitional sections where accuracy requirements are not high, while the trajectory accuracy at key mission locations is effectively maintained.

[0018] 3. The hybrid planning architecture of this invention enables the system to operate in the computationally efficient, gradient-based continuous cooperative deformation stage for most of the time. Only when necessary, i.e. when a topological deadlock is detected, will the system initiate the relatively computationally expensive discrete topology negotiation process. This hierarchical processing strategy avoids the huge computational overhead caused by always using high-complexity discrete search, and significantly improves the overall planning efficiency while ensuring that feasible solutions can be found. Attached Figure Description

[0019] Figure 1 This is a system architecture diagram of the present invention; Figure 2 This is the overall flowchart of the hybrid collaborative planning of the present invention.

[0020] Among them, 100 is the spatiotemporal elastic bundle construction module; 200 is the continuous cooperative deformation module; 300 is the topological deadlock detection module; 400 is the discrete topology negotiation module; 500 is the constraint re-injection and reset module; and 600 is the trajectory execution module. Detailed Implementation

[0021] 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, and 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.

[0022] Example: Please see the appendix Figure 1 and Figure 2 The present invention provides a multi-robotic arm collaborative control system, which includes: a spatiotemporal elastic bundle construction module 100, a continuous collaborative deformation module 200, a topological deadlock detection module 300, a discrete topology negotiation module 400, a constraint re-injection and reset module 500, and a trajectory execution module 600.

[0023] The function of the spatiotemporal elastic bundle construction module 100 is to generate a mathematically continuous and differentiable center trajectory for the motion planning of each robotic arm. This process transforms a high-level, discretized task description into a parameterized curve that can be processed by the subsequent optimization module.

[0024] The parametric characterization process of the center trajectory includes the following steps: To obtain the key pose sequence for a given cooperative task, the system first decomposes it into the pose sequence of each robotic arm. A series of discrete key pose sequences that need to be executed. It defines a series of positions and attitude points that the end effector of the robotic arm must pass through or approach during the task.

[0025] To generate a smooth, continuous trajectory between the aforementioned discrete key poses, the system selects and applies a parametric curve model. In this embodiment of the invention, a B-spline curve model is used because it has local controllability, meaning that adjusting a control point only affects a local area of ​​the trajectory, which is beneficial for subsequent cooperative deformation optimization.

[0026] It should be understood that the parametric curve model described herein is not limited to B-spline curves. In other embodiments, other mathematical tools that can define the shape of a curve with a set of control parameters, such as non-uniform rational B-spline curves, Bézier curves, or piecewise polynomial functions, may also be used. These methods all fall within the scope of parametric representation claimed by this invention.

[0027] The mathematical expression for generating the center trajectory is derived from the acquired key pose sequence. Perform curve fitting or interpolation to calculate a set of B-spline control points. Therefore, the robotic arm The center trajectory Represented as time Functions: ; in: Represents robotic arm At any moment The position of the end effector; It is a set that contains There are 10 control points, and the coordinates of these control points together determine the shape of the entire trajectory; It is a set The first in One control point; It is the order of the B-spline curve; It is the first indivual The definition of the B-spline basis function is well-known in the field and will not be elaborated here.

[0028] In this way, the description of the robotic arm's motion trajectory is transformed from a series of discrete points into a description of a finite number of control points. Fully defined continuous functions The advantage of this representation is that any subsequent adjustments to the trajectory shape can be made by modifying the control point set. The numerical values ​​are used to achieve this, providing a mathematical basis for gradient-based optimization of the continuous cooperative deformation module 200.

[0029] After generating the central trajectory, the spatiotemporal elastic tube construction module 100 also performs a key function: establishing and assigning an elastic modulus field to the trajectory. This elastic modulus field is used to quantify the deformability of different parts of the trajectory in the subsequent collaborative negotiation process, that is, its stiffness against deformation.

[0030] The modeling and assignment process of the elastic modulus field includes the following steps: Identify and segment task sections, and analyze the central trajectory of the system. The corresponding task logic, in the time domain The interior is divided into different sections, which may include at least: Critical sections of the task: These are the parts of the path that have strict requirements for pose accuracy, timing, or tool interaction.

[0031] Transitional movement sections: These refer to path segments that are mainly used to connect key sections and move in relatively open spaces, with weaker constraints on the specific shape of the path.

[0032] Define and apply the modulus assignment rules, and assign them to each time point on the central trajectory according to the segment division. Assign a specific value to the elastic modulus This assignment rule transforms abstract task constraints into quantifiable scalars.

[0033] The assignment rule can be a discretized mapping, with the system pre-setting high modulus values. and low modulus value For all time points identified as critical segments of the mission, the elastic modulus is assigned a value. For all transitional movement segments at any given time point, the elastic modulus is assigned a value. .

[0034] To avoid abrupt changes in the modulus value affecting subsequent gradient optimization, a continuous function can be used to define the elastic modulus field. For example, a Gaussian-like function can be established around each key event point of the task to superimpose the effects of each event. Its mathematical form can be: ; in: It is a robotic arm At any moment The value of the elastic modulus; It is the basic modulus value, usually a small positive number, representing the basic rigidity of the transition movement section; It is an index of the key events of the task; It is the first The precise timing of each mission's key events; Is with the first The magnitude of the modulus associated with an event; the larger the value, the stronger the constraint on that event. Is with the first The timescale parameter associated with each event determines the extent of the high-modulus region's influence on the time axis.

[0035] It should be understood that the method of assigning the elastic modulus is not limited to the two mentioned above. Any function or rule that can assign a quantitative rigidity value to the trajectory path according to the inherent properties of the task falls within the scope of elastic modulus field modeling and assignment defined in this invention.

[0036] To construct a complete elastic modulus field, the above assignment process is executed, ultimately generating a field defined throughout the planning time domain. Continuous or piecewise continuous functions This function forms the core component of the spatiotemporal elastic bundle. It will serve as the key input for the subsequent continuous cooperative deformation module 200 to calculate the interference potential energy and adjust the gradient update, thereby ensuring that the negotiation process respects the preset task constraints.

[0037] After completing the parameterization of the center trajectory and the modeling of the elastic modulus field, the spatiotemporal elastic bundle construction module 100 integrates these elements with geometric information representing the volume and safety margin of the robotic arm entity, thereby constructing a complete spatiotemporal elastic bundle for subsequent collaborative negotiation.

[0038] The integrated construction process includes the following steps: To establish a bundle radius function and represent the physical entity of the robotic arm in spacetime, the system needs to define a center trajectory. Each point on the tube determines a tube radius. This radius function reflects the time... The robotic arm occupies a certain amount of space due to its own configuration, and includes the necessary safety margin.

[0039] Tube radius function The method for determining this is to obtain the time step by solving inverse kinematics. The corresponding robotic arm joint configuration is then determined. The radius of the minimum enveloping sphere capable of encompassing the entire robotic arm body within this configuration is calculated, and a preset safety margin is added to this radius to determine the control radius at that moment. .

[0040] To more accurately characterize the slender structure of a robotic arm, the geometric description of the tube bundle does not have to be limited to a sphere of a single radius. For example, a set of time-varying envelope capsules can be used to characterize the main links of the robotic arm. In this case, the tube bundle radius function It can be a vector or a set of parameters used to define the geometry of these more complex envelopes. All these methods that can characterize the volume occupied by the robotic arm in spacetime fall within the scope of the bundle radius function defined in this invention.

[0041] The spatiotemporal elastic bundle is constructed by integrating the parameterized center trajectory, elastic modulus field, and bundle radius function obtained in the preceding steps into a unified mathematical object, namely, the robotic arm. spatiotemporal elastic bundle Its mathematical structure can be defined as a tuple: ; in: It is composed of control point set The defined parametric center trajectory represents the geometric path of the motion; It is a time-varying function of the tube radius, representing the spatial volume and safe range occupied by the robotic arm at each point on the path; It is an elastic modulus field distributed along the trajectory, representing the negotiability or rigid constraints of each part of the path.

[0042] At this point, the robotic arm The complete motion intention is encapsulated in this spatiotemporal elastic bundle In this context, the data structure, as a self-consistent information unit, not only describes the planned motion path and space occupied by the robotic arm, but more importantly, it uses the elastic modulus field... Carrying metadata about the flexibility of the intent, this spatiotemporal elastic bundle will serve as the basic input for subsequent interferometry detection and gradient optimization in the continuous cooperative deformation module 200.

[0043] The core function of the continuous cooperative deformation module 200 is to establish and quantify the interference relationship between each robotic arm based on the spatiotemporal elastic bundles. The continuous cooperative deformation module 200 transforms the geometric spatiotemporal conflict into a scalar cost that can be processed by optimization algorithms by constructing an interference potential energy field.

[0044] The process of constructing this interference potential field includes the following steps: Detecting spatiotemporal interference, for any pair of robotic arms in the system. and The continuous collaborative deformation module 200 continuously monitors their spatiotemporal elastic bundles. and Whether there is spatiotemporal overlap is detected throughout the planning time domain. Performed within, at any time This detection is equivalent to determining two points originating from the center point. ,radius and center point ,radius If the defined spatial geometries intersect, then calculate the spatial overlap volume at that moment. For the calculation of the intersecting and overlapping volumes between spatial geometric bodies, those skilled in the art can use known geometric calculation methods, which are well-known technologies in the field and will not be elaborated here.

[0045] The system calculates the interferometric repulsion potential energy after detecting spatiotemporal interference, based on the degree of interference and the elastic moduli of the interfering parties. The magnitude of this potential energy reflects the degree of negative impact of the interference on the system's cooperative tasks.

[0046] Instantaneous interference repulsion potential energy The calculation formula can be defined as: ; in: It is a robotic arm and At any moment The instantaneous interference repulsion potential energy generated; It is two tubes at time Spatial overlap volume; and These are the two robotic arms at different times. The value of the elastic modulus; It is a positive potential energy proportionality constant, used to adjust the overall magnitude of potential energy; It is an exponential coefficient greater than or equal to 1. When its value is greater than 1, it can amplify the potential energy generated by severe interference, thereby exerting a stronger repulsive effect.

[0047] The core of this formula lies in the elastic modulus of the two interfering forces. and Introduced as a product term, its technical effect is that when two critical task sections with high elastic moduli interfere, even if the overlapping volume... The repulsive potential energy it generates is very small. The potential energy will also be significantly amplified, thus generating a strong repulsive force to protect these critical paths in subsequent optimization. Conversely, if the interference occurs in two transitional movement segments with low elastic modulus, the resulting repulsive potential energy will be relatively small, allowing the system to adjust these paths at a lower cost.

[0048] It should be understood that the above formula is only one implementation of the present invention. Any function whose value is positively correlated with the degree of spatiotemporal overlap and the elastic modulus of both interfering parties can be used to define the interference repulsion potential energy here. These alternative implementations should be considered to fall within the protection scope of the present invention.

[0049] By performing the above steps, the system transforms the complex geometric interference problem into a quantifiable potential energy field modulated by the elastic modulus. This potential energy field provides a clear objective function with task constraints for subsequent gradient calculation and cooperative deformation optimization.

[0050] After constructing the interference potential field, the main task of the continuous cooperative deformation module 200 is to calculate the driving force that can guide the shape of the spatiotemporal elastic bundle to evolve in the direction of reducing conflict. This process is achieved by calculating the potential gradient and applying a weighted mask based on the elastic modulus.

[0051] The process includes the following steps: Calculate the total repulsive potential energy for each robotic arm. The system first aggregates its data with all other robotic arms. The total repulsive potential energy generated by the interference The total repulsive potential energy is present throughout the entire planning time domain. Upper instantaneous interference repulsive potential energy Integrate the integral and sum over all other robotic arms: ; Here With robotic arm B-spline control point set It is a scalar function of independent variables that aggregates complex, multi-interference systems distributed in space and time into a single, optimizable objective function.

[0052] To calculate the potential energy gradient and determine the deformation direction that causes the fastest decrease in total repulsive potential energy, the system calculates... Relative to the control point set gradient The gradient is a vector whose direction points to the direction in which the total repulsive potential energy increases the fastest. Its negative direction is the optimal deformation direction. The gradient vector can be calculated by using the chain rule to decompose the partial derivative of the total repulsive potential energy with respect to the control point into the derivative of a function with respect to the central trajectory and then with respect to time. The specific implementation is known to those skilled in the art.

[0053] Constructing and applying a weighted mask: Directly applying the gradient described above would indiscriminately modify the entire trajectory, but this would destroy the mission-critical sections with high elastic modulus. To solve this problem, the system constructs and applies a weighted mask, which selectively suppresses or allows the gradient to affect different parts of the trajectory based on the magnitude of the elastic modulus.

[0054] The system first defines a weighting function that varies with time. Its value is related to the elastic modulus For a negative correlation, a feasible functional form is the exponentially decaying function: ; in: It is a moment The weight values ​​range from (0,1]; It is a moment The value of the elastic modulus; It is a positive adjustment coefficient used to control the rate at which the weight decreases as the elastic modulus increases.

[0055] When the elastic modulus When it is very high, this weight value Approaching 0; when the elastic modulus is very low, the weight value Approaching 1.

[0056] Subsequently, the system applies this weighting function during gradient calculation to obtain the final effective gradient applied to the control points. Its conceptual expression is: ; in, The operation represents the time-related weighting function. The effect is applied to the control point The physical meaning of the gradient calculation process is that, when calculating the total gradient, the contribution of the gradient component from the moment of high elastic modulus is weighted by the weighting function. The gradient components from the low elastic modulus moment are significantly weakened, while the gradient components from the low elastic modulus moment remain essentially unchanged.

[0057] By applying this weighted mask, the final effective gradient is obtained. The main guidance trajectory is deformed in transitional movement sections with high negotiation flexibility, while the shape of mission-critical sections is effectively maintained. This ensures that the cooperative collision avoidance process does not compromise the preset core mission constraints.

[0058] The continuous collaborative deformation module 200 adopts a distributed collaborative optimization mechanism, which transforms the collaborative problem of multiple robotic arms into a continuous optimization process in which each arm solves in parallel. Under this mechanism, each robotic arm, as an independent computing unit, coordinates its motion plan through information exchange with other parties and local computing to achieve a conflict-free state for the entire system.

[0059] This distributed collaborative optimization mechanism includes the following steps in each iteration: Broadcast the current spatiotemporal elastic bundle, in the optimization process of the first... At the start of the next iteration, each robotic arm in the system Its current state, i.e., its spatiotemporal elastic bundle It broadcasts to other robotic arms in the system via a communication network.

[0060] Perform local calculations, each robotic arm The controller receives spatiotemporal elastic bundles from other robotic arms Then, computations are performed independently and in parallel, including: The total repulsive potential energy exerted on it by all other robotic arms is calculated using the aforementioned method. Furthermore, the weighted effective gradient used to guide trajectory deformation is calculated. .

[0061] Update the local control point set based on the effective gradients calculated locally. robotic arm The controller employs the gradient descent method to determine the control point set of its B-spline curve. Update the control point set for the next iteration. The update rule can be expressed as: ; in: It is a robotic arm In the The control point set for the next iteration; It is a robotic arm In the current number The control point set for the next iteration; It is in the The calculation for the robotic arm in the next iteration The weighted effective gradient; It is a positive scalar, called the learning rate or step size, and its value determines the magnitude of trajectory deformation in each iteration.

[0062] In the next iteration, after all robotic arms have completed the above steps, a complete distributed iteration is finished, and the system then enters the next iteration. The next iteration continues, and its effect is to make the spatiotemporal elastic bundles of all robotic arms in the system evolve synchronously and cooperatively in the direction of decreasing total potential energy. The termination condition of this process is that the total potential energy of the system converges to below a preset threshold, or is interrupted by the topological deadlock detection module 300, which will be described later.

[0063] Through this distributed collaborative optimization mechanism, the system avoids dependence on a central planner and decomposes the complex global optimization problem into a series of parallel, low-computational-complexity local optimization problems, thereby improving the real-time performance and scalability of collaborative planning.

[0064] The topology deadlock detection module 300 is used to monitor the negotiation state of the system in real time during continuous cooperative deformation and determine whether the system is trapped in a topology deadlock based on a set of quantitative criteria. A topology deadlock is defined as a continuous cooperative deformation process that mathematically converges to a local optimum, but the solution still contains unacceptable motion impulses in physics.

[0065] The detection process for this topological deadlock includes the following steps: The system continuously monitors the effective gradients used to drive trajectory deformation, calculated in the continuous cooperative deformation module 200, to assess gradient stagnation conditions. When the gradient norm of all participating robotic arms is below a preset stagnation threshold When the gradient descent-based optimization process loses its momentum to further reduce the total potential energy of the system, the system reaches a stable point. The mathematical expression for this condition is: ; in: It acts on the robotic arm The effective gradient; The norm of a vector, such as the L2 norm; It is a preset, positive number close to zero, called the gradient stagnation threshold; It represents the total number of robotic arms in the system.

[0066] To assess the high residual conflict condition, while the gradient stagnation condition is satisfied, the system also needs to determine whether there are significant, unresolved residual conflicts. This determination is made by examining any pair of robotic arms. Does the maximum instantaneous interferometric repulsion potential energy between them exceed the preset conflict threshold? This is achieved by... The high residual conflict condition is considered to hold if at least one pair of robotic arms satisfies this condition, which is mathematically expressed as: ; in: It is a robotic arm and At any moment The instantaneous interference repulsion potential energy; It is in the entire planning time domain The operation of finding the maximum value within the range; It is a preset positive number, called the conflict threshold, which represents the maximum level of conflict that the system can tolerate.

[0067] The topological deadlock judgment is made by the topological deadlock detection module 300, which logically combines the two conditions mentioned above. The system determines that it has fallen into a topological deadlock if and only if the gradient stagnation condition and the high residual conflict condition are met simultaneously. This judgment means that the current conflict cannot be resolved by the continuous deformation method of fine-tuning the trajectory shape alone, and a higher-level decision-making mechanism must be initiated.

[0068] Once a topological deadlock is detected, the topological deadlock detection module 300 will also identify and output a set of all robotic arms involved in high residual conflicts. This set will be passed to the subsequent discrete topology negotiation module 400 as the target object for initiating its discrete negotiation process.

[0069] When the topology deadlock detection module 300 confirms that the system is stuck in a topology deadlock, the discrete topology negotiation module 400 is activated. The primary task of the discrete topology negotiation module 400 is to generate a set of macroscopic, discrete, and selectable solutions for the robot arm stuck in the conflict, which is the topology candidate set.

[0070] The process of generating this topology candidate set includes the following steps: Analyzing the conflict characteristics, the discrete topology negotiation module 400 first receives the set of conflicting robotic arms from the topology deadlock detection module 300. And related conflict information, for each pair of conflicting robotic arms The system analyzes its instantaneous interference repulsion potential energy function. Determine the moment of maximum potential energy. and the corresponding center trajectory point and This serves as the core spatiotemporal region of the conflict.

[0071] The system generates candidate temporal reordering schemes based on the characteristic that the conflict occurs in a specific time region. For a pair of conflicting robotic arms, the system generates a set of candidate temporal reordering schemes. This corresponds to two basic sequential execution orders, and the set of candidate schemes can be formally represented as a set containing all possible execution order permutations. For example, for robotic arms and Its temporal rearrangement candidate set is ,in Represents robotic arm Obtaining priority Waiting, and represent Obtaining priority wait.

[0072] The temporal reordering scheme presented here is a specific implementation of a discrete topology solution.

[0073] The system generates candidate spatial detour routes based on the characteristics of the conflict occurring in a specific spatial region. It first identifies the conflict point as the location of the conflict. A local reference coordinate system is established nearby, and then a set of discrete detour directions are defined. Subsequently, the system generates candidate schemes, designates one robotic arm as the detour direction and the other as the hold direction, and selects one from the preset detour directions.

[0074] The system assembles and outputs a topology candidate set, summarizing all temporal rearrangement and spatial bypass candidate schemes generated through the above steps to form a complete topology candidate set. This candidate set will be passed on to the subsequent cost evaluation stage as the basis for decision-making.

[0075] Through the above process, the system transforms the optimization deadlock problem in continuous space into a problem of choosing from a finite, discrete set of macroscopic strategies.

[0076] The discrete topology negotiation module 400 generates a set of topology candidates. Next, each candidate solution needs to be quantitatively evaluated for subsequent decision-making. This evaluation process is performed in a distributed manner and is based on a multi-dimensional cost model to comprehensively measure the impact of each solution on stakeholders.

[0077] The distributed cost evaluation process based on multidimensional cost includes the following steps: Perform local impact simulations of candidate solutions for the topology candidate set. Each candidate solution Every robotic arm affected by it The modification of the motion plan by the scheme needs to be performed locally first.

[0078] Calculate the multidimensional cost vector based on the new trajectory obtained from simulation. Compared with the original trajectory In comparison, robotic arm Calculate the multidimensional cost vector Each dimension of this vector represents a solution. From a specific perspective, the robotic arm The costs incurred, in this embodiment, may include, in the form of a multidimensional cost vector: Time cost : Indicates that the solution was adopted This results in an increase in the total task completion time.

[0079] Energy cost : Indicates that a new trajectory was executed. The estimated value of the additional energy consumption can be obtained by integrating the joint velocities, accelerations, and loads of the new trajectory.

[0080] Path deviation cost : Indicates a new trajectory Compared to the original optimal trajectory The degree of geometric deviation.

[0081] Calculate the scalar cost function; for ease of comparison, for each robotic arm... Targeting the plan Calculated multidimensional cost vector It is converted into a single scalar cost value through a preset cost function. The cost function is typically in the form of a weighted sum: ; in: It is a robotic arm For candidate solutions The final scalar value; , , These are weighting coefficients corresponding to time, energy, and path deviation costs, respectively. These weighting coefficients are predetermined based on the robotic arm. The specific task priority is set; for example, for a time-sensitive task, its time weight is... It will be set to a higher level.

[0082] The cost function here is a specific implementation of the functional features summarized in this invention, which unifies cost dimensions with different physical meanings into a comparable scalar space.

[0083] Broadcast local evaluation results, and calculate its own performance relative to all candidate solutions. scalar value After that, each related robotic arm The evaluation results are broadcast to other members of the consultation group to provide input for the next step of distributed consensus decision-making.

[0084] After collecting cost assessments of the candidate topology set from all relevant parties, the discrete topology negotiation module 400 initiates a distributed consensus decision algorithm, which can select the globally optimal topology solution from the candidate set.

[0085] The process of this distributed consensus decision-making algorithm includes the following steps: The total cost is calculated for each robotic arm participating in the negotiation within the system. After receiving the cost evaluation results broadcast by other robotic arms, for each candidate solution Calculate its total cost In this embodiment, the total cost is the sum of the scalar cost values ​​evaluated by all robotic arms affected by the scheme, and its calculation formula is as follows: ; in: It is a candidate solution The total global cost; It is all due to the execution plan The collection of robotic arms that incur costs; It is a robotic arm The solution calculated in the aforementioned steps The local scalar value.

[0086] The system applies a decision rule to select the optimal solution. This rule, pre-defined, selects the best solution from all candidate solutions. In this embodiment, the decision rule minimizes the total global cost. Based on this rule, the optimal topology solution is determined. It was identified as: ; in: It is the optimal topology scheme that is ultimately selected; Operators represent finding parameters that minimize the objective function.

[0087] The decision rule here is a specific implementation of the functional generalization of the present invention. It should be understood that the decision rule is not limited to minimizing the total cost. In other embodiments, other rules may also be adopted, such as selecting the scheme that minimizes the maximum individual cost, or adopting a voting-based mechanism. These all fall within the scope of the distributed consensus decision algorithm defined in the present invention.

[0088] Synchronizing decision-making results means that, since all participating robotic arms possess complete cost information and apply the same deterministic decision-making rules, they can independently and in parallel compute the same optimal topology. This eliminates the need for a central decision-maker to distribute outcomes. This process ensures that the decision outcome is agreed upon among all relevant parties.

[0089] The completion of this step marks the end of the discrete topology negotiation process, and the optimal topology scheme has been determined. It will be passed to the constraint re-injection and reset module 500 to transform this macroscopic, discrete decision into an executable, new spatiotemporal constraint.

[0090] The constraint re-injection and reset module 500 receives the optimal topology scheme determined by the discrete topology negotiation module 400. Then, its core function is to execute the transformation mechanism, translating this descriptive, macroscopic solution into one or more specific mathematical or logical constraints that can be directly executed by the planning system.

[0091] The conversion mechanism involves the following steps: This transformation mechanism identifies the type of optimal topology scheme by first analyzing the optimal topology scheme. The structure of the solution is used to determine its solution type. The main types include temporal rearrangement solutions and spatial detour solutions.

[0092] Transform the timing rearrangement scheme, if the optimal topology is... For example, determining a timing reordering scheme, such as a robotic arm. Priority should be given to robotic arms If a conflict region is encountered, the system converts it into a temporal inequality constraint. One specific implementation is as follows: First, determine the time window for the priority party to pass through the conflict zone. Then, constraints are generated for the waiting party, requiring the time it takes for them to enter the conflict zone. It must be later than the time when the priority party leaves the area. and add a safety time margin .

[0093] This transformation ultimately generates executable logical constraint expressions: ; This inequality is a concrete logical constraint derived from the abstract temporal topology.

[0094] If the spatial detour scheme is changed, then the optimal topology scheme is... For spatial navigation schemes, for example, determining the robotic arm Should be from the direction bypass robotic arm The system then converts it into a geometric path constraint. One specific implementation is as follows: First, determine the core spatiotemporal point where the conflict occurs, that is, at the moment of conflict. robotic arm Location .

[0095] Then, based on that location and the specified detour direction... Insert a new forced waypoint into the path planning for the detour direction, denoted as . The location of this path point can be calculated as follows: ; in, It is a positive scaling factor, representing the safe distance for detours. This transformation ultimately generates a new scale factor that includes the forced waypoints. The key pose sequence is a sequence of concrete mathematical constraints derived from abstract spatial topology.

[0096] Through the above steps, regardless of the type of discrete solution, it is precisely translated into mathematical or logical constraints that the system can understand and execute. This output provides clear and unambiguous input for subsequent system state reset and replanning.

[0097] After completing the conversion from abstract topology to concrete constraints, the constraint re-injection and reset module 500 is responsible for applying these newly generated constraints to the system and seamlessly resetting the planning state of the relevant robotic arms, guiding the entire collaborative planning process back into the continuous optimization stage.

[0098] The seamless reset and process re-entry process of this system includes the following steps: By applying the newly generated constraints, the system applies the new constraints generated in the aforementioned steps to the corresponding robotic arm. This application process is specifically manifested in modifying the input of the spatiotemporal elastic bundle construction module 100.

[0099] For timing constraints, the system will adjust the task-critical pose sequence of the affected robotic arm. The timestamp is used to ensure that the new sequence is satisfied.

[0100] For geometric path constraints, the system will assign new forced path points. Insert into the mission-critical pose sequence of the affected robotic arm middle.

[0101] Perform a partial reset of the planning state, applying new constraints only to those affected by the optimal topology. The robotic arm is directly affected, and the system re-invokes the spatiotemporal elastic bundle construction module 100 to generate a completely new spatiotemporal elastic bundle. This process will recalculate the parameterized center trajectory based on the updated key pose sequence (which includes new temporal and spatial constraints). Elastic modulus field and the tube radius function Unaffected robotic arms retain their original spatiotemporal elasticity. This method of updating only the relevant parties constitutes part of the seamless reset of this invention, avoiding a global redesign of the entire multi-arm system and significantly improving efficiency.

[0102] Re-entering the continuous cooperative deformation process, after the affected robotic arm completes the update of its spatiotemporal elastic bundle, the entire system—comprising the newly generated spatiotemporal elastic bundle and the unchanged old one—is resubmitted as a whole to the continuous cooperative deformation module 200. Starting from this module, the system restarts a new round of distributed iterative optimization. Since the new spatiotemporal elastic bundle already contains discrete decisions to resolve topological deadlocks, the potential energy local minima that previously caused optimization stagnation have been eliminated. Therefore, the new round of continuous deformation will be able to proceed in a new, better solution space and ultimately converge to a globally conflict-free cooperative motion plan.

[0103] Through the above steps, this invention realizes a complete continuous-discrete-continuous planning loop. This loop can efficiently handle most path adjustments in a gradient optimization manner, and when encountering local optimum traps, it can break out of the deadlock through discrete, consensus-based macro-decision-making, and then seamlessly return to the continuous optimization process, thus taking into account both planning efficiency and global solution.

[0104] In the final stage of this invention, when the continuous cooperative deformation process successfully converges, that is, when there are no longer any conflicts in the system that need to be resolved through negotiation, the trajectory execution module 600 is responsible for converting the final cooperative planning result into instructions that can be executed by the robot hardware.

[0105] The process of generating and distributing the final trajectory may include the following steps: After confirming convergence and extracting the final center trajectory, the trajectory execution module 600 first confirms that the collaborative planning process has converged. The specific convergence criterion is that the total interference repulsion potential energy within the system is lower than a preset final threshold close to zero. After confirming convergence, for each robotic arm The system solidifies its final B-spline control point set. And thereby determine its final, conflict-free central trajectory. The central trajectory represents the ideal path of the robotic arm's end effector as it changes over time in the task space.

[0106] Converting to joint space trajectory, the robot controller directly executes joint space commands rather than task space paths. Therefore, the system needs to convert the center trajectory in the task space. Converted into joint space trajectories executable by the robot controller This conversion process includes: First, for continuous center trajectories Control cycle of the robot controller Discretize the sampling over time to generate a sequence consisting of a dense set of timestamped end pose points. .

[0107] Subsequently, for each sampling pose in the sequence The system calls the inverse kinematics solver to calculate the corresponding joint angle vectors. To ensure smooth movement, when multiple inverse kinematics solutions exist, the system will select the joint angle that is the same as the one from the previous moment. The closest solution. For solving the inverse kinematics of a robot, those skilled in the art can employ methods such as Jacobian matrix iteration or analytical methods, which are well-known techniques in the field and will not be elaborated upon here.

[0108] Generate and issue executable instructions to obtain a complete, discretized sequence of joint space trajectories. Then, the trajectory execution module 600 packages it into one or more instruction packets. The data format of these instruction packets matches the interface of the target robot controller. These packets may contain information such as timestamps, target joint angles, and optional target joint velocities and accelerations. Finally, these instruction packets are sent to the underlying motion controllers of each robotic arm through the communication link to drive the robot entity to accurately execute the conflict-free motion after collaborative planning.

[0109] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A multi-robotic arm collaborative control system, characterized in that, include: A spatiotemporal elastic bundle construction module is used to generate a spatiotemporal elastic bundle for each robotic arm based on the task-critical pose sequence of each robotic arm; the spatiotemporal elastic bundle includes a central trajectory, a bundle radius, and a non-uniform elastic modulus distributed along the central trajectory. A continuous cooperative deformation module is used to calculate the interference repulsion potential energy based on the spatiotemporal interference between the spatiotemporal elastic tubes, and guide the spatiotemporal elastic tubes to deform through a gradient weighted by the elastic modulus to eliminate spatiotemporal interference; A topological deadlock detection module is used to monitor the interference repulsion potential energy and the gradient during the continuous cooperative deformation process to determine whether the system is trapped in a topological deadlock. The discrete topology negotiation module is used to generate a set of topology candidates containing temporal rearrangement schemes or spatial detour schemes when the system is stuck in a topology deadlock, and select an optimal topology scheme from the set of topology candidates through a distributed consensus decision algorithm. The constraint re-injection and reset module is used to convert the optimal topology scheme into one or more specific mathematical or logical constraints, and reset the planning state of the affected robotic arm based on the mathematical or logical constraints, so as to re-enter the continuous cooperative deformation process. The trajectory execution module is used to convert the final center trajectory into a joint space trajectory that can be executed by the robot controller after the continuous cooperative deformation process converges, and then send it to the controller of each robotic arm.

2. The multi-robotic arm collaborative control system according to claim 1, characterized in that, The distribution characteristics of the non-uniform elastic modulus are as follows: At the moment corresponding to the key pose sequence of the mission, the elastic modulus is high, while in the transition section between two adjacent key poses, the elastic modulus is low.

3. The multi-robotic arm collaborative control system according to claim 1, characterized in that, The central trajectory is parameterized by a set of B-spline control points; The continuous cooperative deformation module achieves the deformation of the spatiotemporal elastic tube bundle by applying the gradient to the B-spline control point.

4. The multi-robotic arm collaborative control system according to claim 1, characterized in that, The topological deadlock detection module determines whether the system is trapped in a topological deadlock by judging whether the gradient is lower than a first preset threshold and the interference repulsion potential energy is higher than a second preset threshold simultaneously.

5. The multi-robotic arm collaborative control system according to claim 1, characterized in that, The discrete topology negotiation module is specifically used for: For each affected robotic arm, a multidimensional cost vector is calculated for each candidate solution in the topology candidate set; The multidimensional cost vector is converted into a scalar cost value using a cost function. The scalar cost of all affected robotic arms is aggregated to obtain the total cost of each candidate solution, and the candidate solution with the minimum total cost is selected as the optimal topology solution.

6. The multi-robotic arm collaborative control system according to claim 5, characterized in that, The multidimensional cost vector includes: At least one of time cost, energy cost, and path deviation cost.

7. The multi-robotic arm collaborative control system according to claim 5, characterized in that, The cost function is a weighted sum function, which obtains the scalar cost value by weighted summation of the costs of each dimension of the multidimensional cost vector.

8. The multi-robotic arm collaborative control system according to claim 1, characterized in that, When the optimal topology scheme is a temporal rearrangement scheme, the constraint re-injection and reset module converts the scheme into a temporal inequality constraint, which is used to define the order in which the affected robotic arms pass through the conflict area.

9. The multi-robotic arm collaborative control system according to claim 1, characterized in that, When the optimal topology scheme is a spatial detour scheme, the constraint re-injection and reset module converts the scheme into a forced path point and inserts the forced path point into the task-critical pose sequence of the affected robotic arm.

10. The multi-robotic arm collaborative control system according to claim 1, characterized in that, The trajectory execution module converts the central trajectory into the joint space trajectory by discretizing the central trajectory and calling the inverse kinematics solver for each sampling point.

Images