Double-arm robot cooperative control method based on cooperation index and quadratic programming

Abstract

The invention discloses a two-arm robot cooperative control method based on a cooperation index and quadratic programming, and belongs to the technical field of robot control, and the method comprises the steps: constructing a cooperation index model, fusing a trajectory tracking error, a two-arm synchronization error and a tail end contact force error through the cooperation index model, and introducing a dynamic weight factor to carry out the continuous switching of a control mode; according to the cooperation index model, generating a compliant offset trajectory by adopting admittance control, and updating the cooperation index model by taking the compliant offset trajectory as a trajectory correction item; constructing a quadratic programming problem including joint torque, tail end speed and contact force constraint by taking minimization of a cooperation index as a target; and the quadratic programming problem is solved in real time in each control period, and the optimal joint control torque is obtained and output to the double-arm robot. According to the invention, flexible switching and accurate control of multiple tasks are realized.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, and in particular relates to a collaborative control method for a dual-arm robot based on a cooperation index and quadratic programming. Background Technology

[0002] Compared to single-arm robots, dual-arm robots offer advantages such as strong coordinated operation capabilities, wide working range, and flexible system structure, making them widely used in complex tasks such as flexible assembly, collaborative handling, and human-robot interaction. However, the high task adaptability of dual-arm robots significantly increases the difficulty of collaborative control: on the one hand, it requires meeting the accuracy requirements of trajectory tracking and synchronous movement between the two arms; on the other hand, it also requires real-time and compliant responses to changes in external contact forces. Specifically, firstly, current control strategies cannot simultaneously consider multiple control objectives such as trajectory tracking, dual-arm synchronization, and end-effector force feedback, and the lack of unified modeling among modules leads to conflicts and switching obstacles between control tasks. Secondly, the switching of control modes mostly relies on discrete heuristic judgments, such as setting a certain threshold, which triggers mode switching when the threshold is exceeded. Therefore, the lack of a continuously adjustable and measurable unified index to drive the allocation of control objectives at different task stages results in unstable system response and unpredictable control behavior. Finally, although traditional force-position hybrid control and impedance / admittance control have good compliance, their parameter adjustment is not related to the task control objective and lacks a coupling mechanism with trajectory tracking control and dual-arm motion synchronization control, making it difficult to integrate into high-precision, multi-objective parallel task scenarios and limiting its practical usability in complex interactive operations.

[0003] Currently, relevant technicians have conducted some design and research on cooperative control methods for dual-arm robots. CN118081766A proposes a master-slave unified admittance control method for dual-arm robots oriented towards coordinated tasks. By establishing kinematic models of open-chain and closed-chain systems to obtain the desired end-effector trajectory, the resultant force of the object is decomposed and introduced into the grasping matrix and admittance control design to create a master-slave unified admittance control algorithm. Furthermore, cooperative control strategies are established for non-coordinated, loosely coordinated, and tightly coordinated tasks to maintain system internal force stability under human-robot collaboration and external force interference, achieving compliant operation of the dual-arm robot, applicable to scenarios such as cooperative handling. However, although this method establishes a unified cooperative control framework for non-coordinated, loosely coordinated, and tightly coordinated tasks, it does not construct a unified index to comprehensively measure and coordinate multiple objectives such as trajectory tracking, motion synchronization, and force feedback during the control process. In practical complex tasks, it is difficult to intuitively evaluate and balance the relationship between various control objectives. CN115625711B proposes a cooperative control method for dual-arm robots considering end-effector forces. First, the motion constraints are determined. Then, force analysis and dynamic modeling are performed using Newton's second law and the Lagrange method. Position control is then achieved by combining a disturbance observer with a hyperbolic tangent sliding mode controller. Simultaneously, the desired contact force and desired position are obtained through adaptive impedance control. Finally, after correction, the joint torque is output to complete the transport task. This method uses an impedance model to adjust the end-effector force response to complete the dual-arm transport task, exhibiting a certain degree of compliance. It is suitable for complex operations in uncertain and rigid environments, but lacks unified modeling for multiple task objectives (such as trajectory, synchronization, and force). Secondly, the control strategy primarily optimizes the end-effector contact force, which cannot flexibly adapt to other control tasks. When performing trajectory tracking or dual-arm synchronous motion tasks, trajectory tracking errors and synchronization errors easily occur, leading to task failure. CN118700151A proposes a cooperative control method, device, electronic equipment, and medium for dual-arm robots based on adaptive prediction. First, motion data of the dual-arm robot and information about the external environment are acquired. A motion state prediction model is constructed, and predictive control signals are generated. The predicted and actual control signals are compared to determine the target error. The model control parameters are adaptively adjusted to minimize the target error. Finally, the adjusted parameters are used to generate a target control signal to achieve cooperative motion. However, in real-world complex task scenarios, different stages of the task have different requirements for the robot's motion. For example, some stages may focus more on trajectory tracking, while others may require highly synchronized movement of both arms or precise control of contact forces. This method only achieves cooperative motion by adjusting the model control parameters, without detailed analysis and modeling of each control mode (such as trajectory tracking, synchronized movement of both arms, contact force control, etc.), and lacks a rigorous and unified optimization framework. This results in poor system stability in uncertain, rigid contact environments, affecting the robot's applicability in complex task scenarios.

[0004] In summary, existing collaborative control methods for dual-arm robots have made progress in improving compliance, control accuracy, and adaptability to various tasks, but key shortcomings remain. While CN118081766A establishes a unified admittance control framework applicable to non-coordinated, loosely coordinated, and tightly coordinated tasks, it lacks quantifiable unified indicators for coordinating multiple objectives such as trajectory tracking, motion synchronization, and force control, making it difficult to achieve efficient trade-offs and dynamic adjustments in complex tasks. CN115625711B focuses on end-effector force control, employing impedance control and sliding mode control to achieve coupled position and force control; however, its control objective is too singular, failing to adapt to diverse task requirements and prone to error accumulation in trajectory tracking or dual-arm coordinated tasks. CN118700151A introduces an adaptive predictive control mechanism, improving the system's dynamic response capability, but it ignores differences in task stages and the diversity of control modes, lacking a unified modeling and optimization mechanism, making it difficult to ensure stable system operation under rigid environments and external disturbances. Therefore, there is an urgent need for a collaborative control method that has unified modeling capabilities among multiple control objectives, can dynamically balance task priorities, and takes into account both compliance and stability, in order to meet the widespread application needs of dual-arm robots in complex, human-computer interaction-intensive scenarios. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention provides a cooperative control method for a dual-arm robot based on a cooperative index and quadratic programming, comprising: A collaborative index model is constructed, which integrates trajectory tracking error, dual-arm synchronization error and end-effector contact force error, and introduces dynamic weighting factors to continuously switch control modes. Based on the cooperation index model, admittance control is used to generate a compliant offset trajectory, and the compliant offset trajectory is used as a trajectory correction term to update the cooperation index model. To minimize the collaboration index, a quadratic programming problem is constructed that includes constraints on joint torque, end velocity, and contact force. The quadratic programming problem is solved in real time during each control cycle to obtain the optimal joint control torque and output it to the dual-arm robot.

[0006] Preferably, the process of constructing the collaboration index model includes: Obtain the trajectory tracking error of each arm at the current moment, the motion synchronization error between the two arms, and the end contact force error; The collaborative weighting factor and the force weighting factor are dynamically adjusted according to the task phase. The trajectory tracking error, the motion synchronization error between the two arms, and the end contact force error are linearly fused with the cooperation weighting factor and the force weighting factor to obtain the cooperation index.

[0007] Preferably, the process of dynamically adjusting the collaborative weighting factor and the force weighting factor includes: If both the collaborative weighting factor and the force weighting factor are 0, then the trajectory tracking mode is set. If the coordination weight factor is greater than 0 and the force weight factor is 0, then the mode is set to dual-arm synchronous movement. If both the collaborative weighting factor and the force weighting factor are greater than 0, then the force-position hybrid control mode is set. If the collaborative weighting factor is 0 and the force weighting factor is greater than 0, then the force priority control mode is set.

[0008] Preferably, the process of generating a compliant offset trajectory using admittance control includes: Obtain the force error between the actual contact force and the expected contact force at the robot's end effector; Input the force error into the admittance model to calculate the desired acceleration of the mass-damped-spring system. Discrete integration is performed on the desired acceleration to obtain the compliant offset trajectory.

[0009] Preferably, the process of updating the cooperation index model with the compliant offset trajectory as a trajectory correction term includes: The compliant offset trajectory is superimposed on the original desired trajectory to obtain the corrected reference trajectory; The trajectory tracking error is recalculated based on the corrected reference trajectory; Replace the original trajectory tracking error with the aforementioned trajectory tracking error to reconstruct the cooperation index.

[0010] Preferably, the process of constructing a quadratic programming problem with constraints on joint torque, end-effector velocity, and contact force, with the objective of minimizing the cooperation index, includes: Establish a linear mapping relationship between joint torque and end-effector position increment; The cooperation index is expressed as a linear function of joint torque; Construct a quadratic objective function with joint torque as the optimization variable.

[0011] Preferably, the process of constructing a quadratic objective function with joint torque as the optimization variable includes: Based on the mapping relationship between the cooperation index and the joint torque, calculate the coefficients of the quadratic and linear terms of the objective function; Ignoring the constant term, we obtain the standard quadratic programming form.

[0012] Preferably, the process of constructing a quadratic programming problem that includes constraints on joint torque, end velocity, and contact force further includes: The joint torque limit, end velocity limit, and end contact force limit are written as linear inequalities, and then combined into a unified linear inequality constraint.

[0013] Preferably, the process of solving the quadratic programming problem in real time within each control cycle includes: Within each control cycle, the objective function coefficients and constraints are updated based on the current robot state. The solver is invoked to solve the standard quadratic programming problem online and obtain the optimal joint torque; The optimal joint torque is output to each joint actuator to achieve torque control.

[0014] Preferably, the method further includes cyclically executing the cooperative index model, admittance control, and quadratic programming problem in the same controller to form a closed-loop multi-mode cooperative control.

[0015] Compared with the prior art, the present invention has the following advantages and technical effects: This invention proposes a cooperative control method for a dual-arm robot based on a cooperative index and quadratic programming. By constructing a unified cooperative index model, trajectory tracking error, dual-arm synchronization error, and end-effector contact force error are incorporated into a unified framework. Dynamic weights are used to achieve continuously adjustable task objectives, continuously controllable mode switching, and compliant end-effector posture adjustment. Admittance control is employed to generate compliant offset trajectories, achieving deep integration of trajectory control and force control at the planning layer, thereby improving trajectory accuracy and environmental adaptability. With minimizing the cooperative index as the optimization objective, a quadratic programming control framework containing multiple physical constraints such as joint torque, end-effector velocity, and contact force is established. The optimal control input is solved online in each control cycle, thus balancing control accuracy, response speed, and physical rationality, and enhancing the system's dynamic compliant adjustment capability in complex tasks and rigid environments.

[0016] This invention achieves flexible switching and precise control across multiple tasks by constructing a unified cooperation index model. It proposes a dual-arm robot control framework based on the cooperation index, integrating three control objectives—trajectory tracking error, synchronization error between the two arms, and end-effector contact force error—into a unified evaluation index. By introducing an adjustable task weight factor, the system can adjust its control focus according to the task stage, enabling flexible switching between position-priority, synchronization-priority, and force-priority modes. This achieves task-driven multi-objective scheduling and strategy switching capabilities while ensuring control consistency.

[0017] This invention enhances system compliance and interaction safety by introducing an admittance control mechanism. An admittance control model is introduced into the collaborative framework to model the contact forces between the robot and the environment, achieving compliant control. Admittance control uses the error between the actual and desired forces at the end effector as input to dynamically generate trajectory deviation correction terms, enhancing the system's responsiveness to external disturbances. This ensures high-precision trajectory tracking and synchronized movement of both arms while maintaining a certain degree of compliance with uncertain, rigid environments, and improves interaction safety in human-robot collaboration.

[0018] This invention, based on a quadratic programming optimization problem, ensures real-time control performance and stability. It constructs the minimization of the cooperation exponent as a standard constrained quadratic programming problem, using the torques of each joint as optimization variables and explicitly incorporating physical constraints. By calling a high-efficiency real-time QP solver, the optimal control command is quickly solved within each control cycle, achieving a high-performance control strategy with minimized dynamic error and controllable, constrained input.

[0019] This invention establishes a unified optimization solution framework to improve system integration. By modeling various tasks as unified optimization objectives and solving them uniformly in the same controller, the logical complexity of the control strategy is greatly simplified. The control framework is easy to integrate with mainstream industrial robot platforms, supports embedded deployment or ROS real-time operating environment, and is scalable, allowing the introduction of functional modules such as visual recognition and feedback, making it highly versatile. Attached Figure Description

[0020] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings: Figure 1 This is a schematic diagram of the method flow according to an embodiment of the present invention; Figure 2 This is a schematic diagram illustrating the classification of collaboration modes in an embodiment of the present invention; Figure 3 Admittance control block diagram introduced for embodiments of the present invention; Figure 4 This is a schematic diagram of the overall algorithm framework of an embodiment of the present invention. Detailed Implementation

[0021] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0022] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0023] like Figure 1 and Figure 4 As shown, this embodiment provides a cooperative control method for a dual-arm robot based on a cooperation index and quadratic programming, including: A collaborative index model is constructed, which integrates trajectory tracking error, dual-arm synchronization error and end-effector contact force error, and introduces dynamic weighting factors to continuously switch control modes. Based on the cooperation index model, admittance control is used to generate a compliant offset trajectory, and the compliant offset trajectory is used as a trajectory correction term to update the cooperation index model. To minimize the collaboration index, a quadratic programming problem is constructed that includes constraints on joint torque, end velocity, and contact force. The quadratic programming problem is solved in real time during each control cycle to obtain the optimal joint control torque and output it to the dual-arm robot.

[0024] Furthermore, the process of constructing the collaboration index model includes: Obtain the trajectory tracking error of each arm at the current moment, the motion synchronization error between the two arms, and the end contact force error; The collaborative weighting factor and the force weighting factor are dynamically adjusted according to the task phase. The trajectory tracking error, the motion synchronization error between the two arms, and the end contact force error are linearly fused with the coordination weighting factor and the force weighting factor to obtain the coordination index.

[0025] Furthermore, the process of dynamically adjusting the collaborative weighting factor and the force weighting factor includes: If both the collaborative weighting factor and the force weighting factor are 0, then the trajectory tracking mode is set. If the coordination weight factor is greater than 0 and the force weight factor is 0, then the mode is set to dual-arm synchronous movement. If both the collaborative weighting factor and the force weighting factor are greater than 0, then the force-position hybrid control mode is set. If the collaborative weight factor is 0 and the force weight factor is greater than 0, then the force priority control mode is set.

[0026] Furthermore, the process of generating a compliant offset trajectory using admittance control includes: Obtain the force error between the actual contact force and the expected contact force at the robot's end effector; Input the force error into the admittance model to calculate the desired acceleration of the mass-damped-spring system. Discrete integration of the desired acceleration yields the compliant offset trajectory.

[0027] Furthermore, the process of updating the cooperation index model by using the compliant offset trajectory as a trajectory correction term includes: The compliant offset trajectory is superimposed on the original desired trajectory to obtain the corrected reference trajectory; The trajectory tracking error is recalculated based on the corrected reference trajectory; Replace the original trajectory tracking error with the trajectory tracking error and reconstruct the cooperation index.

[0028] Furthermore, the process of constructing a quadratic programming problem with constraints on joint torque, end-effector velocity, and contact force, with the objective of minimizing the cooperation index, includes: Establish a linear mapping relationship between joint torque and end-effector position increment; The cooperation index is expressed as a linear function of joint torque; Construct a quadratic objective function with joint torque as the optimization variable.

[0029] Furthermore, the process of constructing a quadratic objective function with joint torque as the optimization variable includes: Based on the mapping relationship between the cooperation index and the joint torque, calculate the coefficients of the quadratic and linear terms of the objective function; Ignoring the constant term, we obtain the standard quadratic programming form.

[0030] Furthermore, the process of constructing a quadratic programming problem that includes constraints on joint torque, end-effector velocity, and contact force also includes: The joint torque limit, end velocity limit, and end contact force limit are written as linear inequalities, and then combined into a unified linear inequality constraint.

[0031] Furthermore, the process of solving the quadratic programming problem in real time within each control cycle includes: Within each control cycle, the objective function coefficients and constraints are updated based on the current robot state. The solver is invoked to solve the standard quadratic programming problem online and obtain the optimal joint torque; The optimal joint torque is output to each joint actuator to achieve torque control.

[0032] Furthermore, the method also includes cyclically executing the cooperative index model, admittance control, and quadratic programming problem in the same controller to form a closed-loop multi-mode cooperative control.

[0033] Furthermore, as a preferred implementation method in the above embodiments, this embodiment proposes a cooperative control method for a dual-arm robot based on a cooperative index model and quadratic programming. First, a unified cooperative index model for multiple tasks is constructed, introducing dynamically adjustable weight factors to fuse trajectory error, dual-arm synchronization error, and end-effector contact force error, enabling continuous switching of control modes. Second, an admittance control model is introduced at the planning layer to calculate the desired compliant offset trajectory and correct the cooperative index, thereby enhancing the robot's end-effector's force perception and compliant response capabilities while ensuring tracking accuracy and synchronization stability. Finally, with the goal of minimizing the cooperative index, a standard quadratic programming problem with multi-physics constraints is constructed, and an efficient solver is used to optimize the control torque of each joint online.

[0034] like Figure 1 As shown, the above implementation method specifically includes the following steps: Step 1: Utilize dynamically adjusted collaborative weighting factors according to different task stages. Sum of forces weighting factors To achieve rapid switching of control modes, the system simultaneously calculates the tracking trajectory error, motion synchronization error, and force error of the dual-arm robot under the current state, and constructs a collaborative index model that integrates weighting factors and various errors.

[0035] Step 2: Obtain the contact force between the end effector of the dual-arm robot and the environment, use admittance control to model the external force response of the robot end effector, dynamically generate a compliant offset trajectory, and introduce the compliant offset trajectory as a trajectory correction term into the cooperative index model to improve the compliance and stability of the robot in trajectory tracking and synchronous motion.

[0036] Step 3: Construct a constrained quadratic programming optimization problem with the objective of minimizing the collaboration index.

[0037] Step 4: Call the QP solver to solve the joint control input in real time to achieve multi-mode collaborative control.

[0038] To optimize the above technical solution, the specific measures also include: Step 1 above specifically includes: A unified modeling method for multi-task objectives based on a cooperation index is adopted, incorporating trajectory error, dual-arm synchronization error, and end-effector contact force error into a unified cooperation index model, and using dynamic cooperation weighting factors. Sum of forces weighting factors The three types of errors are fused and modeled, and the weighting factors are adjusted in real time according to the task status to achieve continuous switching of multiple control modes such as trajectory tracking priority, dual-arm synchronous motion, and force control priority.

[0039] In dual-arm robot control, the motion objectives of the robotic arms can be categorized into trajectory tracking objectives, motion synchronization objectives, and contact force objectives. The trajectory tracking objective aims to minimize the error between the robot's current position and its desired position; the motion synchronization objective aims to minimize the cooperative position error between the two arms; and the contact force objective aims to minimize the error between the actual contact force and the desired contact force.

[0040] For each robotic arm Define trajectory tracking error respectively Synchronization error of bi-arm movement Force tracking error .

[0041] Tracking error: ; in, Indicates the robot's end-effector pose. This represents the desired trajectory.

[0042] Synchronization error of bi-arm movement: ; in, These are the robot's two arms. This indicates the synchronization error between the two arms. Indicates the robot's end-effector pose. Indicates the end-effector pose of another robotic arm; Force tracking error: ; in, This represents the actual contact force between the robot and its environment. It indicates the desired contact force.

[0043] In the planning of dual-arm robots, different control objectives may arise at different stages of the task. To describe the switching characteristics of control objectives, a dynamically adjusted cooperative weighting factor is used. Sum of forces weighting factors Integrate multiple objectives together. Among them, This represents the weighting factor for the coordinated position control of the two arms. This represents the weighting factor for force control. For example... Figure 2 As shown, if , This is the trajectory tracking mode; if , This is a synchronized arm movement mode; if , This is a force-position hybrid control mode; if , This is a force-priority control mode.

[0044] The cooperation index (cooperation error), defined based on trajectory tracking error, motion synchronization error, force tracking error, and dynamically adjusted weighting factors, is as follows: ; in, The identity matrix is ​​represented, and the optimization objective is to minimize the cooperation index. The robot is controlled to track its own trajectory, move its two arms synchronously, and track the desired end contact force.

[0045] Step 2 above specifically includes: To enable the dual-arm robot to still have the ability to adapt to external disturbances under high-precision position control objectives such as trajectory tracking and motion synchronization, an admittance control model is used in the planning layer to calculate the desired compliant offset trajectory, and this is introduced into the cooperation index as a trajectory correction term. In this way, while ensuring the accuracy of trajectory tracking and synchronous motion, force-sensing guided trajectory adaptive correction is achieved, enabling the robot to have dynamic compliant behavior.

[0046] Admittance control, in its physical essence, simulates the response of a "mass-damped-spring system" to external forces. Its general expression is: ; in, Represents the mass matrix, Represents the damping matrix. Represents the stiffness matrix. This represents the robot's movement trajectory. Indicates the robot's reference trajectory. Indicates the desired contact force. This indicates the actual contact force.

[0047] Figure 3 The admittance control block diagram shows the contact force between the robot and the environment detected by a six-dimensional force sensor. , and expectation Comparison generates force error signal . As the input to the admittance controller, the output position correction is... From this, the compliant response trajectory offset can be obtained. .

[0048] Since admittance control is decoupled across all dimensions, only one dimension needs to be considered, and its expression is: ; in, , , , , , , Each represents a value in a certain dimension. This represents the actual acceleration at the robot's end effector. This represents the expected acceleration of the robot's end effector. This indicates the actual speed of the robot's end effector. This represents the desired speed of the robot's end effector. In practical control, the admittance model is implemented in discrete form, and can be approximated using the Euler difference as follows: ; in, Indicates the system's control cycle. This indicates the current instruction trajectory. This indicates the trajectory of the instruction at the previous moment. This represents the actual acceleration of the robot's end effector at the current moment. This indicates the actual speed of the robot's end effector at the previous moment. This represents the expected velocity of the robot's end effector at the previous moment. This indicates the actual position of the robot's end effector at the previous moment. This indicates the desired position of the robot's end effector at the previous moment. This represents the expected acceleration of the robot's end effector at the previous moment. This indicates the actual speed of the robot's end effector at the current moment. This indicates the actual speed of the robot's end effector at the previous moment. This indicates the actual position of the robot's end effector at the current moment; The robot's command trajectory at the current moment is obtained from the above formula. This means that the robot end effector should have the desired compliant response when subjected to force.

[0049] Will As a trajectory offset correction term superimposed on the original end target trajectory, the reference trajectory after compliance correction is defined as: ; In the formula, Represents the original terminal target trajectory. This indicates the admittance control trajectory correction term; Updated reference trajectory Used to update trajectory tracking error : ; In the formula, Indicates the current position of the robot. This represents the robot's reference trajectory after compliance correction; This leads to the construction of a new collaboration index: ; In high-precision trajectory tracking or high-precision synchronous motion tasks, admittance control allows robots to maintain the task control objective while exhibiting compliant and continuous attitude adjustments, thereby improving stability and reducing mechanical shock and vibration in uncertain, rigid contact environments.

[0050] Step 3 above specifically includes: To achieve trajectory tracking, synchronized motion, and compliant end-effector control in dual-arm robots across various task scenarios, a quadratic cost function is constructed using the cooperation index as a unified optimization objective. Multiple task-related constraints are then uniformly expressed as linear inequalities, ultimately formalizing the control problem as a standard quadratic programming problem. The optimal control input is then solved online within each control cycle using an efficient solver. The optimization variable is the control torque vector of each joint of the robot. The solution output is the optimal control input used to drive each joint.

[0051] For dual-arm robot systems, trajectory tracking error (position control), synchronization error (arm coordination), and end-effector force error (compliant control) have been defined, and a cooperation index has been constructed based on these three types of errors. The goal is to construct a system that minimizes this cooperation index. For the constrained quadratic programming optimization problem with the objective, the following more detailed steps are taken: Step 3.1: Model the relationship between cooperative error and control input.

[0052] First, the relationship between robot joint torques and end effector motion is derived, and the optimization variable is defined as the control torque vector of each robot joint. The relationship between robot joint torque and end effector motion is as follows: ; in, , , These represent the angle, velocity, and acceleration of each joint, respectively. Represents the inertia matrix; Represents the matrix of Coriolis force and centrifugal force; Represents the gravity vector; This indicates the joint torque.

[0053] By transforming the formula, we can obtain The acceleration of each joint at time t is : ; In the formula, Represents the inertia matrix. Indicates joint torque. Represents the matrix of Coriolis force and centrifugal force. This indicates the current speed of each joint. Represents the gravity vector; Using the difference approximation, we can obtain... The velocities of each joint at each moment: ; in, Indicates the system's control cycle. express The velocities of each joint at any given moment. express The velocities of each joint at any given moment. express The acceleration of each joint at any given moment; Calculate the Jacobian-Jacobi matrix based on the current joint state. Project the joint velocities onto the end effector coordinate system: ; In the formula, express The velocity of the coordinate system at the end of time. Indicates the joint status as Jacobian matrix at time, express The velocities of each joint at any given moment; Based on the above calculation process, the end position increment It can be approximated as the control torque vector of each joint of the robot. The equation: ; Among them, coefficient constant terms .

[0054] Indicates the system's control cycle. express The velocity of the coordinate system at the end of time. Indicates the joint status as Jacobian matrix at time, express The velocities of each joint at any given moment. Represents the matrix of Coriolis force and centrifugal force. This indicates the current speed of each joint. Indicates constant terms; Therefore, the control error can be uniformly expressed as the end-position increment. The expression for this is the control torque vector of each joint of the robot. The expression: ; in, This represents the impedance parameter. This represents the robot's reference trajectory after compliance correction. Indicates the increment of the end position. This represents the increment of the robot's reference trajectory after compliance correction. This indicates the synchronization error of the two arms' movements. This indicates the end effector position of one of the robot's two arms. This indicates the position of the end effector of the other robotic arm in a robot's two-arm design. This represents the increment of the end effector position of a single robotic arm in a dual-arm robot. This represents the increment of the end effector position of the other robotic arm in a robot's two-arm design. Indicates force tracking error, Represents the six-dimensional force at the end. Represents the reference end six-dimensional force. Represents the impedance coefficient matrix; Based on step 2, the collaboration index is modeled as follows: ; Substitute the increment from the end position The control error is uniformly represented and organized as follows: ; in, This represents the proportionality coefficient. This represents the increment coefficient.

[0055] Furthermore, the end-effector position increment is approximated as the control torque vector of each joint of the robot. The equation: Substituting the values, we get: ; in, The projection matrix of the error onto the torque; Indicates a constant offset term. This represents the proportionality coefficient. Represents the coefficient matrix. Represents constant terms. Indicates the increment coefficient; Step 3.2: Construction and Derivation of the Quadratic Programming Objective Function: The overall control objective is defined as minimizing the following objective function: ; in, Indicates the weight of the collaboration error; Indicates the error derivative weight; This represents the torque penalty term; all matrices are symmetric positive definite matrices. This represents the transpose of the cooperation index matrix. This represents the transpose of the differential of the cooperation index matrix. Represents the differential of the cooperation index matrix. This represents the transpose of the joint moment matrix. Represents the joint moment matrix; Based on the derivation in step 3.1: ; make: ; in, , .

[0056] Expand the objective function of the quadratic programming optimization and substitute it into: ; Expand all terms, ignoring the constant term: ; definition: ; Finally, the objective function is transformed into its standard quadratic form: ; Step 3.3: Unify the construction of inequality constraints with the representation of joint moments; To ensure that the control output meets the physical feasibility, safety, and task requirements of the robot system, the constraints in the control system are transformed into joint torques. The linear inequality constraint form: ; In the formula, Denotes the coefficients of linear inequalities. Indicates joint torque. Represents the threshold for linear inequalities; (1) Joint torque constraint; The original constraint of the joint moment is expressed as: ; In the formula, This indicates the minimum joint torque. Indicates joint torque. Indicates the maximum joint torque; Convert to linear inequality form: remember , ,but: In the formula, Represents an n-order identity matrix. This indicates the maximum joint torque. This indicates the minimum joint torque. This represents the joint torque threshold for linear inequalities. Denotes the coefficients of linear inequalities; (2) End velocity constraint; The original constraint on the end-effector velocity is expressed as requiring the robot's end-effector velocity to not exceed the upper limit allowed by the task. : ; Terminal velocity can be obtained through the Jacobian matrix. Obtained by multiplying by the joint velocity: ; From step 3.1, we can see that the joint velocity in one cycle The internal acceleration is obtained by integrating the joint acceleration: ; Substituting, we get: ; definition: ; in, This represents the mapping matrix from joint torque to end-effector velocity. It represents a constant that includes the state at the previous moment, gravity, Coriolis force and centrifugal force, and velocity term.

[0057] Will be determined by joint torque Substituting the robot's end-effector velocity into the original end-effector velocity constraints: ; Further organized as follows: ; remember , ,but: ; (3) End contact force constraint; The original constraint of the end contact force is expressed as: ; First, the force is modeled using an admittance model: ; in, , , These represent the stiffness coefficient, damping coefficient, and mass coefficient, respectively. Indicates the increment from position To force The mixed impedance parameters.

[0058] From step 3.1, we can see that the joint velocity in one cycle The internal acceleration is obtained by integrating the joint acceleration: ; Substituting, we get: ; Contact force Represented as: ; in, This represents the mapping matrix from joint torque to end-effector contact force; This indicates the state at the previous moment, gravity, Coriolis force and centrifugal force, velocity term and impedance parameter. The constant.

[0059] Will be determined by joint torque Substituting the robot end-effector contact force into the original end-effector contact force constraint: ; Further organized as follows: ; remember , ,but: ; Step 3.4: Standardize the quadratic programming problem; The optimization variable is the control torque vector of each joint of the robot. The quadratic programming problem has the following standard form: ; in, and The solution is obtained from step 3.2 above.

[0060] Unify all the above inequality constraints as follows: ; in, , The solution is obtained from step 3.3 above.

[0061] Step 4 above specifically includes: calling the QP solver to solve the joint control input in real time, thereby realizing multi-mode collaborative control.

[0062] Within each control cycle, the controller constructs a quadratic programming problem with control inputs as optimization variables and the objective function minimizing the cooperation exponent, based on the current state, task requirements, and physical constraints. The control system preferably uses the OSQP solver to solve the quadratic programming problem in real time. It employs the ADMM method (Alternating Direction Multiplier Method), supports warm-start and sparse matrix acceleration, and can stably complete the calculation within the control cycle. Each cycle, the controller calls the OSQP solver to generate the control torque vectors for each joint of the robot. The calculation is performed, and the optimal result is sent to the motors of each joint. Finally, the robot uses torque control to complete the corresponding action.

[0063] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A cooperative control method for a dual-arm robot based on a cooperative index and quadratic programming, characterized in that, include: A collaborative index model is constructed, which integrates trajectory tracking error, dual-arm synchronization error and end-effector contact force error, and introduces dynamic weighting factors to continuously switch control modes. Based on the cooperation index model, admittance control is used to generate a compliant offset trajectory, and the compliant offset trajectory is used as a trajectory correction term to update the cooperation index model. To minimize the collaboration index, a quadratic programming problem is constructed that includes constraints on joint torque, end velocity, and contact force. The quadratic programming problem is solved in real time during each control cycle to obtain the optimal joint control torque and output it to the dual-arm robot.

2. The method according to claim 1, characterized in that, The process of building a collaboration index model includes: Obtain the trajectory tracking error of each arm, the motion synchronization error between the two arms, and the end contact force error at the current moment; The collaborative weighting factor and the force weighting factor are dynamically adjusted according to the task phase. The trajectory tracking error, the motion synchronization error between the two arms, and the end contact force error are linearly fused with the cooperation weighting factor and the force weighting factor to obtain the cooperation index.

3. The method according to claim 1, characterized in that, The process of dynamically adjusting the collaborative weighting factor and the force weighting factor includes: If both the collaborative weighting factor and the force weighting factor are 0, then the trajectory tracking mode is set. If the coordination weight factor is greater than 0 and the force weight factor is 0, then the mode is set to dual-arm synchronous movement. If both the collaborative weighting factor and the force weighting factor are greater than 0, then the force-position hybrid control mode is set. If the collaborative weighting factor is 0 and the force weighting factor is greater than 0, then the force priority control mode is set.

4. The method according to claim 1, characterized in that, The process of generating a compliant offset trajectory using admittance control includes: Obtain the force error between the actual contact force and the expected contact force at the robot's end effector; Input the force error into the admittance model to calculate the desired acceleration of the mass-damped-spring system. Discrete integration is performed on the desired acceleration to obtain the compliant offset trajectory.

5. The method according to claim 1, characterized in that, The process of updating the cooperation index model using the compliant offset trajectory as a trajectory correction term includes: The compliant offset trajectory is superimposed on the original desired trajectory to obtain the corrected reference trajectory; The trajectory tracking error is recalculated based on the corrected reference trajectory; Replace the original trajectory tracking error with the aforementioned trajectory tracking error to reconstruct the cooperation index.

6. The method according to claim 1, characterized in that, The process of constructing a quadratic programming problem with constraints on joint torque, end-effector velocity, and contact force, with the objective of minimizing the collaboration exponent, includes: Establish a linear mapping relationship between joint torque and end-effector position increment; The cooperation index is expressed as a linear function of joint torque; Construct a quadratic objective function with joint torque as the optimization variable.

7. The method according to claim 6, characterized in that, The process of constructing a quadratic objective function with joint torque as the optimization variable includes: Based on the mapping relationship between the cooperation index and the joint torque, calculate the coefficients of the quadratic and linear terms of the objective function; Ignoring the constant term, we obtain the standard quadratic programming form.

8. The method according to claim 1, characterized in that, The process of constructing a quadratic programming problem that includes constraints on joint torques, end-effector velocities, and contact forces also includes: The joint torque limit, end velocity limit, and end contact force limit are written as linear inequalities, and then combined into a unified linear inequality constraint.

9. The method according to claim 1, characterized in that, The process of solving the quadratic programming problem in real time within each control cycle includes: Within each control cycle, the objective function coefficients and constraints are updated based on the current robot state. The solver is invoked to solve the standard quadratic programming problem online and obtain the optimal joint torque; The optimal joint torque is output to each joint actuator to achieve torque control.

10. The method according to claim 1, characterized in that, The method also includes cyclically executing the cooperative index model, admittance control, and quadratic programming problem in the same controller to form a closed-loop multi-mode cooperative control.

Images