CN113799134B - Robot control method, device, robot and readable storage medium - Google Patents
Robot control method, device, robot and readable storage medium Download PDFInfo
- Publication number
- CN113799134B CN113799134B CN202111137896.4A CN202111137896A CN113799134B CN 113799134 B CN113799134 B CN 113799134B CN 202111137896 A CN202111137896 A CN 202111137896A CN 113799134 B CN113799134 B CN 113799134B
- Authority
- CN
- China
- Prior art keywords
- robot
- force
- load
- external force
- current
- 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 52
- 238000003860 storage Methods 0.000 title claims abstract description 11
- 238000012937 correction Methods 0.000 claims abstract description 78
- 230000005484 gravity Effects 0.000 claims abstract description 78
- 230000001133 acceleration Effects 0.000 claims abstract description 32
- 230000008569 process Effects 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 7
- 239000011159 matrix material Substances 0.000 description 13
- 238000010586 diagram Methods 0.000 description 8
- 238000000354 decomposition reaction Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 101100502727 Neurospora crassa (strain ATCC 24698 / 74-OR23-1A / CBS 708.71 / DSM 1257 / FGSC 987) fex-1 gene Proteins 0.000 description 2
- 238000009776 industrial production Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000017105 transposition Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000002153 concerted effect Effects 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
- B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
- B25J13/085—Force or torque sensors
-
- 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/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- 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/1612—Programme controls characterised by the hand, wrist, grip 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/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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Human Computer Interaction (AREA)
- Manipulator (AREA)
Abstract
The application discloses a robot control method, a device, a robot and a readable storage medium. When a robot clamps a load, determining a current actual internal force and a current actual external force according to actual generalized forces obtained by force sensors at the tail ends of all mechanical arms of the robot, compensating the current actual external force by using the gravity of the load, a centroid parameter of the load, a current position of each tail end of each mechanical arm of the robot and a preset reference position of each tail end of each mechanical arm of the robot to obtain a current correction external force, and controlling all mechanical arms of the robot to cooperatively move according to the current correction external force, the current actual internal force, a preset expected clamping force and a preset reference position of each tail end of each mechanical arm of the robot under the condition that the reference acceleration of the robot for clamping the load is zero. The current actual external force is compensated so as to avoid the influence of the gravity and the gravity moment of the load on the posture of the load clamped by the robot.
Description
Technical Field
The invention relates to the field of robots, in particular to a robot control method, a robot control device, a robot and a readable storage medium.
Background
With the continuous improvement of the industrial automation level, the application field of the robot is developing in a deeper and wider direction. The intellectualization of the robot is very important in industrial production. In the field of industrial production, a two-arm robot can accomplish more complicated operation tasks than a single-arm robot. The control of the two-arm robot is more complicated than that of the single-arm robot because of having two arms, and the control system of the two-arm robot needs to control the left and right arms of the two-arm robot in a coordinated manner.
The control of current double-armed robot to both arms, owing to by the influence of centre gripping load's gravity, during both arms centre gripping load, even if do not receive exogenic action, also can decompose external force, lead to robot centre gripping load to follow the external force that load gravity decomposed out and move, and then lead to when robot centre gripping load, owing to receive the influence of load gravity, can produce great positional deviation when leading to robot centre gripping load to do the follow-up movement.
Disclosure of Invention
In view of the above problems, the present application provides a robot control method, apparatus, robot, and readable storage medium.
The application provides a robot control method, which comprises the following steps:
When the robot clamps a load, determining a current actual internal force and a current actual external force according to actual generalized forces obtained by force sensors at the tail ends of all mechanical arms of the robot, wherein the current actual internal force is used for clamping the load, and the current actual external force is used for controlling all mechanical arms of the robot to enable the position of the load to be compliant;
compensating the current actual external force by using the gravity of the load, the centroid parameter of the load, the current position of the tail end of each mechanical arm of the robot and a preset reference position of the tail end of each mechanical arm of the robot to obtain a current correction external force;
and under the condition that the reference acceleration of the robot for clamping the load motion is zero, controlling each mechanical arm of the robot to cooperatively move according to the current correction external force, the current actual internal force, the preset expected clamping force and the preset reference position.
The robot control method according to the present application, determining a current actual internal force and a current actual external force according to an actual generalized force obtained by a force sensor at each end of a robot arm, includes:
acquiring each actual generalized force obtained by a force sensor at the tail end of each mechanical arm under a preset coordinate system;
Obtaining each vector from the central point of the contact surface between the tail end of each mechanical arm and the load to the center of mass of the load;
and determining the current actual internal force and the current actual external force by using the actual generalized forces and the vectors.
The robot control method according to the present application, before compensating the current actual external force by using the gravity of the load, the centroid parameter of the load, the current position of each end of the robot arm, and the predetermined reference position of each end of the robot arm to obtain the current correction external force, includes:
determining whether an absolute value of a difference between the current actual internal force and a predetermined desired clamping force is less than a preset threshold;
if the clamping force is smaller than the preset threshold value, controlling the robot to clamp the load and move a preset distance in a direction opposite to the gravity direction of the load;
after moving the preset distance, determining whether the absolute value of the difference between the current actual internal force and the preset expected clamping force is smaller than a preset threshold value;
if the load is smaller than the preset threshold value, determining the gravity of the load according to the force in the direction of the gravity of the load in the actual generalized forces at the tail ends of the mechanical arms, and determining the distance between the center of mass of the load and the tail end of any mechanical arm of the robot according to the gravity of the load and the moment in the direction of the gravity of the load in the actual generalized forces at the tail ends of the mechanical arms;
And if the difference is larger than or equal to the preset threshold value, adjusting the position of each mechanical arm tail end of the robot until the absolute value of the difference between the current actual internal force and the expected clamping force is smaller than the preset threshold value.
The robot control method according to the present application, compensating the current actual external force using the gravity of the load, the centroid parameter of the load, the current position of each end of the robot arm, and the predetermined reference position of each end of the robot arm to obtain the current correction external force, includes:
determining a first rotation angle of the J-th mechanical arm tail end of the robot rotating around a first coordinate axis of a J-th mechanical arm tail end coordinate system in the process from the corresponding current position to the corresponding preset reference position, wherein J is more than or equal to 1 and less than or equal to J, and J is the total number of the mechanical arms of the robot;
determining a second rotation angle of the j-th mechanical arm tail end around a second coordinate axis of a j-th mechanical arm tail end coordinate system in the process that the j-th mechanical arm tail end is from the corresponding current position to the corresponding preset reference position, wherein the first coordinate axis, the second coordinate axis and a coordinate axis in the gravity direction are mutually vertical;
Compensating the current external force corresponding to the terminal of the jth mechanical arm in the current actual external force according to the gravity of the load, the centroid parameter of the load, the first rotation angle corresponding to the jth mechanical arm and the second rotation angle corresponding to the jth mechanical arm to obtain a corrected external force corresponding to the terminal of the jth mechanical arm;
and determining the current correction external force by using the correction external forces corresponding to the tail ends of all the mechanical arms.
The robot control method according to the present application, wherein the controlling of the cooperative movement of the respective robot arms of the robot according to the current corrected external force, the current actual internal force, the predetermined desired gripping force, and the predetermined reference position of the end of each robot arm of the robot, includes:
determining a first external force adjustment amount of each mechanical arm tail end according to the current correction external force;
determining a first internal force adjustment amount of each mechanical arm tail end according to the current actual internal force and the expected clamping force;
and correspondingly controlling the mechanical arms of the robot according to the first external force adjustment amount, the first internal force adjustment amount and the preset reference position of the tail end of each mechanical arm.
The robot control method of the present application further includes:
Under the condition that the reference acceleration of the robot for clamping the load is not zero, determining an accelerated motion compensation amount by using the gravity of the load and the reference acceleration;
respectively compensating the current correction external force, the current actual internal force and the expected clamping force by using the accelerated motion compensation amount;
and controlling the cooperative motion of each mechanical arm of the robot by using the current corrected external force compensated by the accelerated motion compensation amount, the current actual internal force compensated by the accelerated motion compensation amount, the expected clamping force compensated by the accelerated motion compensation amount and the preset reference position.
The robot control method according to the present application, wherein the controlling of the cooperative motion of each robot arm of the robot by using the current corrected external force compensated by the accelerated motion compensation amount, the current actual internal force compensated by the accelerated motion compensation amount, the expected gripping force compensated by the accelerated motion compensation amount, and the predetermined reference position includes:
determining a second external force adjustment amount of each mechanical arm tail end according to the current corrected external force after the compensation of the accelerated motion compensation amount;
determining a second internal force adjustment amount of each mechanical arm tail end according to the current actual internal force compensated by the accelerated motion compensation amount and the expected clamping force compensated by the accelerated motion compensation amount;
And correspondingly controlling the mechanical arms of the robot according to the second external force adjustment amount, the second internal force adjustment amount and the preset reference position of the tail end of each mechanical arm.
The application provides a robot control device, the device includes:
the determining module is used for determining current actual internal force and current actual external force according to actual generalized force obtained by the force sensors at the tail ends of the mechanical arms of the robot when the robot clamps a load, wherein the current actual internal force is used for clamping the load, and the current actual external force is used for controlling the mechanical arms of the robot to enable the position of the load to be compliant;
the compensation module is used for compensating the current actual external force by utilizing the gravity of the load, the centroid parameter of the load, the current position of the tail end of each mechanical arm of the robot and a preset reference position of the tail end of each mechanical arm of the robot to obtain a current correction external force;
and the control module is used for controlling each mechanical arm of the robot to cooperatively move according to the current correction external force, the current actual internal force, the preset expected clamping force and the preset reference position under the condition that the reference acceleration of the robot for clamping load movement is zero.
The application also provides a robot, which comprises a memory and a processor, wherein the memory stores a computer program, and the computer program executes the robot control method when running on the processor.
The application also proposes a readable storage medium storing a computer program which, when run on a processor, executes the robot control method as also proposed by the application.
This application is in during robot centre gripping load, according to current actual internal force and current actual external force are confirmed to the actual generalized force that the terminal force sensor of each arm of robot obtained, utilize the gravity of load the barycenter parameter of load the terminal current position of each arm of robot with the terminal predetermined reference position of each arm of robot is right current actual external force compensates in order to obtain current correction external force under the condition that the reference acceleration of robot centre gripping load motion is zero, according to current correction external force current actual internal force, predetermined expectation clamping force and the terminal predetermined reference position control of each arm of robot each arm concerted movement. The influence of load gravity and gravity moment on the control process of each mechanical arm of the robot is avoided by compensating the current actual external force, the load gravity is prevented from influencing the position compliance, and the load can not accurately follow the external force.
Drawings
In order to more clearly illustrate the technical solution of the present invention, the drawings required to be used in the embodiments will be briefly described below, and it should be understood that the following drawings only illustrate some embodiments of the present invention, and therefore should not be considered as limiting the scope of the present invention. Like components are numbered similarly in the various figures.
Fig. 1 shows a schematic flow chart of a robot control method proposed in an embodiment of the present application;
fig. 2 shows a schematic diagram of a predetermined coordinate system proposed by an embodiment of the present application;
fig. 3 is a schematic flowchart illustrating a method for automatically identifying gravity and a center of mass of a load according to an embodiment of the present application;
FIG. 4 illustrates a schematic view of the coordinate system of the end of each robot arm proposed by the embodiments of the present application;
FIG. 5 is a flow chart illustrating a gravity compensation method according to an embodiment of the present disclosure;
fig. 6 shows a schematic flowchart of a robot dual-arm cooperative control method proposed in an embodiment of the present application;
fig. 7 is a schematic diagram illustrating a dual-arm cooperative control process of a robot according to an embodiment of the present application;
fig. 8 is a schematic flow chart illustrating another robot control method according to an embodiment of the present disclosure;
Fig. 9 shows a schematic structural diagram of a robot control device proposed in an embodiment of the present application;
fig. 10 shows a schematic structural diagram of a robot proposed by the embodiment of the present application.
Description of the main element symbols:
10-a robot control device; 11-a determination module; 12-a compensation module; 13-a control module; 100-a robot; 110-a memory; 120-processor.
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.
The components of embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present invention, presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present invention without making any creative effort, shall fall within the protection scope of the present invention.
Hereinafter, the terms "including", "having", and their derivatives, which may be used in various embodiments of the present invention, are only intended to indicate specific features, numbers, steps, operations, elements, components, or combinations of the foregoing, and should not be construed as first excluding the existence of, or adding to, one or more other features, numbers, steps, operations, elements, components, or combinations of the foregoing.
Furthermore, the terms "first," "second," "third," and the like are used solely to distinguish one from another and are not to be construed as indicating or implying relative importance.
Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which various embodiments of the present invention belong. The terms (such as those defined in commonly used dictionaries) should be interpreted as having a meaning that is consistent with their contextual meaning in the relevant art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein in various embodiments of the present invention.
This application is in during robot centre gripping load, according to current actual internal force and current actual external force are confirmed to the actual generalized force that the terminal force sensor of each arm of robot obtained, can understand, and current actual internal force is used for centre gripping load, and current actual external force is used for controlling each arm of robot so that the load produces the position and follows, controls robot centre gripping load and follows current actual external force motion promptly.
Because of the influence of the gravity of the clamped load, when the two arms clamp the load, even if the external force is not applied, the external force can be decomposed, so that the clamped load of the robot moves along with the external force decomposed by the gravity of the load, and in order to avoid the external force decomposed by the gravity of the load, the clamped load of the robot is controlled to move along with the external force, the current actual external force needs to be compensated to avoid the influence of the gravity of the load.
The premise that the current actual external force is compensated to avoid the influence of load gravity is that the load gravity and the load size need to be obtained first, so that the current actual external force is compensated according to the load gravity and the load size. It will be appreciated that the load weight and load size may be predetermined and input directly to the controller of the robot, but the predetermined load weight and load size requires manual advance measurement and calculation and also requires advance modification of the load weight and load size already recorded in the controller of the robot when the load changes, which is a less flexible approach.
In view of the above, the present application discloses a method for identifying load mass and center of mass, which comprises controlling a robot to clamp a load away from a supporting surface where the load is located, determining the gravity of the load by using the force in the same direction as the gravity of the load obtained by a force sensor at the end of each mechanical arm of the robot, determining the distance between the center of mass of the load and the end of each mechanical arm of the robot by using the moment in the same direction as the gravity of the load obtained by the force sensor at the end of each mechanical arm and the gravity of the load, and then compensating the current actual external force by using the gravity of the load, the distance between the center of mass of the load and the tail end of each mechanical arm of the robot, the current position of the tail end of each mechanical arm of the robot and the preset reference position of the tail end of each mechanical arm of the robot to obtain the current correction external force, and further avoiding the influence of the gravity of the load when the mechanical arm clamps the load to do follow motion.
When a robot clamps a load to carry out carrying operation, the robot needs to accelerate or decelerate according to operation time, operation path length, operation environment and the like, due to the influence of load inertia force in the acceleration or deceleration stage, the expected clamping force of each mechanical arm end of the robot is not equal, the current actual internal force of each mechanical arm end is not equal, the current correction external force (namely the current actual external force compensated by using load gravity) of each mechanical arm end is not equal, in order to avoid position tracking error in the acceleration or deceleration stage, the end with smaller expected clamping force needs to be compensated by using the gravity of the load and the current reference acceleration (the reference acceleration is the acceleration planned in advance when a task is planned), the end with smaller current actual internal force needs to be compensated, and the end with smaller current actual external force needs to be compensated, thereby guaranteeing that the two arms of the robot can stably clamp the load and avoiding the load from falling.
Example 1
In an embodiment of the present application, please refer to fig. 1, which provides a robot control method, including steps S100, S200, and S300:
s100: and when the robot clamps the load, determining the current actual internal force and the current actual external force according to the actual generalized force obtained by the force sensors at the tail ends of the mechanical arms of the robot.
Wherein the current actual internal force is used for clamping the load, and the current actual external force is used for controlling each mechanical arm of the robot to make the load generate position compliance.
The current actual internal force and the current actual external force may be determined by:
acquiring actual generalized forces obtained by force sensors at the tail ends of the mechanical arms in a preset coordinate system; obtaining vectors from the central point of the contact surface between the tail end of each mechanical arm and the load to the center of mass of the load; and determining the current actual internal force and the current actual external force by using the actual generalized forces and the vectors.
Exemplarily, for a two-arm robot, the actual generalized force obtained by the force sensor at the end of each robot arm may be expressed as F ═ F 1 ,F 2 ]，F 1 Representing a representation of an actual generalized force obtained by a force sensor at the end of a first robot arm of said robot in a predetermined coordinate system, F 2 Representing a representation of an actual generalized force obtained by a force sensor at a second arm end of the robot in the predetermined coordinate system. Referring to fig. 2, the predetermined coordinate system may be a load center O 1 Seat established for originThe mark system.
Further, F ═ F can be determined using the following formula 1 ,F 2 ]The corresponding current actual internal force after decomposition:
F i denotes that F ═ F 1 ,F 2 ]Current actual internal force, F, corresponding to the decomposed 1i Representing a current internal force matrix, F, corresponding to a first arm end of the robot 2i Representing a current internal force matrix corresponding to the tail end of a second mechanical arm of the robot, T representing transposition operation on the matrix, I 12 A 12-dimensional unit square matrix is represented,
I 3 representing a three-dimensional unit matrix, O 3 Representing a three-dimensional zero matrix, r j Representing a vector from a center point of a contact surface of the j-th mechanical arm end and the load to the center of mass of the load, r j ＝[p jx ,p jy ,p jz ]，
Is r j J 1 and 2,
the method comprises the steps of firstly performing transposition operation on a matrix and then determining a corresponding inverse matrix;
further, F ═ F can be determined using the following formula 1 ,F 2 ]The current actual external force after decomposition is as follows:
F e denotes that F ═ F 1 ,F 2 ]Current actual external force after decomposition, F 1e Representing a current external force matrix corresponding to a first mechanical arm end of the robot ，F 2e And representing a current external force matrix corresponding to the tail end of the second mechanical arm of the robot.
S200: and compensating the current actual external force by using the gravity of the load, the centroid parameter of the load, the current position of the tail end of each mechanical arm of the robot and a preset reference position of the tail end of each mechanical arm of the robot to obtain a current correction external force.
It will be appreciated that the centre of mass parameter comprises the distance of the centre of mass of the load from the end of each arm of the robot. The compensation includes positive compensation and negative compensation.
Referring to FIG. 3, the gravity of the load and the centroid parameters of the load may be determined using the following methods S10-S50:
s10: determining whether an absolute value of a difference between the current actual internal force and a predetermined desired clamping force is less than a preset threshold.
It can be understood that the absolute value of the difference between the current actual internal force and the predetermined expected clamping force is smaller than the preset threshold value, which indicates that the current actual internal force is approximately equal to the predetermined expected clamping force, and at this time, it can be ensured that the robot can stably clamp the load away from the supporting surface where the load is located.
If the threshold value is smaller than the preset threshold value, the step S20 is executed, and if the threshold value is greater than or equal to the preset threshold value, the step S50 is executed.
S20: and controlling the robot to clamp the load to move a preset distance in a direction opposite to the gravity direction of the load.
It will be appreciated that the predetermined distance should be large enough to ensure that the load is completely clear of the support surface on which the load is located. The actual generalized force obtained by the force sensors at each end of the robot includes the total weight of the load.
S30: after moving the predetermined distance, it is determined whether an absolute value of a difference between the current actual internal force and the predetermined desired clamping force is less than a preset threshold value.
Whether the absolute value of the difference between the current actual internal force and the preset expected clamping force is smaller than a preset threshold value or not is determined, so that whether the current actual internal force obtained by resolving the actual generalized force obtained by the force sensors at the tail ends of the mechanical arms of the robot can stabilize the clamping load or not is determined, and the load is guaranteed not to fall off. If the absolute value of the difference between the current actual internal force and the preset expected clamping force is smaller than the preset threshold value, the fact that the current actual internal force is approximately equal to the preset expected clamping force is shown, and at the moment, the robot can be guaranteed to stably clamp the load.
If the threshold value is smaller than the preset threshold value, the step S40 is executed, and if the threshold value is greater than or equal to the preset threshold value, the step S50 is executed.
S40: and determining the gravity of the load according to the force in the gravity direction of the load in the actual generalized force of each mechanical arm tail end, and determining the distance between the center of mass of the load and any mechanical arm tail end of the robot according to the gravity of the load and the moment in the gravity direction of the load in the actual generalized force of each mechanical arm tail end.
Exemplarily, referring to fig. 4, fig. 4 is a top view of a robot clamping a load, where p _ center is a center of mass of the load, and if a central point of a contact surface between each robot arm end and the load is taken as an origin of each end coordinate system, a positive direction of an x-axis of each end coordinate system is a normal direction of the contact surface, a positive direction of a y-axis of each end coordinate system is a gravity direction of the load, and a positive direction of a z-axis of each end coordinate system is a direction away from the robot, a gravity G of the load is Fy _1+ Fy _2, where Fy _1 represents a force in a y-axis direction of a first robot arm end coordinate system corresponding to the first robot arm end acquired by a first robot arm end sensor, and Fy _2 represents a force in a y-axis direction of a second robot arm end coordinate system corresponding to the second arm end acquired by a second robot arm end sensor; the distance d between the center of mass of the load and each end of the robot arm is (Ty _1+ Ty _2)/G, where Ty _1 represents the moment in the y-axis direction in the first arm end coordinate system corresponding to the first arm end acquired by the first arm end sensor, and Ty _2 represents the moment in the y-axis direction in the second arm end coordinate system corresponding to the second arm end acquired by the second arm end sensor.
S50: and adjusting the position of each mechanical arm tail end of the robot until the absolute value of the difference between the current actual internal force and the expected clamping force is smaller than the preset threshold value.
Further, referring to fig. 5, step S200 includes the following steps S210 to S240:
s210: determining a first rotation angle of the J-th mechanical arm end rotating around a first coordinate axis of a J-th mechanical arm end coordinate system in the process that the J-th mechanical arm end of the robot is from the corresponding current position to the corresponding preset reference position, wherein J is more than or equal to 1 and less than or equal to J, and J is the total number of the mechanical arms of the robot.
S220: and determining a second rotation angle of the j-th mechanical arm tail end around a second coordinate axis of a j-th mechanical arm tail end coordinate system in the process that the j-th mechanical arm tail end is from the corresponding current position to the corresponding preset reference position, wherein the first coordinate axis, the second coordinate axis and the coordinate axis in the gravity direction are mutually vertical.
S230: and compensating the current external force corresponding to the tail end of the jth mechanical arm in the current actual external force according to the gravity of the load, the centroid parameter of the load, the first rotation angle corresponding to the jth mechanical arm and the second rotation angle corresponding to the jth mechanical arm to obtain the corrected external force corresponding to the tail end of the jth mechanical arm.
S240: and determining the current correction external force by using the correction external forces corresponding to the tail ends of all the mechanical arms.
Exemplarily, if a central point of a contact surface between each robot arm end and the load is taken as an origin of each end coordinate system, a positive direction of an x-axis of each end coordinate system is a normal direction of the contact surface, a positive direction of a y-axis of each end coordinate system is a gravity direction of the load, and a positive direction of a z-axis of each end coordinate system is a direction away from the robot, the first coordinate axis in the step S210 and the second coordinate axis in the step S220 are respectively a z-axis and an x-axis.
Further, for example, the two-arm robot may determine the correction force in the x-axis direction of the coordinate system of the end of the first arm corresponding to the end of the first arm in the current correction external force by using the following formula:
Fex_1＝Fx_1+0.5*G*sin(alpha_1)
fex _1 represents a correction force in the x-axis direction of a coordinate system at the end of the first mechanical arm corresponding to the end of the first mechanical arm in the current correction external force, Fx _1 represents a force in the x-axis direction of the coordinate system at the end of the first mechanical arm corresponding to the end of the first mechanical arm in the current actual external force, G represents the gravity of the load, and alpha _1 represents a rotation angle of the end of the first mechanical arm around the positive direction of the z-axis of the coordinate system at the end of the first mechanical arm in the process that the end of the first mechanical arm of the robot moves from the current position to the preset reference position.
The correction force in the y-axis direction of the coordinate system of the end of the first robot arm corresponding to the end of the first robot arm in the current correction external force can be determined by using the following formula:
Fey_1＝Fy_1+0.5*G*cos(theta_1)*cos(alpha_1)
fey _1 represents a correction force in the y-axis direction of a coordinate system of the end of the first mechanical arm corresponding to the end of the first mechanical arm in the current correction external force, Fy _1 represents a force in the y-axis direction of the coordinate system of the end of the first mechanical arm corresponding to the end of the first mechanical arm in the current actual external force, and theta _1 represents a rotation angle of the end of the first mechanical arm around the positive direction of the x-axis of the coordinate system of the end of the first mechanical arm in the process that the end of the first mechanical arm of the robot moves from the current position to the preset reference position.
The correction force in the z-axis direction of the coordinate system of the end of the first robot arm corresponding to the end of the first robot arm in the current correction external force can be determined by using the following formula:
Fez_1＝Fz_1-0.5*G*sin(theta_1)
fez _1 represents the correction force in the z-axis direction of the first arm end coordinate system corresponding to the first arm end in the current correction external force, and Fz _1 represents the force in the z-axis direction of the first arm end coordinate system corresponding to the first arm end in the current actual external force.
The correction torque in the x-axis direction of the coordinate system of the end of the first mechanical arm corresponding to the end of the first mechanical arm in the current correction external force can be determined by using the following formula:
Mex_1＝Mx_1-0.5*d*G*cos(theta_1)
Mex _1 represents a correction moment in the x-axis direction of a coordinate system at the end of the first mechanical arm corresponding to the end of the first mechanical arm in the current correction external force, and Mx _1 represents a moment in the x-axis direction of the coordinate system at the end of the first mechanical arm corresponding to the end of the first mechanical arm in the current actual external force.
The correction force in the x-axis direction of the coordinate system of the end of the second mechanical arm corresponding to the end of the second mechanical arm in the current correction external force can be determined by using the following formula:
Fex_2＝Fx_2-0.5*G*sin(alpha_2)
fex _2 represents a correction force in the x-axis direction of a second mechanical arm end coordinate system corresponding to the second mechanical arm end in the current correction external force, Fx _2 represents a force in the x-axis direction of a second end coordinate system corresponding to the second mechanical arm end in the current actual external force, and alpha _2 represents a rotation angle of the second mechanical arm end around the positive direction of the z-axis of the second end coordinate system in the process that the robot second mechanical arm end moves from the current position to the preset reference position;
the correction force in the y-axis direction of the coordinate system of the end of the second mechanical arm corresponding to the end of the second mechanical arm in the current correction external force can be determined by using the following formula:
Fey_2＝Fy_2-0.5*G*cos(theta_2)*cos(alpha_2)
fey _2 represents a correction force in the y-axis direction of a second mechanical arm end coordinate system corresponding to the second mechanical arm end in the current correction external force, Fy _2 represents a force in the x-axis direction of a second end coordinate system corresponding to the second mechanical arm end in the current actual external force, and theta _2 represents a rotation angle of the second mechanical arm end around the x-axis positive direction of the second end coordinate system in the process that the robot moves from the current position to the preset reference position.
The correction force in the z-axis direction of the second end coordinate system corresponding to the second mechanical arm end in the current correction external force can be determined by using the following formula:
Fez_2＝Fz_2+0.5*G*sin(theta_2)
fez _2 represents a correction force in the z-axis direction of the second robot arm end coordinate system corresponding to the second robot arm end in the current correction external force, and Fz _2 represents a force in the z-axis direction of the second robot arm end coordinate system corresponding to the second robot arm end in the current actual external force.
The correction moment in the x-axis direction of the second end coordinate system corresponding to the end of the first mechanical arm in the current correction external force can be determined by using the following formula:
Mex_2＝Mx_2+0.5*d*G*cos(theta_2)
mex _2 represents a correction moment in the x-axis direction of a second mechanical arm end coordinate system corresponding to the second mechanical arm end in the current correction external force, and Mx _1 represents a moment in the x-axis direction of the second mechanical arm end coordinate system corresponding to the second mechanical arm end in the current actual external force.
S300: and under the condition that the reference acceleration of the robot for clamping the load motion is zero, controlling each mechanical arm of the robot to cooperatively move according to the current correction external force, the current actual internal force, the preset expected clamping force and the preset reference position of the tail end of each mechanical arm of the robot.
Exemplarily, referring to fig. 6, step S300 includes steps S310, S320 and S330:
S310: and determining the first external force adjustment amount of the tail end of each mechanical arm according to the current correction external force.
Admittance control is divided into three control modes, namely a force control mode, an impedance mode and a dragging mode, an external force controller of the robot arm comprises the impedance mode or the dragging mode of the admittance controller, and a corresponding control equation can be selected according to the current operation mode of the robot.
Further, the impedance mode control equation is:
wherein M is d 、B d 、K d Respectively, an inertia matrix, a damping matrix and a stiffness matrix, X, of the expected impedance model c For the commanded position of the robot end, X r Is a predetermined reference position of the robot tip,
and
are each X c The first and second derivatives of (a) and (b),
and
are each X r First and second derivatives of (F) Outer cover For the current correction external force, the impedance mode control equation may determine a first external force adjustment Δ X ═ X c -X r Further, let Δ X + X r As the command position, Δ X + X r Input to the controller of the robot to convert Δ X + X r As commanded positions for adjusting the individual arms of the robot.
Under the impedance mode, when the six-dimensional force sensor at the tail end of each mechanical arm receives external force, the admittance controller outputs a first external force adjustment quantity delta X ═ X c -X r When the external force disappears, the admittance control continues to output the first external force adjustment amount until delta X is equal to X due to the existence of the stiffness term c -X r At this point, the robot returns to the predetermined reference position again.
Further, the drag mode control equation is:
in the drag mode, the current correction external force F Outer cover The admittance controller generates a first external force adjustment Δ X at F Outer cover The speed of change of the first external force adjustment amount is set to zero
And (4) zero setting, namely, the robot keeps the first external force adjustment quantity delta X unchanged, so that the robot stays at the position when the external force disappears.
S320: and determining a first internal force adjustment amount of each mechanical arm tail end according to the current actual internal force and the expected clamping force.
The internal force controller of the robot arm is in a force control mode, and the control equation is as follows:
wherein, F Inner part For the current actual internal force, F d To expect the clamping force, in force control mode, the robot will be at the expected clamping force F d Is driven to stably contact with the clamped object.
S330: and correspondingly controlling the mechanical arms of the robot according to the first external force adjustment amount, the first internal force adjustment amount and the preset reference position of the tail end of each mechanical arm.
Exemplarily, referring to fig. 7, taking a two-arm robot as an example, the actual generalized force F obtained by the force sensors at the two ends of the two arms is ═ F 1 ,F 2 ]After decomposition, the current internal force F corresponding to the tail end of the first mechanical arm is obtained 1i Current internal force F corresponding to the end of the second mechanical arm 2i Obtaining the current external force F corresponding to the tail end of the first mechanical arm 1e Current external force F corresponding to the end of the second mechanical arm 2e 。
Wherein, F 1i ＝-F 2i The current internal force F corresponding to the tail end of the first mechanical arm is used 1i The first internal force adjustment quantity delta X corresponding to the tail end of the first mechanical arm is obtained by inputting the first internal force adjustment quantity delta X to an internal force controller of the first mechanical arm 1i The current internal force F corresponding to the tail end of the second mechanical arm 2i Inputting the first internal force into an internal force controller of the second mechanical arm to obtain a first internal force adjustment quantity delta X corresponding to the tail end of the second mechanical arm 2i 。
Wherein, F 1e ＝F 2e Compensating F using the gravity of the load, the distance between the center of mass of the load and the end of each robot arm of the robot, the current position of the end of each robot arm of the robot, and a predetermined reference position of the end of each robot arm of the robot 1e And F 2e Obtaining the current correction external force, inputting the current correction external force to the external force controller to obtain the tail end of the first mechanical armCorresponding first external force adjustment amount DeltaX 1e A second external force adjustment amount DeltaX corresponding to the end of the second mechanical arm 2e It can be appreciated that the first arm end and the second arm end clamp a load by an internal force and follow the load by an external force, so F 1e ＝F 2e Therefore, the first external force adjustment amount Δ X corresponding to the end of the first mechanical arm 1e Equal to the second external force adjustment quantity delta X corresponding to the tail end of the second mechanical arm 2e 。
Furthermore, the adjustment quantity delta X is adjusted according to a first external force corresponding to the tail end of the first mechanical arm 1e Corresponding first internal force adjustment quantity delta X 1i And a corresponding predetermined reference position X 1r Correspondingly controlling a first mechanical arm of the robot, and adjusting the quantity delta X according to a second external force corresponding to the tail end of a second mechanical arm 2e Corresponding second internal force adjustment amount delta X 2i And a corresponding predetermined reference position X 2r And correspondingly controlling the second mechanical arm of the robot to realize the cooperative motion of all the mechanical arms of the robot.
In order to avoid that the external force decomposed from the gravity of the load influences the robot to clamp the load to do follow-up motion, the current actual external force is compensated by using the gravity of the load, the distance between the center of mass of the load and the tail end of each mechanical arm of the robot, the current position of the tail end of each mechanical arm of the robot and the preset reference position of the tail end of each mechanical arm of the robot to obtain the current correction external force, and under the condition that the reference acceleration of the robot to clamp the load motion is zero, the robot arms of the robot are controlled to cooperatively move according to the current correction external force, the current actual internal force, the preset expected clamping force and the preset reference position of the tail end of each mechanical arm of the robot. The current actual external force is compensated to avoid the influence of load gravity and gravity moment on the pose of the load clamped by the robot, so that the influence of the load gravity and the gravity moment when the robot clamps the load to move along is avoided.
Example 2
According to the other embodiment of the application, under the condition that the reference acceleration of the robot for clamping the load motion is not zero, the gravity and the current reference acceleration of the load are utilized to compensate the end with the smaller expected clamping force, the end with the smaller current actual internal force is compensated, the end with the smaller current actual external force is compensated, the clamping load can be stabilized by the two arms of the robot, and the load falling is avoided.
Further, referring to fig. 8, another robot control method is proposed, which comprises the following steps after steps S100 and S200:
s400: and under the condition that the reference acceleration of the robot for clamping the load is not zero, determining an acceleration motion compensation amount by using the gravity of the load and the reference acceleration.
The acceleration motion compensation amount is G a, G is the gravity of the load, G is the gravity reference acceleration, and a is the reference acceleration of the load motion.
S500: and respectively compensating the current correction external force, the current actual internal force and the expected clamping force by using the accelerated motion compensation amount.
The current correction external force comprises correction external forces corresponding to all the mechanical arms, the current actual internal force comprises internal forces corresponding to all the mechanical arms, the expected clamping force comprises clamping forces corresponding to all the mechanical arms, the minimum correction external force is determined from the correction external forces corresponding to the tail ends of all the mechanical arms, and the minimum correction external force is added with the accelerated motion compensation amount to realize compensation of the current correction external force; determining the minimum internal force from the internal forces corresponding to the tail ends of the mechanical arms, and adding the minimum internal force to the acceleration motion compensation amount to realize the compensation of the current actual internal force; and determining the minimum clamping force from the clamping forces corresponding to the tail ends of the mechanical arms, and adding the minimum clamping force to the accelerated motion compensation amount to realize the compensation of the expected clamping force.
S600: and controlling the cooperative motion of each mechanical arm of the robot by using the current corrected external force compensated by the accelerated motion compensation amount, the current actual internal force compensated by the accelerated motion compensation amount, the expected clamping force compensated by the accelerated motion compensation amount and the preset reference position.
Further, determining a second external force adjustment amount of each mechanical arm tail end according to the current correction external force after the compensation of the accelerated motion compensation amount; determining a second internal force adjustment amount of each mechanical arm tail end according to the current actual internal force compensated by the accelerated motion compensation amount and the expected clamping force compensated by the accelerated motion compensation amount; and correspondingly controlling the mechanical arms of the robot according to the second external force adjustment amount, the second internal force adjustment amount and the preset reference position of the tail end of each mechanical arm.
In order to avoid position tracking errors caused by acceleration or deceleration of the robot arm clamping load, the embodiment compensates one end with a smaller expected clamping force by using the gravity of the load and the current reference acceleration, compensates one end with a smaller current actual internal force, and compensates one end with a smaller current actual external force, so that the robot arm can stably clamp the load and avoid load falling.
Example 3
In an embodiment of the present application, please refer to fig. 9, which proposes a robot control device 10 including: a determination module 11, a compensation module 12 and a control module 13.
A determining module 11, configured to determine, when the robot clamps a load, a current actual internal force and a current actual external force according to actual generalized forces obtained by force sensors at ends of each mechanical arm of the robot, where the current actual internal force is used to clamp the load, and the current actual external force is used to control each mechanical arm of the robot to make a position of the load conform to; a compensation module 12, configured to compensate the current actual external force by using the gravity of the load, the centroid parameter of the load, the current position of the tail end of each mechanical arm of the robot, and a predetermined reference position of the tail end of each mechanical arm of the robot, so as to obtain a current correction external force; and the control module 13 is configured to control each mechanical arm of the robot to cooperatively move according to the current correction external force, the current actual internal force, the predetermined expected clamping force and the predetermined reference position when the reference acceleration of the robot clamping load movement is zero.
Further, the control module 13 is further configured to determine an acceleration compensation amount by using the gravity of the load and a reference acceleration of the robot for clamping the load when the reference acceleration is not zero; respectively compensating the current correction external force, the current actual internal force and the expected clamping force by using the accelerated motion compensation amount; and controlling the cooperative motion of each mechanical arm of the robot by using the current corrected external force compensated by the accelerated motion compensation amount, the current actual internal force compensated by the accelerated motion compensation amount, the expected clamping force compensated by the accelerated motion compensation amount and the preset reference position.
The robot control apparatus 10 disclosed in this embodiment is configured to execute the robot control method according to the foregoing embodiment through the cooperative use of the determination module 11, the compensation module 12, and the control module 13, and the implementation and beneficial effects related to the foregoing embodiment are also applicable in this embodiment, and are not described again here.
Referring to fig. 10, the present application discloses a robot 100, which includes a memory 110 and a processor 120, where the memory 110 stores a computer program, and the computer program executes the robot control method described in the present application when running on the processor 120.
A readable storage medium storing a computer program which, when run on a processor, performs the robot control method described herein.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method can be implemented in other ways. The apparatus embodiments described above are merely illustrative and, for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present invention. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, each functional module or unit in each embodiment of the present invention may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
The functions may be stored in a readable storage medium if they are implemented in the form of software functional modules and sold or used as separate products. Based on such understanding, the technical solution of the present invention or a part of the technical solution that contributes to the prior art in essence can be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned readable storage medium comprises: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk, and various media capable of storing program codes.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention.
Claims (10)
1. A robot control method, characterized in that the method comprises:
when the robot clamps a load, determining a current actual internal force and a current actual external force according to actual generalized forces obtained by force sensors at the tail ends of all mechanical arms of the robot, wherein the current actual internal force is used for clamping the load, and the current actual external force is used for controlling all mechanical arms of the robot to enable the position of the load to be compliant;
compensating the current actual external force by using the gravity of the load, the centroid parameter of the load, the current position of the tail end of each mechanical arm of the robot and a preset reference position of the tail end of each mechanical arm of the robot to obtain a current correction external force;
and under the condition that the reference acceleration of the robot for clamping the load motion is zero, controlling each mechanical arm of the robot to cooperatively move according to the current correction external force, the current actual internal force, the preset expected clamping force and the preset reference position.
2. The robot control method according to claim 1, wherein the determining a current actual internal force and a current actual external force from actual generalized forces obtained by force sensors at ends of respective robot arms of the robot includes:
Acquiring each actual generalized force obtained by a force sensor at the tail end of each mechanical arm under a preset coordinate system;
obtaining each vector from the central point of the contact surface between the tail end of each mechanical arm and the load to the center of mass of the load;
determining the current actual internal force and the current actual external force using the respective actual generalized forces and the respective vectors.
3. The robot control method according to claim 1, wherein before the compensating the current actual external force using the gravity of the load, the centroid parameter of the load, the current position of each robot arm end and the predetermined reference position of each robot arm end of the robot to obtain the current corrected external force, comprises:
determining whether an absolute value of a difference between the current actual internal force and a predetermined desired clamping force is less than a preset threshold;
if the clamping force is smaller than the preset threshold value, controlling the robot to clamp the load and move a preset distance in a direction opposite to the gravity direction of the load;
after moving for a predetermined distance, determining whether the absolute value of the difference between the current actual internal force and the predetermined expected clamping force is less than the preset threshold value;
if the load is smaller than the preset threshold value, determining the gravity of the load according to the force in the direction of the gravity of the load in the actual generalized forces at the tail ends of the mechanical arms, and determining the distance between the center of mass of the load and the tail end of any mechanical arm of the robot according to the gravity of the load and the moment in the direction of the gravity of the load in the actual generalized forces at the tail ends of the mechanical arms;
And if the difference is larger than or equal to the preset threshold value, adjusting the position of each mechanical arm tail end of the robot until the absolute value of the difference between the current actual internal force and the expected clamping force is smaller than the preset threshold value.
4. The robot control method according to claim 1, wherein the compensating the current actual external force using the gravity of the load, the centroid parameter of the load, the current position of each robot arm end of the robot, and the predetermined reference position of each robot arm end of the robot to obtain a current corrected external force comprises:
determining a first rotation angle of the J-th mechanical arm tail end of the robot rotating around a first coordinate axis of a J-th mechanical arm tail end coordinate system in the process from the corresponding current position to the corresponding preset reference position, wherein J is more than or equal to 1 and less than or equal to J, and J is the total number of the mechanical arms of the robot;
determining a second rotation angle of the j-th mechanical arm tail end around a second coordinate axis of a j-th mechanical arm tail end coordinate system in the process that the j-th mechanical arm tail end is from the corresponding current position to the corresponding preset reference position, wherein the first coordinate axis, the second coordinate axis and a coordinate axis in the gravity direction are mutually vertical;
Compensating the current external force corresponding to the tail end of the jth mechanical arm in the current actual external force according to the gravity of the load, the centroid parameter of the load, the first rotation angle corresponding to the jth mechanical arm and the second rotation angle corresponding to the jth mechanical arm to obtain a corrected external force corresponding to the tail end of the jth mechanical arm;
and determining the current correction external force by using the correction external forces corresponding to the tail ends of all the mechanical arms.
5. The robot control method according to claim 1, wherein the controlling of the respective robot arms of the robot in cooperative motion based on the current corrected external force, the current actual internal force, a predetermined desired gripping force, and the predetermined reference position comprises:
determining a first external force adjustment amount of each mechanical arm tail end according to the current correction external force;
determining a first internal force adjustment amount of each mechanical arm tail end according to the current actual internal force and the expected clamping force;
and correspondingly controlling the mechanical arms of the robot according to the first external force adjustment amount, the first internal force adjustment amount and the preset reference position of the tail end of each mechanical arm.
6. The robot control method according to any one of claims 1 to 5, characterized by further comprising:
Under the condition that the reference acceleration of the robot for clamping the load is not zero, determining an accelerated motion compensation amount by using the gravity of the load and the reference acceleration;
respectively compensating the current correction external force, the current actual internal force and the expected clamping force by using the accelerated motion compensation amount;
and controlling the cooperative motion of each mechanical arm of the robot by using the current corrected external force compensated by the accelerated motion compensation amount, the current actual internal force compensated by the accelerated motion compensation amount, the expected clamping force compensated by the accelerated motion compensation amount and the preset reference position.
7. The robot control method according to claim 6, wherein the controlling of the cooperative movement of the respective robot arms of the robot using the current corrected external force compensated for by the accelerated motion compensation amount, the current actual internal force compensated for by the accelerated motion compensation amount, the desired gripping force compensated for by the accelerated motion compensation amount, and the predetermined reference position comprises:
determining a second external force adjustment quantity of the tail end of each mechanical arm according to the current corrected external force after the compensation of the accelerated motion compensation quantity;
determining a second internal force adjustment amount of each mechanical arm tail end according to the current actual internal force compensated by the accelerated motion compensation amount and the expected clamping force compensated by the accelerated motion compensation amount;
And correspondingly controlling the mechanical arms of the robot according to the second external force adjustment amount, the second internal force adjustment amount and the preset reference position of the tail end of each mechanical arm.
8. A robot control apparatus, characterized in that the apparatus comprises:
the determining module is used for determining current actual internal force and current actual external force according to actual generalized force obtained by the force sensors at the tail ends of the mechanical arms of the robot when the robot clamps a load, wherein the current actual internal force is used for clamping the load, and the current actual external force is used for controlling the mechanical arms of the robot to enable the position of the load to be compliant;
the compensation module is used for compensating the current actual external force by utilizing the gravity of the load, the centroid parameter of the load, the current position of the tail end of each mechanical arm of the robot and a preset reference position of the tail end of each mechanical arm of the robot to obtain a current correction external force;
and the control module is used for controlling each mechanical arm of the robot to cooperatively move according to the current correction external force, the current actual internal force, the preset expected clamping force and the preset reference position under the condition that the reference acceleration of the robot for clamping load movement is zero.
9. A robot comprising a memory and a processor, the memory storing a computer program which, when run on the processor, performs the robot control method of any of claims 1 to 7.
10. A readable storage medium, characterized in that it stores a computer program which, when run on a processor, performs the robot control method of any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111137896.4A CN113799134B (en) | 2021-09-27 | 2021-09-27 | Robot control method, device, robot and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111137896.4A CN113799134B (en) | 2021-09-27 | 2021-09-27 | Robot control method, device, robot and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113799134A CN113799134A (en) | 2021-12-17 |
| CN113799134B true CN113799134B (en) | 2022-07-29 |
Family
ID=78938743
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111137896.4A Active CN113799134B (en) | 2021-09-27 | 2021-09-27 | Robot control method, device, robot and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113799134B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115416018B (en) * | 2022-08-17 | 2024-03-15 | 雅客智慧（北京）科技有限公司 | End effector deformation compensation method, device, electronic equipment and storage medium |
| CN116175594B (en) * | 2023-04-26 | 2023-06-30 | 北京宸普豪新科技有限公司 | Robot arm control method and equipment and robot |
| CN117565063B (en) * | 2024-01-16 | 2024-03-29 | 泓浒(苏州)半导体科技有限公司 | Self-adaptive positioning compensation method and system for abrasion of transmission part of wafer carrying manipulator |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH07205075A (en) * | 1994-01-25 | 1995-08-08 | Nippon Steel Corp | Weight compensation method of end effector at force control robot |
| CN104647379A (en) * | 2015-01-19 | 2015-05-27 | 广东工业大学 | Dual-arm robot movement control method under non-linear condition of driver |
| CN106475999A (en) * | 2016-12-23 | 2017-03-08 | 东南大学 | The acceleration control method of the Dual-Arm Coordination based on impedance model under hard conditions |
| CN106695797A (en) * | 2017-02-22 | 2017-05-24 | 哈尔滨工业大学深圳研究生院 | Compliance control method and system based on collaborative operation of double-arm robot |
| CN110977990A (en) * | 2019-12-30 | 2020-04-10 | 苏州艾利特机器人有限公司 | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor |
| WO2020157186A1 (en) * | 2019-01-31 | 2020-08-06 | Franka Emika Gmbh | Teaching process for a robot system consisting of two robot manipulators |
| CN111590564A (en) * | 2020-05-07 | 2020-08-28 | 北京控制工程研究所 | Gravity compensation method and system for six-dimensional force sensor |
| CN112372630A (en) * | 2020-09-24 | 2021-02-19 | 哈尔滨工业大学（深圳） | Multi-mechanical-arm cooperative polishing force compliance control method and system |
-
2021
- 2021-09-27 CN CN202111137896.4A patent/CN113799134B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH07205075A (en) * | 1994-01-25 | 1995-08-08 | Nippon Steel Corp | Weight compensation method of end effector at force control robot |
| CN104647379A (en) * | 2015-01-19 | 2015-05-27 | 广东工业大学 | Dual-arm robot movement control method under non-linear condition of driver |
| CN106475999A (en) * | 2016-12-23 | 2017-03-08 | 东南大学 | The acceleration control method of the Dual-Arm Coordination based on impedance model under hard conditions |
| CN106695797A (en) * | 2017-02-22 | 2017-05-24 | 哈尔滨工业大学深圳研究生院 | Compliance control method and system based on collaborative operation of double-arm robot |
| WO2020157186A1 (en) * | 2019-01-31 | 2020-08-06 | Franka Emika Gmbh | Teaching process for a robot system consisting of two robot manipulators |
| CN110977990A (en) * | 2019-12-30 | 2020-04-10 | 苏州艾利特机器人有限公司 | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor |
| CN111590564A (en) * | 2020-05-07 | 2020-08-28 | 北京控制工程研究所 | Gravity compensation method and system for six-dimensional force sensor |
| CN112372630A (en) * | 2020-09-24 | 2021-02-19 | 哈尔滨工业大学（深圳） | Multi-mechanical-arm cooperative polishing force compliance control method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113799134A (en) | 2021-12-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113799134B (en) | Robot control method, device, robot and readable storage medium | |
| JP6223640B2 (en) | Calibration apparatus and robot system using the same | |
| CN111278613B (en) | Calibration device, calibration method, and control device | |
| JP4850956B2 (en) | Robot with learning control function | |
| CN108356823A (en) | Control system with learning control function and control method | |
| US10328581B2 (en) | Method and device for robotic direct lead-through teaching | |
| CN113561217A (en) | Drive mechanism, robot apparatus measurement and control method, and part manufacturing method | |
| CN113510698B (en) | Mechanical arm control method and device, mechanical arm and readable storage medium | |
| US9844872B1 (en) | Determining sensor parameters and model parameters of a robot | |
| US20130338832A1 (en) | Robot controller which conducts a force control by using a three-axial force sensor | |
| JP6564433B2 (en) | Robot system | |
| JP2019013984A (en) | Robot for performing learning control in application for which constant speed is required and control method therefor | |
| Jin et al. | Switching PD-based sliding mode control for hovering of a tilting-thruster underwater robot | |
| CN111015661A (en) | Active vibration control method and system for flexible load of robot | |
| CN112975965B (en) | Decoupling control method and device of humanoid robot and humanoid robot | |
| CN106826826B (en) | A kind of robot control method and device | |
| CN116175148B (en) | Compliant shaft hole assembly method based on contact state model and self-adaptive impedance control | |
| CN110788859A (en) | Controller parameter universe self-adaptive adjustment system | |
| CN112135718A (en) | Control of a robot | |
| Paine et al. | Uniform Complete Observability of Mass and Rotational Inertial Parameters in Adaptive Identification of Rigid-Body Plant Dynamics | |
| US20220410384A1 (en) | Force Control Parameter Adjustment Method And Force Control Parameter Adjustment Apparatus | |
| CN117067210A (en) | Motion control method and device for clamping-free mechanical arm, computer equipment and medium | |
| İz et al. | Vision-based system identification of a quadrotor | |
| CN116140972A (en) | Robot multi-shaft hole automatic assembly method and device based on equivalent double-shaft model | |
| CN117873155A (en) | Self-adaptive impedance control method and system for air interaction under uncertain contact environment |
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 |