CN117301064B - Double-arm robot safety cooperative control method based on fixed time convergence - Google Patents
Double-arm robot safety cooperative control method based on fixed time convergence Download PDFInfo
- Publication number
- CN117301064B CN117301064B CN202311405159.7A CN202311405159A CN117301064B CN 117301064 B CN117301064 B CN 117301064B CN 202311405159 A CN202311405159 A CN 202311405159A CN 117301064 B CN117301064 B CN 117301064B
- Authority
- CN
- China
- Prior art keywords
- arm
- double
- arm robot
- load
- 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
- 238000000034 method Methods 0.000 title claims abstract description 76
- 230000001360 synchronised effect Effects 0.000 claims abstract description 9
- 238000004458 analytical method Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 30
- 230000008569 process Effects 0.000 claims description 18
- 239000012636 effector Substances 0.000 claims description 11
- 238000013461 design Methods 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000003491 array Methods 0.000 claims description 5
- 230000001939 inductive effect Effects 0.000 claims description 4
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 241001640034 Heteropterys Species 0.000 claims description 2
- 230000009977 dual effect Effects 0.000 claims description 2
- 230000006698 induction Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 11
- 238000004088 simulation Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 238000011217 control strategy Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
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/1674—Programme controls characterised by safety, monitoring, diagnostic
-
- 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
Abstract
The invention discloses a double-arm robot safety cooperative control method based on fixed time convergence, and belongs to the field of double-arm robot control. Firstly, a double-arm robot, a load kinematics model and a dynamics model thereof are established, and then a position controller is designed according to a sliding mode control algorithm which converges based on fixed time according to the dynamics model, so that control moment of each joint of the mechanical arm is obtained; then calculating the internal force in the clamping direction of the tail ends of the left arm and the right arm of the double-arm robot through a dynamic model and load stress analysis; the internal force controller is designed to convert the internal force error into control output force and convert the control output force into control moment of each joint force, so that the safety control of the double-arm robot is realized; and finally, superposing the force and the position control moment and inputting the superposed force and the position control moment into a dynamic model, so that the dual-arm robot realizes synchronous control of the pose and the clamping force. The method can ensure the rapid tracking of the double-arm robot to the expected pose, can realize synchronous control of the clamping force of the double-arm robot, and finally completes the double-arm cooperative carrying task.
Description
Technical Field
The invention relates to the field of control of double-arm robots, in particular to a double-arm robot safety cooperative control method based on fixed time convergence.
Background
With the continuous development of science and technology, the robot technology is a hotspot of current scientific research and technology innovation, and compared with the traditional single mechanical arm, the double-arm robot has the advantages of high flexibility, large load capacity, easy guarantee of precision and the like, is widely applied to the scenes of cooperative transportation, welding, assembly and the like in the fields of industry, aerospace, home service and the like, and simultaneously has application potential in more and more fields along with the continuous progress of electronic technology and computer technology and the continuous reduction of production and manufacturing cost.
Because the double mechanical arms and the operation objects are required to work cooperatively, the double-arm robot control system is more complex than a single mechanical arm: the coordination work of the two arms is not only to consider the kinematic coordination of the tail ends of the two arms, but also to consider the kinematic coordination between the two arms and the target; the kinematic coordination means that the tail ends of the two arms are required to synchronously move and always meet the position and posture constraint relation, so that the tail ends of the mechanical arms keep constant spacing and relative posture to finish the task of carrying the target object; kinetic coordination means that the arm ends must maintain proper contact force during movement in the desired trajectory to meet handling requirements. In the aspect of algorithm control, the track tracking precision of the mechanical arm is ensured by methods such as robust control, self-adaptive control, PID control and the like. However, the error convergence speed and convergence accuracy of such methods are difficult to be ensured, and the safety of the cooperative work cannot be ensured.
Therefore, in the control process of the double-arm robot, on one hand, how to improve the convergence speed of tracking errors, so that the double-arm robot can realize the rapid tracking of the expected pose; on the other hand, how to optimize the synchronous control of the clamping force of the double-arm robot and improve the stability and the accuracy of the double-arm robot is an urgent problem to be solved by the existing double-arm robot algorithm.
In view of this, the present invention has been made.
Disclosure of Invention
The purpose of the invention is that: aiming at the problems and the defects existing in the prior art, the invention aims to provide a double-arm robot safety cooperative control method based on fixed time convergence. The dual-arm robot is considered to carry the scene of the load object, and the translational motion and the rotation are carried out to control the load gesture. Through the mixed control strategy of the pose and the internal force controller, the method can ensure the rapid tracking of the double-arm robot to the expected pose, realize the synchronous control of the clamping force of the double-arm robot and finally finish the double-arm cooperative transportation task.
The technical scheme of the invention is as follows: in order to achieve the above-mentioned purpose, provide a safe cooperative control method of double-arm robot based on fixed time convergence, double-arm robot includes two three-joint mechanical arms of left and right sides, the end effector of the load that the mechanical arm tip can grasp, consider the left and right arms of double-arm robot to translate and rotate and control the load grasped at the same time to realize grasping and carrying operation;
the method comprises the following specific steps:
step 1, establishing a kinematic model of the double-arm robot when the double-arm robot cooperatively carries a load based on a geometric method, and establishing a dynamic model of the double-arm robot by using a Lagrange equation;
step 2, designing a joint position controller based on a sliding mode control algorithm converged in fixed time according to a dynamic model when the double-arm robot cooperatively conveys a load, and obtaining control moment of each joint of the left arm and the right arm so as to realize high-precision track tracking control of the two mechanical arms;
step 3, calculating the clamping internal force of the end effectors of the left arm and the right arm of the double-arm robot through a dynamic model and load stress analysis of the mechanical arm when the double-arm robot cooperatively conveys the load;
step 4, designing an internal force controller to control clamping internal forces of the end effectors of the left arm and the right arm of the double-arm robot in real time so as to avoid damage to a carrying load, converting clamping internal force errors into control output forces of the left arm and the right arm, converting the control output forces into control moment of each joint force of the double-arm robot, avoiding failure of cooperative operation of the left arm and the right arm, and realizing safe control of clamping force of the double-arm robot;
and 5, superposing and inputting the control moment of the internal force controller and the joint position controller into a dynamic model of the double-arm robot by adopting force-position hybrid control, so as to realize synchronous control of the position, the gesture and the clamping force of the double-arm robot.
Further, the method for establishing the kinematic model when the double-arm robot cooperatively conveys the load comprises the following steps:
the centroid relation between the end effectors of the left and right mechanical arms of the double-arm robot and the carried load is determined according to the following steps:
in (x) c ,y c ) And (3) withThe center of mass position coordinates of the load and the gesture of the load are respectively described by angles; record->d is 1/2 of the load length, (x) a ,y a ) And->The position and the gesture of the tail end of the left arm are respectively, and the tail end speed of the left arm of the robot is obtained by deriving the formula (1):
wherein J is o,a The velocity jacobian matrix of the left arm is characterized in that an origin on a character is a variable point and represents the order of a character representative variable relative to time derivative; similarly, the speed of the right arm tail end of the double-arm robot is as follows:
in (x) b ,y b ) And (3) withFor the position and posture of the tail end of the right arm, J o,b And (5) establishing a kinematic model for the right arm tail end velocity jacobian matrix.
Further, the method for establishing the kinetic model of the double-arm robot is as follows:
based on the Lagrangian equation, the kinetic equation for the resulting load is as follows:
wherein M is c ∈R 3×3 Mass inertia matrix representing load carried by robot, C c ∈R 3×3 Representing a coriolis force/centrifugal force matrix,representing the velocity jacobian matrix of a two-arm robot, f= [ F a T ,F b T ] T ∈R 6×1 Representing the acting force of the object to be conveyed on the tail end of the double-arm robot, F ε Representing load dynamics modeling uncertainty term, +.>And->Respectively representing the load speed and the acceleration;
in the case of a two-arm robot gripping a load on the left and right arms, the kinetic equation for the two-arm robot is:
where m=diag ([ M) a ,M b ])∈R 6×6 For a two-arm robot inertial matrix, m=diag (·) represents a diagonal matrix with element·diagonal element, τ w ＝[τ w1 T ,τ w2 T ] T ∈R 6×1 The control moment of the robot joint is represented,represents the angular acceleration of each joint of the left and right mechanical arms, and N= [ N ] 1 T ,N 2 T ] T ∈R 6×1 J=diag [ J ] for the coriolis force/centrifugal force term a ,J b ]∈R 6×6 Jacobian matrix, τ, representing the velocities of the left and right robot ends and the respective joints d ＝[τ d1 T ,τ d2 T ] T ∈R 6×1 Representing an unmodeled term of the double-arm robot and external disturbance;
combining the combined type (2), the formula (3) and the formula (5) to obtain a kinetic equation of the double-arm robot-load:
wherein M is x ＝M c +J o T J -T MJ -1 J o Inertial matrix for a two-arm robot load, d x ＝J o T J -T τ d +F ε As a total disturbance of the two-arm robot-load,n is the coriolis force/centrifugal force term of the two-arm robot-load.
Further, based on a double-arm robot-load dynamics equation, a sliding mode control algorithm is utilized to design a rapid terminal sliding mode control with fixed time convergence, and a joint position controller equation of the double-arm robot for cooperative transportation is obtained: the equations of the position controller for the cooperative conveyance of the two-arm robot are shown in the following formulas (11), (12) and (16):
design fixed time convergence sliding mode surface, will track error e 1 The method comprises the following steps:
e 1 ＝X c -X cd (7)
wherein X is cd Is the expected position and attitude of the load;
to achieve tracking error e 1 The sliding mode surface function has fixed time convergence characteristics, and is designed as follows:
in the method, in the process of the invention,e is a natural constant, wherein a 1 A constant greater than 0 and less than 1, b 1 ＞0、c 1 > 0 and b 1 And c 1 Are all even, diagonal arrays K (e 1 )＝diag([k 1 (e 11 ),k 2 (e 12 ),k 3 (e 13 )]) Medium coefficient Is a self-defined adjustable fixed convergence time, whereinIs greater than 1 odd->An odd number of more than 1 and less than 2, wherein p 1 To p 4 Is odd number, satisfy p 1 ＞p 2 > 0 and 0 < p 3 ＜p 4 Record->And (3) withe 2 The speed tracking error is specifically as follows:
deriving the formula (8), and obtaining:
in the method, in the process of the invention,and K' (e) 1 ) Respectively->And K (e) 1 ) Derivative with respect to time;
to achieve fixed time convergence of the slip form surface, S σ Satisfy the following requirementsWherein k is 3 、k 4 All are normal numbers, 0 < sigma 3 ＜1，σ 4 The control moment of the internal force controller is designed to be tau more than 1 w ＝τ eq +τ es +τ di ，τ eq 、τ es And τ di The method comprises the steps of respectively obtaining an ideal control law, a robust control item and an external disturbance estimation item;
the ideal control law is:
in the method, in the process of the invention,is an intermediate function, specifically:
the robust control term is:
in the method, in the process of the invention, the fixed convergence time is self-defined;
obtainable according to formula (6):
wherein f d ＝-M x -1 ·d x Defining the auxiliary variable V as:
wherein lambda is 1 ＞0、λ 2 ＞0、α 0 > 1; to the disturbance f in the formula (13) d Estimating to improve the position track tracking precision of the carrying load of the double-arm robot, and designing a disturbance observer in the following form:
in the method, in the process of the invention,lambda is the disturbance estimate 3 And lambda' is the normal number, ">Is->An upper bound estimate with an adaptive update rate of +.>δ 0 < 0.01 is a positive constant;
to sum up, the external disturbance estimation term is:
utilize sliding mode surface function to realize that arm orbit tracking error fixed time converges, utilize control moment tau of design w The fixed time convergence of the sliding mode surface is ensured, and finally, the track tracking precision of the double-arm robot when carrying the load is realized to have the characteristic of limited time convergence.
Further, substituting the formulas (2) and (3) into the formula (8) yields a double-arm robot double-arm end clamping force F of:
the internal force and motion-inducing force required to break down the clamped load from the dual-arm terminal force is as follows:
F＝F I +F m (18)
wherein F is I And the internal force required for representing the load of the double arm robot to clamp, F m Representing the motion-induced force required by the dual-arm robot for the clamped load;
the robot double-arm end motion induction force formula obtained according to the formula (18) is:
the clamping internal force expression is obtained as follows:
wherein I is n ∈R 3×3 Representing an identity matrix;
according to the internal force expression (20) and the controlled object model (6), the clamping internal force components to be controlled are as follows:
the left and right arm end gripping internal force can be obtained according to equation (21):
wherein F is Ia And F Ib The clamping internal forces of the tail ends of the left arm and the right arm are respectively.
Further, the actual clamping internal force F of the left and right arm ends of the double-arm robot is utilized Ia And F Ib The internal force error is defined as:
ΔF a ＝F Ia -F Iad (24)
ΔF b ＝F Ib -F Ibd (25)
wherein F is Iad And F Ibd For the desired clamping internal force of the left and right arm ends, the force controller is designed according to the internal force error as follows:
wherein k is p And k I Are all positive gain diagonal arrays, τ a And τ b Control moment of load clamped by the tail ends of the left arm and the right arm is respectively shown and recordedLeft and right for the robotThe force controllers of the two arms obtain control moment.
In step 5, the control torque of the pose controller and the internal force controller required for the left and right arms of the load transported by the double-arm robot obtained in step 2 and step 4 is superimposed to be τ=τ li +τ w And then the design of the pose controller and the internal force controller of the double-arm robot is completed, so that the double-arm robot can realize tracking of the expected pose of the load and synchronous control of the clamping force of the load.
Compared with the prior art, the double-arm robot safety cooperative control method based on fixed time convergence has the beneficial effects that:
(1) In the track tracking aspect, a rapid terminal sliding mode controller with fixed time convergence is adopted; on one hand, the error convergence speed is accelerated by means of the fixed time convergence characteristic of sliding mode control; on the other hand, the method is added with a disturbance observer with newly proposed fixed time convergence characteristic, and the disturbance value can be rapidly estimated under the condition that the disturbance upper bound is unknown, so that the track tracking precision of the double-arm robot during carrying the load is improved.
(2) In the aspect of carrying loads cooperatively by the two arms, the invention adopts a force-position hybrid control strategy for control, thereby ensuring the safety of cooperative control. Compared with the current method for carrying the load by the master arm and the slave arm, the method provided by the invention has the advantages that the clamping force is controlled when the two arms cooperatively move, and the problem of clamping failure caused by overlarge or overlarge clamping force is solved. And the problems of translational motion and rotational motion control in the carrying process of the load are considered, so that the position and the gesture of the object are accurately controlled.
(3) The invention innovates in the aspect of ensuring a sliding mode control time convergence algorithm: when the pose controller is designed, a novel rapid terminal sliding mode surface based on fixed time convergence is provided, the fixed time convergence characteristic of the novel rapid terminal sliding mode surface is guaranteed, the convergence speed and the precision of the novel rapid terminal sliding mode surface are improved through robust control item design, and a novel disturbance observer is designed to accurately estimate and compensate input disturbance caused by uncertain factors outside a system. The method ensures the stability of the system and simultaneously remarkably improves the convergence speed and the accuracy of the system error.
Drawings
FIG. 1 is a flow chart of a dual-arm robot safety cooperative control method based on fixed time convergence;
FIG. 2 is a schematic diagram of a dual-arm robot and its load model used in the present invention;
FIG. 3 is a schematic diagram of a kinematic model of a two-arm robotic arm used in the present invention;
FIG. 4 is a block diagram of a dual-arm robot safety cooperative control method based on fixed time convergence of the present invention;
FIG. 5 is a schematic diagram of a simulation of position trace tracking in an embodiment of the present invention;
FIG. 6 is a schematic diagram illustrating a simulation of a clamping force of a mechanical arm according to an embodiment of the present invention;
FIG. 7 is a schematic diagram of external disturbance tracking along the X direction of a dual-arm robot in an embodiment of the present invention;
FIG. 8 is a schematic diagram of external disturbance tracking along the Y-direction for a dual-arm robot in an embodiment of the present invention;
fig. 9 is a schematic diagram illustrating external disturbance tracking along a rotation direction of a dual-arm robot according to an embodiment of the present invention.
Detailed Description
An embodiment of the present invention will be explained in more detail with reference to fig. 1.
As shown in fig. 1, 2 and 4, the dual-arm robot safety cooperative control method based on fixed time convergence in the present embodiment mainly includes the following steps:
and step 1, establishing a kinematic model when the double-arm robot cooperatively conveys the load, and establishing a dynamic model by using a Lagrange equation. The double-arm robot comprises a left mechanical arm and a right mechanical arm, wherein the end part of the mechanical arm is provided with an end effector capable of clamping an object;
step 2, designing a position controller based on a sliding mode control algorithm converged in fixed time according to a dynamics model to obtain joint control moments of the left mechanical arm and the right mechanical arm;
step 3, calculating the clamping internal force of the tail ends of the left arm and the right arm of the double-arm robot through a dynamic model and load stress analysis;
step 4, designing an internal force controller to control the clamping force in real time so as to avoid damaging an object, converting an internal force error into a control output force, and converting the control output force into each joint force control moment to realize the safety control of the double-arm robot;
and 5, superposing and inputting force and position control moment into a dynamic model by adopting force-position hybrid control, so as to realize synchronous control of the position, the gesture and the clamping force of the double-arm robot.
The following describes embodiments of the invention, examples of which are illustrated in the accompanying drawings and reference to the embodiments are illustrative and are intended to be in no way limiting of the invention.
The invention relates to a double-arm robot safety cooperative control method based on fixed time convergence, which is characterized in that in the step 1, the mechanical arm load is a cuboid which has uniform mass and the mass center is at the geometric center, and the mass is m c Length 2d, height h, centroid coordinates (x c ,y c ) The rotation angle isRecord->According to the geometrical relationship between the mechanical arm and the load, the pose of the tail end of the left mechanical arm can be obtained as follows:
the dual-arm robot designed by the invention considers the translational and rotational directions, and analyzes and considers the translational speed of the load along the X axis and the Y axis and the angular speed around the Z axis. Deriving the formula (1), and rewriting it into a matrix form as follows:
wherein J is o,a Is the velocity jacobian matrix of the left arm. The relationship between the end of the right mechanical arm and the load speed is as follows:
in the method, in the process of the invention,and->Representing the speed of translation of the right arm end in the X-axis and Y-axis, < >>The rotational angular velocity at the time of rotation is expressed, and can be obtained:
the double-arm robot designed by the invention considers the translational and rotational directions at the same time, and for simplifying the algorithm design process, a planar double-arm robot is taken as an example for explanation, and the invention is also applicable to double-arm robots with space motion modes.
The dynamics modeling of an unknown load object grasped by a double-arm robot is as follows:
in the method, in the process of the invention,an inertial matrix representing the load,and C c ∈R 3×3 ,J c Represents moment of inertia, m c Representing the mass of the load that is not known,representing the velocity jacobian matrix of a two-arm robot, f= [ F a T ,F b T ] T ∈R 6×1 Representing force of end effector of double-arm robot, F ε Modeling uncertainty term representing unknown load dynamics, +.>And->Representing the load speed and acceleration, respectively.
Under the condition that the left arm and the right arm of the double-arm robot hold the load, the kinetic equation of the double-arm robot can be obtained as follows:
in the formula, m=diag [ M ] a ,M b ]∈R 6×6 For a dual arm robot mass inertia matrix, m=diag (·) represents a diagonal matrix with element·diagonal element, τ w ＝[τ w1 T ,τ w2 T ] T ∈R 6×1 The control moment of the robot joint is represented,representing the angular acceleration of each joint, n= [ N ] 1 T ,N 2 T ] T ∈R 6×1 For the coriolis force and centrifugal force terms of the robot's arms, j=diag [ J ] a ,J b ]∈R 6×6 Jacobian matrix, τ, representing the velocities of the various joints of a dual-arm end effector and a dual-arm robot d ＝[τ d1 T ,τ d2 T ] T ∈R 6×1 Representing the unmodeled terms of the two-arm robot and external disturbances.
The combination formula (5) and the formula (6) can be obtained through transformation:
from the model illustration of the two-arm robot and its load depicted in fig. 2 and the two-arm robot arm kinematics model depicted in fig. 3, the velocity relationship between the left arm end velocity and the load can be deduced as follows:
in the formula, the anticlockwise angle is set to be positive, and the left arm theta 11 、θ 12 And theta 13 Respectively represent the absolute values of the angles of the three joints relative to the horizontal plane, s 1 、s 12 、s 123 Respectively represent sin (theta) 11 )、sin(θ 11 +θ 12 )、sin(θ 11 +θ 12 +θ 13 )，c 1 、c 12 、c 123 Respectively represent cos (theta) 11 )、cos(θ 11 +θ 12 )、cos(θ 11 +θ 12 +θ 13 ). Combining formula (8) with (2) can result in:
similarly, the velocity expression of the right arm tip with respect to each joint is as follows:
in the formula, the anticlockwise angle is set to be positive, theta 21 、θ 22 And theta 23 Respectively represent the angles of the three joints of the right arm relative to the horizontal plane, S 1 、S 12 、S 123 Respectively represent sin (theta) 21 )、sin(θ 21 +θ 22 )、sin(θ 21 +θ 22 +θ 23 )，C 1 、C 12 、C 123 Respectively represent cos (theta) 21 )、cos(θ 21 +θ 22 )、cos(θ 21 +θ 22 +θ 23 )。
The formula (10) and the formula (3) are combined to obtain
By combining the expression (9) and the expression (11), the speed relationship between the load of the double arm robot and the angles of the joints can be obtained as follows:
deriving the above, the angular acceleration of each joint can be obtained as follows:
bringing equations (12) and (13) to equation (7), combining the homomorphism terms and sorting the available two-arm robot-load dynamics equations are as follows:
wherein M is x ＝M c +J o T J -T MJ -1 J o Is a matrix of inertia which is a matrix of inertia,is the coriolis force/centrifugal force term, d x ＝J o T J -T τ d +F ε Is a model uncertainty term and external environmental disturbance, i.e. total disturbance.
The embodiment of the method for controlling safety coordination of the double-arm robot based on fixed time convergence is characterized in that in the step 2, a fast terminal sliding mode controller with fixed time convergence is designed on the basis of a double-arm robot-load dynamics equation, and a position controller for coordinated conveying of the double-arm robot is obtained.
As shown in fig. 4, a fixed time converging sliding surface is designed, defining the tracking error as:
e 1 ＝X c -X cd (15)
wherein X is cd Is the desired position and attitude of the load. To achieve tracking error e 1 Having a fixed time convergence characteristic, the sliding mode surface function is designed as follows:
in the method, in the process of the invention,wherein a is 1 A constant greater than 0 and less than 1, b 1 ＞0、c 1 > 0 and b 1 And c 1 Are all even, K (e 1 )＝diag([k 1 (e 11 ),k 2 (e 12 ),k 3 (e 13 )]) Coefficient of e is a natural constant, and is a natural constant, for a custom adjustable fixed convergence time,is greater than 1 odd->An odd number of more than 1 and less than 2, wherein p 1 ＞p 2 ＞0，0＜p 3 ＜p 4 And is odd.
Deriving a tracking error formula (15), wherein the available speed tracking error is as follows:
deriving the formula (16) to obtain:
in the method, in the process of the invention,and K' (e) 1 ) Respectively->And K (e) 1 ) Derivative with respect to time.
To ensure the convergence of the fixed time, S is made σ Satisfy the following requirementsWherein k is 3 、k 4 All are normal numbers, 0 < sigma 3 ＜1，σ 4 Design controller as τ > 1 w ＝τ eq +τ es +τ di ，τ eq 、τ es And τ di The ideal control law, the robust control term and the external disturbance estimation term are respectively adopted.
The ideal control law is:
in the middle of，Is defined as
The robust control term is:
in the method, in the process of the invention, is a self-defined adjustable fixed convergence time.
Obtainable according to formula (14):
wherein f d ＝-M x -1 ·d x Is the total disturbance.
Wherein lambda is 1 ＞0、λ 2 ＞0、α 0 > 1. In order to estimate disturbance to improve track tracking accuracy, a disturbance observer is designed as follows:
in the method, in the process of the invention,lambda is an estimate of the disturbance to the error 3 ＞0、λ'＞0，/>For->An upper bound estimate with an adaptive update rate of +.>δ 0 < 0.01 is a positive constant.
To sum up, the disturbance estimation terms are:
the embodiment of the dual-arm robot safety cooperative control method based on fixed time convergence is characterized in that in step 3, the formula (13) obtained in a dynamics model is substituted into the formula (6), and the dual-arm terminal acting force can be obtained as follows:
the force at the end of the arms can be resolved into the internal force and motion-inducing force required to hold the object as follows:
F＝F I +F m (26)
the motion-inducing force formula obtainable according to equation (18) is:
substituting equation (27) into equation (26) yields the clamping force expression as shown in fig. 6:
wherein I is n ∈R 3×3 Representing the identity matrix.
Combining the clamping force expression with the controlled object model, the internal force component can be obtained as follows:
according to formula (29) and the kinematic model combination analysis, the clamping force can be obtained as follows:
the embodiment describes a dual-arm robot safety cooperative control method based on fixed time convergence, which is characterized in that in step 4, the internal force F in the clamping direction obtained in step 3 Ia And F Ib The tracking force error is defined as:
ΔF a ＝F Ia -F Iad (32)
ΔF b ＝F Ib -F Ibd (33)
wherein F is Iad And F Ibd Is the desired clamping internal force of the left and right arm ends. Converting the internal force error into a control torque in the force controller as shown in the following equation:
wherein k is p And k I Are all positive gain diagonal arrays, τ a And τ b And represents the left and right arm end clampsControl moment of holding object, recordThe control moment is obtained for the force controller of the double arm robot.
The embodiment describes a dual-arm robot safety cooperative control method based on fixed time convergence, which is characterized in that in step 5, the position control moment and the force control moment required by the dual-arm robot transportation obtained in step 2 and step 4 are superimposed to be τ=τ li +τ w . The load position and posture tracking method is input into a dynamic model, so that the double-arm robot can track the expected position and posture of the load and synchronously control the clamping force of the load.
Load mass in space of m c =0.05 kg, d=0.05 m, and the double arm robot is in contact with the load object as shown in fig. 2. Test experiment: and (3) carrying out simulation experiments on the double-arm robot safety cooperative control method based on fixed time convergence.
Fig. 2 is a diagram of a two-arm robot and its load model. Fig. 3 is a diagram of a kinematic model of a two-arm robot arm. FIG. 4 is a block diagram of a two-arm robot cooperative transport capacity bit hybrid control algorithm based on a limited time double-loop sliding mode, which realizes the force bit hybrid control by constructing a double-loop sliding mode based position controller and a force controller. Fig. 5 is a simulation result of the position trajectory tracking control of the inventive algorithm. FIG. 6 is a force control simulation result of the inventive algorithm with a simulation time of 1s. Fig. 7-9 illustrate the precise estimation of the disturbance in the translational and rotational directions by the dual-arm robot according to the present invention.
While embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative, and not restrictive, and that all changes, modifications, substitutions, and alterations may be made by those having ordinary skill in the art without departing from the spirit and principles of the present invention. The scope of the invention is defined by the appended claims and equivalents.
Claims (4)
1. The double-arm robot safety cooperative control method based on fixed time convergence is characterized in that the double-arm robot comprises a left mechanical arm and a right mechanical arm, wherein the end part of the mechanical arm is provided with an end effector of a load capable of being clamped, and the left arm and the right arm of the double-arm robot are considered to carry out translational and rotational control on the clamped load to realize clamping and carrying operation;
the method comprises the following specific steps:
step 1, establishing a kinematic model of the double-arm robot when the double-arm robot cooperatively carries a load based on a geometric method, and establishing a dynamic model of the double-arm robot by using a Lagrange equation;
step 2, designing a joint position controller based on a sliding mode control algorithm converged in fixed time according to a dynamic model when the double-arm robot cooperatively conveys a load, and obtaining control moment of each joint of the left arm and the right arm so as to realize high-precision track tracking control of the two mechanical arms;
step 3, calculating the clamping internal force of the end effectors of the left arm and the right arm of the double-arm robot through a dynamic model and load stress analysis of the mechanical arm when the double-arm robot cooperatively conveys the load;
step 4, designing an internal force controller to control clamping internal forces of the end effectors of the left arm and the right arm of the double-arm robot in real time so as to avoid damage to a carrying load, converting clamping internal force errors into control output forces of the left arm and the right arm, converting the control output forces into control moment of each joint force of the double-arm robot, avoiding failure of cooperative operation of the left arm and the right arm, and realizing safe control of clamping force of the double-arm robot;
step 5, adopting force-position hybrid control, and superposing and inputting control moments of an internal force controller and a joint position controller into a dynamic model of the double-arm robot to realize synchronous control of the position, the gesture and the clamping force of the double-arm robot;
the method for establishing the kinematic model when the double-arm robot cooperatively conveys the load comprises the following steps:
the centroid relation between the end effectors of the left and right mechanical arms of the double-arm robot and the carried load is determined according to the following steps:
in (x) c ,y c ) And (3) withThe center of mass position coordinates of the load and the gesture of the load are respectively described by angles; recording deviced is 1/2 of the load length, (x) a ,y a ) And->The position and the gesture of the tail end of the left arm are respectively, and the tail end speed of the left arm of the robot is obtained by deriving the formula (1):
wherein J is o,a The velocity jacobian matrix of the left arm is characterized in that an origin on a character is a variable point and represents the order of a character representative variable relative to time derivative; similarly, the speed of the right arm tail end of the double-arm robot is as follows:
in (x) b ,y b ) And (3) withFor the position and posture of the tail end of the right arm, J o,b The method comprises the steps of establishing a kinematic model for a right arm tail end velocity jacobian matrix;
the method for establishing the dynamic model of the double-arm robot is as follows:
based on the Lagrangian equation, the kinetic equation for the resulting load is as follows:
wherein M is c ∈R 3×3 Mass inertia matrix representing load carried by robot, C c ∈R 3×3 Representing a coriolis force/centrifugal force matrix,representing the velocity jacobian matrix of a two-arm robot, f= [ F a T ,F b T ] T ∈R 6×1 Representing the acting force of the object to be conveyed on the tail end of the double-arm robot, F ε Representing load dynamics modeling uncertainty term, +.>And->Respectively representing the load speed and the acceleration;
in the case of a two-arm robot gripping a load on the left and right arms, the kinetic equation for the two-arm robot is:
where m=diag ([ M) a ,M b ])∈R 6×6 For a two-arm robot inertial matrix, m=diag (·) represents a diagonal matrix with element·diagonal element, τ w ＝[τ w1 T ,τ w2 T ] T ∈R 6×1 The control moment of the robot joint is represented,represents the angular acceleration of each joint of the left and right mechanical arms, and N= [ N ] 1 T ,N 2 T ] T ∈R 6×1 Is Ke' sForce/centrifugal force term, j=diag [ J ] a ,J b ]∈R 6×6 Jacobian matrix, τ, representing the velocities of the left and right robot ends and the respective joints d ＝[τ d1 T ,τ d2 T ] T ∈R 6×1 Representing an unmodeled term of the double-arm robot and external disturbance;
combining the combined type (2), the formula (3) and the formula (5) to obtain a kinetic equation of the double-arm robot-load:
wherein M is x ＝M c +J o T J -T MJ -1 J o Inertial matrix for a two-arm robot load, d x ＝J o T J -T τ d +F ε As a total disturbance of the two-arm robot-load,coriolis force/centrifugal force terms for a dual arm robot-load;
based on a double-arm robot-load dynamics equation, a sliding mode control algorithm is utilized to design a rapid terminal sliding mode control with fixed time convergence, and a joint position controller equation of the double-arm robot for cooperative transportation is obtained: the equations of the position controller for the cooperative conveyance of the two-arm robot are shown in the following formulas (11), (12) and (16):
design fixed time convergence sliding mode surface, will track error e 1 The method comprises the following steps:
e 1 ＝X c -X cd (7)
wherein X is cd Is the expected position and attitude of the load;
to achieve tracking error e 1 The sliding mode surface function has fixed time convergence characteristics, and is designed as follows:
in the method, in the process of the invention,e is a natural constant, wherein a 1 A constant greater than 0 and less than 1, b 1 ＞0、c 1 > 0 and b 1 And c 1 Are all even, diagonal arrays K (e 1 )＝diag([k 1 (e 11 ),k 2 (e 12 ),k 3 (e 13 )]) Medium coefficient Is a self-defined adjustable fixed convergence time, whereinIs greater than 1 odd->An odd number of more than 1 and less than 2, wherein p 1 To p 4 Is odd number, satisfy p 1 ＞p 2 > 0 and 0 < p 3 ＜p 4 Record->And (3) withe 2 The speed tracking error is specifically as follows:
deriving the formula (8), and obtaining:
in the method, in the process of the invention,and K' (e) 1 ) Respectively->And K (e) 1 ) Derivative with respect to time;
to achieve fixed time convergence of the slip form surface, S σ Satisfy the following requirementsWherein k is 3 、k 4 All are normal numbers, 0 < sigma 3 ＜1，σ 4 The control moment of the internal force controller is designed to be tau more than 1 w ＝τ eq +τ es +τ di ，τ eq 、τ es And τ di The method comprises the steps of respectively obtaining an ideal control law, a robust control item and an external disturbance estimation item;
the ideal control law is:
in the method, in the process of the invention,is an intermediate function, specifically:
the robust control term is:
in the method, in the process of the invention, the fixed convergence time is self-defined; obtainable according to formula (6):
wherein f d ＝-M x -1 ·d x Defining the auxiliary variable V as:
wherein lambda is 1 ＞0、λ 2 ＞0、α 0 > 1; to the disturbance f in the formula (13) d Estimating to improve the position track tracking precision of the carrying load of the double-arm robot, and designing a disturbance observer in the following form:
in the method, in the process of the invention,lambda is the disturbance estimate 3 And lambda' is the normal number, ">Is->An upper bound estimate with an adaptive update rate ofδ 0 < 0.01 is a positive constant;
to sum up, the external disturbance estimation term is:
utilize sliding mode surface function to realize that arm orbit tracking error fixed time converges, utilize control moment tau of design w The fixed time convergence of the sliding mode surface is ensured, and finally, the track tracking precision of the double-arm robot when carrying the load is realized to have the characteristic of limited time convergence.
2. The method for controlling safety coordination of a double-arm robot based on fixed time convergence according to claim 1, wherein the double-arm robot double-arm end clamping force F obtained by substituting the formula (2) and the formula (3) into the formula (8) is:
the internal force and motion-inducing force required to break down the clamped load from the dual-arm terminal force is as follows:
F＝F I +F m (18)
wherein F is I And the internal force required for representing the load of the double arm robot to clamp, F m Representing the motion-induced force required by the dual-arm robot for the clamped load;
the robot double-arm end motion induction force formula obtained according to the formula (18) is:
the clamping internal force expression is obtained as follows:
wherein I is n ∈R 3×3 Representing an identity matrix;
according to the internal force expression (20) and the controlled object model (6), the clamping internal force components to be controlled are as follows:
the left and right arm end gripping internal force can be obtained according to equation (21):
wherein F is Ia And F Ib The clamping internal forces of the tail ends of the left arm and the right arm are respectively.
3. The method for controlling safety coordination of a double-arm robot based on convergence of a fixed time according to claim 2, wherein the actual clamping internal force F of the left and right arm ends of the double-arm robot is utilized Ia And F Ib The internal force error is defined as:
ΔF a ＝F Ia -F Iad (24)
ΔF b ＝F Ib -F Ibd (25)
wherein F is Iad And F Ibd For the desired clamping internal force of the left and right arm ends, the force controller is designed according to the internal force error as follows:
wherein k is p And k I Are all positive gain diagonal arrays, τ a And τ b Control moment of load clamped by the tail ends of the left arm and the right arm is respectively shown and recordedThe moment is controlled by the force controllers of the left arm and the right arm of the robot.
4. The method for controlling safety cooperation of a double-arm robot based on fixed time convergence according to claim 3, wherein in step 5, the control moment of the pose controller and the internal force controller required by the left and right arms by the carrying load of the double-arm robot obtained in step 2 and step 4 is superimposed to be τ=τ li +τ w And then the design of the pose controller and the internal force controller of the double-arm robot is completed, so that the double-arm robot can realize tracking of the expected pose of the load and synchronous control of the clamping force of the load.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311405159.7A CN117301064B (en) | 2023-10-27 | 2023-10-27 | Double-arm robot safety cooperative control method based on fixed time convergence |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311405159.7A CN117301064B (en) | 2023-10-27 | 2023-10-27 | Double-arm robot safety cooperative control method based on fixed time convergence |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117301064A CN117301064A (en) | 2023-12-29 |
| CN117301064B true CN117301064B (en) | 2024-04-12 |
Family
ID=89286482
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311405159.7A Active CN117301064B (en) | 2023-10-27 | 2023-10-27 | Double-arm robot safety cooperative control method based on fixed time convergence |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117301064B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002018752A (en) * | 2000-07-10 | 2002-01-22 | Japan Science & Technology Corp | Method for cooperative control of robot |
| WO2018023201A1 (en) * | 2016-08-03 | 2018-02-08 | 孟强 | Adaptive terminal sliding mode 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 |
| CN110842911A (en) * | 2019-09-29 | 2020-02-28 | 哈尔滨工程大学 | Flexible mechanical arm combined modeling and sliding mode control method considering joint motor characteristics |
| CN112947293A (en) * | 2021-02-22 | 2021-06-11 | 中国矿业大学 | Sliding mode-based mechanical arm safety track tracking control method |
| CN115625711A (en) * | 2022-11-08 | 2023-01-20 | 沈阳工业大学 | Double-arm robot cooperative control method considering tail end force |
| CN115981162A (en) * | 2023-01-20 | 2023-04-18 | 青岛大学 | Sliding mode control trajectory tracking method of robot system based on novel disturbance observer |
-
2023
- 2023-10-27 CN CN202311405159.7A patent/CN117301064B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002018752A (en) * | 2000-07-10 | 2002-01-22 | Japan Science & Technology Corp | Method for cooperative control of robot |
| WO2018023201A1 (en) * | 2016-08-03 | 2018-02-08 | 孟强 | Adaptive terminal sliding mode 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 |
| CN110842911A (en) * | 2019-09-29 | 2020-02-28 | 哈尔滨工程大学 | Flexible mechanical arm combined modeling and sliding mode control method considering joint motor characteristics |
| CN112947293A (en) * | 2021-02-22 | 2021-06-11 | 中国矿业大学 | Sliding mode-based mechanical arm safety track tracking control method |
| CN115625711A (en) * | 2022-11-08 | 2023-01-20 | 沈阳工业大学 | Double-arm robot cooperative control method considering tail end force |
| CN115981162A (en) * | 2023-01-20 | 2023-04-18 | 青岛大学 | Sliding mode control trajectory tracking method of robot system based on novel disturbance observer |
Non-Patent Citations (4)
| Title |
|---|
| 仿人机器人协调阻抗控制算法;齐春丽;伍锡如;;桂林电子科技大学学报;20191025(第05期);44-49 * |
| 具有外部扰动的空间刚性机械臂姿态与末端爪手协调运动的Terminal滑模控制算法设计;郭益深;陈力;;应用数学和力学;20080515(第05期);525-532 * |
| 漂浮基闭链双臂空间机器人抓持系统动力学及载荷力/位置混合控制的模糊变结构控制器设计;陈志煌;陈力;;空间科学学报;20110315(第02期);260-269 * |
| 陈志煌 ; 陈力 ; .漂浮基闭链双臂空间机器人抓持系统动力学及载荷力/位置混合控制的模糊变结构控制器设计.空间科学学报.2011,(第02期),260-269. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117301064A (en) | 2023-12-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110421547B (en) | Double-arm robot cooperative impedance control method based on estimation dynamics model | |
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| CN112372630B (en) | Multi-mechanical-arm cooperative polishing force compliance control method and system | |
| CN106475999B (en) | The acceleration control method of Dual-Arm Coordination based on impedance model under hard conditions | |
| Erhart et al. | An impedance-based control architecture for multi-robot cooperative dual-arm mobile manipulation | |
| CN109782601B (en) | Design method of self-adaptive neural network synchronous robust controller of coordinated mechanical arm | |
| CN106313044A (en) | Feedforward torque compensating method used for industrial robot | |
| CN109623812B (en) | Mechanical arm trajectory planning method considering spacecraft body attitude motion | |
| CN110053044B (en) | Model-free self-adaptive smooth sliding mode impedance control method for clamping serial fruits by parallel robot | |
| CN110900604B (en) | Dynamic model design method based on double-mechanical-arm cooperation polishing system | |
| CN110561440A (en) | multi-objective planning method for acceleration layer of redundant manipulator | |
| CN115625711B (en) | Double-arm robot cooperative control method considering tail end force | |
| CN112809666B (en) | 5-DOF mechanical arm strength position tracking algorithm based on neural network | |
| CN111813140A (en) | High-precision trajectory tracking control method for quad-rotor unmanned aerial vehicle | |
| Li et al. | Model-free impedance control for safe human-robot interaction | |
| CN114942593A (en) | Mechanical arm self-adaptive sliding mode control method based on disturbance observer compensation | |
| CN108445778B (en) | Dynamics modeling method for space non-cooperative target non-complete constraint assembly | |
| CN117301064B (en) | Double-arm robot safety cooperative control method based on fixed time convergence | |
| CN107436605B (en) | Underwater robot control method considering multi-body coupling | |
| CN115890735B (en) | Mechanical arm system, mechanical arm, control method of mechanical arm system, controller and storage medium | |
| CN112975960B (en) | Method for establishing robot tail end fine control model and calibrating parameters | |
| CN111975770B (en) | Spinning target capture method adopting space double-arm robot | |
| CN115476356A (en) | Self-adaptive hybrid impedance control method of space manipulator without force sensor | |
| Aladele et al. | Collision reaction through internal stress loading in cooperative manipulation | |
| CN110480641B (en) | Recursive distributed rapid convergence robust control method for mechanical arm |
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 |