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 PDFInfo
- 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
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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
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
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:
τ=τ 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, andare 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 )
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:
Γ(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,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;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,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:
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:
Φ(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,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:
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,is a state constraint set of the two robots,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,is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,in order to limit the input torque,is a Coriolis force matrix C (x) 1 ,x 2 ) The upper bound of (a) is,which is the upper bound of the gravity matrix G,is an inertia matrix M (x) 1 ) The upper bound of (a) is,maximum terminal acceleration, K ξ In order to be a stability coefficient matrix,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:
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 )
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:
τ=τ 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, andare 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:
Γ(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,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;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,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:
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:
Φ(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,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:
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,is a state constraint set of the two robots,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,is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,in order to limit the input torque,is a Coriolis force matrix C (x) 1 ,x 2 ) The upper bound of (a) is,which is the upper bound of the gravity matrix G,is an inertia matrix M (x) 1 ) The upper bound of (a) is,maximum terminal acceleration, K ξ In order to be a matrix of stability coefficients,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:
τ=τ 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, andare 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 )
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;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:
Γ(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,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;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,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:
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:
Φ(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,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:
wherein, x is a state variable,is a state constraint set of the two robots,reference Jacobian matrix for the ith robot, J i Is Jacobian matrix of the ith robot, and is speed precision matrix,is a terminal state constraint set of a two-robot system, W is an output target transformation matrix,is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,in order to limit the input torque,is a Coriolis force matrix C (x) 1 ,x 2 ) The upper bound of (a) is,which is the upper bound of the gravity matrix G,is an inertia matrix M (x) 1 ) The upper bound of (a) is,maximum terminal acceleration, K ξ In order to be a stability coefficient matrix,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:
τ=τ 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, andare 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 )
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;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:
Γ(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,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;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,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:
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:
Φ(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,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:
wherein, x is a state variable,is a state constraint set of the two robots,reference Jacobian matrix for the ith robot, J i Is Jacobian matrix of the ith robot, and is speed precision matrix,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,is a Jacobian matrix J (x) 1 ) The upper bound of (a) is,in order to limit the input torque,is a Coriolis force matrix C (x) 1 ,x 2 ) The upper bound of (a) is,which is the upper bound of the gravity matrix G,is an inertia matrix M (x) 1 ) The upper bound of (a) is,maximum terminal acceleration, K ξ In order to be a stability coefficient matrix,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.
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)
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)
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)
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 |
-
2021
- 2021-08-24 CN CN202110972833.4A patent/CN113664830B/en active Active
Patent Citations (6)
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)
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 | |
CN106475999B (en) | The acceleration control method of Dual-Arm Coordination based on impedance model under hard conditions | |
CN112757306B (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
CN110497411B (en) | Industrial robot collaborative motion control method | |
CN105772917B (en) | A kind of three joint spot welding robot's Trajectory Tracking Control methods | |
CN107704660B (en) | Error compensation method for industrial robot | |
CN111687827B (en) | Control method and control system for coordinating and operating weak rigid member by two robots | |
CN105598968B (en) | A kind of motion planning and control method of parallel mechanical arm | |
Tarn et al. | New nonlinear control algorithms for multiple robot arms | |
CN113664830B (en) | Model prediction impedance control-based double-robot synchronous processing method and system | |
CN109176480A (en) | A kind of sliding-mode control and system of parallel robot | |
Sun et al. | A novel tool path smoothing algorithm of 6R manipulator considering pose-dependent dynamics by designing asymmetrical FIR filters | |
JPH0991004A (en) | Method for estimating load weight | |
CN110587611B (en) | Mechanical arm control method for television set assembly line | |
WO2020149020A1 (en) | Robot control device, robot control method, and robot control program | |
CN116079740A (en) | Robot variable impedance processing method based on observer | |
CN107894709A (en) | Controlled based on Adaptive critic network redundancy Robot Visual Servoing | |
CN108406766B (en) | Synchronous control method for multi-mechanical arm system based on composite integral sliding mode | |
Xu et al. | Extended state observer based dynamic iterative learning for trajectory tracking control of a six-degrees-of-freedom manipulator | |
CN112571411B (en) | Dual-purpose mechanical arm for intelligent production line of television | |
Nahavandi et al. | Automated robotic grinding by low-powered manipulator | |
CN113001549A (en) | Multi-mechanical-arm load distribution method based on generalized grasping inverse matrix | |
Gnad et al. | Computation of dynamic joint reaction forces of PKM and its use for load-minimizing trajectory planning | |
Fernandez et al. | Multi-surface admittance control approach applied on robotic assembly of large-scale parts in aerospace manufacturing | |
Wang et al. | Design of kinematic controller for real-time vision guided robot manipulators |
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 |