CN113664830B - Model prediction impedance control-based double-robot synchronous processing method and system - Google Patents

Model prediction impedance control-based double-robot synchronous processing method and system Download PDF

Info

Publication number
CN113664830B
CN113664830B CN202110972833.4A CN202110972833A CN113664830B CN 113664830 B CN113664830 B CN 113664830B CN 202110972833 A CN202110972833 A CN 202110972833A CN 113664830 B CN113664830 B CN 113664830B
Authority
CN
China
Prior art keywords
robot
matrix
state
moment
force
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110972833.4A
Other languages
Chinese (zh)
Other versions
CN113664830A (en
Inventor
陶波
张宇豪
赵兴炜
丁汉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN202110972833.4A priority Critical patent/CN113664830B/en
Publication of CN113664830A publication Critical patent/CN113664830A/en
Application granted granted Critical
Publication of CN113664830B publication Critical patent/CN113664830B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1682—Dual arm manipulator; Coordination of several manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Abstract

The invention discloses a model prediction impedance control-based double-robot synchronous processing method and a system, belonging to the field of robot control and comprising the following steps: establishing a state space control model of the double robots based on a kinetic equation; calculating an interactive force moment value by using a self-adaptive variable damping control method according to the state variable and the output variable of each robot at the current moment so as to update an interactive force moment item in a state space control model; the method comprises the steps that the output variables corresponding to all robots are the same and are taken as optimization targets, state constraints and input constraints met by the output variables are combined, and a nonlinear model predictive control algorithm is utilized to establish an optimization problem with constraints so as to solve motion moments in a state space control model; and respectively superposing each interactive force moment and the motion moment to obtain corresponding resultant moment, and driving the corresponding robot according to the moment value corresponding to each robot in the resultant moment. The cooperative processing operation of the double robots is realized, and the stability of simultaneous processing of the double robots is ensured.

Description

Model prediction impedance control-based double-robot synchronous processing method and system
Technical Field
The invention belongs to the field of robot control, and particularly relates to a model prediction impedance control-based two-robot synchronous processing method and system.
Background
With the continuous development of the robot technology, the traditional manufacturing mode is gradually developing to a more intelligent and efficient direction, and the machining mode of replacing a machine tool with a robot is widely adopted in industry. The traditional machine tool is low in machining efficiency, multi-robot multi-station cooperative machining is adopted, and machining efficiency can be effectively improved. When multiple robots work simultaneously, the robots have a certain geometric relationship and interaction force relationship, and the maintenance of the relationship between the relative positions and the relative forces of the robots is the key to ensure the quality of the cooperation. By implementing the multi-robot synchronous control method, the motion state and the interaction force state among a plurality of robots can be effectively kept constant, and the method has important significance for improving the manufacturing precision and quality.
Conventional robot control methods generally include a PID motion control method and a resistance force control method. The PID motion control method ensures that the motion state converges to a reference value through inverse kinematics solution and PID motion feedback; the impedance force control method converts force errors into motion errors of the robot through a position and force rigidity model of the robot so as to realize force tracking control. For the processing task of the double robots, the robots need to meet the requirements of constant contact force in the normal direction, ensure constant movement speed in the tangential direction, keep the symmetry of the movement positions of the double robots, balance the forces, and meet the requirements of multiple targets of movement and force at the same time. Therefore, the conventional single-target controller cannot meet the requirement of multi-robot processing, and the stability of the dual-robot processing system is difficult to ensure.
Disclosure of Invention
Aiming at the defects and improvement requirements of the prior art, the invention provides a method and a system for synchronously processing two robots based on model prediction impedance control, and aims to realize the cooperative processing operation of the two robots, eliminate the relative error between the two robots and ensure the stability of the simultaneous processing of the two robots.
In order to achieve the above object, according to an aspect of the present invention, there is provided a two-robot synchronous processing method based on model predictive impedance control, including: s1, establishing a state space control model of the double robots based on a kinetic equation by taking the interaction force moment and the movement moment of the double robots as input variables, joint angles and joint angular velocities as state variables, and the terminal poses, terminal velocities and external forces borne by the terminals of the robots in a three-dimensional space as output variables; s2, calculating an interactive force moment value by using a self-adaptive variable damping control method according to the state variable and the output variable of each robot at the current moment, and updating the interactive force moment in the state space control model into the interactive force moment value; s3, establishing an optimization problem with constraints by using a nonlinear model predictive control algorithm by taking the same output variable corresponding to each robot as an optimization target and combining the state constraint and the input constraint which are met by the output variable so as to solve the motion moment in the state space control model; and S4, respectively superposing the interaction force moments and the movement moments to obtain corresponding resultant moments, and driving the corresponding robots according to the moment values corresponding to the robots in the resultant moments.
Further, the state space control model is:
Figure BDA0003226357550000021
τ＝τ u +τ v
wherein x is 1 Is a joint angle state matrix variable, x, of a dual robot system 2 Is a joint angular velocity state matrix variable of the double-robot system,
Figure BDA0003226357550000022
Figure BDA0003226357550000023
and
Figure BDA0003226357550000024
are respectively x 1 And x 2 A derivative of (a); m (x) 1 ) Is an inertia matrix of a two-robot system, M -1 (x 1 ) Is M (x) 1 ) The inverse matrix of (d); tau is joint resultant force matrix of the double-robot system, tau u And τ v Respectively an interaction force moment and a movement moment; j (x) 1 ) Jacobian matrix, J, for a two-robot system T (x 1 ) Is J (x) 1 ) The transposed matrix of (2); f (x) 1 ，x 2 ) The external force matrix is an external force matrix of the double-robot system; c (x) 1 ，x 2 ) A Coriolis force matrix of a dual-robot system; g (x) 1 ) A gravity matrix of a double-robot system; f is a joint friction force matrix of the double-robot system.
Further, the joint friction matrix is:
f＝(f 1 ，f 2 )
Figure BDA0003226357550000031
wherein f is i Is the joint friction of the ith robot, i is 1, 2; f. of a，j 、f b，j 、f c，j The first friction coefficient, the second friction coefficient and the third friction coefficient are respectively used for correcting the corresponding relation between the friction model and the actually measured friction; q. q.s i Is the joint angular velocity of the ith robot.
Further, the value of the interaction force moment is:
Figure BDA0003226357550000032
Figure BDA0003226357550000033
Γ(t)＝Γ(t-T)+α(F 1 (t-T)-F 2 (t-T))/D h
wherein, tau u，i The value of the interaction force moment of the ith robot is 1, 2; j. the design is a square i Is the jacobian matrix of the ith robot,
Figure BDA0003226357550000034
is J i The transposed matrix of (2); k h Is the stiffness coefficient, P, of the robot i The terminal pose of the ith robot is shown; p i r The terminal reference pose of the ith robot is obtained; d h The damping coefficient of the robot is shown, and delta D is the variation of the damping coefficient; v i The tip speed of the ith robot; v i r The reference speed is the tail end reference speed of the ith robot; Γ (t) is an integral function, and t is the current time;
Figure BDA0003226357550000036
is e x Derivative of the estimated value, e x The deviation between the reference pose at the tail end and the pose at the tail end of the double-robot system,
Figure BDA0003226357550000037
delta is a preset coefficient; t is a sampling period; alpha is a force feedback coefficient; f 1 (T-T) is the external force applied to the first robot at the time of T-T, F 2 And (T-T) is the external force applied to the second robot at the T-T moment.
Still further, the optimization objectives are:
Figure BDA0003226357550000038
wherein J (x (t), τ) v (t)) is a cost function, x (t) and τ v (t) state variables and moments of motion in the state space control model, respectively; t is p For predicting step length, t is the current moment; Ψ (x) e (s)，τ v (s)) as a rolling cost term, x e (s) is the error state, s is the integration time variable; phi (x) e (t+T p )，τ v (t+T p ) Is a terminal cost term.
Further, the scrolling cost item and the terminal cost item are respectively:
Figure BDA0003226357550000041
Φ(x e (t+T p )，τ v (t+T p ))＝x e (t+T p ) T Hx e (t+T p )
wherein x is e (s)＝(e P (s)，e V (s)，e F (s)，e i，p (s)，e i，V (s)，e i，F (s))，e P (s)、e V (s)、e F (s) pose deviation, speed deviation and force deviation of the two-robot system, e i，p (s) deviation of the terminal pose of the ith robot from the terminal reference pose, e i，V (s) deviation of the tip speed of the ith robot from the tip reference speed, e i，F (s) the external force applied to the tail end of the ith robot and the tail end referenceThe deviation of the external force is such that,
Figure BDA0003226357550000042
is x e (s) a transposed matrix; q is a state coefficient matrix, R is an input coefficient matrix, and H is a terminal state matrix.
Further, the state constraint and the input constraint are:
Figure BDA0003226357550000043
Figure BDA0003226357550000044
Figure BDA0003226357550000045
wherein x is a state variable, x 1 Is a joint angle state matrix variable, x, of a dual robot system 2 Is a joint angular velocity state matrix variable of the double-robot system,
Figure BDA0003226357550000046
is a state constraint set of the two robots,
Figure BDA0003226357550000047
reference Jacobian matrix for the ith robot, J i Is Jacobian matrix of the ith robot, and Λ is speed precision matrix, χ E Is a terminal state constraint set of a two-robot system, W is an output target transformation matrix,
Figure BDA0003226357550000049
is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,
Figure BDA00032263575500000410
in order to limit the input torque,
Figure BDA00032263575500000411
is a Coriolis force matrix C (x) 1 ，x 2 ) The upper bound of (a) is,
Figure BDA00032263575500000412
which is the upper bound of the gravity matrix G,
Figure BDA00032263575500000413
is an inertia matrix M (x) 1 ) The upper bound of (a) is,
Figure BDA00032263575500000414
maximum terminal acceleration, K ξ In order to be a stability coefficient matrix,
Figure BDA00032263575500000415
is the input constraint set.
According to an aspect of the present invention, there is provided a dual robot synchronous processing system based on model predictive impedance control, including: the model establishing module is used for establishing a state space control model of the double robots based on a kinetic equation by taking the interaction force moment and the motion moment of the double robots as input variables, taking joint angles and joint angular velocities as state variables and taking the terminal poses, terminal velocities and external forces borne by the terminals of the robots in a three-dimensional space as output variables; the interactive force moment calculation and update module is used for calculating an interactive force moment value by using a self-adaptive variable damping control method according to the value of the state variable and the value of the output variable of each robot at the current moment and updating the interactive force moment in the state space control model into the interactive force moment value; the motion moment solving module is used for establishing an optimization problem with constraints by using a nonlinear model predictive control algorithm by taking the same output variable corresponding to each robot as an optimization target and combining the state constraint and the input constraint which are met by the output variable so as to solve the motion moment in the state space control model; and the driving module is used for respectively superposing the interaction force moments and the movement moments to obtain corresponding resultant moments, and driving the corresponding robots according to the moment values corresponding to the robots in the resultant moments.
Generally, by the above technical solution conceived by the present invention, the following beneficial effects can be obtained: the method comprises the following steps of dividing synchronous processing feedback control of the double robots into a self-adaptive impedance part and a nonlinear model prediction part, wherein the self-adaptive impedance part solves interactive force moment based on a variable damping principle through external force state information fed back by the robots so as to synchronize external forces of the two robots, so that the robots have compliance, the change of processing force is more flexible, and the relative force error of the double robots is compensated; the nonlinear model prediction part is used for solving the optimal motion moment of the robots through an optimization equation on the basis of the impedance model so as to synchronize the motion of the two robots and compensate the relative motion error of the two robots; therefore, the relative motion error and the relative force error of the double robots are compensated simultaneously, the motion synchronization and the interaction force balance between the double robots are ensured, the robot cooperative interaction task is suitable for complex robot cooperative interaction tasks, the robot cooperative interaction task processing method is particularly suitable for processing large complex components by multiple robots, the processing efficiency is improved, the processing quality is ensured, the workpiece deformation is avoided, and the stability and the quality of a processing system are improved.
Drawings
Fig. 1 is a flowchart of a two-robot synchronous processing method based on model predictive impedance control according to an embodiment of the present invention;
fig. 2 is a control process diagram of a two-robot synchronous processing method based on model predictive impedance control according to an embodiment of the present invention;
fig. 3A and fig. 3B are application scene diagrams of the dual-robot synchronous cooperative processing of the thin-walled workpiece according to the embodiment of the present invention;
fig. 4 is a convergence response diagram of relative position errors of two robots in practical application of the method according to the embodiment of the present invention;
fig. 5 is a convergence response diagram of relative speed errors of two robots in practical application of the method according to the embodiment of the present invention;
fig. 6 is a convergence response diagram of relative force errors of two robots in practical application of the method according to the embodiment of the present invention;
fig. 7 is a block diagram of a dual-robot synchronous processing system based on model predictive impedance control according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
In the present application, the terms "first," "second," and the like (if any) in the description and the drawings are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order.
The embodiment provides a dual-robot synchronous processing method based on model predictive impedance control, as shown in fig. 2. The feedback control module in this embodiment may be divided into two sub-modules, an adaptive impedance part and a nonlinear model prediction part. The self-adaptive impedance force synchronous controller solves the interaction force moment based on the variable damping principle through the state information fed back by the robots so as to synchronize the external forces of the two robots; the nonlinear model prediction motion controller solves the motion moment of the robot through an optimization equation on the basis of an impedance model, and superimposes the motion moment and the interaction force moment to be input into a joint driver of the robot, so that real-time synchronous operation of the double robots is realized.
Referring to the opposite-side cooperative processing scenario of the double robots shown in fig. 3A, the double robots simultaneously process two sides of the slender thin-walled workpiece, and when the motion state of the robots is inconsistent with the applied force, the thin-walled workpiece may deform, and the robots move disorderly, resulting in poor processing quality. When the synchronous machining method of the two robots in the embodiment is adopted, the relative motion and the relative force of the two robots can be kept consistent, so that the deformation of workpieces is avoided, and the stability and the quality of a machining system are improved, as shown in fig. 3B.
Fig. 1 is a flowchart of a two-robot synchronous processing method based on model predictive impedance control according to an embodiment of the present invention. Referring to FIG. 1, the method includes operation S1-operation S4.
And operation S1, establishing a state space control model of the double robots based on a kinetic equation, with the interaction force moment and the movement moment of the double robots as input variables, the joint angles and the joint angular velocities as state variables, and the terminal poses, the terminal velocities and the external forces borne by the terminals of the robots in a three-dimensional space as output variables.
In this embodiment, the two robots include a first robot and a second robot, and the joint kinetic equation of the two robots includes a mass matrix M, a coriolis force matrix C, a gravity matrix G, and a joint friction force f. A Newton Euler model is adopted, and a kinetic equation of the robot is established through the kinetic parameters of the robot:
Figure BDA0003226357550000071
the joint friction force f belongs to an uncertain item and is described by an S function, and the method specifically comprises the following steps:
f＝(f 1 ，f 2 )
Figure BDA0003226357550000072
wherein f is i Is the joint friction of the ith robot, i is 1, 2; f. of a，j 、f b，j 、f c，j The first friction coefficient, the second friction coefficient and the third friction coefficient are respectively used for correcting the corresponding relation between the friction model and the actually measured friction; q. q.s i Is the joint angular velocity of the ith robot.
The established state space control model is as follows:
Figure BDA0003226357550000081
τ＝τ u +τ ν
wherein x is 1 Is a joint angle state matrix variable, x, of a dual robot system 1 ＝[q 1 q 2 ] T ，q 1 、q 2 Respectively setting the joint angles of a first robot and a second robot in the double-robot system; x is the number of 2 Is a joint angular velocity state matrix variable of the two-robot system,
Figure BDA0003226357550000082
Figure BDA0003226357550000083
and
Figure BDA0003226357550000084
are respectively x 1 And x 2 A derivative of (d); m (x) 1 )＝diag(M 1 ，M 2 ) For the inertia matrix of a two-robot system, diag () represents the diagonal matrix, M 1 And M 2 Inertia matrices, M, for the first and second robots, respectively -1 (x 1 ) Is M (x) 1 ) The inverse matrix of (d); tau is joint resultant force matrix of the double-robot system, and tau is tau u +τ v ，τ u And τ v Respectively interaction force moment and movement moment, tau u ＝[τ u，1 τ u，2 ] T ，τ u，1 And τ u，2 The interaction force moments, tau, of the first and second robot, respectively v ＝[τ v，1 τ v，2 ] T ，τ v，1 And τ v，2 The motion moments of the first robot and the second robot respectively; j (x) 1 ) Jacobian matrix, J, for a two-robot system T (x 1 ) Is J (x) 1 ) Transposed matrix of, J (x) 1 )＝diag(J 1 ，J 2 )，J 1 And J 2 Jacobian matrices for the first robot and the second robot, respectively; f (x) 1 ，x 2 ) The external force matrix is an external force matrix of the double-robot system; c (x) 1 ，x 2 ) Coriolis force matrix, C (x), for a two-robot system 1 ，x 2 )＝diag(C 1 ，C 2 )，C 1 And C 2 A Coriolis force matrix for the first robot and the second robot, respectively; g (x) 1 ) Gravity matrix, G (x), for a two-robot system 1 )＝diag(G 1 ，G 2 )，G 1 And G 2 The gravity matrixes of the first robot and the second robot are respectively; f is the joint friction force matrix of the double-robot system, and f is (f) 1 ，f 2 )，f 1 And f 2 The joint friction of the first robot and the second robot respectively.
And operation S2, calculating an interaction force torque value by using an adaptive variable damping control method according to the values of the state variables and the output variables of the robots at the current time, and updating the interaction force torque in the state space control model to the interaction force torque value.
In this embodiment, the interactive force torque value calculated by using the adaptive variable damping control method is as follows:
Figure BDA0003226357550000091
Figure BDA0003226357550000092
Γ(t)＝Γ(t-T)+α(F 1 (t-T)-F 2 (t-T))/D h
wherein, tau u，i The value of the interaction force moment of the ith robot is 1, 2; j. the design is a square i Is the jacobian matrix of the ith robot,
Figure BDA0003226357550000093
is J i The transposed matrix of (2); k h Is the stiffness coefficient, P, of the robot i The terminal pose of the ith robot is shown; p i r The terminal reference pose of the ith robot is obtained; d h The damping coefficient of the robot is shown, and delta D is the variation of the damping coefficient; v i The tip speed of the ith robot; v i r The reference speed is the tail end reference speed of the ith robot; Γ (t) is the productDividing a function, wherein t is the current moment;
Figure BDA0003226357550000096
is e x Derivative of the estimated value, e x Is the deviation between the reference pose and the terminal pose of the double-robot system,
Figure BDA0003226357550000097
delta is a predetermined coefficient, a small amount, for avoiding the denominator being zero; t is a sampling period; alpha is a force feedback coefficient, alpha is an updating factor of gamma (t) and belongs to (0, 1); f 1 (T-T) is the external force applied to the first robot at the time of T-T, F 2 And (T-T) is the external force applied to the second robot at the T-T moment. Γ (0) ═ 0, setting the reference stiffness of the robot to K, for example h ＝1000，D h ＝100，α＝0.001，δ＝0.000001。
And substituting the interactive force moment value calculated by the self-adaptive variable damping control method into the state space control model so as to further solve the value of the motion moment by utilizing a nonlinear model predictive control algorithm.
And operation S3, taking the same output variables corresponding to each robot as an optimization target, combining the state constraint and the input constraint met by the output variables, and establishing an optimization problem with the constraint by using a nonlinear model predictive control algorithm to solve the motion moment in the state space control model.
In this embodiment, the optimization objective is:
Figure BDA0003226357550000098
wherein J (x (t), τ) v (t)) is a cost function, x (T) and τ v (t) state variables and moments of motion in the state space control model, respectively; t is p For predicting step length, t is the current moment; Ψ (x) e (s)，τ v (s)) is a rolling cost term, x e (s) is the error state, s is the integration time variable; phi (x) e (t+T p )，τ v (t+T p ) Is a terminal cost term.
Rolling cost term Ψ (x) in optimization objective e (s)，τ v (s)) and a terminal cost term Φ (x) e (t+T p )，τ v (t+T p ) Respectively are:
Figure BDA0003226357550000101
Φ(x e (t+T p )，τ v (t+T p ))＝x e (t+T p ) T Hx e (t+T p )
wherein x is e (s)＝(e P (s)，e V (s)，e F (s)，e i，p (s)，e i，V (s)，e i，F (s))，e P (s)、e V (s)、e F (s) pose deviation, speed deviation and force deviation of the two-robot system, e i，p (s) deviation of the terminal pose of the ith robot from the terminal reference pose, e i，V (s) deviation of the tip speed of the ith robot from the tip reference speed, e i，F (s) is the deviation of the external force applied to the tail end of the ith robot and the tail end reference external force,
Figure BDA0003226357550000102
is x e (s) a transposed matrix; q is a state coefficient matrix, R is an input coefficient matrix, and H is a terminal state matrix.
In this embodiment, the state constraint and the input constraint are:
Figure BDA0003226357550000103
Figure BDA0003226357550000104
Figure BDA0003226357550000105
wherein x is a state variable, x 1 Is a joint angle state matrix variable, x, of a dual robot system 2 Is a joint angular velocity state matrix variable of the double-robot system,
Figure BDA0003226357550000106
is a state constraint set of the two robots,
Figure BDA0003226357550000107
reference Jacobian matrix for the ith robot, J i Is Jacobian matrix of the ith robot, and Λ is speed precision matrix, χ E Is a terminal state constraint set of a two-robot system, W is an output target transformation matrix,
Figure BDA0003226357550000109
is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,
Figure BDA00032263575500001010
in order to limit the input torque,
Figure BDA00032263575500001011
is a Coriolis force matrix C (x) 1 ，x 2 ) The upper bound of (a) is,
Figure BDA00032263575500001012
which is the upper bound of the gravity matrix G,
Figure BDA00032263575500001013
is an inertia matrix M (x) 1 ) The upper bound of (a) is,
Figure BDA00032263575500001014
maximum terminal acceleration, K ξ In order to be a matrix of stability coefficients,
Figure BDA00032263575500001015
is the input constraint set.
And establishing an optimization problem with constraints by using a nonlinear model predictive control algorithm according to the optimization target and the state constraints and the input constraints which are satisfied by the output variables, and solving the optimization problem to obtain the motion torque in the state space control model.
And operation S4, respectively superimposing each interaction force moment and the motion moment to obtain a corresponding resultant moment, and driving the corresponding robot according to the moment value corresponding to each robot in the resultant moment.
Superimposed movement moment tau u With interaction of force moment tau ω，i And the resultant moment of the ith robot is input into a joint driver of the ith robot, so that the first robot and the second robot are controlled to keep a synchronous relation during the cooperative processing. In the dual robot processing scenario shown in fig. 3A and 3B, using the control parameters in operation S2 and operation S3, when the initial position error between the two robots is 0.06m, the relative position error, the relative speed error, and the relative force error of the dual robots are shown in fig. 4, 5, and 6, respectively, as the control strategy in the joint driver becomes effective. Referring to fig. 4-6, it can be seen that the relative position error, the relative speed error and the relative force error of the two robots are gradually eliminated and finally converged to 0, so as to implement the synchronous processing of the two robots.
Fig. 7 is a block diagram of a dual-robot synchronous processing system based on model predictive impedance control according to an embodiment of the present invention. Referring to fig. 7, the model-predictive impedance control-based dual-robot synchronous processing system 700 includes a model establishing module 710, an interactive force moment calculating and updating module 720, a motion moment solving module 730, and a driving module 740.
The model building module 710 performs operation S1, for example, to use the interaction force moment and the motion moment of the two robots as input variables, the joint angle and the joint angular velocity as state variables, and the end pose, the end velocity, and the external force applied to the end of each robot in the three-dimensional space as output variables, and build a state space control model of the two robots based on a kinetic equation.
The interaction force torque calculation and update module 720, for example, performs operation S2, and is configured to calculate an interaction force torque value by using an adaptive variable damping control method according to the values of the state variables and the output variables of the robots at the current time, and update the interaction force torque in the state space control model to the interaction force torque value.
The motion torque solving module 730, for example, executes operation S3, and is configured to establish an optimization problem with constraints by using a nonlinear model predictive control algorithm to solve the motion torque in the state space control model, in combination with the state constraint and the input constraint that the output variables satisfy, with the output variables corresponding to the robots being the same as each other as the optimization target.
The driving module 740 performs operation S4, for example, to superimpose each interaction force moment and the motion moment to obtain a corresponding resultant moment, and drive the corresponding robot according to a moment value corresponding to each robot in the resultant moment.
The dual-robot synchronous processing system 700 based on model predictive impedance control is used for executing the dual-robot synchronous processing method based on model predictive impedance control in the embodiments shown in fig. 1 to 6. For details that are not described in this embodiment, please refer to the two-robot synchronous processing method based on model predictive impedance control in the embodiments shown in fig. 1 to fig. 6, which is not described herein again.
It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (2)

1. A dual-robot synchronous processing method based on model prediction impedance control is characterized by comprising the following steps:
s1, establishing a state space control model of the double robots based on a kinetic equation by taking the interaction force moment and the motion moment of the double robots as input variables, joint angles and joint angular velocities as state variables and the terminal poses, terminal velocities and external forces borne by the terminals of the robots in a three-dimensional space as output variables, wherein the state space control model is as follows:
Figure FDA0003683716370000011
τ＝τ u +τ v
wherein x is 1 Is a joint angle state matrix variable, x, of a dual robot system 2 Is a joint angular velocity state matrix variable of the double-robot system,
Figure FDA0003683716370000012
Figure FDA0003683716370000013
and
Figure FDA0003683716370000014
are respectively x 1 And x 2 A derivative of (a); m (x) 1 ) Is an inertia matrix of a two-robot system, M -1 (x 1 ) Is M (x) 1 ) The inverse matrix of (d); tau is joint resultant force matrix of the double-robot system, tau u And τ v Respectively an interaction force moment and a movement moment; j (x) 1 ) Jacobian matrix, J, for a two-robot system T (x 1 ) Is J (x) 1 ) The transposed matrix of (2); f (x) 1 ,x 2 ) The external force matrix is an external force matrix of the double-robot system; c (x) 1 ,x 2 ) A Coriolis force matrix of a dual-robot system; g (x) 1 ) A gravity matrix of a double-robot system; f is a joint friction force matrix of the double-robot system;
the joint friction force matrix is:
f＝(f 1 ,f 2 )
Figure FDA0003683716370000015
wherein f is i Is the joint friction of the ith robot, i is 1, 2; f. of a,j 、f b,j 、f c,j The first friction coefficient, the second friction coefficient and the third friction coefficient are respectively used for correcting the friction model and the real friction coefficientMeasuring the corresponding relation between the friction forces;
Figure FDA0003683716370000016
is the joint angular velocity of the ith robot;
s2, calculating an interactive force moment value by using a self-adaptive variable damping control method according to the state variable and the output variable of each robot at the current moment, and updating the interactive force moment in the state space control model into the interactive force moment value, wherein the interactive force moment value is as follows:
Figure FDA0003683716370000021
Figure FDA0003683716370000022
Γ(t)＝Γ(t-T)+α(F 1 (t-T)-F 2 (t-T))/D h
wherein, tau u,i The value of the interaction force moment of the ith robot is 1, 2; j. the design is a square i Is the jacobian matrix of the ith robot,
Figure FDA0003683716370000023
is J i The transposed matrix of (2); k h Is the stiffness coefficient, P, of the robot i The terminal pose of the ith robot is shown; p i r The terminal reference pose of the ith robot is obtained; d h The damping coefficient of the robot is shown, and delta D is the variation of the damping coefficient; v i The tip speed of the ith robot; v i r The reference speed is the tail end reference speed of the ith robot; Γ (t) is an integral function, and t is the current time;
Figure FDA0003683716370000024
is e x Derivative of the estimated value, e x The deviation between the reference pose at the tail end and the pose at the tail end of the double-robot system,
Figure FDA0003683716370000025
delta is a preset coefficient; t is a sampling period; alpha is a force feedback coefficient; f 1 (T-T) is the external force applied to the first robot at the time of T-T, F 2 (T-T) is the external force applied to the second robot at the T-T moment;
s3, taking the same output variable corresponding to each robot as an optimization target, combining the state constraint and the input constraint met by the output variable, and establishing an optimization problem with constraint by using a nonlinear model predictive control algorithm to solve the motion moment in the state space control model, wherein the optimization target is as follows:
Figure FDA0003683716370000026
wherein J (x (t), τ) v (t)) is a cost function, x (t) and τ v (t) state variables and moments of motion in the state space control model, respectively; t is p Is a predicted step length; Ψ (x) e (s),τ v (s)) is a rolling cost term, x e (s) is the error state, s is the integration time variable; phi (x) e (t+T p ),τ v (t+T p ) ) is a terminal cost term;
the rolling cost item and the terminal cost item are respectively as follows:
Figure FDA0003683716370000031
Φ(x e (t+T p ),τ v (t+T p ))＝x e (t+T p )Hx e (t+T p ) T
wherein x is e (s)＝(e P (s),e V (s),e F (s),e i,P (s),e i,V (s),e i,F (s))，e P (s)、e V (s)、e F (s) pose deviation, speed deviation and force deviation of the two-robot system, e i,P (s) deviation of the terminal pose of the ith robot from the terminal reference pose, e i,V (s) deviation of the tip speed of the ith robot from the tip reference speed, e i,F (s) is the deviation of the external force applied to the tail end of the ith robot and the tail end reference external force,
Figure FDA0003683716370000032
is x e (s) a transposed matrix; q is a state coefficient matrix, R is an input coefficient matrix, and H is a terminal state matrix;
the state constraint and the input constraint are:
Figure FDA0003683716370000033
Figure FDA0003683716370000034
Figure FDA0003683716370000035
wherein, x is a state variable,
Figure FDA0003683716370000036
is a state constraint set of the two robots,
Figure FDA0003683716370000037
reference Jacobian matrix for the ith robot, J i Is Jacobian matrix of the ith robot, and is speed precision matrix,
Figure FDA0003683716370000038
is a terminal state constraint set of a two-robot system, W is an output target transformation matrix,
Figure FDA0003683716370000039
is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,
Figure FDA00036837163700000310
in order to limit the input torque,
Figure FDA00036837163700000311
is a Coriolis force matrix C (x) 1 ,x 2 ) The upper bound of (a) is,
Figure FDA00036837163700000312
which is the upper bound of the gravity matrix G,
Figure FDA00036837163700000313
is an inertia matrix M (x) 1 ) The upper bound of (a) is,
Figure FDA00036837163700000314
maximum terminal acceleration, K ξ In order to be a stability coefficient matrix,
Figure FDA00036837163700000315
is an input constraint set;
and S4, superposing the interaction force moments and the movement moments to obtain corresponding resultant moments, and driving the corresponding robots according to the moment values corresponding to the robots in the resultant moments.
2. A dual-robot synchronous processing system based on model prediction impedance control is characterized by comprising:
the model establishing module is used for establishing a state space control model of the double robots based on a kinetic equation by taking the interaction force moment and the motion moment of the double robots as input variables, taking joint angles and joint angular velocities as state variables and taking the tail end poses, the tail end velocities and the external forces borne by the tail ends of the robots in a three-dimensional space as output variables, wherein the state space control model is as follows:
Figure FDA0003683716370000041
τ＝τ u +τ v
wherein x is 1 Is a joint angle state matrix variable, x, of a dual robot system 2 Is a joint angular velocity state matrix variable of the double-robot system,
Figure FDA0003683716370000042
Figure FDA0003683716370000043
and
Figure FDA0003683716370000044
are respectively x 1 And x 2 A derivative of (a); m (x) 1 ) Is an inertia matrix of a two-robot system, M -1 (x 1 ) Is M (x) 1 ) The inverse matrix of (d); tau is joint resultant force matrix of the double-robot system, tau u And τ v Respectively an interaction force moment and a movement moment; j (x) 1 ) Jacobian matrix, J, for a two-robot system T (x 1 ) Is J (x) 1 ) The transposed matrix of (2); f (x) 1 ,x 2 ) The external force matrix is an external force matrix of the double-robot system; c (x) 1 ,x 2 ) A Coriolis force matrix of a dual-robot system; g (x) 1 ) A gravity matrix of a double-robot system; f is a joint friction force matrix of the double-robot system;
the joint friction force matrix is:
f＝(f 1 ,f 2 )
Figure FDA0003683716370000045
wherein f is i Is the joint friction of the ith robot, i is 1, 2; f. of a,j 、f b,j 、f c,j Respectively a first friction coefficient and a second friction coefficientAnd a third friction coefficient for correcting the corresponding relationship between the friction model and the measured friction;
Figure FDA0003683716370000048
is the joint angular velocity of the ith robot;
an interactive force moment calculation and update module, configured to calculate an interactive force moment value by using a self-adaptive variable damping control method according to a value of a state variable and a value of an output variable of each robot at a current time, and update an interactive force moment in the state space control model to the interactive force moment value, where the interactive force moment value is:
Figure FDA0003683716370000046
Figure FDA0003683716370000047
Γ(t)＝Γ(t-T)+α(F 1 (t-T)-F 2 (t-T))/D h
wherein, tau u,i The value of the interaction force moment of the ith robot is 1, 2; j. the design is a square i Is the jacobian matrix of the ith robot,
Figure FDA0003683716370000051
is J i The transposed matrix of (2); k h Is the stiffness coefficient, P, of the robot i The terminal pose of the ith robot is shown; p i r The terminal reference pose of the ith robot is obtained; d h The damping coefficient of the robot is shown, and delta D is the variation of the damping coefficient; v i The tip speed of the ith robot; v i r The reference speed is the tail end reference speed of the ith robot; Γ (t) is an integral function, and t is the current time;
Figure FDA0003683716370000052
is e x The derivative of the estimated value of the time,e x the deviation between the reference pose at the tail end and the pose at the tail end of the double-robot system,
Figure FDA0003683716370000053
delta is a preset coefficient; t is a sampling period; alpha is a force feedback coefficient; f 1 (T-T) is the external force applied to the first robot at the time of T-T, F 2 (T-T) is the external force applied to the second robot at the T-T moment;
the motion torque solving module is used for establishing an optimization problem with constraints by using a nonlinear model predictive control algorithm by taking the same output variable corresponding to each robot as an optimization target and combining the state constraint and the input constraint which are met by the output variable, so as to solve the motion torque in the state space control model, wherein the optimization target is as follows:
Figure FDA0003683716370000054
wherein J (x (t), τ) v (t)) is a cost function, x (t) and τ v (t) state variables and moments of motion in the state space control model, respectively; t is p Is a predicted step length; Ψ (x) e (s),τ v (s)) is a rolling cost term, x e (s) is the error state, s is the integration time variable; phi (x) e (t+T p ),τ v (t+T p ) ) is a terminal cost term;
the rolling cost item and the terminal cost item are respectively as follows:
Figure FDA0003683716370000055
Φ(x e (t+T p ),τ v (t+T p ))＝x e (t+T p )Hx e (t+T p ) T
wherein x is e (s)＝(e P (s),e V (s),e F (s),e i,P (s),e i,V (s),e i,F (s))，e P (s)、e V (s)、e F (s) pose deviation, speed deviation and force deviation of the two-robot system, e i,P (s) deviation of the terminal pose of the ith robot from the terminal reference pose, e i,V (s) deviation of the tip speed of the ith robot from the tip reference speed, e i,F (s) is the deviation of the external force applied to the tail end of the ith robot and the tail end reference external force,
Figure FDA0003683716370000061
is x e (s) a transposed matrix; q is a state coefficient matrix, R is an input coefficient matrix, and H is a terminal state matrix;
the state constraint and the input constraint are:
Figure FDA0003683716370000062
Figure FDA0003683716370000063
Figure FDA0003683716370000064
wherein, x is a state variable,
Figure FDA0003683716370000065
is a state constraint set of the two robots,
Figure FDA0003683716370000066
reference Jacobian matrix for the ith robot, J i Is Jacobian matrix of the ith robot, and is speed precision matrix,
Figure FDA0003683716370000067
is a terminal state constraint set of a dual-robot system, and W is an output targetThe scaling transformation matrix is used for scaling the transformation matrix,
Figure FDA0003683716370000068
is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,
Figure FDA0003683716370000069
in order to limit the input torque,
Figure FDA00036837163700000610
is a Coriolis force matrix C (x) 1 ,x 2 ) The upper bound of (a) is,
Figure FDA00036837163700000611
which is the upper bound of the gravity matrix G,
Figure FDA00036837163700000612
is an inertia matrix M (x) 1 ) The upper bound of (a) is,
Figure FDA00036837163700000613
maximum terminal acceleration, K ξ In order to be a stability coefficient matrix,
Figure FDA00036837163700000614
is an input constraint set;
and the driving module is used for respectively superposing the interaction force moments and the movement moments to obtain corresponding resultant moments, and driving the corresponding robots according to the moment values corresponding to the robots in the resultant moments.
CN202110972833.4A 2021-08-24 2021-08-24 Model prediction impedance control-based double-robot synchronous processing method and system Active CN113664830B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110972833.4A CN113664830B (en) 2021-08-24 2021-08-24 Model prediction impedance control-based double-robot synchronous processing method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110972833.4A CN113664830B (en) 2021-08-24 2021-08-24 Model prediction impedance control-based double-robot synchronous processing method and system

Publications (2)

Publication Number Publication Date
CN113664830A CN113664830A (en) 2021-11-19
CN113664830B true CN113664830B (en) 2022-08-02

Family

ID=78545454

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110972833.4A Active CN113664830B (en) 2021-08-24 2021-08-24 Model prediction impedance control-based double-robot synchronous processing method and system

Country Status (1)

Country Link
CN (1) CN113664830B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114700954B (en) * 2022-05-12 2023-12-22 中国计量大学 Six-degree-of-freedom industrial robot hole making rigidity optimization method

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104552284A (en) * 2013-10-15 2015-04-29 库卡实验仪器有限公司 Method for handling objects using at least two industrial robots, and industrial robots
CN109807902A (en) * 2019-04-08 2019-05-28 青岛大学 A kind of double-mechanical arm strength based on Backstepping/position fuzzy hybrid control method
CN110421547A (en) * 2019-07-12 2019-11-08 中南大学 A kind of tow-armed robot collaboration impedance adjustment based on estimated driving force model
CN110962129A (en) * 2019-12-20 2020-04-07 中国科学院宁波材料技术与工程研究所 Impedance control method for mechanical arm
CN111319036A (en) * 2018-12-15 2020-06-23 天津大学青岛海洋技术研究院 Self-adaptive algorithm-based mobile mechanical arm position/force active disturbance rejection control method
CN111730599A (en) * 2020-07-08 2020-10-02 深圳市优必选科技股份有限公司 Impedance control method and device, impedance controller and robot

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
ITUA20163608A1 (en) * 2016-05-19 2017-11-19 Milano Politecnico PROCEDURE AND DEVICE FOR CONTROL OF THE HANDLING OF ONE OR MORE COLLABORATIVE ROBOTS

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104552284A (en) * 2013-10-15 2015-04-29 库卡实验仪器有限公司 Method for handling objects using at least two industrial robots, and industrial robots
CN111319036A (en) * 2018-12-15 2020-06-23 天津大学青岛海洋技术研究院 Self-adaptive algorithm-based mobile mechanical arm position/force active disturbance rejection control method
CN109807902A (en) * 2019-04-08 2019-05-28 青岛大学 A kind of double-mechanical arm strength based on Backstepping/position fuzzy hybrid control method
CN110421547A (en) * 2019-07-12 2019-11-08 中南大学 A kind of tow-armed robot collaboration impedance adjustment based on estimated driving force model
CN110962129A (en) * 2019-12-20 2020-04-07 中国科学院宁波材料技术与工程研究所 Impedance control method for mechanical arm
CN111730599A (en) * 2020-07-08 2020-10-02 深圳市优必选科技股份有限公司 Impedance control method and device, impedance controller and robot

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于手持导航器的协作机器人引导控制技术研究;洪鹰等;《天津大学学报(自然科学与工程技术版)》;20200903(第11期);全文 *
机械臂在未知环境下基于力/位/阻抗的多交互控制;段星光等;《机器人》;20170715(第04期);全文 *

Also Published As

Publication number Publication date
CN113664830A (en) 2021-11-19

Similar Documents

Publication Publication Date Title
Slotine et al. Adaptive manipulator control: A case study
CN110497411B (en) Industrial robot collaborative motion control method
CN106475999B (en) The acceleration control method of Dual-Arm Coordination based on impedance model under hard conditions
CN107704660B (en) Error compensation method for industrial robot
CN112757306A (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
Tarn et al. New nonlinear control algorithms for multiple robot arms
CN110053044B (en) Model-free self-adaptive smooth sliding mode impedance control method for clamping serial fruits by parallel robot
CN111687827B (en) Control method and control system for coordinating and operating weak rigid member by two robots
CN113427483A (en) Double-machine manpower/bit multivariate data driving method based on reinforcement learning
CN113664830B (en) Model prediction impedance control-based double-robot synchronous processing method and system
CN109176480B (en) Sliding mode control method and system for parallel robot
CN111805536A (en) Self-adaptive sliding mode control method for fruit sorting parallel robot mechanism considering coupling effect
JPH0991004A (en) Method for estimating load weight
CN110587611B (en) Mechanical arm control method for television set assembly line
CN116810792A (en) Fuse and booster double-robot assembly flexible control method based on neural network
Owen et al. A multi-arm robotic system for optimal sculpting
Nahavandi et al. Automated robotic grinding by low-powered manipulator
Murao et al. Predictive visual feedback control with eye-in/to-hand configuration via stabilizing receding horizon approach
CN112571411B (en) Dual-purpose mechanical arm for intelligent production line of television
CN113001549A (en) Multi-mechanical-arm load distribution method based on generalized grasping inverse matrix
JP7028196B2 (en) Robot control device, robot control method, and robot control program
Gnad et al. Computation of dynamic joint reaction forces of PKM and its use for load-minimizing trajectory planning
Ren et al. Joint torque control of a collaborative robot based on active disturbance rejection with the consideration of actuator delay
Wang et al. Design of kinematic controller for real-time vision guided robot manipulators
Shen et al. Teleoperated single-master-multiple-slave system for cooperative manipulations in task space

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant