JPS61120208A - Robot controller - Google Patents

Abstract

PURPOSE:To attain an easy and smooth follow-up action of a robot to a travel ing object by attaching a shift detector at the tip of a robot to control plural motion axes with the detection value of said detector and at the same time starting a follow-up action synchronously with the start of a shift of the travel ing object. CONSTITUTION:A multi-axes robot 1 of a 4-joint parallel link mechanism has its base part 3 fixed on a floor surface 2 with the 1st-5th axes 4-8 turned in directions theta1-theta5 respectively. The shaft 8 set at the tip of the robot 1 contains a shift detector 9 having a displacement detecting lever 10. A traveling object 13 having a hook pin 13a has a rectilinear axis toward Z on a track 16 set between fixed parts 14 and 15. Then the motors of axes 5-6 are driven with control by a follow-up controller 17 based on each detection value of the detector 9 which secured hook between the lever 10 and the object 13 so that the change of said detection value is set at zero. In addition, a start device 18 and a start device 19 are provided to start the controller 17 synchronously with the shift start of the object 13. Then the actuation of the controller 17 is stopped at a prescribed set value.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to a control technique that allows a robot to easily follow a moving object.

[Conventional technology]

Conventionally, when a robot is caused to follow a moving workpiece, the moving speed of the workpiece is detected and the playback motion of the robot is sequentially corrected in accordance with the detection result.

[Problem that the invention seeks to solve]

However, conventional corrections not only require advanced techniques such as coordinate transformation of teach data and linear interpolation, but also require even more subtle movements when the workpiece does not move smoothly. There were many problems such as the need to provide complicated functions for tracking.

This invention is basically an attempt to solve such problems, but in addition, it also makes it possible to easily automate tracking control in order to expand practical applications. .

Measures to solve problem C] Therefore, the robot control device according to the present invention is as follows.

A displacement detection frame capable of engaging with a moving body and movable in a plurality of directions, and a plurality of displacement detectors each detecting movement of the displacement detection frame in a plurality of directions are installed at the tip of a robot having a plurality of motion axes. Based on the detection values of the plurality of displacement detectors when the displacement detection frame of the attached movement detection device is engaged with the moving body and the moving body is moved, A follow-up control means that drives and controls each of the plurality of motion axes necessary for the following movement with respect to the moving body, a starting means that starts the following control means in synchronization with the start of movement of the moving body, and a plurality of motion axes necessary for the following movement. and stop means for stopping the operation of the follow-up control means when each current value of reaches each predetermined set value.

[For production]

With this configuration, not only can the robot follow the moving body very easily and smoothly, but also the following action can be started and stopped automatically.

〔Example〕

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

FIG. 1 is an overall configuration diagram showing an embodiment of the present invention.

In the figure, reference numeral 1 is a motor-driven robot with a four-bar parallel link mechanism and a multi-axis (multi-part) type having five operating axes. A first shaft 4 rotates in the direction of arrow θ1, a second shaft 5 rotates in the direction of arrow θ2 with respect to the first shaft 4, and a second shaft 5 rotates in the direction of arrow θ3 with respect to the second shaft 5. Consisting of a third shaft 6, a fourth shaft 7 that rotates in the direction of arrow θ4 with respect to the third shaft 6, a fifth shaft 8 that rotates in the direction of arrow θ5 with respect to the fourth shaft 7, etc. .

S is a movement detection device attached to the fifth shaft 8 which is the tip of the robot 1, and as shown in an enlarged view in FIG. Matching hole 1
0a, and slides in the direction of arrow Y with respect to a shaft member 12 that is swingably attached to the base 11 in the direction of arrow θX, and also slides in the direction of arrow θX with shaft member 12 relative to the base 11. The displacement detection frame 10 is provided between the displacement detection frame 10 and the shaft member 12, and the arrow Y of the displacement detection frame 10
and a linear potentiometer as a displacement detector that does not appear in the figure to detect sliding displacement in the direction.

It is provided with a rotary potentiometer as a displacement detector (also not shown in the figure) for detecting the rocking displacement of the displacement detection frame 10 in the direction of arrow θX, which is provided inside the base body 11.

Note that the displacement detection frame 10 of this movement detection device S is as follows.

For example, it is assumed that it has a return habit of returning to intermediate positions in the sliding range in the direction of arrow Y and the swinging range in the direction of arrow θX.

Further, as the movement detection group @S, it is possible to use a device having the above-mentioned movable function among devices called a so-called joystick (operation frame).

Returning to FIG. 1, 13 is a movable body equipped with an engaging pin 1!1a, and 1 moves linearly in the direction of arrow Z on a track 16 constructed between fixed parts 14 and 15 as shown in the figure. .

However, in this embodiment, the arrow Z along which this moving body 1!1 moves
The direction is parallel to one coordinate axis direction (for example, literally the Z coordinate axis direction) in the three-dimensional space, and is also parallel to the direction of the surface formed by the second axis 5 and third axis 6 of the robot 1 with respect to the floor surface 2. It is assumed that there is

For example, when the movable body 1'5 is stopped at a predetermined position, by playing back the robot 1, the engagement hole 10a of the displacement detection frame 10 in the movement detection device S attached to the fifth shaft 8 is opened. Engagement pin 1 of transfer 6 body 13!
1a in a relatively rotatable manner.

That is, the engaging hole 10 of the displacement detection frame 10 is played back based on teaching data obtained by teaching the robot 1 in advance by a drive control device to be described later.
A is brought closer to the engagement pin 13a from the direction of the arrow as shown in FIG. 3, and the two are engaged.

Note that the swing plane of the displacement detection rod 10 when the engagement hole 10a and the engagement pin 13a are engaged is parallel to the direction of arrow Z, which is the moving direction of the movable body 13.

Returning to FIG. 1, reference numeral 17 is a follow-up control device, which after engaging the displacement detection rod 10 attached to the fifth axis 8 of the robot 1 with the movable body 13, moves the movable body 13. Based on the respective detection values of the two potentiometers described above in the movement detection device 2B in a state in which the displacement detection rod 10 is moved by starting 13 required for follow-up movement.
Motors (not shown) that respectively drive the shaft 5 and the third shaft 6 are sequentially controlled.

That is, when the moving body 13 moves from Z to z2 in the above state as shown in FIG. 4, the detection values of the two potentiometers of the movement detection device S change from (ao, bo) to (a++ bt). Change.

Here, -aural is the detection value of the linear potentiometer that detects the sliding displacement of the displacement detection rod 10 in the arrow Y direction (see Figure 2), and bo, b is the detection value of the displacement detection rod 10 in the arrow direction. This is the detected value of the rotary potentiometer that detects the swing displacement in the θX direction (see FIG. 2).

Therefore, this displacement amount (a+ −ao + bt −
If the second axis 5 and third axis 6 of the robot 1 are moved so as to cancel bo) to (0,0), the movement of the moving body 13 is followed, and the following control device! 17 is the second axis 5
and each drive motor of the third axis 6 (a+ aQ , b
Drive control is performed so that l bo) → (, O, O).

Returning to FIG. 1, reference numeral 18 denotes a starting device, which starts the follow-up control device 17 in synchronization with the start of movement of the moving body 13.

IS is a stop device that stops the operation of the follow-up control device 17 when the current values of the second axis 5 and third axis 6 necessary for follow-up movement with respect to the moving body 13 reach respective predetermined set values. do.

Next, referring to FIG. 5, follow-up control device 17. Starting device 1
8. The configuration of the control system including the system and the stop device will be explained.

Reference numeral 20 denotes a microcomputer, which controls the drive (servo) control devices that drive and control the first to fifth axes 4 to 8 in the robot 1 shown in FIG. 1, respectively, and also controls the follow-up control device 17.

21 is a drive (servo) for the second axis 5 in the robot 1
It is a control device, and includes a target value register 22° and subtractors 2B and D.
/A converter 24. Gain adjuster 25, analog switch 26. Drive circuit (servo amplifier) 27. A/D converter 2
8. Current value register 29. and a comparator 30, etc.

The drive control device 21 controls a motor 31 for driving the second shaft 5 by receiving a speed feedback signal from a tacho generator 32 attached to the rotating shaft 31a and a potentiometer attached to the rotating shaft 31a via a reducer 33. The drive is controlled using position feedback signals from 34 units.

The movement target value for the second axis 5 from the microphone column computer 20 is written into the target value register 22 at the required timing, and this written movement target value is output to the subtracter 23 and the comparator 30. Ru.

The subtracter 23 is obtained by sequentially A/D converting the moving target value output from the target value register 22 and the position feedback signal from the potentiometer 34 using the A/D converter 28, and The deviation from the current value of the second axis 5, which is written while being updated sequentially, is expressed as /iI'r5.

The D/A converter 24 receives the second axis 5 calculated by the subtracter 23.
The gain adjuster 25 performs gain adjustment on the deviation signal from the D/A converter 24 to a required signal level. do.

The analog switch 26 is a starting/starting switch for the second axis 5, which will be described later.
The flip-flop circuit 52 in the stop section 48 is turned on only when it is reset, allowing the deviation signal whose gain has been adjusted by the gain adjuster 25 to pass through.

The drive circuit 27 applies a drive current to the motor 31 in accordance with the deviation between a deviation signal from the analog switch 26 or a follow-up signal from a follow-up control section 17A for the second axis 5, which will be described later, and a speed feedback signal from the tacho generator 32. and rotates the motor '51.

Then, the comparator 30 outputs a positioning completion signal to the microcomputer 20 when the movement target value from the target value register 22 and the current value from the current value register 2nd match. Each time the position data is received, the position data previously stored in the internal memory by teaching is sequentially written into the target value register 22 as a movement target value.

Therefore, if the flip-flop circuit 52 of the start/stop unit 48 for the second axis 5, which will be described later, is reset, the second axis 5 drive control device 21
controls the rotation of the motor 31 in order to cause the second shaft 5 to perform a playbank operation according to the FTP control specifications in accordance with each movement target value from the microcomputer 20 by the functions of the above-mentioned parts.

Follow-up control section 17A for second axis 5 constitutes follow-up control device 17 shown in FIG. 1 together with follow-up control section 17B for third axis 6.
is the reference value register 35. Subtractor 3G, D/A converter 3
7. Gain adjuster 38. Analog switch 3B and A
/D converter 40 and the like.

However, since the drive circuit 27 in the drive control device 21 for the second axis S plays an important role in making the follow-up control section 17A function during follow-up control, the drive circuit 27 that also serves as this drive control device 21 also performs follow-up control. This is a component of the control unit 17A.

When the enable signal is input from the microcomputer 20, the reference value register 35 receives the detection from the rotary potentiometer 41 that detects the swinging displacement of the displacement detection rod 10 in the movement detection device [9] shown in FIGS. The detection signal from the rotary potentiometer 41 is
The rocking displacement detection value obtained by successive A/D conversion by the D converter 40 is stored as a reference detection value and held until the enable signal is input again.

The subtracter 3 calculates the deviation between the value stored in the reference value register 35 and the detected swing displacement value from the A/D converter 40, and outputs the deviation value as a follow-up value.

Therefore, after the reference detection value is stored in the reference register 35, if the rocking displacement detection value changes, the follow-up value becomes a value indicating the amount of change.

The D/A conversion word 37 converts the tracking saliva from the subtraction rfi 36 into D.
/A conversion and output as an analog tracking signal.

A gain adjustment W 38 adjusts the gain of the follow-up signal from the D/A converter 37 to a required signal level.

The analog switch on the 3rd is the startup/startup for the second axis 5, which will be described later.
It turns on only when the flip-flop circuit 52 of the stop section 48 is set, and passes the follow-up signal whose gain has been adjusted by the gain adjustment r638, and outputs it to the drive circuit 27 in the single-child motion control device 21 for the second axis 5. .

The relationship between the positive and negative signs of the follow-up signal output to the drive circuit 27 and the rotational direction of the motor 31 for the second shaft 5 is such that the motor 51 always rotates in the direction that makes the follow-up signal zero. shall be stipulated in the following.

The tracking control unit 17B for the other third axis 6 constituting the tracking control device 17 is also configured in exactly the same manner as the tracking control unit [17A], and is configured with the movement detection device [1] shown in FIGS. 1 to 3.
A detection signal from a linear potentiometer 42 that detects the sliding displacement of the displacement rod 10 at 9 is input.

Further, the drive control device 21 for the second shaft 5 and the drive control device 43 for the third shaft 6, which are configured in four beds.

When the flip-flop circuit in the start/stop unit 5B for the third axis 6, which will be described later, is reset.

Rotation 1111 of the motor 44 to rotate the third shaft 6 by one rotation
Using the speed feedback signal from the tacho generator 45 attached to the rotary shaft 44a and the position feedback signal from the potentiometer 47 attached to the rotary shaft 44a via the deceleration V & 46, the third axis 6 moves each moving target 1 from the microcomputer 20. When the rotation of the motor 44 is controlled so that the playback operation is performed according to the FTP control specifications as described above, and the flip-flop circuit for the third axis 6 is set, the drive circuit as a component of the follow-up control section 17B. rotates the motor 44 in accordance with the deviation between the follow-up signal from the follow-up control section 17B and the speed feedback signal from the tacho generator 45.

The start/stop unit 48 for the second axis 5, which together with the start/stop unit 53 for the third axis 6 constitutes the start device 18 and the stop device 1 shown in FIG. 1, includes a comparator 4, a setter 50. Comparator 51
．． and set/reset type flip-flop circuit (F
F) 52 etc.

The comparator 4th is the reference value register 35 of the follow-up control section 17A.
The reference detection value stored in the A/D converter 40 is compared with the swing displacement detection value from the A/D converter 40, and the FF 52 is set when the two change from a state in which they match to a state in which they do not match.

That is, on the fourth day of this comparator, when the displacement detection rod 10 of the movement detection device S is engaged with the stationary moving body 13, the reference detection value and the swing displacement detection value match. However, when the movable body 13 that had been stopped starts to move, the detected rocking displacement value changes and the two become inconsistent, which is used to detect whether or not the movable body 13 has started moving. When the moving body 13 starts moving and the reference detected value and the rocking displacement detected value do not match, the tracking control unit 17
Set to start A (in this embodiment, turn on the analog switch 3 days).

The setting device 50 is for defining a stop value indicating a position at which the second axis 5 stops following movement.

The comparator 51 controls the follow-up control unit 17 when the current value of the current value register 29 of the drive control device 21 for the second axis 5 reaches and becomes equal to the stop value set in the setter SO.
The FF 52 is reset to stop the operation of the second axis 5, and the microcomputer 20 is notified that the current value of the second axis 5 has reached the stop value of the setting device 50.

Ftr 52 is in a reset state (Q= ``O'', Q='l
), and when set by the output of the comparator 4, the analog switch 39 of the follow-up control section 17A is turned on, and when reset by the output of the comparator 51, the analog switch 26 of the drive control device 21 is turned on.

And also the start/stop unit 53 for the other third shaft 6, which constitutes the start device 18 and the stop device 1.

It is configured exactly the same as the starting WJJ/stop part 48, and the third axis 6
It performs the same function for the follow-up control section 17B and the microcomputer 20.

A second axis 5 necessary for following movement with respect to the moving body 13
and the first axis 4 other than the third axis 6. 4th axis 7° and 5th axis 8
For each axis of motion, the ! for the second axis 5, respectively. A drive control device configured similarly to drive control device 21 is connected to microcomputer 20 .

The rotation of each drive motor is controlled.

Note that, for convenience of explanation, the follow-up control section 17B will be described below.

Drive control′! A position 43. Each part in the start/stop section 53 is designated by the same reference numeral corresponding to each part of the follow-up control section 17A, the drive control device 21, and the start/stop section 48, respectively.

Next, the operation of the embodiment configured as described above will be explained.

First, at initial startup, start/stop section 48°53 F
Since F52 has been reset, the analog switches 3S of the follow-up control units 17A and 17B for the second and third axes 6 are off, and the analog switch 26 of the drive control device 21.43 is on. , thereby including the drive control device 21.43.
Each drive control device is.

The rotation of the motors (including motors 31 and 44) for each operating axis can be controlled based on each movement target value from the microcomputer 20, and the robot 1 performs playback operations as before.

Next, when you want the robot 1 to follow the moving body 13, first attach the movement detection device 10S to the fifth axis 8, and then move the robot 1 with the moving body 13 stopped at a desired position. By playing hacking, the engagement hole 10a of the displacement sensing rod 10 of the movement detection device attached to the fifth shaft 8 is connected to the engagement pin 13a of the moving body 13 through the state shown in FIG. 6 and then to the state shown in FIG. Engage as shown.

When a positioning completion signal indicating that the playback related to the engagement operation of the robot 1 has been completed is input to the microcomputer 20 from each drive control device of the first axis 4 to the fifth axis 8, the microcomputer 20 is the second. An enable signal is output to the reference register 35 of the follow-up control units 17A and 17B for the third axes 5 and 6.

When this enable signal is output to each reference register 35, the rocking displacement detection value and the sliding displacement detection value at that time are fixedly stored as respective reference detection values.

The moving object 16. starts moving in the direction of the arrow Z at the required timing by the action of a control device (not shown), the displacement detection rod 10 of the movement detection device @S swings and slides to detect each reference detection value and swing displacement. Since the value and the detected sliding displacement value do not match, the start/stop means 48.
By immediately setting the FF 52 and turning on the analog switch 3 of the follow-up control units 17A and 17B, the comparator 4 of No. 53 starts the follow-up control units 17A and 17B, and the analog switch 26
is turned off to stop the operation related to positioning control of both bags [21, 43].

Then, when the moving body 13 moves, the tracking control section 17A.

When +7B starts, each subtracter 36 outputs a follow-up value according to the change in each detected value, and a follow-up signal corresponding to the follow-up values is sent to the drive control device [
The signal is input to the drive circuit 27 of 21, 43, and each motor 31, 44 rotates.

When the motor '51.44 rotates, the second shaft 5 and the second shaft 6 move so as to make each of the above follow-up values zero, in other words, to cancel out the rocking and sliding displacement of the displacement detection rod 10. Therefore, the base body 11 of the transfer and saving device S, which is the tip of the robot 1, follows the moving body 13.

In this way, even if the moving body 13 moves continuously in the Z direction, the tip of the robot 1 can smoothly follow the moving body 13.

Note that the second axis 5 and the third axis 6 are performing follow-up operations.

1st axis4. Fourth axis7. Each drive control device of the fifth link 8 continues positioning control in a state where the target value = current value so that the displacement detection rod 10 maintains the attitude of engagement with the moving body 13 when it is stopped. do.

Then, the following movement with respect to the moving body 13 progresses, and the second axis 5
When the current value of the third axis 6 reaches the stop value of the setter 50 of the start/stop unit 48.53 and they match, each comparator 51 immediately notifies the microcomputer 20 of this fact and also turns the FF 52 on. Reset and follow-up control section 17A
, 17B, the operation of these devices is stopped, and the drive control device 21.43
The analog switch 26 is turned on so that each positioning control can be performed.

Then, when each comparator 51 informs that the current values of the second axis 5 and the third axis 6 have reached their respective stop values, the microcomputer 20 immediately plays back the required movement axes of the robot 10, for example. A process is performed in which the displacement detection rod 10 of the movement detection device W9 attached to the fifth shaft 8 is separated from the moving body 13.

In this way, in this embodiment, not only can a smooth follow-up operation be easily realized, but also its starting and stopping can be performed automatically, also serving as control switching with normal positioning control (FTP control).

In addition, in the above embodiment, the engagement hole 10a and the engagement pin 13a
We have described an example in which the moving body 13 and the displacement detection rod 10 are engaged with each other by using the above method, but if the above-mentioned disengagement operation cannot be performed quickly, it is possible to use a strong electromagnet instead, for example, to detect the displacement. It is preferable to provide it on the rod 10 so that the two can be engaged by the electromagnetic attraction force generated by the electromagnet.

In this way, the engagement between the two can be immediately released by simply turning off the excitation current of the electromagnet.

In addition, in the above embodiment, the FF is
52 is set, but in addition to this, it may be set by a movement start command of the moving body 13.

Furthermore, in the above embodiment, the arrow Z direction in which the moving body 13 moves is parallel to the one coordinate axis direction in the three-dimensional space, and is parallel to the plane formed by the second axis 5 and the third axis 6 of the robot 1. Although it was described above that the direction is also parallel to the floor surface 2, such a condition does not necessarily have to be provided.

However, when the robot 1 is moved to follow a moving body moving in any direction in three-dimensional space without applying the above conditions, the displacement detection frame 10 in the movement detection device S is set in the direction of the arrow θX in FIG. In addition to being able to swing in orthogonal directions,
A rotary potentiometer for detecting the swing displacement is provided, and a follow-up control section configured similarly to the follow-up control sections 17A and 17B is also provided in the drive control device for the first shaft 4 of the robot 1, and this follow-up control section is provided with the above-mentioned follow-up control section. 6. It is also necessary to input the detection signal of the rotary potentiometer. In the above embodiment, the robot 1 having a four-bar parallel linkage mechanism having five operating axes has been described as an example, but the present invention is not limited to this and can be applied to any type of multi-axis robot.

〔Effect of the invention〕

As explained above, according to the present invention.

A movement detection device equipped with a plurality of displacement detectors, each of which detects movement in a plurality of directions of a displacement detection frame that can engage with a moving body and is movable in a plurality of directions, at the tip of a robot having a plurality of motion axes. is attached, and the displacement detection frame of the attached movement detection device is engaged with the moving object to move the moving object. Based on the detection values of the plurality of displacement detectors when the moving object is moved, the following movement to the moving object is performed. In addition to driving and controlling each of the plurality of necessary movement axes, the following movement is started in synchronization with the start of movement of the moving object, and the current values of each of the plurality of movement axes necessary for the following movement are set in advance. Since the following movement is stopped when each predetermined setting value is reached, the robot can not only follow the moving object very easily and smoothly, but also have the ability to start and stop the following movement. can be performed automatically, and follow-up control can be performed without fundamentally changing the PTP control method.

If it is possible to automate the start and stop of the follow-up operation, it is also possible to automate the switching between control and normal positioning control as in the embodiment.

[Brief explanation of drawings]

FIG. 1 is an overall configuration diagram showing an embodiment of the present invention. FIG. 2 is an enlarged view showing the structure of the movement detection device S, and FIG. 3 is a diagram illustrating an engagement mode between the displacement detection frame 10 of the movement detection device 9 and the moving body 13. FIG. 4 is a diagram for explaining the tracking principle of this invention, and FIG.
The figure is a block diagram showing one embodiment of the control system of the present invention. 1...Robot 3...Base 4...-th axis 5...Second axis 6...Third axis
7...Fourth axis 8...Fifth axis Day...
-Movement detection device 10...displacement detection frame 10.・・・
・Engagement hole 15...Movable body 13a...Engagement pin 17...Following control device 17A, 17B...Following control unit 18...Starting device 1st...Stopping device 2L
43...Drive control device 31.44...Motor 48.53...Start/stop section Fig. 3 Fig. 4

Claims (1)

[Claims] 1. In a robot having a plurality of motion axes, a displacement detection rod that can be engaged with a moving body and movable in a plurality of directions, and a plurality of displacement detectors that respectively detect movements of the displacement detection rod in the plurality of directions. a movement detecting device provided with the robot is attached to the tip of the robot, and each of the plurality of displacement detectors detects when the moving body is moved by engaging a displacement detection rod of the movement detecting device with the moving body. a follow-up control means that sequentially drives and controls each of a plurality of motion axes necessary for follow-up movement with respect to the movable body based on the value; and a starting means that starts the follow-up control means in synchronization with the start of movement of the movable body; The robot is characterized in that it is configured to include a stop means for stopping the operation of the follow-up control means when the current values of the plurality of motion axes necessary for the follow-up movement reach respective predetermined set values. control device.