JP4635971B2 - External force detection method, manipulator control method, and manipulator control system - Google Patents

Description

  The present invention relates to an external force detection method for calculating the magnitude of an external force acting on the manipulator while a manipulator used mainly for industrial use is working, a manipulator control method using the external force detection method, and a manipulator control About the system.

  In general, in a production site such as a factory, a manipulator including a hand unit and a robot arm is used. Such a manipulator is used for gripping an object, moving the object from one place to another, or changing the direction in which the gripped object is placed. An operation according to a predetermined pattern is performed by a program stored in the section.

  The above-described manipulator is required to perform a predetermined operation during normal use. However, if an abnormality occurs, it may be necessary to perform an operation different from the operation according to the predetermined pattern. is there. For example, if there is an object that interferes with the arm that constitutes the manipulator during the operation of the manipulator, the arm stops movement or avoids interference with the object. Have to do.

Therefore, in order to avoid interference with such an object, it is conceivable to provide a detection means such as a contact sensor and a force detection means such as a force sensor on the entire surface of the arm part constituting the manipulator (for example, Patent Document 1). The manipulator configured as described above can detect the presence of an object that interferes with the surface of the arm unit, and can further calculate the external force applied to the arm unit by the interfered object.
JP 2001-38664 A

  As described above, in order to detect whether or not object interference has occurred in the manipulator, it is necessary to configure a structure such as covering the entire surface of the arm portion with a cover portion having force detection means. However, the arm part that constitutes the manipulator generally includes an articulated drive part in many cases, and constitutes a structure in which a cover part is provided so as to cover the entire surface of the structure including such a joint part. Is often difficult.

  The present invention has been made to solve such a problem, and can detect an external force acting on an arm portion by causing an object to interfere with the manipulator without covering the entire surface of the manipulator with a sensor portion. It is an object of the present invention to provide an external force detection method, a manipulator control method using the external force detection method, and a manipulator control system.

  The external force detection method according to the present invention is a manipulator including an arm portion composed of a plurality of link members including a fixed link member attached to a fixed surface, and a hand portion fixed to the arm portion, wherein the fixed link A first detector that is attached to the member and detects the magnitude of a first acting force that acts on the attachment location, and a second action force that is attached to the hand portion and acts on the attachment location. A second detection unit for detecting the position, a position and orientation calculation unit for calculating a position and orientation of the first detection unit and the second detection unit, and the mass of each of the fixed link member and the hand unit; Then, from the inertial force generated by each operation, an estimated value of the acting force acting on the attachment location of the first detection unit and the attachment location of the second detection unit is obtained, respectively, and the first detection unit and the first detection unit The estimated value is subtracted from the detection value detected by the first detection unit, and the first acting force acting on the attachment location of the first detection portion and the second action acting on the attachment location of the second detection portion. When the external force is not acting on the arm portion based on the position and orientation of the first detection unit and the second detection unit when the correction values of the applied force are respectively obtained and detected. It is determined whether or not the determined correction value satisfies a predetermined relationship between a determined acting force acting on the attachment location in the fixed link member and acting force acting on the attachment location in the hand portion. When these correction values are out of the predetermined relationship, it is determined that an external force is applied to the arm portion.

  When the external force is not applied to the arm portion at the location where the acting force acts on the fixed link member and the location where the acting force acts on the hand portion, the external force detection method It is used to satisfy a predetermined relationship between acting forces. That is, when such a predetermined relationship is not satisfied, it can be detected that an external force is acting on the arm portion without covering the entire arm portion with the sensor portion.

  Further, in such an external force detection method, when the hand unit is gripping an object and the gripping position (center of gravity position) of the object gripped by the hand unit is known, the gripped object, the hand unit, The mass and inertial force combined with

  Furthermore, the present invention proposes to control the operation of the manipulator using the above-described external force detection method. That is, an arm part composed of a plurality of link members including a fixed link member attached to a fixed surface, a hand part fixed to the arm part, and a drive part for driving the arm part and the hand part, A manipulator control method for controlling a manipulator provided with a magnitude of a first acting force acting on a predetermined location of the fixed link member and a magnitude of a second acting force acting on a predetermined location of the hand part An action force detection step for detecting the position, a position and orientation calculation step for calculating a position and orientation of a predetermined location where the first action force and the second action force act on the fixed link member and the hand part, respectively, and the fixing From the respective masses of the link member and the hand part and the inertial force generated by the respective operations, the predetermined position of the fixed link member and the hand part. The magnitude of the acting force obtained in the acting force estimation step is subtracted from the magnitude of the acting force detected in the acting force detection step and the acting force estimation step for estimating the magnitude of the acting force. Based on the action force correction step for correcting the magnitude of the acting force acting on the predetermined portion of the fixed link member and the hand portion, and the position and orientation of the fixed link member and the hand member at which the acting force is detected. A predetermined relationship between the acting force acting on the predetermined portion of the fixed link member and the acting force acting on the predetermined portion of the hand portion, which is determined when no external force is acting on the arm portion, A determination step for determining whether or not the value of the corrected applied force obtained by the force correction step is satisfied. When it is determined that the corrected applied force value is out of the predetermined relationship, it is determined that an external force is acting on the arm unit, and the movement of the arm unit by the drive unit is stopped. It is characterized by.

  According to such a manipulator control method, when an external force is detected in the arm portion, the manipulator can be controlled so as to stop the movement of the manipulator, so that damage to the object due to interference of the manipulator can be avoided.

  In such a manipulator control method, the degree that the aforementioned acting force does not satisfy a predetermined relationship may be obtained, and it may be determined whether or not to stop the movement of the arm unit based on the degree.

  The present invention also provides a manipulator control system for executing the manipulator control method as described above. That is, as described above, an arm portion composed of a plurality of link members including a fixed link member attached to a fixed surface, a hand portion fixed to the arm portion, and the arm portion and the hand portion are driven. A manipulator control system comprising a manipulator having a drive unit and a control unit for controlling the manipulator, wherein the magnitude of the first acting force acting on a predetermined location of the fixed link member, An acting force detector that detects the magnitude of a second acting force acting on a predetermined location; a position and orientation calculator that calculates the position and orientation of the location where the acting force is applied in the fixed link member and the hand portion; and An action acting on a predetermined portion of the fixed link member from the mass of each of the fixed link member and the hand portion and the inertial force generated by each operation. The magnitude of the action force obtained by the action force estimation section from the force, the action force estimation section for estimating the action force acting on the hand section, and the magnitude of the action force detected by the action force detection section And a fixed force member that corrects the magnitudes of the first acting force and the second acting force that act on the predetermined portion of the fixed link member and the hand portion, and the fixed link that detects the acting force. Based on the position and orientation of the predetermined location of the member and the hand member, the acting force acting on the predetermined location in the fixed link member and the acting force on the predetermined location in the hand portion are determined when no external force is applied to the arm portion. A determination unit that determines whether or not the corrected value of the applied force obtained in the applied force correction step satisfies a predetermined relationship with the applied force. The value of the acting force after correction, if it is determined not to satisfy the predetermined relationship, by the control unit, is characterized by stopping the movement of the arm portion by the drive unit.

  Also in such a manipulator control system, when an external force is detected in the arm portion, the manipulator can be controlled so as to stop the movement of the manipulator, so that damage to the object due to the interference of the manipulator can be avoided.

  As described above, according to the present invention, it is possible to detect an external force acting on the arm portion when an object interferes with the manipulator without covering the entire surface of the manipulator with the sensor portion.

Embodiment 1 of the Invention
The manipulator control system according to the first embodiment of the present invention will be described below with reference to FIGS. 1 to 3. In this embodiment, an example is shown in which a manipulator is fixed to a floor surface as a fixed surface, and the manipulator is driven by a drive unit.

  FIG. 1 is a schematic view schematically showing a manipulator control system 1 including a manipulator 20 fixed to a floor surface P and a control unit 10 for controlling the manipulator. In the manipulator control system 1, the manipulator 20 includes an arm portion 22 having one end fixed to the floor surface P, and a robot hand 30 as a hand portion attached to the other end of the arm portion 22.

  The manipulator 2 includes a fixed link member 21 including a fixed portion 211 fixed to the floor surface P, a rotating portion 212 that is rotatably driven with respect to the fixed portion 211, a first link member 221 and a first link member 221. An arm portion 22 including a two-link member 222 and a robot hand 23 are provided. The fixed portion 211 is a fixed shaft embedded in the floor surface P, and the rotating portion 212 connected to the fixed shaft so as to be rotatable is connected to the fixed portion 211 by a servo motor as a driving portion (not shown). Drive to take a motion with one degree of freedom.

  The first link member 221 and the second link member 222 of the arm portion 22 are pivotably connected so as to have one degree of freedom, and are driven by a servo motor as a drive portion (not shown) like the pivot portion 212. Is done. Further, the rotating part 211 and the first link member 221 operate integrally by being connected via the shaft member 24. That is, the first link member 221 also rotates together with the rotation unit 212 as the rotation unit 212 rotates with respect to the fixed unit 211. As described above, the rotating portion included in the manipulator 2 acts as a joint portion in the manipulator 2.

  The robot hand 23 has a configuration in which an object is gripped by a pair of plate-shaped gripping members being relatively close to and separated from each other by a motor (not shown). It is fixed. The movement of the robot hand 23 for gripping an object is controlled independently of the movement of the arm unit 22, but may be controlled according to the movement of the arm unit 22.

  Further, the shaft members 24 and 25 include, for example, a first sensor 31 serving as a second detection unit, a first sensor 31 capable of transmitting a detection signal indicating the magnitude of the received force, and the Two sensors 32 are attached, and the magnitude of the force acting on the attachment location of each shaft member is detected. The first sensor 31 and the second sensor 32 not only detect the magnitude of the force acting on the shaft member, but also will be described later based on the length of each attached link member and the amount of driving the link member. The position and orientation calculation unit calculates its own relative position and orientation.

  The control unit 10 is a computer having an arithmetic processing unit such as a so-called CPU, and controls the operation of the manipulator 20 according to a predetermined program. Specifically, the control unit 10 includes a motor control unit 11 that controls driving of a servo motor provided in the manipulator, an action force detection unit 12 that detects action force acting on the fixed link member 21 and the robot hand 23, and action force. The position and orientation calculation unit 13 that detects the position and orientation of the first sensor 31 and the second sensor 32 attached to the fixed link member 21 and the robot hand 23, the position and orientation detected by the position and orientation calculation unit 13, etc. The action force estimation unit 14 that estimates the action force from the information, the action force correction unit 15 that corrects the value of the detected action force, and the determination that determines whether or not the corrected action force satisfies a predetermined relationship Part 16. Hereinafter, it demonstrates in detail using FIG.

  The motor control unit 11 provided in the control unit 10 is a servo controller provided with a program for controlling the drive of a servo motor for driving each link member, arm unit, hand unit and the like provided in the manipulator 20. is there. A program for controlling the motor drive is stored in a storage area (not shown) in the control unit 10 and causes the manipulator 20 to perform a desired operation.

The acting force detection unit 12 receives detection signals from the first sensor 31 and the second sensor 32 and analyzes the received detection signal to analyze the fixed link member 211 and the first sensor 31 and the second sensor 32 in the robot hand 23. The acting forces F ₁ and F ₂ acting on the location where the are attached are calculated.

The position and orientation calculation unit 13 determines the position and orientation S ₁ and S ₂ of the first sensor 31 and the second sensor 32 when the first sensor 31 and the second sensor 32 detect the acting forces F ₁ and F _2. calculate. These positions and orientations are obtained from the length of each attached link member, the amount driven, and the like.

  The acting force estimation unit 14 acts on a portion of the fixed link member 211 and the robot hand 23 where the first sensor 31 and the second sensor 32 are attached separately from the acting force calculated by the acting force detection unit 12. The acting force acting by the influence of the inertial force and the like is estimated. These acting forces are obtained using a known function that is obtained from the motor control unit 11 and is specified by three parameters of an angle, an angular velocity, and an angular acceleration of the rotating joint.

  The acting force correction unit 15 subtracts the acting force obtained by the estimation from the acting force calculated based on the detection values obtained from the first sensor 31 and the second sensor 32, and the fixed link member 211, the robot The value of the acting force acting on each hand 23 is corrected.

The determination unit 16 determines whether or not the acting forces F ₁ ′ and F ₂ ′ acting on the fixed link member 211 and the robot hand 23 obtained by being corrected by the acting force correcting unit 15 satisfy a predetermined relationship. Determine whether. Such a predetermined relationship is determined by a predetermined Jacobian matrix J that associates the position / orientation S ₁ , S ₂ calculated by the position / orientation calculation unit 13 with the moving speed at which each sensor unit moves. In other words, the transposed matrix J ^T of the Jacobian matrix J described above, when the manipulator is operated without being a force from the outside, associates an action force acting on the attachment point of each sensor unit in a predetermined relationship Is. Accordingly, the acting force F ₁ of the corrected ', F _2', when expressed in a known relationship with the transposed matrix J ^T, a force from the outside to the manipulator (external force) is not acting I can say no. Conversely, if it cannot be expressed by a known relational expression, it can be said that an external force is acting on the manipulator.

  In the manipulator control system 1 configured as described above, a procedure for detecting an external force applied to the arm unit 22 and controlling the movement of the manipulator 20 will be described with reference to FIG.

  The operation of the manipulator 20 is controlled by a motor control unit 11 provided in the control unit 10 according to a predetermined program, and a desired operation is performed (STEP 101). At that time, the first sensor 31 attached to the shaft member 24 that connects the arm portion 22 and the fixed link member 211 and the first sensor 25 attached to the shaft member 25 that connects the robot hand 23 and the second link member 25. The signals detected by the two sensors 32 are transmitted to the acting force detection unit at minute time intervals (STEP 102).

The acting force detector 12 receives the detection signals transmitted at such a minute time interval and calculates acting forces F ₁ and F ₂ acting on the fixed link member 211 and the robot hand 23, respectively. On the other hand, the position / orientation calculation unit 13 calculates the positions / orientations S ₁ and S ₂ of the first sensor unit 31 and the second sensor unit 32 when the detection signal is received. Then, these calculated values are transmitted to the acting force estimation unit 14 (STEP 103).

Next, the acting force estimation unit 14 receives parameters such as the driving amount (angle) and driving speed (angular velocity) of each joint from the motor control unit 11, and calculates a known function specified by these parameters. By using this, the acting forces g ₁ and g ₂ acting on the fixed link member 211 and predetermined locations (shaft members 24 and 25) in the robot hand 23 are obtained (STEP 203). Then, the acting force correction unit 15 subtracts these acting forces g ₁ and g ₂ from the values of the acting forces F ₁ and F 2 transmitted from the acting force detection unit 12, and sends them to the fixed link member 211 and the robot hand 23. Correction values F ₁ ′ and F ₂ ′ for the acting force to be applied are calculated (STEP 104).

In the determination unit 16, the relationship between the acting force f ₁ acting on the fixed link member and the acting force f ₂ acting on the hand portion, which is determined when no external force acts on the arm portion 22, using a transposed matrix J ^T of the matrix J, for example, it is defined as shown in equation 1 below.
f ₂ = J ^T · f ₁ Formula 1

Therefore, the determination unit 15 was calculated by the position and orientation calculation unit 13, the position and orientation S ₂ of the position and orientation S ₁ and the second sensor 32 of the first sensor _31, and was calculated from the acting force estimating unit 14, the fixed By substituting the acting force F ₁ ′ acting on a predetermined position (shaft member 24) of the link member 211 into f _{1 of the} above formula 1, the predetermined position (shaft member 25) of the robot hand 23 when no external force is applied. acting force F ₂ acting on the _"to calculate the (STEP 105). in this manner was calculated, action force F ₂ acting on a predetermined location of the robot hand _23", the correction value calculated at the working force correction section 15 F ₂ 'is compared with the determination unit 16 (STEP 106), if the difference is greater than a predetermined threshold ΔF is determined that the external force is acting on the arm portion 22 (STEP 10 ), And to stop the movement of the manipulator 20 by the motor control unit 11 (STEP 108). On the contrary, when the difference between the calculated applied force F ₂ ″ and the correction value F ₂ ′ is smaller than ΔF, the external force is not applied to the arm portion 22 or the applied external force is considered. It is determined that the power is not good, and the control of the normal manipulator 20 is continued, and the procedure returns to STEP 103 where the signals detected by the first sensor 31 and the second sensor 32 attached to the manipulator are received again, and the same procedure is performed. repeat.

With such control, according to the manipulator control system in the present embodiment,
When an external force of a certain value or more is applied to the arm portion 22 of the manipulator 20, the external force is detected and the movement of the manipulator is stopped without covering the entire arm portion including the driven joint portion in the manipulator. it can.

  In the above-described embodiment, the movement of the manipulator is stopped when an external force of a certain level or more is applied to the manipulator (arm portion), but the present invention is not limited to this. That is, at the moment when it is detected that an external force is applied to the manipulator, a warning sound or the like may be output to inform surrounding workers that the manipulator is interfering with an external object. . Or you may perform drive control of the manipulator based on the magnitude | size of the acting external force, such as changing the speed of the movement of the manipulator so that it may be proportional to the magnitude | size of the acting external force.

  Furthermore, in the above-mentioned embodiment, although the example regarding the manipulator attached to the fixed floor surface is demonstrated, the manipulator system which can be moved by making the side surface of the movable body movable as a fixed surface, for example It is also possible to apply to. That is, the external force detection method and the manipulator control method according to the present invention can be applied to the arm portion used in the humanoid robot.

It is the schematic which shows roughly the whole structure of the manipulator control system which concerns on 1st Embodiment. It is a block diagram which shows notionally the internal structure of the control part contained in the manipulator control system shown in FIG. 2 is a flowchart showing a procedure for detecting an external force acting on a manipulator in the manipulator control system shown in FIG. 1.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 ... Manipulator control system 20 ... Manipulator 10 ... Control part 12 ... Action force detection part 13 ... Position and orientation calculation part 14 ... Action force estimation part 15 ... Action force correction part 16 ... Determination part 21 ... Fixed link member 22 ... Arm part 221 ... First link member 222 ... Second link member 23 ... Hand part (robot hand)
31 ... 1st sensor (1st detection part)
32 ... 2nd sensor (2nd detection part)
P: Fixed surface (floor surface)

Claims (4)

In a manipulator comprising: an arm part composed of a plurality of link members including a fixed link member attached to a fixed surface; and a hand part fixed to the arm part.
A first detector that is attached to the fixed link member and detects the magnitude of a first acting force acting on the attachment location;
A second detection unit that is attached to the hand unit and detects the magnitude of a second acting force acting on the attachment location;
A position and orientation calculation unit that calculates the position and orientation of the first detection unit and the second detection unit;
Due to the influence of the own weight and inertial force when no external force is applied from the own weight of each of the fixed link member, the arm part and the hand part and the inertial force generated by each operation , the first detection unit Each of the estimated values of the acting force acting on the attachment location of the second detection portion and the attachment location of the second detection unit is obtained,
A first acting force acting on an attachment location of the first detection unit by subtracting the estimated value from detection values detected by the first detection unit and the second detection unit, and a second detection Each of the correction values of the second acting force acting on the attachment location of the part is obtained,
The attachment location on the fixed link member determined when no external force is applied to the arm portion based on the position and orientation of the first detection portion and the second detection portion when these acting forces are detected. It is determined whether or not the determined correction value satisfies a predetermined relationship between the acting force acting on the attachment portion and the acting force acting on the attachment portion in the hand portion, and these correction values are the predetermined relationship. An external force detection method characterized by determining that an external force has acted on the arm portion when the arm is off. A manipulator comprising: an arm part composed of a plurality of link members including a fixed link member attached to a fixed surface; a hand part fixed to the arm part; and a drive part for driving the arm part and the hand part. A manipulator control method for controlling
An acting force detecting step for detecting a magnitude of a first acting force acting on a predetermined location of the fixed link member and a magnitude of a second acting force acting on a prescribed location of the hand portion;
A position and orientation calculation step of calculating the position and orientation of the predetermined location where the first acting force and the second acting force act on the fixed link member and the hand part;
The fixed link members, with each of its own weight of the arm and the hand portion, from the inertial force generated by each of the operation, in the case where no external force is applied, due to the influence of the own weight and the inertial force, the fixing link member and the hand An action force estimation step for estimating the magnitude of the action force acting on the predetermined portion of the part,
The acting force acting on the predetermined location in the fixed link member and the hand portion by subtracting the magnitude of the acting force obtained in the acting force estimation step from the magnitude of the acting force detected in the acting force detection step. An action force correction step for correcting the magnitude of
Based on the positions and orientations of the fixed link member and the predetermined position of the hand member that detected the applied force, the applied force acting on the predetermined position of the fixed link member determined when no external force is applied to the arm portion; A determination step of determining whether or not a value of the corrected applied force obtained by the applied force correction step satisfies a predetermined relationship with the applied force acting on the predetermined portion in the hand unit. And
When it is determined by the determination step that the corrected value of the applied force is out of the predetermined relationship, it is determined that an external force is applied to the arm unit, and the arm of the arm unit by the drive unit is determined. A manipulator control method characterized by stopping movement.   The degree to which the corrected value of the acting force deviates from a predetermined relationship is calculated, and the movement of the arm unit is stopped only when the degree exceeds a predetermined threshold value. The manipulator control method described in 1. A manipulator having an arm part composed of a plurality of link members including a fixed link member attached to a fixed surface, a hand part fixed to the arm part, and a drive part for driving the arm part and the hand part; A manipulator control system comprising a control unit for controlling the manipulator,
An acting force detector that detects a magnitude of a first acting force acting on a predetermined location of the fixed link member and a magnitude of a second acting force acting on a prescribed location of the hand portion;
In the fixed link member and the hand unit, a position and orientation calculation unit that calculates the position and orientation of the place where the acting force is applied, and
The fixed link members, with each of its own weight of the arm and the hand portion, from the inertial force generated by each of the operation, in the case where no external force is applied, due to the influence of the own weight and the inertial force, given the fixed link member An acting force acting on the location, an acting force estimating portion for estimating an acting force acting on the hand portion,
A first acting on the fixed link member and the predetermined portion of the hand portion by subtracting the magnitude of the acting force obtained by the acting force estimating section from the magnitude of the acting force detected by the acting force detecting section. An action force correction unit that corrects the magnitude of the action force and the second action force;
Based on the positions and orientations of the fixed link member and the predetermined position of the hand member that detected the applied force, the applied force acting on the predetermined position of the fixed link member determined when no external force is applied to the arm portion; A determination unit that determines whether or not a value of the corrected applied force obtained by the applied force correction step satisfies a predetermined relationship with the applied force acting on the predetermined portion in the hand unit. And
When the determination unit determines that the corrected force value does not satisfy the predetermined relationship, the control unit stops the movement of the arm unit by the drive unit. Manipulator control system.

Images