Dual robot arm shared control method and apparatus

Abstract

The present application belongs to the technical fields of multi-robot arm cooperative work and man-machine co-integration manufacturing, and discloses a double-robot arm shared control method and equipment, steps are as follows: (1) using a dynamic system to plan the spatial velocity field of an object in the process of double-arm cooperative clamping object movement; (2) in the process of double-arm clamping object carrying, an interactive external force input impedance controller is applied to the object by an operator, the impedance controller outputs the velocity of the object dragged by the operator; the parameter of the impedance controller is adjusted through a virtual damping field, and then the interactive feeling of the operator is adjusted; (3) based on a cooperation arbitration factor, the double-arm movement instruction corresponding to the spatial velocity field of the object and the cooperation instruction corresponding to the velocity of the object dragged by the operator are fused to obtain a total instruction, and then the double-arm cooperative work and man-machine shared cooperation complete a task through the obtained total instruction; wherein the cooperation arbitration factor is set based on the interactive external force. The present application improves adaptability.

Description

Technical Field

[0001] This invention belongs to the technical field of multi-robotic arm collaborative operation and human-machine integrated manufacturing, and more specifically, relates to a dual-robotic arm shared control method and equipment. Background Technology

[0002] With the development of intelligent manufacturing, dual-robotic arm collaborative systems are widely used in the handling of irregular, large-sized materials. However, traditional fully autonomous control lacks flexibility in the face of dynamic environments such as unknown obstacles or workpiece pose shifts. Therefore, human-robot shared control strategies allow operators to directly intervene in the underlying control loop through physical contact, using human cognitive abilities to guide the robot to avoid obstacles or achieve precise alignment.

[0003] However, existing dual-arm shared control systems still face technical challenges in practical engineering applications: First, decoupling from dynamic interactive external forces is difficult. Traditional modeling relies on unknown object centers of mass, failing to isolate the interference of gravity and inertial forces during acceleration and deceleration, which can easily lead to the system misinterpreting human dragging intentions. Second, fixed impedance results in a stiff feel. Globally fixed impedance parameters cannot reconcile the contradiction between "lightweight obstacle avoidance and dragging" and "stable autonomous tracking," making it difficult to adapt to multi-stage tasks. Summary of the Invention

[0004] In view of the above-mentioned defects or improvement needs of the existing technology, the present invention provides a dual-arm shared control method and device, which aims to solve the problem of poor adaptability caused by the use of fixed impedance in the existing dual-arm shared control.

[0005] To achieve the above objectives, according to one aspect of the present invention, a dual-arm shared control method is provided, comprising the following steps: (1) Use dynamic systems to plan the spatial velocity field of the object being gripped by two arms in a coordinated manner; (2) During the process of moving an object held by the double arms, the operator applies an interactive external force to the object to an impedance controller, and the impedance controller outputs the speed at which the operator drags the object; wherein, the parameters of the impedance controller are adjusted by a virtual damping field, thereby adjusting the operator's interactive feel; the positions reached by the object in the working space of the double arms are respectively set with damping values ​​to form the virtual damping field. (3) Based on the collaborative arbitration factor, the double-arm motion command corresponding to the spatial velocity field of the object and the collaborative command corresponding to the speed at which the operator drags the object are fused to obtain the total command, and then the double-arm collaboration and human-machine shared collaboration are controlled through the obtained total command to complete the task; wherein, the collaborative arbitration factor is set based on the interactive external force.

[0006] Furthermore, the spatial velocity field of the object satisfies the Lyapunov global asymptotic stability condition.

[0007] Furthermore, the mass parameter matrix of the impedance controller is fixed, the damping parameter matrix is ​​variable, and the stiffness matrix is ​​zero; the expression for the impedance controller is:

[0008] In the formula, and These are the virtual mass and the damping matrix, respectively; External forces interacting with the operator; The expected velocity of the object caused by the interaction force.

[0009] Furthermore, the trajectory contour error between the object's current actual pose and the reference trajectory is calculated in real time, and a contour error intervention threshold is set. When the trajectory contour error is greater than the contour error intervention threshold or the amplitude of the interactive external force exceeds the static friction threshold, the operator's intention to cooperate is determined, and the cooperation arbitration factor decreases nonlinearly and smoothly from 1 to 0.

[0010] Furthermore, a torque balance equation is established to balance the torque caused by the equivalent torque at the ends of the two arms and the torque caused by the gravity of the object, thereby constructing a high-dimensional overdetermined equation system containing at least 18 independent calibration equations.

[0011] Furthermore, the least squares method with added nonlinear regularization terms is used to solve the high-dimensional overdetermined equations to obtain the optimal solution for the mass and centroid position vector of the object.

[0012] Furthermore, an external disturbance observer is used to filter out the robot arm's own gravity, Coriolis force, and centrifugal force. At the same time, the mass and center of mass vector of the object are obtained in real time to remove the inertial disturbance terms caused by the object's gravitational torque and acceleration, so as to obtain the interactive external forces acting on the object.

[0013] Furthermore, the expression for the interactive external force is:

[0014] In the formula, For interactive external forces; It is a six-dimensional generalized acceleration vector, including three-dimensional linear acceleration and three-dimensional angular acceleration; This is the angular velocity vector of the object's central frame relative to the world coordinate system; Let be the inertial tensor matrix of the object in its own central frame; This is the vector of the force exerted on the object by the ends of the arms; Let g be the weight of the object and the gravitational torque it causes.

[0015] The present invention also provides a dual-robotic arm shared control system, the system including a memory and a processor, the memory storing a computer program, and the processor executing the computer program to perform the dual-robotic arm shared control method as described above.

[0016] The present invention also provides a computer-readable storage medium storing machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the dual-arm shared control method as described above.

[0017] In summary, compared with the prior art, the dual-arm shared control method and equipment provided by this invention have the following advantages: 1. Damping values ​​are set at the positions reached by the object within the working space of the two arms, forming the virtual damping field. The damping parameters of the impedance controller are then adjusted through the virtual damping field to ensure the interactive feel of the operator in different task areas. In this way, a virtual damping field of corresponding shape can be set according to the working range of the robotic arm, thereby improving the agility and safety of human-machine interaction and enhancing adaptability in dynamic environments.

[0018] 2. This invention utilizes a dynamic system to pre-plan the speed of the object-carrying trajectory using a dual-arm gripper, decomposing the speed into trajectory normal and tangential velocities to construct a spatial velocity field. This ensures that the object generates a speed tending towards the desired trajectory when deviating from the desired position, and continuously adjusts the approaching speed to achieve a smooth arrival as it approaches the endpoint.

[0019] 3. This invention integrates the dual-arm motion commands corresponding to the spatial velocity field of the object and the collaborative commands corresponding to the speed at which the operator drags the object, based on a collaborative arbitration factor, to obtain a total command. The total command is then used to control the dual-arm collaboration and human-machine collaborative task completion. In other words, the control authority is dynamically adjusted through the collaborative arbitration factor, realizing the unification of autonomous dual-arm operation and human-machine collaboration. It incorporates the operator's experience, thereby enhancing the flexibility of the robotic arm operation and ensuring the successful implementation of dual-arm collaborative handling and installation tasks.

[0020] 4. The spatial velocity field satisfies the Lyapunov global asymptotic stability condition, ensuring that within the workspace of the robotic arm, regardless of where the object is dragged by the operator during the human-machine interaction phase, once the operator's intention to cooperate approaches zero, the spatial velocity field can guide the object to smoothly converge to the preset reference trajectory.

[0021] 5. The generalized external disturbance observer is used to filter out the robot arm's own gravity, Coriolis force, and centrifugal force. At the same time, the mass and center of mass vector of the object are adjusted in real time to remove the inertial disturbance terms caused by the object's gravitational torque and acceleration, so as to obtain the interactive external forces acting on the object and achieve accurate acquisition of interactive external forces.

[0022] 6. The impedance controller is configured with a fixed mass parameter matrix and a variable damping parameter matrix, and its stiffness matrix is ​​set to zero to avoid unnecessary rebound and position oscillation during release in human-machine physical interaction. Attached Figure Description

[0023] Figure 1 This is a flowchart of a dual-robotic arm shared control method provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of the coordinate system for a double-arm clamping object according to a preferred embodiment of the present invention; Figure 3 This is an interactive external force curve diagram of the compensated system according to a preferred embodiment of the present invention; Figure 4 These are schematic diagrams of virtual damping fields constructed according to preferred embodiments of the present invention, namely, square damping fields, spherical damping fields, and cylindrical damping fields; Figure 5 This is a graph showing the variation of interactive external forces and cooperative arbitration factors according to a preferred embodiment of the present invention; Figure 6 (a) and (b) in the figure are respectively diagrams showing the position change and contour error change of the object held by the two arms according to the preferred embodiment of the present invention. Detailed Implementation

[0024] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0025] This invention provides a dual-arm shared control method. The method generates motion speed and virtual damping field through a dynamic system to adjust the interactive feel. The shared control framework integrates human-machine control commands to realize human-machine shared industrial assembly tasks of the dual-arm system. At the same time, it reduces the cognitive load of the operator and optimizes decision-making, enhances adaptability in dynamic environments, and establishes a virtual damping field through spatial area division to achieve dynamic adjustment of the human-machine interactive operation feel.

[0026] Please see Figure 1 The method mainly includes the following steps: S1, Utilize a dynamic system to plan the spatial velocity field of the object being gripped by both arms in a coordinated manner, and obtain the object's moving speed.

[0027] Specifically, after unifying the points of application of the interactive external forces and the motion reference points to the coordinate system {b} of the clamping position center, the unknown position vector from the center of mass of the object to the center of the clamp on the robotic arm is decomposed and equivalently represented as a known relative vector along the clamping direction of the two arms.

[0028] A spatial velocity field for the object is constructed using a dynamic system, with the reference coordinate system being the center coordinate system {b} of the clamping position. The desired velocity of the object is decomposed into the tangential velocity and the normal velocity along the trajectory. The expression for the desired velocity generated by the dynamic system is as follows:

[0029] in, The normal velocity required to pull the object back to the desired trajectory. The normal velocity coefficient, The current point is the closest projection point to the desired trajectory. Let be the tangential velocity of the motion along the tangential direction of the projection point. The tangential velocity coefficient, In order to be in The unit tangent vector at that point, This is a tangential velocity attenuation factor used to reduce the velocity near the end of the trajectory. The tangential velocity. Wherein Adjustable distance threshold To adjust Then adjust the range of speed decay.

[0030] The established spatial velocity field satisfies the Lyapunov global asymptotic stability condition, ensuring that within the workspace of the robotic arm, regardless of where the object is dragged by the operator during the human-machine interaction phase, once the operator's intention to cooperate approaches zero, the spatial velocity field can guide the object to smoothly converge toward the preset reference trajectory.

[0031] S2 accurately estimates the interactive external forces applied by the operator to the object during the handling process of the object held by the dual arms, in order to obtain the intention of human-machine collaboration.

[0032] Specifically, the system controls the dual robotic arms to grip an object and executes no fewer than six independent static calibration poses within the workspace. It simultaneously collects data from the corresponding joint positions of the two arms and the end force / torque sensor data, and then matches the data sets together.

[0033] By using a grasping matrix to transform the measured torques at the ends of the two arms to the reference coordinate system, and eliminating the internal clamping forces caused by the closed-chain constraints, a torque balance equation is established to balance the torques caused by the equivalent torques at the ends of the two arms and the torques caused by the object's gravity. This leads to the construction of a high-dimensional overdetermined equation system containing at least 18 independent calibration equations, expressed as follows:

[0034] The high-dimensional overdetermined equations are solved using the least squares method with added nonlinear regularization terms to obtain the optimal solution for the precise mass and centroid position vector of the target object. The expression for the optimal solution is as follows:

[0035] A generalized external disturbance observer is constructed to filter out the robot arm's own gravity, Coriolis force, and centrifugal force. Simultaneously, the obtained mass and center-of-mass position vector of the object are used to remove inertial disturbance terms caused by the object's gravitational torque and acceleration, thus obtaining the interactive external forces acting on the object. The expression for the interactive external forces is:

[0036] S3. Based on the obtained interactive external force, an impedance controller is designed, and a virtual damping field is designed according to the working space of the two arms. The parameters of the impedance controller are adjusted through the virtual damping field, thereby adjusting the operator's interactive feel. The input of the impedance controller is the interactive external force, and the output is the speed at which the operator drags the object. The position reached by the object in the working space is correspondingly set with a damping value, forming the virtual damping field.

[0037] The impedance controller is configured with a fixed mass parameter matrix and a variable damping parameter matrix, with its stiffness matrix term set to zero to avoid unnecessary rebound and positional oscillations during release in human-machine physical interaction. The expression for the impedance controller is:

[0038] in, The matrix represents the virtual mass and damping matrix, with the diagonal lines corresponding to the coefficients in the three motion directions. For the operator's interactive external force, The desired velocity of the object's motion caused by the interaction force. The virtual mass matrix in the impedance controller is set to... .

[0039] In the workspace of the robotic arm, different types of three-dimensional virtual damping fields are preset using spatial geometric parameters according to the distribution of environmental obstacles and the stage requirements of specific assembly tasks. The boundary shape of the three-dimensional virtual damping field includes square, cylindrical or spherical.

[0040] The square virtual damping field divides the workspace into There are three square regions, each corresponding to a constant damping value. And assuming the boundary position is To ensure a smooth transition of damping at the boundary, the width of the transition region is set to [value missing]. :

[0041] Among them, threshold values ​​are set for the cylindrical and spherical virtual damping fields. Used to enclose the target region For constant damping, These are the concave damping coefficient and the convex damping coefficient, respectively, and their expressions are:

[0042] The center coordinates of the cylindrical and spherical VDFs are respectively and The reference coordinate system is the right arm base, and the VDF parameters are set to... .

[0043] When an object enters the low-damping region, the reaction resistance of the operator dragging the object is reduced, improving the agility of the interaction; when the object is in the high-damping region, a high damping coefficient is used to enhance the control rigidity of the dual-arm system and suppress high-frequency jitter.

[0044] S4. Based on the intention of human-machine collaboration, a collaboration arbitration factor is set. Based on the collaboration arbitration factor, the double-arm movement command corresponding to the spatial velocity field of the object and the collaboration command corresponding to the speed at which the operator drags the object are fused to obtain a total command. Then, the obtained total command is used to control the double-arm collaboration and human-machine shared collaboration to complete the task.

[0045] Specifically, the trajectory contour error between the object's current actual pose and the reference trajectory is calculated in real time, and a contour error intervention threshold is set, with the threshold set to 2mm.

[0046] When the trajectory contour error exceeds the contour error intervention threshold or the amplitude of the interactive external force exceeds the static friction threshold, it is determined that the operator intends to cooperate. The cooperation arbitration factor then decreases non-linearly and smoothly from 1 to 0, as expressed in the following expression:

[0047] in, To decide The control coefficients for the changes are set in the experiment. At this point, the overall command is entirely controlled by the impedance controller, allowing the operator to smoothly drag the object to move.

[0048] By fusing the dynamic system generation speed and the cooperative arbitration factor calculation speed, the final speed of the object held by both arms is obtained, as expressed below:

[0049] in, As a collaborative arbitration factor, The velocity vector output by the dynamic system. The impedance velocity is calculated based on the interaction of external forces between collaborators. The desired velocity of the object held by the two arms after fusion.

[0050] To verify the effectiveness and superiority of the human-machine shared control method proposed in this invention, a turbine sleeve assembly experiment was conducted for method verification. For this embodiment, as... Figure 2 As shown, this invention describes the forces and motion of the object in the clamping center coordinate system {b}. Figure 3 The diagram shows the interactive external forces after removing the effects of the object's gravity and inertia. The error of the interactive external forces is stable within ±0.2N, and the error of the interactive external torque is between -0.005 and 0.05N. Internally, the proposed method for estimating external forces in interactions lays the foundation for recognizing the interaction intentions of collaborators. The damping field settings used to adjust the interactive feel are as follows: Figure 4 As shown, the colors at different spatial locations correspond to the magnitudes of the damping values. The collaborative arbitration factor in the shared control method proposed in this invention is used to dynamically adjust control authority based on the operator's interactive intent, such as... Figure 5 As shown, a turbine simulation assembly platform was built, and a human-machine collaborative assembly experiment was conducted. The equipment included two UR16e robotic arms, an ATI six-dimensional force sensor, etc. Figure 6 The diagram shows the trajectory and contour error changes during the entire dual-arm handling and assembly process, indicating that the dual-arm system can achieve precise handling trajectory tracking, and that human-machine collaborative operation can intervene in a timely manner to perform compliant dragging and obstacle avoidance and guide the sleeve position to make fine adjustments for successful assembly.

[0051] The present invention also provides a dual-robotic arm shared control system, the system including a memory and a processor, the memory storing a computer program, and the processor executing the computer program to perform the dual-robotic arm shared control method as described above.

[0052] The present invention also provides a computer-readable storage medium storing machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the dual-arm shared control method as described above.

[0053] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for shared control of two robotic arms, characterized in that, The steps are as follows: (1) Use dynamic systems to plan the spatial velocity field of an object by coordinating the movement of the two arms in gripping the object; (2) During the process of moving an object held by the double arms, the operator applies an interactive external force to the object to the impedance controller, and the impedance controller outputs the speed at which the operator drags the object; wherein, the parameters of the impedance controller are adjusted by a virtual damping field, thereby adjusting the operator's interactive feel; damping values ​​are set at the positions reached by the object in the working space of the double arms, forming the virtual damping field. (3) Based on the collaborative arbitration factor, the double-arm motion command corresponding to the spatial velocity field of the object and the collaborative command corresponding to the speed at which the operator drags the object are fused to obtain the total command, and then the double-arm collaboration and human-machine shared collaboration are controlled through the obtained total command to complete the task; wherein, the collaborative arbitration factor is set based on the interactive external force.

2. The dual-arm shared control method as described in claim 1, characterized in that: The spatial velocity field of the object satisfies the Lyapunov global asymptotic stability condition.

3. The dual-arm shared control method as described in claim 1, characterized in that: The mass parameter matrix of the impedance controller is fixed, the damping parameter matrix is ​​variable, and the stiffness matrix is ​​zero; the expression of the impedance controller is: In the formula, and These are the virtual mass and the damping matrix, respectively; External forces interacting with the operator; The expected velocity of the object caused by the interaction force.

4. The dual-arm shared control method as described in claim 1, characterized in that: The trajectory contour error between the object's current actual pose and the reference trajectory is calculated in real time, and a contour error intervention threshold is set. When the trajectory contour error is greater than the contour error intervention threshold or the amplitude of the interactive external force exceeds the static friction threshold, the operator's cooperation intention is determined to intervene, and the cooperation arbitration factor decreases non-linearly and smoothly from 1 to 0.

5. The dual-arm shared control method as described in any one of claims 1-4, characterized in that: A torque balance equation is established, which balances the torque caused by the equivalent torque at the ends of the two arms and the torque caused by the gravity of the object. Then, a high-dimensional overdetermined equation system containing at least 18 independent calibration equations is constructed.

6. The dual-arm shared control method as described in claim 5, characterized in that: The high-dimensional overdetermined equations are solved by the least squares method with added nonlinear regularization terms to obtain the optimal solution for the mass and centroid position vector of the object.

7. The dual-arm shared control method as described in claim 6, characterized in that: An external disturbance observer is used to filter out the robot arm's own gravity, Coriolis force, and centrifugal force. At the same time, the mass and center of mass vector of the object are obtained in real time to remove the inertial disturbance terms caused by the object's gravitational torque and acceleration, so as to obtain the interactive external forces acting on the object.

8. The dual-arm shared control method as described in claim 7, characterized in that: The expression for the interactive external force is: In the formula, For interactive external forces; It is a six-dimensional generalized acceleration vector, including three-dimensional linear acceleration and three-dimensional angular acceleration; This is the angular velocity vector of the object's central frame relative to the world coordinate system; Let be the inertial tensor matrix of the object in its own central frame; This is the vector of the force exerted on the object by the ends of the arms; Let g be the weight of the object and the gravitational torque it causes.

9. A dual-robotic arm shared control system, characterized in that: The system includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it performs the dual robotic arm shared control method according to any one of claims 1-8.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the dual-arm shared control method according to any one of claims 1-8.

Images