Double-arm robot real-time cooperative control system based on constraint optimization

Abstract

The invention discloses a double-arm robot real-time cooperative control system based on constraint optimization. The system is composed of a double-arm collaborative optimization layer and a safety control layer, task coordination and obstacle avoidance are combined, and collaborative control of a double-arm robot under the condition that safety constraint conditions are met is achieved. According to the method, accurate cooperation and obstacle avoidance control of two arms in a complex environment can be realized, and the method is suitable for a multi-task and multi-constraint robot cooperation control scene. Meanwhile, the method has good real-time performance, and an efficient and safe control strategy is provided for the double-arm robot system.

Description

Technical Field

[0001] This invention belongs to the field of artificial intelligence and robotics technology, specifically relating to a real-time collaborative control system for a dual-arm robot based on constraint optimization. Background Technology

[0002] With the continuous advancement of industrial automation and intelligent manufacturing, dual-arm robots, thanks to their highly flexible structure and excellent collaborative capabilities, have been widely used in complex scenarios such as assisted operation, collaborative assembly, and human-robot interaction. Compared with traditional single-arm systems, dual-arm systems are more suitable for tasks requiring high coordination and precision, significantly improving the robot's adaptability in multi-task parallel processing and highly complex operations.

[0003] In practical applications, dual-arm robots often need to complete collaborative tasks in dynamic environments while ensuring system safety and avoiding potential collisions with surrounding obstacles. Therefore, their control systems not only need trajectory tracking capabilities but also real-time obstacle avoidance and safety assurance capabilities. Especially when robotic arms work collaboratively, the movement of one arm can affect the behavior of the other, making traditional single-arm obstacle avoidance methods insufficient for the needs of dual-arm collaborative control.

[0004] Existing obstacle avoidance technologies mainly include responsive methods based on artificial potential fields and optimization-based control methods. Artificial potential field methods plan motion paths by simulating attraction and repulsion, but these methods are prone to problems such as unreachable targets and getting trapped in local minima, and their adaptability to dynamic obstacles is weak. Optimization control methods, such as model predictive control, can handle more complex dynamic environments and constraints, but they typically have high computational costs, poor real-time performance, and are difficult to apply to high-degree-of-freedom dual-arm systems. In recent years, the Control Barrier Function (CBF) method has been increasingly applied to robot obstacle avoidance control due to its good mathematical properties and theoretical feasibility. Its higher-order extension—Higher-Order Control Barrier Function (HoCBF)—can be used to handle systems with high relative orders and is suitable for safety constraint control of multi-joint robotic arms. However, existing CBF and HoCBF methods are mostly applied to single-arm robots, lacking consideration for constraint consistency and coordination mechanisms in dual-arm cooperative behavior, making it difficult to ensure the overall safety and task continuity of the dual-arm system during cooperation.

[0005] To address the issue of dual-arm collaboration, existing technologies have proposed several shared control or learning-based methods to improve collaborative capabilities. However, these methods often lack a unified safety control mechanism or rely on centralized computing structures, which is detrimental to the stable deployment of the system in complex environments. Furthermore, some methods only focus on obstacle avoidance by the end effector, neglecting the motion safety of the intermediate joints of the robotic arm, thus leaving potential collision risks. Summary of the Invention

[0006] To overcome the shortcomings of existing technologies, this invention provides a constraint optimization-based real-time collaborative control system for dual-arm robots, which solves the technical problems of existing master-slave dual-arm robots in avoiding obstacles while ensuring the motion safety of the intermediate joints of the robotic arms, and in ensuring the overall safety and task continuity of the dual-arm system during the collaborative process.

[0007] To achieve the above objectives, the specific technical solution of the present invention is as follows:

[0008] A constraint optimization-based real-time collaborative control system for a dual-arm robot includes the following steps:

[0009] Step S1: Construct the error cost function between the initial reference trajectory and actual pose of the master arm and slave arm, and define the cooperative constraints between the two arms.

[0010] Step S2: The cooperative constraints are formalized into linear equality constraints, and the internal forces / torques are converted into end attitude correction quantities through admittance control, thereby realizing the dynamic update of the compliant cooperative constraints.

[0011] Step S3: Jointly model the trajectory tracking and cooperative constraints to solve the weighted optimization problem. Use ADMM to iteratively solve the expected trajectory of the master arm, the expected trajectory of the slave arm, and the Lagrange multiplier to obtain the expected trajectory of both arms that meets the cooperative requirements.

[0012] Step S4: Establish a safety control layer by modeling the key joints of the main arm as spheres and defining the minimum safe distance between each key point and the obstacle by combining forward kinematics and obstacle geometry models.

[0013] Step S5: Design HoCBF for key joints of the main boom, construct time-varying inequality constraints on the control input, and transform them into linear constraints on the control torque.

[0014] Step S6: The reference trajectory generated by the collaborative optimization layer is converted into a reference torque, and combined with safety constraints, the optimal safe control torque of the main arm is solved through QP optimization; the slave arm control directly tracks the desired trajectory to generate the control torque. The final control input of the main arm and the control input of the slave arm are respectively applied to the joint actuators of the dual-arm robot to achieve safe cooperative motion, wherein the constraints of the safety control layer take precedence over the objectives of the collaborative control layer.

[0015] Furthermore, in step S1, the error cost function f(x) between the initial reference trajectory and the actual pose of the master arm and slave arm is... e,1 ) and g(x e,2 As shown in Formula 1 and Formula 2:

[0016]

[0017] Where, xe,1 Current pose of the end effector of the main arm, x r,1 Initial reference trajectory of the main arm end effector, x e,2 To determine the current pose of the end effector, x r,2 This is the initial reference trajectory from the end effector of the arm.

[0018] Furthermore, the cooperative constraint between the master arm and slave arm in step S2 represents the desired relationship between the relative poses of the end effectors of the master arm and slave arm. According to the cooperative task requirements, the cooperative constraint between the two end effectors is expressed as a linear equality constraint, as shown in Formula 3:

[0019] T1x e,1 +T2x e,2 =c (3)

[0020] Where T1 and T2 are the transformation matrices of the master arm and slave arm, respectively, used to map the end states of each arm to a unified cooperative constraint space. c is the desired cooperative state.

[0021] Furthermore, admittance control is introduced in step S2 to control the internal force / torque W acting on the two robotic arms. int Transformed into a cooperative correction quantity c through the admittance model adj Specifically, as shown in Formula 4:

[0022]

[0023] Among them, M c B c and K c W represents the inertia matrix, damping matrix, and stiffness matrix, respectively. int =[F T T T ] T W represents the force and torque acting on the cooperative correction variable. o,1 and W o,2 P represents the force applied at the ends of both arms. o,1 and P o,2 The adjustment matrix for maneuvering the two arms is determined based on the actual clamping configuration. At this point, the cooperative constraint is updated to T1x. e,1 +T2x e,2 =c r , where c r =c+c adj .

[0024] Furthermore, the specific calculation steps of step S3 are as follows:

[0025] Step S3.1: Solve the weighted optimization problem using ADMM, as shown in Equation 5:

[0026]

[0027] Here, α and β are the weighting coefficients of the master arm and slave arm, respectively, used to balance the importance of trajectory tracking between the two robotic arms.

[0028] Step S3.2, construct the augmented Lagrangian function, as shown in Equation 6:

[0029]

[0030] Where λ is a Lagrange multiplier, used to introduce constraints into the objective function, and a penalty term ensures that the constraints are satisfied. ρ is a penalty parameter, controlling the intensity of the penalty for the constraints during iteration.

[0031] Furthermore, the ADMM is solved through the following iterative steps:

[0032] Step S3.1.1: Update the desired trajectory of the main arm. fixed and λ k Seeking answers regarding The sub-optimization problem is solved using the formula shown in Formula 7:

[0033]

[0034] Step S3.1.2, update the desired trajectory from the arm. fixed and λ k Seeking answers regarding The sub-optimization problem is solved using the formula shown in Formula 8:

[0035]

[0036] Step S3.1.3, update the Lagrange multiplier λ k+1 :

[0037]

[0038] Step S3.1.4: Repeat steps S3.1.1 to S3.1.3, iterating continuously until the original residual is found. and dual residual The norms of all are less than the preset tolerance ∈ p and ∈ d :

[0039]

[0040] in:

[0041]

[0042] η abs and η rel These are the preset absolute and relative tolerances. The original residuals reflect the degree to which the current solution violates the constraints. When ||r|| k When ||s|| approaches zero, it indicates that the constraints are basically satisfied. The dual residual reflects the changing trend of the optimization variables. k When the value approaches zero, it indicates that the optimization variables have stabilized and the algorithm is close to convergence. The penalty parameter ρ of ADMM can be adaptively adjusted according to the magnitude of the original residual and the dual residual to accelerate convergence and improve robustness. Finally, the optimized output satisfies the desired end-effector trajectory of the cooperative constraints. and

[0043] Furthermore, in step S4, the i-th joint of the main arm is approximated as a sphere, and the position of the i-th safety critical point of the main arm in the world coordinate system is set to p. i This point passes through the forward kinematics p i =fk i (q) is obtained, where q is the joint angle vector of the main arm, and fk i This represents the forward kinematic mapping of the robotic arm. Simultaneously, it performs geometric modeling of obstacles in the environment.

[0044] Furthermore, in step S5, HoCBF is introduced during the design for the i-th joint of the main arm, as shown in Formula 14:

[0045]

[0046] obstacle p obs,i The location can be obtained through actual measurement. This h i The relative order of the control input is typically 2. This is to ensure that the system's safe state always remains within the safe set h. i (t,p i For values ​​≥ 0, the following constraint inequality 15 must be satisfied:

[0047]

[0048] Where u is the control input, and This is the Lie derivative. K0 and K1 are positive definite constants used to control the rate of convergence towards the interior of the safe boundary.

[0049] Furthermore, the calculation of the derivatives in step S5 depends on the affine nonlinear system of the robotic arm, as shown in Equation 16:

[0050]

[0051] Wherein, function f i (x i ) and g i (x i Local Lipschitz continuity, where x represents the nonlinear drift vector field and nonlinear control vector field of the i-th joint system of the robotic arm, respectively. i F represents the system state of the i-th joint. i M represents the control input of the i-th joint system in Cartesian space. x,i C x,i and G x,i Let f(x) represent the inertia matrix, Coriolis force matrix, and gravity matrix of the i-th joint system in Cartesian space, respectively. Let be the pseudo-inverse of the Jacobian matrix of the i-th joint system, and τ be the input torque of the robotic arm. Ultimately, this inequality can be transformed into a linear constraint form A concerning the actual control torque τ of the main arm. i τ≤B i .

[0052] Furthermore, the QP problem constructed in step S5 is shown in Equation 17:

[0053]

[0054] in:

[0055]

[0056]

[0057] Where, p e,i,j p represents the j-th component of the position vector of the i-th joint of the main arm. obs,j f represents the j-th component of the obstacle position vector. i,j (x) represents the j-th component of the nonlinear drift vector field of the i-th joint of the main arm, g i,j (x) represents the j-th component of the nonlinear control vector field of the i-th joint of the main arm, τ max This indicates the maximum constraint limit on the control input of the main arm.

[0058] Compared with the prior art, the present invention has the following advantages:

[0059] 1. This invention, starting from kinematics, establishes a cooperative constraint model between the two arms. Using Active Dynamic Model (ADMM), the cooperative problem is divided into sub-problems for the master arm and slave arm, which are solved separately to coordinate the behavior of the two robotic arms. When the motion state of a single robotic arm changes, even if the other robotic arm is not directly affected, it can adaptively adjust based on the ADMM's cooperative mechanism to maintain cooperative consistency, thus enhancing the system's task continuity and cooperative stability in obstacle environments.

[0060] 2. This invention introduces HoCBF (Homogeneous-Conceptual-Functional) at the control layer to model the minimum safe distance between each joint of the robotic arm and external obstacles. By constructing HoCBF inequality constraints that satisfy system safety requirements and embedding them into the QP (Quickness-Based Practicality) framework, it ensures that the control input always keeps the system within a strictly defined safe feasible region. Furthermore, this invention clearly defines a safety-priority control hierarchy at the control layer. When the collaborative task objective conflicts with safety constraints, the controller prioritizes the safety conditions, ensuring the absolute safety of system operation. This invention is applicable to safe collaborative operation of dual-arm robots under multiple constraints, improving the safety and practicality of the control system. Attached Figure Description

[0061] Figure 1 This is a flowchart illustrating the control method of the present invention;

[0062] Figure 2 This is a schematic diagram of the collaborative principle in this invention;

[0063] Figure 3 This is a schematic diagram of the three-dimensional trajectory of the two arms in this invention;

[0064] Figure 4 This is a diagram illustrating the trajectory tracking effect of the main arm in this invention.

[0065] Figure 5 This is a diagram illustrating the effect of slave arm trajectory tracking in this invention.

[0066] Figure 6 This is a comparison diagram of the distances between each joint of the main arm and the obstacle under and without constraints in this invention.

[0067] Figure 7 This is a diagram showing the HoCBF variation of each joint in the main arm of the present invention;

[0068] Figure 8 This is a simulation effect diagram of the present invention. Detailed Implementation

[0069] In the description of this invention, it should be understood that the terms "one end", "the other end", "outer side", "upper", "inner side", "horizontal", "coaxial", "center", "end", "length", "outer end", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing this invention and simplifying the description, and are not intended to indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this invention.

[0070] The invention will now be further described with reference to the accompanying drawings.

[0071] like Figure 1 and 2As shown, a real-time collaborative control system for a dual-arm robot based on constraint optimization includes the following steps:

[0072] Step S1: Construct the error cost function between the initial reference trajectory and actual pose of the master arm and slave arm, and define the cooperative constraints (relative pose relationship) between the two arms to support the cooperative operation required by the task.

[0073] Here, the current pose x of the end effector of the main arm is defined. e,1 Initial reference trajectory x of the end effector of the main arm r,1 Define the current pose x of the end effector. e,2 From the initial reference trajectory x of the end effector of the arm r,2 . Figure 2 This illustration demonstrates the relative pose relationship between the end effectors of two robotic arms. A cost function is defined for each arm to quantify the deviation between its actual end effector trajectory and a reference trajectory. The cost function f(x) represents the error between the initial reference trajectory and the actual pose of the master and slave arms. e,1 ) and g(x e,2 As shown in Formula 1 and Formula 2:

[0074]

[0075] Step S2: The cooperative constraints are formalized into linear equality constraints, and the internal forces / torques are converted into end attitude correction quantities through admittance control, thereby realizing the dynamic update of the compliant cooperative constraints.

[0076] Specifically, the cooperation constraint between the two arms represents the desired relationship between the relative poses of the end effectors of the master and slave arms. Based on the requirements of the cooperation task, the cooperation constraint between the end effectors of the two arms is expressed as a linear equality constraint, as shown in Equation 3:

[0077] T1x e,1 +T2x e,2 =c (3)

[0078] Where T1 and T2 are the transformation matrices of the master arm and slave arm, respectively, used to map the end states of each arm to a unified cooperative constraint space. c is the desired cooperative state.

[0079] To enhance the compliance of the collaboration, admittance control is introduced, which controls the internal force / torque W acting on the two robotic arms. int Transformed into a cooperative correction quantity c through the admittance model adj Specifically, as shown in Formula 4:

[0080]

[0081] Among them, M c B c and Kc W represents the inertia matrix, damping matrix, and stiffness matrix, respectively. int =[F T T T ] T W represents the force and torque acting on the cooperative correction variable. o,1 and W o,2 P represents the force applied at the ends of both arms. o,1 and P o,2 The adjustment matrix for maneuvering the two arms is determined based on the actual clamping configuration. At this point, the cooperative constraint is updated to T1x. e,1 +T2x e,2 =c r , where c r =c+c adj .

[0082] Step S3 involves jointly modeling trajectory tracking and cooperative constraints to solve a weighted optimization problem. ADMM is used iteratively to solve for the desired trajectory of the master arm, the desired trajectory of the slave arm, and the Lagrange multipliers, yielding the desired trajectories of both arms that meet the cooperative requirements. The specific calculation steps are as follows:

[0083] Step S3.1: Solve the weighted optimization problem using ADMM, as shown in Equation 5:

[0084]

[0085] Here, α and β are the weighting coefficients of the master arm and slave arm, respectively, used to balance the importance of trajectory tracking between the two robotic arms.

[0086] Step S3.2, construct the augmented Lagrangian function, as shown in Equation 6:

[0087]

[0088] Here, λ is a Lagrange multiplier used to introduce constraints into the objective function, and a penalty term is used to ensure that the constraints are satisfied. ρ is a penalty parameter that controls the intensity of the penalty for the constraints during the iteration process.

[0089] In this embodiment, ADMM is solved through the following iterative steps:

[0090] Step S3.1.1: Update the desired trajectory of the main arm. fixed and λ k Seeking answers regarding The sub-optimization problem is solved using the formula shown in Equation 7. Since the cost function is quadratic and the constraints are linear, this sub-problem has an analytical solution.

[0091]

[0092] Step S3.1.2, update the desired trajectory from the arm. fixed and λ k Seeking answers regarding The sub-optimization problem has a specific solution formula as shown in Formula 8, and it also has an analytical solution.

[0093]

[0094] Step S3.1.3, update the Lagrange multiplier λ k+1 :

[0095]

[0096] Step S3.1.4: Repeat steps S3.1.1 to S3.1.3, iterating continuously until the original residual is found. and dual residual The norms of all are less than the preset tolerance ∈ p and ∈ d :

[0097]

[0098] in:

[0099]

[0100] η abs and η rel These are the preset absolute and relative tolerances. The original residuals reflect the degree to which the current solution violates the constraints. When ||r|| k When ||s|| approaches zero, it indicates that the constraints are basically satisfied. The dual residual reflects the changing trend of the optimization variables. k When the value approaches zero, it indicates that the optimization variables have stabilized and the algorithm is close to convergence. The penalty parameter ρ of ADMM can be adaptively adjusted according to the magnitude of the original residual and the dual residual to accelerate convergence and improve robustness. Finally, the optimized output satisfies the desired end-effector trajectory of the cooperative constraints. and

[0101] Dual-arm trajectory tracking effect passed Figure 3 , Figure 4 , Figure 5 It was showcased. Figure 3 The demonstration showed the trajectory tracking effect of two robotic arms in three-dimensional space. Figure 4 The results demonstrate the trajectory tracking performance of the main arm in the x, y, and z axes. Figure 5 This demonstrates the trajectory tracking effect of the arm in the x, y, and z axes respectively. Figure 3In the diagram, color changes represent changes over time, and three axes represent the position of the robotic arm's end effector in space. Figure 4 and Figure 5 In the diagram, the horizontal axis represents time, and the vertical axis represents the position of the robotic arm's end effector in that axis direction.

[0102] Step S4: Establish a safety control layer to ensure that the main arm tracks the desired trajectory generated by the cooperative constraint optimization layer. At the same time, all its key joints can maintain a safe distance from obstacles in the environment.

[0103] Furthermore, the key joints of the main arm are modeled as spheres. Combining forward kinematics and obstacle geometry, the minimum safe distance between each key point and the obstacle is defined. Specifically, the i-th joint of the main arm is approximated as a sphere, and the position of the i-th safety key point (the center of the joint sphere) in the world coordinate system is set to p. i This point passes through the forward kinematics p i =fk i (q) is obtained, where q is the joint angle vector of the main arm, and fk i This represents the forward kinematic mapping relationship of the robotic arm.

[0104] Simultaneously, geometric modeling is performed on obstacles in the environment, such as the distance p from the surface of a cylindrical pipe obstacle. i Let p be the nearest point. obs,i For each keypoint p i Define a minimum safe distance d safe,i Then the safe state space φ i The description is shown in Formula 13:

[0105]

[0106] Step S5: Design HoCBF for key joints of the main boom, construct time-varying inequality constraints on the control input, and transform them into linear constraints on the control torque.

[0107] Specifically, for the i-th joint of the main arm, HoCBF is introduced in the design, as shown in Formula 14:

[0108]

[0109] obstacle p obs,i The location can be obtained through actual measurement. This h i The relative order of the control input is typically 2. This is to ensure that the system's safe state always remains within the safe set h. i (t,p i For values ​​≥ 0, the following constraint inequality 15 must be satisfied:

[0110]

[0111] Where u is the control input, and These are the Lie derivatives. K0 and K1 are positive definite constants used to control the rate of convergence towards the safety boundary. The calculation of each derivative depends on the affine nonlinear system of the robotic arm, as shown in Equation 16:

[0112]

[0113] Wherein, function f i (x i ) and g i (x i Local Lipschitz continuity, where x represents the nonlinear drift vector field and nonlinear control vector field of the i-th joint system of the robotic arm, respectively. i F represents the system state of the i-th joint. i M represents the control input of the i-th joint system in Cartesian space. x,i C x,i and G x,i Let f(x) represent the inertia matrix, Coriolis force matrix, and gravity matrix of the i-th joint system in Cartesian space, respectively. Let be the pseudo-inverse of the Jacobian matrix of the i-th joint system, and τ be the input torque of the robotic arm. Ultimately, this inequality can be transformed into a linear constraint form A concerning the actual control torque τ of the main arm. i τ≤B i .

[0114] In order to track the boom reference torque τ generated by the upper controller as closely as possible while satisfying all safety constraints. r The QP problem is constructed as shown in Formula 17:

[0115]

[0116] in:

[0117]

[0118] Where, p e,i,j p represents the j-th component of the position vector of the i-th joint of the main arm. obs,j f represents the j-th component of the obstacle position vector. i,j (x) represents the j-th component of the nonlinear drift vector field of the i-th joint of the main arm, g i,j (x) represents the j-th component of the nonlinear control vector field of the i-th joint of the main arm, τ max This indicates the maximum constraint limit on the control input of the main arm.

[0119] Step S6: The reference trajectory generated by the collaborative optimization layer is converted into a reference torque, and combined with safety constraints, the optimal safe control torque of the main arm is solved through QP optimization; the slave arm control directly tracks the desired trajectory to generate the control torque. The final control input of the main arm and the control input of the slave arm are respectively applied to the joint actuators of the dual-arm robot to achieve safe cooperative motion, wherein the constraints of the safety control layer take precedence over the objectives of the collaborative control layer.

[0120] Specifically, the desired trajectory from the arm obtained by the collaborative optimization layer... The input standard trajectory tracking controller generates the control torque of the slave arm. The desired trajectory of the main arm obtained from the collaborative optimization layer The input standard trajectory tracking controller generates the control reference torque τ of the main boom. r , will τ r The optimal safety control torque of the main boom is obtained by solving Formula 17 in step S5. Will and The input control is applied to the joint actuators of the master arm and slave arm respectively. At this time, the input control simultaneously meets the requirements of both cooperation and safety, and the constraints of the safety control layer take precedence over the objectives of the cooperative optimization layer.

[0121] Please refer to the safety control effect of this method. Figure 6 and Figure 7 , Figure 6 This demonstrates the distances between each joint of the main arm and the obstacle, with and without safety constraints. Figure 7 The simulation demonstrates the HoCBF changes of each joint of the main arm during the mission. It can be observed that after applying safety constraints, the HoCBF of each key joint of the main arm is non-negative, indicating that the distance to the obstacle is greater than the preset safety distance and remains within the safe range. The simulation results of this method are as follows: Figure 8 As shown, under safe conditions, the dual-arm system successfully completed the set collaborative tasks.

[0122] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A real-time collaborative control system for a dual-arm robot based on constraint optimization, characterized in that: Includes the following steps: Step S1: Construct the error cost function between the initial reference trajectory and actual pose of the master arm and slave arm, and define the cooperative constraints between the two arms; Step S2: The cooperative constraints are formalized into linear equality constraints, and the internal forces / torques are converted into end attitude corrections through admittance control, thereby realizing the dynamic update of the compliant cooperative constraints. Step S3: Jointly model the trajectory tracking and cooperative constraints to solve the weighted optimization problem. Use ADMM to iteratively solve the expected trajectory of the master arm, the expected trajectory of the slave arm, and the Lagrange multiplier to obtain the expected trajectory of both arms that meets the cooperative requirements. Step S4: Establish a safety control layer by modeling the key joints of the main arm as spheres and defining the minimum safe distance between each key point and the obstacle by combining forward kinematics and obstacle geometry model. Step S5: Design HoCBF for key joints of the main boom, construct time-varying inequality constraints on the control input, and transform them into linear constraints on the control torque. At the same time, track the main boom reference torque generated by the upper controller and construct the QP problem. Step S6: The reference trajectory generated by the collaborative optimization layer is converted into a reference torque, and combined with safety constraints, the optimal safety control torque of the main boom is solved by QP optimization. The arm control directly tracks the desired trajectory to generate control torque; The final control input of the main arm and the control input of the slave arm are respectively applied to the joint actuators of the dual-arm robot to achieve safe cooperative motion, wherein the constraints of the safety control layer take precedence over the objectives of the cooperative control layer.

2. The real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 1, characterized in that: In step S1, the error cost function f(x) between the initial reference trajectory and the actual pose of the master arm and slave arm is... e,1 ) and g(x e,2 As shown in Formula 1 and Formula 2: Where, x e,1 Current pose of the end effector of the main arm, x r,1 Initial reference trajectory of the main arm end effector, x e,2 To determine the current pose of the end effector, x r,2 This is the initial reference trajectory from the end effector of the arm.

3. The real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 2, characterized in that: The cooperative constraint between the master arm and slave arm in step S2 represents the desired relationship between the relative poses of the end effectors of the master arm and slave arm; according to the cooperative task requirements, the cooperative constraint between the two end effectors is represented as a linear equality constraint, as shown in Formula 3: T1x e,1 +T2x e,2 =c (3) Where T1 and T2 are the transformation matrices of the master arm and slave arm, respectively, used to map the end states of each arm to a unified cooperative constraint space; c is the desired cooperative state.

4. The real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 3, characterized in that: In step S2, admittance control is introduced to control the internal force / torque W acting on the two robotic arms. int Transformed into a cooperative correction quantity c through the admittance model adj Specifically, as shown in Formula 4: Among them, M c B c and K c W represents the inertia matrix, damping matrix, and stiffness matrix, respectively. int =[F T T T ] T W represents the force and torque acting on the cooperative correction variable. o,1 and W o,2 P represents the force applied at the ends of both arms. o,1 and P o,2 The adjustment matrix for maneuvering the two arms is determined based on the actual clamping configuration; at this point, the cooperative constraint is updated to T1x. e,1 +T2x e,2 =c r , where c r =c+c adj .

5. A real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 4, characterized in that: The specific calculation steps for step S3 are as follows: Step S3.1: Solve the weighted optimization problem using ADMM, as shown in Equation 5: Where α and β are the weighting coefficients of the master arm and slave arm, respectively, used to balance the importance of trajectory tracking between the two robotic arms; Step S3.2, construct the augmented Lagrangian function, as shown in Equation 6: Where λ is a Lagrange multiplier used to introduce constraints into the objective function, and the constraint satisfaction is ensured through a penalty term; ρ is a penalty parameter that controls the penalty intensity of the constraints during the iteration process.

6. The real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 5, characterized in that: The ADMM is solved through the following iterative steps: Step S3.1.1: Update the desired trajectory of the main arm. fixed and λ k Seeking answers regarding The sub-optimization problem is solved using the formula shown in Formula 7: Step S3.1.2, update the desired trajectory from the arm. fixed and λ k Seeking answers regarding The sub-optimization problem is solved using the formula shown in Formula 8: Step S3.1.3, update the Lagrange multiplier λ k+1 : Step S3.1.4: Repeat steps S3.1.1 to S3.1.3, iterating continuously until the original residual is found. and dual residual The norms of all are less than the preset tolerance ∈ p and ∈ d : in: η abs and η rel These are the preset absolute and relative tolerances; the original residuals reflect the degree to which the current solution violates the constraints; when ||r k When || is close to zero, it indicates that the constraints are basically satisfied; the dual residual reflects the changing trend of the optimization variables; when ||s k When the value approaches zero, it indicates that the optimization variables have stabilized and the algorithm is close to convergence. The penalty parameter ρ of ADMM can be adaptively adjusted according to the magnitude of the original residual and the dual residual to accelerate convergence and improve robustness. Finally, the optimized output satisfies the desired end effector trajectory of the cooperative constraints. and 7. A real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 6, characterized in that: In step S4, the i-th joint of the main arm is approximated as a sphere, and the position of the i-th safety critical point of the main arm in the world coordinate system is set to p. i This point passes through the forward kinematics p i =fk i (q) is obtained, where q is the joint angle vector of the main arm, and fk i The forward kinematic mapping of the robotic arm is represented; simultaneously, geometric modeling of obstacles in the environment is performed; then the safety state space φ i The description is shown in Formula 13: Where, p obs,i The distance p from the obstacle surface i The nearest point; d safe,i For each safety critical point p i A minimum safe distance is defined.

8. A real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 7, characterized in that: In step S5, HoCBF is introduced during the design for the i-th joint of the main arm, as shown in Formula 14: obstacle surface distance p i The nearest point p obs,i The location can be obtained through actual measurement; this h i The relative order of the control input is typically 2; this is to ensure that the system's safe state always remains within the safe set h. i (t,p i For values ​​≥ 0, the following constraint inequality 15 must be satisfied: Where u is the control input, and K is the Lie derivative; K0 and K1 are positive definite constants used to control the rate of convergence toward the interior of the safe boundary.

9. A real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 8, characterized in that: The calculation of the derivatives in step S5 depends on the affine nonlinear system of the robotic arm, as shown in Equation 16: Wherein, function f i (x i ) and g i (x i Local Lipschitz continuity, where x represents the nonlinear drift vector field and nonlinear control vector field of the i-th joint system of the robotic arm, respectively. i F represents the system state of the i-th joint. i M represents the control input of the i-th joint system in Cartesian space; x,i C x,i and G x,i Let represent the inertia matrix, Coriolis force matrix, and gravity matrix of the i-th joint system in Cartesian space, respectively. Let be the pseudo-inverse of the Jacobian matrix of the i-th joint system, and τ be the input torque of the robotic arm; ultimately, this inequality can be transformed into a linear constraint form A with respect to the actual control torque τ of the main arm. i τ≤B i .

10. A real-time collaborative control system for a dual-arm robot based on constraint optimization according to claim 9, characterized in that: The QP problem constructed in step S5 is shown in Equation 17: in: Where, p e,i,j p represents the j-th component of the position vector of the i-th joint of the main arm. obs,j f represents the j-th component of the obstacle position vector. i,j (x) represents the j-th component of the nonlinear drift vector field of the i-th joint of the main arm, g i,j (x) represents the j-th component of the nonlinear control vector field of the i-th joint of the main arm, τ max This indicates the maximum constraint limit on the control input of the main arm.

Images