JP2529280B2 - Control device for articulated robot - Google Patents

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for an articulated robot used for assembly work and the like.

2. Description of the Related Art In recent years, articulated robots have become popular as a leader in automation of production, and higher speed positioning is desired.

In the current positioning of articulated robots, compensation for positional deviation due to temperature drift of the hardware part of the control device, gravity or reaction force of the cable connected to each arm, that is, offset compensation is performed for each posture of the robot. There is a problem that it needs to be performed. In the past, in order to compensate for this offset, an integral term proportional to the sum of deviations between the target position for positioning and the current position was added to the speed command.

Problems to be Solved by the Invention However, in the above-mentioned configuration, since the offset compensation is performed by the integral term for each positioning, the compensation time is included in each positioning time, and as a result, the entire positioning time becomes long. Had. Fourth as a reference
The relationship between the positioning time and the moving position is shown in FIG. A when no offset compensation is performed, in FIG. B when offset compensation is performed by the integral term, and in FIG.

In view of the above problems, the present invention replaces the convergence value of the integral term at the time of positioning with the offset compensation term,
A controller for a multi-joint robot, which is characterized in that offset compensation is automatically performed and compensation time by an integral term at the time of positioning is shortened.

Means for Solving the Problems In order to solve the above problems, the first control system for an articulated robot according to the present invention, as shown in FIG. 1, is proportional to the deviation between the target position and the current position. 1, a proportional term calculation means for calculating the speed command, an integral term calculation means for calculating a second speed command proportional to the sum of the deviations, a convergence determination means for determining the convergence of the second speed command, and a convergence determination. An offset term calculating means for adding the convergence value at time to the third speed command and subtracting it from the second speed command; and storing the third speed command for each joint for each posture of the articulated robot. An offset term storage means is provided.

As shown in FIG. 2, the control device for the second articulated robot according to the present invention includes a proportional term calculating means for calculating a first speed command proportional to the deviation between the target position and the current position, and a sum of the deviations. An integral term calculation means for calculating a second speed command proportional to, a convergence determination means for determining the convergence of the second speed command,
Offset term calculation means for adding the convergence value at the time of convergence determination to the third speed command and subtracting it from the second speed command, and storing the third speed command for each joint for each posture of the articulated robot By using the stored offset term storage method and the third speed instruction stored by the offset term storage means, the third undetermined speed instruction in the vicinity is predicted, and the temporarily determined value is offset stored. It is provided with an offset term predicting means to be stored in the means.

The control device for a third articulated robot according to the present invention, as shown in FIG. 3, calculates the first term of speed proportional to the deviation between the target position and the current position, and a proportional term calculation means for calculating the sum of the deviations. An integral term calculating means for calculating a proportional second speed command, and a convergence determining means for determining the convergence of the second speed command,
Offset term calculation means for adding the convergence value at the time of convergence determination to the third speed command and subtracting it from the second speed command, and storing the third speed command for each joint for each posture of the articulated robot Offset term storage means and offset abnormality determination means for determining the offset abnormality and outputting a warning when the peak value or the average value of the third speed command stored in the offset storage means exceeds the set region. It is equipped with.

Action In the first aspect of the invention, once the offset compensation is performed by the integral term, the converged value is stored in the offset compensation term storage means and added to the speed command. Therefore, in the subsequent operation, there is the same condition that there is no offset deviation. Therefore, the offset compensation time by the integral term is reduced. That is, the positioning time can be shortened, and the effect is particularly great in repeated work such as cycle operation. Also, even if offset deviation occurs during operation due to temperature change, change over time, etc., the integration term at that point is automatically added again to the offset compensation term, so stable operation can be obtained without extending the positioning time. .

According to the second aspect of the invention, the offset term in the robot posture that is not calculated using the calculated offset term can be predicted and tentatively determined, so that the offset compensation time by the integral term can be shortened by the tentatively determined amount. Becomes That is, the initial offset term calculation time can be shortened.

In the third invention, when the average value or the peak value of the offset term is out of the allowable range, a warning is automatically output. Therefore, the hardware offset of the control device is reviewed before a serious problem occurs, and the mechanical system It is possible to know the optimum time for abnormality inspection, which can be an effective means of safety measures.

Example A control device for an articulated robot according to an example of the present invention will be described below with reference to the drawings.

FIG. 5 is an overall configuration diagram when the control device for the articulated robot in this embodiment is configured by software.
In FIG. 5, 8 is a proportional term calculation routine that is a proportional term calculation means, 9 is an integral term calculation routine that is an integral term calculation means, 10 is a convergence determination routine that is convergence determination means, and 11 is an offset term calculation means. Offset term calculation routine,
12 is an offset term memory that is offset term storage means,
Reference numeral 13 is an offset term prediction routine which is offset term prediction means, and 14 is an offset abnormality determination routine which is offset abnormality determination means.

Assume a two-axis robot with a movable range of ± 160 ° for each axis, and position each axis by outputting a speed command calculated from the deviation per unit time. Let us proceed with the example.

The proportional term calculation routine 8 calculates a first speed command V _CMD1 which is a basic positioning speed command. The deviation Ri, where Ti is the target position at time i and Ci is the current position, is expressed by the equation (1) Ri = Ti-Ci ………… (1), and V _CMD1i is the proportional gain P _P. V _CMD1i = P _P × Ri ... (2) A flow chart of the proportional term calculation routine 8 is shown in FIG.

Next, the integral term calculation routine 9 calculates an integral term for offset compensation. Assuming that the total deviation is Si, the relationship with the previous total deviation Si-1 is expressed by equation (3).

Si = Si-1 + Ri (3) If the proportional gain with respect to the sum Si of these deviations is Pi, the second speed command V _CMD2i will be V _CMD2i = Pi x Si _...... (4). Here, the second speed command V _CMD2i is for offset compensation, and basically, since it is sufficient to add it to the speed command only when the deviation enters within a certain range W, the total deviation Si is (3) ′. We will define it with an expression.

Si = Si-1 + Ri (| Ri | <W) = 0 (| Ri | W) ... (3) 'FIG. 7 shows a flow chart of the integral term calculation routine 9.

The convergence determination routine 10 determines whether the second speed command V _CMD2 has converged to the same value for a certain period of time. Specifically, _while the value of V _CMD2 is the same as that of the previous time, the convergence determination counter CNT is incremented, and if the set number of times CNTX is reached, it is determined that the convergence has been reached. _When the value of V _CMD2 changes, CNT is cleared to zero. FIG. 8 shows the convergence determination routine 10
The flow chart of is shown.

If the convergence determination routine 10 determines that the second speed command V _CMD2 has converged, the offset term calculation routine 11
Then, the value of V _CMD2i at that time is _added as the offset term to the third speed command V _CMD3 which is the offset term currently stored in the offset term memory 12, and the second speed command V _CMD2i and the total sum Si of the deviations are cleared to zero. To do. The offset term memory 12 is divided and stored for each axis according to the posture of the robot, but here, as an example of the offset term memory for one axis, division as shown in Table 1 is assumed. For example, if the robot posture at the target value when the second speed command V _CMD2i converges is 1 axis = -80 °, 2 axes = -150 °, the offset term is added to V _{CMD3 (3.1)} . FIG. 9 shows a flow chart of the offset term calculation routine 11.

First and second speed commands calculated by the above routine
V _CMD1i, the V _CMD2i and third velocity command V _CMD obtained by adding the speed command V _CMD3 that are stored in the offset term memory,
Positioning of each axis is performed by outputting the data at every unit time.

After the above processing is performed for each positioning point and the convergence value of the integral term is stored in the offset term memory 12, the offset compensation time by the integral term is reduced, and as a result, the positioning time is shortened.

Next, a means for reducing the initial offset term calculation time will be described. When the offset term is considered in a divided area for the robot posture as shown in Table 1, it is considered that the adjacent term continuously changes. Ie 1
If the offset term of one region is determined, it is highly possible that the neighborhood also has the same offset term value. When the offset term is stored in the offset term memory, the offset prediction routine 13 tentatively determines the same area and stores the neighboring area where the offset term is not determined. If the stored areas are scattered, the values between the interpolated areas are stored by linear interpolation. FIG. 10 shows an example of provisionally determining the offset term. The area that is in contact with the determined area is the primary neighborhood area, and the area that is one away is the secondary neighborhood area.
The flow chart of the offset prediction routine 13 is shown in the figure.

By performing the above-described processing, it is possible to predict the region in which the offset term is undecided to be a close value and to make a provisional decision, so that the convergence time by the initial offset compensation time integration term can be shortened, that is, the initial offset term calculation time can be shortened.

Next, the offset abnormality determination routine 14 will be described. If the average value or the peak value of the offset term exceeds the allowable range, the hardware of the control device may be deteriorated or the robot mechanism system may be abnormal, which may lead to a failure or an accident. Therefore, it is effective for safety to detect the abnormality before the average value or peak value of the offset term goes out of the allowable range. The average of the offset terms stored in the offset term memory 12 is AV _CMD3 , and the allowable average value is A
V _max , peak value of offset term PV _CMD3 , allowable peak value
When PV _max is set, the warning output conditions are shown below.

FIG. 12 shows a flow chart of the offset abnormality determination routine 14.

The offset abnormality can be detected by performing the above processing every time data is written in the offset term memory 12.

EFFECTS OF THE INVENTION As described above, the first aspect of the present invention includes the proportional term calculating means for calculating the first speed command proportional to the deviation and the integral term calculating means for calculating the second speed command proportional to the sum of the deviations. A convergence determination means for determining the speed of the second speed command, an offset term calculation means for adding the convergence value at the time of the convergence determination to the third speed command, and subtracting it from the second speed command; An offset term storage means is provided for storing the velocity command for each joint with respect to each posture of the articulated robot, and the value once offset-compensated by the integral term is stored as an offset term, so that the offset by the subsequent integral term is offset. The compensation time can be reduced and the positioning time can be shortened, and its practical effect is great.

In addition, in the second invention, offset term prediction means is added to the configuration of the first invention to predict an undetermined offset term region using the determined offset term.
Since the provisional decision can be made, the initial offset term calculation time can be shortened.

Further, in the third invention, by adding the offset abnormality determining means to the configuration of the first invention, when the average value or the peak value of the offset term is out of the allowable range, a warning is automatically output. It can be an effective means of safety measures that can be dealt with before a major problem occurs.

[Brief description of drawings]

1 is an overall configuration diagram of the first invention, FIG. 2 is an overall configuration diagram of the second invention, FIG. 3 is an overall configuration diagram of the third invention, and FIG.
FIG. 5 is a diagram showing the relationship between positioning time and movement position with or without offset, and FIG. 5 is an overall configuration diagram in an embodiment of the present invention.
6 is a flow chart of the proportional term calculation routine, FIG. 7 is a flow chart of the integral term calculation routine 9, FIG. 8 is a flow chart of the convergence determination routine, and FIG. 9 is a flow chart of the offset term calculation routine.
FIG. 10 is a diagram showing an example of provisionally determining an offset term, FIG. 11 is a flowchart of an offset prediction routine, and FIG. 12 is a flowchart of an offset abnormality determination routine. 1 ... Proportional term calculation means, 2 ... Integral term calculation means, 3 ...
Convergence determination means, 4 ... Offset term calculation means, 5 ... Offset term storage means, 6 ... Offset term prediction means, 7
...... Offset abnormality determination means.

Claims (3)

(57) [Claims] 1. A controller for a robot, which performs positioning by a speed command per unit time to an actuator having multiple joints and driving each joint, in a target position and current position of each joint calculated for each unit time. Proportional term calculation means for calculating a first speed command proportional to the deviation, and integral term calculation means for calculating a second speed command proportional to the sum of the deviations,
Convergence determining means for determining whether or not the second speed instruction has converged to a constant value for a set time, and convergence of the second speed instruction when the convergence determining means determines that the second speed instruction has converged. An offset term calculating means for adding a value to the third speed command which is an offset compensation term and subtracting it from the second speed command, and the third speed command for each joint for each posture of the articulated robot A control device for an articulated robot, characterized by comprising offset term storage means for storing each, and obtaining a speed command by adding the first, second, and third speed commands. 2. A robot controller that performs positioning by a speed command for each unit time to an actuator that has multiple joints and drives each joint, in a target position and a current position of each joint calculated for each unit time. Proportional term calculation means for calculating a first speed command proportional to the deviation, and integral term calculation means for calculating a second speed command proportional to the sum of the deviations,
Convergence determining means for determining whether or not the second speed instruction has converged to a constant value for a set time, and convergence of the second speed instruction when the convergence determining means determines that the second speed instruction has converged. An offset term calculating means for adding a value to the third speed command which is an offset compensation term and subtracting it from the second speed command, and the third speed command for each joint for each posture of the articulated robot The third speed in the undetermined vicinity of the articulated robot is determined by using the offset term storage means stored for each and the third speed command stored in the offset term storage means. A control device for an articulated robot comprising an offset term prediction means for predicting a command, tentatively determining it, and storing it in the offset term storage means. 3. A robot controller that performs positioning by a speed command per unit time to an actuator that has multiple joints and drives each joint, and a target position and a current position of each joint calculated for each unit time. Proportional term calculation means for calculating a first speed command proportional to the deviation, and integral term calculation means for calculating a second speed command proportional to the sum of the deviations,
Convergence determining means for determining whether or not the second speed instruction has converged to a constant value for a set time, and convergence of the second speed instruction when the convergence determining means determines that the second speed instruction has converged. An offset term calculating means for adding a value to the third speed command which is an offset compensation term and subtracting it from the second speed command, and the third speed command for each joint for each posture of the articulated robot An offset term storage means for storing each of them, and a peak output or an average value of the third speed command stored in the offset term storage means exceeding the set area, respectively, is determined to be an offset abnormality and a warning is output. A control device for an articulated robot equipped with an offset abnormality determining means for performing the following.