JPH05185386A - Linear motion control system for robot finger - Google Patents

Abstract

PURPOSE:To provide a linear motion control system for robot fingers, with which the operator can correct the motion of a finger (s) when it makes linear motion in conformity to a motion program. CONSTITUTION:A linear motion command means 10 conducts interpolation calculation in conformity to a motion program so as to make a robot finger 41 perform a linear motion at a specified speed and gives comand signals in correspondence to the target position at each sampling point. This target position at each sampling point is determined by evaluating for example the target position set previously and the current position fed back from a locational servo means 20. The specific speed is altered when an alter command for the specific speed is received from a speed alter command means 30, and interpolation calculation is conducted on the basis of the altered specific speed, and a command signal in accordance with the target position at each sampling point is emitted. The locational servo means 20 makes location of the robot finger 41 to put it into linear motion.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot hand linear movement control method for controlling the linear movement of a robot hand, and more particularly to a linear movement of a robot hand which is coordinated with remote control for linear movement control of the robot hand based on an operation program. control method.

When a robot is placed in an extreme environment such as a space environment where humans cannot go directly, a method of controlling the robot from a remote place is required. In such a case, remote operation by master-slave control is generally used in the past, but in the future, in order to reduce the burden on the operator and increase the reliability of work, autonomous operation by a program and human judgment or direct human It is important to take a control form that cooperates with remote operation. The present invention relates to a case where this control mode is applied to linear movement control of a robot hand, which is the basis of autonomous operation.

[0003]

2. Description of the Related Art Conventionally, linear movement control of a robot hand has been an important function in the field of FA and the like. This linear movement control will be described with reference to FIGS. 11 and 12.

FIG. 11 is an explanatory diagram of the linear movement control of the robot hand. In the figure, it is assumed that the robot hand moves at a specified linear velocity v over a distance d from point A to point B in space. In this case, the robot language (motion program) “line MoveA to B with
Interpolation calculation is performed based on “V” to obtain the target position Pr (n) (= (Xn, Yn)) at each sampling point. The target position Pr (n) is given by the following equation (1).
The starting point A (XA, YA) (= Pr (0) = (X0, Y0
)), End point B (XB, YB) (= Pr (m) = (Xm,
Ym)).

Xn = X0 + (XB−XA) / m · n Yn = Y0 + (YB−YA) / m · n (1) where m is the number of samples required for movement, It is given by the following equation (2).

M = d / (VΔt) where d: moving distance (= {(XB −XA) ² + (YB
-YA) ² } ^1/2 ) V: Designated movement speed Δt: Sampling time FIG. 12 is a block diagram of a conventional linear movement control system for a robot hand. In the figure, the interpolation calculation unit 100 shows an operation program “line Move A to B wit”.
The interpolation calculation from the start point A to the end point B is performed based on "hV", and a command signal corresponding to the target position Pr (n) at each sampling point is output. The positioning servo system 200 drives the robot (manipulator) 400 according to the command signal and positions the robot hand 410. The robot hand 410 moves linearly at a constant speed V from the starting point A to the ending point B while passing through the target position Pr (n) at each sampling point.

As described above, in the conventional linear movement control, when the linear movement is started based on a predetermined operation program, the linear movement is moved at a constant speed until the end point is reached. Therefore, the operator could not intervene in the linear movement. Moreover, even if it intervened, it was only stopped at best. The linear movement control based on such an operation program is fully utilized in the field such as FA where the work environment is maintained.

[0008]

However, when executed in an unprepared environment, it is necessary to be able to modify its programmed behavior at run time in situations that are not expected at the time of programming. For example, when there is an unexpected obstacle during linear movement by the operation program between the start point A and the end point B, it is necessary to perform an operation for avoiding the obstacle at the time of execution.

The present invention has been made in view of the above circumstances, and it is an object of the present invention to provide a linear movement control system for a robot hand, which allows an operator to correct the movement of the robot hand when the robot hand moves linearly according to an operation program. To aim.

[0010]

1, 2 and 3 are block diagrams of the principle of the present invention. Referring to FIG. 1, a linear movement control system for a robot hand according to the present invention outputs a command signal for causing the robot hand 41 to perform a linear movement at a specified speed based on a predetermined operation program.
And a positioning servo means 20 for positioning the robot hand 41 according to the command signal, and a speed change command means 30 for commanding the linear movement command means 10 to change the designated speed.

Referring to FIG. 2, the robot hand linear movement control system according to the present invention, in addition to the configuration of FIG. 1, corrects the linear movement command from the linear movement command means 10 to change the trajectory of the linear movement. It has a command means 50.

In FIG. 3, in addition to the configuration of FIG. 1, a filter 60 is provided between the speed change command means 30 and the linear movement command means 10, and the speed change command means 30 passes through the filter 60. The designated speed change command is input to the linear movement command means 10.

[0013]

In FIG. 1, the linear movement command means 10 performs interpolation calculation based on a predetermined operation program so as to cause the robot hand 41 to linearly move at a specified speed, and responds to the target position at each sampling point. Output a command signal.
The target position is obtained, for example, by evaluating a preset target position and the current position fed back from the positioning servo means 20. Further, when a designated speed change command is received from the speed change command means 30, the designated speed is changed, interpolation calculation is performed based on the changed designated speed, and a command signal corresponding to the target position at each sampling point is generated. Output. In response to the command signal, the positioning servo means 20 positions the robot hand 41 and causes the robot hand 41 to move linearly from the start point to the end point via the target position at each sampling point. In this way, the robot hand 41 moves linearly at a specified speed based on a predetermined operation program, and if the specified speed is changed during the linear movement, the robot hand 41 moves linearly based on the changed specified speed. I do. Therefore, according to the operator's instruction, the speed of the robot hand can be changed if necessary,
It is possible to stop and reverse the moving direction.

In FIG. 2, the trajectory correction command means 50 is
The trajectory of the linear movement is changed by correcting the linear movement command from the linear movement command means 10. In this way, the robot hand 41
Is linearly moved from the start point to the end point based on the operation program, the trajectory of the operator is corrected according to the instruction of the operator as necessary. Therefore, even if there is an obstacle, the obstacle can be avoided and passed.

In FIG. 3, by providing the filter 60, even if a discontinuous designated speed change command is given, the speed can be smoothly changed by the command.

[0016]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 4 is a diagram showing an overall configuration for executing the robot hand linear movement control system of the present invention. In the figure, the robot hand linear movement control system of the present invention comprises a host computer 3, a robot control computer 1, a manipulator controller 2, and a manipulator (robot) 40A. Robot language loading unit 35 of host computer 3
Loads the programmed robot language string (operation program) into the robot language interpreter 11A of the robot control computer 1. The robot language interpreter 11A interprets, for example, a robot language string relating to linear movement as shown in (a), (b) and (c) below, and the information A, B, C, D, V is provided to the interpolation calculation unit 11. send.

(A): line Move A to B with V1 (b): line Move B to C with V2 (c): line Move C to D with V3 The interpolation calculator 11 is based on the information, and the robot 40A.
Interpolation calculation is performed to control the linear movement of the target position Pr (n) at each sampling point, and the target position Pr (n) is calculated.
The command signal according to (n) is sent to the manipulator controller 2
Output to the positioning servo system 21. When calculating the target position Pr (n) at each sampling point, the interpolation calculation unit 1
1 evaluates the current position Pact and the final target position Pref for each sampling, and determines the target position Pr at the sampling.
Find (n). The current position Pact is fed back from the positioning servo system 21.

The servo motor of the positioning servo system 21 is
The robot hand 41A is driven according to the pulse signal from the interpolation calculation unit 11, and the driving causes the robot hand 41A to pass through the target position Pr (n) at each sampling point, and from the start point Pr (0) to the end point (final target position) Pref. Move straight to. For example,
When the robot language strings (a), (b), and (c) are executed, the robot hand 41A has the linear velocity command value V1 from the point A to the point B, and the linear velocity command value from the point B to the point C. V2
Then, from the point C to the point D, each linearly moves at the linear velocity command value V3.

The host computer 3 is provided with a speed modifier generator 31. The speed modifier generating unit 31 includes a dial 32 connected to the host computer 3.
The velocity modifier Vmod is generated according to the rotation amount of.
By rotating the dial 32, the operator can specify the speed modifier Vmod. Velocity modifier Vmod is a robot language interpreter 11A
To the interpolation calculation unit 11 via the filter 61. The velocity modifier Vmod is a floating-point number that can take positive and negative values, and the interpolation calculator 11 multiplies the preset linear velocity command value V by this velocity modifier Vmod to change the linear velocity. .. The velocity modifier Vmod is normally initialized to the value 1.

However, when the speed modifier Vmod is given by the operator's operation of the dial 32, it may be given discontinuously.
A reversing operation such as changing to 1 is also included. Therefore, the filter 61 is provided as described above, and the velocity modifier Vmod is input to the interpolation calculation unit 11 via the filter 61. If, for example, a first-order lag filter or a moving average filter is inserted as the filter 61, the speed can be smoothly changed even with an extreme change in the speed modifier Vmod.

The host computer 3 is also provided with an orbit modifier generator 51. The trajectory modifier generation unit 51 uses the position correction signal (Δ) proportional to the tilt amount of the joystick 52 connected to the host computer 3.
Xmod, ΔYmod) are sequentially generated and output as the trajectory modifier Pmod. The operator can arbitrarily specify the trajectory modifier Pmod by manually operating the joystick 52. When the joystick 52 is not operated by the operator, the value 0 is output, and the robot hand 41A performs the movement operation according to the operation program.

The trajectory modifier Pmod is the interpolation calculation unit 1
1 is input to the adder 11B provided on the output side. The adder 11B outputs the target position Pr (n) (= (Xn, Yn)) at each sampling point output from the interpolation calculation unit 11.
And the position correction signals (ΔXmod, ΔYmod) are added,
The new target position Pr (n) obtained by the addition is output to the positioning servo system 21 as a position command signal. The position command signal corrects the linear trajectory of the robot hand 41A.

The adder 11B and the positioning servo system 2
1 and positioning servo system 20 and interpolation calculation unit 11
There is a conversion unit (not shown) between and, and the conversion between the XY coordinate data and the joint angle data of the robot 40A is performed.

As described above, the operator can remotely change the speed and correct the trajectory of the robot 40A that operates as a program by remote control, if necessary. Therefore,
It is possible to enhance the interactivity between the robot 40A and a human (operator). Further, the robot 40A can be made to perform an efficient work by combining the program operation and the remote operation.

FIG. 5 is a diagram showing a flowchart for changing the speed of the robot hand. Here, it is assumed that the robot hand 41A moves linearly from the starting point A (starting point position PA = Pr (0)) to the ending point B (final target position PB = Pref). The numbers following S in the figure represent step numbers. [S1] The velocity modifier Vmod is obtained. As described above, this velocity modifier Vmod is generated by the operator's operation of the dial 32, and the interpolation computing unit 11 and the robot language interpreter 11 of the robot control computer 1 are generated.
Input to A. [S2] Final target position Pre in this sampling
Determine f as follows.

If Vmod ≧ 0 then Pref = PBelse Pref = PA Therefore, when the velocity modifier Vmod is positive, the moving direction of the robot hand 41A is in the direction of the end point B, and when it is negative, it is in the direction of the start point A. Will be done. [S3] The specified speed Vcur and the specified movement amount Δd in this sampling are calculated from the following equations (2) and (3).

Vcur = V × Vmod (2) Δd = Vcur × Δt (3) where Δt: sampling time Therefore, when the velocity modifier Vmod is 0, Vcu
r = 0 and the robot hand 41A stops. [S4] The current position Pact of the robot hand 41A is obtained.
This current position Pact is obtained by feedback from the positioning servo system 20, and is obtained by, for example, coordinate conversion of each joint angle detection data by the encoder of the servo motor. [S5] The distance d between the current position Pact of the robot hand 41A and the final target position Pref is calculated from the following equation (4). However, the current position Pact (= (Xact, Yact)) and the final target position Pref (= (Xref, Yref)) are set. d = determined from ^{sqrt {(Xref -Xact) 2 +} (Yref -Yact) 2} ··· (4) [S6] following equation sample number i (integer) required to move (5).

I = d / Δd (5) [S7] It is determined whether or not the sample number i is i = 0. If i = 0, the process proceeds to step S8, and if not, the process proceeds to step S9. [S8] The robot hand 41A is almost at the final target position Pref
Therefore, the final target position Pref is output as the target position Pr (n) (= (Xn, Yn)) in this sampling. [S9] Target position Pr (n) in this sampling
(= (Xn, Yn)) is calculated from the following equations (6) and (7) and output, and the process returns to step S1.

Xn = Xact + (Xref−Xact) / i (6) Yn = Yact + (Yref−Yact) / i (7) FIG. 6 shows the velocity by the velocity modifier Vmod. It is a figure which shows the example of control, (A) is a case where a moving direction is reversed.
(B) shows the case of stopping and (C) shows the case of decelerating, respectively. As shown in FIG. 6A, when the velocity modifier Vmod is changed from positive to negative, the robot hand 4
The moving direction of 1A is reversed from the A → B direction to the B → A direction. Further, as shown in FIG. 6B, when the velocity modifier Vmod = 0, the movement of the robot hand 41A is stopped. Further, as shown in FIG. 6C, when the velocity modifier Vmod is reduced, for example, 1 → 0.5 → 0.1, the robot hand 41A decelerates accordingly.

FIG. 7 is a diagram showing an example of a locus when the orbit modifier Pmod is applied. Orbit modifier P
The algorithm in the interpolation calculation unit 11 when mod acts is the same as that shown in the flowchart of FIG. In the figure, the robot hand 41A is "line
According to Move A to B with V ”,
Moving from start point A to end point B. P1 on the way
When the trajectory modifier Pmod acts at a point, the robot hand 41A is subjected to trajectory correction according to the position correction signals (ΔXmod, ΔYmod) of the trajectory modifier Pmod, and the robot hand 41A moves from point P1 to point P2. .. When the action section ΔP of the trajectory modifier Pmod ends and the trajectory modifier Pmod disappears at the point P2, the current position Pact (P2 point) fed back from the positioning servo system 21 at that time and the final target point B are constantly fed back. Since the interpolation calculation is performed between the robot hand 41A, the robot hand 41A finally moves to the target point B. Therefore, "lin
Even if the trajectory modifier Pmod modifies the trajectory during the execution of "e Move A toB with V", the robot hand 41A will initially specify the final target point B (however, if Vmod <0, finally, the A point). ) Stops the movement.

Next, the movement control of the robot hand according to the present invention will be described with reference to FIGS. 8, 9 and 10. FIG. 8 is a diagram showing a route of a robot hand planned by a robot language string. In the figure, the robot hand is, according to the examples (a), (b) and (c) of the robot language sequence described above,
It moves in the order of A->B->C-> D, and the speed during that time also changes according to the designated speed, V1->V2-> V3. In this case, the robot hand 41A is controlled by the program operation so that
You can avoid 91 and 92.

FIG. 9 is a diagram showing a case where there is an obstacle in the path of the robot hand. When an operation is executed by a program,
It is assumed that an obstacle 93, which was not assumed when the program was created, existed as shown in the figure. At this time, for example, while moving between BCs, the speed modifier Vmod is set to 0 by the dial operation at the P3 point, the movement of the robot hand 41A by the program operation is stopped, and in that state, the trajectory is corrected by the remote operation by the joystick. The obstacle 93 is avoided. Speed modifier Vmo again at P4
By setting d to 1, the robot hand 41A returns to the program operation and reaches the final target point D based on the program.

In this example, the speed modifier Vmod is set to 0 for safety, and the joystick 52 is used for remote control after the program operation is stopped. However, the speed modifier Vmod is set to a small value. It is also possible to set the speed to slow down and superimpose the remote operation by the joystick 52 on the operation.

FIG. 10 is a diagram showing a case where the moving direction of the robot hand is reversed. In the figure, robot hand 4
1A can reverse the moving direction while moving between CDs.
This reversal is done by changing the setting of the velocity modifier Vmod from positive to negative. The velocity modifier Vmod is input to the robot language interpreter 11A. In the robot language interpreter 11A, when the velocity modifier Vmod is positive, the robot language sequence is in positive order, and when the velocity modifier Vmod is negative, it is in reverse order. Run. In this way, the robot language interpreter 11A
By providing the reverse execution function of the robot language,
It becomes possible to reverse the moving direction by the velocity modifier Vmod as shown in 0.

[0035]

As described above, according to the present invention, when the designated speed is changed when the robot hand moves linearly based on a predetermined operation program, the linear movement is performed based on the changed specified speed. As configured. Therefore, according to the operator's instruction, the speed of the robot hand can be changed, stopped, and the moving direction can be reversed.

Further, while the robot hand is linearly moving from the starting point to the ending point based on a predetermined operation program, the trajectory of the robot hand can be corrected as needed according to the instruction of the operator. Therefore, even if there is an obstacle, it is possible for the robot hand to pass through the obstacle while avoiding it.

As described above, the operator can remotely change the speed and correct the trajectory of the programmed robot, if necessary. Therefore, the interactivity between the robot and the human (operator) can be enhanced. In addition, the robot can be made to perform efficient work by combining program operation and remote control.

Further, a filter is provided so that the command for changing the designated speed is inputted to the linear movement command means via this filter. Therefore, even if a discontinuous designated speed change command is given, the speed change according to the command can be smoothly performed.

[Brief description of drawings]

FIG. 1 is a principle block diagram of the present invention.

FIG. 2 is a principle block diagram of the present invention.

FIG. 3 is a block diagram of the principle of the present invention.

FIG. 4 is a diagram showing an overall configuration for executing a linear movement control system for a robot hand according to the present invention.

FIG. 5 is a diagram showing a flowchart for executing speed change of a robot hand.

FIG. 6 is a diagram showing an example of speed control by a speed modifier Vmod, in which (A) shows a case where the moving direction is reversed.
(B) shows the case of stopping and (C) shows the case of decelerating, respectively.

FIG. 7 is a diagram showing an example of a locus when an orbit modifier Pmod is applied.

FIG. 8 is a diagram showing a path of a robot hand planned by a robot language string.

FIG. 9 is a diagram showing a case where an obstacle exists in the path of the robot hand.

FIG. 10 is a diagram showing a case where the moving direction of the robot hand is reversed.

FIG. 11 is an explanatory diagram of linear movement control of a robot hand.

FIG. 12 is a block diagram of a conventional linear movement control system for a robot hand.

[Explanation of symbols]

 10 linear movement command means 20 positioning servo means 30 speed changing means 40 robot (manipulator) 41 robot hand 50 trajectory correction command means 60 filter

Claims (6)

[Claims] 1. A robot hand linear movement control method for controlling a linear movement of a robot hand (41) according to a predetermined operation program.
And a linear movement command means (10) for outputting a command signal for performing linear movement at a designated speed, and positioning of the robot hand (41) according to the command signal output from the linear movement command means (10). A linear movement control method for a robot hand, comprising: a positioning servo means (20) for performing the above; and a speed change instruction means (30) for instructing the linear movement instruction means (10) to change the designated speed. 2. The linear movement command means (10) includes a preset target position and the positioning servo means (2).
The linear movement control system for the robot hand according to claim 1, wherein the target position at the time of this sampling is obtained by evaluating the current position fed back from 0). 3. The linear movement control method for a robot hand according to claim 1, further comprising trajectory correction command means (50) for correcting the linear movement command and changing the trajectory of the linear movement. 4. The command from the speed change command means (30) to the linear movement command means (10) is performed via a filter (60).
The linear movement control method of the robot hand described. 5. The linear movement command means (10) changes the speed of the robot hand (41), stops moving, and reverses the moving direction based on a command from the speed change command means (30). The linear movement control system for the robot hand according to claim 1. 6. The linear movement control method for a robot hand according to claim 5, wherein the linear movement command means (10) executes the predetermined operation program in reverse order when the movement direction is reversed.