JPH06270079A - Articulated robot control device - Google Patents

Abstract

PURPOSE:To provide a practical control device resistant to a calculation error and disturbance by calculating a control input signal according to a prescribed control law so that a transfer function matrix reaching an angle position output signal to a position command signal to a robot becomes a desired transfer function matrix having no interference element. CONSTITUTION:A position command signal v(k) generated from a position command signal generating device 3 is inputted to an operation device 2, and here, the optimal control input signal u(k) according to the position command signal v(k) is found by operation according to a prescribed expression by using respective parameters K(z), H(z) and q<-1> (z) in order to make processing as a diagonal desired transfer function having no interference element possible to a control object (articulated type robot) 1 of a transfer function having an interference element, and this is outputted to a control device 4 of the control object 1. Output y(k) of the control object 1 is detected by an angle position output signal detecting device 5, and the detecting signal is fed back to the operation device 2. In this way, since a disturbance compensating function is contained in an observer, realization of robust noninterference of an articulated robot can be attained.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an articulated robot controller, and more particularly to a robust decoupling controller for an articulated robot.

[0002]

2. Description of the Related Art In the case of a multi-joint robot such as a SCARA type robot, in addition to the usual inertial force and frictional force of each axis, inertial interference force, Coriolis force, centrifugal force, etc. act between each axis. If there is such a mutual interference force, a command for only one axis affects the outputs of other axes, and an originally undesired response appears. In particular, since the inertial interference force depends on the acceleration, it has a bad influence on overshoot and tact at the time of stop.

Conventionally, the influence of the drive unit for each axis is weakened by a high reduction ratio, and these interference forces are only "integral compensation" as "other disturbances", thus forming a PI servo system for each axis with a simple structure. It was common to do. However, the demand for robot performance has increased year by year, the arm speed has increased, and the tact has become less than half of the conventional level. In response to such a request, the influence of dynamics that cannot be completely compensated by PI control, and the influence of interference force or the like cannot be ignored even at high reduction ratio drive.

On the other hand, recently, robust control based on disturbance estimation has attracted attention. This is to compose a control system by adding a state (speed) observer to the compensated system after compensating for the disturbance by the disturbance estimation observer.
A lot of calculation is required. Further, since a step-like disturbance is assumed as the disturbance, there is no change in that the transient response has a large deviation. In order to further improve the system, the direct drive method is advantageous, but at that time, the influence of such a disturbance becomes extremely large, and it is essential to deal with it more actively.

[0005]

On the other hand, there is a decoupling control method for compensating the mutual interference force and controlling so that there is no apparent interference in the output response of the command, and various methods have been proposed so far. Is coming. However, these are often complicated in theory and configuration, or require accurate parameters and calculation accuracy. Furthermore, if a compensator is inserted to add robustness, the order increases and the amount of calculation increases.

Although the speed of microprocessors has recently been improved, the amount of calculation is still a big problem in practice due to economic reasons. Since most of the arithmetic operations are performed as integers, the arithmetic error affects the control system as a disturbance.

An object of the present invention is to provide a practical and non-interacting decoupling controller for a multi-joint robot that solves the above problems and is strong against calculation errors and disturbances.

[0008]

In order to achieve the above object, an articulated robot control apparatus of the present invention is an articulated robot according to a control input signal calculated by a predetermined arithmetic unit according to a predetermined position command signal. Is configured to detect the angular position output signal of the robot, and the arithmetic unit sets the transfer function matrix from the position command signal to the angular position output signal to a desired transfer function matrix without interference elements. In addition, the gist is that the control input signal is calculated based on a predetermined control law.

[0009]

In the present invention, the disturbance compensation function is implicitly included in the state (speed) observer. This allows
Only the torque command input to the robot and the position output of the robot achieve the control purpose while maintaining robustness with a small controller order (that is, a small amount of calculation). In this case, the control purpose is to decoupling the articulated robot and to arrange poles in the desired model (transfer function) for each axis. That is,
It is possible to provide an extremely simple design method and control algorithm that not only has a small number of controller orders but also simultaneously achieves the three functions of disturbance compensation, decoupling, and pole placement in one step.

[0010]

DESCRIPTION OF THE PREFERRED EMBODIMENTS Generally, the equation of motion of an n-axis articulated robot can be expressed as the following equation.

[0011]

[Equation 1]

Where M () is an n × n inertia matrix, D
() Is an n × n matrix relating to viscous friction, Coriolis force, centrifugal force, and back electromotive force, and each non-diagonal element is an interference term. K is an n × n diagonal gain matrix of the motor, driver, drive mechanism, etc., and f is an n-dimensional vector representing Coulomb friction and gravity. Further, θ, dθ / dt and d ² θ / dt ² respectively represent an axis angle, an angular velocity and an angular acceleration, and u is an n-dimensional control input signal applied to the driver.

An actual control algorithm is implemented as software on a microprocessor, and the input is zero-order held by a DA converter and discretized at a sampling time τ _s (s). Thus, when the equation (1) is expressed by the discrete state equation,

[0014]

[Equation 2]

Here, η (k) is a constant value disturbance term such as Coulomb friction and gravity. The original continuous system does not have zeros (roots of the numerator polynomial of the transfer function), but such discretization makes it possible to have zeros near the unit circle. If the sampling time is shortened, the zeros will be outside the unit circle. It becomes a certain non-minimum phase system. This will be dealt with later. Due to the disturbance compensation function described below, the constant-value disturbance η in equation (2) is
Since (k) is completely compensated for, the η term is excluded, and equations (2) and (3) are rrp decomposed into a lead operator z (for a time series signal s (k), s (k + 1) = an operator n × n polynomial matrix P (z), R (z) such that zs (k)
When expressed by

[0016]

## EQU00003 ## y (k) = R (z) P.sup.- ¹ (z) u (k) (4). This expression (4) is a discrete transfer function matrix to be controlled, which is one expression method when dealing with a multivariable system.

Here, in order to simplify the subsequent discussion, the operator δ = z−1 is newly used. This represents the polynomial matrix to be controlled as follows.

P (δ) = Iδ ² + P ₁ δ + P ₀ (5) R (δ) = R ₁ δ + R ₀ (6) In the case of robot positioning control, P ₀ = 0,
In addition to diagonal elements, P ₁ , R ₁ and R ₀ have non-diagonal non-zero interference elements. Further, R (δ) is regular (there is an inverse matrix), but this assumption holds for a normal robot.

Next, a disturbance model is obtained. Η in equation (2)
The disturbance model representing is η (t) = η ₀ + η ₁ for each axis.
t + ... + η _d t _d , and its z-transform by δ,

[Equation 5] η (δ) = η ₀ / δ + ... + η _d / δ ^{d + 1} (7) Since η is a constant value disturbance, d = 0. Note that η ₁ is unknown.

The control object of the present invention is that the dynamic characteristics of the robot are made non-interfering, and the transfer function for each axis is pole-arranged so as to match the desired transfer function. That is, the transfer function m (δ) = m ₀ / (δ ² + m ₁ δ + m ₀ ) for each axis
So that the desired transfer function matrix from the command v (k) to y (k) is

Y (k) = N (δ) M ⁻¹ (δ) v (k) (8) M (δ) = I (δ ² + m ₁ δ + m ₀ ) N (δ) = Im ₀ For example, the transfer matrix is diagonalized and the interference element becomes 0, which makes the matrix non-interfering.

Next, with respect to the controlled object (equation (4)) represented by the transfer function matrix having the interference element, the position command v
A control law for matching the transfer function matrix from (k) to y (k) with diagonal N (δ) M ⁻¹ (δ) without interference elements is obtained.

First, since the degree of each axis to be controlled (maximum column degree) is 2 and the degree of disturbance is d + 1 = 1, an arbitrary (2-1) + (d + 1) = second degree is obtained by these degrees. Let q (δ) = δ ² + q ₁ δ + q ₀ be the monic stable polynomial of

## EQU00007 ## Let Q (.delta.) = Iq (.delta.) (9). This q (δ) becomes the characteristic polynomial of the observer and can be set independently of the pole placement model of the control system. Then, as in the following equation, a first-order n that is one-order lower than the order of Q (δ)
Consider a × n polynomial matrix K (δ) and a quadratic n × n polynomial matrix H (δ) that is the same as Q (δ).

[0022]

## EQU8 ## K (δ) = K ₁ δ + K ₀ (10) H (δ) = H ₂ δ ² + H ₁ δ + H ₀ (11) For any polynomial matrix F (δ) with the maximum column row degree of the third order , Equations (10), (1 that satisfy the polynomial equation (12)
The polynomial matrices K (δ) and H (δ) as in 1) are obtained by coefficient comparison.

[0023]

## EQU9 ## K (δ) P (δ) + H (δ) R (δ) = F (δ) (12) At this time, by setting K ₁ = Iq ₁ and i = 0 to d, η
The disturbance compensating function for asymptotically setting (k) to 0 can be included in the control law to prevent the steady deviation from occurring. Furthermore, by selecting K ₁ and i = 0 to d in this way, the other coefficients of K (δ) and H (δ) can be uniquely determined. Now, in equation (12), F (δ) is

[0024]

[Equation 10] F (δ) = Q (δ) {P (δ) -GM (δ) N ⁻¹ (δ) R ₀ } (13) When G = N (δ) R ₀ ⁻¹ , the following is obtained. Control law of

[0025]

Consider u (k) = Q ⁻¹ (δ) {K (δ) u (k) + H (δ) y (k)} + Gv (k) (14). From equations (4) and (14)

[0026]

Y (k) = R (δ) {Q (δ) P (δ) -K (δ) P (δ) -H (δ) R (δ)} ⁻¹ Q (δ) Gv (k ) (15)

Substituting equations (12) and (13) into equation (15),

Y (δ) = R (δ) {Q (δ) GM (δ) N ⁻¹ (δ) R ₀ } ⁻¹ Q (δ) Gv (k) = R (δ) R ₀ ⁻¹ N (δ) M ^-1 (δ) G ^-1 Q ^-1 (δ) Q (δ) G v (k) = R (δ) R ₀ ^-1 N (δ) M ^-1 (δ) v (k) (16) Q (δ) in the second line of equation (16) is the diagonal element q
Since (δ) is selected as the stable polynomial, Q ⁻¹ (δ) Q
It can be canceled as (δ) = I.

Here, R (δ) has a zero point in the vicinity of the unit circle because the robot is held in the 0th order and discretized in a short sampling time.

R (δ) R ₀ ⁻¹ = R ₁ R ₀ ⁻¹ δ + I ≈I (0.5δ + 1) = I (z + 1) / 2 (17)

The following approximation is performed. Therefore, the formula (1
Substituting 7) into equation (16) gives

## EQU15 ## y (k) ≈N (δ) M ⁻¹ (δ) (z + 1) v (k) / 2 (18), and new v (k) = (z + 1) v (k) / By modifying the reference input so as to be 2, decoupling and pole placement can be achieved at the same time.

FIG. 1 shows the overall configuration of the control system of the present invention based on the control law defined by the above-mentioned equation (14).
In the figure, 1 is an articulated robot to be controlled, 2 is a computing device, 3 is a position command signal generating device, 4 is a driving device, and 5 is an angular position output signal detecting device. The actual transfer function (discrete transfer function matrix) of the robot 1 is R (z)
At P ⁻¹ (z), the arithmetic unit 2 has a diagonal N with no interference element with respect to the controlled object 1 of the transfer function having the interference element.
In order to enable the processing of (δ) M ⁻¹ (δ) as a desired transfer function, the optimum control input signal u (k) corresponding to the position command signal v (k) is calculated by the equation (14). Is output. In the above description, the n × n polynomial matrix has been described, but the present invention is applicable even when n = 1.

Next, in the analysis of the disturbance response characteristic of the control system of FIG. 1, the reference input is set to v (k) = 0, and the disturbance η
The transfer characteristic from (k) to the output y (k) is examined. First, since the disturbance η (k) is applied in the form of being added to the input u (k) as in the equation (2), the equation (4) is

Y (k) = R (δ) P ⁻¹ (δ) {u (k) + η (k)} (19)

From equations (12), (13), (14) and (19),

Y (k) = R (δ) R ₀ ⁻¹ N (δ) G ⁻¹ {Q (δ) −K (δ)} × M ⁻¹ (δ) Q ⁻¹ (δ) η ( k) (20)

In this equation, the stationary response, that is, y (k) when k → ∞, is

[0034]

[Equation 18]

It is Where η (δ) = η ′ (δ) /
It can be expressed as δ ^{d + 1} . Further, since K ₁ = Iq ₁ and i = 0 to d are selected, Q (δ) -K (δ) = δ
^It is expressed as ^{d + 1} {Q ′ (δ) −K ′ (δ)}, and the denominator δ ^{d + 1} of η (δ) is canceled. Therefore, equation (21)
Is

[0036]

[Formula 19]

Where denominator matrix M ⁻¹ (δ) Q ⁻¹
Since (δ) is stably selected, the value of the equation (22) becomes 0. That is, when k → ∞, the influence of the disturbance η (δ) on the output converges to zero. Therefore, this system does not produce a steady deviation with respect to the disturbance vector η (δ) as in the equation (7).

FIG. 2 shows an embodiment of an articulated robot controller of the present invention based on the control system of FIG.
For example, a SCARA type robot is used, and the number of control axes of this robot is four. However, the θ ₁ axis (the 1st axis) and the θ ₂ axis (the 1st axis) which mainly constitute a horizontal two-link mechanism with interference are used. 2 axes)
The control method of the present invention is applied to. As the arithmetic unit 2,
For example, a digital signal processor (DSP) is used,
P (z) and the like are determined as follows for arithmetic control by this device. That is, the parameter of the controlled object 1 is
Obtained by a parameter identification test by the method of least squares including the driving device 4,

[0039]

[Equation 20]

It is The two poles of the desired model are 20ra
d / s and 800 rad / s, the observer was set to 120 rad / s of Butterworth pole of the second order. The parameters of the control law (equation (14)) are as follows.

[0041]

[Equation 21]

FIG. 3B shows a position command signal v (k) when only the θ ₁ axis of the robot 1 is operated as shown in FIG.
Is a plot of the deviation of the actual position with respect to, and the dotted line shows the result by the conventional method, actually by the method of the present invention. As is clear from the figure, in the case of the conventional servo control method for each axis without decoupling, the θ ₂ axis, which should be the 0 command, is large even though the interference force is considerably small at high deceleration. Although it is oscillating, when the decoupling control is performed by the method of the present invention, it is understood that the θ ₂ axis hardly oscillates and the influence of the interference force is sufficiently compensated.

Further, FIG. 4B is similar to FIG.
The result of operating only the θ ₂ axis as shown in (a) is shown. In this case, the influence of interference is small in both of the two types, and the shake of the non-operating axis θ ₁ axis is small. However, it is apparent that the non-interacting control of the present invention causes less shake and has better characteristics.

Further, FIG. 5 (b) shows θ as shown in FIG. 5 (a).
The time response characteristics of position deviation when both ₁ and θ ₂ axes are operated simultaneously are shown. As is clear from the figure, if the conventional method is not used for decoupling, a large disturbance of the operation occurs during deceleration and stop, which delays the tact, but when the decoupling control of the method of the present invention is performed, It can be seen that the disturbance is small and the tact delay is gone.

[0045]

As described above, according to the present invention, by including the disturbance compensation function in the observer, a control algorithm for simultaneously achieving robust decoupling and pole placement of an articulated robot can be simply constructed. . Although this algorithm handles multivariable systems and has a disturbance compensation function, the amount of computation is very small. For example, if the parameter is minimized, the product-sum operation is performed about 20 times for two axes of the scalar robot. Further, in the calculation of control parameters (parameter design), parameters that achieve all of disturbance compensation, decoupling, and pole placement at the same time can be obtained only by comparing polynomial coefficients. Therefore CA
It is easy to convert to D and is very practical. Further, in the embodiment using the SCARA type robot, the overshoot at the time of stop is eliminated as compared with the conventional servo system for each axis, even though the drive mechanism has a high reduction ratio, thereby shortening the tact time. And its effectiveness is clear.

[Brief description of drawings]

FIG. 1 is a block diagram showing an overall configuration of a control system of the present invention.

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

FIG. 3 is a characteristic diagram showing a deviation of an actual position with respect to a position command signal when only the θ ₁ axis of the robot is operated.

FIG. 4 is a characteristic diagram similar to FIG. 3 when only the θ ₂ axis is operated.

FIG. 5 is a time response characteristic diagram of position deviation when both θ ₁ and θ ₂ axes are simultaneously operated.

[Explanation of symbols]

 1 Robot 2 Computing Device 3 Position Command Signal Generator 4 Driving Device 5 Angular Position Output Signal Detection Device

Claims (1)

[Claims] 1. A structure for driving and controlling an articulated robot by a control input signal calculated by a predetermined arithmetic unit according to a predetermined position command signal and detecting an angular position output signal of the robot, The arithmetic device is adapted to calculate the control input signal based on a predetermined control law so that the transfer function matrix from the position command signal to the angular position output signal becomes a desired transfer function matrix without interference elements. An articulated robot controller characterized by the above.