CN111687827B - Control method and control system for coordinating and operating weak rigid member by two robots - Google Patents

Abstract

The invention provides a control method and a control system for coordinately operating a weak rigid member by two robots, which realize that a guide track simultaneously meets the safety restriction requirements of a Cartesian space and a joint space of the robots, solve the problem of dangerous guide tracks caused by too large guide force or sudden change in a guide control algorithm, and improve the smoothness and the safety of the guide tracks; the response speed of the system is greatly improved while stability is guaranteed, the tracking error of the object track can be reduced while the internal force control effect is guaranteed, and the method has important significance for realizing real-time internal force control of the industrial robot based on the position.

Description

A control method and control system for coordinated operation of weak rigid components by two robots

technical field

The invention belongs to the technical field of robot control, in particular to a control method and a control system for double robots to coordinately operate weak rigid components.

Background technique

The application of single-arm robots in spraying, welding, grinding, assembly and other related fields is very mature, but in some applications for large parts and large-sized workpieces, single-arm robots are limited by their load capacity and working range. some work. With the continuous expansion of the application scope of robots, there are more and more application scenarios of the multi-robot coordinated control system. The coordinated motion of the dual robots is the basis and premise of the coordinated control of the system, and it is also the technical core to realize the handling action in the assembly process of weakly rigid components. The aircraft panel and many aircraft parts are weakly rigid or flexible parts. Coordinated motion errors will bring serious pulling, squeezing and shearing stress to operating objects such as weakly rigid components, and will cause deformation and damage to the objects in severe cases. The current multi-robot coordinated control technology is also inconvenient to realize the random motion of weakly rigid components in space with 6 degrees of freedom. In terms of internal force control and load distribution, most of the current research is aimed at small robots and combined with their dynamic models for torque control, but Industrial robots themselves are difficult to model accurately and commercial industrial robots do not open torque control loops. To this end, it is necessary to perform interpolation control on the robot's guiding trajectory and adjust its internal force control and trajectory compensation.

SUMMARY OF THE INVENTION

The technical problem solved by the present invention is to provide a control method and a control system for double robots to coordinately operate weak rigid components, so that the guiding trajectory can meet the safety restriction requirements of the robot Cartesian space and joint space at the same time, and solve the problem of excessive guiding force due to excessive guiding force. The dangerous guiding trajectory problem caused by large or sudden changes improves the smoothness and safety of the guiding trajectory, improves the response speed of the system, and reduces the tracking error of the object trajectory.

The technical solution that realizes the object of the present invention is:

A control method for two robots to coordinately operate weak rigid components, comprising the following steps:

Step 1: Take the weakly rigid component as the direct manipulation object, build the kinematic closed-loop chain model of the dual robot, and design the dual robot guidance control based on the model, the dual robot guidance control includes force coordinate transformation, stiffness control and guidance force threshold control;

Step 2: According to the kinematic constraint relationship between the robot end and the operating object, design the multi-space adaptive interpolation control of the guiding trajectory in Cartesian space and joint space, including:

Step 2-1: According to the excessive angular velocity and/or excessive angular acceleration of the operating object, interpolate the rotation guidance trajectory;

Step 2-2: Recalculate the speed and acceleration of the robot end and the discrete points of the object guidance trajectory, and then interpolate the movement guidance trajectory according to the excessive movement speed and/or the movement acceleration of the operating object;

Step 2-3: According to the motion constraints of each joint, perform adaptive interpolation of the joint space on the guiding trajectory;

Step 3: Design the dual-input fuzzy control and object trajectory tracking error compensation control based on the impedance model according to the internal force calculation model of the operating object, including:

Step 3-1: Adopt the benchmark adjustment strategy of internal force control, fix the robot end on one side as the control benchmark, and adjust the robot end on the other side according to the internal force;

Step 3-2: Establish the internal force fuzzy control framework based on the impedance model: input the internal force deviation and the pose adjustment amount of the previous cycle to the fuzzy controller, then output the pose adjustment amount of the current cycle and control the internal force of the dual robot system;

Step 3-3: Compensate the trajectory according to whether there is a response lag error at the fixed end. Further, in the control method for the coordinated operation of weakly rigid components by dual robots of the present invention, establishing a kinematic closed-loop chain model of the dual robots in step 1 specifically includes the following steps:

Step 1-1: The kinematic model of the operation object coordinate system {c} relative to the world coordinate system {W} is:

^W T _c = ^W T _R1 · ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c

^W T _c = ^W T _R2 · ^R2 T _e2 · ^e2 T _c2 · ^c2 T _c

^W T _R1 and ^W T _R2 are the transformation relationship between the installation position of each robot base and the world coordinate system, and are fixed constant matrices; {R1}, {R2} represent the respective base coordinate systems of robot A and robot B; {e1 } and {e2} represent the end tool coordinate systems of robot A and robot B, respectively, which coincide with the gripping point at the end of the robot; {c1} and {c2} represent the coordinate systems after translation and extension of {e1} and {e2}, respectively, and The origin of the coordinate system coincides with the origin of {c}; ^c1 T _c , ^c2 T _c represent the homogeneous change matrix of the object coordinate system {c} relative to {c1}, {c2} respectively; ^e1 T _c1 , ^e2 T _c2 respectively represent the homogeneous transformation matrices of {c1} and {c2} relative to the robot end coordinate systems {e1} and {e2}; ^R1 T _e1 and ^R2 T _e2 respectively represent the homogeneous transformation between the robot end coordinate system and their respective base coordinate systems matrix; ^W T _R1 and ^W T _R2 respectively represent the homogeneous transformation matrix of the robot base coordinate system {R1}, {R2} relative to the world coordinate system {W};

1-2: The world coordinate system {W} is coincident with the base coordinate system {R1} of one of the robots, and the four-point calibration method is used to determine the transformation relationship ^R1 T _R2 between the base coordinate systems, then the kinematics closed loop The chain determines the end virtual link matrix between the ends of the dual robot:

^e1 T _e2 = [ ^R1 T _e1 ] ^-1 · ^R1 T _R2 · ^R2 T _e2

1-3: Based on the virtual link matrix ^e1 T _e2 , the position adjustment ratio K is introduced to determine the origin of the double robot end translation coordinate systems {c1} and {c2}, and the object coordinate system {c} is established at the robot end Translate the origin of the coordinate systems {c1} and {c2}, then the homogeneous transformation matrix between the robot end tool coordinate systems {e1}, {e2} and their translated coordinate systems {c1}, {c2} is:

In the formula, E ₃ represents the 3rd-order unit matrix, O _1×3 represents the zero vector with 1 row and 3 columns, and P{·} represents the position vector for extracting the homogeneous matrix, ^e1 T _c1 and ^e2 T _c2 only follow ^e1 T The position vector of _e2 moves, and ^c1 T _c2 and ^c2 T _c1 only change according to the rotation matrix ^e1 R _e2 of ^e1 T _e2 ;

1-4: Establish the coordinate system transformation relationship between the object coordinate system {c} and the end translation coordinate systems {c1}, {c2}, where A is a constant homogeneous rotation matrix:

^c T _c2 = ^c T _c1 · ^c1 T _c2

1-5: Determine the kinematic drive control equation of each robot through the closed-loop kinematics model of the dual robots:

^R1 T _e1 = ^R1 T _c · [ ^e1 T _c ] ^-1 = [ ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c ] · [ ^e1 T _c1 · ^c1 T _c ] ^-1

^R2 T _e2 = [ ^R1 T _R2 ] ^-1 · ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c · [ ^e2 T _c2 · ^c2 T _c1 · ^c1 T _c ] ^-1

^R1 T _e1 , ^R2 T _e2 are the pose homogeneous matrices in the respective coordinate systems of the two robots.

Further, in the control method of the double-robot coordinated operation of weakly rigid components of the present invention, step 3-2 specifically includes three parts: variable fuzzification, fuzzy rule design and de-fuzzification of internal force difference ΔF _i and position adjustment amount Δx, specifically:

Step 3-2-1: Fuzzy input and output:

Set the basic universe, determine the quantification factor and scale factor, use the triangular membership function to fuzzify the data that does not exceed the boundary of the universe, and set the data beyond the boundary of the universe as the boundary value of the universe;

Step 3-2-2: Design fuzzy rules:

1) If the internal force F _i and the position adjustment amount X of the previous cycle are in the same direction, the robot is still in the internal force adjustment trend of the previous cycle, the direction of the output control amount remains unchanged, and the magnitude is based on the internal force and the position adjustment amount in the previous cycle. weight selection;

2) If the direction of the internal force F _i and the position adjustment amount X of the previous cycle is different, the adjustment direction of the internal force of the robot in this cycle is different from the adjustment direction of the previous cycle, reducing the output control amount;

At the same time, compare the weight of the position adjustment amount X in the previous cycle with the weight of the internal force F _i . If the weight of the position adjustment amount X in the previous cycle is large, the smaller control output is still selected according to the direction of the previous cycle, and the "inertial effect" in the adjustment direction is maintained. ”; if the weight of the internal force F _i is large, select the smaller control output according to the adjustment direction of the internal force;

3) Defuzzification: The center of gravity method is used to make fuzzy judgment on the output control quantity, and the controller outputs the specific position adjustment quantity.

Further, in the control method of the double-robot coordinated operation of the weakly rigid member of the present invention, step 3-3 specifically includes:

1) There is no response lag error at the fixed end:

When the end of robot A is a fixed end and there is no error caused by the dynamic response lag, the error at the end of robot B includes the coordinate system calibration error Δd, the end of robot B due to eccentric load The dynamic response lag error Δs ₂ , the robot B As the dynamic adjustment end, under the internal force control system of single-end adjustment, the position adjustment amount ΔD of robot B will compensate the deviation of the end to a certain extent. The control execution instructions of the two robots are:

x ₁ (n+1)=x _obj (n+1) · T ₁

x ₂ (n+1)=x _obj (n+1)·T ₂ +ΔD

ΔD=Δd+Δs ₂

In the formula, x ₁ (n+1) and x ₂ (n+1) represent the dual robot end trajectory to be issued, x _obj (n+1) represents the theoretical motion trajectory of the object planned in the control cycle, T ₁ , T ₂ represent the transformation matrix of the object trajectory and the robot end trajectory, respectively;

2) There is a response lag error at the fixed end:

When the end of robot A is used as a fixed end, there is a response lag error Δs ₁ caused by eccentric load, and there is a coordinate system calibration error Δd and a response lag error Δs ₂ at the end of robot B. When the eccentric load causes the actual response position of the end robot When it lags behind the position issued by the theoretical control layer, the response lag error Δs ₁ of the fixed end of robot A is calculated as follows:

为机器人上个控制周期的实际位置；In the formula, x ₁ (n) is the delivery position of the theoretical control layer of the previous control cycle, and

is the actual position of the robot in the last control cycle;

When there is a lag response deviation Δs ₁ at the fixed end, the position adjustment amount ΔD output by the fuzzy controller during the internal force control process not only includes all the position errors of robot B, but also includes the response lag deviation Δs ₁ of robot A. Finally, the overall track offset Δs ₁ and Δs ₁ are used as the compensation amount Δe_comp of the trajectory tracking error of the operation object. At this time, the position control commands are:

x ₁ (n+1)=x _obj (n+1) · T-Δs ₁

x ₂ (n+1)=x _obj (n+1)·T ₂ +ΔD-Δs ₁

ΔD=Δd+Δs ₁ +Δs ₂ .

A control system for dual robots to coordinately operate weak rigid components, comprising a dual robot motion guidance module, an internal force control and trajectory compensation module, and a multi-sensor UDP and TCP/IP hybrid communication module, wherein: the dual robot motion guidance module includes a force The coordinate transformation module, the stiffness control module and the guiding force threshold control module, wherein the force coordinate transformation module obtains the coordinate system transformation relationship of the sensor relative to the world coordinate system, and the stiffness control module obtains the stiffness control relationship and converts the force information into the object's The motion trajectory is then decoupled through the kinematic closed-loop chain to drive each sub-robot system. The guiding force threshold control module controls the output guiding force vector by setting the guiding force threshold, reducing the force signal fluctuation in the initial state of the sensor and causing the object position fluctuation;

The internal force control and trajectory compensation module includes a multi-robot kinematics calculation module, a trajectory tracking error compensation module, an internal force single-end adjustment module, and a single-robot servo control module. The closed-loop kinematics model solves the motion trajectories of each robot end, which is compensated by the trajectory accuracy compensation module and then output to the single robot servo control module. The trajectory tracking error compensation module calculates the trajectory tracking error online to generate the trajectory real-time compensation amount and superimpose it on each robot. On the end trajectory of , the internal force single-end adjustment module calculates the object internal force of the robot at the adjustment end in real time according to the signal of each robot end sensor, and outputs the position compensation amount of the end of the robot at the adjustment end through the fuzzy controller based on the impedance model.

Compared with the prior art, the present invention adopts the above technical scheme, and has the following technical effects:

1. The guiding trajectory of the present invention meets the safety restriction requirements of the robot Cartesian space and joint space at the same time, solves the dangerous guiding trajectory problem caused by too large guiding force or sudden change in the guiding control algorithm, and improves the guiding Smoothness and safety of trajectories.

2. The present invention greatly improves the response speed of the system while ensuring the stability, and can reduce the tracking error of the object trajectory while ensuring the internal force control effect, which is of great significance for industrial robots to realize real-time internal force control based on position.

Description of drawings

Figure 1 is a schematic diagram of a double robot motion closed-loop system.

Figure 2 is a schematic diagram of the internal force model of the dual robot.

Fig. 3 is a trajectory error analysis diagram.

FIG. 4 is a schematic diagram of the internal force control of the operation object.

Figure 5 is a schematic diagram of a fuzzy control architecture based on an impedance model.

FIG. 6 is a schematic diagram of membership functions.

FIG. 7 is a schematic diagram of trajectory error compensation when the fixed end has no response lag error.

FIG. 8 is a schematic diagram of trajectory error compensation when there is a response lag error at the fixed end.

Detailed ways

Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein the same or similar reference numerals refer to the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, but not to be construed as a limitation of the present invention.

Example 1

A control method for two robots to coordinately operate weak rigid components, comprising the following steps:

Step 1: Take the weakly rigid component as the direct manipulation object, build the kinematic closed-loop chain model of the dual robot, and design the dual robot guidance control based on the model, the dual robot guidance control includes force coordinate transformation, stiffness control and guidance Force threshold control.

The core goal of the multi-robot coordinated control system is to achieve the desired motion of the object to be gripped. It is simpler and more intuitive to directly use the operating object as the control object, and it is easier to realize the complex motion trajectory of the object to be gripped.

The end of the robot tool and the grasping point of the object are regarded as a fixed connection and there is no relative movement. At this time, the double robot and the operating object form a kinematic closed-loop chain. The closed-loop motion model of the double robot is shown in Figure 1 below, and the geometric relationship in Figure 1 shows that , the kinematic model of the object coordinate system {c} relative to the world coordinate system {W} is:

^W T _c = ^W T _R1 · ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c

^W T _c = ^W T _R2 · ^R2 T _e2 · ^e2 T _c2 · ^c2 T _c

Among them: {W} represents the system world coordinate system; {R1}, {R2} represent the respective base coordinate systems of robot A and robot B; {e1}, {e2} represent the end tool coordinate system of robot A and robot B, respectively , which coincides with the grasping point at the end of the robot; {c} represents the coordinate system of the operating object. In order to realize the complex guidance trajectory, the origin of the reference system can be adjusted by moving; {c1} and {c2} represent {e1}, { e2} The coordinate system after translation and extension, and the origin of the coordinate system coincides with the origin of {c}; ^c1 T _c , ^c2 T _c respectively represent the homogeneous pose of the object coordinate system {c} relative to {c1}, {c2} Change matrix; ^e1 T _c1 , ^e2 T _c2 represent the homogeneous transformation matrix of {c1}, {c2} relative to the robot end coordinate system {e1}, {e2} respectively; ^R1 T _e1 , ^R2 T _e2 respectively represent the robot end coordinate are the homogeneous transformation matrices of the robot base coordinate system and their respective base coordinate systems; ^W T _R1 and ^W T _R2 represent the homogeneous transformation matrices of the robot base coordinate systems {R1} and {R2} relative to the world coordinate system {W}, respectively; ^W T _R1 and ^W T _R2 is the transformation relationship between the installation position of each robot base and the world coordinate system, and is a fixed constant matrix.

In order to facilitate control and ensure the calibration accuracy, the world coordinate system {W} and the base coordinate system {R1} of one of the robots are coincident. At this time, only the transformation matrix ^R1 T _R2 between the respective base coordinate systems of the two robots is Yes, the four-point calibration method is used to determine the transformation relationship ^R1 T _R2 between the base standard systems.

After obtaining the base-mark conversion relationship, the end virtual link matrix between the two robot ends is determined by the kinematic closed-loop chain:

^e1 T _e2 = [ ^R1 T _e1 ] ^-1 · ^R1 T _R2 · ^R2 T _e2

In order to facilitate the operator to guide the movement of the object according to his own operation intention, the position adjustment ratio K is introduced on the basis of the virtual link matrix ^e1 T _e2 to determine the origin of the double robot end translation coordinate systems {c1} and {c2}. The object coordinate system {c} is established at the origin of the translation coordinate system {c1} and {c2} of the robot end. Therefore, the position of the object coordinate system can be changed only by adjusting the ratio K (0~1).

Since the translation position of the robot end coordinate system is determined by the position adjustment ratio K, the homogeneous transformation matrix between the robot end tool coordinate systems {e1}, {e2} and their translated coordinate systems {c1}, {c2} for:

In the formula, E ₃ represents the 3rd-order unit matrix, O _1×3 represents the zero vector with 1 row and 3 columns, and P{·} represents the position vector for extracting the homogeneous matrix, ^e1 T _c1 and ^e2 T _c2 only follow ^e1 T The position vector of _e2 is moved. And ^c1 T _c2 and ^c2 T _c1 only change according to the rotation matrix ^e1 R _e2 of ^e1 T _e2 .

At the same time, establish the coordinate system transformation relationship between the object coordinate system {c} and the end translation coordinate systems {c1}, {c2}, where A is a constant homogeneous rotation matrix:

^c T _c2 = ^c T _c1 · ^c1 T _c2

Finally, the kinematic drive control equation of each robot can be determined through the closed-loop kinematics model of the two robots:

^R1 T _e1 = ^R1 T _c · [ ^e1 T _c ] ^-1 = [ ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c ] · [ ^e1 T _c1 · ^c1 T _c ] ^-1

^R2 T _e2 = [ ^R1 T _R2 ] ^-1 · ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c · [ ^e2 T _c2 · ^c2 T _c1 · ^c1 T _c ] ^-1

By constantly changing the homogeneous change matrix ^R1 T _c of the operating object in the world coordinate system {R1}, the pose homogeneous matrices ^R1 T _e1 and ^R2 T _e2 in the respective coordinate systems of the dual robots can be decoupled and calculated, and substituted into the respective single robots. Inverse kinematics can solve the driving angle of each joint.

Step 2: According to the kinematic constraint relationship between the robot end and the operating object, design the multi-space adaptive interpolation control of the guiding trajectory in Cartesian space and joint space.

The position constraints in the kinematic constraint relationship are:

Calculate the end pose matrix of the robot according to the position of the operating object:

^R1 T _c = ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c = ^R1 T _R2 · ^R2 T _e2 · ^e2 T _c2 · ^c2 T _c

^R1 T _c is the homogeneous change matrix of the operation object in the world coordinate system {R1}, ^R1 T _e1 , ^R2 T _e2 are the homogeneous matrix of the dual robots in their respective coordinate systems, ^c1 T _c , ^c2 T _c , ^e1 T _c1 , ^e2 T _c2 are constant matrices determined by the object grasping method, and ^R1 T _R2 is the base coordinate system calibration matrix.

The velocity constraint in the kinematic constraint relationship is:

When the rigid grasping method is adopted between the double robot end and the operation object, the speed relationship between the operation object and the double robot end is:

v _c , w _c represent the linear velocity and angular velocity of object motion, v _ei , w _ei represent the linear velocity and angular velocity of the i-th robot end, r _ei =[ _{reix r eiy r eiz} ] ^T means that the i-th robot end is in the position vector in the object coordinate system;

Then, the speed constraint equation of the robot end is:

E₃表示3阶单位矩阵，O₃表示3阶零矩阵。in,

E ₃ represents a unit matrix of order 3, and O ₃ represents a zero matrix of order 3.

The acceleration constraint in the kinematic constraint relationship is:

The acceleration constraint relationship between the operation object and the robot end is:

Then, the acceleration constraint equation of the robot end is:

According to the excessive angular velocity and/or excessive angular acceleration of the operating object, the rotation guidance trajectory is interpolated:

1) When the angular velocity of the operating object is too large and the angular velocity of the end of the robot exceeds the limit, calculate the over-limit ratio R _{velo_ei} of the angular velocity of each robot end, select the maximum over-limit ratio R _{velo_END} for deceleration interpolation, and obtain:

R _{velo_ei} =ABS(w _ei )/w _{max_ei} ,i∈[1,2]

R _{velo_END} =max(R _{velo_e1} ,R _{velo_e2} ),R _{velo_END} >1

R _{velo_END} is the adjustment parameter of the guidance trajectory interpolation, and the rotation angle of the discrete guidance trajectory of the operation object is interpolated based on the constraints of the angular velocity of the robot end:

Δθ _{vcal_END} (n)=Δθ _initial (n)/R _{velo_END}

2) When the angular acceleration of the operating object is too large and the angular acceleration of the end of the robot exceeds the limit, calculate the excess ratio R _{accel_ei} of the angular acceleration of the end of each robot, select the maximum excess ratio R _{accel_END} for interpolation, and obtain:

R _{accel_END} =max(R _{accel_e1} ,R _{accel_e2} ),R _{accel_END} >1

R _{accel_END} is the adjustment parameter of the guidance trajectory interpolation, and the rotation angle of the guidance trajectory of the operation object is interpolated based on the angular acceleration constraint of the robot end:

3) When the angular velocity of the operating object is too large and the angular acceleration is too large at the same time, which causes the robot to exceed the limit, the minimum value of the two is selected as the rotation angle of the guiding trajectory of the interpolation operating object;

That is, Δθ _{cal_END} is expressed as the discrete trajectory point output value of this cycle as:

After the rotation guidance trajectory of the operating object is interpolated, recalculate the speed _{ve ei} , acceleration wi _ei of the robot end, and the discrete point Δx _initial (n) of the object guidance trajectory, and then recalculate the movement speed and/or acceleration of the operating object according to the If it is too large, it will be interpolated for its moving guide trajectory:

1) When the moving speed of the object is too large and the speed of the robot end exceeds the limit, calculate the speed overrun ratio R _{velo_ei} of each robot end, and select the maximum overrun ratio R _{velo_END} to perform deceleration interpolation, and obtain:

R _{velo_ei} =ABS(v _ei )/v _{max_ei} ,i∈[1,2]

R _{velo_END} =max(R _{velo_e1} ,R _{velo_e2} ),R _{velo_END} >1

R _{velo_END} is the adjustment parameter of the guidance trajectory interpolation, and the discrete trajectory points (including position and attitude) of the object guidance trajectory are interpolated with the robot end speed constraint as follows:

Δx _{vcal_END} (n)=Δx _initial (n)/R _{velo_END}

2) When the moving acceleration of the object is too large and the acceleration of the robot end exceeds the limit, calculate the acceleration overrun ratio R _{accel_ei} of each robot end, select the maximum overrun ratio R _{accel_END} for interpolation, and obtain:

R _{accel_END} =max(R _{accel_e1} ,R _{accel_e2} ),R _{accel_END} >1

R _{accel_END} is the adjustment parameter of the guidance trajectory interpolation, and the discrete trajectory points (including position and attitude) of the object guidance trajectory are interpolated with the robot end acceleration constraints as follows:

3) When the operating object has both too large moving speed and too large moving acceleration, which causes the robot to exceed the limit, the minimum value of the two is selected as the discrete trajectory point of the guiding trajectory of the interpolation operating object;

That is, Δx _{cal_END} is expressed as the discrete trajectory point output of this cycle as:

According to the motion constraints of each joint, the joint space adaptive interpolation is performed on the guiding trajectory:

最大限制加速度为

机器人末端速度与关节速度之间通过机器人雅克比矩阵J(q_i)表示,两机器人的关节速度

表示为：Set the maximum joint speed limit of each robot in the joint space as

The maximum limit acceleration is

The relationship between the robot end speed and the joint speed is represented by the robot Jacobian matrix J(q _i ), the joint speed of the two robots

Expressed as:

Interpolate the joint guidance trajectory of the robot according to the excessive joint speed and/or joint acceleration of the robot: 1) When the joint speed of the robot is too large and the limit exceeds the limit, calculate the ratio of the joint angular velocity of each joint of the robot exceeding the limit, and select the maximum The over-limit ratio R _{velo_Ji} is interpolated to decelerate, and we get:

R _{velo_JOINT} =max(R _{velo_J1} ,R _{velo_J2} )

R _{velo_JOINT} is the adjustment parameter of the guidance trajectory interpolation. Interpolate the discrete guidance trajectory points Δx _{vcal_JOINT} after Cartesian space interpolation with the joint angular velocity overrun ratio per unit time, and obtain:

Δx _{vcal_JOINT} (n)=Δx _{cal_END} (n)/R _{velo_JOINT}

2) When the joint acceleration of the robot is too large and the overrun is caused, calculate the overrun ratio of the joint angular acceleration of each joint of the robot, select the maximum overrun ratio R _{accel_Ji} for interpolation, and obtain:

R _{accel_JOINT} =max(R _{accel_J1} ,R _{accel_J2} )

R _{accel_JOINT} is the adjustment parameter of the guidance trajectory interpolation, and the discrete guidance trajectory point Δx _{cal_END} after Cartesian space interpolation is interpolated with the robot joint angular acceleration overrun ratio to obtain:

3) When the joint speed of the robot is too large and the acceleration of the joint is too large at the same time, which causes the limit to exceed, the smaller value of the two is selected as the interpolation;

That is, Δx is expressed as the discrete trajectory point output of this cycle as:

Step 3: Design dual-input fuzzy control and object trajectory tracking error compensation control based on impedance model according to the internal force calculation model of the operating object.

Considering that the dual robot system jointly grips an object, the gripping point is rigidly fixed by default without relative sliding of the position, as shown in Figure 2. In the process of motion execution, each robot end will apply force vector f _i and moment vector _mi to the operating object, and the object will be subjected to the resultant force F _obj exerted by the robot end. The relationship is as follows:

表示对象坐标系与第i个机器人末端坐标系之间的雅克比变化矩阵，其矩阵表达式：In the formula,

Represents the Jacobian change matrix between the object coordinate system and the ith robot end coordinate system, and its matrix expression:

In the formula, ( _{re ei} ) ^× is the obliquely symmetric matrix of the position vector r _ei =[ _{reix r eiy r eiz} ] ^T from each end to the object coordinate system. E ₃ represents a 3-dimensional identity matrix, and O ₃ represents an empty matrix.

From the point of view of the force decomposition of the robot end, part of the force exerted by the robot end is converted into the motion driving force f _M to drive the object to move (usually called the external force applied by the robot to the object), and the other part is converted into the object internal force f _I , and Object internal forces do not affect the motion state of the object. Except in special cases, the internal force will generally have a negative impact on the system control, so the internal force should be controlled to the minimum value as much as possible to avoid a series of deformation and damage to the operation object during the assembly process. It can be seen from the above that the decomposition expression of the robot end force is:

f=f _M + f _I

的零空间，由此可得对象运动驱动力和内力的计算公式：Since the internal force f _I does not produce a net resultant force on the object, it is located at

The null space of , from which the calculation formulas of the driving force and internal force of the object motion can be obtained:

Step 3-1: Adopt the benchmark adjustment strategy of internal force control, fix the robot end on one side as the control benchmark, and adjust the robot end on the other side according to the internal force.

Since the internal force exerted by the two robot ends on the object is a set of interaction forces, controlling one end can achieve internal force control. The "reference adjustment" strategy is to fix the robot end on one side as the control reference, and the robot end on the other side adjusts accordingly according to the internal force. Single-ended adjustment can effectively shorten the stabilization time, and can achieve better internal force control effect in the fixed control cycle of the control system.

In order to ensure the control accuracy of the internal force control system, it is necessary to analyze the model error of the internal force system before determining the internal force fixed end and the internal force adjustment end. The control error of the closed-loop kinematics model of the dual-robot mainly occurs in the calibration results between the base frames. Since the world coordinate system coincides with the base coordinate system of one of the robots, the respective ends of the two robots are not affected to the same extent by the calibration results of the base coordinate system.

的影响，基座系的标定结果

与真实值^R1T_R2之间的关系为：Subject to calibration error

The effect of , the calibration results of the base system

The relationship with the true value ^R1 T _R2 is:

其中

和

受标定结果的位置误差和姿态误差影响，

和

受

的位置误差影响，

则受

的姿态误差影响。The intermediate term of the control process affected by the error term is:

in

and

Affected by the position error and attitude error of the calibration result,

and

by

The position error effect of ,

be subject to

attitude error.

在计算过程中可与其逆矩阵

相抵消，机器人A基本不受影响，所以标定误差主要表现在机器人B的末端位姿表达式中。In order to facilitate control implementation and reduce calibration errors, the world coordinate system of the dual-robot collaborative system coincides with the base coordinate system of robot A. The calculation chain of the base coordinate system of robot A is obviously shorter than that of robot B. and the error term

In the calculation process, the inverse matrix can be

In contrast, robot A is basically unaffected, so the calibration error is mainly manifested in the expression of the end pose of robot B.

Based on the above error analysis results, the control accuracy of the single-end adjustment of the internal force is analyzed in detail. In Figure 3, the triangles numbered 1 and 2 represent the end of the robot, the concentric circles represent the object being handled, the dashed line represents the initial trajectory affected by the calibration error, and the solid line represents the actual motion trajectory of each robot. If the fixed end is selected on the side with high control accuracy (the side of number 1 in Figure 3(a)), the initial trajectory deviation of the adjustment end is directly compensated during the internal force control process, thereby ensuring the position control accuracy of the operating object, As shown in Figure 3(a). If the fixed end is selected on the side with the larger position error (the side with the number 2 in Figure 3(b)), the robot adjusting end on the other side will adjust the motion along with the fixed end during the internal force control process. The trajectories of the fixed ends are all offset by the influence of the trajectory deviation of the fixed ends, as shown in Fig. 3(b). Therefore, when the dual-robot system adopts the "reference adjustment" strategy for internal force control, robot A is used as the fixed end, and robot B is used as the motion adjustment end.

Step 3-2: Establish the internal force fuzzy control framework based on the impedance model: input the internal force deviation and the pose adjustment amount of the previous cycle to the fuzzy controller, then output the pose adjustment amount of the current cycle and control the internal force of the dual robot system.

The internal force control model of the dual robot system is equivalent to creating a force and position transformation rule at the internal force adjustment end, as shown in Figure 4. The impedance model is equivalent to a 6-dimensional virtual spring damper in space, which converts the internal force into the position adjustment amount Δx of the adjustment end of the robot's internal force. Δx includes both the displacement adjustment amount and the rotation adjustment amount, which is the same as the method used by the force sensor to calculate the pose increment of the guiding action of the object. The position compensation converted from the internal force force vector is directly superimposed on the actual position amount for output, while the internal force The attitude compensation of torque vector conversion needs to be converted into the corresponding Euler angle compensation, and then the Euler angle compensation is superimposed on the actual Euler angle for output, so as to realize the 6-dimensional control of the internal force in space.

In the process of internal force control, the system detects the internal force deviation between the object and the end of the double robot in real time, ΔF _i =F _i -F _id , where F _i is the actual internal force of the system, and F _id is the expected value of the internal force. Usually, F _id =0 is used to obtain the maximum value. Excellent internal force control effect. Once the internal force is detected, the dual robot system enters the internal force control mode, and the impedance relationship between the internal force deviation ΔF _i and the position adjustment amount Δx is:

分别表示机器人末端位置调整量及其调整量变化速度。where B and K are the damping coefficient and stiffness coefficient matrices in impedance control, respectively, Δx and

Respectively represent the adjustment amount of the robot end position and the change speed of the adjustment amount.

In the control system, the change speed of the compensation amount is obtained from the time difference between the previous and previous control cycles, n represents the current control cycle, and n-1 represents the last executed cycle, thus obtaining:

The discrete mathematical relationship between the internal force difference ΔF _i and the position adjustment amount Δx is:

In order to obtain the expression of the final robot end position adjustment Δx, then:

作为模糊控制器的输入控制量，机器人末端调整量的输出函数为：If the traditional fuzzy controller is applied in the dual robot internal force control system, the internal force deviation ΔF and its deviation change speed are usually used.

As the input control quantity of the fuzzy controller, the output function of the adjustment quantity of the robot end is:

In order to combine the two control methods of impedance control and fuzzy control, ΔF _i (n) and Δx (n-1) are used as the input control variables of the fuzzy controller. On the premise of ensuring the high response characteristics of the fuzzy control, the impedance control is retained. The second-order system characteristics of , the fuzzy system is equivalent to adaptive reasoning control of Δt/(KΔt+B) and B/(KΔt+B) including stiffness coefficient K and damping coefficient B algebraic term, avoiding the impedance control of K , the value problem of B. The position adjustment output function of this fuzzy architecture is:

Δx(n)=Fuzzy(ΔF _i ,Δx(n-1))

From this, a dual-input fuzzy control architecture based on the impedance model is obtained, as shown in Figure 5. By inputting the internal force deviation and the pose adjustment amount of the previous cycle to the fuzzy controller, the pose adjustment amount of the current cycle can be output and the internal force control of the dual robot system can be realized.

On the basis of the dual-input internal force fuzzy control architecture based on the impedance model, the dual-input fuzzy control algorithm needs to be designed in detail in combination with the actual application scenario of dual-robot internal force control. The algorithm mainly includes three parts: variable fuzzification of internal force difference ΔF _i and position adjustment Δx, fuzzy rule design, and de-fuzzification.

(1) Fuzzy input and output

In order to ensure the safety of the internal force control of the dual robot system, a safe internal force limit value is first determined. If the internal force limit is exceeded, it will be controlled according to the maximum limit internal force to avoid the sudden change of internal force caused by excessive position adjustment. At the same time, in the area with large internal force, the universe of discourse is sparsely divided to make the internal force respond quickly; the area with small internal force is divided densely, so as to improve the internal force control accuracy as much as possible. Therefore, the domain of the internal force is selected as F _i ={-12,-6,-3,0,3,6,12}, and the corresponding fuzzy subset A _i (i=1,2,3,4, 5,6) are expressed in language as: {NB, NM, NS, Z, PS, PM, PB}, and at the same time, the triangular membership function with less calculation amount in the actual control is used for variable fuzzification, and the membership function u of the internal force F _{i A} ( _Fi ) is shown in Fig. 6(a).

Since the end of the robot and the object are rigidly connected to achieve tight coordination control, a small position deviation will generate a large internal force. The choice of the universe of discourse for the position deviation should not be too large. Similarly, the principle of "sparse outside and dense inside" is adopted when dividing the universe of internal force. Select the universe of discourse X={-0.1,-0.05,-0.02,0,0.02,0.05,0.1}, the corresponding fuzzy subset B _i (i=1,2,3,4,5 , 6) is expressed as: {NB, NM, NS, Z, PS, PM, PB}. In the same way, the output control quantity of the system is also the position adjustment quantity, which takes the universe of discourse X1={-0.1,-0.05,-0.02,0,0.02,0.05,0.1}, and the corresponding fuzzy subset C _i (i = 1, 2, 3, 4, 5, 6) are expressed as: {NB, NM, NS, Z, PS, PM, PB}. The variable fuzzification is carried out by using the triangular membership function with a small amount of calculation in the actual control. The membership function u _B (X) of the input position adjustment value X and the output position adjustment value X1 membership function u _C (X1) are the same, as shown in Figure 6(b) ) shown.

(2) Fuzzy rule design

The improved dual-input fuzzy controller takes the internal force and the position adjustment amount of the previous cycle as input, and the position adjustment amount of the previous cycle reflects the adjustment trend of the robot end in the previous cycle, and it will be used as a feedforward judgment to decide the position adjustment of this cycle. amount of choice. Usually, the internal force has positive and negative points of tension and pressure, and the position adjustment also has a one-to-one corresponding positive and negative control amount to adjust the internal force. Therefore, the formulation principles of the fuzzy control inference rules of this system are as follows:

1) If the internal force F _i is in the same direction as the position adjustment value X of the previous cycle, it means that the robot is still in the internal force adjustment trend of the previous cycle, the direction of the output control value remains unchanged, and the magnitude is adjusted according to the internal force and the position of the previous cycle. Amount of weight to choose.

2) If the direction of the internal force F _i and the position adjustment amount X of the previous cycle is different, it means that the adjustment direction of the internal force of the robot in this cycle is different from the adjustment direction of the previous cycle, and the output control amount should be reduced as appropriate. At the same time, compare the weight of the position adjustment amount X in the previous cycle with the weight of the internal force F _i . If the weight of the position adjustment amount X in the previous cycle is large, the smaller control output is still selected according to the direction of the previous cycle, and the "inertial effect" in the adjustment direction is maintained. ; If the weight of the internal force F _i is large, select the smaller control output according to the internal force adjustment direction to ensure the smooth transition of the internal force adjustment direction.

On the basis of maintaining the original characteristics of fuzzy control, the control principle introduces the second-order system characteristics of spring damping in impedance control. The control amount of the previous cycle is used as an inertial amount to predict the behavior of this cycle. Finally, the fuzzy control rule table is summarized and formulated as follows:

Table 1 Fuzzy control rule table

(3) Defuzzification

After the fuzzy inference is completed based on the fuzzy rule base, the output position adjustment amount X1 is obtained as a fuzzy set expressed in fuzzy language, so the last step of the fuzzy controller is to make a fuzzy judgment on the output control amount to obtain the specific position adjustment amount X1. Among them, the center of gravity method is the most commonly used deblurring method. The research results of some scholars also show that the output of the center of gravity method is continuous rather than jumping, and can give a relatively stable output. The calculation formula is as follows:

In the formula, μ _A (F _i ) and μ _B (X) are the functional membership degrees of the internal force and the position adjustment of the previous cycle, respectively, Δx _ij is the output domain value corresponding to different membership degrees in the fuzzy inference rule base, and the final fuzzy decision The output Δx of the fuzzy controller is obtained as the position adjustment amount of this cycle, and it is saved for the calculation of the next cycle.

Step 3-3: Compensate the trajectory according to whether there is a response lag error at the fixed end.

(1) There is no response lag error at the fixed end

Through error analysis, it is known that the mechanical error of the robot body caused by the mechanical structure and equipment usage is usually small, and it is difficult to calculate the size of the robot mechanical error in real time in the online real-time control of industrial robots, so it is ignored in this paper. The calibration of the multi-robot base calibration system is the basis of the coordinated control work, but the inevitable calibration error is mainly manifested in the robot B side in the mechanical system of this paper.

When the end of robot A is a fixed end and there is no error caused by the dynamic response lag, the possible errors at the end of robot B mainly include the coordinate system calibration error Δd and the dynamic response lag error Δs of the end of robot B due to eccentric load. ₂ , as shown in Figure 7. Robot B is used as the dynamic adjustment end. Under the internal force control system of single-end adjustment, the position adjustment amount ΔD of robot B will compensate the deviation of the end to a certain extent. The control execution instructions of the two robots are:

x ₁ (n+1)=x _obj (n+1) · T ₁

x ₂ (n+1)=x _obj (n+1)·T ₂ +ΔD

ΔD=Δd+Δs ₂

In the formula, x ₁ (n+1) and x ₂ (n+1) represent the dual robot end trajectory to be issued, x _obj (n+1) represents the theoretical motion trajectory of the object planned in the control cycle, T ₁ , T ₂ represent the transformation matrix of the object trajectory and the robot end trajectory, respectively.

(2) When there is a response lag error at the fixed end

When the fixed end of robot A has response lag error Δs ₁ caused by eccentric load, the end of robot B still has coordinate system calibration error Δd and response lag error Δs ₂ . During the internal force control process, the trajectory of the object and the trajectory of the adjustment end will also be offset, as shown in Figure 8.

When the actual response position of the terminal robot lags behind the position issued by the theoretical control layer due to eccentric load, the lag error at this time can be calculated in real time in the controller, so first calculate the response lag error Δs ₁ of the fixed end of robot A as follows:

为机器人上个控制周期的实际位置，通过关节电机实时角度反馈进行机器人正运动学求解获得。In the formula, x ₁ (n) is the delivery position of the theoretical control layer of the previous control cycle, and

is the actual position of the robot in the last control cycle, obtained by solving the forward kinematics of the robot through the real-time angle feedback of the joint motor.

When there is a lag response deviation Δs ₁ at the fixed end, the position adjustment amount ΔD output by the fuzzy controller during the internal force control process not only includes all the position errors of robot B, but also includes the response lag deviation Δs ₁ of robot A. At this time, the running track of the object will be offset as a whole after the single-end adjustment. In order to compensate the trajectory tracking accuracy of the object, each robot end finally sends the overall trajectory offset Δs ₁ , so as to compensate the target trajectory error caused by various errors at the fixed end and the adjustment end to a certain extent, so Δs ₁ will be used as the operation object The compensation amount Δe_comp of the trajectory tracking error. The position control commands at this time are:

x ₁ (n+1)=x _obj (n+1) · T-Δs ₁

x ₂ (n+1)=x _obj (n+1)·T ₂ +ΔD-Δs ₁

ΔD=Δd+Δs ₁ +Δs ₂

Example 2

A dual-robot coordinated control system includes a dual-robot motion guidance module, an internal force control and trajectory compensation module, and a multi-sensor UDP and TCP/IP hybrid communication module.

The dual-robot motion guidance module includes a force coordinate transformation module, a stiffness control module, and a guidance force threshold control module. The force coordinate transformation module obtains the coordinate system transformation relationship of the sensor relative to the world coordinate system, and the stiffness control module obtains the stiffness control relationship. The force information is converted into the motion trajectory of the object, and then each sub-robot system is driven by the kinematic closed-loop chain decoupling. The guiding force threshold control module controls the output guiding force vector by setting the guiding force threshold, reducing the initial state of the sensor. The position of the object fluctuates due to the fluctuation of the force signal.

The internal force control and trajectory compensation module includes a multi-robot kinematics solution module, a trajectory tracking error compensation module, an internal force single-end adjustment module, and a single robot servo control module. The object internal force control system is divided into four parts: multi-robot kinematics calculation module, trajectory accuracy compensation module, internal force single-end adjustment module, and single-robot servo control module. Calculate the motion trajectory of each robot end, and then drive the single robot servo control module to run; the sensors at each robot end perform force decoupling in real time to calculate the internal force of the object at the regulating end robot B, and transmit it to the improved fuzzy control based on the impedance model The position compensation amount of the end of robot B is output in the device to realize the internal force control of the object in the dual robot system; then the trajectory tracking error is calculated online through the designed object trajectory tracking error compensation algorithm, and the trajectory deviation amount is superimposed as the trajectory real-time compensation amount To the end trajectory of each robot, the adaptive online compensation of the object trajectory tracking error is realized, and the trajectory tracking accuracy of the dual-robot system in the case of response lag is improved.

The above are only some embodiments of the present invention. It should be pointed out that for those skilled in the art, some improvements can be made without departing from the principles of the present invention, and these improvements should be regarded as the present invention. scope of protection.

Claims (5)

1. a control method of a double robot coordinated operation weak rigid member, is characterized in that, comprises the following steps: Step 1: Take the weakly rigid component as the direct manipulation object, build the kinematic closed-loop chain model of the dual robot, and design the dual robot guidance control based on the model, the dual robot guidance control includes force coordinate transformation, stiffness control and guidance force threshold control; Step 2: According to the kinematic constraint relationship between the robot end and the operating object, design the multi-space adaptive interpolation control of the guiding trajectory in Cartesian space and joint space, including: Step 2-1: According to the excessive angular velocity and/or excessive angular acceleration of the operating object, interpolate the rotation guidance trajectory; Step 2-2: Recalculate the speed and acceleration of the robot end and the discrete points of the object guiding trajectory, and then interpolate the movement guiding trajectory according to the excessive moving speed and/or excessive moving acceleration of the operating object; Step 2-3: According to the motion constraints of each joint, perform adaptive interpolation of the joint space on the guiding trajectory; Step 3: Design the dual-input fuzzy control and object trajectory tracking error compensation control based on the impedance model according to the internal force calculation model of the operating object, including: Step 3-1: Adopt the benchmark adjustment strategy of internal force control, fix the robot end on one side as the control benchmark, and adjust the robot end on the other side according to the internal force; Step 3-2: Establish the internal force fuzzy control framework based on the impedance model: input the internal force deviation and the pose adjustment amount of the previous cycle to the fuzzy controller, then output the pose adjustment amount of the current cycle and control the internal force of the dual robot system; Step 3-3: Compensate the trajectory according to whether there is a response lag error at the fixed end. 2. the control method of double robot coordinated operation weak rigid member according to claim 1, is characterized in that, the kinematics closed-loop chain model of establishing double robot in step 1 specifically comprises the following steps: Step 1-1: The kinematic model of the operation object coordinate system {c} relative to the world coordinate system {W} is: ^W T _c = ^W T _R1 · ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c ^W T _c = ^W T _R2 · ^R2 T _e2 · ^e2 T _c2 · ^c2 T _c ^W T _R1 and ^W T _R2 are the transformation relationship between the installation position of each robot base and the world coordinate system, and are fixed constant matrices; {R1}, {R2} represent the respective base coordinate systems of robot A and robot B; {e1 } and {e2} represent the end tool coordinate systems of robot A and robot B, respectively, which coincide with the gripping point at the end of the robot; {c1} and {c2} represent the coordinate systems after translation and extension of {e1} and {e2}, respectively, and The origin of the coordinate system coincides with the origin of {c}; ^c1 T _c , ^c2 T _c represent the homogeneous change matrix of the object coordinate system {c} relative to {c1}, {c2} respectively; ^e1 T _c1 , ^e2 T _c2 respectively represent the homogeneous transformation matrices of {c1} and {c2} relative to the robot end coordinate systems {e1} and {e2}; ^R1 T _e1 and ^R2 T _e2 respectively represent the homogeneous transformation between the robot end coordinate system and their respective base coordinate systems matrix; ^W T _R1 and ^W T _R2 represent the homogeneous transformation matrix of the robot base coordinate system {R1}, {R2} relative to the world coordinate system {W}, respectively; Step 1-2: The world coordinate system {W} is coincident with the base coordinate system {R1} of one of the robots, and the four-point calibration method is used to determine the transformation relationship between the base coordinate systems ^R1 T _R2 , then the kinematics The closed-loop chain determines the end virtual link matrix between the two robot ends: ^e1 T _e2 = [ ^R1 T _e1 ] ^-1 · ^R1 T _R2 · ^R2 T _e2 Step 1-3: On the basis of the virtual link matrix ^e1 T _e2 , the position adjustment ratio K is introduced to determine the origin of the double robot end translation coordinate systems {c1} and {c2}, and the object coordinate system {c} is established on the robot. The origin position of the end translation coordinate system {c1}, {c2}, the homogeneous transformation matrix between the robot end tool coordinate system {e1}, {e2} and its translated coordinate system {c1}, {c2} is:

In the formula, E ₃ represents the 3rd-order unit matrix, O _1×3 represents the zero vector with 1 row and 3 columns, and P{·} represents the position vector for extracting the homogeneous matrix, ^e1 T _c1 and ^e2 T _c2 only follow ^e1 T The position vector of _e2 moves, and ^c1 T _c2 and ^c2 T _c1 only change according to the rotation matrix ^e1 R _e2 of ^e1 T _e2 ; Step 1-4: Establish the coordinate system transformation relationship between the object coordinate system {c} and the end translation coordinate systems {c1}, {c2}, where A is a constant homogeneous rotation matrix: ^c1 T _c =A;

^c T _c2 = ^c T _c1 · ^c1 T _c2 Step 1-5: Determine the kinematic drive control equation of each robot through the closed-loop kinematics model of the dual robots: ^R1 T _e1 = ^R1 T _c · [ ^e1 T _c ] ^-1 = [ ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c ] · [ ^e1 T _c1 · ^c1 T _c ] ^{-1 R2} T _e2 = [ ^R1 T _R2 ] ^-1 · ^R1 T _e1 · ^e1 T _c1 · ^c1 T _c · [ ^e2 T _c2 · ^c2 T _c1 · ^c1 T _c ] ^{-1 R1} T _e1 , ^R2 T _e2 are the pose homogeneous matrices in the respective coordinate systems of the two robots. 3. The control method for the coordinated operation of weakly rigid components by double robots according to claim 1, wherein step 3-2 specifically includes variable fuzzification, fuzzy rule design and removal of internal force difference ΔF _i and position adjustment amount Δx. Fuzzy three parts, specifically: Step 3-2-1: Fuzzy input and output: Set the basic universe, determine the quantification factor and scale factor, use triangular membership function to fuzzify the data that does not exceed the boundary of the universe, and set the data beyond the boundary of the universe as the boundary value of the universe; Step 3-2-2: Design fuzzy rules: 1) If the internal force F _i and the position adjustment amount X of the previous cycle are in the same direction, the robot is still in the internal force adjustment trend of the previous cycle, the direction of the output control amount remains unchanged, and the magnitude is based on the internal force and the position adjustment amount in the previous cycle. weight selection; 2) If the direction of the internal force F _i and the position adjustment amount X of the previous cycle is different, the adjustment direction of the internal force of the robot in this cycle is different from the adjustment direction of the previous cycle, reducing the output control amount; At the same time, compare the weight of the position adjustment amount X in the previous cycle with the weight of the internal force F _i . If the weight of the position adjustment amount X in the previous cycle is large, the smaller control output is still selected according to the direction of the previous cycle, and the "inertial effect" in the adjustment direction is maintained. ”; if the weight of the internal force F _i is large, select the smaller control output according to the adjustment direction of the internal force; Step 3-2-3: Defuzzification: The center of gravity method is used to make fuzzy judgment on the output control amount, and the controller outputs the specific position adjustment amount. 4. The control method of the double-robot coordinated operation of weakly rigid components according to claim 1, wherein step 3-3 specifically comprises: 1) There is no response lag error at the fixed end: When the end of robot A is a fixed end and there is no error caused by the dynamic response lag, the error at the end of robot B includes the coordinate system calibration error Δd, the end of robot B due to eccentric load The dynamic response lag error Δs ₂ , the robot B As the dynamic adjustment end, under the internal force control system of single-end adjustment, the position adjustment amount ΔD of robot B will compensate the deviation of the end to a certain extent. The control execution instructions of the two robots are: x ₁ (n+1)=x _obj (n+1) · T ₁ x ₂ (n+1)=x _obj (n+1)·T ₂ +ΔD ΔD=Δd+Δs ₂ In the formula, x ₁ (n+1) and x ₂ (n+1) represent the dual robot end trajectory to be issued, x _obj (n+1) represents the theoretical motion trajectory of the object planned in the control cycle, T ₁ , T ₂ represents the transformation matrix of the object trajectory and the robot end trajectory respectively; 2) There is a response lag error at the fixed end: When the end of robot A is used as a fixed end, there is a response lag error Δs ₁ caused by eccentric load, and there is a coordinate system calibration error Δd and a response lag error Δs ₂ at the end of robot B. When the eccentric load causes the actual response position of the end robot When it lags behind the position issued by the theoretical control layer, the response lag error Δs ₁ of the fixed end of robot A is calculated as follows:

In the formula, x ₁ (n) is the delivery position of the theoretical control layer of the previous control cycle, and

is the actual position of the robot in the last control cycle; When there is a lag response deviation Δs ₁ at the fixed end, the position adjustment amount ΔD output by the fuzzy controller during the internal force control process not only includes all the position errors of robot B, but also includes the response lag deviation Δs ₁ of robot A. Finally, the overall track offset Δs ₁ and Δs ₁ are used as the compensation amount Δe_comp of the trajectory tracking error of the operation object. At this time, the position control commands are: x ₁ (n+1)=x _obj (n+1) · T-Δs ₁ x ₂ (n+1)=x _obj (n+1)·T ₂ +ΔD-Δs ₁ ΔD=Δd+Δs ₁ +Δs ₂ . 5. A dual-robot coordinated control system is characterized in that, comprising dual-robot motion guidance module, internal force control and trajectory compensation module, multi-sensor UDP and TCP/IP mixed communication module, wherein: dual-robot motion guidance module includes The force coordinate transformation module, the stiffness control module and the guiding force threshold control module, wherein the force coordinate transformation module obtains the coordinate system transformation relationship of the sensor relative to the world coordinate system, and the stiffness control module obtains the stiffness control relationship and converts the force information into objects Then, each sub-robot system is driven by the kinematic closed-loop chain decoupling. The guiding force threshold control module controls the output guiding force vector by setting the guiding force threshold to reduce the force signal fluctuation in the initial state of the sensor. cause the object position to fluctuate; The internal force control and trajectory compensation module includes a multi-robot kinematics calculation module, a trajectory tracking error compensation module, an internal force single-end adjustment module, and a single-robot servo control module. The closed-loop kinematics model solves the motion trajectories of each robot end, which is compensated by the trajectory accuracy compensation module and then output to the single robot servo control module. The trajectory tracking error compensation module calculates the trajectory tracking error online to generate the trajectory real-time compensation amount and superimpose it on each robot. On the end trajectory of , the internal force single-end adjustment module calculates the object internal force of the robot at the adjustment end in real time according to the signal of each robot end sensor, and outputs the position compensation amount of the end of the robot at the adjustment end through the fuzzy controller based on the impedance model.

Images