A cooperative control method and device of a dual-arm robot, a terminal device, and a storage medium

Abstract

The application discloses a kind of double-arm robot cooperative control method, device, terminal equipment and storage medium, belong to robot technical field, the method is, according to initial position, placement position, first initial angle and second initial angle, determine the target angle of each joint of left arm and right arm at each moment in the carrying process;In the carrying process, the actual angle of each joint on the left arm and right arm at the current moment is acquired;According to actual angle and target angle, the angle error of each joint is calculated, and according to the angle error, the first target torque of left arm and the second target torque of right arm are calculated;According to the first target torque and the second target torque, the first control instruction of left arm and the second control instruction of right arm are generated respectively, and left arm and right arm are controlled respectively.Therefore, by implementing the present application, the problem of lack of real-time perception and feedback of system state in the prior art can be solved, and dynamic disturbance cannot be responded.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a collaborative control method, apparatus, terminal device, and storage medium for a dual-arm robot. Background Technology

[0002] In modern industrial material handling scenarios, dual-arm robots are gradually becoming core equipment in logistics warehousing, precision assembly, and flexible manufacturing. As tasks become increasingly complex, industrial environments place higher demands on robots' control precision, environmental adaptability, and dynamic response speed. Dual-arm robots must ensure that the trajectory error of the end effector is controlled within an extremely small range to meet stringent process requirements such as precision docking and handling of fragile items. However, in actual material handling conditions, robotic arm systems often face coupled interference from multiple uncertainties, severely impacting the stability and reliability of control.

[0003] Specifically, these uncertainties mainly manifest in the following aspects: the lubrication state of the joint changes with temperature, wear, and operating time, leading to nonlinear abrupt changes in friction torque; unexpected collisions may occur in the working environment, such as contact with other equipment, workpieces, or operators; such external impacts can instantly introduce large-scale disturbances, posing a severe test to the robustness of the control system. Faced with the above complex operating conditions, traditional open-loop control strategies, lacking real-time perception and feedback of the system state, cannot respond to dynamic disturbances, easily leading to error accumulation and amplification, and even causing problems such as system instability, task failure, or equipment damage. Summary of the Invention

[0004] This invention provides a collaborative control method, device, terminal equipment, and storage medium for a dual-arm robot. The method can solve the problems in the prior art that lack real-time perception and feedback of system status and cannot respond to dynamic disturbances.

[0005] One embodiment of the present invention provides a cooperative control method for a dual-arm robot, comprising: Obtain the initial position and placement position of the object to be transported, the first initial angle of each joint of the left arm of the dual-arm robot, and the second initial angle of each joint of the right arm; Based on the initial position, placement position, first initial angle, and second initial angle, the carrying motion is planned for the left and right arms, and the target angles of each joint of the left and right arms are determined at each moment during the carrying process. Perform coordinated control operations at every moment during the handling process until the object to be handled is moved to its placement location; The coordinated control operation includes: Obtain the actual angles of each joint in the left and right arms at the current moment; Based on the actual angle and the target angle at the corresponding moment, calculate the angle error of each joint, and based on the angle error, calculate the first target torque of the left arm and the second target torque of the right arm; Based on the first target torque and the second target torque, the first control command for the left arm and the second control command for the right arm are generated respectively. The left and right arms are controlled according to the first and second control commands, respectively.

[0006] Furthermore, the angle error includes: tracking error and synchronization error; The step of calculating the angle error of each joint based on the actual angle and the target angle at the corresponding moment, and calculating the first target torque of the left arm and the second target torque of the right arm based on the angle error, includes: Based on the actual angle, calculate the synchronization error between each joint on the left arm and the corresponding joint on the right arm; Based on the actual angle and the target angle at the corresponding moment, calculate the tracking error of each joint during the transportation process; The tracking error is input into the preset state feedback controller of the dual-arm robot to generate the first basic torque of the left arm and the second basic torque of the right arm. Based on the synchronization error, the first basic torque and the second basic torque are corrected respectively to generate the first target torque of the left arm and the second target torque of the right arm.

[0007] Furthermore, the construction of the state feedback controller includes: Obtain the nonlinear dynamic control equations of the dual-arm robot; Construct an initial eigenvalue matrix and an initial free matrix; wherein the initial eigenvalue matrix is ​​a diagonal matrix and its elements are preset desired closed-loop poles, and the initial free matrix is ​​constructed based on preset linear control characteristics and an adjustable parameter; With the goal of maximizing the control stability of the dual-arm robot, the distribution of matrix elements in the initial eigenvalue matrix and the adjustable parameters of the initial free matrix are iteratively optimized to generate the target eigenvalue matrix and the target free matrix. Based on the target eigenvalue matrix and the target free matrix, a finite eigenvector matrix is ​​constructed to characterize the relationship between the linear control characteristics of the dual-arm robot and the desired closed-loop poles. Based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, a feedback gain matrix is ​​calculated to map the tracking error to linear control parameters, and a state feedback controller is constructed based on the feedback gain matrix.

[0008] Furthermore, the step of calculating the feedback gain matrix for mapping the tracking error to linear control parameters based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix includes: Calculate the closed-loop system matrix based on the target eigenvalue matrix and the finite eigenvector matrix; Calculate the parameter matrix based on the control parameter matrix and the finite eigenvector matrix in the dynamic control equations; Calculate the feedback gain matrix based on the finite eigenvector matrix, parameter matrix, and closed-loop system matrix.

[0009] An embodiment of the present invention also provides a cooperative control device for a dual-arm robot, comprising: The data acquisition module is used to acquire the initial position and placement position of the object to be transported, the first initial angle of each joint of the left arm of the dual-arm robot, and the second initial angle of each joint of the right arm. The motion planning module is used to plan the carrying motion of the left and right arms based on the initial position, placement position, first initial angle and second initial angle, and to determine the target angle of each joint of the left and right arms at each moment in the carrying process. The collaborative control module is used to perform collaborative control operations at every moment during the handling process until the object to be handled is moved to the placement position; The coordinated control operation includes: Obtain the actual angles of each joint in the left and right arms at the current moment; Based on the actual angle and the target angle at the corresponding moment, calculate the angle error of each joint, and based on the angle error, calculate the first target torque of the left arm and the second target torque of the right arm; Based on the first target torque and the second target torque, the first control command for the left arm and the second control command for the right arm are generated respectively. The left and right arms are controlled according to the first and second control commands, respectively.

[0010] Furthermore, the angle error includes: tracking error and synchronization error; The collaborative control module calculates the angle error of each joint based on the actual angle and the target angle at the corresponding moment, and calculates the first target torque of the left arm and the second target torque of the right arm based on the angle error, including: Based on the actual angle, calculate the synchronization error between each joint on the left arm and the corresponding joint on the right arm; Based on the actual angle and the target angle at the corresponding moment, calculate the tracking error of each joint during the transportation process; The tracking error is input into the preset state feedback controller of the dual-arm robot to generate the first basic torque of the left arm and the second basic torque of the right arm. Based on the synchronization error, the first basic torque and the second basic torque are corrected respectively to generate the first target torque of the left arm and the second target torque of the right arm.

[0011] Furthermore, it also includes: controller building modules; The controller construction module is used to obtain the nonlinear dynamic control equations of the dual-arm robot; Construct an initial eigenvalue matrix and an initial free matrix; wherein the initial eigenvalue matrix is ​​a diagonal matrix and its elements are preset desired closed-loop poles, and the initial free matrix is ​​constructed based on preset linear control characteristics and an adjustable parameter; With the goal of maximizing the control stability of the dual-arm robot, the distribution of matrix elements in the initial eigenvalue matrix and the adjustable parameters of the initial free matrix are iteratively optimized to generate the target eigenvalue matrix and the target free matrix. Based on the target eigenvalue matrix and the target free matrix, a finite eigenvector matrix is ​​constructed to characterize the relationship between the linear control characteristics of the dual-arm robot and the desired closed-loop poles. Based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, a feedback gain matrix is ​​calculated to map the tracking error to linear control parameters, and a state feedback controller is constructed based on the feedback gain matrix.

[0012] Furthermore, the controller construction module calculates a feedback gain matrix for mapping the tracking error to linear control parameters based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, including: Calculate the closed-loop system matrix based on the target eigenvalue matrix and the finite eigenvector matrix; Calculate the parameter matrix based on the control parameter matrix and the finite eigenvector matrix in the dynamic control equations; Calculate the feedback gain matrix based on the finite eigenvector matrix, parameter matrix, and closed-loop system matrix.

[0013] This application also provides a terminal device, including: One or more processors; A memory, coupled to the processor, for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement a collaborative control method for a dual-arm robot as described in the above embodiments of the invention.

[0014] This application also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements a cooperative control method for a dual-arm robot as described in the above embodiments.

[0015] The following benefits can be obtained by implementing the present invention: This invention provides a collaborative control method, device, terminal equipment, and storage medium for a dual-arm robot. The method first plans the handling actions of the left and right arms based on the initial position, placement position, first initial angle, and second initial angle, determining the target angles of each joint of the left and right arms at each moment during the handling process. Then, during the handling process, the actual angles of each joint are acquired in real time, and the angle error between the actual angle and the target angle is calculated. Based on the angle error of each joint, a first target torque for the left arm and a second target torque for the right arm are generated. Finally, a first control command for the left arm and a second control command for the right arm are generated to control the left and right arms respectively, achieving real-time perception and feedback of the robot system's state and effectively responding to dynamic disturbances during the handling process. Attached Figure Description

[0016] To more clearly illustrate the technical solution of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0017] Figure 1 This is a flowchart illustrating a collaborative control method for a dual-arm robot according to a certain embodiment of this application; Figure 2 This is a flowchart illustrating the collaborative control operation in a collaborative control method for a dual-arm robot according to a certain embodiment of this application. Figure 3 This is a schematic diagram of the structure of a collaborative control device for a dual-arm robot according to a certain embodiment of this application; Figure 4 This is a schematic diagram of the structure of a terminal device provided in a certain embodiment of this application. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this application clearer, the technical solutions of this application will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0019] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains; the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the application; the terms “comprising” and “having”, and any variations thereof, in the specification, claims, and foregoing description of the drawings are intended to cover non-exclusive inclusion.

[0020] In the description of the embodiments of this application, technical terms such as "first" and "second" are used only to distinguish different objects and should not be construed as indicating or implying relative importance or implicitly specifying the number, specific order, or primary and secondary relationship of the indicated technical features. In the description of the embodiments of this application, "multiple" means two or more, unless otherwise explicitly defined.

[0021] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0022] In the description of the embodiments in this application, the term "and / or" is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Additionally, the character " / " in this document generally indicates that the preceding and following related objects have an "or" relationship.

[0023] In the description of the embodiments of this application, the term "multiple" refers to two or more (including two), similarly, "multiple sets" refers to two or more (including two sets), and "multiple pieces" refers to two or more (including two pieces).

[0024] In the description of the embodiments of this application, unless otherwise expressly specified and limited, technical terms such as "installation," "connection," "joining," and "fixing" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. For those skilled in the art, the specific meaning of the above terms in the embodiments of this application can be understood according to the specific circumstances.

[0025] See Figure 1 and Figure 2To address the problems in the prior art, an embodiment of the present invention provides a cooperative control method for a dual-arm robot, comprising: S1. Obtain the initial position and placement position of the object to be transported, the first initial angle of each joint of the left arm of the dual-arm robot, and the second initial angle of each joint of the right arm. S2. Based on the initial position, placement position, first initial angle and second initial angle, plan the handling action for the left and right arms, and determine the target angle of each joint of the left and right arms at each moment in the handling process. In a preferred embodiment of the present invention, before the transport begins, the transport path needs to be determined. Based on the initial position and placement position of the object to be transported, the paths that the distal ends of the left and right arms need to traverse during the transport process are calculated using inverse kinematics. The paths of the distal ends are then transformed into target angle functions of each joint over time. It should be noted that, for the handling task, after converting the pose into a function of the joint angles over time using inverse kinematics, it was found that the angle functions of the left (right) arm are opposites. In this embodiment, each arm is assumed to have seven joints, i.e., the target angle function is expressed as: ; in, Let f(θ1)-f(θ7) be the target angle function, where f(θ1)-f(θ7) are the target angles of the 1st to 7th joints in a single arm, and θ1-θ7 are the joint parameters of the 1st to 7th joints in a single arm.

[0026] S3. Perform coordinated control operations at every moment during the handling process until the object to be handled is moved to the placement position; The coordinated control operation includes: S31. Obtain the actual angles of each joint on the left and right arms at the current moment; In a preferred embodiment of the present invention, during the transport process, an internal joint sensor or camera device is used to collect the actual angle function θ of the left and right arms at the current moment in real time. r(t) The actual angle function has the same structure as the target angle function, and is composed of the target angles of the seven joints of a single arm.

[0027] S32. Based on the actual angle and the target angle at the corresponding moment, calculate the angle error of each joint, and based on the angle error, calculate the first target torque of the left arm and the second target torque of the right arm. Preferably, the angle error includes: tracking error and synchronization error; The step of calculating the angle error of each joint based on the actual angle and the target angle at the corresponding moment, and calculating the first target torque of the left arm and the second target torque of the right arm based on the angle error, includes: Based on the actual angle, calculate the synchronization error between each joint on the left arm and the corresponding joint on the right arm; based on the actual angle and the target angle at the corresponding moment, calculate the tracking error of each joint during the handling process; input the tracking error into the preset state feedback controller of the dual-arm robot to generate the first basic torque of the left arm and the second basic torque of the right arm; based on the synchronization error, correct the first basic torque and the second basic torque to generate the first target torque of the left arm and the second target torque of the right arm.

[0028] In a preferred embodiment of the present invention, the actual angle function θ at the current moment is... r(t) The target angle θ planned in step S2 d(t) The difference is used to obtain the tracking error θ. e : θ e =θ r -θ d ; Where, θ e This is for tracking error.

[0029] Furthermore, during the bi-arm handling process, the two arms need to coordinate. If the actual angle of the i-th joint of the left arm is greater than the actual angle of the i-th joint of the right arm, it indicates that the left and right arms are out of sync. Therefore, in this embodiment, while tracking the trajectories of the left and right arms, the synchronization error of the actual angles of their corresponding joints is calculated in real time. Based on the characteristic that the angle functions of corresponding joints between the left and right arms are opposites when handling objects, in this embodiment, the synchronization error between corresponding joints is processed by a PID controller to generate a correction signal: negative feedback is given to the left arm, while positive feedback is given to the right arm, so as to enhance the coordination between the left and right arms as much as possible and suppress external disturbances.

[0030] Furthermore, state feedback controller Represented as: ; Where u is the first target torque or the second target torque of the right arm. and q are the first feedback gain matrix and the second feedback gain matrix, respectively. e From a practical perspective, For the target angle, θ = θ e To track errors, B −1 Let I be a 7x7 identity matrix, and G(q) e ) is the gravity matrix, and v is the correction signal corresponding to the synchronization error.

[0031] The expression for the correction signal generated by the PID controller is as follows: ; in For proportional gain; For integral gain; Let be the differential gain, and s be the synchronization error.

[0032] S33. Generate the first control command for the left arm and the second control command for the right arm based on the first target torque and the second target torque, respectively. S34. Control the left arm and the right arm respectively according to the first control command and the second control command.

[0033] In a preferred embodiment of the present invention, a first control command and a second control command are generated based on a first target torque and a second target torque, respectively, and the commands are finally sent to the motor drivers of the left and right arms to drive the joint movement.

[0034] Preferably, the construction of the state feedback controller includes: Obtain the nonlinear dynamic control equations of the dual-arm robot; construct an initial eigenvalue matrix and an initial free matrix; wherein the initial eigenvalue matrix is ​​a diagonal matrix and its elements are preset desired closed-loop poles, and the initial free matrix is ​​constructed based on preset linear control characteristics and an adjustable parameter; with the goal of maximizing the control stability of the dual-arm robot, iteratively optimize the distribution of matrix elements in the initial eigenvalue matrix and the adjustable parameter of the initial free matrix to generate a target eigenvalue matrix and a target free matrix; based on the target eigenvalue matrix and the target free matrix, construct a finite eigenvector matrix to characterize the relationship between the linear control characteristics and the desired closed-loop poles of the dual-arm robot; based on the target eigenvalue matrix, the dynamic control equations, and the finite eigenvector matrix, calculate the feedback gain matrix used to map the tracking error to linear control parameters, and construct a state feedback controller based on the feedback gain matrix.

[0035] Preferably, the step of calculating the feedback gain matrix for mapping the tracking error to linear control parameters based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix includes: Calculate the closed-loop system matrix based on the target eigenvalue matrix and the finite eigenvector matrix; calculate the parameter matrix based on the control parameter matrix and the finite eigenvector matrix in the dynamic control equations; and calculate the feedback gain matrix based on the finite eigenvector matrix, the parameter matrix, and the closed-loop system matrix.

[0036] In a preferred embodiment of the present invention, the standard form of the second-order all-drive system equations is: ; Where, vector It is the system's state vector, a vector It is the system's control vector, vector It is about , and A piecewise continuous vector function. It is a parameter vector. Let be the coefficient matrix of the second-order all-drive system equations. For the second-order all-drive system equations, the following assumptions are made: Assumption 1. Parameter vector Included in a compact set In, that is .

[0037] Assumption 2. , , and .

[0038] Assumption 3. , and .

[0039] Assumption 1 is primarily to ensure the system has definite parameters, Assumption 2 is primarily to guarantee the system's totality, and Assumption 3 is primarily to guarantee the system's highest order of 2.

[0040] Based on the second-order all-drive system equations, the dynamic control equations of the dual-arm robot are derived and established.

[0041] Where M, D, and G are the mass matrix, inertia matrix, and gravity matrix, respectively.

[0042] Furthermore, before starting the parametric design of the state feedback controller, it is necessary to verify whether the seven-degree-of-freedom error dynamics equations satisfy assumptions 1, 2, and 3. For assumption 1, It must be contained within some compact set. For assumption 2, the robotic arm is a typical fully driven system, with each degree of freedom having a controller, satisfying... For assumption 3, The system is a second-order equation system. Therefore, the seven-degree-of-freedom error dynamic system satisfies conditions 1, 2, and 3, and a state feedback controller can be parametrically designed.

[0043] After verification, the state feedback controller is constructed, and the steps include: Step 1. Construct the eigenvalue matrix and free matrix ; In step 1, the eigenvalue matrix It is The values ​​on the main diagonal of this matrix are the eigenvalues ​​of the transformed linear time-invariant system. To ensure the convergence of the linear time-invariant system, the following steps are taken: All elements on the main diagonal lie in the negative half-plane. Free matrix. Its function is to satisfy Established.

[0044] Step 2. Design a finite eigenvector matrix and parameter matrix ; In step 2, the matrix This is the parameter matrix of the new time-invariant system. It is a finite eigenvector matrix, and its function is to make the eigenvalue matrix designed in step 1... Able to be diagonalized to a matrix . It is the system's parameter matrix, and its function is to obtain... and .

[0045] Step 3. Calculate performance indicators ; It is a measure of the system's dynamic performance. Where ||·|| represents the norm of the matrix, which is determined when... , Then, through calculation The numerical values ​​can be used to calculate the performance of the system. If JJ does not meet the design requirements, the eigenvalue distribution or free matrix Z in step 1 can be adjusted (for example, by changing the α value), and steps 1 to 3 can be repeated until satisfactory performance indicators are obtained.

[0046] After steps 1, 2, and 3, the original seven-degree-of-freedom Lagrange error dynamics equations are transformed into a linear time-invariant system with desired eigenvalues. This system can be expressed as: ; In the formula: , , , ; The first feedback gain matrix can be obtained by solving the above equation. Second feedback gain matrix Among them, A C21 A C 22 For A C The lower half of the sub-block. In practical applications, the gain matrix is ​​often calculated directly using W and V, avoiding explicit inversion.

[0047] Finally, based on the solved first feedback gain matrix Second feedback gain matrix A state feedback controller is constructed: .

[0048] The above control method has two main advantages over other control methods. First, it can transform the open-loop controlled system into a first-order linear time-invariant closed-loop system with arbitrarily configurable characteristic structures, thus facilitating the control of the original system's performance. Second, it explicitly provides the design degrees of freedom, thereby enabling convenient optimization of system performance.

[0049] Furthermore, after constructing the state feedback controller, its stability was verified through the following steps. In the known In the case of a linear time-invariant system, the characteristic equation is expressed as: And the eigenvalue matrix Elements on the main diagonal Therefore, all the characteristic roots of the system's characteristic equation lie in the left half-plane, thus the linear time-invariant system is stable. It can converge to 0.

[0050] See Figure 3 This invention provides a collaborative control device for a dual-arm robot, comprising: The data acquisition module is used to acquire the initial position and placement position of the object to be transported, the first initial angle of each joint of the left arm of the dual-arm robot, and the second initial angle of each joint of the right arm. The motion planning module is used to plan the carrying motion of the left and right arms based on the initial position, placement position, first initial angle and second initial angle, and to determine the target angle of each joint of the left and right arms at each moment in the carrying process. The collaborative control module is used to perform collaborative control operations at every moment during the handling process until the object to be handled is moved to the placement position; The coordinated control operation includes: Obtain the actual angles of each joint in the left and right arms at the current moment; Based on the actual angle and the target angle at the corresponding moment, calculate the angle error of each joint, and based on the angle error, calculate the first target torque of the left arm and the second target torque of the right arm; Based on the first target torque and the second target torque, the first control command for the left arm and the second control command for the right arm are generated respectively. The left and right arms are controlled according to the first and second control commands, respectively.

[0051] It should be noted that before the transport begins, the transport path needs to be clearly defined. Based on the initial and placement positions of the object to be transported, the paths that the distal ends of the left and right arms need to traverse during the transport process are calculated using inverse kinematics. These distal end paths are then transformed into target angle functions of each joint over time. It should be noted that, for the handling task, after converting the pose into a function of the joint angles over time using inverse kinematics, it was found that the angle functions of the left (right) arm are opposites. In this embodiment, each arm is assumed to have seven joints, i.e., the target angle function is expressed as: ; in, Let f(θ1)-f(θ7) be the target angle function, where f(θ1)-f(θ7) are the target angles of the 1st to 7th joints in a single arm, and θ1-θ7 are the joint parameters of the 1st to 7th joints in a single arm.

[0052] Furthermore, the angle error includes: tracking error and synchronization error; The collaborative control module calculates the angle error of each joint based on the actual angle and the target angle at the corresponding moment, and calculates the first target torque of the left arm and the second target torque of the right arm based on the angle error, including: Based on the actual angle, calculate the synchronization error between each joint on the left arm and the corresponding joint on the right arm; Based on the actual angle and the target angle at the corresponding moment, calculate the tracking error of each joint during the transportation process; The tracking error is input into the preset state feedback controller of the dual-arm robot to generate the first basic torque of the left arm and the second basic torque of the right arm. Based on the synchronization error, the first basic torque and the second basic torque are corrected respectively to generate the first target torque of the left arm and the second target torque of the right arm.

[0053] It should be noted that the actual angle function θ at the current moment... r(t) The target angle θ planned in step S2 d(t) The difference is used to obtain the tracking error θ. e : θ e =θ r -θ d ; Where, θ e This is for tracking error.

[0054] Furthermore, during the bi-arm handling process, the two arms need to coordinate. If the actual angle of the i-th joint of the left arm is greater than the actual angle of the i-th joint of the right arm, it indicates that the left and right arms are out of sync. Therefore, in this embodiment, while tracking the trajectories of the left and right arms, the synchronization error of the actual angles of their corresponding joints is calculated in real time. Based on the characteristic that the angle functions of corresponding joints between the left and right arms are opposites when handling objects, in this embodiment, the synchronization error between corresponding joints is processed by a PID controller to generate a correction signal: negative feedback is given to the left arm, while positive feedback is given to the right arm, so as to enhance the coordination between the left and right arms as much as possible and suppress external disturbances.

[0055] Furthermore, state feedback controller Represented as: ; Where u is the first target torque or the second target torque of the right arm. and q are the first feedback gain matrix and the second feedback gain matrix, respectively. e From a practical perspective, For the target angle, θ = θ e To track errors, B −1 Let I be a 7x7 identity matrix, and G(q) e ) is the gravity matrix, and v is the correction signal corresponding to the synchronization error.

[0056] The expression for the correction signal generated by the PID controller is as follows: ; in For proportional gain; For integral gain; Let be the differential gain, and s be the synchronization error.

[0057] It should be noted that a first control command and a second control command are generated based on the first target torque and the second target torque, respectively, and the commands are finally sent to the motor drivers of the left and right arms to drive the joint movement.

[0058] Preferably, the construction of the state feedback controller includes: Obtain the nonlinear dynamic control equations of the dual-arm robot; construct an initial eigenvalue matrix and an initial free matrix; wherein the initial eigenvalue matrix is ​​a diagonal matrix and its elements are preset desired closed-loop poles, and the initial free matrix is ​​constructed based on preset linear control characteristics and an adjustable parameter; with the goal of maximizing the control stability of the dual-arm robot, iteratively optimize the distribution of matrix elements in the initial eigenvalue matrix and the adjustable parameter of the initial free matrix to generate a target eigenvalue matrix and a target free matrix; based on the target eigenvalue matrix and the target free matrix, construct a finite eigenvector matrix to characterize the relationship between the linear control characteristics and the desired closed-loop poles of the dual-arm robot; based on the target eigenvalue matrix, the dynamic control equations, and the finite eigenvector matrix, calculate the feedback gain matrix used to map the tracking error to linear control parameters, and construct a state feedback controller based on the feedback gain matrix.

[0059] Preferably, the step of calculating the feedback gain matrix for mapping the tracking error to linear control parameters based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix includes: Calculate the closed-loop system matrix based on the target eigenvalue matrix and the finite eigenvector matrix; calculate the parameter matrix based on the control parameter matrix and the finite eigenvector matrix in the dynamic control equations; and calculate the feedback gain matrix based on the finite eigenvector matrix, the parameter matrix, and the closed-loop system matrix.

[0060] In a preferred embodiment of the present invention, the standard form of the second-order all-drive system equations is: ; Where, vector It is the system's state vector, a vector It is the system's control vector, vector It is about , and A piecewise continuous vector function. It is a parameter vector. Let be the coefficient matrix of the second-order all-drive system equations. For the second-order all-drive system equations, the following assumptions are made: Assumption 1. Parameter vector Included in a compact set In, that is .

[0061] Assumption 2. , , and .

[0062] Assumption 3. , and .

[0063] Assumption 1 is primarily to ensure the system has definite parameters, Assumption 2 is primarily to guarantee the system's totality, and Assumption 3 is primarily to guarantee the system's highest order of 2.

[0064] Based on the second-order all-drive system equations, the dynamic control equations of the dual-arm robot are derived and established.

[0065] Where M, D, and G are the mass matrix, inertia matrix, and gravity matrix, respectively.

[0066] Furthermore, before starting the parametric design of the state feedback controller, it is necessary to verify whether the seven-degree-of-freedom error dynamics equations satisfy assumptions 1, 2, and 3. For assumption 1, It must be contained within some compact set. For assumption 2, the robotic arm is a typical fully driven system, with each degree of freedom having a controller, satisfying... For assumption 3, The system is a second-order equation system. Therefore, the seven-degree-of-freedom error dynamic system satisfies conditions 1, 2, and 3, and a state feedback controller can be parametrically designed.

[0067] After verification, the state feedback controller is constructed, and the steps include: Step 1. Construct the eigenvalue matrix and free matrix ; In step 1, the eigenvalue matrix It is The values ​​on the main diagonal of this matrix are the eigenvalues ​​of the transformed linear time-invariant system. To ensure the convergence of the linear time-invariant system, the following steps are taken: All elements on the main diagonal lie in the negative half-plane. Free matrix. Its function is to satisfy Established.

[0068] Step 2. Design a finite eigenvector matrix and parameter matrix ; In step 2, the matrix This is the parameter matrix of the new time-invariant system. It is a finite eigenvector matrix, and its function is to make the eigenvalue matrix designed in step 1... Able to be diagonalized to a matrix . It is the system's parameter matrix, and its function is to obtain... and .

[0069] Step 3. Calculate performance indicators ; It is a measure of the system's dynamic performance. Where ||·|| represents the norm of the matrix, which is determined when... , Then, through calculation The numerical values ​​can be used to calculate the performance of the system. If JJ does not meet the design requirements, the eigenvalue distribution or free matrix Z in step 1 can be adjusted (for example, by changing the α value), and steps 1 to 3 can be repeated until satisfactory performance indicators are obtained.

[0070] After steps 1, 2, and 3, the original seven-degree-of-freedom Lagrange error dynamics equations are transformed into a linear time-invariant system with desired eigenvalues. This system can be expressed as: ; In the formula: , , , ; The first feedback gain matrix can be obtained by solving the above equation. Second feedback gain matrix Among them, A C 21 A C 22 For A C The lower half of the sub-block. In practical applications, the gain matrix is ​​often calculated directly using W and V, avoiding explicit inversion.

[0071] Finally, based on the solved first feedback gain matrix Second feedback gain matrix A state feedback controller is constructed: .

[0072] The above control method has two main advantages over other control methods. First, it can transform the open-loop controlled system into a first-order linear time-invariant closed-loop system with arbitrarily configurable characteristic structures, thus facilitating the control of the original system's performance. Second, it explicitly provides the design degrees of freedom, thereby enabling convenient optimization of system performance.

[0073] Furthermore, after constructing the state feedback controller, its stability was verified through the following steps. In the known In the case of a linear time-invariant system, the characteristic equation is expressed as: And the eigenvalue matrix Elements on the main diagonal Therefore, all the characteristic roots of the system's characteristic equation lie in the left half-plane, thus the linear time-invariant system is stable. It can converge to 0.

[0074] Furthermore, it also includes: controller building modules; The controller construction module is used to obtain the nonlinear dynamic control equations of the dual-arm robot; Construct an initial eigenvalue matrix and an initial free matrix; wherein the initial eigenvalue matrix is ​​a diagonal matrix and its elements are preset desired closed-loop poles, and the initial free matrix is ​​constructed based on preset linear control characteristics and an adjustable parameter; With the goal of maximizing the control stability of the dual-arm robot, the distribution of matrix elements in the initial eigenvalue matrix and the adjustable parameters of the initial free matrix are iteratively optimized to generate the target eigenvalue matrix and the target free matrix. Based on the target eigenvalue matrix and the target free matrix, a finite eigenvector matrix is ​​constructed to characterize the relationship between the linear control characteristics of the dual-arm robot and the desired closed-loop poles. Based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, a feedback gain matrix is ​​calculated to map the tracking error to linear control parameters, and a state feedback controller is constructed based on the feedback gain matrix.

[0075] Furthermore, the controller construction module calculates a feedback gain matrix for mapping the tracking error to linear control parameters based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, including: Calculate the closed-loop system matrix based on the target eigenvalue matrix and the finite eigenvector matrix; Calculate the parameter matrix based on the control parameter matrix and the finite eigenvector matrix in the dynamic control equations; Calculate the feedback gain matrix based on the finite eigenvector matrix, parameter matrix, and closed-loop system matrix.

[0076] It is understood that the above-described device embodiments correspond to the method embodiments of the present invention, and can realize the cooperative control method for a dual-arm robot provided by any of the above-described method embodiments of the present invention.

[0077] It should be noted that the device embodiments described above are merely illustrative, and some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Furthermore, in the accompanying drawings of the device embodiments provided by this invention, the connection relationships between modules indicate that they have communication connections, which can specifically be implemented as one or more communication buses or signal lines. Those skilled in the art can understand and implement this without any creative effort.

[0078] See Figure 4 One embodiment of this application also provides a terminal device, including: One or more processors; A memory, coupled to the processor, for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement a collaborative control method for a dual-arm robot as described above.

[0079] The processor controls the overall operation of the terminal device to complete all or part of the steps of the aforementioned collaborative control method for a dual-arm robot. The memory stores various types of data to support the operation of the terminal device. This data may include, for example, instructions for any application or method used to operate on the terminal device, as well as application-related data. The memory can be implemented using any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read-Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk.

[0080] In an exemplary embodiment, the terminal device may be implemented by one or more application-specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), controllers, microcontrollers, microprocessors, or other electronic components to execute a collaborative control method for a dual-arm robot as described in any of the foregoing embodiments, and to achieve the same technical effects as the methods described above.

[0081] In another exemplary embodiment, a computer-readable storage medium including a computer program is also provided. When executed by a processor, the computer program implements the steps of a cooperative control method for a dual-arm robot as described in any of the foregoing embodiments. For example, the computer-readable storage medium may be the aforementioned memory including the computer program, which may be executed by a processor of a terminal device to complete the cooperative control method for a dual-arm robot as described in any of the foregoing embodiments and achieve the same technical effects as the described method.

[0082] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A cooperative control method for a dual-arm robot, characterized in that, include: Obtain the initial position and placement position of the object to be transported, the first initial angle of each joint of the left arm of the dual-arm robot, and the second initial angle of each joint of the right arm; Based on the initial position, placement position, first initial angle, and second initial angle, the carrying motion is planned for the left and right arms, and the target angles of each joint of the left and right arms are determined at each moment during the carrying process. Perform coordinated control operations at every moment during the handling process until the object to be handled is moved to its placement location; The coordinated control operation includes: Obtain the actual angles of each joint in the left and right arms at the current moment; Based on the actual angle and the target angle at the corresponding moment, calculate the angle error of each joint, and based on the angle error, calculate the first target torque of the left arm and the second target torque of the right arm; Based on the first target torque and the second target torque, the first control command for the left arm and the second control command for the right arm are generated respectively. The left and right arms are controlled according to the first and second control commands, respectively.

2. The cooperative control method for a dual-arm robot as described in claim 1, characterized in that, The angle error includes: tracking error and synchronization error; The step of calculating the angle error of each joint based on the actual angle and the target angle at the corresponding moment, and calculating the first target torque of the left arm and the second target torque of the right arm based on the angle error, includes: Based on the actual angle, calculate the synchronization error between each joint on the left arm and the corresponding joint on the right arm; Based on the actual angle and the target angle at the corresponding moment, calculate the tracking error of each joint during the transportation process; The tracking error is input into the preset state feedback controller of the dual-arm robot to generate the first basic torque of the left arm and the second basic torque of the right arm. Based on the synchronization error, the first basic torque and the second basic torque are corrected respectively to generate the first target torque of the left arm and the second target torque of the right arm.

3. The cooperative control method for a dual-arm robot as described in claim 2, characterized in that, The construction of the state feedback controller includes: Obtain the nonlinear dynamic control equations of the dual-arm robot; Construct an initial eigenvalue matrix and an initial free matrix; wherein the initial eigenvalue matrix is ​​a diagonal matrix and its elements are preset desired closed-loop poles, and the initial free matrix is ​​constructed based on preset linear control characteristics and an adjustable parameter; With the goal of maximizing the control stability of the dual-arm robot, the distribution of matrix elements in the initial eigenvalue matrix and the adjustable parameters of the initial free matrix are iteratively optimized to generate the target eigenvalue matrix and the target free matrix. Based on the target eigenvalue matrix and the target free matrix, a finite eigenvector matrix is ​​constructed to characterize the relationship between the linear control characteristics of the dual-arm robot and the desired closed-loop poles. Based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, a feedback gain matrix is ​​calculated to map the tracking error to linear control parameters, and a state feedback controller is constructed based on the feedback gain matrix.

4. The cooperative control method for a dual-arm robot as described in claim 3, characterized in that, The step of calculating the feedback gain matrix for mapping the tracking error to linear control parameters based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix includes: Calculate the closed-loop system matrix based on the target eigenvalue matrix and the finite eigenvector matrix; Calculate the parameter matrix based on the control parameter matrix and the finite eigenvector matrix in the dynamic control equations; Calculate the feedback gain matrix based on the finite eigenvector matrix, parameter matrix, and closed-loop system matrix.

5. A collaborative control device for a dual-arm robot, characterized in that, include: The data acquisition module is used to acquire the initial position and placement position of the object to be transported, the first initial angle of each joint of the left arm of the dual-arm robot, and the second initial angle of each joint of the right arm. The motion planning module is used to plan the carrying motion of the left and right arms based on the initial position, placement position, first initial angle and second initial angle, and to determine the target angle of each joint of the left and right arms at each moment in the carrying process. The collaborative control module is used to perform collaborative control operations at every moment during the handling process until the object to be handled is moved to the placement position; The coordinated control operation includes: Obtain the actual angles of each joint in the left and right arms at the current moment; Based on the actual angle and the target angle at the corresponding moment, calculate the angle error of each joint, and based on the angle error, calculate the first target torque of the left arm and the second target torque of the right arm; Based on the first target torque and the second target torque, the first control command for the left arm and the second control command for the right arm are generated respectively. The left and right arms are controlled according to the first and second control commands, respectively.

6. The collaborative control device for a dual-arm robot as described in claim 5, characterized in that, The angle error includes: tracking error and synchronization error; The collaborative control module calculates the angle error of each joint based on the actual angle and the target angle at the corresponding moment, and calculates the first target torque of the left arm and the second target torque of the right arm based on the angle error, including: Based on the actual angle, calculate the synchronization error between each joint on the left arm and the corresponding joint on the right arm; Based on the actual angle and the target angle at the corresponding moment, calculate the tracking error of each joint during the transportation process; The tracking error is input into the preset state feedback controller of the dual-arm robot to generate the first basic torque of the left arm and the second basic torque of the right arm. Based on the synchronization error, the first basic torque and the second basic torque are corrected respectively to generate the first target torque of the left arm and the second target torque of the right arm.

7. The collaborative control device for a dual-arm robot as described in claim 6, characterized in that, Also includes: Controller building blocks; The controller construction module is used to obtain the nonlinear dynamic control equations of the dual-arm robot; Construct an initial eigenvalue matrix and an initial free matrix; wherein the initial eigenvalue matrix is ​​a diagonal matrix and its elements are preset desired closed-loop poles, and the initial free matrix is ​​constructed based on preset linear control characteristics and an adjustable parameter; With the goal of maximizing the control stability of the dual-arm robot, the distribution of matrix elements in the initial eigenvalue matrix and the adjustable parameters of the initial free matrix are iteratively optimized to generate the target eigenvalue matrix and the target free matrix. Based on the target eigenvalue matrix and the target free matrix, a finite eigenvector matrix is ​​constructed to characterize the relationship between the linear control characteristics of the dual-arm robot and the desired closed-loop poles. Based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, a feedback gain matrix is ​​calculated to map the tracking error to linear control parameters, and a state feedback controller is constructed based on the feedback gain matrix.

8. The collaborative control device for a dual-arm robot as described in claim 7, characterized in that, The controller construction module calculates the feedback gain matrix for mapping the tracking error to linear control parameters based on the target eigenvalue matrix, the dynamic control equation, and the finite eigenvector matrix, including: Calculate the closed-loop system matrix based on the target eigenvalue matrix and the finite eigenvector matrix; Calculate the parameter matrix based on the control parameter matrix and the finite eigenvector matrix in the dynamic control equations; Calculate the feedback gain matrix based on the finite eigenvector matrix, parameter matrix, and closed-loop system matrix.

9. A terminal device, characterized in that, include: One or more processors; A memory, coupled to the processor, for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement a cooperative control method for a dual-arm robot as described in any one of claims 1-4.

10. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements a cooperative control method for a dual-arm robot as described in any one of claims 1-4.

Images