CN112936278B - Man-machine cooperation control method and device for robot and robot - Google Patents
Man-machine cooperation control method and device for robot and robot Download PDFInfo
- Publication number
- CN112936278B CN112936278B CN202110179249.3A CN202110179249A CN112936278B CN 112936278 B CN112936278 B CN 112936278B CN 202110179249 A CN202110179249 A CN 202110179249A CN 112936278 B CN112936278 B CN 112936278B
- Authority
- CN
- China
- Prior art keywords
- robot
- mechanical arm
- force
- load
- tail end
- 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 42
- 230000005484 gravity Effects 0.000 claims description 45
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000013016 damping Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 5
- 101100001676 Emericella variicolor andK gene Proteins 0.000 claims description 4
- 101100379080 Emericella variicolor andB gene Proteins 0.000 claims description 3
- 230000004044 response Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 239000000463 material Substances 0.000 description 6
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000003993 interaction Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 208000012260 Accidental injury Diseases 0.000 description 1
- 241001123248 Arma Species 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
- 230000006378 damage Effects 0.000 description 1
- 230000008034 disappearance Effects 0.000 description 1
- 208000014674 injury Diseases 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 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
-
- 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
- B25J18/00—Arms
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Manipulator (AREA)
Abstract
The embodiment of the application provides a human-computer cooperation control method and device of a robot and the robot, the method is applied to the robot comprising a plurality of mechanical arms, the tail end of each mechanical arm is provided with respective control modes in different motion directions, wherein an admittance force control mode is arranged in the normal direction of a load contact surface, and the method comprises the following steps: under the condition that the actual contact force of the tail end of each mechanical arm in the normal direction is controlled to reach the expected acting force according to the admittance force control mode, external force applied from the outside is detected, the position offset in the corresponding direction is calculated according to the external force component of the tail end of each mechanical arm in each moving direction and the control mode in the corresponding moving direction, and the position offset in each moving direction is used for responding to load following operation together. According to the robot, under the condition that a plurality of mechanical arms clamp the load together, the load can move along with the hands and stay at a required position, and therefore man-machine cooperation is achieved.
Description
Technical Field
The application relates to the technical field of robot control, in particular to a human-computer cooperation control method and device for a robot and the robot.
Background
Generally, when a humanoid robot appears as a service robot in a production and living scene, a scene of human-computer interaction and cooperative work inevitably exists. For example, when a child does a hand, the robot assists, for example, the child grips with both hands to pick up a manual material box on a shelf and moves to a place beside a table where the child does the hand, and when the child needs to use a certain tool in the material box, the child causes the robot to grip the material box by slightly dragging the hand of the robot gripping the material box, moves the material box to a place where the child needs to move, and holds the material box at the place or returns to an initial position, and so on.
However, the simple application scenario faces many technical challenges, for example, when using offline programmed robot position control, it is difficult to match the randomness and unpredictability of human intent actions in time and space positions in human-computer collaboration. Or, when the two arms of the robot cooperate to clamp the load, it is necessary to ensure that the load does not fall down to cause object damage or even accidental injury of people.
Disclosure of Invention
In view of the above, an object of the present application is to provide a method and an apparatus for controlling a robot in a human-machine cooperation manner, and a robot.
The embodiment of the application provides a man-machine cooperation control method of robot, is applied to the robot including a plurality of arms, and every arm end is equipped with respective control mode on different directions of motion, different directions of motion include the normal direction of load contact surface, the normal direction is equipped with admittance power accuse mode, a plurality of arms can be used for when contacting the load be in receive opposite direction's effort on the normal direction, the method includes:
under the condition that the actual contact force of the tail end of each mechanical arm in the normal direction of the load contact surface is controlled to reach the expected acting force according to the admittance force control mode, whether an external force applied to any tail end of the mechanical arm and/or the load from the outside exists or not is detected;
if external force exists, calculating the position offset in the corresponding direction according to the external force component of the tail end of each mechanical arm in each motion direction and the control mode in the corresponding motion direction, and controlling the corresponding mechanical arm to move according to the position offset of the corresponding mechanical arm in each motion direction and the initial position of the corresponding mechanical arm when the external force is applied so as to respond to load following operation.
In one embodiment, before the detecting whether there is an external force applied from the outside to any of the robot arm ends and/or the load, the method further comprises:
And under the condition of controlling the tail end of the corresponding mechanical arm to keep the actual contact force equal to the expected acting force, controlling all the mechanical arms to move to the target position according to a preset planning route.
In one embodiment, the different movement directions include a gravity direction perpendicular to a normal direction of the load contact surface, and another direction perpendicular to the gravity direction and the normal direction, respectively, the gravity direction and the another direction are respectively provided with an admittance impedance mode or an admittance dragging mode, and each mechanical arm end is provided with a corresponding preset threshold value in the gravity direction and the another direction;
calculating a positional offset of a corresponding robot arm tip in the gravity direction or the other direction, including:
and calculating the position offset of the tail end of the corresponding mechanical arm in the gravity direction or the other direction according to the difference between the external force component applied to the tail end of the corresponding mechanical arm in the gravity direction or the other direction and the corresponding preset threshold value and the control mode in the corresponding direction.
In one embodiment, in the admittance impedance mode, when it is detected that the external force component disappears for a predetermined time, the end of the mechanical arm is controlled to return to an initial position when receiving the external force component in the corresponding movement direction;
And under the admittance dragging mode, when the external force component disappears, controlling the tail end of the mechanical arm to stay at the current position in the corresponding movement direction.
In one embodiment, the control equation of the admittance force control mode is:
wherein,F f for the actual contact force fed back by the end of the robot arm in the normal direction,F d for desired action of the end of the arm in said normal directionForce;M d andB d respectively an inertia matrix and a damping matrix; Ẋ r1 And Ẍ r1 Respectively a first derivative and a second derivative of the initial position of the tail end of the mechanical arm in the normal direction; Ẋ c1 And Ẍ c1 Respectively a desired velocity and a desired acceleration of the robot arm tip in said normal direction.
In one embodiment, the governing equation of the admittance impedance mode is:
wherein,M d 、B d andK d respectively an inertia matrix, a damping matrix and a rigidity matrix,
the external force component applied to the tail end of the mechanical arm in the gravity direction or the other direction; x r2 、Ẋ r2 And Ẍ r2 Sequentially obtaining an initial position of the tail end of the mechanical arm in the gravity direction or the other direction and a first derivative and a second derivative of the initial position; x c2 、Ẋ c2 And Ẍ c2 In turn, a desired position, a desired velocity and a desired acceleration of the end of the robot arm in the direction of gravity or in the other direction.
In one embodiment, the governing equation of the admittance drag mode is:
wherein,
the external force component applied to the tail end of the mechanical arm in the gravity direction or the other direction;B d is a damping matrix; Ẋ r3 For the end of a robot armA first derivative of an initial position in the direction of gravity or the other direction; Ẋ c3 Is the desired velocity of the robot arm tip in the direction of gravity or in the other direction.
In one embodiment, each robot arm end is provided with a six-dimensional force sensor, and the pre-acquisition of the preset threshold values of each robot arm end in the gravity direction and the other direction includes:
and under the condition that the tail end of the corresponding mechanical arm is controlled to keep an actual contact force equal to the expected acting force, controlling the tail ends of all the mechanical arms to move upwards by a preset height along the Z direction of a world coordinate system, detecting contact forces of the tail ends of the corresponding mechanical arms in the gravity direction and the other direction through the six-dimensional force sensor when the tail ends of all the mechanical arms reach the preset height, and taking the contact force in the corresponding direction as the preset threshold value.
The embodiment of this application still provides a man-machine cooperation controlling means of robot, is applied to the robot including a plurality of arms, and every arm is terminal to be equipped with respective control mode in different directions of motion, different directions of motion include the normal direction of load contact surface, the normal direction is equipped with admittance power accuse mode, a plurality of arms can be used for when contact load be in produce the opposite direction's effort in the normal direction, the device includes:
The contact control module is used for controlling the actual contact force of the tail end of the corresponding mechanical arm in the normal direction along the load contact surface to reach the expected acting force according to the admittance force control mode;
the external force detection module is used for detecting whether external force applied to any mechanical arm tail end and/or the load exists or not under the condition that the actual contact force of each mechanical arm tail end in the normal direction reaches the expected acting force;
and the dragging response module is used for calculating the position offset in the corresponding direction according to the external force component received by the tail end of each mechanical arm in each motion direction and the control mode in the corresponding motion direction if external force exists, and controlling the corresponding mechanical arm to move according to the position offset of the corresponding mechanical arm in each motion direction and the initial position when the external force is received so as to respond to load following operation.
Embodiments of the present application further provide a robot, including a processor, a memory, and at least two robot arms, where the at least two robot arms are configured to cooperatively clamp a load, and the memory stores a computer program, and when the computer program is executed on the processor, the method for controlling the robot in a human-machine cooperation manner is implemented.
In one embodiment, the robot is a two-arm robot.
Embodiments of the present application also provide a readable storage medium storing a computer program that, when executed on a processor, implements the above-described human-machine cooperation control method for a robot.
The embodiment of the application has the following beneficial effects:
according to the man-machine cooperation control method of the robot, multiple control modes based on admittance control are arranged in different motion directions of the tail end of the mechanical arm, wherein the admittance force control mode for realizing force tracking is arranged in the normal direction of the contact surface of the load, and the tail ends of the mechanical arms can generate acting forces in opposite directions (namely opposite directions) in the normal direction due to different contact positions with the load, so that constant-force clamping of the load by all the mechanical arms in the normal direction can be realized, meanwhile, when the applied external force is detected, the motion control is carried out by using the control modes in the corresponding motion directions according to the external force components in the corresponding motion directions, the load can be operated by following a user under clamping, and the man-machine cooperation purpose and the like are achieved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained from the drawings without inventive effort.
Fig. 1 is a schematic flow chart illustrating a human-machine cooperation control method of a robot according to an embodiment of the present application;
fig. 2 is a schematic diagram illustrating an admittance control flow of a human-computer cooperative control method of a robot according to an embodiment of the present application;
fig. 3 is a schematic diagram illustrating force control in a normal direction of a load contact surface in the human-computer cooperation control method of the robot according to the embodiment of the present application;
fig. 4 is a schematic structural diagram illustrating a human-machine cooperation control apparatus of a robot according to an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments.
The components of the embodiments of the present application, generally described and illustrated in the figures herein, can be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present application, presented in the accompanying drawings, is not intended to limit the scope of the claimed application, but is merely representative of selected embodiments of the application. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present application without making any creative effort, shall fall within the protection scope of the present application.
Hereinafter, the terms "including", "having", and their derivatives, which may be used in various embodiments of the present application, are intended to indicate only 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.
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 the various embodiments of the present application 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.
Example 1
Fig. 1 shows a flowchart of a human-machine cooperation control method of a robot according to an embodiment of the present application. The method can be applied to the scenes of human-computer interaction, cooperative work and the like of a robot with a plurality of mechanical arms, for example, the robot can be a humanoid robot with two arms, a robot with three or more mechanical arms and the like. The mechanical arms are all mechanical arms for performing joint control according to corresponding position instructions.
In this embodiment, the human-computer cooperation control method of the robot can realize a better human-computer cooperation function by setting corresponding control modes based on admittance control in different movement directions of the tail ends of the mechanical arms, for example, when a plurality of mechanical arms clamp a heavy object, the robot can move along with a human hand to ensure that a load does not fall off, so that the safety during human-computer cooperation is improved. In addition, when different control modes are adopted, the load can stay at the position where the externally applied force/torque disappears or can automatically return to the initial position after reaching the expected position, so that the requirements under different scenes and the like are met.
The admittance control is a control mode of inputting an external force to the tail end of the mechanical arm to adjust the motion state of the tail end of the mechanical arm. For example, fig. 2 shows an admittance control system. Wherein,M d 、B d andK d sequentially establishing an inertia matrix, a damping matrix and a rigidity matrix of the impedance model, wherein F is an actual contact force fed back by the tail end of a mechanical arm of the robot and can be obtained by detecting through a force sensor arranged at the tail end of the mechanical arm; x c For desired positioning of the robot arm end of the robot in cartesian space (also called task space) Position, X r The reference position and the position offset of the tail end of the mechanical arm of the robot in the Cartesian space
=X c -X r By shifting the calculated position
And a reference position X r And the superposed input is used as a position command in a position controller, so that the control of each joint of the mechanical arm is realized.
In order to realize that the robot can clamp the load and simultaneously respond to the dragging operation (i.e. load following operation) of the user to the load, the embodiment is provided with at least two control modes for controlling the motion of the tail end of each mechanical arm in different directions. Typically, for each robot arm end of the robot, in cartesian space, a single robot arm end has three directions of motion, which in one embodiment may be selected in sequence as a normal direction of a load contact surface to be acted upon by the robot arm end, a load gravity direction and another direction perpendicular to the normal direction and the gravity direction, respectively. The load contact surface refers to a surface of a load when the end of the front robot arm is in contact with the load. For multiple robots, the load contact surface contacted by each robot is often different, for example, for a rectangular load, two robots positioned opposite each other may contact two parallel contact surfaces. It is known that when a plurality of robot arms simultaneously contact a load, the respective robot arms generate contact forces in opposite directions in the normal direction, thereby achieving stable gripping.
In addition, for convenience of calculation, the coordinate system of the force sensor may be set corresponding to the motion coordinate system of the end of the robot arm, so that the forces in three directions detected by the force sensor on the end of the robot arm are directly taken as the forces received by the end of the robot arm in three directions, for example, the normal direction corresponds to the Z direction, the gravity direction corresponds to the X direction, and the other direction corresponds to the Y direction.
Exemplary control modes of the present embodiment may include, but are not limited to, an admittance force control mode, and an admittance impedance mode and/or admittance drag mode, among others. The admittance force control mode can be used for realizing accurate force following in the corresponding direction; the control equation of the admittance impedance mode has a rigidity term, so that the initial position in the corresponding direction can be gradually returned from the position where the external force disappears after the external force disappears; the admittance drag mode may be used to achieve a stay at a position where the external force disappears in a corresponding direction after the external force disappears. Because the admittance impedance mode or the admittance dragging mode conflict, generally, the robot does not work in the admittance impedance mode and the admittance dragging mode simultaneously, and for the multifunctional robot, the admittance impedance mode or the admittance dragging mode can be selected through mode selection to work in different application scenes.
In one embodiment, the motion of the end of each robot arm in the direction normal to the load contact surface may be controlled by the admittance force control mode described above; the same control mode can be selected for the gravity direction and the other direction, such as the admittance impedance mode or the admittance drag mode, for example, but different control modes can also be selected, such as the admittance impedance mode or the admittance drag mode for the gravity direction and the position control mode for the other direction.
Next, the control method of the above three control modes will be explained. It is to be understood that the control principle of each robot arm of the robot of the present embodiment is the same, and the following control steps are described with a single robot arm as a control object.
For the admittance force control mode, the force tracking device is mainly used for realizing the force tracking of the tail end of the mechanical arm in the corresponding direction, thereby achieving the constant force control. Exemplarily, the control equation of the admittance force control mode is:
wherein,F f for the actual contact force of the end of the robot arm in the corresponding direction,F d a desired force for the end of the arm in the corresponding direction;M d andB d an inertia matrix and a damping matrix of the expected impedance model, respectively; Ẋ r1 And Ẍ r1 Respectively a first derivative and a second derivative of the initial position of the tail end of the mechanical arm in the corresponding direction; Ẋ c1 And Ẍ c1 Respectively the desired velocity and the desired acceleration of the robot arm tip in the corresponding direction. It will be appreciated that when the admittance force control mode is used to control movement of the end of the robot arm in a direction normal to the load contact surface, the corresponding direction described above is the normal direction.
The position offset between the initial position and the expected position of the mechanical arm tail end in the normal direction can be solved under the condition that the actual contact force and the expected acting force are known by the equation, and then the position movement control of the mechanical arm tail end in the normal direction is carried out according to the position offset, so that the mechanical arm tail end reaches the expected position to generate the required expected acting force on the load. It will be appreciated that since there is no stiffness term in the equation, if an external force is applied first, the end of the arm will stay in the direction where the force disappears if it is detected that the external force component disappears.
For example, the expected force required by the tip of each robot arm in the normal direction when all the robot arms collectively grip the load can be calculated from information such as the weight of the load. For a two-arm robot, as shown in fig. 3, the expected acting force is the constant force control of the two arms of the robot in the relative horizontal direction. The actual contact force, i.e. the interaction force between the end of the robot arm and the outside world, can be detected by, for example, a six-dimensional force sensor disposed at the end of the robot arm.
For the admittance impedance mode, the flexible control of the tail end of the mechanical arm in the direction different from the direction adopting the admittance force control mode is mainly realized. Exemplarily, the control equation of the admittance impedance mode is:
wherein,M d 、B d andK d respectively an inertia matrix, a damping matrix and a stiffness matrix of the desired impedance model,
external force or external force component applied to the outside in the corresponding direction can be detected by a force sensor at the tail end of the mechanical arm; x r2 、Ẋ r2 And Ẍ r2 The initial position, the first derivative (initial speed) and the second derivative (initial acceleration) of the initial position of the mechanical arm tail end in the corresponding direction are sequentially obtained; x c2 、Ẋ c2 And Ẍ c2 In turn, the desired position, desired velocity and desired acceleration of the end of the robot arm in the corresponding direction. It is understood that when the above-mentioned gravity direction and the other direction both adopt the admittance impedance mode, the gravity direction or the other direction is the above-mentioned corresponding direction.
It is to be noted that, since the stiffness term is present in the control equation, after detecting that the external force component disappears, after controlling to stop the robot arm end for a predetermined time, the controller will continue to output to control the robot arm end to return from the desired position in the other direction to the initial position at the time of starting the movement when the external force is applied in the other direction. The gravity direction is the same.
For the admittance drag mode, it is mainly used to realize the movement of the end of the robot arm in the corresponding direction different from the direction in which the admittance force control mode is employed. Exemplarily, the control equation of the admittance drag mode is:
wherein,F outer cover External force or external force component applied to the corresponding direction for the outside can be detected by a force sensor at the tail end of the mechanical arm;B d a damping matrix that is a model of the desired impedance; Ẋ r3 For the end of the arm in the corresponding directionA first derivative of the initial position of (a); Ẋ c3 The desired velocity of the end of the robot arm in the corresponding direction. Since there is no stiffness term in the governing equation, upon detection of the disappearance of the external force component, the end of the mechanical arm will stay in the direction at the position where the force disappeared.
In one embodiment, each arm of the robot employs an admittance force control mode in a direction normal to the load contact surface, while the other motion directions employ the same admittance impedance mode or admittance drag mode. Then, as shown in fig. 1, the man-machine cooperation control method of the robot will be described in detail below.
Step S110, under the condition that the actual contact force of each mechanical arm tail end in the normal direction of the load contact surface is controlled to reach the expected acting force according to the admittance force control mode, whether external force applied to any mechanical arm tail end and/or load exists is detected.
In a practical scenario, as shown in fig. 3, taking a rectangular cartridge load as an example, the load contact surfaces are two side surfaces in the horizontal direction, and the robot will perform force control in the horizontal direction on the load to be clamped to achieve stable clamping of the load in the normal direction of the load contact surface.
Exemplarily, when the actual contact force of each robot arm end to the load reaches the expected force, which is detected by the force sensor arranged at the robot arm end, it indicates that all robot arm ends of the robot are in contact with the load and the actual contact force equal to the expected force is generated at each load contact surface. It will be appreciated that the desired force will generally ensure that the load is picked up smoothly without falling off when all the arms are controlled to move upwardly.
Wherein, prior to detecting that the actual contact force reaches the desired force, the method further comprises:
and controlling the corresponding mechanical arm tail end to reach the expected position according to the initial position of the corresponding mechanical arm tail end in the normal direction of the load contact surface, the actual contact force and the expected acting force in an admittance force control mode, so that the corresponding mechanical arm tail end generates the contact force on the load.
Optionally, after the actual contact force of the tail end of the corresponding mechanical arm in the normal direction reaches the expected acting force, each mechanical arm can be controlled to maintain the actual contact force equal to the expected acting force, and all the mechanical arms are controlled to move to the target position according to the preset planned route.
For example, in an application scenario where a two-arm robot performs man-machine cooperation with a child when drawing, when the robot holds a load of a magazine from a shelf, the robot can be taken up and walk to a position where the child is located, and then can enter a waiting state to respond to a dragging operation of the child.
Step S120, if external force exists, calculating the position offset in the corresponding direction according to the external force component received by the tail end of each mechanical arm in each motion direction and the control mode in the corresponding motion direction, and controlling the corresponding mechanical arm to move according to the position offset of the corresponding mechanical arm in each motion direction and the initial position when the external force is received so as to respond to the load following operation.
Illustratively, when a user desires to drag the load, an external force may be directly applied to any of the robot arm tips and/or the load. Generally, the external force includes external force components in three movement directions, which can be detected by a force sensor mounted at the end of the mechanical arm, such as a six-dimensional force sensor, and of course, if no external force is applied in a certain direction, the external force component in the direction is 0.
In order to eliminate the influence of the friction force of the weight of the load in the gravity direction and the other direction due to the need of clamping the load to move, in this embodiment, weight thresholds are preset in the gravity direction and the other direction at the tail end of each mechanical arm, if the external force component received in the gravity direction or the other direction is greater than the preset threshold in the corresponding direction, a position offset will be generated in the corresponding direction, otherwise, the mechanical arm does not move.
In one embodiment, calculating the amount of positional offset of the end of the corresponding robot arm in the direction of gravity comprises: and calculating the position offset of the tail end of the corresponding mechanical arm in the gravity direction according to the difference value between the external force component of the tail end of the corresponding mechanical arm in the gravity direction and the weight threshold value and the control equation of the adopted admittance impedance mode or admittance dragging mode. For the other direction, the position offset in the other direction can be calculated similarly.
And the detection for obtaining the preset threshold values of each mechanical arm end in the gravity direction and the other direction can be carried out by separating the contacted load from the supporting surface. For example, all the robot arm tips may be controlled to move upward by a preset height in the Z direction of the world coordinate system while controlling the corresponding robot arm tips to maintain an actual contact force equal to the expected force, and when the preset height is reached, contact forces of the corresponding robot arm tips in the gravity direction and the other direction are detected by the force sensors disposed at the robot arm tips, and the contact force in the corresponding direction is set as the preset threshold value.
For step S120, exemplarily, for a direction in which each mechanical arm end can generate a position offset, a displacement offset amount in the corresponding direction may be calculated by using a control equation of the corresponding control mode according to an external force component in the corresponding direction and a preset threshold, and then a desired position in the corresponding direction may be determined by using the displacement offset amount and an initial position when the external force is applied. When there is displacement in only one direction, the load will translate in that direction with the external force; if the position offset is generated in a plurality of directions, the mechanical arm moves along the direction of the external force, namely along the direction of the resultant force of the external force components in three directions.
It can be understood that if each mechanical arm of the robot is also subjected to an external force component in the normal direction of the load contact surface, taking a two-arm robot as an example, since the actual contact forces of the ends of the left and right mechanical arms will also change, a corresponding position offset can be calculated by using the admittance force control mode, and the corresponding mechanical arm is subjected to force following control according to the position offset, thereby ensuring that the load cannot fall.
The man-machine cooperation control method of the robot of the embodiment controls the tail ends of all the mechanical arms in the normal direction on the contact surface where the load is clamped by using an admittance force control mode, so that constant force clamping of the load is realized; meanwhile, control modes such as an admittance impedance mode or an admittance dragging mode and the like are adopted in other two movement directions, so that the load can respond to the dragging operation of a person on the load under the condition that the load is clamped, the purpose of better man-machine cooperation is achieved, and the user experience is further improved.
Example 2
Referring to fig. 4, based on the method of embodiment 1, this embodiment provides a human-machine cooperative control apparatus 100 for a robot, which is applied to a robot including a plurality of robot arms, each robot arm end is provided with a respective control mode in a different motion direction, the different motion direction includes a normal direction of a load contact surface, the normal direction is provided with an admittance force control mode, and the plurality of robot arms can be used for generating acting forces in opposite directions in the normal direction when contacting a load. Exemplarily, the human-machine cooperation control apparatus 100 of the robot includes:
the contact control module 110 is configured to control an actual contact force of the end of the corresponding robot arm in a direction along a normal of the load contact surface to reach a desired acting force according to the admittance force control mode;
an external force detection module 120, configured to detect whether there is an external force applied from the outside to any robot arm end and/or the load when an actual contact force of each robot arm end in the normal direction reaches a desired acting force;
and the dragging response module 130 is configured to, if an external force exists, calculate a position offset in a corresponding direction according to a control mode in the corresponding movement direction based on an external force component received by the end of each mechanical arm in each movement direction, and control the corresponding mechanical arm to move according to the position offset of the corresponding mechanical arm in each movement direction and an initial position when the external force is received, so as to respond to a load following operation.
It is to be understood that the apparatus of the present embodiment corresponds to the method of embodiment 1 described above, and the alternatives of embodiment 1 described above are equally applicable to the present embodiment, and therefore, the description thereof will not be repeated.
The application further provides a robot, exemplarily comprising a processor, a memory and at least two mechanical arms, wherein the at least two mechanical arms can be used for cooperatively clamping a load, the memory stores a computer program, and the processor executes the computer program, so that the mobile terminal executes the above-mentioned human-computer cooperation control method for the robot or the functions of each module in the above-mentioned human-computer cooperation control device for the robot. For example, the robot may be a two-arm robot or the like.
The present application also provides a readable storage medium for storing the computer program used in the robot.
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 application. 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 application 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, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application or portions thereof that substantially contribute to the prior art may be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a smart phone, a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: 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 other various media capable of storing program codes.
The above description is only for the specific embodiments of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily think of the changes or substitutions within the technical scope of the present application, and shall be covered by the scope of the present application.
Claims (12)
1. A human-computer cooperative control method for a robot, the method being applied to a robot including a plurality of robot arms, each robot arm tip being provided with a respective control pattern in a different direction of movement, the different direction of movement including a normal direction of a load contact surface, the normal direction being provided with an admittance force control pattern, the plurality of robot arms being operable to receive oppositely directed forces in the normal direction when contacting a load, the method comprising:
under the condition that the actual contact force of the tail end of each mechanical arm in the normal direction of the load contact surface is controlled to reach the expected acting force according to the admittance force control mode, whether an external force applied to any tail end of the mechanical arm and/or the load from the outside exists or not is detected;
if external force exists, calculating the position offset in the corresponding direction according to the external force component of the tail end of each mechanical arm in each motion direction and the control mode in the corresponding motion direction, and controlling the corresponding mechanical arm to move according to the position offset of the corresponding mechanical arm in each motion direction and the initial position of the corresponding mechanical arm when the external force is applied so as to respond to load following operation.
2. The method of claim 1, wherein prior to said detecting the presence of external forces exerted by the environment on any of the robot arm tips and/or the load, the method further comprises:
and under the condition of controlling the tail end of the corresponding mechanical arm to keep the actual contact force equal to the expected acting force, controlling all the mechanical arms to move to the target position according to a preset planning route.
3. The method according to claim 1, wherein the different directions of motion comprise a direction of gravity perpendicular to a normal direction of the load contacting surface, and another direction perpendicular to the direction of gravity and the normal direction, respectively, the direction of gravity and the another direction being provided with an admittance impedance mode or an admittance drag mode, respectively, each robot arm tip being provided with a corresponding preset threshold in the direction of gravity and the another direction;
calculating a positional offset of a corresponding robot arm tip in the gravity direction or the other direction, including:
and calculating the position offset of the tail end of the corresponding mechanical arm in the gravity direction or the other direction according to the difference between the external force component of the tail end of the corresponding mechanical arm in the gravity direction or the other direction and the corresponding preset threshold value and the control mode in the corresponding direction.
4. A method according to claim 3, characterized by controlling the mechanical arm tip to return to the initial position when the external force component is received in the corresponding movement direction when it is detected that the external force component disappears for a predetermined time in the admittance impedance mode;
and under the admittance dragging mode, when the external force component disappears, controlling the tail end of the mechanical arm to stay at the current position in the corresponding movement direction.
5. The method of claim 3, wherein the governing equation for the admittance impedance mode is:
wherein,M d 、B d andK d respectively an inertia matrix, a damping matrix and a rigidity matrix,
the external force component applied to the tail end of the mechanical arm in the gravity direction or the other direction; x r2 、Ẋ r2 And Ẍ r2 Sequentially obtaining an initial position of the tail end of the mechanical arm in the gravity direction or the other direction and a first derivative and a second derivative of the initial position; x c2 、Ẋ c2 And Ẍ c2 In turn, a desired position, a desired velocity and a desired acceleration of the end of the robot arm in the direction of gravity or in the other direction.
6. The method of claim 3, wherein the governing equation for the admittance drag mode is:
wherein, 
The external force component applied to the tail end of the mechanical arm in the gravity direction or the other direction;B d is a damping matrix; Ẋ r3 For the arm end at said gravityA first derivative of the initial position in the direction or the further direction; Ẋ c3 Is the desired velocity of the robot arm tip in the direction of gravity or in the other direction.
7. The method of any of claims 1 to 6, wherein the governing equation for the admittance force control mode is:
wherein,F f for the actual contact force fed back by the end of the robot arm in the normal direction,F d a desired force for the end of the arm in the normal direction;M d andB d respectively an inertia matrix and a damping matrix; Ẋ r1 And Ẍ r1 Respectively a first derivative and a second derivative of the initial position of the tail end of the mechanical arm in the normal direction; Ẋ c1 And Ẍ c1 Respectively a desired velocity and a desired acceleration of the robot arm tip in said normal direction.
8. A method according to claim 3, wherein each robot arm tip is provided with a six-dimensional force sensor, the pre-acquisition of the preset threshold values of each robot arm tip in the direction of gravity and in the other direction comprising:
and under the condition that the tail end of the corresponding mechanical arm is controlled to keep an actual contact force equal to the expected acting force, controlling the tail ends of all the mechanical arms to move upwards by a preset height along the Z direction of a world coordinate system, detecting contact forces of the tail ends of the corresponding mechanical arms in the gravity direction and the other direction through the six-dimensional force sensor when the tail ends of all the mechanical arms reach the preset height, and taking the contact force in the corresponding direction as the preset threshold value.
9. A human-computer cooperative control apparatus of a robot, applied to a robot including a plurality of robot arms each having a distal end provided with respective control modes in different directions of movement including normal directions of a load contact surface, the normal directions being provided with admittance force control modes, the plurality of robot arms being operable to generate forces in opposite directions in the normal directions when contacting a load, the apparatus comprising:
the contact control module is used for controlling the actual contact force of the tail end of the corresponding mechanical arm in the normal direction along the load contact surface to reach the expected acting force according to the admittance force control mode;
the external force detection module is used for detecting whether external force applied to any mechanical arm tail end and/or the load exists or not under the condition that the actual contact force of each mechanical arm tail end in the normal direction reaches the expected acting force;
and the dragging response module is used for calculating the position offset in the corresponding direction according to the external force component received by the tail end of each mechanical arm in each motion direction and the control mode in the corresponding motion direction if external force exists, and controlling the corresponding mechanical arm to move according to the position offset of the corresponding mechanical arm in each motion direction and the initial position when the external force is received so as to respond to load following operation.
10. A robot, characterized by comprising a processor, a memory and at least two robot arms for cooperatively gripping a load, the memory storing a computer program which, when executed on the processor, implements the human-machine cooperative control method of a robot according to any one of claims 1-8.
11. A robot as claimed in claim 10, characterized in that the robot is a two-arm robot.
12. A readable storage medium, characterized in that it stores a computer program which, when executed on a processor, implements a human-machine cooperation control method of a robot according to any one of claims 1-8.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110179249.3A CN112936278B (en) | 2021-02-07 | 2021-02-07 | Man-machine cooperation control method and device for robot and robot |
| PCT/CN2021/132999 WO2022166329A1 (en) | 2021-02-07 | 2021-11-25 | Human-robot collaboration control method and device for robot, and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110179249.3A CN112936278B (en) | 2021-02-07 | 2021-02-07 | Man-machine cooperation control method and device for robot and robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112936278A CN112936278A (en) | 2021-06-11 |
| CN112936278B true CN112936278B (en) | 2022-07-29 |
Family
ID=76244955
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110179249.3A Active CN112936278B (en) | 2021-02-07 | 2021-02-07 | Man-machine cooperation control method and device for robot and robot |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN112936278B (en) |
| WO (1) | WO2022166329A1 (en) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112936278B (en) * | 2021-02-07 | 2022-07-29 | 深圳市优必选科技股份有限公司 | Man-machine cooperation control method and device for robot and robot |
| CN113499094B (en) * | 2021-07-08 | 2023-07-25 | 中山大学 | Heart color Doppler ultrasound examination device and method guided by vision and force feedback |
| CN113855474B (en) * | 2021-08-25 | 2023-10-31 | 上海傅利叶智能科技有限公司 | Method and device for controlling two rehabilitation robots and rehabilitation robot system |
| CN113510713B (en) * | 2021-09-09 | 2021-12-31 | 深圳市优必选科技股份有限公司 | Control method and device for robot cooperative transportation and computer equipment |
| CN114083538B (en) * | 2021-11-30 | 2023-06-20 | 深圳市优必选科技股份有限公司 | Method, device and robot for realizing stable clamping of double mechanical arms |
| CN116079712B (en) * | 2022-12-01 | 2024-08-06 | 杭州电子科技大学 | Three-dimensional space clamping height control method of mechanical arm |
| CN115946120B (en) * | 2023-01-09 | 2024-02-13 | 上海艾利特机器人有限公司 | Mechanical arm control method, device, equipment and medium |
| CN115922733B (en) * | 2023-01-31 | 2024-06-11 | 北京理工大学 | Man-machine sharing control method for robot for hard bone tissue operation |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2011115287A1 (en) * | 2010-03-15 | 2011-09-22 | 学校法人立命館 | Master/slave system and method for controlling same |
| CN103600354A (en) * | 2013-11-08 | 2014-02-26 | 北京卫星环境工程研究所 | Spacecraft mechanical arm flexible follow-up control gravity compensation method |
| CN103640022A (en) * | 2013-11-13 | 2014-03-19 | 北京卫星环境工程研究所 | Flexible follow-up control method for spacecraft mechanical arm |
| CN107053179A (en) * | 2017-04-21 | 2017-08-18 | 哈尔滨思哲睿智能医疗设备有限公司 | A kind of mechanical arm Compliant Force Control method based on Fuzzy Reinforcement Learning |
| CN108789363A (en) * | 2018-05-25 | 2018-11-13 | 雅客智慧（北京）科技有限公司 | It is a kind of that teaching system and method are directly dragged based on force snesor |
| CN109223444A (en) * | 2018-10-23 | 2019-01-18 | 上海电气集团股份有限公司 | Healing robot and its man-machine coordination interaction force control method |
| CN208409885U (en) * | 2018-06-27 | 2019-01-22 | 西华大学 | Heterogeneous remote control system based on multiple robots |
| CN110605721A (en) * | 2019-10-24 | 2019-12-24 | 苏州艾利特机器人有限公司 | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor |
| CN111890372A (en) * | 2020-09-29 | 2020-11-06 | 江西明天高科技股份有限公司 | Control method of follow-up mechanical arm |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9308645B2 (en) * | 2012-03-21 | 2016-04-12 | GM Global Technology Operations LLC | Method of inferring intentions of an operator to move a robotic system |
| DE102018200864B3 (en) * | 2018-01-19 | 2019-02-07 | Kuka Deutschland Gmbh | Method and system for controlling a robot |
| CN109249394B (en) * | 2018-09-27 | 2022-04-15 | 上海电气集团股份有限公司 | Robot control method and system based on admittance control algorithm |
| CN110394802B (en) * | 2019-08-06 | 2021-03-30 | 前元运立(北京)机器人智能科技有限公司 | Polishing robot and position compensation method |
| CN112936278B (en) * | 2021-02-07 | 2022-07-29 | 深圳市优必选科技股份有限公司 | Man-machine cooperation control method and device for robot and robot |
-
2021
- 2021-02-07 CN CN202110179249.3A patent/CN112936278B/en active Active
- 2021-11-25 WO PCT/CN2021/132999 patent/WO2022166329A1/en active Application Filing
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2011115287A1 (en) * | 2010-03-15 | 2011-09-22 | 学校法人立命館 | Master/slave system and method for controlling same |
| CN103600354A (en) * | 2013-11-08 | 2014-02-26 | 北京卫星环境工程研究所 | Spacecraft mechanical arm flexible follow-up control gravity compensation method |
| CN103640022A (en) * | 2013-11-13 | 2014-03-19 | 北京卫星环境工程研究所 | Flexible follow-up control method for spacecraft mechanical arm |
| CN107053179A (en) * | 2017-04-21 | 2017-08-18 | 哈尔滨思哲睿智能医疗设备有限公司 | A kind of mechanical arm Compliant Force Control method based on Fuzzy Reinforcement Learning |
| CN108789363A (en) * | 2018-05-25 | 2018-11-13 | 雅客智慧（北京）科技有限公司 | It is a kind of that teaching system and method are directly dragged based on force snesor |
| CN208409885U (en) * | 2018-06-27 | 2019-01-22 | 西华大学 | Heterogeneous remote control system based on multiple robots |
| CN109223444A (en) * | 2018-10-23 | 2019-01-18 | 上海电气集团股份有限公司 | Healing robot and its man-machine coordination interaction force control method |
| CN110605721A (en) * | 2019-10-24 | 2019-12-24 | 苏州艾利特机器人有限公司 | Mechanical arm dragging teaching method based on terminal six-dimensional force sensor |
| CN111890372A (en) * | 2020-09-29 | 2020-11-06 | 江西明天高科技股份有限公司 | Control method of follow-up mechanical arm |
Non-Patent Citations (1)
| Title |
|---|
| 基于力信息的人机协调运动控制方法;李军强等;《计算机集成制造系统》;20180815(第08期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2022166329A1 (en) | 2022-08-11 |
| CN112936278A (en) | 2021-06-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112936278B (en) | Man-machine cooperation control method and device for robot and robot | |
| CN108621156B (en) | Robot control device, robot system, robot, and robot control method | |
| US11090814B2 (en) | Robot control method | |
| CN109070365B (en) | Object operating device and object operating method | |
| US20190036337A1 (en) | System for robotic 3d printing | |
| US11040449B2 (en) | Robot control system and method of controlling a robot | |
| JP5338297B2 (en) | Robot control device | |
| EP2939797A2 (en) | Motion limiting device and motion limiting method | |
| EP3147087A1 (en) | Robot apparatus, robot controlling method, program, recording medium, and assembly manufacturing method | |
| CN110303499B (en) | Mechanical arm homing method and device | |
| Farkhatdinov et al. | Improving mobile robot bilateral teleoperation by introducing variable force feedback gain | |
| CN111360851B (en) | Hybrid servo control device and method for robot integrating touch and vision | |
| KR101307782B1 (en) | Direct teaching and playback method of robot and robot control system | |
| He et al. | Bidirectional human–robot bimanual handover of big planar object with vertical posture | |
| JP2023086924A (en) | Control method, control system and program | |
| US10035264B1 (en) | Real time robot implementation of state machine | |
| CN113814985B (en) | Robot control method, control cabinet and system | |
| CN112894827B (en) | Method, system and device for controlling motion of mechanical arm and readable storage medium | |
| Katzschmann et al. | Towards online trajectory generation considering robot dynamics and torque limits | |
| US12030192B2 (en) | Motion model calculation device, control device, joint mechanism, and motion model calculation method | |
| JP6322948B2 (en) | Robot control apparatus, robot system, robot, robot control method, and program | |
| Chen et al. | Uncertainty-aware arm-base coordinated grasping strategies for mobile manipulation | |
| CN115113517A (en) | Bilateral force feedback method, device, equipment and medium | |
| KR20220116289A (en) | Creating control programs for robot manipulators | |
| JP6314410B2 (en) | Robot control apparatus, robot system, robot, robot control method, and program |
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 |