A distributed adaptive control method and system for multi-robot cooperation

Abstract

The present application belongs to the technical field of robot cooperative control, and particularly relates to a distributed adaptive control method and system for multi-robot cooperation, which comprises the following steps: decoupling a pure internal force vector from the data of a terminal six-dimensional force sensor by using pre-identified dynamic parameters, calculating a cooperative confrontation index according to the pure internal force vector and a relative position vector, calculating an adaptive internal force penalty weight by nonlinear mapping based on the cooperative confrontation index, establishing a state space equation of the system based on a double integral model, introducing an equivalent environment stiffness matrix, mapping a prediction error vector in a prediction time domain into a predicted internal force vector, constructing a cost function containing the product of the adaptive internal force penalty weight and the predicted internal force vector, obtaining an optimal control increment sequence by minimizing the cost function, and updating a speed command to drive the movement of the robot arm. The present application effectively improves the robustness and execution stability of the system, and ensures the safety of cooperation.

Description

Technical Field

[0001] This invention relates to the field of robot cooperative control technology. More specifically, this invention relates to a distributed adaptive control method and system for multi-robotic arm cooperation. Background Technology

[0002] In modern intelligent manufacturing production lines, the collaborative handling of heavy loads and rigid objects by multiple robotic arms has become a core process. In such systems, the controllers of each robotic arm maintain a high degree of synchronization in their motion states through communication to ensure the stability and safety of the task. Traditional centralized control architectures face computing power bottlenecks and single-point failure risks when dealing with large-scale collaborative nodes. Distributed model predictive control (DMPC) algorithms have become the main technical means to solve such multi-agent collaborative control problems because they can effectively handle multivariable constraints and perform online rolling optimization.

[0003] Despite the excellent performance of the DMPC algorithm in trajectory tracking, it still has significant shortcomings in real-world physical interaction scenarios: In terms of control performance trade-offs, multi-arm collaboration requires finding a balance between position tracking accuracy and compliance. Existing control methods often use fixed weighting coefficients to construct the cost function, meaning the weights of the position error term and the control increment term remain constant throughout the operation. However, fixed weighting coefficients make it difficult to find the optimal balance between position tracking accuracy and compliance: if high position weights are set to pursue high accuracy, the system stiffness becomes too large, generating enormous internal forces when motion errors occur, damaging the workpiece; if low weights are set to pursue compliance, the system stiffness is insufficient, making it difficult to guarantee trajectory accuracy during handling, leading to collaboration failure. This control strategy, lacking adaptive adjustment capabilities, struggles to adapt to dynamically changing collaborative environments and complex task requirements. Summary of the Invention

[0004] To address the technical problem of balancing positional accuracy and compliance in existing multi-manipulator control methods, the present invention provides solutions in the following aspects.

[0005] In a first aspect, the present invention provides a distributed adaptive control method for multi-manipulator collaboration, comprising: real-time acquisition of state measurement values ​​of the manipulators; decoupling pure internal force vectors from the end-effector six-dimensional force sensor data of the state measurement values ​​using pre-identified dynamic parameters; calculating a collaboration antagonism index based on the pure internal force vector and the relative position vector, and calculating an adaptive internal force penalty weight based on the collaboration antagonism index through nonlinear mapping; establishing the state-space equation of the system based on a double integral model, and calculating the prediction error vector for future moments in the prediction time domain; introducing an equivalent environmental stiffness matrix characterizing the rigidity of the workpiece and fixture, and mapping the prediction error vector in the prediction time domain to a predicted internal force vector; constructing a cost function containing the product of the adaptive internal force penalty weight and the predicted internal force vector, obtaining the optimal control increment sequence by minimizing the cost function, and updating the speed command to drive the manipulator motion.

[0006] Preferably, when the pure internal force vector is decoupled from the end-effector six-dimensional force sensor data of the state measurement value using pre-identified dynamic parameters, the decoupled pure internal force vector is equal to the end-effector six-dimensional force sensor data minus the resultant dynamic force required to maintain the current motion. The resultant dynamic force is calculated from the pre-identified dynamic parameters and is equal to the inertial force term characterized by the product of the equivalent inertia of the robotic arm in Cartesian space and the Cartesian acceleration of the robotic arm, the nonlinear force term characterized by the product of the Coriolis force and the centrifugal force of the robotic arm in Cartesian space and the Cartesian velocity of the robotic arm, and the sum of the gravity compensation term of the robotic arm in Cartesian space.

[0007] Preferably, the cooperative resistance index is obtained by multiplying the projection term and the activation term; wherein, the projection term is calculated by dividing the absolute value of the dot product of the decoupled pure internal force vector and the relative position vector between the local robot and the neighboring robot obtained by communication by the magnitude of the relative position vector; the activation term is constructed based on the Sigmoid function, and its independent variable is the difference between the magnitude of the decoupled pure internal force vector and the safe internal force dead zone threshold.

[0008] Preferably, the adaptive internal force penalty weight is equal to the base weight plus a dynamic increment; the dynamic increment is obtained by multiplying the difference between the saturation weight and the base weight by a function term about the cooperative resistance index; the function term is the ratio of the numerator to the denominator, which consists of a natural exponential function term containing the product of the cooperative resistance index and the adjustment sensitivity coefficient.

[0009] Preferably, the prediction error vector for the future time step is equal to the sum of the system's autonomous evolution part and the control input influence part; the system's autonomous evolution part is obtained by multiplying the state transition matrix by the error vector containing position and velocity errors at the current time step; the control input influence part is obtained by multiplying the input control matrix by the control increment to be solved.

[0010] Preferably, the equivalent environmental stiffness matrix is ​​set by experimental determination, including: during the system initialization phase, controlling the robotic arm to hold the workpiece in a stationary state, and controlling one arm to generate a small and known positional displacement in each axis direction of Cartesian space. Record the change in force sensor readings According to the formula Calculate the stiffness values ​​of each axis and construct a diagonal matrix as the preset equivalent environmental stiffness matrix.

[0011] Preferably, the predicted internal force vector is equal to the preset equivalent environmental stiffness matrix multiplied by the equivalent position deviation at that moment; the equivalent position deviation is calculated by extracting the position error component from the prediction error vector at that future moment using the output selection matrix, and then subtracting the predicted value of the relative position deviation at that future moment.

[0012] Preferably, the predicted relative position deviation at the future time is equal to the difference between the theoretical expected position of the neighboring robot arm at the future time and the theoretical expected position of the robot arm at the future time.

[0013] Preferably, the cost function is the sum of the squares of three weighted norms over the prediction time domain; the first term is the square of the weighted Euclidean norm of the prediction error vector and the state weight matrix; the second term is the product of the square of the Euclidean norm of the predicted internal force vector and the adaptive internal force penalty weight at the current time; and the third term is the square of the weighted Euclidean norm of the control increment to be solved and the control increment weight matrix.

[0014] Secondly, the present invention provides a distributed adaptive control system for multi-robotic arm collaboration, including a processor and a memory, wherein the memory stores computer program instructions, and when the computer program instructions are executed by the processor, the aforementioned distributed adaptive control method for multi-robotic arm collaboration is implemented.

[0015] By adopting the above technical solution, a computer program for a distributed adaptive control method for multi-robotic arm collaboration is generated and stored in a memory for loading and execution by a processor. This allows for the creation of a terminal device based on the memory and processor, facilitating its use.

[0016] The beneficial effects of this invention are as follows:

[0017] (1) By combining the dynamic model of the robotic arm, the present invention removes the inertial force, Coriolis force, centrifugal force and gravity components from the total force collected by the six-dimensional force sensor, and decouples the pure internal force vector caused only by asynchronous cooperation or external constraints. This effectively solves the problem that when the traditional method directly uses sensor data, it is easy to misjudge the inertial force generated by the load gravity or high-speed motion as the system's internal force of resistance, and improves the accuracy of the system's perception of the real cooperative state.

[0018] (2) The present invention constructs a cooperative resistance index that integrates the direction and amplitude of internal force, and dynamically calculates the adaptive internal force penalty weight through nonlinear mapping. When the cooperation is smooth, the weight is low and the system maintains high stiffness to ensure high-precision trajectory tracking. When cooperative resistance is detected, such as mutual pulling or squeezing, the weight increases rapidly and the system automatically switches to compliant mode to release internal force. This mechanism solves the contradiction between position accuracy and compliance caused by the use of fixed weights in the traditional DMPC algorithm.

[0019] (3) The present invention introduces an equivalent environmental stiffness matrix into the prediction model of the DMPC algorithm and establishes a mapping relationship from the predicted position error to the predicted internal force. This enables the controller to not only adjust the current internal force, but also to predict the magnitude of the internal force that may be generated by the future trajectory during the rolling optimization process. Thus, it can actively avoid the motion trend that generates large internal forces during the control quantity solution stage, which significantly improves the safety of multi-arm collaboration.

[0020] (4) By simultaneously constraining position error, predicting internal force and control increment in the cost function and solving it using a distributed architecture, this invention can effectively address the asynchronous problem caused by communication delay or external disturbance, effectively improve the robustness and execution stability of the system, ensure the smooth operation of each robotic arm actuator, prevent workpiece damage or motor overload caused by sudden changes in internal force, and ensure collaborative safety. Attached Figure Description

[0021] Figure 1 This is a flowchart illustrating a distributed adaptive control method for multi-robotic arm collaboration in this invention.

[0022] Figure 2 This is a schematic diagram illustrating pure internal force monitoring based on dynamic decoupling;

[0023] Figure 3 This is a schematic diagram illustrating the adaptive internal force penalty weights;

[0024] Figure 4 This is a schematic diagram illustrating the internal force penalty term in the DMPC cost function. Detailed Implementation

[0025] 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, not all, of the embodiments of the present invention. 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.

[0026] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0027] This invention discloses a distributed adaptive control method for multi-robotic arm collaboration, referring to... Figure 1 This includes steps S1-S4:

[0028] S1: Real-time acquisition of the robotic arm's state measurement values, and decoupling of the pure internal force vector from the end-effector six-dimensional force sensor data of the state measurement values ​​using pre-identified dynamic parameters.

[0029] It should be noted that since the six-dimensional force sensor at the end of the robotic arm measures the resultant force on the end, which includes the gravity of the load itself, the inertial force generated by the motion, and the internal force caused by the lack of coordination, directly using the sensor data will misjudge the normal load gravity and acceleration process as system resistance. Therefore, this embodiment uses a dynamic model to extract the pure internal force component caused only by the lack of coordination from the mixed sensor data.

[0030] Specifically, each robotic arm controller acquires status measurements in real time at high frequency, including joint positions. Joint velocity and end-effector six-dimensional force sensor data The joint positions are determined using the Jacobian matrix of the robotic arm. and joint velocity Mapping to the Cartesian plane, we obtain the Cartesian acceleration of the robotic arm. With Descartes speed Low-pass filtering is applied to the data from the end-effector six-dimensional force sensor.

[0031] The Jacobian matrix of the robotic arm is a transformation matrix that maps joint velocities to end-effector Cartesian velocities. It is determined by the geometric configuration (DH) parameters of the robotic arm and can be calculated directly by calling the robotic arm control library.

[0032] Furthermore, the inertial matrix, Coriolis force and centrifugal force matrix and gravity vector of the joint space are obtained through offline identification experiments, and the Jacobian matrix is ​​used to convert them into Cartesian space parameters as pre-identified robotic arm dynamic parameters, which are then stored inside the controller.

[0033] To distinguish between load gravity and cooperative internal forces, a built-in robotic arm dynamics model is used for decoupling. Specifically, the pre-identified robotic arm dynamics parameters stored inside the controller are used, combined with the real-time motion state, to estimate the theoretical dynamic components required to maintain the current motion. The decoupled pure internal force vector is then extracted from the sensor data. The specific calculation formula is as follows:

[0034] ;

[0035] In the formula, for The pure internal force vector after decoupling at any time is used to characterize the additional stress caused by asynchronous cooperation or external constraints. for The end-time six-dimensional force sensor data, i.e., the measured total resultant force; , , These are the inertia matrix, Coriolis force and centrifugal force matrix, and gravity vector in the Cartesian space parameters, respectively. , These represent the joint positions and joint velocities of the robotic arm, respectively; Let be the equivalent inertia of the robotic arm in Cartesian space. The Coriolis force and centrifugal force of the robotic arm in Cartesian space. Used to compensate for nonlinear forces under high-speed motion. This is the gravity compensation term for the robotic arm in Cartesian space, representing the gravitational component of the robotic arm itself; , These are the Cartesian acceleration and Cartesian velocity of the robotic arm, respectively.

[0036] The calculation formula separates the internal force components caused purely by external constraints or asynchronous cooperation by subtracting the total resultant force measured by the sensors from the resultant force of dynamics required to maintain the current trajectory, including inertial forces, Coriolis forces, centrifugal forces, and gravity. When the vector is close to zero, it indicates that the current force is reasonable. A large modulus indicates the presence of abnormal internal forces.

[0037] For example, Figure 2 This is a schematic diagram of pure internal force monitoring based on dynamic decoupling. The blue curve represents the amplitude of the pure internal force successfully extracted from the raw sensor data. In the normal cooperative range of 0-4s and 7-10s, the internal force remains at a low level, below the safe internal force dead zone threshold represented by the red dashed line. In the yellow interval of 4-7s, due to the resistance caused by the deviation of the robotic arm trajectory, the decoupled internal force increases significantly, far exceeding the safety threshold. Therefore, step S1 can effectively filter out the dynamic interference generated by the robotic arm's own movement and accurately identify the abnormal force conditions caused by the asynchronous collaboration.

[0038] It should be noted that this invention, by combining the dynamic model of the robotic arm, separates the inertial force, Coriolis force, centrifugal force, and gravitational components from the total resultant force collected by the six-dimensional force sensor, decoupling the pure internal force vector caused only by asynchronous cooperation or external constraints. This effectively solves the problem that traditional methods, when directly using sensor data, easily misjudge the inertial force generated by the load gravity or high-speed motion as the system's internal resistance force, thus improving the accuracy of the system's perception of the true cooperative state.

[0039] S2: Calculate the cooperative resistance index based on the pure internal force vector and the relative position vector, and calculate the adaptive internal force penalty weight based on the cooperative resistance index through nonlinear mapping.

[0040] It should be noted that a large internal force amplitude alone does not necessarily indicate cooperative resistance; it may be due to normal squeezing operations. Therefore, the system state cannot be accurately judged based solely on the magnitude of the force. Thus, this embodiment constructs a cooperative resistance index that integrates the internal force direction characteristics and amplitude characteristics, and dynamically adjusts the control weights accordingly.

[0041] Specifically, based on the relative position vectors of the local robot and the neighboring robot arm, and the decoupled pure internal force vectors, the cooperative resistance index is calculated. The specific calculation formula is as follows:

[0042] ;

[0043] In the formula, for The index of collaboration and confrontation at any given moment; for The pure internal force vector after decoupling at any given moment; for The relative position vector between the local machine and the neighboring robotic arm obtained through communication at any given time; Indicates the modulus length; The magnitude of the relative position vector; The magnitude of the decoupled pure internal force vector; Represents the dot product of vectors; Indicates taking the absolute value; It is a natural exponential function; The dead zone threshold for safe internal force is used to filter out sensor noise floor.

[0044] in, This is a projection term used to quantify the component of internal force in the direction of the line connecting them. The larger this component is, the more the internal force tends to pull or compress. The activation term is used to determine whether the internal force amplitude exceeds the safety threshold; the cooperative resistance index is only activated when the direction of the internal force coincides with the direction of the line connecting the two arms (i.e., the projection term is large and the amplitude exceeds the threshold). Output a high value.

[0045] Among them, the dead zone threshold of safe internal force Used to filter out sensor noise floor and set allowable basic internal forces, if If the setting is too small, the system will misinterpret normal friction or minor disturbances as adversarial behavior, causing frequent fluctuations in the control weights. If the threshold is set too high, the system will react slowly to real-world challenges, potentially damaging the workpiece. Therefore, in other embodiments, implementers can adjust the safe internal force dead zone threshold according to the actual implementation situation. The value range is set to 5% to 10% of the maximum allowable force on the workpiece. In this embodiment, Set to 8% of the maximum allowable force on the workpiece.

[0046] Furthermore, based on the cooperative adversarial index, the adaptive internal force penalty weight in the DMPC cost function is calculated through nonlinear mapping. The specific calculation formula is as follows:

[0047] ;

[0048] In the formula, for The adaptive internal force penalty weight at any given time; The base weight corresponds to the normal mode, and its value is relatively small to ensure position tracking accuracy. This is a saturation weight, corresponding to the compliant mode, with a maximum value to forcibly suppress internal force; Furthermore, the two differ by a huge order of magnitude, thus enabling the switching between two drastically different operating modes in optimized control. Therefore, in this embodiment, =0.1, =1000; for The index of collaboration and confrontation at any given moment; To adjust the sensitivity coefficient.

[0049] in, Control the rate at which the weights grow with the adversarial exponent, if If the weight is too small, the weight adjustment will lag, which may cause the workpiece to be damaged before the weight is increased. If the sensitivity coefficient is too high, the system becomes overly sensitive, and even minor disturbances can trigger a high weighting, leading to unstable position control. Therefore, adjusting the sensitivity coefficient is necessary. The value range is [0.1, 0.5]. In this embodiment, the value is... Set it to 0.3.

[0050] The calculation formula utilizes a modified form of the hyperbolic tangent function to construct a smooth mapping from the cooperative adversarial index to the weights; when When smaller, Maintain at the base weight Nearby, the system maintains high stiffness; when When it increases, Rapidly approaching saturation weights The system switches to compliant mode.

[0051] For example, Figure 3 This is a schematic diagram of the adaptive internal force penalty weight; the green curve represents the adaptive internal force penalty weight calculated according to step S2. Within the normal cooperative range of 0-4s and 7-10s, due to the relatively small internal force and the lack of obvious antagonistic characteristics, Stabilize at the base weight Near the black dotted line, the system maintains a high-rigidity position control mode; once it enters the 4-7 second resistance zone, the internal force increases and the direction exhibits resistance. The response was rapid and the increase was exponential, reaching near-saturation weight at its peak. This demonstrates the sensitivity and adaptability of the method, enabling it to instantly switch control strategies when an adversarial risk is detected.

[0052] It should be noted that this invention constructs a cooperative resistance index that integrates the direction and amplitude of internal force, and dynamically calculates the adaptive internal force penalty weight through nonlinear mapping based on this index: when cooperation is smooth, the weight is low, and the system maintains high stiffness to ensure high-precision trajectory tracking; when cooperative resistance is detected, such as mutual pulling or squeezing, the weight increases rapidly, and the system automatically switches to compliant mode to release internal force; this solves the contradiction between position accuracy and compliance caused by the use of fixed weights in traditional DMPC.

[0053] S3: Establish the state-space equation of the system based on the double integral model and calculate the prediction error vector at future times in the prediction time domain; introduce an equivalent environmental stiffness matrix to map the prediction error vector in the prediction time domain to the prediction internal force vector; construct a cost function that includes the product of adaptive internal force penalty weight and prediction internal force vector, and obtain the optimal control increment sequence by minimizing the cost function.

[0054] It should be noted that since the existing DMPC framework usually uses position or velocity as state variables, it is difficult for the controller to directly optimize future internal forces. Therefore, this embodiment introduces an equivalent environmental stiffness matrix to establish a mapping relationship from position error to internal forces, giving the controller the ability to predict future internal forces and avoid them in advance.

[0055] Specifically, the robotic arm controller runs a trajectory planner that generates a path based on task requirements, such as the start and end points of the transport. The theoretical expected position and theoretical expected velocity at time; and the robotic arm in The difference between the joint position and joint velocity at any given time is used to obtain the position error and velocity error, which together form the joint position and velocity error. Error vector at time step .

[0056] Furthermore, based on the double integral model, the state-space equations of the system are established:

[0057] ;

[0058] in, for The prediction error vector at time step; for The error vector at any given time includes position error and velocity error; The control increment to be solved; Let be the state transition matrix, and ; For the input control matrix, and ; To control the cycle, It is the identity matrix; This represents the dot product of vectors.

[0059] Furthermore, an equivalent environmental stiffness matrix is ​​introduced. Predicting the time domain The future position state within is mapped to the predicted internal force vector, and the specific calculation formula is as follows:

[0060] ;

[0061] In the formula, In order to be in Predicting the future The predicted internal force vector at time t; Belongs to the prediction time domain ; The preset equivalent environmental stiffness matrix characterizes the rigidity of the workpiece and the fixture. The larger the value, the harder the workpiece is considered by the system and the more sensitive it is to positional deviation. For based on The future obtained from the error vector at time step The prediction error vector at time step is derived from the state-space equations of the system. The output selection matrix is ​​used to select from the prediction error vector. The position error component is extracted from it; therefore, ; For the future The predicted relative position deviation at any given time is calculated based on the theoretical expected position of the neighboring robot arm at the future time, and is equal to the neighboring robot arm's position at the future time. The theoretical expected position of the robotic arm at a given moment and its future position The difference between the theoretical expected positions at each moment; This represents the dot product of vectors.

[0062] in, Physically, it characterizes the rigidity of the contact between the end effector of a robotic arm and the environment, i.e., the workpiece being gripped and the fixture. It describes the magnitude of the reaction force exerted by the environment on the robotic arm when there is a deviation between the end effector and the desired position, and it follows the generalized Hooke's Law. The experimental setup includes: during the system initialization phase, controlling the robotic arm to hold the workpiece in a stationary state, and controlling one arm to generate a small and known positional displacement in each axis direction of Cartesian space. For example, 1 mm, recording the change in force sensor readings. According to the formula Calculate the stiffness values ​​of each axis and construct a diagonal matrix as the preset equivalent environmental stiffness matrix. .

[0063] The calculation formula is based on the generalized form of Hooke's Law, which transforms the position deviation into the dimension of force through environmental stiffness, enabling the controller to assess the magnitude of internal forces that the currently planned trajectory may generate in the future.

[0064] It should be noted that this invention introduces an equivalent environmental stiffness matrix into the prediction model of DMPC, and establishes a mapping relationship from the predicted position error to the predicted internal force. This enables the controller not only to adjust for the current internal force, but also to predict the magnitude of the internal force that may be generated by the future trajectory during the rolling optimization process. As a result, it can actively avoid the motion trend that generates large internal forces during the control quantity solution stage, which significantly improves the safety of multi-arm collaboration.

[0065] Furthermore, each robotic arm solves for the adaptive DMPC cost function in each control cycle. The formula for the cost function is:

[0066] ;

[0067] In the formula, For the robotic arm in The cost function at time step; For prediction in the time domain; For based on The future obtained from the error vector at time step The prediction error vector at time step; This is the state weight matrix, used to set the priority of position tracking; for The adaptive internal force penalty weight at any given time; In order to be in Predicting the future The predicted internal force vector at time t; The control increment to be solved; To control the incremental weight matrix; For the prediction error vector With the state weight matrix The square of the weighted Euclidean norm, and , Indicates transpose; The control increment to be solved With control increment weight matrix The square of the weighted Euclidean norm, and ; This represents the dot product of vectors.

[0068] In robotic arm control, different indicators have varying degrees of importance; therefore, the state weight matrix... and control increment weight matrix Both belong to regulators; position error weight matrix The matrix determines the positional accuracy of the robotic arm. Therefore, if the requirement for positional accuracy is much greater than the requirement for speed stability, then... Weight of corresponding position Set it to be very large, and assign a weight to the corresponding speed. Set it very small; control the incremental weight matrix The control increment weight matrix determines the intensity of the robotic arm's movements and is used to measure the cost of changing the motor output. Therefore, if a very smooth motor output without abrupt changes is desired, the control increment weight matrix should be increased. If the motor is allowed to output power at high speed, and the goal is to reach the target as quickly as possible, then the control increment weight matrix should be reduced. ;

[0069] The cost function contains three optimization objectives: the first term... Used to minimize position tracking errors and ensure operational accuracy; the second item This is an internal force penalty term used to minimize the predicted internal force, ensuring cooperative compliance, and the weight of this term is... It is time-varying; the third item The optimizer uses these three indicators to constrain the control increment and ensure the smooth operation of the actuator; the optimizer finds the optimal control sequence by weighing these three indicators.

[0070] For example, Figure 4 This is a schematic diagram of the internal force penalty term in the DMPC cost function; where existing algorithms use fixed small weights. Even when the internal force is extremely dangerous within 4-7 seconds, its proportion in the cost function remains small. This means that the optimizer will still prioritize optimizing the position tracking error while ignoring the huge internal force, which can easily lead to workpiece damage or equipment overload. This solution uses adaptive weighting. When confrontation occurs, due to The surge in internal force terms amplifies the cost instantly. Under the DMPC framework, the enormous cost forces the quadratic programming solver to prioritize finding control increments that can reduce this cost. Therefore, this scheme can force the controller to sacrifice a certain position tracking accuracy when conflict occurs, actively yielding to eliminate dangerous internal forces, thereby achieving compliant cooperation and system protection, which existing algorithms cannot do.

[0071] It should be noted that by simultaneously constraining position error, predicting internal force, and controlling increment in the cost function, and using a distributed architecture for solving, this invention can effectively address the asynchronous problems caused by communication delays or external disturbances, ensure the smooth operation of each robotic arm actuator, and prevent workpiece damage or motor overload caused by sudden changes in internal force.

[0072] S4: Update the speed command to drive the robotic arm movement based on the optimal control increment sequence.

[0073] Specifically, an embedded quadratic programming solver is used to solve the constructed optimization problem in real time to obtain the optimal control increment sequence in the prediction time domain. Based on the rolling optimization principle of model predictive control, only the first element of the optimal control increment sequence is applied to the system to update the current speed command and send it to the servo driver. At the next moment, the state measurement value of the robotic arm is updated, and steps S1 to S3 are repeated to achieve high-frequency closed-loop adaptive control.

[0074] This invention also discloses a distributed adaptive control system for multi-robotic arm collaboration, including a processor and a memory. The memory stores computer program instructions, which, when executed by the processor, implement a distributed adaptive control method for multi-robotic arm collaboration according to the present invention.

[0075] The system also includes other components well known to those skilled in the art, such as communication buses and communication interfaces, the settings and functions of which are known in the art and will not be described in detail here.

Claims

1. A distributed adaptive control method for multi-robotic arm collaboration, characterized in that, include: The state measurement values ​​of the robotic arm are collected in real time, and the pure internal force vector is decoupled from the end six-dimensional force sensor data of the state measurement values ​​using pre-identified dynamic parameters; The cooperative resistance index is calculated based on the pure internal force vector and the relative position vector. This index is obtained by multiplying a projection term and an activation term. The projection term is calculated by dividing the absolute value of the dot product of the decoupled pure internal force vector and the relative position vector between the local robot and the neighboring robot acquired through communication by the magnitude of the relative position vector. The activation term is constructed based on the Sigmoid function, with its independent variable being the difference between the magnitude of the decoupled pure internal force vector and the safe internal force dead zone threshold. This safe internal force dead zone threshold is used to filter sensor noise and set the allowable basic internal force, with a value range of 5% to 10% of the maximum allowable force on the workpiece. An adaptive internal force penalty weight is calculated based on the cooperative resistance index through nonlinear mapping. This adaptive internal force penalty weight is equal to the basic weight plus a dynamic increment. The dynamic increment is obtained by multiplying the difference between the saturation weight and the basic weight by a function term related to the cooperative resistance index. This function term is the ratio of the numerator to the denominator, consisting of a natural exponential function term containing the product of the cooperative resistance index and the sensitivity adjustment coefficient. The formula for this function term is: In the formula, for The index of collaboration and confrontation at any given moment; To adjust the sensitivity coefficient, the value range is [0.1, 0.5]; The system's state-space equations are established based on a double-integral model, and the prediction error vector for future moments in the prediction time domain is calculated. This prediction error vector for future moments is equal to the sum of the system's autonomous evolution part and the control input influence part. The system's autonomous evolution part is obtained by multiplying the state transition matrix by the error vector containing position and velocity errors at the current moment. The control input influence part is obtained by multiplying the input control matrix by the control increment to be solved. An equivalent environmental stiffness matrix characterizing the rigidity of the workpiece and the fixture is introduced. This equivalent environmental stiffness matrix is ​​set through experimental determination, including: during the system initialization phase, controlling the robotic arm to hold the workpiece in a stationary state, and controlling one arm to generate a small and known positional displacement in each axis direction of Cartesian space. Record the change in force sensor readings According to the formula Calculate the stiffness values ​​of each axis and construct a diagonal matrix as the preset equivalent environmental stiffness matrix; map the prediction error vector in the prediction time domain to the predicted internal force vector, which is equal to the preset equivalent environmental stiffness matrix multiplied by the equivalent position deviation at that moment; the equivalent position deviation is calculated by extracting the position error component from the prediction error vector at that future moment using the output selection matrix, and then subtracting the predicted relative position deviation value at that future moment; the predicted relative position deviation value at that future moment is equal to the difference between the theoretical expected position of the neighboring robot arm at the future moment and the theoretical expected position of this robot arm at the future moment; A cost function is constructed that includes the product of adaptive internal force penalty weights and the predicted internal force vector. The cost function is the sum of the squares of three weighted norms over the prediction time domain. The first term is the square of the weighted Euclidean norm of the prediction error vector and the state weight matrix. The second term is the product of the square of the Euclidean norm of the predicted internal force vector and the adaptive internal force penalty weight at the current time. The third term is the square of the weighted Euclidean norm of the control increment to be solved and the control increment weight matrix. The optimal control increment sequence is obtained by minimizing the cost function, and the speed command is updated to drive the robot arm movement.

2. The distributed adaptive control method for multi-robotic arm collaboration according to claim 1, characterized in that, The pure internal force vector is decoupled from the end-effector six-dimensional force sensor data of the state measurement value using pre-identified dynamic parameters. The decoupled pure internal force vector is equal to the end-effector six-dimensional force sensor data minus the resultant dynamic force required to maintain the current motion. The resultant dynamic force is calculated by the pre-identified dynamic parameters and is equal to the inertial force term represented by the product of the equivalent inertia of the robotic arm in Cartesian space and the Cartesian acceleration of the robotic arm, the nonlinear force term represented by the product of the Coriolis force and the centrifugal force of the robotic arm in Cartesian space and the Cartesian velocity of the robotic arm, and the sum of the gravity compensation term of the robotic arm in Cartesian space.

3. A distributed adaptive control system for multi-robotic arm collaboration, characterized in that, include: A processor and a memory, the memory storing computer program instructions that, when executed by the processor, implement a distributed adaptive control method for multi-robotic arm collaboration according to any one of claims 1-2.

Images