CN111687827A - 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

Control method and control system for coordinating and operating weak rigid member by two robots

Technical Field

The invention belongs to the technical field of robot control, and particularly relates to a control method and a control system for coordinately operating a weak rigid component by two robots.

Background

The application of the single-arm robot in the related fields of spraying, welding, grinding, assembling and the like is mature, but in some application occasions aiming at large parts and large-size workpieces, the single-arm robot cannot be used for certain work due to the limitation of the load capacity and the working range of the single-arm robot. With the continuous expansion of the application range of the robot, the application scenes of the multi-robot coordination control system are more and more. The coordinated movement of the double robots is the basis and the premise of system coordinated control, and is also the technical core for realizing the carrying action in the assembly process of the weak rigid member, the aircraft wall plate and a plurality of aircraft parts are weak rigid or flexible parts, the coordinated movement error between the double robots can bring serious pulling, extruding and shearing stress to the operation objects such as the weak rigid member, and the deformation and damage of the objects can be caused in serious cases. The existing multi-robot coordination control technology is not convenient for realizing random movement of a weak rigid component in 6 degrees of freedom in space, and in the aspects of internal force control and load distribution, most of the existing researches aim at a small robot and combine a dynamic model thereof to carry out torque control, but the industrial robot is difficult to accurately model and a commercial industrial robot does not open a torque control loop. For this reason, interpolation control of the robot guide trajectory and adjustment and trajectory compensation of its internal force control are required.

Disclosure of Invention

The technical problem to be solved by the invention is to provide a control method and a control system for coordinately operating a weak rigid member by two robots, so that a guide track simultaneously meets the safety limit requirements of a Cartesian space and a joint space of the robot, the problem of dangerous guide track caused by too large guide force or sudden change is solved, the smoothness and the safety of the guide track are improved, the response speed of the system is improved, and the tracking error of an object track is reduced.

The technical solution for realizing the purpose of the invention is as follows:

a control method for coordinating and operating a weak rigid member by two robots comprises the following steps:

step 1: taking a weak rigid member as a direct operation object, building a kinematics closed-loop chain model of the double robots, and designing double robot guide control based on the model, wherein the double robot guide control comprises force coordinate transformation, rigidity control and guide force threshold control;

step 2: according to the kinematic constraint relation between the tail end of the robot and an operation object, multi-space adaptive interpolation control of a guide track in a Cartesian space and a joint space is designed, and the method specifically comprises the following steps:

step 2-1: interpolating the rotation guide track of the operation object according to overlarge angular speed and/or overlarge angular acceleration of the operation object;

step 2-2: recalculating the speed and the acceleration of the tail end of the robot and discrete points of the object guide track, and interpolating the movement guide track according to the overlarge moving speed and/or overlarge moving acceleration of the operation object;

step 2-3: according to the motion constraint conditions of all joints, performing joint space self-adaptive interpolation on the guide track;

and step 3: designing dual-input fuzzy control and object trajectory tracking error compensation control based on an impedance model according to an internal force calculation model of an operation object, and specifically comprising the following steps:

step 3-1: adopting a reference adjusting strategy of internal force control, fixing the tail end of the robot at one side as a control reference, and adjusting the tail end of the robot at the other side according to the internal force;

step 3-2: establishing an internal force fuzzy control framework based on an impedance model: inputting the deviation value of the internal force and the pose adjustment value of the previous period to a fuzzy controller, outputting the pose adjustment value of the current period and carrying out internal force control on the double-robot system;

step 3-3: the trajectory is compensated for whether there is a response lag error at the fixed end. Further, the method for controlling the double-robot coordinated operation of the weak rigid member of the invention, wherein the establishment of the kinematics closed-loop chain model of the double-robot in the step 1 specifically comprises the following steps:

step 1-1: the kinematic model of the object coordinate system { c } with respect to the world coordinate system { W } is:

^WT_c＝^WT_R1·^R1T_e1·^e1T_c1·^c1T_c

^WT_c＝^WT_R2·^R2T_e2·^e2T_c2·^c2T_c

^WT_R1and^WT_R2the transformation relation between the installation position of each robot base and a world coordinate system is a fixed constant matrix; { R1}, and { R2} respectively represent the respective base coordinate systems of Robot1 and Robot 2; { e1}, and { e2} respectively represent the terminal tool coordinate systems of Robot1 and Robot2, and coincide with the Robot terminal grabbing point; { c1}, and { c2} respectively represent coordinate systems after { e1}, and { e2} are extended in translation, and the origin of the coordinate system coincides with the origin of { c };^c1T_c、^c2T_crespectively representing the position and posture homogeneous change matrixes of the object coordinate system { c } relative to { c1} and { c2 };^e1T_c1、^e2T_c2respectively represent homogeneous transformation matrixes of { c1}, { c2} relative to robot end coordinate systems { e1}, and { e2 };^R1T_e1、^R2T_e2respectively representing a homogeneous transformation matrix of a robot terminal coordinate system and each self-based coordinate system;^WT_R1、^WT_R2respectively represent homogeneous transformation matrixes of a robot base coordinate system { R1} and { R2} relative to a world coordinate system { W };

1-2: the world coordinate system { W } is coincided with the base coordinate system { R1} of one robot, and the conversion relation between the base coordinate systems is determined by adopting a four-point calibration method^R1T_R2Determining a terminal virtual link matrix between the two robot terminals by the kinematic closed-loop chain:

^e1T_e2＝[^R1T_e1]^-1·^R1T_R2·^R2T_e2

1-3: in a virtual link matrix^e1T_e2On the basis, a position adjustment proportion K is introduced to determine the origin positions of two robot end translation coordinate systems { c1} and { c2}, and an object coordinate system { c } is established at the origin positions of the robot end translation coordinate systems { c1} and { c2}, so that the robot end translation coordinate system { c2} is obtainedThe homogeneous transformation matrix between the robot end tool coordinate systems { e1}, { e2} and their translated coordinate systems { c1}, { c2} is:

in the formula, E₃Denotes an identity matrix of order 3, O_1×3Represents a zero vector of 1 row and 3 columns, wherein P {. cndot.) represents a position vector for extracting a homogeneous matrix,^e1T_c1and^e2T_c2in merely according to^e1T_e2The position vector of (a) is moved,^c1T_c2and^c2T_c1then only in accordance with^e1T_e2Of the rotation matrix^e1R_e2Making a change;

1-4: establishing a coordinate system transformation relation between an object coordinate system { c } and terminal translation coordinate systems { c1}, { c2}, wherein A is a constant homogeneous rotation matrix:

^cT_c2＝^cT_c1·^c1T_c2

1-5: determining a kinematic driving control equation of each robot through a closed-loop kinematic model of the double robots:

^R1T_e1＝^R1T_c·[^e1T_c]^-1＝[^R1T_e1·^e1T_c1·^c1T_c]·[^e1T_c1·^c1T_c]^-1

^R2T_e2＝[^R1T_R2]^-1·^R1T_e1·^e1T_c1·^c1T_c·[^e2T_c2·^c2T_c1·^c1T_c]^-1

^R1T_e1、^R2T_e2the position and attitude homogeneous matrix of the two robots in respective coordinate systems.

Further, the control method for coordinately operating the weak rigid member by the two robots of the invention specifically comprises the step 3-2 of an internal force difference value delta F_iAnd a variable fuzzification, fuzzy rule design and deblurring part 3 of the position adjustment quantity delta x specifically comprises the following steps:

step 3-2-1: fuzzifying input and output quantity:

setting a basic domain, determining a quantization factor and a scale factor, performing variable fuzzification on data which does not exceed the domain boundary by adopting a triangular membership function, and setting the data which exceeds the domain boundary as a domain boundary value;

step 3-2-2: designing a fuzzy rule:

1) if the internal force F_iIf the direction of the position adjustment quantity X is the same as that of the previous period, the robot is still in the internal force adjustment trend of the previous period, the direction of the output control quantity is kept unchanged, and the size is selected according to the weight of the internal force and the position adjustment quantity of the previous period;

2) if the internal force F_iIf the position adjustment quantity X direction of the robot in the previous period is different, the internal force adjustment direction of the robot in the current period is different from the adjustment direction of the previous period, and the output control quantity is reduced;

at the same time, the upper period position is adjusted by X and the internal force F_iIf the weight of the upper period position adjustment quantity X is great, selecting a smaller control output quantity according to the upper period direction, and keeping the inertial effect in the adjustment direction; if the internal force F_iIf the weight of the internal force is large, selecting a smaller control output quantity according to the internal force adjusting direction;

3) defuzzification: and fuzzy judgment is carried out on the output control quantity by adopting a gravity center method, and the controller outputs a specific position adjustment quantity.

Further, the control method for coordinating and operating the weak rigid member by the double robots of the invention specifically comprises the following steps 3-3:

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

when the end of the robot1 is a fixed end and there is no error due to the dynamic response lag, the error at the end of the robot2 includes a coordinate system calibration error Δ d and a dynamic response lag error Δ s due to an offset load at the end of the robot2₂The robot2 serves as a dynamic adjusting end, under the internal force control system with single-end adjustment, the position adjustment quantity delta D of the robot2 compensates the deviation of the tail end to a certain extent, and the control execution instructions of the two robots are respectively as follows:

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) represents the double robot end trajectory, x, to be delivered immediately_obj(n +1) represents the planned theoretical motion trajectory of the object in the control period, T₁、T₂A transformation matrix respectively representing an object trajectory and a robot end trajectory;

2) the fixed end has response lag error:

when the tail end of the robot1 is used as a fixed end, a response lag error deltas caused by unbalance loading exists₁At the end of the robot2, there are a coordinate system calibration error Δ d and a response lag error Δ s₂When the actual response position of the tail end robot lags behind the issuing position of the theoretical control layer due to unbalance loading, the response lag error deltas of the fixed end of the robot1 is calculated₁The following were used:

in the formula, x₁(n) is the issuing position of the theoretical control layer of the last control cycle, and

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

when the fixed end has a hysteresis response deviation deltas₁The position adjustment amount Δ D output from the fuzzy controller during the internal control includes not only all the position errors of the robot2 but also the response delay deviation Δ s of the robot1₁And the tail end of each robot finally sends out the overall deviation Delta s of the track₁，Δs₁The compensation amount Δ e _ comp of the trajectory tracking error as the operation target, at this time, the position control commands are respectively:

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 coordinating and operating weak rigid members by two robots comprises a double-robot motion guide module, an internal force control and track compensation module, and a UDP (user datagram protocol) and TCP/IP (transmission control protocol/Internet protocol) hybrid communication module with multiple sensors, wherein: the double-robot motion guiding module comprises a force coordinate transformation module, a rigidity control module and a guiding force threshold control module, wherein the force coordinate transformation module acquires a coordinate system conversion relation of the sensor relative to a world coordinate system, the rigidity control module acquires a rigidity control relation, converts force information into a motion track of an object and then drives each sub-robot system through kinematics closed-loop chain decoupling, and the guiding force threshold control module controls an output guiding force vector by setting a guiding force threshold and reduces the fluctuation of the position of the object caused by the fluctuation of a force signal in an initial state of the sensor;

the internal force control and track compensation module comprises a multi-robot kinematics calculation module, a track tracking error compensation module, an internal force single-ended adjustment module and a single-robot servo control module, wherein the multi-robot kinematics calculation module calculates the motion track of each robot end through a closed-loop kinematics model according to the motion track of a given object, the motion track is compensated through a track precision compensation module and then output to the single-robot servo control module, the track tracking error compensation module performs online calculation on track tracking errors to generate track real-time compensation quantity which is superposed on the tail end track of each robot, the internal force single-ended adjustment module calculates the object internal force of the adjustment end robot in real time according to signals of each robot end sensor, and the position compensation quantity of the adjustment end robot end is output through a fuzzy controller based on an impedance model.

Compared with the prior art, the invention adopting the technical scheme has the following technical effects:

1. the guiding track of the invention simultaneously meets the safety restriction requirements of the Cartesian space and the joint space of the robot, solves the problem of dangerous guiding track caused by too large guiding force or sudden change in a guiding control algorithm, and improves the smoothness and the safety of the guiding track.

2. The method disclosed by the invention has the advantages that the response speed of the system is greatly improved while the stability is ensured, the tracking error of the object track can be reduced while the internal force control effect is ensured, and the method has an important significance for realizing real-time internal force control of the industrial robot based on the position.

Drawings

Fig. 1 is a schematic diagram of a dual robot motion closed loop system.

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

Fig. 3 is a diagram of trajectory error analysis.

Fig. 4 is a schematic view of the force control in the operation object.

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

FIG. 6 is a schematic representation of membership functions.

FIG. 7 is a schematic diagram of track error compensation for fixed-end unresponsive lag errors.

FIG. 8 is a schematic diagram of track error compensation in the presence of response lag error at the fixed end.

Detailed Description

Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the accompanying drawings are illustrative only for the purpose of explaining the present invention, and are not to be construed as limiting the present invention.

Example 1

A control method for coordinating and operating a weak rigid member by two robots comprises the following steps:

step 1: and taking the weak rigid member as a direct operation object, building a kinematics closed-loop chain model of the double robots, and designing double robot guide control based on the model, wherein the double robot guide control comprises force coordinate transformation, rigidity control and guide force threshold control.

The core objective of the multi-robot coordination control system is to realize the expected movement of the clamped object, and directly using the operation object as the control object is simpler and more intuitive, and the complex movement track of the clamped object is easier to realize.

The end of the robot tool and the object grabbing point are regarded as fixed connection and do not have relative movement, at this time, the double robots and the operation object form a kinematic closed-loop chain, the closed-loop kinematic model of the double robots is shown in fig. 1, and the geometrical relationship of fig. 1 shows that the kinematic model of the object coordinate system { c } relative to the world coordinate system { W } is:

^WT_c＝^WT_R1·^R1T_e1·^e1T_c1·^c1T_c

^WT_c＝^WT_R2·^R2T_e2·^e2T_c2·^c2T_c

wherein: { W } represents the system world coordinate system; { R1}, and { R2} respectively represent the respective base coordinate systems of Robot1 and Robot 2; { e1}, and { e2} respectively represent the tip tool coordinate systems of Robot1, Robot2, coinciding with the Robot tip grasping point; { c } represents a coordinate system of the operation object, the origin position of which is movably adjusted in order to realize a complicated guide trajectory; { c1}, and { c2} respectively represent coordinate systems after { e1}, and { e2} are extended in translation, and the origin of the coordinate system coincides with the origin of { c };^c1T_c、^c2T_crespectively representing the position and posture homogeneous change matrixes of the object coordinate system { c } relative to { c1} and { c2 };^e1T_c1、^e2T_c2denote { c1} and { c2} relative to the robot tip, respectivelyA homogeneous transformation matrix of coordinate systems { e1}, { e2 };^R1T_e1、^R2T_e2respectively representing a homogeneous transformation matrix of a robot terminal coordinate system and respective base coordinate systems;^WT_R1、^WT_R2respectively represent homogeneous transformation matrixes of a robot base coordinate system { R1} and a { R2} relative to a world coordinate system { W };^WT_R1and^WT_R2the transformation relation between the installation position of each robot base and the world coordinate system is a fixed constant matrix.

In order to facilitate control and ensure the calibration precision, the world coordinate system { W } is coincided with the base coordinate system { R1} of one robot, and only the transformation matrix between the base coordinate systems of the two robots is needed^R1T_R2Namely, the conversion relation between the base coordinate systems is determined by adopting a four-point calibration method^R1T_R2。

After the base coordinate conversion relation is obtained, determining a tail end virtual connecting rod matrix between the tail ends of the double robots by a kinematic closed-loop chain:

^e1T_e2＝[^R1T_e1]^-1·^R1T_R2·^R2T_e2

in order to facilitate the operator to guide the object to move according to the operation intention, the virtual link matrix^e1T_e2The position adjustment proportion K is introduced on the basis of the position adjustment proportion K to determine the origin positions of the two robot end translation coordinate systems { c1} and { c2 }. And establishing an object coordinate system c at the origin positions of the robot end translation coordinate systems c1 and c 2. Therefore, the position of the object coordinate system can be changed by only adjusting the ratio K (0-1).

Since the translational 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 system { e1}, { e2} and its translated coordinate systems { c1}, { c2} is:

in the formula, E₃Denotes an identity matrix of order 3, O_1×3Represents a zero vector of 1 row and 3 columns, wherein P {. cndot.) represents a position vector for extracting a homogeneous matrix,^e1T_c1and^e2T_c2in merely according to^e1T_e2The position vector of (a) is moved. While^c1T_c2And^c2T_c1then only in accordance with^e1T_e2Of the rotation matrix^e1R_e2A change is made.

Meanwhile, establishing a coordinate system transformation relation between an object coordinate system { c } and terminal translation coordinate systems { c1}, { c2}, wherein A is a constant homogeneous rotation matrix:

^cT_c2＝^cT_c1·^c1T_c2

finally, the kinematics driving control equation of each robot can be determined through the closed-loop kinematics model of the double robots:

^R1T_e1＝^R1T_c·[^e1T_c]^-1＝[^R1T_e1·^e1T_c1·^c1T_c]·[^e1T_c1·^c1T_c]^-1

^R2T_e2＝[^R1T_R2]^-1·^R1T_e1·^e1T_c1·^c1T_c·[^e2T_c2·^c2T_c1·^c1T_c]^-1

by changing the homogeneous change matrix of the operation object in the world coordinate system R1 continuously^R1T_cThe pose homogeneous matrix of the double robots under respective coordinate systems can be calculated in a decoupling mode^R1T_e1、^R2T_e2Substituted into respective single machineThe inverse kinematics of the robot can solve the driving angle of each joint.

Step 2: and designing multi-space adaptive interpolation control of the guide track in a Cartesian space and a joint space according to the kinematic constraint relation between the tail end of the robot and an operation object.

The position constraint in the kinematic constraint relationship is:

calculating an end pose matrix of the robot according to the position of the operation object:

^R1T_c＝^R1T_e1·^e1T_c1·^c1T_c＝^R1T_R2·^R2T_e2·^e2T_c2·^c2T_c

^R1T_cis a homogeneous change matrix of the operation object in a world coordinate system R1,^R1T_e1、^R2T_e2is a pose homogeneous matrix of the two robots under respective coordinate systems,^c1T_c、^c2T_c、^e1T_c1、^e2T_c2is a constant matrix determined by the manner in which the object is grabbed,^R1T_R2the matrix is scaled for the base coordinate system.

The velocity constraint in the kinematic constraint relationship is:

when the two robot terminals and the operation object adopt a rigid grabbing mode, the speed relationship between the operation object and the two robot terminals is as follows:

v_c、w_clinear and angular velocity, v, representing the motion of an object_ei、w_eiRepresenting linear and angular velocities, r, of the ith robot end_ei＝[r_eixr_eiyr_eiz]^TRepresenting a position vector of the ith robot terminal in the object coordinate system;

then, the velocity constraint equation of the robot end is:

wherein the content of the first and second substances,

E₃denotes an identity matrix of order 3, O₃Representing a zero matrix of order 3.

The acceleration constraints in the kinematic constraint relationship are:

the acceleration constraint relation between the operation object and the tail end of the robot is as follows:

then, the robot tip acceleration constraint equation is:

according to the overlarge angular velocity and/or the overlarge angular acceleration of the operation object, the rotation guide track is interpolated:

1) when the angular speed of the operation object is overlarge to cause the angular speed of the tail end of the robot to exceed the limit, calculating the ratio R of the angular speed of the tail end of each robot to exceed the limit_{velo_ei}Selecting the maximum overrun ratio R_{velo_END}And carrying out deceleration interpolation to 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}for the adjustment parameters of the guidance track interpolation, the rotation angle of the discrete guidance track of the interpolation operation object by using the robot terminal angular velocity constraint condition is as follows:

Δθ_{vcal_END}(n)＝Δθ_initial(n)/R_{velo_END}

2) when the angular acceleration of the operation object is overlarge to cause the angular acceleration of the tail end of the robot to exceed the limit, calculating the tail end angle of each robotAcceleration overrun ratio R_{accel_ei}Selecting the maximum overrun ratio R_{accel_END}Interpolation is performed to obtain:

R_{accel_END}＝max(R_{accel_e1},R_{accel_e2}),R_{accel_END}＞1

R_{accel_END}for the adjustment parameters of the guidance track interpolation, the rotation angle of the object guidance track in the process of interpolation operation by using the robot terminal angular acceleration constraint condition is as follows:

3) when the robot is out of limit due to the fact that the angular velocity of the operation object is too large and the angular acceleration of the operation object is too large, the minimum value of the two is selected as the rotation angle of the interpolation operation object guide track;

i.e., Δ θ_{cal_END}The output value of the discrete trace point as the period is expressed as:

recalculating the velocity v of the robot end after interpolation of the rotation guide track of the operation object_eiAcceleration w_eiAnd discrete points deltax of object guide trajectory_initial(n), interpolating the movement guide track according to the overlarge movement speed and/or the overlarge movement acceleration of the operation object:

1) when the speed of the tail end of the robot exceeds the limit due to the overlarge moving speed of the object, calculating the speed exceeding proportion R of the tail end of each robot_{velo_ei}Selecting the maximum overrun ratio R_{velo_END}And carrying out deceleration interpolation to 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}for adjusting parameters of the guidance track interpolation, discrete track points (including positions and postures) of the guidance track of the interpolation object by using the robot terminal speed constraint condition are as follows:

Δx_{vcal_END}(n)＝Δx_initial(n)/R_{velo_END}

2) when the acceleration of the tail end of the robot exceeds the limit due to the overlarge moving acceleration of the object, calculating the acceleration overrun proportion R of the tail end of each robot_{accel_ei}Selecting the maximum overrun ratio R_{accel_END}Interpolation is performed to obtain:

R_{accel_END}＝max(R_{accel_e1},R_{accel_e2}),R_{accel_END}＞1

R_{accel_END}for adjusting parameters of the guidance track interpolation, discrete track points (including positions and postures) of the guidance track of the object interpolated by the robot terminal acceleration constraint condition are as follows:

3) when the robot exceeds the limit due to the fact that the moving speed and the moving acceleration of the operation object are too high at the same time, selecting the minimum value of the moving speed and the moving acceleration as a discrete track point of the interpolation operation object guide track;

i.e., Δ x_{cal_END}The output quantity of the discrete trace points for this period is expressed as:

according to the motion constraint conditions of each joint, performing joint space self-adaptive interpolation on the guide track:

setting the maximum joint limiting speed of each robot in the joint space to

Maximum limited acceleration of

The terminal speed and the joint speed of the robot are controlled by a Jacobian matrix J (q)_i) Indicating, joint velocities of both robots

Expressed as:

according to the fact that the robot joint speed is too high and/or the joint acceleration is too high, joint guide tracks of the robot are interpolated: 1) when the robot joint speed is too high to cause the exceeding, calculating the exceeding proportion of the joint angular speed of each joint of the robot, and selecting the maximum exceeding proportion R_{velo_Ji}And (4) performing interpolation speed reduction to obtain:

R_{velo_JOINT}＝max(R_{velo_J1},R_{velo_J2})

R_{velo_JOINT}for adjusting parameters of guide track interpolation, discrete guide track points Deltax after Cartesian space interpolation are subjected to joint angular velocity overrun proportion in unit time_{vcal_JOINT}Interpolation is performed to obtain:

Δx_{vcal_JOINT}(n)＝Δx_{cal_END}(n)/R_{velo_JOINT}

2) when the acceleration of the joints of the robot is overlarge to cause the ultralimit, calculating the ultralimit proportion of the angular acceleration of the joints of the robot, and selecting the maximum ultralimit proportion R_{accel_Ji}Interpolation is performed to obtain:

R_{accel_JOINT}＝max(R_{accel_J1},R_{accel_J2})

R_{accel_JOINT}for adjusting parameters of guide track interpolation, discrete guide track points Delta x after Cartesian space interpolation are subjected to robot joint angular acceleration overrun proportion_{cal_END}Interpolation is performed to obtain:

3) when the joint of the robot has the condition that the joint speed is too high and the joint acceleration is too high to cause the overrun, selecting the smaller value of the two as interpolation;

that is, Δ x is expressed as the output quantity of discrete trace points in the present period as:

and step 3: and designing dual-input fuzzy control and object trajectory tracking error compensation control based on an impedance model according to an internal force calculation model of the operation object.

Considering a dual robot system to clamp an object together, the clamping points default to a rigid attachment without relative sliding of the positions, as shown in fig. 2. During the motion execution process, each robot end exerts a force vector f on the operation object_iAnd moment vector m_iThe object is subjected to a resultant force F applied by the robot end_objWherein the relationship is:

in the formula (I), the compound is shown in the specification,

representing a Jacobian change matrix between an object coordinate system and an ith robot end coordinate system, wherein the matrix expression comprises:

in the formula (r)_ei)^×For each end-to-object coordinate system position vector r_ei＝[r_eixr_eiyr_eiz]^TIs used to form the oblique symmetric matrix. E₃Representing a 3-dimensional identity matrix, O₃An empty matrix is represented.

From the view point of robot end force decomposition, a part of force applied by the robot end is converted into motion driving force f_MTo drive the object to move (generally called as the external force applied to the object by the robot), and the other part is converted into the internal force f of the object_IAnd the force in the object does not affect the motion state of the object. Except for special conditions, the internal force generally has negative influence on system control, so the internal force is controlled to be the minimum value as much as possible, and a series of deformation damage to an operation object in the assembling process is avoided. From the above, the robot end force decomposition expression is:

f＝f_M+f_I

because of internal forces f_IDoes not produce a net resultant force on the object, so it is located

The calculation formula of the driving force and the internal force of the object motion can be obtained by the following steps:

step 3-1: and adopting a reference adjusting strategy of internal force control, fixing the tail end of the robot at one side as a control reference, and adjusting the tail end of the robot at the other side according to the internal force in a follow-up manner.

Since the internal forces exerted on the object by the two robot tips are a set of interaction forces, controlling one tip enables internal force control. The 'reference adjustment' strategy is that the tail end of the robot on one side is fixed to be used as a control reference, and the tail end of the robot on the other side is adjusted according to the internal force. The single-end regulation can effectively shorten the stabilization time, and can obtain better internal force control effect in the fixed control period of the control system.

In order to ensure the control precision of the internal force control system, the model error of the internal force system needs to be analyzed before the internal force fixing end and the internal force adjusting end are determined. The control error of the closed-loop kinematic model of the double robot mainly occurs on the calibration result between the base calibration systems. Because the world coordinate system is superposed with the base coordinate system of one robot, the influence degree of the tail ends of the two robots by the calibration result of the base coordinate system is different.

Subject to calibration error

Influence of, calibration results of the base system

And true value^R1T_R2The relationship between them is:

the intermediate term of the control process affected by the error term is

Wherein

And

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

and

to be received

The influence of the position error of (a),

then receive

The attitude error influence.

In order to facilitate the control and reduce the calibration error, the world coordinate system of the two-robot cooperation system coincides with the base coordinate system of the robot 1. The base coordinate system calculation loop chain of robot1 is significantly shorter than that of robot 2. And error term of

Can be used for calculating the inverse matrix thereof

In contrast, the robot1 is basically not affected, so the calibration error is mainly expressed in the terminal pose expression of the robot 2.

And specifically analyzing the control precision of the internal force single-ended adjustment based on the error analysis result. In fig. 3, the triangles numbered by the numerals 1 and 2 represent the robot ends, the concentric circles represent the carried operation objects, the broken lines represent the initial trajectories affected by the calibration error, and the solid lines represent the actual movement trajectories of the respective robots. If the fixed end is selected to be on the side with high control precision (side 1 in fig. 3 a), the initial trajectory deviation of the adjusting end is directly compensated in the internal force control process, so that the position control precision of the operation object is ensured, as shown in fig. 3 a. If the fixed end is selected to be on the side with larger position error (side 2 in fig. 3 b), the robot adjusting end on the other side will be adjusted with the fixed end in the process of internal force control, and the trajectories of the operation object and the adjusting end are both deviated under the influence of the deviation of the trajectories of the fixed end, as shown in fig. 3 b. Therefore, when the double-robot system adopts a 'reference adjustment' strategy to perform internal force control, the robot1 is used as a fixed end, and the robot2 is used as a motion adjustment end.

Step 3-2: establishing an internal force fuzzy control framework based on an impedance model: and inputting the internal force deviation amount and the last period of pose adjustment amount to the fuzzy controller, outputting the pose adjustment amount of the current period and carrying out internal force control on the double-robot system.

The internal force control model of the dual-robot system is equivalent to creating a force-to-position transformation rule at the internal force adjusting end, as shown in fig. 4. And the impedance model is equivalent to a 6-dimensional virtual spring damper in space, and converts the internal force into the position adjustment quantity delta x of the internal force adjusting end of the robot. Delta x simultaneously comprises displacement adjustment quantity and rotation adjustment quantity, and is the same as the method for solving the pose increment of the object guide action by the force sensor, the position compensation converted from the internal force vector is directly superposed on the actual position quantity for output, the attitude compensation converted from the internal force torque vector needs to be converted into Euler angle compensation quantity in a corresponding form, and then the Euler angle compensation quantity is superposed on the actual Euler angle for output, so that the 6-dimensional control of the internal force in the space is realized.

In the internal force control process, the system detects the internal force deviation delta F between the object and the tail ends of the double robots in real time_i＝F_i-F_idIn which F is_iIs the actual internal force of the system, F_idFor internal force desired value, let F normally_idThe best internal force control effect can be obtained when the pressure is 0. Once the existence of the internal force is detected, the double-robot system enters an internal force control mode, and the deviation delta F of the internal force_iThe impedance relationship with the position adjustment amount Δ x is:

wherein B, K are the damping coefficient and stiffness coefficient matrices in impedance control, Δ x and

respectively showing the adjustment amount of the tail end position of the robot and the change speed of the adjustment amount.

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

difference of internal forces Δ F_iThe discrete mathematical relationship with the position adjustment Δ x is:

to obtain an expression of the final robot tip position adjustment amount Δ x, then:

if the conventional fuzzy controller is applied to the internal force control system of the double robot, the internal force deviation amount delta F is usually adopted_iAnd speed of change of deviation amount thereof

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

to combine the two control methods of impedance control and fuzzy control, let Δ F_i(n) and delta x (n-1) are used as input control quantity of the fuzzy controller, and on the premise of ensuring high response characteristic of the fuzzy controller, the second-order system characteristic of impedance control is kept, and the fuzzy systemThe system is equivalent to self-adaptive reasoning control over an algebraic term containing a stiffness coefficient K and a damping coefficient B of a delta t/(K delta t + B) term and a B/(K delta t + B) term, so that the problem of value taking of K, B in impedance control is avoided. The output function of the position adjustment quantity of the fuzzy framework is as follows:

Δx(n)＝Fuzzy(ΔF_i,Δx(n-1))

a dual-input fuzzy control architecture based on an impedance model is thus obtained, as shown in fig. 5. And inputting the internal force deviation value and the last period of pose adjustment value into the fuzzy controller, outputting the period of pose adjustment value and realizing the internal force control of the double-robot system.

On the basis of a dual-input internal force fuzzy control framework based on an impedance model, a dual-input fuzzy control algorithm needs to be designed in detail by combining with the practical application scene of dual-robot internal force control. The algorithm mainly comprises an internal force difference value delta F_iAnd 3 parts of variable fuzzification, fuzzy rule design, deblurring and the like of the position adjustment quantity delta x.

(1) Input and output fuzzification

In order to ensure the safety of the force control in the double-robot system, a safety internal force limit value is firstly determined. If the internal force is beyond the limit, the control is carried out according to the maximum limit internal force, and the phenomenon of internal force mutation caused by overlarge position adjustment amount is avoided. Meanwhile, in the area with large internal force, the domain division is sparse so that the internal force can respond quickly; the areas with small internal force are densely divided, so that the internal force control precision is improved as much as possible. Therefore, the domain of internal force is selected to be F_i-12, -6, -3,0,3,6,12}, the corresponding fuzzy subset a in the theoretical domain_iThe language expression of (i ═ 1,2,3,4,5,6) is: { NB, NM, NS, Z, PS, PM, PB }, and simultaneously, adopting a trigonometric membership function with small calculated amount in actual control to perform variable fuzzification and internal force F_iMembership function u of_A(F_i) As shown in fig. 6 a.

Because the close coordination control between the robot end and the object is realized through a rigid fixed connection mode, a small position deviation amount can generate a large internal force. The selection of the universe of location deviation cannot be too large, and the principle of 'sparse outside and dense inside' during division of the universe of internal force is adopted in the same way. SelectingTaking a domain X { -0.1, -0.05, -0.02,0,0.02,0.05,0.1} of the input control quantity, and corresponding fuzzy subset B on the domain_iThe language expression of (i ═ 1,2,3,4,5,6) is: { NB, NM, NS, Z, PS, PM, PB }. Similarly, the output control quantity of the system is also the position adjustment quantity, and the corresponding fuzzy subset C on the theory domain is X1 { -0.1, -0.05, -0.02,0,0.02,0.05,0.1}, and the theory domain corresponds to_iThe language expression of (i ═ 1,2,3,4,5,6) is: { NB, NM, NS, Z, PS, PM, PB }. Performing variable fuzzification by adopting a triangular membership function with small calculated amount in actual control, and inputting a membership function u of the position adjustment amount X_B(X) and membership function u of output position adjustment X1_C(X1) is the same as shown in FIG. 6 b.

(2) Fuzzy rule design

The improved double-input fuzzy controller takes the internal force and the position adjustment quantity of the upper period as input, the position adjustment quantity of the upper period reflects the regulation trend of the upper period at the tail end of the robot, and the position adjustment quantity of the upper period is used as feed-forward judgment to decide the selection of the position adjustment quantity of the period. Usually, the internal force has a positive and negative difference between the tension and the pressure, and the position adjustment also has a one-to-one corresponding positive and negative control quantity to adjust the internal force, so the formulation principle of the fuzzy control inference rule of the system is as follows:

1) if the internal force F_iThe direction of the position adjustment amount X is the same as that of the previous period, at this time, the robot is still in the internal force adjustment trend of the previous period, the direction of the output control amount is kept unchanged, and the magnitude is selected according to the weight of the internal force and the position adjustment amount of the previous period.

2) If the internal force F_iIn this case, the robot has a difference in the direction of the internal force adjustment direction from the direction of the upper cycle adjustment amount X, and the output control amount should be reduced as appropriate. At the same time, the upper period position is adjusted by X and the internal force F_iIf the weight of the upper period position adjustment quantity X is great, selecting a smaller control output quantity according to the upper period direction, and keeping the inertial effect in the adjustment direction; if the internal force F_iIf the weight of the internal force is large, a smaller control output quantity is selected according to the internal force adjusting direction, and smooth transition of the internal force adjusting direction is ensured.

The control principle introduces the second-order system characteristic of spring damping in impedance control on the basis of keeping the original characteristic of fuzzy control. And (3) predicting the behavior of the period by taking the control quantity of the previous period as an inertia quantity, and finally summarizing and formulating a fuzzy control rule table as follows:

TABLE 1 fuzzy control rules Table

(3) Defuzzification

After fuzzy inference is completed based on the fuzzy rule base, a fuzzy set of the output position adjustment quantity X1 expressed in a fuzzy language is obtained, so that the final step of the fuzzy controller is to perform fuzzy judgment on the output control quantity to obtain a specific position adjustment quantity X1. The gravity center method is a deblurring method which is adopted in many cases, and research results of some scholars also show that the output of the gravity center method is continuous rather than jumping and can give more stable output, and the calculation formula is as follows:

in the formula, mu_A(F_i)、μ_B(X) degree of membership, Deltax, of functions of internal force and upper periodic position adjustment, respectively_ijAnd finally carrying out fuzzy judgment to obtain the output delta x of the fuzzy controller, namely the position adjustment quantity of the period, and storing the output delta x for the calculation of the next period, wherein the output delta x is the output theoretic domain value corresponding to different membership degrees in the fuzzy inference rule base.

Step 3-3: the trajectory is compensated for whether there is a response lag error at the fixed end.

(1) Absence of response lag error at the fixed end

It is known from error analysis that the mechanical error of a robot body caused by the use condition of a mechanical structure and equipment is usually very small, and the size of the mechanical error of the robot is difficult to calculate in real time in the online real-time control of the industrial robot, so the error is ignored in the text. The calibration of the multi-robot base system is the basis of the coordination control work, but the inevitable calibration error is mainly reflected on the end of the robot2 in the mechanical system.

When the end of the robot1 is a fixed end and there is no error due to dynamic response lag, the possible errors at the end of the robot2 mainly include a coordinate system calibration error Δ d and a dynamic response lag error Δ s due to an offset load at the end of the robot2₂As shown in fig. 7. The robot2 is used as a dynamic adjusting end, under the internal force control system with single-end adjustment, the position adjustment quantity delta D of the robot2 compensates the deviation of the tail end to a certain extent, and the control execution instructions of the two robots are respectively as follows:

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) represents the double robot end trajectory, x, to be delivered immediately_obj(n +1) represents the planned theoretical motion trajectory of the object in the control period, T₁、T₂And the transformation matrixes respectively represent the object track and the robot tail end track.

(2) In the case of a fixed end with a response lag error

When the fixed end of the robot1 has a response lag error deltas caused by unbalance loading₁In this case, the coordinate system calibration error Δ d and the response lag error Δ s still exist at the end of the robot2₂. During the internal force control, the trajectory of the object and the trajectory of the adjustment end will also be shifted, as shown in fig. 8.

When the actual response position of the end robot lags behind the issuing position of the theoretical control layer due to unbalance loading, the lag error at the moment can be calculated in real time in the controller, so the response lag error deltas of the fixed end of the robot1 is firstly calculated₁The method comprises the following steps:

in the formula, x₁(n) is the issuing position of the theoretical control layer of the last control cycle, and

and performing positive kinematics solution on the actual position of the robot in the last control period through real-time angle feedback of a joint motor to obtain the actual position of the robot.

When the fixed end has a hysteresis response deviation deltas₁The position adjustment amount Δ D output from the fuzzy controller during the internal control includes not only all the position errors of the robot2 but also the response delay deviation Δ s of the robot1₁. The moving track of the object after single-end adjustment is shifted integrally. In order to compensate the track tracking precision of the object, the tail end of each robot finally issues a track overall deviation delta s₁So as to compensate the object track error caused by various errors of the fixed end and the adjusting end to a certain extent, therefore, Delta s₁The amount of compensation Δ e _ comp for the trajectory tracking error to be the object of operation. The position control commands at this time are respectively:

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 double-robot coordination control system comprises a double-robot motion guide module, an internal force control and track compensation module and a multi-sensor UDP and TCP/IP mixed communication module.

The double-robot motion guiding module comprises a force coordinate transformation module, a rigidity control module and a guiding force threshold control module, wherein the force coordinate transformation module acquires a coordinate system transformation relation of the sensor relative to a world coordinate system, the rigidity control module acquires the rigidity control relation, converts force information into a motion track of an object and then drives each sub-robot system through kinematics closed-loop chain decoupling, and the guiding force threshold control module controls an output guiding force vector by setting a guiding force threshold and reduces the position fluctuation of the object caused by the fluctuation of a force signal in an initial state of the sensor.

The internal force control and track compensation module comprises a multi-robot kinematics resolving module, a track tracking error compensation module, an internal force single-end adjusting module and a single-robot servo control module. The object internal force control system is divided into 4 parts, namely a multi-robot kinematics resolving module, a track precision compensation module, an internal force single-end regulation module, a single-robot servo control module and the like, firstly, the motion track of a given object is resolved through a closed-loop kinematics model to calculate the motion track of the tail end of each robot, and then the single-robot servo control module is driven to operate; force decoupling is carried out by a sensor at the tail end of each robot in real time to calculate the internal force of the object at the robot2 at the adjusting end, and the internal force is transmitted into an improved fuzzy controller based on an impedance model to output the position compensation quantity of the tail end of the robot2, so that the internal force control of the object in the double-robot system is realized; and then, the track tracking error is calculated on line by a designed object track tracking error compensation algorithm, and the track deviation value is used as a track real-time compensation value to be superposed on the tail end track of each robot, so that the self-adaptive online compensation of the object track tracking error is realized, and the track tracking precision of the double-robot system under the conditions of response lag and the like is improved.

The foregoing is directed to embodiments of the present invention and, more particularly, to a method and apparatus for controlling a power converter in a power converter, including a power converter, a power.

Claims (5)

1. A control method for coordinating and operating a weak rigid member by two robots is characterized by comprising the following steps:

step 1: taking a weak rigid member as a direct operation object, building a kinematics closed-loop chain model of the double robots, and designing double robot guide control based on the model, wherein the double robot guide control comprises force coordinate transformation, rigidity control and guide force threshold control;

step 2: according to the kinematic constraint relation between the tail end of the robot and an operation object, multi-space adaptive interpolation control of a guide track in a Cartesian space and a joint space is designed, and the method specifically comprises the following steps:

step 2-1: interpolating the rotation guide track of the operation object according to overlarge angular speed and/or overlarge angular acceleration of the operation object;

step 2-2: recalculating the speed and the acceleration of the tail end of the robot and discrete points of the object guide track, and interpolating the movement guide track according to the overlarge moving speed and/or overlarge moving acceleration of the operation object;

step 2-3: according to the motion constraint conditions of all joints, performing joint space self-adaptive interpolation on the guide track;

and step 3: designing dual-input fuzzy control and object trajectory tracking error compensation control based on an impedance model according to an internal force calculation model of an operation object, and specifically comprising the following steps:

step 3-1: adopting a reference adjusting strategy of internal force control, fixing the tail end of the robot at one side as a control reference, and adjusting the tail end of the robot at the other side according to the internal force;

step 3-2: establishing an internal force fuzzy control framework based on an impedance model: inputting the deviation value of the internal force and the pose adjustment value of the previous period to a fuzzy controller, outputting the pose adjustment value of the current period and carrying out internal force control on the double-robot system;

step 3-3: the trajectory is compensated for whether there is a response lag error at the fixed end.

2. The method for controlling the weak-rigidity member through the coordination operation of the two robots according to claim 1, wherein the step 1 of establishing the kinematics closed-loop chain model of the two robots specifically comprises the following steps:

step 1-1: the kinematic model of the object coordinate system { c } with respect to the world coordinate system { W } is:

^WT_c＝^WT_R1·^R1T_e1·^e1T_c1·^c1T_c

^WT_c＝^WT_R2·^R2T_e2·^e2T_c2·^c2T_c

^WT_R1and^WT_R2the transformation relation between the installation position of each robot base and a world coordinate system is a fixed constant matrix; { R1}, and { R2} respectively represent the respective base coordinate systems of Robot1 and Robot 2; { e1}, and { e2} respectively represent the terminal tool coordinate systems of Robot1 and Robot2, and coincide with the Robot terminal grabbing point; { c1}, and { c2} respectively represent coordinate systems after { e1}, and { e2} are extended in translation, and the origin of the coordinate system coincides with the origin of { c };^c1T_c、^c2T_crespectively representing the position and posture homogeneous change matrixes of the object coordinate system { c } relative to { c1} and { c2 };^e1T_c1、^e2T_c2respectively represent homogeneous transformation matrixes of { c1}, { c2} relative to robot end coordinate systems { e1}, and { e2 };^R1T_e1、^R2T_e2respectively representing a homogeneous transformation matrix of a robot terminal coordinate system and respective base coordinate systems;^WT_R1、^WT_R2respectively represent homogeneous transformation matrixes of a robot base coordinate system { R1} and a { R2} relative to a world coordinate system { W };

1-2: the world coordinate system { W } is coincided with the base coordinate system { R1} of one robot, and the conversion relation between the base coordinate systems is determined by adopting a four-point calibration method^R1T_R2Determining a terminal virtual link matrix between the two robot terminals by the kinematic closed-loop chain:

^e1T_e2＝[^R1T_e1]^-1·^R1T_R2·^R2T_e2

1-3: in a virtual link matrix^e1T_e2On the basis of the coordinate system, a position adjustment proportion K is introduced to determine the origin positions of the translation coordinate systems { c1} and { c2} of the tail ends of the double robots, and the object coordinate system { c } is established at the tail ends of the robotsThe origin positions of the translational coordinate systems { c1}, { c2} are the homogeneous transformation matrix between the robot end tool coordinate system { e1}, { e2} and the translated coordinate systems { c1}, and { c2} as follows:

in the formula, E₃Denotes an identity matrix of order 3, O_1×3Represents a zero vector of 1 row and 3 columns, wherein P {. cndot.) represents a position vector for extracting a homogeneous matrix,^e1T_c1and^e2T_c2in merely according to^e1T_e2The position vector of (a) is moved,^c1T_c2and^c2T_c1then only in accordance with^e1T_e2Of the rotation matrix^e1R_e2Making a change;

1-4: establishing a coordinate system transformation relation between an object coordinate system { c } and terminal translation coordinate systems { c1}, { c2}, wherein A is a constant homogeneous rotation matrix:

^c1T_c＝A；

^cT_c2＝^cT_c1·^c1T_c2

1-5: determining a kinematic driving control equation of each robot through a closed-loop kinematic model of the double robots:

^R1T_e1＝^R1T_c·[^e1T_c]^-1＝[^R1T_e1·^e1T_c1·^c1T_c]·[^e1T_c1·^c1T_c]^-1

^R2T_e2＝[^R1T_R2]^-1·^R1T_e1·^e1T_c1·^c1T_c·[^e2T_c2·^c2T_c1·^c1T_c]^-1

^R1T_e1、^R2T_e2the position and attitude homogeneous matrix of the two robots in respective coordinate systems.

3. The method as claimed in claim 1, wherein step 3-2 includes the step of controlling the weak rigid member by using the two robots in coordination, wherein the step includes an internal force difference Δ F_iAnd a variable fuzzification, fuzzy rule design and deblurring part 3 of the position adjustment quantity delta x specifically comprises the following steps:

step 3-2-1: fuzzifying input and output quantity:

setting a basic domain, determining a quantization factor and a scale factor, performing variable fuzzification on data which does not exceed the domain boundary by adopting a triangular membership function, and setting the data which exceeds the domain boundary as a domain boundary value;

step 3-2-2: designing a fuzzy rule:

1) if the internal force F_iIf the direction of the position adjustment quantity X is the same as that of the previous period, the robot is still in the internal force adjustment trend of the previous period, the direction of the output control quantity is kept unchanged, and the size is selected according to the weight of the internal force and the position adjustment quantity of the previous period;

2) if the internal force F_iIf the position adjustment quantity X direction of the robot in the previous period is different, the internal force adjustment direction of the robot in the current period is different from the adjustment direction of the previous period, and the output control quantity is reduced;

at the same time, the upper period position is adjusted by X and the internal force F_iIf the weight of the upper period position adjustment quantity X is great, selecting a smaller control output quantity according to the upper period direction, and keeping the inertial effect in the adjustment direction; if the internal force F_iIf the weight of the internal force is large, selecting a smaller control output quantity according to the internal force adjusting direction;

3) defuzzification: and fuzzy judgment is carried out on the output control quantity by adopting a gravity center method, and the controller outputs a specific position adjustment quantity.

4. The method for controlling the weak-rigidity member through the coordination operation of the two robots as claimed in claim 1, wherein the step 3-3 specifically comprises:

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

when the end of the robot1 is a fixed end and there is no error due to the dynamic response lag, the error at the end of the robot2 includes a coordinate system calibration error Δ d and a dynamic response lag error Δ s due to an offset load at the end of the robot2₂The robot2 serves as a dynamic adjusting end, under the internal force control system with single-end adjustment, the position adjustment quantity delta D of the robot2 compensates the deviation of the tail end to a certain extent, and the control execution instructions of the two robots are respectively as follows:

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) represents the double robot end trajectory, x, to be delivered immediately_obj(n +1) represents the planned theoretical motion trajectory of the object in the control period, T₁、T₂A transformation matrix respectively representing an object trajectory and a robot end trajectory;

2) the fixed end has response lag error:

when the tail end of the robot1 is used as a fixed end, a response lag error deltas caused by unbalance loading exists₁In the meantime, the robot2 has a coordinate system calibration error Δ d and a response lag error Δ s at its end₂When the actual response position of the tail end robot lags behind the issuing position of the theoretical control layer due to unbalance loading, the response lag error deltas of the fixed end of the robot1 is calculated₁The following were used:

in the formula, x₁(n) is the issuing position of the theoretical control layer of the last control cycle, and

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

when the fixed end has a hysteresis response deviation deltas₁The position adjustment amount Δ D output from the fuzzy controller during the internal force control includes not only all the position errors of the robot2 but also the response delay deviation Δ s of the robot1₁And the tail end of each robot finally sends out the overall deviation Delta s of the track₁，Δs₁The compensation amount Δ e _ comp of the trajectory tracking error as the operation target, at this time, the position control commands are respectively:

x₁(n+1)＝x_obj(n+1)·T-Δs₁

x₂(n+1)＝x_obj(n+1)·T₂+ΔD-Δs₁

ΔD＝Δd+Δs₁+Δs₂。

5. the utility model provides a two robot coordinated control system which characterized in that, includes two robot motion guide module, internal force control and track compensation module, multi-sensor's UDP and TCP/IP mixed communication module, wherein: the double-robot motion guiding module comprises a force coordinate transformation module, a rigidity control module and a guiding force threshold control module, wherein the force coordinate transformation module acquires a coordinate system conversion relation of the sensor relative to a world coordinate system, the rigidity control module acquires a rigidity control relation, converts force information into a motion track of an object and then drives each sub-robot system through kinematics closed-loop chain decoupling, and the guiding force threshold control module controls an output guiding force vector by setting a guiding force threshold and reduces the position fluctuation of the object caused by the fluctuation of a force signal in an initial state of the sensor;

the internal force control and track compensation module comprises a multi-robot kinematics calculation module, a track tracking error compensation module, an internal force single-ended adjustment module and a single-robot servo control module, wherein the multi-robot kinematics calculation module calculates the motion track of each robot end through a closed-loop kinematics model according to the motion track of a given object, the motion track is compensated through a track precision compensation module and then output to the single-robot servo control module, the track tracking error compensation module performs online calculation on track tracking errors to generate track real-time compensation quantity which is superposed on the tail end track of each robot, the internal force single-ended adjustment module calculates the object internal force of the adjustment end robot in real time according to signals of each robot end sensor, and the position compensation quantity of the adjustment end robot end is output through a fuzzy controller based on an impedance model.

Images