CN111037560A - Cooperative robot compliance control method and system - Google Patents
Cooperative robot compliance control method and system Download PDFInfo
- Publication number
- CN111037560A CN111037560A CN201911358551.4A CN201911358551A CN111037560A CN 111037560 A CN111037560 A CN 111037560A CN 201911358551 A CN201911358551 A CN 201911358551A CN 111037560 A CN111037560 A CN 111037560A
- Authority
- CN
- China
- Prior art keywords
- robot
- representing
- joint
- force
- control
- 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.)
- Granted
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/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/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/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)
- Manipulator (AREA)
Abstract
The invention discloses a cooperative robot compliance force control method and a cooperative robot compliance force control system, wherein the method comprises the following steps: acquiring and initializing state variables of the robot; obtaining a current rotation matrix based on the initialized state variable; reading current state feedback information of the robot based on a current rotation matrix; the method comprises the steps of constructing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in a robot system based on current state feedback information; rewriting a joint moment function, and obtaining a final constraint optimization model; updating the state variable and the control moment in the final constraint optimization model based on the dynamic neural network model; and judging whether the current time is greater than the task time, if so, ending the compliance force control, and otherwise, returning to obtain the current rotation matrix. In the embodiment of the invention, high-precision force control in the contact force direction and motion control in the free motion direction can be realized simultaneously.
Description
Technical Field
The invention relates to the technical field of intelligent control of robots, in particular to a cooperative robot compliance control method and system.
Background
A collaborative robot is a robot that is capable of working in concert with humans within a common workspace. The cooperative robot can directly work with human staff in parallel without isolation by using a safety fence, has the characteristics of quick production line deployment, simple task switching, good man-machine friendliness and the like, has wide application prospects in the fields of medical care, light industrial assembly, electronic information, home service and the like, and is considered as an important carrier for realizing industrial 4.0 and intelligent manufacturing 2025.
The most important premise for the cooperative robot to enter practical application is to realize 'man-machine co-fusion', wherein the improvement of the flexibility of the system is particularly important. The force control can improve the flexibility of the system, enhance human-computer interaction and provide intelligent response, so that the robot can operate autonomously in weak structured and unstructured environments. Therefore, the application fields of the robot are wider, such as flexible assembly, double-arm coordination operation, man-machine interaction, dexterous hand grabbing of objects, foot type robot gait control and the like. The method has strong application value and practical significance for the research of redundant robot manpower control.
The existing force control method for a cooperative robot with redundant degrees of freedom is mainly realized based on the pseudo-inverse calculation of a Jacobian matrix of the robot; however, the following problems are common to this method: 1) pseudo-inverse computation of the Jacobian matrix results in too high a computational cost; 2) physical constraints of the system are difficult to handle.
Disclosure of Invention
The invention aims to overcome the defects of the prior art, and provides a cooperative robot compliance force control method and a cooperative robot compliance force control system, which can simultaneously realize high-precision force control in a contact force direction and motion control in a free motion direction, can realize online optimization of joint torque, and can ensure that a robot does not exceed physical constraint in the compliance force control process.
In order to solve the technical problem, an embodiment of the present invention provides a compliance force control method for a cooperative robot, where the method includes:
acquiring and initializing state variables of the robot;
obtaining a current rotation matrix based on the initialized state variable;
reading current state feedback information of the robot based on the current rotation matrix;
constructing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;
rewriting a joint moment function, and obtaining a final constraint optimization model;
updating the state variable and the control moment in the final constraint optimization model based on the dynamic neural network model;
and judging whether the current time is greater than the task time, if so, ending the control of the compliance force of the robot, and otherwise, returning to obtain the current rotation matrix based on the initialized state variable.
Optionally, before obtaining the state variables of the robot and initializing, the method further includes:
respectively modeling in a tool coordinate system and a base coordinate system according to the orthogonal characteristic of the contact force between the robot motion and the workpiece to obtain a robot motion modeling system;
wherein the base mark represents R0(x0,y0,z0) (ii) a Representation R of the tool coordinate Systemt(xt,yt,zt)。
Optionally, the contact force between the robot and the workpiece and z in the tool coordinate systemtParallel, while xtAnd ytDefining a free motion of the robotic end effector;
in the operation process of the robot, the actual position x and the expected track x of the robot end effectordWith a slight deviation in the base coordinate system R0(x0,y0,z0) And said tool coordinate system Rt(xt,yt,zt) Described below as the deviation deltaX of the robot from the desired trajectory under the base mark and the robot at the tool, respectivelyDeviation deltaX from the desired trajectory in a coordinate systemt。
Optionally, the modeling is performed in the tool coordinate system and the base coordinate system respectively, and a process of obtaining the robot motion modeling system is as follows:
in the tool coordinate system Rt(xt,yt,zt) Since the friction between the robot and the workpiece is neglected, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:
Ft＝kf∑tδXt； (1)
wherein k isfRepresenting a stiffness coefficient; sigmatA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate systemtThe relationship with its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1;
definition of
Then in said tool coordinate system Rt(xt,yt,zt) Lower position tracking error etComprises the following steps:
using a rotation matrix S with a known contact surfacetDescribing the tool coordinate system Rt(xt,yt,zt) And a base mark system R0(x0,y0,z0) The rotational relationship between them;
definitions F and e0Are each the base coordinate system R0(x0,y0,z0) Lower delta XtAnd FtThe corresponding description of (1) then includes:
et＝Ste0； (4)
δXt＝StδX； (5)
by the simultaneous expression of formulas (1) to (5), there are:
wherein δ X represents the robot in the base coordinate system R0(x0,y0,z0) (iv) deviation from the desired trajectory; f is in the base mark system R0(x0,y0,z0) A lower contact force;
in the base mark system R0(x0,y0,z0) The medium displacement δ X may be described as δ X ═ X-XdWherein the desired trajectory xdIs R0The desired position signal described in (1), and therefore, equations (6) - (7) are rewritten as:
defining the expected track and the command force of the robot as x respectivelydAnd Fd(ii) a Then, according to the descriptions of equations (5) and (9), the control objective can be described as designing a position-oriented redundant robot to control the strategy such that the contact force F → F described by equation (8)dWhile making the tracking error e described by equation (9)0→0；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
Optionally, in the robot motion modeling system, for simplifying the description, definition is performed
rd＝[Fd；0]，
Then formula (8) and formula (9) are rewritten as:
A(f(θ)-xd)＝r； (10)
the control objective is described by designing the joint such that r is rd；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
Optionally, the constructing an equality constraint for implementing compliance force control based on the current state feedback information includes:
feedback information of current state obtained under robot motion modeling system for given expected track xdContact force FtThe goal of achieving position-to-control is:
then an error vector is defined:
e＝r-rd＝[F-Fd；e0]； (16)
the equation can be reconstructed in the velocity layer as follows:
wherein k represents a positive control constant;
representing a joint angular velocity of the robot;
representing the first derivative of the error vector;
a first derivative representing the desired trajectory;
is represented by rdFirst derivative of rd＝[Fd；0]，FdRepresenting the force commands of the robot.
Optionally, the inequality constraints of joint angles, joint angular velocities, and joint moments in the robot system include:
the inequality constraint normalization is described as an inequality constraint for the velocity layer:
wherein the content of the first and second substances,
then the inequality constraint of the joint moments can be rewritten as:
wherein, β>0, then the system R is marked on the base of the end effector and the workpiece0(x0,y0,z0) When the lower contact force is F, the expression derivation of the moment of action it exerts at each joint can be found:
the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):
wherein the content of the first and second substances,
j represents a Jacobian matrix;
representing the first derivative of joint moment constraint, tau representing joint moment constraint, β representing positive control parameter, theta representing the joint angle of the robot;
representing a joint angular velocity of the robot; h represents a real number array;
a first derivative representing a commanded force of the robot;
representing a real number.
Optionally, the rewriting the joint moment function and obtaining the final constraint optimization model includes:
simplifying the objective function by using the command force F of the robotdInstead of in the base mark system R0(x0,y0,z0) The following contact forces F, then:
if the objective function described in equation (13) is defined in the joint angle layer, the final control quantity is the joint angular velocity
Thus passing throughFinding G2For the gradient of θ, an alternative description of it on the velocity layer is obtained:
to JT(θ)FdThe derivation yields:
wherein the content of the first and second substances,
the method comprises the following steps:
let H ═ H1,…,Hn]Then the above formula can describe the number as:
due to the second term in equation (15)
Not correlation, then choose the final objective function to choose as
By introducing a correction term
For in the objective function
And (4) carrying out convex processing, wherein the final constraint optimization model is as follows:
wherein the content of the first and second substances,
a rank representing a commanded force of the robot; j represents a Jacobian matrix;
representing a joint angular velocity of the robot; r isrRepresents a reference instruction; a denotes a matrix of abbreviations.
Optionally, the updating the state variables and the control moments in the final constraint optimization model based on the dynamic neural network model includes:
defining input state variables
For dual state variables in the constraint optimization model, the lagrangian function is selected as follows:
according to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constraint optimization model is equivalently expressed as:
wherein, PΩ(. cndot.) is a clipping function defined as:
in solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is:
wherein the content of the first and second substances,
represents a real number;
representing input state quantity lambda2The first derivative of (a);
representing input state quantity lambda1The first derivative of (a);
represents the joint angular velocity;
represents the joint angular acceleration; j represents a Jacobian matrix; r isrRepresents a reference instruction; a represents a shorthand matrix; g1Represents the first element; g2mRepresents the 2 m-th element; e represents a positive constant.
In addition, an embodiment of the present invention further provides a compliance force control system for a cooperative robot, where the system includes:
an initialization module: the robot state variable acquiring and initializing system is used for acquiring state variables of the robot and initializing the state variables;
an obtaining module: the device comprises a processor, a processor and a controller, wherein the processor is used for obtaining a current rotation matrix based on an initialization state variable;
a reading module: the current state feedback information of the robot is read based on the current rotation matrix;
constructing a module: the system comprises a robot system, a controller and a controller, wherein the robot system is used for establishing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;
and a rewriting module: the joint moment function is rewritten, and a final constraint optimization model is obtained;
an update module: the state variable and the control moment in the final constraint optimization model are updated based on the dynamic neural network model;
a judging module: and the method is used for judging whether the current time is greater than the task time, if so, ending the control of the compliance force of the robot, and otherwise, returning to obtain the current rotation matrix based on the initialized state variable.
In the embodiment of the invention, the method can simultaneously realize high-precision force control in the contact force direction and motion control in the free motion direction; the online optimization of the joint torque can be realized; and to ensure that the robot does not exceed its physical constraints during compliance force control.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic flow diagram of a cooperative robotic compliance control method in an embodiment of the present invention;
FIG. 2 is a schematic diagram of robot position-force control in an embodiment of the present invention;
FIG. 3 is a schematic structural component diagram of a cooperative robotic compliance control system in an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Examples
Referring to fig. 1, fig. 1 is a schematic flow chart of a compliance force control method of a cooperative robot according to an embodiment of the present invention.
As shown in fig. 1, a method of cooperative robotic compliance control, the method comprising:
s11: acquiring and initializing state variables of the robot;
in a specific implementation process of the present invention, before obtaining and initializing the state variables of the robot, the method further includes: orthogonal features for contact force between the robot motion and workpieceRespectively modeling in a tool coordinate system and a base coordinate system to obtain a robot motion modeling system; wherein the base mark represents R0(x0,y0,z0) (ii) a Representation R of the tool coordinate Systemt(xt,yt,zt)。
Further, the contact force between the robot and the workpiece and z in the tool coordinate systemtParallel, while xtAnd ytDefining a free motion of the robotic end effector;
in the operation process of the robot, the actual position x and the expected track x of the robot end effectordWith a slight deviation in the base coordinate system R0(x0,y0,z0) And said tool coordinate system Rt(xt,yt,zt) Described below as the deviation deltaX of the robot from the desired trajectory in the base coordinate system and the deviation deltaX of the robot from the desired trajectory in the tool coordinate system, respectivelyt。
Further, the process of performing respective modeling in the tool coordinate system and the base coordinate system to obtain the robot motion modeling system is as follows:
in the tool coordinate system Rt(xt,yt,zt) Since the friction between the robot and the workpiece is neglected, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:
Ft＝kf∑tδXt； (1)
wherein k isfRepresenting a stiffness coefficient; sigmatA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate systemtThe relationship with its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1;
definition of
Then in said tool coordinate system Rt(xt,yt,zt) Lower position tracking error etComprises the following steps:
using a rotation matrix S with a known contact surfacetDescribing the tool coordinate system Rt(xt,yt,zt) And a base mark system R0(x0,y0,z0) The rotational relationship between them;
definitions F and e0Are each the base coordinate system R0(x0,y0,z0) Lower delta XtAnd FtThe corresponding description of (1) then includes:
et＝Ste0； (4)
δXt＝StδX； (5)
by the simultaneous expression of formulas (1) to (5), there are:
wherein δ X represents the robot in the base coordinate system R0(x0,y0,z0) (iv) deviation from the desired trajectory; f is in the base mark system R0(x0,y0,z0) A lower contact force;
in the base mark system R0(x0,y0,z0) The medium displacement δ X may be described as δ X ═ X-XdWherein the desired trajectory xdIs R0The desired position signal described in (1), thereforeThe equations (6) to (7) are rewritten as:
defining the expected track and the command force of the robot as x respectivelydAnd Fd(ii) a Then, according to the descriptions of equations (5) and (9), the control objective can be described as designing a position-oriented redundant robot to control the strategy such that the contact force F → F described by equation (8)dWhile making the tracking error e described by equation (9)0→0；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
Further, in the robot motion modeling system, for simplifying the description, definitions are defined
rd＝[Fd；0]，
Then formula (8) and formula (9) are rewritten as:
A(f(θ)-xd)＝r； (10)
the control objective is described by designing the joint such that r is rd；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
Specifically, first, the tool coordinates are determined based on the orthogonal characteristics of the robot motion and the contact forceModeling in the system and the base standard system respectively; to avoid loss of generality, a coordinate system R is definedt(xt,yt,zt) And a base mark system R0(x0,y0,z0) The contact force between the robot and the workpiece and z in the tool coordinate system, as shown in particular in fig. 2tParallel, while xtAnd ytDefining a free motion of the robotic end effector; in the operation process, the actual position x and the expected track x of the robot end effectordThere is a slight deviation in the base mark R0(x0,y0,z0) And a tool coordinate system Rt(xt,yt,zt) Described below as the deviation deltaX of the robot from the desired trajectory in the base coordinate system and the deviation deltaX of the robot from the desired trajectory in the tool coordinate system, respectivelyt。
In the tool coordinate system Rt(xt,yt,zt) Since the friction between the robot and the workpiece is neglected, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:
Ft＝kf∑tδXt； (1)
wherein k isfRepresenting a stiffness coefficient; sigmatA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate systemtAnd its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1.
Definition of
Then in the tool coordinate system Rt(xt,yt,zt) Lower position tracking error etComprises the following steps:
known conditions at the contact surfaceUnder the condition of using a rotation matrix StDescribing the tool coordinate System Rt(xt,yt,zt) And a base mark system R0(x0,y0,z0) The rotational relationship between them.
Definitions F and e0Are respectively a base mark system R0(x0,y0,z0) Lower delta XtAnd FtThe corresponding description of (1) then includes:
et＝Ste0； (4)
δXt＝StδX； (5)
by the simultaneous expression of formulas (1) to (5), there are:
wherein δ X represents the robot in said base coordinate system R0(x0,y0,z0) (iv) deviation from the desired trajectory; f is in the base mark system R0(x0,y0,z0) Lower contact force.
It is noted that in the base system R0(x0,y0,z0) The medium displacement δ X may be described as δ X ═ X-XdWherein the desired trajectory xdIs R0The desired position signal described in (1), and therefore, equations (6) - (7) are rewritten as:
defining the expected track and the command force of the robot as x respectivelydAnd Fd(ii) a Then, according to the descriptions of equations (5) and (9), the control objective can be described as designing a position-oriented redundant robot to control the strategy such that the contact force F → F described by equation (8)dWhile making the tracking error e described by equation (9)0→0；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
For simplicity of description, define
rd＝[Fd；0]，
Then formula (8) and formula (9) are rewritten as:
A(f(θ)-xd)＝r； (10)
the control objective is described as being achieved by designing the joint so that r is rd；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
After the system modeling model of the robot is constructed, the state variables of the robot are then obtained and then initialized accordingly.
S12: obtaining a current rotation matrix based on the initialized state variable;
in the specific implementation process of the invention, after the initialization state variable is obtained, the current rotation matrix is obtained according to the initialization state variable; i.e. rotation matrix S within the modeled robot systemt。
S13: reading current state feedback information of the robot based on the current rotation matrix;
in the practice of the inventionIn the process, feedback information of robot state feedback is obtained through the current rotation matrix, namely the state feedback information obtained through the robot system is included in the base system R0(x0,y0,z0) The contact force below, the joint angular velocity of the robot, and the joint angle of the robot.
S14: constructing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;
in a specific implementation process of the present invention, the constructing an equality constraint for implementing compliance force control based on the current state feedback information includes: feedback information of current state obtained under robot motion modeling system for given expected track xdContact force FtThe goal of achieving position-to-control is:
then an error vector is defined:
e＝r-rd＝[F-Fd；e0]； (16)
the equation can be reconstructed in the velocity layer as follows:
wherein k represents a positive control constant;
representing a joint angular velocity of the robot;
representing the first derivative of the error vector;
a first derivative representing the desired trajectory;
is represented by rdFirst derivative of rd＝[Fd；0]，FdRepresenting the force commands of the robot.
Further, the inequality constraint of joint angle, joint angular velocity and joint moment in the robot system includes: the inequality constraint normalization is described as an inequality constraint for the velocity layer:
wherein the content of the first and second substances,
then the inequality constraint of the joint moments can be rewritten as:
wherein, β>0, then the system R is marked on the base of the end effector and the workpiece0(x0,y0,z0) When the lower contact force is F, the expression derivation of the moment of action it exerts at each joint can be found:
the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):
wherein the content of the first and second substances,
j represents a Jacobian matrix;
representing the first derivative of joint moment constraint, tau representing joint moment constraint, β representing positive control parameter, theta representing the joint angle of the robot;
representing a joint angular velocity of the robot; h represents a real number array;
a first derivative representing a commanded force of the robot;
representing a real number.
First, basic QP problem description is performed; when the contact force of the end effector and the workpiece is F, the acting torque exerted by the end effector at each joint is as follows:
τ＝JT(θ)F； (11)
from the viewpoint of energy saving, the objective function is selected to be tauTτ/2 describes the energy consumption of the system; meanwhile, when the contact force F is large, in order to avoid safety risk caused by excessive moment generated on a certain joint, joint moment constraint tau is introduced on the basis of joint angle constraint and angular speed constraintmin≤τ≤τmax(ii) a The position-force control problem for redundant robots is described as a QP problem as follows:
min G1＝FTJ(θ)JT(θ)F/2； (12a)
s.t.rd＝A(f(θ)-xd)； (12b)
θmin≤θ≤θmax； (12c)
τmin≤JT(θ)Fd≤τmax； (12e)
then carrying out constraint reconstruction of equality and inequality; according to the above formula (10) and rdFor a given desired trajectory xdAnd a command force FdThe goal of achieving position-force control is:
then an error vector is defined:
e＝r-rd＝[F-Fd；e0]； (16)
the equation can be reconstructed in the velocity layer as follows:
wherein k represents a positive control constant;
representing a joint angular velocity of the robot;
representing the first derivative of the error vector;
a first derivative representing the desired trajectory;
is represented by rdFirst derivative of rd＝[Fd；0]，FdRepresenting the force commands of the robot.
For the above inequality constraints (12c) and (12d), with reference to the processing method in the above, inequality constraint normalization is described as an inequality constraint of the velocity layer:
wherein the content of the first and second substances,
the inequality constraint of joint moments (12e) can be rewritten as:
wherein, β>0, then the system R is marked on the base of the end effector and the workpiece0(x0,y0,z0) When the lower contact force is F, derivation of the expression of the moment of action applied at each joint by equation (11) can be obtained:
the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):
wherein the content of the first and second substances,
j represents a Jacobian matrix;
representing the first derivative of joint moment constraint, tau representing joint moment constraint, β representing positive control parameter, theta representing the joint angle of the robot;
representing a joint angular velocity of the robot; h represents a real number array;
a first derivative representing a commanded force of the robot;
representing a real number.
In summary, the redundant robot position-force control problem based on the constraint-optimization idea is described as follows on the angular velocity layer:
wherein the content of the first and second substances,
s15: rewriting a joint moment function, and obtaining a final constraint optimization model;
in the specific implementation process of the invention, the rewriting of the joint moment function and the obtaining of the final constraint optimization model comprise:
simplifying the objective function by using the command force F of the robotdInstead of in the base mark system R0(x0,y0,z0) The following contact forces F, then:
if the objective function described in equation (13) is defined in the joint angle layer, the final control quantity is the joint angular velocity
Thus by finding G2For the gradient of θ, an alternative description of it on the velocity layer is obtained:
to JT(θ)FdThe derivation yields:
wherein the content of the first and second substances,
the method comprises the following steps:
let H ═ H1,…,Hn]Then the above formula can describe the number as:
due to the second term in equation (15)
Not correlation, then choose the final objective function to choose as
By introducing a correction term
For in the objective function
And (4) carrying out convex processing, wherein the final constraint optimization model is as follows:
wherein the content of the first and second substances,
a rank representing a commanded force of the robot; j represents a Jacobian matrix;
representing a joint angular velocity of the robot; r isrRepresents a reference instruction; a denotes a matrix of abbreviations.
Specifically, a number of non-linear features are included in equation (12), including Jacobian matrices, and real-time contact force
This makes subsequent controller design difficult, thus simplifying the objective function: using command force F of the robotdInstead of in the base mark system R0(x0,y0,z0) The following contact forces F, then:
at FdIndependent of theta, using FdThe non-linearity degree of the target function can be greatly reduced; on the other hand, with reasonable controller design, the contact force F will eventually converge to FdThus the objective function before and after the replacement is the mostFinally, the target function as described in equation (13) is defined in the joint angle layer, since the final controlled variable is the joint angular velocity
Thus by finding G2For the gradient of θ, an alternative description of it on the velocity layer is obtained:
to JT(θ)FdThe derivation yields:
wherein the content of the first and second substances,
the method comprises the following steps:
let H ═ H1,…,Hn]Then the above formula can describe the number as:
due to the second term in equation (15)
Not correlation, then choose the final objective function to choose as
The objective function pair described by the formula (21a)
Non-convex by introducing a correction term
For in the objective function
And (4) carrying out convex processing, wherein the final constraint optimization model is as follows:
wherein the content of the first and second substances,
a rank representing a commanded force of the robot; j represents a Jacobian matrix;
representing a joint angular velocity of the robot; r isrRepresents a reference instruction; a denotes a matrix of abbreviations.
S16: updating the state variable and the control moment in the final constraint optimization model based on the dynamic neural network model;
in a specific implementation process of the present invention, the updating of the state variables and the control moments in the final constraint optimization model based on the dynamic neural network model includes: defining input state variables
For dual state variables in the constraint optimization model, the lagrangian function is selected as follows:
according to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constraint optimization model is equivalently expressed as:
wherein, PΩ(. cndot.) is a clipping function defined as:
in solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is:
wherein the content of the first and second substances,
represents a real number;
representing input state quantity lambda2The first derivative of (a);
representing input state quantity lambda1The first derivative of (a);
represents the joint angular velocity;
represents the joint angular acceleration; j represents a Jacobian matrix; r isrRepresents a reference instruction; a represents a shorthand matrix; g1Represents the first element; g2mRepresents the 2 m-th element; e represents a positive constant.
In particular, input state variables are defined
To constrain the dual state variables of equations (22b) and (22c), the lagrangian function is chosen as:
according to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constraint optimization model is equivalently expressed as:
wherein, PΩ(. cndot.) is a clipping function defined as:
in solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is:
wherein the content of the first and second substances,
represents a real number;
representing input state quantity lambda2The first derivative of (a);
representing input state quantity lambda1The first derivative of (a);
represents the joint angular velocity;
represents the joint angular acceleration; j represents a Jacobian matrix; r isrRepresents a reference instruction; a represents a shorthand matrix; g1Represents the first element; g2mRepresents the 2 m-th element; e represents a positive constant.
S17: judging whether the current time is greater than the task time;
in the implementation process of the present invention, it is necessary to determine whether the current time is greater than the task time, and if so, execute S18, otherwise return to S12.
S18: if so, ending the control of the compliance force of the robot, otherwise, returning to obtain the current rotation matrix based on the initialized state variable.
In the implementation process of the invention, when the current time is judged to be greater than the task time, the control of the compliance force of the robot is finished.
In the embodiment of the invention, the method can simultaneously realize high-precision force control in the contact force direction and motion control in the free motion direction; the online optimization of the joint torque can be realized; and to ensure that the robot does not exceed its physical constraints during compliance force control.
Examples
Referring to fig. 3, fig. 3 is a schematic structural composition diagram of a compliance force control system of a cooperative robot according to an embodiment of the present invention.
As shown in fig. 3, a cooperative robotic compliance control system, the system comprising:
the initialization module 21: the robot state variable acquiring and initializing system is used for acquiring state variables of the robot and initializing the state variables;
in a specific implementation process of the present invention, before obtaining and initializing the state variables of the robot, the method further includes: respectively modeling in a tool coordinate system and a base coordinate system according to the orthogonal characteristic of the contact force between the robot motion and the workpiece to obtain a robot motion modeling system; wherein the base mark represents R0(x0,y0,z0) (ii) a Representation R of the tool coordinate Systemt(xt,yt,zt)。
Further, the contact force between the robot and the workpiece and z in the tool coordinate systemtParallel, while xtAnd ytDefining a free motion of the robotic end effector;
in the operation process of the robot, the actual position x and the expected track x of the robot end effectordWith a slight deviation in the base coordinate system R0(x0,y0,z0) And said tool coordinate system Rt(xt,yt,zt) Described below as the deviation deltaX of the robot from the desired trajectory in the base coordinate system and the deviation deltaX of the robot from the desired trajectory in the tool coordinate system, respectivelyt。
Further, the process of performing respective modeling in the tool coordinate system and the base coordinate system to obtain the robot motion modeling system is as follows:
in the tool coordinate system Rt(xt,yt,zt) Since the friction between the robot and the workpiece is neglected, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:
Ft＝kf∑tδXt； (1)
wherein k isfRepresenting a stiffness coefficient; sigmatA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate systemtThe relationship with its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1;
definition of
Then in said tool coordinate system Rt(xt,yt,zt) Lower position tracking error etComprises the following steps:
using a rotation matrix S with a known contact surfacetDescribing the tool coordinate system Rt(xt,yt,zt) And a base mark system R0(x0,y0,z0) The rotational relationship between them;
definitions F and e0Are each the base coordinate system R0(x0,y0,z0) Lower delta XtAnd FtThe corresponding description of (1) then includes:
et＝Ste0； (4)
δXt＝StδX； (5)
by the simultaneous expression of formulas (1) to (5), there are:
wherein δ X represents the robot in the base coordinate system R0(x0,y0,z0) (iv) deviation from the desired trajectory; f is in the base mark system R0(x0,y0,z0) A lower contact force;
in the base mark system R0(x0,y0,z0) The medium displacement δ X may be described as δ X ═ X-XdWherein the desired trajectory xdIs R0The desired position signal described in (1), and therefore, equations (6) - (7) are rewritten as:
defining the expected track and the command force of the robot as x respectivelydAnd Fd(ii) a Then, according to the descriptions of equations (5) and (9), the control objective can be described as designing a position-oriented redundant robot to control the strategy such that the contact force F → F described by equation (8)dWhile making the tracking error e described by equation (9)0→0；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
Further, in the robot motion modeling system, for simplifying the description, definitions are defined
rd＝[Fd；0]，
Then formula (8) and formula (9) are rewritten as:
A(f(θ)-xd)＝r； (10)
the control objective is described by designing the joint such that r is rd；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
Specifically, modeling is respectively carried out in a tool coordinate system and a base coordinate system according to the orthogonal characteristic aiming at the motion and contact force of the robot; to avoid loss of generality, a coordinate system R is definedt(xt,yt,zt) And a base mark system R0(x0,y0,z0) The contact force between the robot and the workpiece and z in the tool coordinate system, as shown in particular in fig. 2tParallel, while xtAnd ytDefining a free motion of the robotic end effector; in the operation process, the actual position x and the expected track x of the robot end effectordThere is a slight deviation in the base mark R0(x0,y0,z0) And a tool coordinate system Rt(xt,yt,zt) Described below as the deviation deltaX of the robot from the desired trajectory in the base coordinate system and the deviation deltaX of the robot from the desired trajectory in the tool coordinate system, respectivelyt。
In the tool coordinate system Rt(xt,yt,zt) Since the friction between the robot and the workpiece is neglected, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:
Ft＝kf∑tδXt； (1)
wherein k isfRepresenting a stiffness coefficient; sigmatA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate systemtAnd its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1.
Definition of
Then in the tool coordinate system Rt(xt,yt,zt) Lower position tracking error etComprises the following steps:
using a rotation matrix S with a known contact surfacetDescribing the tool coordinate System Rt(xt,yt,zt) And a base mark system R0(x0,y0,z0) The rotational relationship between them.
Definitions F and e0Are respectively a base mark system R0(x0,y0,x0) Lower delta XtAnd FtThe corresponding description of (1) then includes:
et＝Ste0； (4)
δXt＝StδX； (5)
by the simultaneous expression of formulas (1) to (5), there are:
wherein δ X represents the robot in said base coordinate system R0(x0,y0,z0) (iv) deviation from the desired trajectory; f is in the base mark system R0(x0,y0,z0) Lower contact force.
It is noted that the base mark T0(x0,y0,z0) The medium displacement δ X may be described as δ X ═ X-XdWherein the desired trajectory xdIs R0The desired position signal described in (1), and therefore, equations (6) - (7) are rewritten as:
defining the expected track and the command force of the robot as x respectivelydAnd Fd(ii) a Then, according to the descriptions of equations (5) and (9), the control objective can be described as designing a position-oriented redundant robot to control the strategy such that the contact force F → F described by equation (8)dWhile making the tracking error e described by equation (9)0→0；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
For simplicity of description, define
rd＝[Fd；0]，
Then formula (8) and formula (9) are rewritten as:
A(f(θ)-xd)＝r； (10)
the control objective is described as being achieved by designing the joint so that r is rd；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
After the system modeling model of the robot is constructed, the state variables of the robot are then obtained and then initialized accordingly.
The obtaining module 22: the device comprises a processor, a processor and a controller, wherein the processor is used for obtaining a current rotation matrix based on an initialization state variable;
in the specific implementation process of the invention, after the initialization state variable is obtained, the current rotation matrix is obtained according to the initialization state variable; i.e. the rotation matrix St within the modeled robot system.
The reading module 23: the current state feedback information of the robot is read based on the current rotation matrix;
in the implementation process of the invention, feedback information of robot state feedback is obtained through the current rotation matrix, namely the state feedback information obtained through the robot system is included in the base standard system R0(x0,y0,z0) The contact force below, the joint angular velocity of the robot, and the joint angle of the robot.
The building module 24: the system comprises a robot system, a controller and a controller, wherein the robot system is used for establishing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;
in a specific implementation process of the present invention, the constructing an equality constraint for implementing compliance force control based on the current state feedback information includes: feedback information of current state obtained under robot motion modeling system for given expected track xdContact force FtThe goal of achieving position-to-control is:
then an error vector is defined:
e＝r-rd＝[F-Fd；e0]； (16)
the equation can be reconstructed in the velocity layer as follows:
wherein k represents a positive control constant;
representing a joint angular velocity of the robot;
representing the first derivative of the error vector;
a first derivative representing the desired trajectory;
is represented by rdFirst derivative of rd＝[Fd；0]，FdRepresenting the force commands of the robot.
Further, the inequality constraint of joint angle, joint angular velocity and joint moment in the robot system includes: the inequality constraint normalization is described as an inequality constraint for the velocity layer:
wherein the content of the first and second substances,
then the inequality constraint of the joint moments can be rewritten as:
wherein, β>0, then the system R is marked on the base of the end effector and the workpiece0(x0,y0,z0) When the lower contact force is F, the expression derivation of the moment of action it exerts at each joint can be found:
the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):
wherein the content of the first and second substances,
j represents a Jacobian matrix;
representing the first derivative of joint moment constraint, tau representing joint moment constraint, β representing positive control parameter, theta representing the joint angle of the robot;
representing a joint angular velocity of the robot; h represents a real number array;
a first derivative representing a commanded force of the robot;
representing a real number.
First, basic QP problem description is performed; when the contact force of the end effector and the workpiece is F, the acting torque exerted by the end effector at each joint is as follows:
τ＝JT(θ)F； (11)
from the viewpoint of energy saving, the objective function is selected to be tauTτ/2 describes the energy consumption of the system; meanwhile, when the contact force F is large, in order to avoid safety risk caused by excessive moment generated on a certain joint, joint moment constraint tau is introduced on the basis of joint angle constraint and angular speed constraintmin≤τ≤τmax(ii) a The position-force control problem for redundant robots is described as a QP problem as follows:
minG1＝FTJ(θ)JT(θ)F/2； (12a)
s.t.rd＝A(f(θ)-xd)； (12b)
θmin≤θ≤θmax； (12c)
τmin≤JT(θ)Fd≤τmax； (12e)
then carrying out constraint reconstruction of equality and inequality; according to the above formula (10) and rdFor a given desired trajectory xdAnd a command force FdThe goal of achieving position-force control is:
then an error vector is defined:
e＝r-rd＝[F-Fd；e0]； (16)
the equation can be reconstructed in the velocity layer as follows:
wherein k represents a positive control constant;
representing a joint angular velocity of the robot;
representing the first derivative of the error vector;
a first derivative representing the desired trajectory;
is represented by rdFirst derivative of rd＝[Fd；0]，FdRepresenting the force commands of the robot.
For the above inequalityBundles (12c) and (12d), with reference to the processing method in the above, describe inequality constraint normalization as inequality constraints for the velocity layer:
wherein the content of the first and second substances,
the inequality constraint of joint moments (12e) can be rewritten as:
wherein, β>0, then the system R is marked on the base of the end effector and the workpiece0(x0,y0,z0) When the lower contact force is F, derivation of the expression of the moment of action applied at each joint by equation (11) can be obtained:
the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):
wherein the content of the first and second substances,
j represents a Jacobian matrix;
representing the first derivative of the joint moment constraint,. tau.representing the joint moment constraint,. β representing the positive control parameter,. theta.Representing a joint angle of the robot;
representing a joint angular velocity of the robot; h represents a real number array;
a first derivative representing a commanded force of the robot;
representing a real number.
In summary, the redundant robot position-force control problem based on the constraint-optimization idea is described as follows on the angular velocity layer:
wherein the content of the first and second substances,
the rewriting module 25: the joint moment function is rewritten, and a final constraint optimization model is obtained;
in the specific implementation process of the invention, the rewriting of the joint moment function and the obtaining of the final constraint optimization model comprise:
simplifying the objective function by using the command force F of the robotdInstead of in the base mark system R0(x0,y0,z0) Lower contactForce F, then:
if the objective function described in equation (13) is defined in the joint angle layer, the final control quantity is the joint angular velocity
Thus by finding G2For the gradient of θ, an alternative description of it on the velocity layer is obtained:
to JT(θ)FdThe derivation yields:
wherein the content of the first and second substances,
the method comprises the following steps:
let H ═ H1,…,Hn]Then the above formula can describe the number as:
due to the second term in equation (15)
Not correlation, then choose the final objective function to choose as
By introducing a correction term
For in the objective function
And (4) carrying out convex processing, wherein the final constraint optimization model is as follows:
wherein the content of the first and second substances,
a rank representing a commanded force of the robot; j represents a Jacobian matrix;
representing a joint angular velocity of the robot; r isrRepresents a reference instruction; a denotes a matrix of abbreviations.
Specifically, a number of non-linear features are included in equation (12), including Jacobian matrices, and real-time contact force
This makes subsequent controller design difficult, thus simplifying the objective function: using command force F of the robotdInstead of in the base mark system R0(x0,y0,z0) The following contact forces F, then:
at FdIndependent of theta, using FdThe non-linearity degree of the target function can be greatly reduced; on the other hand, with reasonable controller design, the contact force F will eventually converge to FdTherefore, the objective function before and after replacement is finally equivalent, the objective function as described in equation (13) is defined in the joint angle layer, and the final control quantity is the joint angular velocity
Thus by finding G2For the gradient of θ, an alternative description of it on the velocity layer is obtained:
to JT(θ)FdThe derivation yields:
wherein the content of the first and second substances,
the method comprises the following steps:
let H ═ H1,…,Hn]Then the above formula can describe the number as:
due to the second term in equation (15)
Not correlation, then choose the final objective function to choose as
The objective function pair described by the formula (21a)
Non-convex by introducing a correction term
For in the objective function
And (4) carrying out convex processing, wherein the final constraint optimization model is as follows:
wherein the content of the first and second substances,
a rank representing a commanded force of the robot; j represents a Jacobian matrix;
representing a joint angular velocity of the robot; r isrRepresents a reference instruction; a denotes a matrix of abbreviations.
The update module 26: the state variable and the control moment in the final constraint optimization model are updated based on the dynamic neural network model;
in the implementation of the invention, the input state is definedVariables of
To constrain the dual state variables of equations (22b) and (22c), the lagrangian function is chosen as:
according to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constraint optimization model is equivalently expressed as:
wherein, PΩ(. cndot.) is a clipping function defined as:
in solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is:
wherein the content of the first and second substances,
represents a real number;
representing input state quantity lambda2The first derivative of (a);
representing input state quantity lambda1The first derivative of (a);
represents the joint angular velocity;
represents the joint angular acceleration; j represents a Jacobian matrix; r isrRepresents a reference instruction; a represents a shorthand matrix; g1Represents the first element; g2mRepresents the 2 m-th element; e represents a positive constant.
The judging module 27: and the method is used for judging whether the current time is greater than the task time, if so, ending the control of the compliance force of the robot, and otherwise, returning to obtain the current rotation matrix based on the initialized state variable.
In the specific implementation process of the invention, whether the current time is greater than the task time or not needs to be judged, and if so, the current time is less than the task time; and when the current time is judged to be greater than the task time, ending the control of the compliance force of the robot.
In the embodiment of the invention, the method can simultaneously realize high-precision force control in the contact force direction and motion control in the free motion direction; the online optimization of the joint torque can be realized; and to ensure that the robot does not exceed its physical constraints during compliance force control.
Those skilled in the art will appreciate that all or part of the steps in the methods of the above embodiments may be implemented by associated hardware instructed by a program, which may be stored in a computer-readable storage medium, and the storage medium may include: a Read Only Memory (ROM), a Random Access Memory (RAM), a magnetic or optical disk, or the like.
In addition, the method and system for controlling compliance force of a cooperative robot provided by the embodiment of the present invention are described in detail above, and a specific example should be adopted herein to explain the principle and the implementation manner of the present invention, and the description of the above embodiment is only used to help understanding the method and the core idea of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.
Claims (10)
1. A method of cooperative robotic compliance control, the method comprising:
acquiring and initializing state variables of the robot;
obtaining a current rotation matrix based on the initialized state variable;
reading current state feedback information of the robot based on the current rotation matrix;
constructing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;
rewriting a joint moment function, and obtaining a final constraint optimization model;
updating the state variable and the control moment in the final constraint optimization model based on the dynamic neural network model;
and judging whether the current time is greater than the task time, if so, ending the control of the compliance force of the robot, and otherwise, returning to obtain the current rotation matrix based on the initialized state variable.
2. The cooperative robot compliance force control method of claim 1, wherein before obtaining and initializing the state variables of the robot, further comprising:
respectively modeling in a tool coordinate system and a base coordinate system according to the orthogonal characteristic of the contact force between the robot motion and the workpiece to obtain a robot motion modeling system;
wherein the base mark represents R0(x0，y0，z0) (ii) a Representation R of the tool coordinate Systemt(xt，yt，zt)。
3. A method of cooperative robotic compliance force control as claimed in claim 2, wherein the contact force between the robot and workpiece and z in the tool coordinate systemtParallel, while xtAnd ytDefining a free motion of the robotic end effector;
in the operation process of the robot, the actual position x and the expected track x of the robot end effectordWith a slight deviation in the base coordinate system R0(x0，y0，z0) And said tool coordinate system Rt(xt，yt，zt) Described below as the deviation deltaX of the robot from the desired trajectory in the base coordinate system and the deviation deltaX of the robot from the desired trajectory in the tool coordinate system, respectivelyt。
4. A cooperative robotic compliance control method as claimed in claim 3, wherein the separate modelling within the tool coordinate system and the base coordinate system, the process of obtaining a robotic motion modelling system is as follows:
in the tool coordinate system Rt(xt，yt，zt) Since the friction between the robot and the workpiece is neglected, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:
Ft＝kf∑tδXt； (1)
wherein k isfRepresenting a stiffness coefficient; sigmatA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate systemtThe relationship with its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1;
definition of
Then in said tool coordinate system Rt(xt，yt，zt) Lower position tracking error etComprises the following steps:
using a rotation matrix S with a known contact surfacetDescribing the tool coordinate system Rt(xt，yt，zt) And a base mark system R0(x0，y0，z0) The rotational relationship between them;
definitions F and e0Are each the base coordinate system R0(x0，y0，z0) Lower delta XtAnd FtThe corresponding description of (1) then includes:
et＝Ste0； (4)
δXt＝StδX； (5)
by the simultaneous expression of formulas (1) to (5), there are:
wherein δ X represents the robot in the base coordinate system R0(x0，y0，z0) (iv) deviation from the desired trajectory; f is in the base mark system R0(x0，y0，z0) A lower contact force;
in the base mark system R0(x0，y0，z0) The medium displacement δ X may be described as δ X ═ X-XdWherein the desired trajectory xdIs R0The desired position signal described in (1), and therefore, equations (6) - (7) are rewritten as:
defining the expected track and the command force of the robot as x respectivelydAnd Fd(ii) a Then, according to the descriptions of equations (5) and (9), the control objective can be described as designing a position-oriented redundant robot to control the strategy such that the contact force F → F described by equation (8)dWhile making the tracking error e described by equation (9)0→0；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
5. The collaborative robotic compliance force control method of claim 4, wherein the robot is configured to control compliance force of the robotFor simplicity of description in a motion modeling system, definitions are defined
rd＝[Fd；0]，
Then formula (8) and formula (9) are rewritten as:
A(f(θ)-xd)＝r； (10)
the control objective is described by designing the joint such that r is rd；∑tRepresenting a parameter matrix describing the contact;
representing a parameter matrix describing the motion.
6. The cooperative robotic compliance force control method of claim 1, wherein said constructing an equality constraint to implement compliance force control based on the current state feedback information comprises:
feedback information of current state obtained under robot motion modeling system for given expected track xdContact force FtThe goal of achieving position-to-control is:
then an error vector is defined:
e＝r-rd＝[F-Fd；e0]； (16)
the equation can be reconstructed in the velocity layer as follows:
wherein k represents a positive control constant;
representing a joint angular velocity of the robot;
representing the first derivative of the error vector;
a first derivative representing the desired trajectory;
is represented by rdFirst derivative of rd＝[Fd；0]，FdRepresenting the force commands of the robot.
7. The cooperative robotic compliance control method of claim 1, wherein the inequality constraints of joint angle, joint angular velocity, and joint moment within the robotic system include:
the inequality constraint normalization is described as an inequality constraint for the velocity layer:
wherein the content of the first and second substances,
then the inequality constraint of the joint moments can be rewritten as:
wherein β > 0, the system R is the base of the end effector and the workpiece0(x0，y0，z0) When the lower contact force is F, the expression derivation of the moment of action it exerts at each joint can be found:
the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):
wherein the content of the first and second substances,
j represents a Jacobian matrix;
representing the first derivative of joint moment constraint, tau representing joint moment constraint, β representing positive control parameter, theta representing the joint angle of the robot;
representing a joint angular velocity of the robot; h represents a real number array;
a first derivative representing a commanded force of the robot;
representing a real number.
8. The cooperative robotic compliance control method of claim 1, wherein the adapting the joint moment function and obtaining a final constraint optimization model comprises:
simplifying the objective function by using the command force F of the robotdInstead of in the base mark system R0(x0，y0，z0) The following contact forces F, then:
if the objective function described in equation (13) is defined in the joint angle layer, the final control quantity is the joint angular velocity
Thus by finding G2For the gradient of θ, an alternative description of it on the velocity layer is obtained:
to JT(θ)FdThe derivation yields:
wherein the content of the first and second substances,
the method comprises the following steps:
let H ═ H1，…，Hn]Then the above formula can describe the number as:
due to the second term in equation (15)
Not correlation, then choose the final objective function to choose as
By introduction ofA correction term
For in the objective function
And (4) carrying out convex processing, wherein the final constraint optimization model is as follows:
wherein the content of the first and second substances,
a rank representing a commanded force of the robot; j represents a Jacobian matrix;
representing a joint angular velocity of the robot; r isrRepresents a reference instruction; a denotes a matrix of abbreviations.
9. The cooperative robotic compliance force control method of claim 1, wherein the updating the state variables and the control moments in the final constrained optimization model based on the dynamic neural network model comprises:
defining input state variables
For dual state variables in the constraint optimization model, the lagrangian function is selected as follows:
according to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constraint optimization model is equivalently expressed as:
wherein, PΩ(. cndot.) is a clipping function defined as:
in solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is:
wherein the content of the first and second substances,
represents a real number;
representing input state quantity lambda2The first derivative of (a);
representing input state quantity lambda1The first derivative of (a);
represents the joint angular velocity;
represents the joint angular acceleration; j represents a Jacobian matrix; r isrRepresents a reference instruction; a represents a shorthand matrix; g1Represents the first element; g2mRepresents the 2 m-th element; e represents a positive constant.
10. A cooperative robotic compliance control system, the system comprising:
an initialization module: the robot state variable acquiring and initializing system is used for acquiring state variables of the robot and initializing the state variables;
an obtaining module: the device comprises a processor, a processor and a controller, wherein the processor is used for obtaining a current rotation matrix based on an initialization state variable;
a reading module: the current state feedback information of the robot is read based on the current rotation matrix;
constructing a module: the system comprises a robot system, a controller and a controller, wherein the robot system is used for establishing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;
and a rewriting module: the joint moment function is rewritten, and a final constraint optimization model is obtained;
an update module: the state variable and the control moment in the final constraint optimization model are updated based on the dynamic neural network model;
a judging module: and the method is used for judging whether the current time is greater than the task time, if so, ending the control of the compliance force of the robot, and otherwise, returning to obtain the current rotation matrix based on the initialized state variable.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911358551.4A CN111037560B (en) | 2019-12-25 | 2019-12-25 | Cooperative robot compliance control method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911358551.4A CN111037560B (en) | 2019-12-25 | 2019-12-25 | Cooperative robot compliance control method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111037560A true CN111037560A (en) | 2020-04-21 |
| CN111037560B CN111037560B (en) | 2021-06-25 |
Family
ID=70240336
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911358551.4A Active CN111037560B (en) | 2019-12-25 | 2019-12-25 | Cooperative robot compliance control method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111037560B (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111941421A (en) * | 2020-06-22 | 2020-11-17 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | Self-adaptive fuzzy force tracking control method based on multi-robot cooperative operation |
| CN112873208A (en) * | 2021-01-29 | 2021-06-01 | 佛山树客智能机器人科技有限公司 | Anti-noise and dynamic constraint robot real-time motion planning method and device |
| CN113084828A (en) * | 2021-04-02 | 2021-07-09 | 上海电气集团股份有限公司 | Motion control method, device, equipment and storage medium |
| CN113146615A (en) * | 2021-01-29 | 2021-07-23 | 佛山树客智能机器人科技有限公司 | Multi-robot cooperative transport control method and device |
| CN113618741A (en) * | 2021-08-24 | 2021-11-09 | 深圳市优必选科技股份有限公司 | Robot joint pose optimization method, robot control method and robot |
| CN113703319A (en) * | 2021-08-26 | 2021-11-26 | 合肥工业大学 | Joint module inequality constraint optimal robust control method based on reinforcement learning |
| CN114260896A (en) * | 2021-12-20 | 2022-04-01 | 广东省科学院智能制造研究所 | Compliance force control method and system for cooperative robot |
| US20220305645A1 (en) * | 2021-03-25 | 2022-09-29 | Mitsubishi Electric Research Laboratories, Inc. | System and Method for Robotic Assembly Based on Adaptive Compliance |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101804627A (en) * | 2010-04-02 | 2010-08-18 | 中山大学 | Redundant manipulator motion planning method |
| CN107378952A (en) * | 2017-08-16 | 2017-11-24 | 华南理工大学 | A kind of solution method that redundancy mechanical arm end effector posture is kept |
| CN107662211A (en) * | 2017-10-16 | 2018-02-06 | 西北工业大学 | A kind of robot for space forecast Control Algorithm based on quanta particle swarm optimization |
| US20180111270A1 (en) * | 2016-10-24 | 2018-04-26 | Seiko Epson Corporation | Control apparatus, robot, and robot system |
| CN108714894A (en) * | 2018-05-03 | 2018-10-30 | 华南理工大学 | A kind of dynamic method for solving dual redundant mechanical arm and colliding with each other |
| DE102018210864B3 (en) * | 2018-07-02 | 2019-07-18 | Kuka Deutschland Gmbh | Method and system for controlling a robot |
| EP3546134A1 (en) * | 2018-03-28 | 2019-10-02 | BAE SYSTEMS plc | Collaborative robot system |
-
2019
- 2019-12-25 CN CN201911358551.4A patent/CN111037560B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101804627A (en) * | 2010-04-02 | 2010-08-18 | 中山大学 | Redundant manipulator motion planning method |
| US20180111270A1 (en) * | 2016-10-24 | 2018-04-26 | Seiko Epson Corporation | Control apparatus, robot, and robot system |
| CN107378952A (en) * | 2017-08-16 | 2017-11-24 | 华南理工大学 | A kind of solution method that redundancy mechanical arm end effector posture is kept |
| CN107662211A (en) * | 2017-10-16 | 2018-02-06 | 西北工业大学 | A kind of robot for space forecast Control Algorithm based on quanta particle swarm optimization |
| EP3546134A1 (en) * | 2018-03-28 | 2019-10-02 | BAE SYSTEMS plc | Collaborative robot system |
| CN108714894A (en) * | 2018-05-03 | 2018-10-30 | 华南理工大学 | A kind of dynamic method for solving dual redundant mechanical arm and colliding with each other |
| DE102018210864B3 (en) * | 2018-07-02 | 2019-07-18 | Kuka Deutschland Gmbh | Method and system for controlling a robot |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111941421A (en) * | 2020-06-22 | 2020-11-17 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | Self-adaptive fuzzy force tracking control method based on multi-robot cooperative operation |
| CN111941421B (en) * | 2020-06-22 | 2022-02-18 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | Self-adaptive fuzzy force tracking control method based on multi-robot cooperative operation |
| CN113146615A (en) * | 2021-01-29 | 2021-07-23 | 佛山树客智能机器人科技有限公司 | Multi-robot cooperative transport control method and device |
| CN112873208A (en) * | 2021-01-29 | 2021-06-01 | 佛山树客智能机器人科技有限公司 | Anti-noise and dynamic constraint robot real-time motion planning method and device |
| US20220305645A1 (en) * | 2021-03-25 | 2022-09-29 | Mitsubishi Electric Research Laboratories, Inc. | System and Method for Robotic Assembly Based on Adaptive Compliance |
| US11673264B2 (en) * | 2021-03-25 | 2023-06-13 | Mitsubishi Electric Research Laboratories, Inc. | System and method for robotic assembly based on adaptive compliance |
| CN113084828A (en) * | 2021-04-02 | 2021-07-09 | 上海电气集团股份有限公司 | Motion control method, device, equipment and storage medium |
| CN113618741A (en) * | 2021-08-24 | 2021-11-09 | 深圳市优必选科技股份有限公司 | Robot joint pose optimization method, robot control method and robot |
| CN113618741B (en) * | 2021-08-24 | 2022-07-29 | 深圳市优必选科技股份有限公司 | Robot joint pose optimization method, robot control method and robot |
| CN113703319A (en) * | 2021-08-26 | 2021-11-26 | 合肥工业大学 | Joint module inequality constraint optimal robust control method based on reinforcement learning |
| CN113703319B (en) * | 2021-08-26 | 2023-07-28 | 合肥工业大学 | Joint module inequality constraint optimal robust control method based on reinforcement learning |
| CN114260896A (en) * | 2021-12-20 | 2022-04-01 | 广东省科学院智能制造研究所 | Compliance force control method and system for cooperative robot |
| CN114260896B (en) * | 2021-12-20 | 2024-01-09 | 广东省科学院智能制造研究所 | Flexible force control method and system for cooperative robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111037560B (en) | 2021-06-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111037560B (en) | Cooperative robot compliance control method and system | |
| CN110039542B (en) | Visual servo tracking control method with speed and direction control function and robot system | |
| Cheah et al. | Adaptive Jacobian vision based control for robots with uncertain depth information | |
| Xu et al. | Dynamic neural networks based kinematic control for redundant manipulators with model uncertainties | |
| Steinmetz et al. | Simultaneous kinesthetic teaching of positional and force requirements for sequential in-contact tasks | |
| WO2007127723A2 (en) | Control of robots from human motion descriptors | |
| Rambow et al. | Autonomous manipulation of deformable objects based on teleoperated demonstrations | |
| Sun et al. | Single master bimanual teleoperation system with efficient regulation | |
| Haviland et al. | A purely-reactive manipulability-maximising motion controller | |
| Mazare et al. | Adaptive variable impedance control for a modular soft robot manipulator in configuration space | |
| Markdahl et al. | Distributed cooperative object attitude manipulation | |
| Zhou et al. | Collision-free compliance control for redundant manipulators: an optimization case | |
| Prats et al. | Vision force control in task-oriented grasping and manipulation | |
| Huang et al. | Toward generalizable robotic dual-arm flipping manipulation | |
| Yu et al. | A haptic shared control algorithm for flexible human assistance to semi-autonomous robots | |
| Hu et al. | Neural learning of stable dynamical systems based on extreme learning machine | |
| CN105467841B (en) | A kind of class nerve control method of humanoid robot upper extremity exercise | |
| Papageorgiou et al. | Learning by demonstration for constrained tasks | |
| Padula et al. | A new pseudoinverse for manipulator collision avoidance | |
| Jia et al. | Tri-criteria optimization motion planning at acceleration-level of dual redundant manipulators | |
| Buss et al. | Advanced telerobotics: Dual-handed and mobile remote manipulation | |
| Cervantes et al. | Vision-based PID control of planar robots | |
| Zhai et al. | Effective learning and online modulation for robotic variable impedance skills | |
| Heyu et al. | Impedance control method with reinforcement learning for dual-arm robot installing slabstone | |
| Xue et al. | Dynamic finger gaits via pivoting and adapting contact forces |
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 | ||
| CP01 | Change in the name or title of a patent holder |
Address after: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong Patentee after: Institute of intelligent manufacturing, Guangdong Academy of Sciences Address before: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong Patentee before: GUANGDONG INSTITUTE OF INTELLIGENT MANUFACTURING |
|
| CP01 | Change in the name or title of a patent holder |