JPH04160604A - Robot control system - Google Patents

Abstract

PURPOSE:To interpolate a correct locus by non-interferentially processing from a servo motor torque command value up to a speed applied in a rectangular coordinate system by means of a double disturbance suppressing loop. CONSTITUTION:Operation from the torque command up to the angular velocity commands of respective joint spindles are non-interferentially executed and outputted by the 1st non-interferential loop. Operation from the torque command up to coordinates on the rectangular axes is non-interferentially processed by a Jacobian matrix and the 2nd disturbance suppressing loop based upon the non-interferentially processed angular speed. When a control loop for inputting a command value on the rectangular axes is integrated into a plant in which operation from the torque command up to the coordinates on the rectangular axes are to be non-interferentially processed, conversion processing can be satisfied only by forward conversion and reverse conversion requiring much processing time for converting a moving variable on the rectangular coordinates into a rotational variable in each joint is made unnecessary. Consequently, a calculation time for coordinate conversion can be shortened, an interpolation interval can be reduced and a correct locus can be interpolated.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to a robot control system for an articulated robot, and particularly to a robot control system that simplifies coordinate transformation between an orthogonal coordinate system and the amount of movement of each axis.

[Conventional technology]

Currently used articulated robots use TCP (T
The movement locus, etc. of OOL CENTBRPOINT) is commanded in a Cartesian coordinate system, and this position command is converted into the joint angle of each driving joint by inverse transformation. This angle command value is given to a servo system set up for each servo motor of each joint to drive the servo motor and give the desired trajectory to the TCP of the robot.

[Problem to be solved by the invention]

On the other hand, if the speed is constant, the number of interpolation points that can be taken on the trajectory of the robot's TCP increases as the time for the above conversion process is shortened. Of course, the trajectory accuracy improves as the number of interpolation points increases.

Of these transformation processes, the one that takes the most time is the inverse transformation from orthogonal coordinates to joint coordinates. Therefore, if this inverse conversion process can be eliminated, the conversion process time can be significantly shortened and trajectory accuracy can be improved.

The present invention has been made in view of these points, and it is an object of the present invention to provide a robot control method that does not require inverse conversion processing from orthogonal coordinates to joint coordinates.

[Means to solve the problem]

In order to solve the above problems, the present invention provides a robot control method for controlling a multi-jointed robot, in which a torque command is received, and an angular velocity command for each joint is obtained by removing interference with other axes using a first disturbance suppression loop. A first decoupling loop that outputs a movement command on the orthogonal axes, a Jacobian matrix that converts the angular velocity into a movement command on the orthogonal axes, and a second disturbance suppression loop that outputs a movement command on the orthogonal axes. A robot control method is provided, which is characterized by having a configuring loop.

[Effect]

The first decoupling loop decouples the angular velocity command of each joint axis from the torque command and outputs it.

This non-interfering angular velocity command is applied with a Jacobian matrix and the second disturbance suppression loop is used to make the torque command to the coordinates on the orthogonal axis non-interfering. By creating a control loop that inputs the command value on the orthogonal axes in a plant where the torque command and the coordinates on the orthogonal axes are made non-interfering, the conversion is only in the forward direction, which saves a lot of processing time. Eliminates the need for inverse transformation of the required amount of movement on orthogonal coordinates into the amount of rotation for each joint.

〔Example〕

FIG. 2 is a hardware configuration diagram of a robot system for implementing the present invention. The host processor 9 is a processor that controls the entire robot. The robot position command Xd (i) is sent from the host processor 9 to the shared RA.
Written to M10.

Note that the ROM is coupled to the host processor 9.

RAM etc. are omitted.

DS that controls the servo motor 22 built into the robot
P (digital signal processor) 11 is RO
According to the system program of M12, shared RAM10
The position command Xd (i) of - is read at fixed time intervals.

The DSPII converts the angular velocity of each axis obtained from the pulse coder 23 into feedback in an orthogonal coordinate system using a Jacobian matrix. After that, a control loop is formed using the above-mentioned Xd and this feedback to realize a trajectory on the orthogonal axes.

That is, the input torque T1 of the current loop, that is, the current command value, is calculated from the angular velocity and velocity feedback of each joint axis, and the PWM waveform for driving the servo motor 22 is generated by the current loop, and the DSL (digital servo LSI) It is sent to the servo amplifier 21 via 14.

The servo amplifier 21 receives the PWM command and drives the servo motor 22. The servo motor 22 is operated via a reduction gear.
The arm 26 is driven. The servo motor 22 has a built-in pulse coder 23, and feeds back pulses to the DSP II via the DSL 14.

In the following explanation, the joint axes will be explained using three axes, but since the control system of all servo motors is the same, only the servo motor 22 for one axis is shown in Fig. 2, and the other servo motors are not shown. It is omitted.

Next, consider a three-axis robot as the control target.

The relationship between the torque Tr (vector) and the angular velocity θ(1) and angular acceleration θ[2] of each axis can be expressed by the following equation.

Trq=M(θ)θ[2] +h(θ, θ(1)) Here, the torque Trq is a vector, and Tr q 1
: Torque command Trq2 to I-item servo motor: 2
Torque command Trq3 to the servo motor of the third axis: Torque command to the servo motor of the third axis. M(θ): Inertia matrix h(θ, θf11) This is a nonlinear term consisting of Nicoliolis force, centripetal force, gravity, viscosity term, etc.

A servo system shown in FIG. 3 is constructed at each joint. That is,
The current input U is input to an adder 31, and the output of an element 39, which will be described later, is added thereto. The output of adder 31 is input to elements 33 and 36 of plant 32. Element 33 is the torque constant of the motor, its output is torque, and the disturbance is added to adder 34. The output of adder 34 is provided to element 35. The angular velocity of the servo motor 32 is Y-〇(th (1)).

On the other hand, the current command value that is the output of the adder 31 is sent to the model 36 of the element 33, and the output of the element 36 is added to the adder 37. Moreover, the angular velocity Y of the servo motor 32 is differentiated (
×S) sent to element 38;

The output of the element 38 is the servo motor 32 which differentiates the angular velocity Y.
corresponds to the estimated angular acceleration of The output of this element 38 is subtracted from the output of element 36 in an adder 37. Adder 37
The output of is multiplied by [1/Ktm (i)] by element 39,
Converted to current command value.

The output of element 39 is added to command value U by adder 31. Considering the transfer function between input U and output Y in FIG. 3, the following equation holds true.

(Y/U)=Ktm (i)/(Jm (i)S) Looking at this equation (2), the input U is independent from other axes. In other words, the interference system of equation (1) has been made non-interfering, and can be expressed by the following equation.

M a * U v - θ + n - (3) where, Uv: A vector in which the elements of U are arranged vertically. It can be expressed by equation (4) shown in FIG.

Although non-interference is realized here by the prediction element, non-interference can also be achieved by zeroing by the disturbance estimation observer. In some cases, the disturbance estimation observer can provide more stable control because it does not have a differential element.

Next, consider the velocity on the orthogonal coordinates from the angular velocity of each joint.

X”' = Jacob*θ(l l , ,,,
, , , , , , , , , , (5) By substituting
Consider the components of ob*Ma. Jacob can be expressed by equation (6) shown in FIG. Therefore, Jaco
b*Ma can be expressed by equation (7) in FIG.

Next, the matrix of equation (7) is made non-interfering using the same method as described above. First, pay attention to the first axis. That is, [Ktm (1)/Jm (1)] ・Ja Consider leaving the component of 11. For this purpose, a servo system shown in FIG. 5 is constructed. Input UUv (1) is input to adder 41 and added to the output of element 47, which will be described later. The output of adder 41 [Jv (1) is input to system 42. Here, the system 42 is the servo system shown in FIG. 3, and is made non-interfering as described above. The output θ(1) of the system 42 is a vector and is input to the Jacobian matrix 43.

The output X (1) ``' of the Jacobian matrix 43 is the velocity in orthogonal coordinates. This output and input to the adder 45. The adder 45 converts the output Uv(1) of the adder 41 into a coefficient Kt using an element 44.
The output of element 46 is subtracted from the output multiplied by m(1). The output of the adder 45 is an element 47 with a coefficient [1/Ktm (1
)] and is added to the command value UUv (1) by the adder 41.

Here, the functions of elements 44, 46, 47 and adder 45 are the same as in FIG. 3, and from input UUv (1), output
(1) The transfer function up to l is X (1)'/U
Uv (1) −[:Ktm (1)/ (Jm (1)S)]*Ja
ll, and non-interference can be achieved. Similarly, two-shaft opening,
The third axis can also be made non-interfering.

In the above explanation, Uv (1) to X (1) ”'
Although the system up to
The largest term of the system from (1) to X (1) '', [Ktm (1)/Jm (1)S'3XJa 11[
K Lrn (2) /Jm (2) SE xJ a
12[Ktm (3)/Jm (3)SE X
Considering the maximum value of Ja 13, Uv (1) for it
It is better to make the system from X(1)[+1 non-interfering.

In the same way for other axes, terms for non-interference can be considered and non-interference can be measured.

Next, configure a servo system using this non-interfering system.

That is, by decoupling, the system shown in FIG. 6 is obtained for each axis. That is, for input UUv (i), output X (i) "' is obtained.

Next, a velocity loop and a position loop can be coupled to the system shown in FIG. 6 to construct a non-interfering servo system. FIG. 1 is a block diagram of a non-interfering servo system. The input Xd (i) is subtracted by the feedback X (i) in an adder 62 .

Further, the input Xd (i) is input to a differential element 61 forming a feedforward loop, and the output of the differential element 61 is input to an adder 64. The output of the adder □62 is multiplied by a gain K by an element 63 and added to an adder 64. In the adder 64, the speed feed pack X (i)
"' is subtracted, and the output of adder 64 is output to element 65.

Element 65 is an element having an integral gain of 1 and a proportional gain of 2, and its output is UUv (i). The output UUv (i) is input to element 66 . Element 66 is the decoupled system shown in FIG.
By integrating "' with the integral element 67, the position X (i) on the orthogonal axis is obtained.

Therefore, coordinate transformation in the servo system shown in Figure 1 requires only forward calculations of the Jacobian matrix, forward transformation, and decoupling, and control on orthogonal coordinates is possible, and the calculation time for coordinate transformation is is shortened, the interpolation interval can be shortened, and accurate trajectories can be interpolated.

In the above explanation, the servo system has been explained using a three-axis servo motor, but it can be similarly applied to a robot with four or more axes.

〔Effect of the invention〕

As explained above, in the present invention, since the torque command value of the servo motor to the speed given in the orthogonal coordinate system is made non-interfering by the double disturbance suppression loop, the coordinate transformation is
Only the forward calculation of the Jacobian matrix, forward transformation, and decoupling,
Control on orthogonal coordinates is now possible, calculation time for coordinate transformation is shortened, and interpolation intervals can be kept small.
Accurate trajectories can be interpolated.

[Brief explanation of the drawing]

Fig. 1 is a block diagram of a non-interfering servo system, Fig. 2 is a hardware configuration diagram of a robot system for implementing the present invention, Fig. 3 is a block diagram of a non-interfering servo system, and Fig. 4 is a block diagram of a non-interfering servo system. The figure shows the transformation determinant, etc., FIG. 5 is a diagram showing an example of a servo system for making one axis non-interfering, and FIG. 6 is a block diagram of the non-interfering system. 9 Host processor 11 DSP (Digital Signal Processor) 21 Servo amplifier 22 Servo motor 26 Arm 66 Non-interference system patent applicant Fanuc Co., Ltd. agent Patent attorney Takeshi Hattori

Claims (4)

[Claims] (1) In a robot control method for controlling a robot with multiple joints, a first non-interference method receives a torque command and outputs an angular velocity command for each joint excluding interference with other axes using a first disturbance suppression loop. a Jacobian matrix that converts the angular velocity into a movement command on the orthogonal axes, and a second decoupling loop that outputs the movement command on the orthogonal axes using a second disturbance suppression loop. Characteristic robot control method. (2) The first disturbance suppression loop includes a torque prediction means for predicting output torque from the current command value, and an actual torque prediction means for predicting the actual torque from the angular acceleration by differentiating the angular velocity of the servo motor. The robot control system according to claim 1, characterized in that: (3) The robot control method according to claim 1, wherein the first disturbance suppression loop is constituted by a disturbance estimation observer. (4) The robot control method according to claim 1, wherein the first non-interfering loop is used as a plant, and the amount of movement on orthogonal coordinates is input.