JP2709001B2 - Work Object Position Detector for Force Control Robot - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a position detecting device for a work object, and more particularly, to a work object functioning as a touch sensor incorporated in a position and force control device for controlling the operation of a multi-degree-of-freedom work machine. Related to a position detecting device.

[0002]

2. Description of the Related Art When performing a desired operation on a work object based on position and force control with a multi-degree-of-freedom work machine such as an industrial robot, errors caused by positioning of the work object,
It is necessary to correct position data by data processing in a control device that detects errors due to various deformations and controls position and force. For this reason, it is necessary to provide a position detecting device for accurately detecting the position of the work target.

Conventionally, limit switches, optical or ultrasonic distance sensors, visual recognition devices, and the like have been used as position detection devices for detecting the position of a work object. These position detection devices are configured as dedicated detection devices that send detection signals to the control device. That is, a special dedicated device other than the control device is required.

[0004] Further, when a dedicated device is provided, a sensor or a camera is attached to a peripheral portion of the hand effector.
Get in the way of work.

In addition, it is required that the distance between the hand effector and the sensor or the like and the mounting direction of the sensor or the like be accurately set. If this condition is not satisfied, a deviation occurs between the position of the work target detected by the sensor and the position where the work is performed by the hand effector.

As a method for solving such a problem, Japanese Patent Laid-Open No. Sho 61-170805 discloses a configuration in which a force control function included in a position / force control device of an industrial robot is used to detect the position of a work object by the robot itself. No. was proposed. The configuration disclosed in this document has a speed control loop and a current control loop, and when approaching the work object and coming into contact with the work object by the action of the speed control loop, the force control is simulated by the action of the current control loop. Do. At this time, the contact state is detected by detecting that the value of the position detector does not change, and the position of the work target is detected by reading the detection value of the position detector at that time.

[0007]

In the conventional industrial robot technology, a force sensor is not used in the force control, and the force control is performed in a simulated manner using current control. That is, the torque of the motor is controlled based on the current control, and the force applied to the hand effector is not controlled while a force sensor is attached to the end of the robot arm to detect the force. For this reason, the force at the contact point between the hand effector and the work target cannot be accurately controlled.

In other words, according to the prior art, it is difficult to freely change the contact force or obtain an optimum contact force in the state of contact between the hand effector and the work object. In general, the hand portion of the robot undergoes bending deformation in the contact state, and therefore, there is a problem that the position of the work target cannot be accurately detected unless the contact force of the hand portion is accurately grasped. .

In the above-mentioned prior art, the contact state is detected by the fact that the output value of the position detector does not change. However, it takes a certain time to know that the output value has stopped changing. Need. In particular, there is a disadvantage that when the bending occurs at the hand portion of the robot, it takes time, and the detection value differs depending on which time point is the contact time point.

SUMMARY OF THE INVENTION It is an object of the present invention to provide a force control robot for detecting a position of a work object by actual contact with a hand of the robot and quickly detecting an accurate position based on an accurate contact force. To provide a position detecting device.

[0011]

An apparatus for detecting the position of a work object in a force control robot according to the present invention is configured as follows.

A force control robot to which the work object position detecting apparatus according to the present invention is applied is a force detecting means for detecting at least one axial force applied to a hand effector, and a position detecting means for detecting a position of the hand effector. Means and a position and force control means for executing at least a force control mode in which the moving speed of the hand effector is determined by a deviation between a preset force target value and a force detection value detected by the force detection means. The configuration of the position detecting device is added to the position and force control means. The position detecting device includes: a contact operation executing unit that moves the hand effector in the force control mode and contacts the work object when the hand effector is in a non-contact state; and a magnitude of the force detected by the force detecting unit. When the force exceeds a preset force, the contact effector and the contact determination means for determining that the work object has contacted, and when the contact determination means determines that the contact state, it is detected by the position detection means at the time of contact And contact position storage means for storing data relating to the determined position.

In the above-mentioned configuration, preferably, when the hand effector is in contact with the work target, a detachment operation executing means for moving the hand effector from the contact state to the non-contact state in the force control mode. Is provided.

In each of the above structures, preferably, there is provided a position correcting means for correcting the position data detected by the position detecting means, excluding a deflection amount element of the robot hand, based on the force data detected by the force detecting means. .

[0015]

According to the work object position detecting device of the present invention, in a force control robot, if a force target value is set in a contact direction using a force control mode, a reaction force is applied to a hand effector in a non-contact movement in the air. Is not applied, and the detection force of the force sensor becomes zero, so that the hand effector moves at a speed proportional to the force target value. The movement of the force control mode causes the hand effector to come into contact with the work object. When the hand effector comes into contact with the work object, the force sensor detects the reaction force from the work object and is controlled so that the target force and the detected force are equal,
The robot comes to rest in the force control state. In this way, the hand effector can smoothly contact the object and maintain a preset contact force. This contact force can be set freely.

Since the contact is detected based on the value detected by the force sensor, quick contact can be detected, and the magnitude of the force at the time of detection can be known.

In this manner, the contact force and the force at the time of contact determination can be set freely, and therefore, it is possible to make the contact force optimal and remove the influence of the bending of the robot hand.

Specifically, for example, by setting the contact force and the force at the time of contact determination to a small force that does not cause a problem in deflection, or by setting the same force as when a robot works. Causes the same amount of deflection as during operation, eliminating the effects of deflection.

Further, in the configuration in which the deflection is corrected by using the detection force obtained by the force sensor, more accurate position detection can be performed.

After storing the contact position in the position detection configuration,
In the force control mode, if the detachment operation is performed to shift the hand effector from the contact state to the non-contact state, the force control mode is continuously maintained during the position detection operation. Therefore, since there is no need to perform mode switching, there is no discontinuous movement, troubles such as excessive force are unlikely to occur, and smooth operation can be performed.

[0021]

Embodiments of the present invention will be described below with reference to the accompanying drawings.

FIG. 1 shows a position and force control device of a multi-degree-of-freedom work machine including a work object position detecting device. In FIG. 1, reference numeral 1 denotes a robot main body, and an arm 2 having a plurality of joints is mounted on a support base 3. Based on the movable action of each joint of the arm 2, the arm tip moves to a position required for work, and the overall posture of the arm 2 changes to a posture required for work. Wrist part 4 located at the tip of arm 2
A six-axis force sensor 5 is attached to the
Is attached with a hand effector 7 for performing required work on the work 6 which is the work target. A plurality of motors for operating each joint of the arm 2 and the like are built in the robot body 1. Each of these motors is provided with a goniometer 8 for measuring the amount of drive, and the goniometer 8 obtains shaft angle data of each joint.

Next, the configuration of a control device for controlling the operation of the robot body 1 will be described.

In FIG. 1, the components of the control device are shown by block circuits. The control device is configured by software technology using a computer, for example. This control device controls the position and the force for performing the required work in the operation of the robot body 1.

In FIG. 1, reference numeral 9 denotes a force target value setting unit;
Is a position target value setting unit. X,
Target value <fr> of pressing force for each coordinate axis of y and z
Is set in advance, and the force target value setting unit 9 outputs the set target value <fr> to the subtractor 11. The setting of the force target value is performed by teaching data. Here, the symbol “<fr>” means that fr is a vector quantity. The meaning of “<>” is the same hereinafter.

The position target value setting unit 10 previously sets a target value <pr> of the position of the hand effector 7 for each coordinate axis, and the position target value setting unit 10 subtracts the set target value from the subtractor 12. Output to For setting the position target value, for example, a value obtained by interpolating the teaching data is used.

The force detected by the force sensor 5 is calculated by the force calculation unit 1
The force calculation unit 13 converts the sensor coordinate system into a reference coordinate system, which is a hand coordinate system, and performs gravity compensation by subtracting the gravity of the hand effector 7. Thereby, at the contact point between the work 6 and the hand effector 7, the hand effector 7
Is calculated. The force data obtained by the force calculation unit 13 is provided to the subtractor 11. On the other hand, the angle data of each axis detected by the goniometer 8 is sent to the position calculation unit 14, where the position <p> of the hand effector 7 in the absolute coordinate system is calculated based on the angle data. Position calculation unit 14
The data at the position obtained in step (1) is supplied to the subtractor 12.

In the subtractor 11, the force <f> and the force target value <f
r>, and the deviation <Δf> = <f> − <f
r> is output. On the other hand, in the subtractor 12, the position <p>
Is compared with the position target value <pr>, and the deviation <Δp> is output. The deviations <Δf> and <Δ
p> is input to the position / force control calculation unit 15.

The position / force control calculation section 15 selects at least one of the position control mode and the force control mode by setting parameters, and calculates a control command value for executing the selected control mode. It has the function to do. In the position control mode, the target speed vi is calculated in proportion to the element Δpi of <Δp>. In force control mode,
A target speed vi proportional to the element Δfi of <Δf> is calculated. The above calculation is performed for each coordinate axis. Note that Δ
The control may be performed in the compliance control mode using pi and Δfi.

The speed command value representing the target speed <v> calculated as described above is output from the position / force control calculation unit 15 and supplied to the drive command unit 20. In the drive command unit 20,
The speed command value <v> is converted into a drive command value <θ> for each drive motor of the robot body 1. The converted drive command value is supplied to each drive motor of the robot body 1, and the motor operates at a required speed according to the command value.

The operation command section 21 is an element for commanding / setting a control mode of a control device for controlling position and force. The operation command unit 21 has a function of storing operation data taught in advance, operating the control device based on the operation data, and causing the robot body 1 to reproduce the teaching operation. In the present embodiment, in particular, the control mode switching command 22
To the position / force control calculation unit 15, interpolate the taught position data and provide the interpolated data 24 to the position target value setting unit 10, or output the taught force target value 24 to output the force target value setting unit 9 is provided.

The operation command unit 21 further includes a command 26 for instructing the contact operation instruction unit 25 to issue a contact operation instruction.
Or receives a contact determination signal 28 from the contact determination unit 27 and provides a position storage command 30 to the position storage unit 29.

The contact operation instructing section 25 is provided for the position / force control calculating section 15 based on the command 26 from the operation command section 21 in preparation for the contact operation of the hand effector 7 of the robot body 1. A command 22A for setting the moving direction of 7 (the direction in which contact with the work 6 is started) to the force control mode for contact is output. The contact operation instructing unit 25 also outputs a value suitable for movement for contact to the force target value setting unit 9 as the force target value 23.

As described above, the contact operation instruction unit 25 controls the movement direction of the hand effector 7 via the force target value setting unit 9 and the position / force control calculation unit 15 according to the command of the operation command unit 21. It has a function of setting the mode to the force control mode, and instructing to move the hand effector 7 in the force control mode and bring it into contact with the work 6. The contact force determination unit 27 receives an output signal of the force calculation unit 13 and monitors the force output by the force calculation unit 13. When a predetermined contact force when the hand effector 7 comes into contact with the work 6 is detected, a contact determination signal 2
8 to the operation command section 21.

The position storage unit 29 receives the position storage command 30 given from the operation command unit 21, receives the position data <p> output by the position calculation unit 14 at that time, and stores the position storage command 3
The reference position sent together with 0 and the deviation of the position data are stored.

Next, the operation of the control device will be described.
An example of the operation is shown in the flowchart of FIG. This operation is a control operation of the control device when the hand effector 7 moving in the air in a non-contact state approaches the work 6 from an arbitrary direction, contacts the work 6, and detects the position.

When the operation command unit 21 gives the contact operation command 26 to the contact operation instruction unit 25, the contact operation instruction unit 25 outputs the command 22 A to the position / force control calculation unit 15 and the work 6 of the hand effector 7 The control mode in the approach direction to the vehicle is set as force control, and the control mode in other directions is set as position control (step 40). As for the approach direction, the force control is performed by the contact operation instruction unit 25 in a state where the target force value set in the target force value setting unit 9 is fr.

In the next step 41, a movement operation in the force control mode is performed in the approaching direction with respect to the movement of the hand effector 7 based on the above-mentioned set state. Since the force control is performed for the movement in the approach direction, the control device calculates the difference between the force target value fr set in the force target value setting unit 9 and the force f calculated by the force calculation unit 13 by a subtractor. 11 is required. Since the description here covers only the approaching direction, the force target value and the force are treated as scalar quantities. However, in the air in the non-contact state, since the force received by the hand effector 7 is 0,
The value of f = −fr is input to the position / force control calculation unit 15. In the force control mode, the position / control calculation unit 15 outputs a speed v proportional to Δf. Assuming that the gain of the position / force control calculation unit 15 at this time is Kc, v = −Kc · fr is input to the drive command unit 20, and the robot body 1 moves at the above-described speed command value v. In other words, the robot body 1
Operates at a speed proportional to the target force value fr set in the target force value setting unit 9. In the following description, such a moving operation is referred to as “movement in the force control mode”. In the directions other than the approaching direction, the position control mode is set, and the position target value is held at a fixed place, so that the vehicle does not move in directions other than the approaching direction. Therefore, the hand effector 7 moves linearly in the approaching direction.

In the above-mentioned moving state, the contact force determining unit 2
7 inputs the force obtained by the force calculation unit 13 at an appropriate timing, and determines whether the force obtained in the approach direction is greater than the force f ₀ set in the contact determination unit 27 (step S7). 42). In the movement in the air, the contact force at the hand effector 7, that is, the force applied as an external force is 0, and therefore steps 41 and 42 are repeated during that time. In addition,
The force f ₀ set in the contact determination unit 27 is a value smaller than the force target value fr for moving in the force control mode.

When the hand effector 7 comes into contact with the work 6, the force sensor 5 detects the contact force, and the force calculation unit 15 outputs a signal corresponding to the force detected by the force sensor. Therefore, the force f is input to the subtracter 11, and Δf becomes f−fr.
The force control is performed so that Δf becomes zero. Step 4
The movement at 3 becomes a normal force control state, f = fr, the speed v becomes 0, and the hand effector 7 stops.

[0041] At step 43, approaching force is larger than the set value f _0. In this step, based on the position storage command 30 from the operation command unit 21, the position storage unit 29 stores the current position of the hand effector 7 output from the position calculation unit 14. Or, the position storage command 30
The deviation from the reference position sent together with is stored. While step 43 is performed, the hand effector 7 is maintained in contact with the work 6 based on the force control.

As described above, the position when the hand effector 7 moving in the air contacts the work 6 is detected. The detected position is, for example, relative to the position of the work taught.
It is used to correct the positional deviation of the work during the actual work or to measure the position and shape of the work that is not taught.

In the above embodiment, the hand effector 7 approaches the work 6 under the force control state, makes smooth contact, and stops on the work 6 with a preset pressing force. Since the pressing force at this time can be set freely, it is possible to set an appropriate pressing force so that the bending of the hand effector and the work does not matter. Further, by setting the above-mentioned pressing force to the same pressing force as when actually working, it may not be necessary to consider the influence of the bending. The detection of the contact state when the hand effector 7 comes into contact with the surface of the work 6 is performed using a force detection system and a force control system including the force sensor 4, and the force detection system and the force control system are provided as touch sensors. Since the function is provided, the contact can be detected quickly.

In the above embodiment, the operation of the robot after the contact position of the hand effector 7 on the work 6 is stored, for example, by correcting the work displacement based on the detected position data and continuing. Perform work such as grinding.

Next, another embodiment of the present invention will be described with reference to FIG. In this embodiment, a control configuration including a part for executing an operation for moving the hand effector 7 away from the work 6 is shown. In the above-described position detection operation, when a number of positions are detected, or when the next work position is different, the hand effector 7 in contact with the work 6 moves away from the work 6 to another place. Go to In such a case, the control configuration of the present embodiment is suitable.

The configuration of FIG. 3 is basically the same as the configuration shown in FIG. Hereinafter, only the differences will be described. The different component is that a detachment operation instruction unit 31 is added. The detachment operation instructing unit 31 gives a force target value in the detaching direction to the force target value setting unit 9 based on a command 32 for instructing the detachment operation from the operation instruction unit 21.

The operation of this embodiment will be described with reference to the flowchart shown in FIG. Steps 40 to 43 are the same as the operations described with reference to FIG. In step 44, the detachment operation instruction unit 31 changes the force target value in the contact direction set in the target force value setting unit 9 to a force target value in the detachment direction having the opposite sign. As a result, the direction of force control is changed from the approach direction to the release direction.

Next, at step 45, movement in the force control mode is performed according to the force target value set at step 44. That is, since the force target value is set to a value in the opposite direction in the case of the contact operation, the moving direction is opposite to that in the case of the contact operation. Therefore, the detaching operation is performed.

In step 46, the operation command section 21 measures the time from the start of the separation operation, and repeats steps 45 and 46 until a preset time has elapsed.
After a lapse of the set time, the withdrawal operation is completed and the operation proceeds to the next operation.

As described above, the detachment operation of the hand effector 7 from the work 6 is performed by moving in the force control mode, so that the robot control mechanism is maintained in the force control mode from step 40 to step 46. You. In other words, the position detection operation is performed without outputting a control mode switching signal from the operation command unit 21 to the position / force control calculation unit 15 to switch the control mode, and without any trouble such as generation of excessive force due to mode switching. Can be performed smoothly.

In step 46, the set fixed time is monitored, but as another configuration, a fixed distance may be monitored.

The force target value for the detaching operation may be obtained by simply reversing the sign of the force target value for the contact operation. Further, values having opposite signs and different absolute values may be set.

FIG. 5 shows another embodiment. In this embodiment, the position is corrected in consideration of the deflection of the hand effector 7 and the work 6. The technical contents relating to the correction are described in Japanese Patent Application No. 2-66310.

In the configuration shown in FIG. 5, a position correction unit 33 is added to the configuration shown in FIG. The position correction unit 33 includes data on the force output from the force calculation unit 13 and the position calculation unit 14.
Input the data regarding the position of the hand effector 7 output by the user, and the position data 3 corrected based on these data.
4 is output.

[0055] In the position correcting unit 33, which is set by measuring the vicinity advance the contact point, and rigidity K _R of the contact direction of the robot, including the end effector and workpiece, the contact force f, the deflection of the contact direction The amount Δe is calculated by f / K _R , the position in the contact direction is corrected using the calculated amount of deflection, and output is performed.

As described above, since the position is corrected in consideration of the deflection amount, accurate position detection can be performed.

The characteristic configuration of this embodiment can be added to the configuration shown in FIG.

[0058]

As is apparent from the above description, according to the present invention, a hand effector for performing a required operation is attached to the tip of the arm, which is provided with a position and force control device and a detection mechanism related thereto. In the force control robot, the position of the work target can be accurately detected by moving the hand effector in the force control mode and detecting the contact force with the work target. Using force control based on a force sensor and a position sensor and a position and force control device provided in the robot,
Implement a touch sensor. According to the position detection device of the present invention, there is no need to provide a special device for detection specially, so that it does not hinder the original work in the configuration of the robot and is advantageous. The structure for position detection can be formed compactly.

Since the contact of the hand effector with the work target is detected using the detection force of the force sensor, the contact can be detected quickly.

Further, since the position of the work object is detected by making direct contact with the hand effector performing the work, there is no need to consider the shift between the work point and the sensor means, and the work object suitable for the work is not required. Is a position detection method.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a first embodiment of the present invention.

FIG. 2 is a flowchart illustrating the operation of the first embodiment.

FIG. 3 is a block diagram showing a second embodiment of the present invention.

FIG. 4 is a flowchart illustrating the operation of the second embodiment.

FIG. 5 is a block diagram showing a third embodiment of the present invention.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 Robot main body 2 Arm 5 Force sensor 6 Work 7 Hand effector 9 Force target value setting part 10 Position target value setting part 13 Force calculation part 14 Position calculation part 15 Position / force control calculation part 21 Operation command part 25 Contact operation instruction part 27 Contact force determination unit 29 Position storage unit 31 Separation operation instruction unit 33 Position correction unit

 ──────────────────────────────────────────────────続 き Continuation of front page (56) References JP-A-62-24991 (JP, A) JP-A-61-170805 (JP, A) JP-A-3-270864 (JP, A) JP-A-4- 69179 (JP, A)

Claims (3)

(57) [Claims] 1. A force detecting means for detecting at least one axial force applied to a hand effector, a position detecting means for detecting a position of the hand effector, a force target value set in advance, and the force detecting means. In a force control robot having a position and force control means for executing at least a force control mode in which a moving speed of the hand effector is determined by a deviation from the detected force detection value, when the hand effector is in a non-contact state Contact operation executing means for moving the hand effector in the force control mode and contacting the work target; and when the magnitude of the force detected by the force detecting means exceeds a preset force, the hand effector And a contact determination unit that determines that the work object has contacted, and when the contact determination unit determines that it is in a contact state,
And a contact position storage means for storing data relating to a position detected by the position detection means at the time of contact, and a position detection device for a work target in the force control robot. 2. The apparatus according to claim 1, wherein the hand effector is in a force control mode when the hand effector is in contact with the work object. An apparatus for detecting a position of a work object in a force control robot, comprising: a detachment operation execution unit that moves from a contact state to a non-contact state. 3. The apparatus for detecting the position of a work object in a force control robot according to claim 1, wherein the position data detected by the position detection means is based on the force data detected by the force detection means. An apparatus for detecting the position of a work object in a force control robot, comprising: a position correcting means for performing correction excluding a deflection amount element of a section.