JPS59220806A - Controlling method of industrial robot - Google Patents

Abstract

PURPOSE:To shorten an arithmetic time and to realize precise track control by calculating and storing the inertia and gravitational torque at an operation teaching point of a robot in a memory, and compensating parameters of each axis servo system on the basis of the data when the robot is operated. CONSTITUTION:A robot controller 2 is connected to a teaching box 3 and provided with the arithmetic processing circuit consisting of a microprocessor 201, RAM202, ROM203, memory 205 for teaching data storage, etc. The output of a processing circuit is applied to a D/A converter 207 through an interface circuit 206 and converted into an analog signal, which is applied to a system that controls a servo system for each axis which consists of a position control circuit 208, speed control circuit 209, current control circuit 210, etc. When the robot is taught operation from the box 3, the inertia and gravitational torque at an operation teaching point are stored in the memory 205 and parameters of each servo system are compensated on the basis of the stored data to shorten the arithmetic time of a processor 201.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Application of the Invention] The present invention relates to a method for feedforwardly compensating for fluctuations in the moment of inertia and gravitational torque associated with the motion of a robot. Concerning methods for controlling industrial robots capable of trajectory control.

[The station behind the invention]

Conventionally, this type of industrial robot control method involves calculating the momentary position (d command) for each axis drive system by performing appropriate interpolation calculations from the position data of each axis of the robot that has been taught in advance. Each axis drive system was servo-controlled by this position command value. At this time, the control parameters of each axis servo thread were used at a constant value after WrAI was adjusted to show the optimum position servo characteristics. .on the other hand,
A robot control system is a control system in which the moment of inertia and gravitational torque essentially fluctuate due to changes in the posture of the robot as it moves. Therefore, in such a method, a difference occurs in the response of each axis servo system as the robot moves, resulting in a decrease in the positioning accuracy and trajectory control performance of the robot.

To deal with this, methods have been proposed that compensate for changes in inertia and gravitational torque according to changes in the robot's posture.
In order to calculate inertia and gravitational torque in real time as the robot moves, a large number of multiplications, additions and subtractions are required, which increases the calculation time, forcing the sampling period to be lengthened accordingly.

[Purpose of the invention]

An object of the present invention is to provide a method for controlling an industrial robot that can achieve highly accurate trajectory control without increasing calculation time.

[Summary of the invention]

The key point of the present invention is to calculate the inertia and gravitational torque at the teaching point when teaching the robot to move and store them in memory, and to compensate the parameters of the servo system for each axis based on these data when the robot moves. This is what I did.

[Embodiments of the invention]

Embodiments of the present invention will be described below with reference to FIG. 1 and subsequent figures.

FIG. 1 is a perspective view showing an embodiment of an industrial robot and its control device to which the control method according to the present invention is applied. It consists of a teaching box 3 for teaching.

In the figure, 01 to θ are the control angles of the robot, and θ
, rotation of the main body 101, bending of the upper arm 102, bending of the forearm 103, bending of the grip 105, grip 105.
The twist angle of each is shown.

In this embodiment, the robot F gauge is moved to a target posture according to the command from the teaching box 3.
By storing the teaching points in the memory of the control device 2, the robot is taught the motion.

Next, FIG. 2 shows the configuration of the robot control device 2. The robot control device stores the lff1f data of the teaching point in the memory, calculates the inertia and gravitational torque at the robot teaching point, and calculates the momentary position command value for each axis servo system during robot operation. It is composed of an arithmetic processing circuit for performing the operation and a servo thread for each axis for individually servo-controlling each of the five drive axes. Here, the arithmetic processing circuit includes a microprocessor 201 and its peripheral elements RAM 202. FLOM203% and a relatively large-capacity memo for storing data such as teaching point position data, teaching point inertia, and gravity torque! It is composed of digital circuits such as 7205. Furthermore, this arithmetic processing circuit is connected to the teaching box 3 via an interface circuit 204 and to each axis servo system via an interface circuit 206, respectively. Here, the position command value obtained by the arithmetic processing circuit is converted into an analog quantity by the D/A converter 207, and becomes a command value to each axis servo system.

Each axis servo system is composed of analog circuits, including a position control circuit 208 (215)%, a speed control circuit 209 (216), a current control circuit 210 (217)%, and a
11 (218) Karasei, DC%-1'212 (21
9). Here, the motor current, speed, and position are determined by the current detector 222 (223) and the p-y generator 1, respectively.
1213 (220) Potentiometer 214 (221
) is detected and fed back to each single-axis servo system.

Such servo circuits for each axis are prepared as many as the number of drive axes. Here, the control parameters of the speed control circuit 209 (216) are compensated based on inertia and gravity torque data at the teaching point.

Now, in the robot l shown in Fig. 1, which has a five-degree-of-freedom link mechanism, the effects of changes in inertia and gravitational torque are particularly noticeable: rotation of the main body (θ1 axis), bending of the upper arm (θ, axis), and bending of the forearm. (θ3 axes). Therefore, in this embodiment, inertia and gravitational torque are compensated for these three axes.

Furthermore, the description of the following embodiments will be made regarding only these three axes.

The equivalent model at this time is shown in Figure 3. Here, it is assumed that the portion of the grip corresponding to the hand of the robot and the load gripped by the gripper are concentrated at the tip of the forearm 103 of the robot. Now, the orthogonal coordinate system x shown in Figure 3
Let yz be old and the two axes be vertical. Rotation angle θ1 of each link. If θ2゜θ3 is chosen as shown in Figure 3 for the xyz coordinate system, θ, the moment of inertia with respect to the rotation of the axis J1,
Top Katoluk τ13. The moment of inertia J1% for the rotation of the θ2 axis, the moment of inertia for the rotation of the B-axis (heavy torque τgt + 'moment of inertia for the rotation of the B axis) Js, and the torque τ, 3 are given by the following equations, respectively.

J 1= 828i11202 + 2 at”iII
θ, 5in(θ2+03)+assin2(θ2+θa
) 10L (1) τ,,=o(2) J2 =32 10a, +2 a4 focus θs 1I2+
IS (3)7M2-(a,,8inθ2
+aa”(θ2+θs > ) g (4)J
a = as + Is (5
) r, 3=-a6sin (θ! 10θs)g
(6) Here, at r-aII Ja4 *
aII * aa is at "mtl-t: ten ("s
+mt) 12 (7)a3 =m,
80m, t”,
(s) at = (”s L
ws +mW ts) tt (9)
aa ”mt4t + (ms + mw) 4
(10)a, =ms4. +m,,z,
(11).

However, in each of the above formulas, ml: Mass of the third axis link tl F Length of the third axis link t118 Length from the root of the i-axis link to the center of gravity of the third axis link I+ 'Third axis Moment of inertia m with respect to the center of gravity of the link: mass g of the tip of the third axis link: gravitational acceleration.

As shown in formulas (1) to (6), J8. τ, 2. τ, 3
changes as a function of θ2°θ8, and J2 changes as a function of θ.

Therefore, the characteristics of the servo system for each axis are θ2. It fluctuates as θ8 changes. An object of the present invention is to compensate for changes in inertia and gravitational torque expressed by equations (i) to (6). A method of compensating for inertia and gravitational torque in this embodiment will be described below.

In this embodiment, when teaching the robot movement, three joint angles θ1. θ2. θ, is stored in the memory 205 for recording TTE data. At the same time, 01
From the value of ~θ, according to formulas (1) to (6),
Moment of inertia J l of each axis servo system at the teaching point
% k and force/torque τg+ (' = 1 to 3), respectively. This calculation is performed by a microprocessor 201, which is a component of the control device shown in FIG. Calculation formulas (1) to (6) are calculated using the ROM 203 of the control device.
Write it as a program in . Trigonometric functions are calculated by indexing an 8IN function table stored in the same ROM 203. These calculations require many multiplications, additions and subtractions, but since the robot's motion teaching is performed off-line, the time required for these calculations is not a problem.

The moment of inertia and gravitational torque obtained in this way are
As shown in FIG. 4, each teaching point is stored in a teaching data memory together with joint angle data.

Now, when the robot is operating, the taught robot position is interpolated using an appropriate function to obtain each joint angle θ for each sampling period, and this is used as the position command value for each axis servo system. At this time, the values of the moment of inertia and gravitational torque determined by the method described above are similarly interpolated for each sampling period, and the parameters of the servo system of each axis are compensated using these values.

FIG. 5 shows a method of interpolating the moment of inertia and gravitational torque between teaching points P and P2 for the i-th axis. Moment of inertia at the ``th sampling, ++(j), i
The fkatorque τ, (j) is calculated by the following formula.

Here, JIIlτ11. are the moment of inertia and the gravitational torque of the i-th axis at the teaching point P1, respectively, and Δ
J I(k), Δτ, , (k) represent the amount of change in each sampling period. This change acid 01 is calculated as follows.

Here, Δmount (k) is a change in joint angle between samplings, and is obtained by the following equation.

Δ01 (k>=θ+(k-1)i required)
(16) Also, multiply Δθ by (k) and calculate n from the command value for each axis servo system for each sampling period.

3) can be found from equations (1) to (6) as follows.

(j-1 to 3) is a force that is a function of θ, tj=x to 3). In normal robot control, changes in the moment of inertia and gravitational torque between teaching points are smooth. “(j
-1 to 3) are small and can be considered to be almost constant.

Therefore, as shown in Figure 6, for each sampling period,
ヱ±j......The δθIydθ amount and the σθ amount are repeatedly calculated at 2 μm, and the values are updated each time the calculation is performed, and are kept constant during that time. This calculation is (17)-422)
Store the formula in TI and OM as a program and execute it. Also, remove the divided e? t, . is stored in memory as bθj+dθ.

Set, JI, τ11 total: Used for Atun. In this way, δJ+ ar,.

Using equations (14) and (15) from 11 koku + w,
Then calculate ΔJl(k), Δτ, ,(k), and from these values (12).

According to equation (13), the moment of inertia J+(j) and τ1(j) at the sampling time are determined.

A method of correcting the parameters of the servo system for each axis using Jl(j), τ, and +(j) obtained as described above will be explained with reference to FIG. In FIG. 7, Jl and τ11 are the moment of inertia and gravitational torque that change with the movement of the robot. Here, the position control circuit 208 is approximated by a simple gain Kp, and the current control circuit 210 is approximated by a first-order lag system with a time constant Tc. Also, let KT be a torque constant. J++
τ1. With the change in the gain Ks of the speed control circuit 209
l and offsela) Ko.

is compensated as shown below.

Ks+(J)=Kso−Js(J)
(23) As a result, it is possible to completely compensate for the gravitational torque that acts as a load on each axis servo system, and furthermore, the gain of the speed control system can be adjusted to compensate for changes in the acidic moment, so each axis servo system The response characteristics of the system can be kept constant. As a result, highly accurate trajectory control can be achieved.

In this way, according to this implementation, changes in the moment of inertia and gravitational torque can be calculated by simple interpolation calculations for each sampling period using the moment of inertia and vehicle torque at the teaching point calculated in advance. Since it is possible to compensate for this, the calculation time can be significantly reduced compared to a method that performs calculation and compensation for each sampling period. Furthermore, since parameter changes in the servo system for each axis can be compensated for in a feedforward manner, the response can be made uniform, and highly accurate trajectory control can therefore be realized.

In addition, although the above embodiment describes a case in which a five-degree-of-freedom link mechanism is approximated to a three-degree-of-freedom model and compensation is performed for three axes, the present invention is not limited to this (d'
, 'x < , V can be similarly applied when inertia, gravity, and torque are compensated for all five axes. Furthermore, 5
It goes without saying that the control method of the present invention can also be applied to cases where the number of degrees of freedom is greater than the number of degrees of freedom.

〔Effect of the invention〕

According to the present invention, highly accurate control performance for trajectory commands can be obtained, and parameter compensation can be performed without compromising the sensitivity of the memory that stores the compensation amount.
The time required to calculate the amount of compensation for each sampling period is short.

[Brief explanation of drawings]

FIG. 1 is a perspective view of an industrial robot and its control device to which a control method according to an embodiment of the present invention is applied, FIG. 2 is a block diagram showing the configuration of the control device of FIG. 1, and FIG. 1 is a schematic diagram showing the analytical model of the embodiment shown in FIG.
The figure is a schematic diagram showing a compensation parameter interpolation method in the embodiment of FIG. 1, FIG. 9 is a timing diagram for determining the parameter change rate in the embodiment of FIG. 1, and FIG. 7 is a diagram of the parameters in the embodiment of FIG. FIG. 2 is a block diagram illustrating a compensation method. 2... Robot control device, 3... Teaching box, 201... Microprocessor, 205...
Teaching data storage memory, 208... position control circuit,
Police 3ω in the 5th ward

Claims (1)

[Claims] 1. The tip position and posture of an industrial robot having a multi-degree-of-freedom movement mechanism consisting of a plurality of link mechanisms, each of which is driven by an individual axis servo system, are taught, and the In a device that is controlled based on teaching data, when teaching the operation of the industrial robot, the moment of inertia and gravitational torque of the servo system for each axis at the teaching point are calculated and stored in a memory together with other teaching data. From the data of the moment of inertia and the gravitational torque at the teaching point, the moment of inertia and the gravitational torque of the servo system of each axis are calculated when the industrial robot operates. A method for controlling an industrial robot, characterized in that the control parameters of the servo system for each axis are compensated based on the calculated values.