JP2594423B2 - Industrial robot controller - Google Patents

Description

Description: BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for an industrial robot that performs a predetermined work by tracking a moving work target by a robot manipulator itself.

2. Description of the Related Art Conventionally, when working or assembling a work (hereinafter referred to as a work) mounted on a belt conveyor by an industrial robot of this type, the belt conveyor is transferred at a constant speed, and the belt conveyor is moved. A control method is employed in which a robot is fixedly arranged at a predetermined position along a line and a workpiece is processed or assembled while following a manipulator based on a transfer speed of a belt conveyor.
The industrial robot includes information such as a reference position on the work based on the taught work coordinate system, a work start position and a work end position on the work based on the work coordinate system, and a coordinate system of the taught belt conveyor. A work program including the upper conveyor reference position and work commands such as linear or circular interpolation, the speed of the hand of the manipulator, etc. is stored, and when a work start instruction is given, the position data of the work program is stored by the main program. The trajectory of the hand of the manipulator has been obtained sequentially based on the value of the designated program pointer.

[Problems to be Solved by the Invention] However, even if the transfer speed of the belt conveyor is to be kept constant, when the work is mounted on the belt, the speed decreases due to the load at the time of mounting. Sometimes. In such a case, in order to increase the transfer speed, the drive unit transfers the belt conveyor with a larger driving force, but fluctuations in the transfer speed of the belt conveyor cannot be avoided. Therefore, it is necessary to perform control corresponding to the change in the transfer speed of the belt conveyor. Generally, a method of detecting the transfer speed of the conveyor and controlling the movement of the manipulator in synchronization with the change in the transfer speed of the belt conveyor is adopted. (For example, JP-A-58-56125).

That is, clock pulses transmitted in proportion to the conveyor speed are counted, sampled at predetermined minute intervals, and controlled by treating them as the belt conveyor speed. FIG. 7 is an explanatory diagram showing the state at that time. Here, it is assumed that the actual transfer speed of the belt conveyor fluctuates in a sine wave shape. As shown,
The sampled transport speed is inside the sine wave in the region where the transport speed is increasing, and outside the sine wave in the region where the transport speed is decreasing. For this reason, an amount corresponding to the area of the hatched portion becomes an error in the movement amount, and an error occurs in the trajectory of the manipulator.
That is, a delay error appears in a region where the transfer speed is increasing, and a lead error appears in a region where the transfer speed is decreasing.

The control method for controlling the manipulator based on the sampled transfer speed has a problem that the hand of the manipulator cannot be accurately positioned on the work.

The present invention has been made in order to solve such a problem, and an industrial robot that can accurately position a hand of a manipulator on a workpiece on a conveyor even when a transfer speed of a belt conveyor fluctuates. It is an object of the present invention to provide a control device.

[Means for Solving the Problems] A control device for an industrial robot according to the present invention includes a speed detecting means for detecting a conveyor speed, and an output of the speed detecting means taken every predetermined minute time, and the speed of the conveyor is determined. Calculates the moving distance vector of the conveyor in a very short time based on and the moving direction of the conveyor, and adds the calculated result and the moving distance vector of the hand of the manipulator on the work coordinate in the very short time to obtain the tip of the manipulator in the short time. And the travel distance vector of
Based on the calculation result, a trajectory planning unit that calculates a target position vector of the hand of the manipulator after a very short time, and the output of the speed detecting means are fetched every predetermined very short time, and the current transfer speed (V _n ) of the conveyor, Transfer speed (V _n-1 ) and transfer speed (V _n-1 ) a minute before the present
Based on the transfer speed (V _n−2 ) that is a minute before the time, the first-order difference amount and the second-order difference amount indicate the transfer distance of the conveyor corresponding to the change in the transfer speed of the conveyor in the next minute time. Speed change prediction means for predicting the correction amount (V _kn ) and outputting the prediction result as a vector, and a conveyor speed for adding the output of the trajectory planning unit and the output of the speed change prediction means and outputting the addition result A compensator; and a manipulator drive controller that drives and controls the manipulator based on the output of the conveyor speed compensator.

[Operation] The present invention not only calculates the transfer distance of the conveyor based on the transfer speed taken every predetermined minute time as in the prior art, but also predicts a change in the transfer speed during the next minute time. The change in the predicted transfer speed is reflected on the transfer distance of the conveyor.

FIG. 8 is an explanatory diagram illustrating a method of estimating the correction amount (V _kn ) of the conveyor transfer distance according to the present invention. Referring to the case shown in the figure, the current (nΔt) transfer speed (V _n ) of the conveyor, a minute before the current time (n
-1) transfer in Δt velocity (V _n-1), and further based on the minute time before (n-2) takes the Δt transport speed (V _n-1), by its first-order difference amount and second difference amount The correction amount (V _kn ) of the conveyor transfer distance corresponding to the change of the conveyor transfer speed in the next minute time is predicted (hatched portion in the figure). For this reason, the transfer distance of the conveyor takes into account the change in the transfer speed, and the phenomenon that the transfer speed is delayed when the transfer speed is increased or advanced when the transfer speed is lowered as in the conventional case (FIG. 7) is avoided. I have.

FIG. 9 is an explanatory view in which the correction amount (V _kn ) of the conveyor transfer distance is reflected on the conveyor transfer distance. This shows an example in which the manipulator is moved from the current position to the next position where the robot is taught. The trajectory planning unit obtains a moving distance vector A of the conveyor in a very short time based on the moving speed and the moving direction of the conveyor taken in at a certain timing in the same manner as in the related art, and calculates the moving distance vector A and the manipulator in the short time. And the moving distance vector B of the hand of the manipulator on the workpiece coordinates is added to calculate the moving distance vector C of the hand of the manipulator in a very short time. Find D. The speed change predicting means calculates the above correction amount ( _Vkn ).
, And the conveyor speed compensator adds the two outputs to obtain an arrival position f as shown in the figure. The manipulator drive control unit positions the hand of the manipulator at the arrival position.

Embodiment FIG. 1 is an overall configuration diagram of a control device for a tracking robot according to an embodiment of the present invention. In the figure, 100
Is a robot controller, 120 is a servomotor that drives the joints of the robot manipulator, and 121 is this servomotor 1
Reference numeral 20 denotes a position detector, reference numeral 123 denotes a conveyor device, reference numeral 124 denotes a conveyor speed detecting means, for example, a conveyor speed detector, and reference numeral 125 denotes a reference position (hereinafter referred to as a work base) 126 of a work (work) on the conveyor device 123. 2) is a synchronization signal generator that generates a signal of "1" when passing through a specific position (hereinafter referred to as a conveyor base).

In the robot controller 100, 101 is a program storage unit of the robot, 102 is a program execution unit that outputs a target position, a trajectory, and a speed of the hand of the manipulator, and 103 is a target position of the hand of the manipulator from the robot coordinate reference to the work coordinate reference. The target position converter 104 for conversion detects the conveyor speed, which changes every moment, every Δt time, and when the synchronization signal generator 125 generates a signal of “1”, the distance accumulation is set to zero; The distance accumulator accumulates the conveyor transfer distance for each conveyor, 105 decomposes the conveyor accumulated distance into the direction cosine in the conveyor transfer direction based on the robot coordinate system, which is the output of the program execution unit 102, and adds the position to the position of the conveyor base To calculate the position of the conveyor base based on the robot coordinate system. By positioning adding the target position of Kubesu reference is the target position generation unit that converts the robot coordinate system based on the target point of the work on the conveyor at the current (time n.DELTA.t), 106
Is a trajectory planning unit that plans the hand position of the manipulator at a time (n + 1) Δt in the near future in order to linearly move the target point and the current hand position of the manipulator of the robot by the output of the program execution unit 102.

A conveyor speed prediction unit 107 calculates a correction amount of the conveyor moving distance for the next Δt time from the conveyor speed and the change rate thereof, and a conveyor speed vector 108 decomposes the correction amount into a direction cosine in the conveyor transfer direction. The generation unit. The conveyor speed prediction unit 107 and the vector generation unit 108
Constitute speed change prediction means which is a feature of the present invention.

109 is a conveyor speed compensator that adds the position of the output of the trajectory planning unit 106 and the output of the vector generator 108.110 is a controller that gives each joint angle of the manipulator to realize the position of the hand of the given manipulator. A manipulator coordinate conversion unit that outputs an angle to a positioning control unit, which will be described later, 111 is a positioning unit that is attached to the manipulator and outputs an error voltage so that a value of a position detector 121 that detects a joint angle of the manipulator matches a specified angle. The feedback control unit 112 is a current amplification unit that converts and amplifies the error voltage into a servo motor drive current.

Next, the operation of the control device shown in FIG. 1 will be described with reference to the following drawings. FIG. 2 is an explanatory diagram showing the hand of the manipulator and the moving position of the work, and FIG. 3 is a flowchart showing the operation of the robot control device. The processing units indicated by reference numerals 301, 301a, 302, 303, and 304 in FIG. 3 indicate the processing contents of the program execution unit 102 in FIG. FIG. 4 is a flowchart showing details of the movement control unit in FIG. 3, FIG. 5 is an explanatory diagram showing an example of a work program stored in the program storage unit 101, and FIG. FIG.

First, an example of the overall processing flow for positioning the hand of the manipulator on the work placed on the conveyor will be described with reference to the flowchart of FIG. Here, it is assumed that the hand of the manipulator is moved linearly in accordance with the work program shown in FIG. 5, and the path shown in FIG. 2 is moved. In the example of FIG. 2, the work path of the work is taught at a work position A (position indicated by a dashed line) corresponding to the teaching station. The work is placed on the conveyor 123 and transferred, and during the transfer, the hand of the manipulator is moved along the work path (131, 132, 133). . Note that the program itself in FIG. 5 is a general program conventionally used in this type of control device.

(1) The pointer initialization unit 301 is a program pointer 101a
(FIG. 5) is substituted with 0.

(2) The program calculation execution unit 301a extracts the instruction code indicated by the program pointer 101a. That is, in the example of FIG. 5, the work base definition instruction code “00” is extracted, and when the program operation execution unit 301a determines this “00”, the work base value stored next to the instruction code is changed to the sixth. Assign to the work variable P _{WBS in the} figure.

(3) The instruction determination unit 302 branches to the end when the instruction code is “99” of the end instruction code, branches to the label MOVS when it is determined to be “04” of the movement instruction code, and otherwise. Branches to label NEXT. In this example, branch to the label NEXT.

(4) The program pointer incrementing unit 305 adds “1” to the program pointer 101a and branches to the label Mloop.

(5) In the label Mloop, the program operation execution unit 301a is again executed
There is running, the same procedure as that in (2), a work base value is substituted into the work variable P _CBS in FIG. 6, (3),
By the same processing as (4), the program pointer 101a becomes "2".

(6) The program calculation execution unit 301a extracts 02, which is the conveyor vector definition code, and performs the following calculation processing.

The result of this operation is assigned to the work variable _PCVCT1 in FIG. This is the direction cosine of the conveyor transfer direction vector as viewed from the robot coordinate system. Next, the same processing as (3) and (4) is performed, and the program pointer 101a becomes "3".

(7) The program calculation execution unit 301a extracts “03”, which is the hand speed definition code of the manipulator, and substitutes the work base value into the work variable F in FIG. Then
The same processing as (3) and (4) is performed, and the program pointer 101a becomes "4".

(8) The program calculation execution unit 301a extracts the linear movement code “04”, substitutes the target position of the hand of the manipulator into the work variable P _dest in FIG. 6, and performs the label MOVS based on the processing of (3). Branch to

(9) The target position conversion unit 103 creates the work variable P _dest D by performing the following calculation on the work variables P _dest and P _WBS .

_{P dest D = (P WBS)} -1 (P dest) Here, the inverse matrix of P _WBS superscript -1 P _WBS,
The operation + indicates a matrix product, which is hereinafter referred to as position addition. The work variable P _dest D that is the result of the above calculation is
It represents the target position based on the work variable P _WBS indicating the work base, and as a result of the position addition, it becomes P _{dest in} FIG.

(10) Next, the processing shifts to the movement control section 400, and the processing is repeatedly executed until the movement completion determining section 303 determines that the processing is completed.

(11) When the movement is completed, the movement completion determining unit 303 branches to the label NEXT, and the program pointer 101a becomes “5”.

In (12), the program calculation execution unit 301a extracts the end code 99, returns to the process of (3), branches to the end, and completes the operation.

The above control operation is performed by a conventional control device (for example,
No. 56125) does not differ from the positioning control.
Next, details of the movement control unit 400 will be described with reference to the flowchart of FIG. 4 to clarify the features of the present invention.

Next, based on the flowchart of FIG.
The operation of 400 will be described.

(13) The Δt timer unit 401 (see FIG. 1) controls the movement control by Δ
A timer for realizing discrete-time control for each t. Hereinafter, a current time is represented by nΔt, and a past time is represented by (n−
1) At and the near future time are called (n + 1) At.

(14) (see FIG. 1) conveyor speed input unit 402 substitutes substitutes the value of the working variable V _n-1 to V _n-2, also the value of V _n to V _n-1, (n- 1) the transfer distance between the transfer distance or Δt conveyor from Δt to nΔt enter the conveyor speed detector 124 of FIG. 1 is substituted into V _n.

(15) In the transfer distance accumulator 104, the synchronization in the first view after substituting the value of the work variable sigma _n-1 to the sigma _n signal generator 12
The state of 5 is substituted into the work variable σ _n .

(15a) If σ _n-1 = 0 and σ _n = 1, 0 is substituted into the work variable 1 _n representing the transfer distance from the conveyor base to 1 _n
= 0. That is, when the workpiece reaches the synchronization point, synchronization signal generating unit 125 resets the work variable 1 _n to detect it. In the example of FIG. 2, the work variable _1n is reset when the work is transferred by the conveyor 123 and reaches the work position B.

(15b) If σ _n-1 = 0 and σ _n-1 = 1, then 1
and _n = 1 _n + V _n, accumulating conveyor transfer distance of time n · Delta] t.

(16) The target position generation unit 105 performs the following operation on the work variables _PCVCT , P _dest and the above 1 _{n as} a result of the processes (6) and (9), and outputs the result to the work variable P _destn Substitute for

P _destn = (P _CBS ) ｛(P _CVCT ) (1 _n )｝ (P _dest D) The work variable P _destn sets the target position on the workpiece at time nΔt with the conveyor base (P _CBS ) as the starting point and the conveyor transfer direction ( P _CVCT ) The position moved by the distance 1 _{n in the} direction, that is, the second position
In the example shown in the figure, a work position C which has been moved by a distance 1 _n from the work position B is shown, and the tip of the manipulator is positioned at the teaching point 131 at this point.

(17) the trajectory planning unit 106, the (7) of the results and work variable F indicating the moving speed of the workpiece is, the target position is the output of the current value P _n of the end of the time _nΔt (16) P destn Is operated as in the following equation to create a near future value at the time (n + 1) Δt and substitute it for the work variable P _{n + 1} .

In the above equation, the second item on the right side is a moving distance vector of the hand of the manipulator on the workpiece coordinate in a very short time, and the third item is a moving distance vector of the conveyor in a very short time.

In addition, the moving distance vector of the hand of the manipulator in this minute time is obtained by adding the above-described second item and third item.

In the above equation, P _{n + 1} on the left side is a target position vector of the hand, and Δt indicates a minute time.

Here, if P _{n + 1} 0) P _destn is exceeded, P
_{n + 1} = P _destn and the movement completion flag are set to ON and the above (10)
Contributes to the processing.

(18) On the other hand, the conveyor speed prediction unit 107 performs the following operation on the work variables V _n , V _n−1 and V _n−2 output from the above (15), and performs the following operation from the time nΔt to (n + 1) Δt. A correction amount V _kn (see FIG. 8) of the conveyor transfer distance between them is predicted. Correction amount V _kn
Is represented by the following equation.

_{V kn = K 1 (V n} -V n-1) + K 2 {(V n -V n-1) - (V n-1 -V
_n-2 )｝ V _n : The transfer speed of the belt conveyor taken in by the encoder at time (nΔt) V _n-1 : The transfer speed of the belt conveyor taken in by the encoder at time (n-1) Δt V _n-2 : Encoder Is the transfer speed of the belt conveyor taken at time (n−2) Δt, K ₁ , K ₂ : constant (forward gain) 0 ≦ K ₁ ≦ 2-3, 0 ≦ K ₂ ≦ 2-3 In the first section “K ₁ (V _n −V _n-1 )”,
(V _n −V _n−1 ) is a first-order difference. For example, if K ₁ is the reciprocal of the sampling period (: Δt), the first term is a first derivative.

The second term “K ₂ ｛(V _n −V _n−1 ) − (V _n−1 −V
_n-2 )｝ ”, ｛(V _n −V _n−1 ) − (V _n−1 −
V _n−2 ) _２ is a second-order difference, and if K ₂ is the reciprocal of the square of the sampling period, the second term is a second derivative.

As shown in FIG. 8, the correction amount (V _kn ) of the conveyor transfer distance obtained here is based on the sampled transfer speed in both the region where the conveyor transfer speed is increasing and decreasing. The transfer distance is appropriately corrected.

(19) The conveyor speed vector generation unit (108) predicts that the conveyor will be transferred between (n + 1) Δt from time nΔt by performing the following operation on the work variable V _kn and the output _PCVCT of (6). The working variable Pv _kn is obtained by decomposing V _kn , which is the correction amount of the distance, into vector components based on the robot coordinate system.

Pv _kn = (P _CVCT ) (V _kn ) (20) The conveyor speed compensator 109 _{compares the} work variable Pv _kn with (1
The following operation is performed on P _{n + 1} which is the output of 7), and the time n
Compensation for the conveyor transfer between Δt and (n + 1) Δt is performed.

P _{n + 1} = (P _{n + 1} ) (Pv _kn ) Note that the above equation is represented by the vector in FIG.

(21) The manipulator coordinate conversion unit 110 calculates the work variable P
_The indirect angle J _{n + 1} of the manipulator whose hand is equal to _{n + 1} is calculated and output to the positioning control unit 111.

_{Jn + 1} = g ( _{Pn + 1} ) Here, g is a function for obtaining an angle from the position and orientation of the manipulator at hand.

Further, by substituting P _{n +1} into the current value of the tip of the manipulator at the time (n + 1) Δt to make P _n = P _{n + 1} , the tip of the manipulator can be moved while following the work being transferred. Will be drawn on the work. Here, in the process where the work moves from the work position C to the work position D, the hand of the manipulator moves on the straight path 132.

That is, in the example of FIG. 2, as described above, the work is moved at the work position A corresponding to the teaching station from the work path (from the teaching point 131 to the teaching point
133) is taught, the work is placed on the conveyor 123 and moved, and the work base 12 of the work is moved at the work position B.
6 passes through the conveyor base, and after Δt, the tip of the manipulator is positioned at the teaching point 131 at the work position C, and the tip is moved on the linear path 132 from the teaching point 131. speed
Not only V _n, considered in estimating the correction amount V _kn of the transfer distance of the conveyor in response to changes in the transport speed of the conveyor in response to changes in the transport speed between the next Delta] t, is also moving distance corresponding to the speed variation You are looking for the moving distance. Therefore,
While the work moves from the work position C to the work position D, the hand of the manipulator is accurately positioned and moved on the straight path 132. Then, the same processing is repeated every Δt, and the hand of the manipulator moves on the straight path 132, and finally reaches the teaching point 133.

In the above embodiment, the case of a linear trajectory has been described. However, the present invention may be applied to a case of a function trajectory. The present invention can also be applied to a case where work is performed while holding the position.

[Effects of the Invention] As described above, according to the present invention, the output of the conveyor speed detecting means for detecting the transfer speed of the conveyor is fetched every predetermined minute time, and the moving distance vector of the conveyor at the predetermined minute time and the minute time And the moving distance vector of the hand of the manipulator on the workpiece coordinate at the time is calculated to obtain a target position vector of the hand of the manipulator in a very short time. Further, the present conveyor speed (V _n ), Based on the three points of the transfer speed (V _n-1 ) and the transfer speed (V _n-2 ) a minute before the time of the transfer speed (V _n-1 ). Based on the difference, the amount of correction of the conveyor transfer distance (V
_kn ), the prediction result is output as a vector, the target position vector is added to the vector of the prediction result, and positioning is performed based on the addition result. The minutes are appropriately compensated, and therefore, even if the conveyor speed fluctuates, the hand of the manipulator can be accurately positioned on the work on the conveyor.

[Brief description of the drawings]

FIG. 1 is a block diagram showing the overall configuration of a tracking robot control device according to an embodiment of the present invention. FIG. 2 is an explanatory view showing the manipulator hand and the moving position of the work in the embodiment. FIG. 3 is a flowchart showing the operation of the control device in the embodiment. FIG. 4 is a flowchart showing the processing contents of the movement control unit in the embodiment. FIG. 5 is an explanatory diagram showing the configuration of the work program storage unit in the embodiment. FIG. 6 is an explanatory diagram showing symbols of fluctuation and the like in the work program storage section of FIG. FIG. 7 is a timing chart for explaining problems of the conventional control device. FIG. 8 is a timing chart for explaining the operation of the present invention. FIG. 9 is a vector diagram showing a compensation method based on speed fluctuation in the embodiment of the present invention and FIG. In the figure, 101 is a program storage unit, 102 is a program execution unit, 104 is a distance accumulation unit, 106 is a trajectory planning unit, 107 is a conveyor speed prediction unit, 110 is a manipulator coordinate conversion unit, 1
11 is a positioning feedback control unit, and 112 is a power amplification unit. In the drawings, the same reference numerals indicate the same parts.

Claims (1)

(57) [Claims] 1. An industrial robot controller for performing a predetermined operation by a manipulator on a work object transferred by a conveyor, a speed detection unit detecting a transfer speed of the conveyor, and the speed detection unit. Is output every predetermined minute time, and based on the transfer speed and the transfer direction of the conveyor, a movement distance vector of the conveyor in the minute time is calculated, and the calculation result and the tip of the manipulator in the minute time are calculated. The moving distance vector on the work coordinates of the manipulator is added to calculate the moving distance vector of the hand of the manipulator in the short time, and based on the calculation result, the target position vector of the hand of the manipulator after the short time is calculated. A trajectory planning unit to be calculated; Uptake between every current transport speed (V _n) of the conveyor and the transporting speed before the minute time than the current (V _n-1), and further the micro-time than the time of the transport speed (V _n-1) Based on the previous transfer speed (V _n−2 ), the correction amount of the transfer distance of the conveyor corresponding to the change of the transfer speed of the conveyor in the next minute time by the first-order difference amount and the second-order difference amount ( V _kn ), a speed change prediction unit that outputs the prediction result as a vector, a conveyor speed compensation unit that outputs an addition result of the output of the speed change prediction unit and the output of the trajectory planning unit, A control device for an industrial robot, comprising: a manipulator drive control unit that drives and controls a manipulator based on an output of a conveyor speed compensation unit.