JPH0250482B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a method for controlling a two-arm multi-axis robot.

In recent years, welding, painting,
In response to the strong demand for automation of assembly, processing, inspection, measurement processes, etc., various types of industrial robots are being actively developed.

The mainstream industrial robots are so-called intelligent robots that can act autonomously and have various motion, recognition, and sensory functions similar to those of human upper limbs, for example. It's coming.

In the case of such intelligent robots, in order to have movement functions similar to those of human upper limbs, for example, two robots with multiple rotation axes arranged symmetrically in imitation of humans are used. It may take the form of a two-arm multi-axis (multi-joint) robot with two arms.

Therefore, in this invention, such two
The purpose of this study is to provide a control method suitable for a multi-axis arm robot.

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

First, with reference to FIGS. 1 to 3, an overview of the basic operation of the numerical control function generation IC used in the embodiment of the present invention and how it is used in this embodiment will be outlined.

The numerical control function generation IC (hereinafter simply referred to as "IC") shown in FIG. 1 performs internal arithmetic processing according to the coordinate values and function form set from the outside.
It generates interpolation pulses on the X and Y axes that accurately trace the locus of the function to a set coordinate value.

In other words, target data (X _i , X _i ) is input from the data input terminals D ₁ to _{D 7} of this IC, and a calculation command pulse of a predetermined period is input to the calculation command pulse input terminal.
When fp is input, for example, if the set function form is a straight line, an interpolation pulse of the number of pulses corresponding to the X _i coordinate value is generated from the X-axis output terminal D _x in synchronization with the calculation command pulse fp as shown in Figure P _x is an interpolated pulse P _y from the Y-axis output terminal D _y with the number of pulses corresponding to the Y _i coordinate value.
are output respectively.

Therefore, by using these interpolation pulses P _x and P _y (the number of pulses indicates the amount of movement), for example, the driving part of an XY plotter can be moved from the origin O in FIG. 2 to the coordinates (X _i , Y _i ) can be moved in a straight line as shown by the dashed line.

By the way, the interpolation pulse output from this IC
The periods of P _x and P _y are determined by the period of the calculation command pulse fp, so if the period of the calculation command pulse fp is varied, the period of the interpolation pulses P _x and P _y will also change.

For example, if the period of the calculation command pulse fp is varied as shown in FIG. 3, the period of the interpolation pulse P _y changes as shown (the same applies to the interpolation pulse P _x ).

Therefore, the interpolation pulse P _x whose period is varied by controlling the period of the calculation command pulse fp,
Using P _y , the speed of the motor can be controlled.

Therefore, in the embodiments of the present invention described below, this IC is used as speed control means.
However, since each axis of the two-arm multi-axis robot, which is the premise of this invention, is driven by one drive motor, only the interpolation pulse _Py is used.

Note that the number of pulses of the interpolation pulse P _y is uniquely determined by the coordinate data (X _i , X _i ) given to the IC, so the amount of movement of the axis can be controlled using only this interpolation pulse P _y , so the speed Along with the control, position control can also be performed.

In addition, as an IC that performs the above-mentioned function,
For example, there is "KM3701" manufactured by Tokosha.

Next, the types of speed modes in this invention will be explained with reference to FIG.

(a) Speed mode As shown in Figure 4 A, speed from zero to speed v
After accelerating to this speed, this speed ν is held for a certain period of time, and then the speed is decelerated to zero.

(b) Speed mode As shown in Figure 4 (b), from speed zero to speed v
After accelerating to this speed, this speed v is maintained.

(c) Speed mode As shown in FIG. 4C, the speed v is maintained.

(d) Speed mode As shown in Figure 4 (d), after holding the speed v for a certain period of time, the speed is decelerated to zero.

The above speed modes ~ are the speed modes of the drive motors that drive each axis of the robot's two arms, and the variable cycle patterns of the calculation command pulses that are input to the IC that drives and controls the aforementioned drive motors. (speed pattern).

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

In the figure, a central control device 1 composed of a computer etc. sends movement data and the aforementioned speed mode and its speed mode to an axis drive dedicated control section 2 that drives and controls each axis of the two arms of the robot. It transfers speed pattern data corresponding to the robot, performs signal processing of sensors installed in various parts of the robot (not shown), and performs sequence control according to the work of the robot.

In particular, each data to be transferred to the shaft drive dedicated control section 2 shall take the following form.

That is, in the two-arm multi-axis robot of the present invention, a plurality of intermediate points (the longer the movement distance, the more intermediate points) are set between the movement start position of each axis of the two arms and the target position. The movement data of each axis indicates each intermediate point and target position, respectively.
Further, one type of speed mode and its speed pattern data are determined for each movement route sequentially connecting the movement start position, each intermediate point, and the target position.

The axis drive dedicated control unit 2 includes a central processing unit (CPU) 3 and a program memory (ROM) as shown in the figure.
4, consists of a microcomputer consisting of a data memory (RAM) 5, an input/output device (I/O) 6, etc., and checks the data transferred from the central control unit 1 according to the program stored in the ROM 4. and speed constant setting circuits 7, 8 and IC R ₁
~IC _Ro , IC L ₁ ~ Data transfer to IC _Lo (details will be described later).

IC R ₁ to IC _Ro and IC L ₁ to IC _Lo are the numerical control function generating ICs explained in FIG. Used as a drive control means to control rotation amount and speed.

That is, IC R ₁ ~ IC _Ro , and IC L ₁ ~ IC _Lo
Data input terminals D ₀ to _{D 7} receive movement data x ₁ to x _2o in the form of coordinate data (X _i , Y _i ) indicating the intermediate points and target positions from the axis drive dedicated control unit 2, respectively.
are inputted sequentially at a predetermined timing, and the calculation command pulses are input from the speed constant setting circuits 7 and 8 to each calculation command pulse input terminal.
Interpolation pulses P _YR1 to P YRo and _{P YL1 to} which can control the rotation amount and rotation speed of each drive motor by the pulse number and cycle as described above by f _PR and f _PL .
P _YLo are output respectively.

The speed constant setting circuits 7 and 8 for the right side and left arm are as follows:
Based on the speed pattern data S _DR and S _DL that correspond to any of the speed modes shown in Fig. 4 A to D transferred from the shaft drive dedicated control section 2, fixed-period clock pulses of the respective internal clock pulse oscillators are generated. The speed pattern daughters S _DR and S _DL are formed and output as computation command pulses f _PR and f _PL reflecting the clock pulse period, respectively, by controlling the frequency division rate of the frequency division period.

For example, in the case of the speed mode, the predetermined initial period is shortened at a predetermined rate of change to the period Tv corresponding to the speed v, and when the period reaches Tv, the period is held for a certain period of time. Then, after a certain period of time has elapsed, the cycle Tv is lengthened at a predetermined rate of change, and when the cycle reaches the initial cycle, the output is stopped.

The calculation command pulses f _PR and f _PL output from these speed constant setting circuits 7 and 8, respectively, are
It is divided into IC R ₁ to IC _Ro and IC L ₁ to _{IC Lo} .
In this way, by making the calculation command pulses equal for each arm, all the speed modes of each axis for each arm are unified, thereby realizing smooth movement of the arm.

Next, referring to FIG. 6, an outline of the control executed by the microcomputer constituting the shaft drive dedicated control section 2 will be explained.

STEP1 All axes movement data x ₁ to x _2o , speed mode data, transferred from central controller 1,
Then, the speed pattern data corresponding to the speed mode is taken in and stored in the RM5. However, each of these data corresponds to one of the plurality of intermediate points and target positions described above, and is captured in order from the movement start position.

STEP 2 In the data import process in STEP 1, the presence or absence of data is checked. If there is no data, proceed to STEP 3, and if there is, proceed to STEP 4.

STEP3 Since there is no captured data (next cycle data), if the speed mode of the execution cycle is or, perform a stop process to set the speed to zero and end the control. Note that if the speed mode of the execution cycle is or, the control is immediately terminated.

STEP4 Since there is captured data (next cycle data), set the speed mode in the execution cycle.
Read from RAM5.

STEP5~7 Check whether the speed mode of the execution cycle read in STEP4 is ~, then go to STEP8, then go to STEP9,
If so, proceed to STEP 10, then proceed to STEP 11.

STEP 8 to 11 Check the rationality of the speed mode data (next cycle data) imported in STEP 1 and the speed mode data of the execution cycle. That is, in STEP8, if the speed mode is selected or not, proceed to STEP13, otherwise proceed to STEP12. In STEP9, if the speed mode is or is not, go to STEP13, otherwise go to STEP12. In STEP10, for speed mode or if STEP13
If not, proceed to STEP 12.
In STEP11, if the speed mode is correct or not, proceed to STEP13, otherwise proceed to STEP12.

Note that such a check is performed if there is an error when programming the speed mode in the central controller 1, or if there is an error during data transfer between the central controller 1 and the shaft drive dedicated control unit 2. This is for safety measures against outbreaks. It is obvious that the drive motor cannot be driven according to the speed mode with combinations other than those described above.

STEP12 Since the speed of the next cycle is unreasonable, for example, if the speed mode is
Forcibly change Nara, Nara, and Nara. In addition, ni, ni,
You can change it to and . Then, when the change processing is completed, the process advances to STEP13.

STEP13 At the end of the execution cycle, transfer the speed pattern data S _DR and S _DL corresponding to the reasonable speed mode of the next cycle to the speed constant setting circuits 7 and 8, respectively, and transfer the movement data x _{1 to 2o} . IC R ₁ ~ _{IC Ro} , and IC L ₁ ~ IC _Lo
, return to STEP 1, and repeat the above steps. In addition, the pulse generation end signal is output at the end of the execution cycle, when IC R ₁ ~ IC _Ro and IC L ₁ ~ IC _Lo finish outputting the interpolation pulses P _YR1 ~ P _YRo and P _YL1 ~ P _YLo , respectively. Can be detected by

By the way, the speed mode corresponding to the first intermediate point is unreasonable, so as a countermeasure, check whether the speed mode imported between STEP1 and STEP2 is the first one. If it is the first one, go to STEP 2, if it is the first one, select that speed mode or time.
All you have to do is add a step group to advance to STEP 12 or STEP 2, respectively.

In addition, in the flowchart shown in Figure 6, each data corresponding to one position is imported in STEP 1, but after importing each data corresponding to all positions at once, it is necessary to import each data corresponding to all positions at once, and then The rationality of the speed mode may also be determined.

As described above, the control method according to the present invention is suitable as a control method for a two-arm multi-axis robot.

[Brief explanation of drawings]

1 to 3 are diagrams for explaining the outline of the basic operation of the numerical control function generation IC used in the embodiment of the present invention and how to use it in the embodiment of the present invention. , respectively, are diagrams for explaining the types of speed modes in this invention, FIG. 5 is a block configuration diagram of a control device showing one embodiment of this invention, and FIG. 6 is a diagram for explaining the shaft drive exclusive control of FIG. 5. FIG. 2 is a flowchart showing an example of a program executed by a microcomputer that constitutes the unit. 1... Central control unit, 2... Axis drive dedicated control unit, 7, 8... Speed constant setting circuit, R ₁ ~ R _o , L ₁
~L _o ...Function generation IC for numerical control.

Claims (1)

[Claims] 1. In a two-arm multi-axis robot, a plurality of intermediate points are set between the movement start position of each axis of the two arms and the target position, and each intermediate point of the movement start position and the target position are set. The speed modes of each axis of each arm for each movement path connected in sequence are all unified into one of the following four types (a) to (d), and each speed mode information is transmitted from the central control unit of the robot to the axis. In addition to transmitting the data to the drive-only control unit, in this axis drive-only control unit, the speed mode of the next cycle for the speed mode during the execution cycle is set to (a) or (b) for (a) below.
(c) or (d) for the following (b), (c) for the following (c)
or
(d) For (d) below, enable the speed mode for the next cycle only when (a) or (b), and in other cases, force the speed mode to be changed to a rational speed mode as unreasonable. A method for controlling a two-arm multi-axis robot. (a) After accelerating from zero speed to a constant speed, maintain the constant speed for a certain period of time, and then decelerate to zero speed. (b) After accelerating from zero speed to a constant speed, maintain the constant speed. (c) Maintain a constant speed. (d) After maintaining a constant speed for a certain period of time, the speed is reduced to zero.