Multi-mechanical-arm cooperative control method

Abstract

The invention provides a multi-mechanical-arm cooperative control method which comprises the following steps: modeling initial states and virtual constraints of mechanical arms, constructing an overall description of a multi-mechanical-arm cooperative task, decomposing the task into sub-tasks of the mechanical arms based on the description, and optimizing load distribution; the initial state is represented by a quintuple and comprises the position, posture, linear velocity, angular velocity and load information of the tail end of the mechanical arm; the virtual constraints comprise relative positions, relative postures, synchronous speeds, load balancing and power optimization constraints and are used for limiting kinematics and dynamics behaviors of the multiple mechanical arms; on the basis of the initial state and the virtual constraint, load distribution is optimized by utilizing a genetic algorithm, and a smooth trajectory meeting the collaborative task requirement is generated by adopting a quintic polynomial interpolation method; and the pose deviation of the multiple mechanical arms is corrected based on a virtual spring damper by combining trajectory tracking control and collaborative error compensation. According to the method, the movement efficiency and the task completion quality of the multi-mechanical-arm cooperative task are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotic arm control, and particularly relates to a multi-robotic arm collaborative control method. Background Art

[0002] With the improvement of industrial automation level, the application of multi-robotic arm collaborative operation in fields such as manufacturing, assembly, and handling is becoming increasingly widespread. A multi-robotic arm cooperation system can complete complex tasks that are difficult to achieve by a single robotic arm through the coordinated movement of multiple robotic arms. It has broad application potential especially in scenarios such as manufacturing, assembly, and handling. Through the collaborative cooperation of multiple robotic arms, production efficiency can be improved, the load pressure on a single arm can be reduced, and at the same time, refined and flexible operations can be achieved to meet the requirements of modern industry for high precision and high efficiency. The core of multi-robotic arm cooperation lies in how to effectively plan the movement trajectories of each robotic arm and achieve reasonable task decomposition and load balancing. Existing multi-robotic arm control technology solutions face many challenges in practical applications, such as:

[0003] (1) Usually, a simple master-slave control method is adopted, lacking a systematic modeling of the interaction relationship between robotic arms, insufficient consideration of the dynamic coupling and environmental constraints between robotic arms, and it is difficult to ensure the collaborative control accuracy and stability under complex working conditions;

[0004] (2) In terms of task planning, existing methods often adopt fixed task allocation strategies, failing to fully consider the load balancing problem, resulting in low system operation efficiency;

[0005] (3) In terms of trajectory planning, traditional methods mostly focus on the kinematic planning of a single robotic arm, insufficient consideration of the synchronization and coordination between multiple robotic arms, and it is easy to cause the failure of collaborative operation.

[0006] Therefore, we need to develop a multi-robotic arm collaborative control method that can improve the multi-robotic arm collaborative control efficiency through accurate modeling, load optimization, and smooth trajectory planning. Summary of the Invention

[0007] The purpose of the present invention is to provide a multi-robotic arm collaborative control method to solve the problems of the existing multi-robotic arm collaborative control scheme lacking accurate modeling, load optimization, and collaborative synchronization, resulting in low efficiency, as mentioned in the above background art.

[0008] To achieve the above purpose, the present invention provides a multi-robotic arm collaborative control method, and the method is specifically as follows:

[0009] Obtain the initial state of the end effector of the robotic arm, including position information, attitude information, linear velocity, angular velocity, and load information;

[0010] Define the virtual constraints of the robotic arms, including relative position constraints, relative attitude constraints, synchronous speed constraints, load balancing constraints, and power optimization constraints, which are used to constrain the geometric relationship and dynamic behavior between multiple robotic arms;

[0011] Define the multi-robotic-arm collaborative task as a seven-tuple. Based on the seven-tuple, decompose the multi-robotic-arm collaborative task into subtasks, and perform load distribution optimization through a genetic algorithm. Combine the initial state and the virtual constraints, and assign the subtasks to each robotic arm with the goal of load balancing;

[0012] The load distribution optimization through the genetic algorithm is to regard each load element to be distributed as a gene locus, encode an allocation scheme as a chromosome, and use the negative exponential function of the moment imbalance degree generated by the external forces assigned to different robotic arms as the fitness function to evaluate and select the optimal load allocation scheme to achieve load balancing;

[0013] Generate the multi-robotic-arm collaborative trajectory based on the subtasks, plan the collaborative trajectory by using the fifth-order polynomial interpolation method, use the initial state as the initial boundary condition, and ensure the synchronism and consistency of the collaborative trajectory through the virtual constraints;

[0014] Through closed-loop control, calculate the trajectory tracking control instruction, and adopt the combination of motion error compensation and collaborative error compensation to achieve the precise collaborative motion of multiple robotic arms. Among them, the collaborative error compensation is realized through a virtual spring damper, and its collaborative compensation torque calculation formula is:

[0015]

[0016] Among them, and are respectively the pose and velocity of the end effector of the th robotic arm; is the Jacobian matrix of the th robotic arm; and are the stiffness and damping coefficients.

[0017] Based on the foregoing scheme, the decomposition into subtasks specifically includes:

[0018] Based on the spatial distribution of the task object, define the execution area of the multi-robotic-arm collaborative task, and define the total working space of the multi-robotic arms as , including the free space and the obstacle space in two parts, and the expression is: ;

[0019] Based on the single-arm workspace of each robotic arm, taking the virtual constraint as an optimization condition, the relative position constraint and relative attitude constraint are used to determine the cooperative operation area between robotic arms, avoid motion conflicts, and partition the free space to obtain a set of feasible partitions .

[0020] Based on the foregoing solution, the load distribution optimization by genetic algorithm is to perform load balancing distribution of the multi-robotic-arm cooperative task in the set of feasible partitions , including:

[0021] For a dual-robotic-arm cooperative task, assume the positions of the ends of the two robotic arms are respectively and , and the force distribution of the task object is discretized into a load set , where is the th external force acting on the task object, is its moment arm, and the force balance condition for multi-robotic-arm cooperative operation is expressed as: ;

[0022] Define the input and output of the load distribution optimization problem as the robotic arm configuration set and the load set respectively; among them, the robotic arm configuration is represented by the end position and the attitude matrix of the robotic arm. For a dual-robotic-arm system, the robotic arm configuration set can be expressed as ; the load set contains several load elements, and each load element consists of the acting point position and the load magnitude . Let and represent the load subsets assigned to the two robotic arms, satisfying .

[0023] Based on the foregoing solution, the evolution process of the genetic algorithm includes: using a selection operator with an elitist retention strategy to copy the individuals with the highest fitness in the previous generation to the next generation; using a crossover operator with single-point crossover to randomly exchange the gene segments after the crossover point of two parent individuals; and using a mutation operator to randomly change a certain gene value in an individual with a preset probability

[0024] Based on the foregoing solution, evaluate the pros and cons of the load distribution scheme represented by each chromosome in the genetic algorithm through a fitness function, obtain and output the optimal load distribution scheme. If there are only two robotic arms, the fitness function is:

[0025]

[0026]

[0027] Among them, represents the moment imbalance caused by the external forces applied to the two robotic arms; and are the positions of the ends of the two robotic arms respectively and the mean values of which are used to represent the moment centers of the respective robotic arms; represents the 2-norm of the vector; the fitness is the negative exponential function of, when is smaller, that is, the moment is more balanced, the higher the fitness.

[0028] Based on the foregoing scheme, the quintic polynomial interpolation method is specifically as follows:

[0029] A smooth and continuous trajectory is fitted between the starting position, the ending position, and the intermediate key point positions of each robotic arm; let the th robotic arm's joint space variable be , where is the joint degree of freedom, given the time interval , then the trajectory of the th robotic arm is represented by a quintic polynomial as:

[0030]

[0031] To determine the polynomial coefficients , the starting position and its velocity and acceleration and the ending position and its velocity and acceleration need to be given, a total of 6 boundary conditions.

[0032] Based on the foregoing scheme, the closed-loop control decomposes the controller for the end effector of the robotic arm into two parts: motion error compensation and cooperative error compensation, and the expression is: , where, is the control moment applied to the th robotic arm joint, is the trajectory tracking control moment, is the cooperative compensation moment;

[0033] The motion error compensation is used to improve the accuracy of a single robotic arm, while the cooperative error compensation is used to ensure the coordination and accuracy of multiple robotic arms in cooperative tasks.

[0034] Based on the foregoing scheme, the trajectory tracking control instruction is calculated using the computed torque method, and the control law is the trajectory tracking control moment , which is used to compensate for the error of a single robotic arm deviating from the desired trajectory, and the expression is:

[0035]

[0036] Wherein, are respectively the angle, angular velocity, and angular acceleration of the th robotic arm joint, is the inertia matrix of the robotic arm, is the Coriolis force and centripetal force term, is the gravity term, is the frictional torque, and are the proportional gain matrix and differential gain matrix of the controller.

[0037] Based on the foregoing solution, the closed-loop control is implemented in a control cycle, and the control cycle includes: reading the encoder to obtain the angle and speed of the current robotic arm joint; exchanging the pose and speed of the end of each robotic arm through the communication network; and combining the desired joint angle and speed obtained by looking up the table from the reference trajectory to calculate the trajectory tracking control torque and the cooperative compensation torque.

[0038] Based on the foregoing solution, the seven-tuple is: , Wherein, represents the task object, including the workpiece to be operated and the target pose; represents the operation environment information, including the obstacle distribution information for defining the obstacle space and the environmental constraints for determining the safety boundary ; defines the type of action that the robotic arm should execute; contains the parameters required for executing the action for planning the motion path; describes the division of labor mode of the task among multiple robotic arms; gives the constraint conditions for task completion; defines the evaluation index for the quality of the multi-robotic arm cooperative task completion, which is used to evaluate the control effect in real time and serve as the basis for adjusting the controller parameters.

[0039] The present invention has the following advantages and effects compared with the prior art:

[0040] (1) The virtual structure method is adopted to uniformly model the motion of multiple robotic arms. Through geometric and mechanical constraints, the cooperative motion control between the robotic arms is realized, improving the accuracy of system modeling. A dual control structure including trajectory tracking control and cooperative compensation control is designed, effectively improving the control accuracy and robustness of the system.

[0041] (2) In terms of task allocation, a load balancing optimization method based on the genetic algorithm is proposed. By constructing a reasonable fitness function, the optimal load allocation among multiple robotic arms is achieved, improving the system operation efficiency.

[0042] (3) In terms of trajectory planning, the quintic polynomial interpolation method is used to generate a smooth and continuous cooperative trajectory for multiple robotic arms, ensuring the harmony and continuity of the motion of multiple robotic arms. Description of the Drawings

[0043] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following will briefly introduce the drawings required for the description of the specific embodiments or the prior art. In all the drawings, similar elements or parts are generally identified by similar reference numerals. In the drawings, the elements or parts are not necessarily drawn to scale.

[0044] Figure 1 It is a flowchart of a cooperative control method for multiple robotic arms provided by an embodiment of the present invention. Detailed Embodiments

[0045] To more clearly explain the purpose, technical solutions and advantages of the present invention, the following will, in combination with the drawings in the embodiments of the present invention, clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. The exemplary embodiments can be implemented in various forms and should not be construed as limited to the examples described herein; on the contrary, providing these embodiments makes the present invention more comprehensive and complete, and conveys the concept of the exemplary embodiments to those skilled in the art in an all-round way.

[0046] In addition, the described features, structures or characteristics can be combined in any suitable way in one or more embodiments. In the following description, many specific details are provided to give a full understanding of the embodiments of the present invention. However, those skilled in the art will realize that the technical solutions of the present invention can be practiced without one or more of the specific details, or other methods, components, devices, steps, etc. can be adopted. In other cases, well-known methods, devices, implementations or operations are not shown or described in detail to avoid obscuring various aspects of the present invention.

[0047] The block diagrams shown in the drawings are merely functional entities and do not necessarily correspond to physically independent entities. That is, these functional entities can be implemented in software form, or implemented in one or more hardware modules or integrated circuits, or implemented in different networks and / or processor devices and / or microcontroller devices.

[0048] The flowcharts shown in the drawings are only illustrative and do not necessarily include all the content and operations / steps, nor are they necessarily executed in the described order. For example, some operations / steps can be decomposed, while some operations / steps can be combined or partially combined, so the actual execution order may change according to the actual situation.

[0049] The present invention will be described in detail below in conjunction with specific embodiments:

[0050] As shown in the attached Figure 1 drawings, an embodiment of the present invention provides a multi-robot-arm cooperative control method, and the specific steps of this method are as follows:

[0051] Step S1: Perform mathematical modeling and virtual constraint modeling on the initial state of the robot arm;

[0052] Step S2: Construct an overall description of the multi-robot-arm cooperative task, and based on the overall description, decompose the multi-robot-arm cooperative task into sub-tasks of each robot arm and perform load distribution;

[0053] Step S3: Generate a multi-robot-arm cooperative trajectory based on the sub-tasks of each robot arm;

[0054] Step S4: After the synchronous trajectory, through closed-loop control, calculate the trajectory tracking control instruction of the multi-robot-arm cooperative task, and realize the cooperative movement of the multi-robot-arm to complete the multi-robot-arm cooperative task.

[0055] Preferably, in step S1, the mathematical modeling of the initial state of the robot arm is specifically:

[0056] Obtain the real-time state information of the end effector of the robot arm, including: the position information of the end of the robot arm , the attitude information , the linear velocity and the angular velocity , as well as the load information , define the initial state of the th robot arm as a five-tuple, as follows:

[0057] , where the position of the end of the robot arm is described by a three-dimensional vector in the Cartesian coordinate system as: , where is the coordinate of the end effector of the robotic arm in the Cartesian coordinate system; the attitude of the end effector of the robotic arm is represented by Euler angles: , are respectively the rotation angles of the end effector of the robotic arm around the axis; the linear velocity of the end effector of the robotic arm is represented by a three-dimensional vector as: , , where are the linear velocity components of the end effector of the robotic arm in the Cartesian coordinate system; the angular velocity of the end effector of the robotic arm is represented by a three-dimensional vector as: , where are respectively the angular velocities of the end effector of the robotic arm around the axis; the load at the end effector of the robotic arm is represented by a six-dimensional vector of the resultant force and the resultant moment: , , where are the three force components of the force applied to the end effector of the robotic arm, are the moment components of the moment applied to the end effector of the robotic arm.

[0058] It should be noted that coordinating and controlling the motion of multiple robotic arms requires applying certain kinematic constraints to the robotic arms. In this embodiment, the virtual structure method is used to describe and control the motion of multiple robotic arms;

[0059] Preferably, the virtual constraint modeling in step S1 includes:

[0060] Define the virtual connection between the robotic arms as a set of geometric constraints and mechanical constraints. The geometric constraints include relative position constraints and relative attitude constraints, which are used to maintain the rigid body motion relationship between the end effectors of the robotic arms;

[0061] The relative position constraint can be expressed as: , where and are respectively the position vectors of the ends of robotic arm and robotic arm , represents the two-norm, is the expected distance between the ends of robotic arm and robotic arm ;

[0062] The relative attitude constraint can be expressed as: , where and are the attitude matrices of the ends of robotic arm and robotic arm , is the logarithmic mapping of the rotation matrix, is the robotic arm and robotic arm Tolerance value of the attitude deviation at the end;

[0063] The mechanical constraints are specifically:

[0064] Apply synchronous speed constraints to the cooperative motion and force state of the robotic arms: , where and are the velocity vectors at the ends of robotic arms and robotic arm respectively, is the upper limit of the speed deviation, and the synchronous speed constraint is used to ensure the consistency of the motion of each robotic arm;

[0065] Load balance constraint: , where and are the resultant forces acting on the ends of robotic arms and robotic arm respectively, is the upper limit of the load deviation, and the load balance constraint is used to achieve the balanced distribution of the loads of multiple robotic arms;

[0066] Power optimization constraint: , where is the dissipated power of robotic arm , is the resultant force exerted by multiple robotic arms on the target object, and the power optimization constraint is used to optimize the power consumption distribution of multiple robotic arms on the premise of meeting the task load.

[0067] Preferably, the overall description in step S2 is based on typical applications including industrial assembly, handling, and machining, and models the cooperative task of the multiple robotic arms as a seven-tuple as follows:

[0068]

[0069] Where represents the task object, including the workpiece to be operated and the target pose; represents the job environment information, including the obstacle distribution information for defining the obstacle space , the environmental constraints for determining the safety boundary , etc.; defines the type of action that the robotic arm should execute; contains the parameters required for executing the action, such as the position information of the grasping point for determining the load application point, the assembly axis direction for planning the motion path, etc.; describes the division of labor mode of the task among multiple arms; gives the constraint conditions for task completion, such as the accuracy requirement for determining the tolerance of moment balance, the time limit for determining the search range of the optimal solution, etc.; Define the evaluation index for the completion quality of the multi-robot arm collaborative task, which is used to evaluate the control effect in real time and serve as the basis for adjusting the controller parameters;

[0070] Based on the above seven-tuple model, the method of this embodiment will be implemented in sequence: divide the total working space according to the job environment information defined by Env, perform load optimization based on Obj and Param, generate a multi-robot arm collaborative trajectory that meets the constraints by combining Action and Cons, implement the multi-robot arm collaborative task through closed-loop control, and use Eval for evaluation.

[0071] It should be noted that the execution process of the multi-robot arm collaborative task is transformed into a process of gradually instantiating and conditionally optimizing the seven-tuple; among them, the decomposition and allocation of the multi-robot arm collaborative task are necessary intermediate steps for mapping the overall task description to the control instructions of each robot arm, and have a significant impact on the system performance.

[0072] Preferably, in step S2, the decomposition into subtasks for each robot arm and the load distribution are performed through a hierarchical multi-arm task decomposition and allocation method, which partitions the multi-robot arm collaborative task into subtasks within the working space and allocates the subtasks to each robot arm with the goal of load balancing, including the following:

[0073] It should be noted that the collaborative operation of multiple robot arms can describe the kinematic characteristics of the robot arms in various ways such as Cartesian space, joint space, and working space;

[0074] Step S202: Define the execution area of the multi-robot arm collaborative task based on the spatial distribution of the task objects;

[0075] Exemplarily, define the total working space of the multi-robot arm as , including the free space and the obstacle space in two parts, and the expression is: ; the obstacle space contains all the unreachable areas in the total working space , and its boundary is determined by the geometric representation of the obstacles in the job environment information Env; to avoid the risk of collision, a certain safety margin is set outside the boundary to form the safety boundary of the free space , and all reachable configurations of the robot arm need to be limited within , and the expression is: .

[0076] Step S203: Based on the single-arm workspace of each robotic arm, using the virtual constraint as an optimization condition, where the geometric constraint is used to determine the collaborative operation area between robotic arms and avoid motion conflicts, divide the free space ;

[0077] Exemplarily, let the kinematic model of the th robotic arm be , and its single-arm workspace be . Then a feasible division of the total workspace of the multi-robotic arms is: , that is, the working areas of each robotic arm do not overlap with each other, and their union covers the free space . Each division unit corresponds to a potential control partition and task allocation scheme. That is, each division of the total workspace W will generate: a control partition (i.e., the spatial range that the robotic arm is responsible for controlling) and a task allocation scheme (i.e., how the tasks within this control partition are assigned to the robotic arms), corresponding to Alloc in the seven-tuple (describing how tasks are divided among multiple arms). To balance the flexibility and motion efficiency of the multi-robotic arm collaborative task, select a set of feasible partitions with appropriate division granularity and satisfying the task completion constraint condition Cons .

[0078] Step S204: In the set of feasible partitions , based on the initial state of the robotic arm (such as position information, attitude information, linear velocity, angular velocity, load information), evaluate the current motion ability and load capacity of the robotic arm, and perform balanced allocation of the multi-robotic arm collaborative task load;

[0079] Exemplarily, taking the collaborative task of a dual-robotic arm as an example, let the positions of the ends of the two robotic arms be and respectively, and the force distribution of the task object such as assembly and handling is discretized into a load set , where is the th external force acting on the task object, is its moment arm, and the force balance condition for the collaborative operation of the multi-robotic arm is expressed as: , that is, the moments of the ends of the two robotic arms with respect to each load are balanced, which is used as the load balance target. Considering the random distribution of the loads to be operated, a genetic algorithm is used to solve the optimization problem of the load balance target, simulating the process of natural selection, and finding the optimal solution to the problem by simulating the "evolution" process, and using the virtual constraint as an optimization condition, where the mechanical constraint is used to ensure the balanced distribution of the load. The following are the specific steps of the optimization solution:

[0080] Step S2041: Define the input and output of the optimization problem, which are the robotic arm configuration set and the load set respectively. Among them, the robotic arm configuration is represented by the end position and the attitude matrix of the robotic arm. For a dual-robotic arm system, the robotic arm configuration set can be expressed as ; the load set contains several load elements, and each load element consists of the acting point position and the load magnitude . Let and represent the load subsets assigned to the two robotic arms, satisfying ; model the optimization problem, regard each load element as a gene locus, a gene value of 1 means allocating this load to robotic arm 1, and a gene value of 0 means allocating it to robotic arm 2. A load allocation scheme corresponds to a chromosome, which consists of gene loci, where is the total number of loads; the population consists of chromosomes, which is represented by a two-dimensional array ;

[0081] Step S2042: Evaluate the quality of each load allocation scheme represented by a chromosome through a fitness function, and obtain and output the optimal load allocation scheme. The fitness function is as follows:

[0082]

[0083]

[0084] where represents the torque imbalance degree generated by the external forces allocated to the two robotic arms; and are the means of the positions and of the ends of the two robotic arms respectively, which are used to represent the torque centers of the respective robotic arms; represents the 2-norm of the vector; the fitness is the negative exponential function of . When is smaller, that is, the torque is more balanced, the fitness is higher; the fitness evaluation can be performed at the beginning of each generation and executed again after selection, crossover, and mutation to ensure the improvement of each generation;

[0085] Specifically, the selection operator adopts the elitist retention strategy, that is, in each iteration, the individuals with the highest fitness in the previous generation are copied to the next generation unchanged;

[0086] Specifically, the crossover operator adopts single-point crossover; in each iteration, the individuals in the population are first sorted according to fitness, and then two individuals are randomly selected as the parents, and with a probability they are crossed; during the crossover, a gene position is randomly selected as the crossover point, and the gene segments after the crossover point of the two parents are exchanged to form two new individuals;

[0087] Specifically, the mutation operator mutates each individual in the population with a preset probability The specific operation is to randomly select a gene position and change its gene value from 0 to 1 or from 1 to 0;

[0088] Specifically, after the above genetic evolution, the load distribution scheme corresponding to the individual with the highest fitness in the population is the optimal load distribution scheme, and the best partition scheme considering environmental constraints and load balancing is obtained, realizing the balanced distribution of the load between the manipulator 1 and the manipulator 2. The environmental constraints include the total working space W and the obstacle space , and this scheme distributes the load in to the manipulator 1, and distributes the load in to the manipulator 2.

[0089] Preferably, in step S3, the generation of the multi-manipulator cooperative trajectory is based on the initial state of the manipulator under the given target configuration of the multi-manipulator cooperative task and the environmental constraints. During the trajectory planning process, the joint angles, linear velocities, and angular velocities of the initial state are used as the initial boundary conditions for quintic polynomial interpolation, and a feasible trajectory from the current configuration to the target configuration is planned for each manipulator. Specifically:

[0090] The quintic polynomial interpolation method is used to fit a smooth and continuous trajectory between the starting position, ending position, and intermediate key point positions of each manipulator; let the joint space variables of the -th manipulator be , where is the joint degree of freedom, and a given time interval is set. Then, the trajectory of the -th manipulator is represented by a quintic polynomial as:

[0091]

[0092] To determine the polynomial coefficients , the starting position and its velocity and acceleration , , and the ending position and its velocity and acceleration , , , a total of 6 boundary conditions. Substituting them into the fifth-degree polynomial gives:

[0093]

[0094]

[0095]

[0096]

[0097]

[0098]

[0099] After arranging into matrix form: , where is a 6×6 Vandermonde matrix, is a 6×1 vector determined by the boundary conditions, and the polynomial coefficients can be obtained by matrix inversion: , thereby determining the fifth-degree polynomial interpolation curve of the th robotic arm joint, that is, the trajectory of the th robotic arm; repeating the above process for the joint variables of all robotic arms, the complete cooperative trajectory of the multi-robotic arms can be obtained, and the trajectory is discretized into: form.

[0100] It should be noted that during the trajectory generation process, the virtual constraints are used to guide the calculation of the multi-robotic arm cooperative trajectory; for example, the relative position constraint and the relative attitude constraint are used to ensure that the trajectories of the multi-robotic arm ends are synchronized and consistent, and the constraint conditions of speed and load are used as boundary conditions in the fifth-degree polynomial interpolation to ensure that the generated trajectory meets the requirements of the multi-robotic arm cooperative task.

[0101] Preferably, in the closed-loop control in step S4, the controller for the robotic arm end effector is decomposed into two parts: motion error compensation and cooperative error compensation, and the expression is: , where is the control torque applied to the th robotic arm joint, is the trajectory tracking control torque, is the cooperative compensation torque;

[0102] It should be noted that motion error compensation focuses on improving the accuracy of a single robotic arm, while cooperative error compensation ensures the coordination and accuracy of multi-robotic arms in cooperative tasks. The combination of the two can effectively improve the overall performance of the multi-robotic arm system.

[0103] Preferably, the trajectory tracking control instruction in step S4 is calculated by the computed torque method, and the control law is the trajectory tracking control torque , which is used to compensate for the error of a single robotic arm deviating from the desired trajectory, and the expression is:

[0104]

[0105] where are respectively the angle, angular velocity, and angular acceleration of the th robotic arm joint, is the inertia matrix of the robotic arm, is the Coriolis force and centripetal force term, is the gravity term, is the frictional torque, and are the proportional gain matrix and differential gain matrix of the controller;

[0106] Specifically, due to the modeling error of the initial state of the robotic arm and the coupling between the robotic arm joints, an additional compensation control is introduced to the control law, and the obtained control law is the collaborative compensation torque , which is:

[0107]

[0108] where and are respectively the pose and velocity of the end effector of the th robotic arm; is the Jacobian matrix of the th robotic arm; and are the stiffness and damping coefficients. By introducing a virtual spring-damper between the robotic arms, the pose deviation is introduced into the control law of the robotic arm movement to compensate for the position and velocity deviation between multiple robotic arms, ensure the accuracy of collaborative movement, and then combine the trajectory control and collaborative control to obtain a complete multi-robotic arm control law as follows:

[0109] , where and are the desired joint angle and desired angular velocity corresponding to the reference trajectory.

[0110] Preferably, the realization of the collaborative movement of multiple robotic arms in step S4 is completed based on the above-mentioned complete multi-robotic arm control law, specifically including:

[0111] The system initializes by reading the robotic arm parameters and the communication address table, creates master and slave controller objects, sets the synchronization managers for the master and slave stations, and determines the process data object mapping, i.e., the data objects to be synchronized (such as position, speed, and torque).

[0112] Establish a listening connection to listen for status information from other robotic arms.

[0113] Switch to the operation mode to enable the drive and the controller.

[0114] Control loop: Read the encoder to obtain the angles and speeds of the current robotic arm joints, and exchange the poses and speeds of the ends of each robotic arm through the communication network.

[0115] Obtain the desired joint angles and speeds by looking up the reference trajectory table, calculate the cooperative compensation torque and calculate the trajectory tracking control torque using the inverse dynamics model, add and together, and after torque limiting, output to the actuator.

[0116] Record and analyze the errors during the robotic arm control process, including position tracking errors, communication timeouts, etc.

[0117] Finally, close the communication link, reset the motor enable, and exit the servo control.

[0118] In this embodiment, by defining the initial state of the robotic arm and using the virtual structure method to achieve the unified description and control of the multi-robotic arm movement, the reasonable allocation of subtasks of the multi-robotic arm collaborative task is realized using the load balancing optimization algorithm based on the genetic algorithm. The synchronization and continuity of the multi-robotic arm movement are ensured through collaborative trajectory planning, and the control accuracy and anti-interference ability of the system are improved through the dual control structure. The present invention can be widely applied to the collaborative operation control of industrial robots, providing effective technical support for improving the automation production efficiency.

[0119] Other embodiments of the present invention will be readily apparent to those skilled in the art upon consideration of the specification and practice of the invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present invention that follow the general principles of the present invention and include known common general knowledge or conventional technical means in the technical field not disclosed in the present invention. The specification and examples are only to be considered as exemplary, and the true scope and spirit of the present invention are pointed out by the claims. It should be understood that the present invention is not limited to the exact structures described above and shown in the drawings, and various modifications and changes can be made without departing from its scope. The scope of the present invention is only limited by the appended claims.

Claims

1. A multi-robot-arm collaborative control method, characterized in that Including: Obtain the initial state of the end effector of the robotic arm, including position information, attitude information, linear velocity, angular velocity, and load information; Define the virtual constraints of the robotic arm, including relative position constraints, relative attitude constraints, synchronous speed constraints, load balance constraints, and power optimization constraints, which are used to constrain the geometric relationship and dynamic behavior between multiple robotic arms; Define the multi-robotic-arm collaborative task as a seven-tuple. Based on the seven-tuple, decompose the multi-robotic-arm collaborative task into subtasks, and perform load distribution optimization through a genetic algorithm. Combine the initial state and the virtual constraints, and allocate the subtasks to each robotic arm with the goal of load balance; The load distribution optimization through the genetic algorithm is to regard each load element to be allocated as a gene locus, encode an allocation scheme as a chromosome, and use the negative exponential function of the torque imbalance degree generated by the external forces allocated to different robotic arms as the fitness function to evaluate and select the optimal load allocation scheme to achieve load balance; Generate a multi-robotic-arm collaborative trajectory based on the subtasks, plan the collaborative trajectory by using the fifth-order polynomial interpolation method, use the initial state as the initial boundary condition, and ensure the synchronism and consistency of the collaborative trajectory through the virtual constraints; Through closed-loop control, the trajectory tracking control command is calculated, and the precise cooperative motion of multiple robotic arms is achieved by combining motion error compensation and cooperative error compensation. Among them, the cooperative error compensation is realized through a virtual spring damper, and its cooperative compensation torque calculation formula is: , where and are the poses and velocities of the end effectors of the th robotic arm respectively; is the Jacobian matrix of the th robotic arm; and are the stiffness and damping coefficients.

2. The multi-robot-arm collaborative control method according to claim 1, characterized in that The decomposition into subtasks specifically includes: Based on the spatial distribution of the task objects, define the execution area of the multi-manipulator collaborative task, and define the total working space of the multi-manipulator as , including the free space and the obstacle space in two parts, and the expression is: ; Based on the single-arm workspace of each robotic arm, taking the virtual constraint as the optimization condition, the relative position constraint and the relative attitude constraint are used to determine the cooperative operation area between the robotic arms, avoid motion conflicts, and for the free space is divided to obtain a set of feasible partitions .

3. A multi-robot-arm collaborative control method according to claim 2, characterized in that The load distribution optimization through the genetic algorithm is performed in the feasible partition set to perform load balancing distribution of the collaborative tasks of the multi-manipulator arms, including: For a cooperative task of two robotic arms, let the positions of the end - effectors of the two robotic arms be respectively and , and the force distribution of the task object is discretized into a load set , where is the th external force acting on the task object, is its moment arm, and the force - balance condition for cooperative operation of multiple robotic arms is expressed as: ； Define the inputs and outputs of the load distribution optimization problem as the set of robotic arm configurations and the set of loads, respectively; among them, the robotic arm configuration is represented by the end position and the attitude matrix . For a dual-robotic arm system, the set of robotic arm configurations can be expressed as ; the set of loads contains several load elements, and each load element consists of the acting point position and the load magnitude . Let and represent the subsets of loads assigned to the two robotic arms, satisfying .

4. A multi-robot-arm cooperative control method according to claim 3, characterized in that, The evolutionary process of the genetic algorithm includes: using a selection operator with an elitist retention strategy to copy the individuals with the highest fitness in the previous generation to the next generation; using a crossover operator with single-point crossover to randomly exchange the gene segments after the crossover point of two parent individuals; and using a mutation operator to randomly change a certain gene value in an individual with a preset probability. individuals to the next generation; using a crossover operator with single-point crossover to randomly exchange the gene segments after the crossover point of two parent individuals; and using a mutation operator to randomly change a certain gene value in an individual with a preset probability.

5. A multi-robot-arm collaborative control method according to claim 4, wherein Evaluate the advantages and disadvantages of the load distribution scheme represented by each chromosome in the genetic algorithm through the fitness function, obtain and output the optimal load distribution scheme. If there are only two robotic arms, the fitness function is: , , where represents the moment imbalance generated by the external forces applied to the two robotic arms; and are the positions of the ends of the two robotic arms and respectively, and their mean values are used to represent the moment centers of the robotic arms; represents the 2-norm of the vector; the fitness is 's negative exponential function. When is smaller, that is, when the moment is more balanced, the fitness is higher.

6. A multi-robot-arm collaborative control method according to claim 1, characterized in that The fifth-order polynomial interpolation method is specifically: Fit a smooth and continuous trajectory between the starting position, ending position, and intermediate key point positions of each robotic arm; Let the joint space variables of the th robotic arm be , where is the joint degree of freedom, given the time interval , then the trajectory of the th robotic arm is expressed by a fifth-degree polynomial as: ； To determine the polynomial coefficients , it is necessary to specify the starting position, its velocity, and its acceleration , and the ending position, its velocity, and its acceleration , for a total of six boundary conditions 7. A multi-robot-arm collaborative control method according to claim 1, wherein The closed-loop control decomposes the controller for the end effector of the robotic arm into two parts: motion error compensation and collaborative error compensation, and the expression is: , where is the control torque applied to the th robotic arm joint, is the trajectory tracking control torque, is the collaborative compensation torque; The motion error compensation is used to improve the accuracy of a single robotic arm, while the collaborative error compensation is used to ensure the coordination and accuracy of multiple robotic arms in a collaborative task.

8. A multi-robot-arm collaborative control method according to claim 7, characterized in that The trajectory tracking control instruction is calculated by the computed torque method, and the control law is the trajectory tracking control torque , which is used to compensate for the error of a single robotic arm deviating from the desired trajectory. The expression is as follows: , where are the angle, angular velocity, and angular acceleration of the th robotic arm joint respectively, is the inertia matrix of the robotic arm, is the Coriolis force and centripetal force term, is the gravity term, is the frictional torque, and are the proportional gain matrix and derivative gain matrix of the controller.

9. A multi-robot-arm collaborative control method according to claim 8, wherein, The closed-loop control is implemented in a control cycle, which includes: reading the encoder to obtain the current angle of the robot arm joint and speed ; Exchange the position and posture of each robot end through the communication network and speed ; and combined with the desired joint angle obtained by looking up the reference trajectory table and speed , calculate the trajectory tracking control torque and the collaborative compensation torque.

10. A multi-robot-arm collaborative control method according to claim 1, characterized in that, The seven-tuple is as follows: , where represents the task object, including the workpiece to be operated and the target pose; represents the operation environment information, including the obstacle distribution information for defining the obstacle space and the environmental constraints for determining the safety boundary ; defines the type of action that the robotic arm should execute; contains the parameters required for executing the action and is used for planning the motion path; describes the division of labor mode of the task among multiple arms; gives the constraint conditions for task completion; defines the evaluation index for the completion quality of the multi-robotic-arm collaborative task, which is used to evaluate the control effect in real time and serves as the basis for adjusting the controller parameters.

Images