JPH08323670A - Manipulator controller - Google Patents

Abstract

PURPOSE: To provide a manipulator controller which performs joint torque control stably with high precision' by using torque sensor feedback control. CONSTITUTION: A joint torque control operation means uses a target joint torque value, external disturbance applied to a motor 1, and external disturbance applied to an arm 4 as inputs for fluctuation of a predetermined arm inertia moment and determines transmission characteristics from these inputs to an output torque value detected by a torque sensor 3 and transmission characteristics from these inputs to a deviation between the output torque value and a target joint torque value by using a structuring singular value so that these transmission characteristics are below predetermined values.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for a manipulator, and more particularly to a control device for a manipulator which performs stable and highly accurate joint torque control using torque sensor feedback control.

[0002]

2. Description of the Related Art Generally, a robot manipulator for assembling generally performs contact work, but contact may start before the target position due to displacement of the work or the like. At this time, if the position control system of the manipulator maintains a high servo rigidity, the position control system will generate an excessive force according to the position deviation generated from the target position, which will hinder the work and the manipulator. I had to come.

Conventionally, as a countermeasure against this, there is a method of providing a compliance characteristic by lowering the servo rigidity. However, in the conventional position control manipulator, if the servo rigidity is lowered, the friction of the transmission system cannot be suppressed, so that there is a practical problem that the required position accuracy cannot be obtained in the free space before contact. It was Conventionally, as a method of suppressing the friction of the transmission system, there is a method of performing joint torque control by detecting torque after the speed reducer of each axis of the manipulator and performing torque feedback.

[0004]

As a joint torque control method, the applicant of the present invention believes that stabilization can be facilitated by simultaneously performing speed feedback of the motor, and therefore proportional-integral-derivative (PID) control using speed feedback together. And devised state feedback control (Japanese Patent Application No. 6-2
54006). However, in the case of PID control, if the noise of the torque sensor signal is large, the D gain cannot be increased, so that resonance of the transmission system cannot be suppressed and sufficiently stabilized. As a result, the PI gain cannot be increased, and the friction of the transmission system cannot be sufficiently suppressed. Also, in the case of state feedback control, if the posture of the arm changes or the load on the hand changes, the arm inertia moment, which is one of the parameters to be controlled, fluctuates from the design condition, and it tends to become unstable. was there.

On the other hand, as a control method that suppresses and stabilizes the resonance of the transmission system even when the parameter to be controlled fluctuates and suppresses the disturbance applied to the actuator, the control designed using the structured singular value as a design index. System has been proposed (Society for Control and Automation, Robust Control Symposium, Material pp.3)
1-34. 1994). However, since the control system is a position control system, there is a problem in that it cannot have compliance characteristics. Further, although the angle of the arm after the speed reducer is the state quantity to be controlled, there is no sensor for detecting the state quantity to be controlled, and there is a practical problem that the control system cannot be wideband because feedback is impossible. .

The present invention solves the above problems.
Provided is a manipulator control device having a joint torque controller that makes the joint torque follow the target joint torque even when the arm inertia moment fluctuates, suppresses friction of the transmission system, and suppresses resonance of the transmission system to stabilize. The purpose is to do.

[0007]

In order to achieve the above object, a control device for a manipulator according to the present invention comprises a motor for driving a link via a transmission mechanism and a torque detecting means for detecting an output torque after the transmission mechanism. A target joint torque value generating means for generating a target joint torque value, and a joint torque control calculating means for calculating a motor operation amount from the generated target joint torque value and the output torque value detected by the torque detecting means. The joint torque control calculating means receives as inputs the target joint torque value, the disturbance applied to the motor, and the disturbance applied to the link in response to a change in a predetermined arm inertia moment, and the torque detection means from those inputs. The transfer characteristics up to the output torque value detected in step 1 and the transfer characteristics from the input to the deviation between the output torque value and the target joint torque value are the predetermined maximum values. It becomes configured using the structured singular value to be less than the.

[0008]

The control device for a manipulator of the present invention having the above-described configuration detects the joint torque, which is the state quantity to be controlled, and feeds it back, so that the arm inertia moment fluctuates within a predetermined range. Also, in a predetermined frequency band, the output torque value detected by the torque sensor is made to follow the target joint torque value, and the joint torque control system realized by using the structured singular value as the design index is The disturbance applied to the link is suppressed below a predetermined level to suppress and stabilize the resonance of the transmission system.

The control device for a manipulator according to the present invention having the above-mentioned structure enables high-speed and high-accuracy joint torque control with the same joint torque controller even if the arm inertia moment fluctuates. It has many practical advantages such as a simplified structure.

[0010]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Representative embodiments of the present invention will be described in detail below with reference to the drawings.

[0011]

First Embodiment First, a manipulator control device according to a first embodiment to which the present invention is applied when controlling at least one axis of a multi-axis manipulator will be described.

In the manipulator control apparatus of the first embodiment, each axis of the manipulator M is provided with a motor 1 for driving an arm 4 as a link. As shown in FIG. 1, the driving force of the motor 1 provided on each axis is transmitted via a reduction gear 2 as a transmission mechanism and a joint torque sensor 3 that detects an output value after the reduction gear 2 to each axis arm. 4 to drive each arm 4 about the axis.
Generally, the speed reducer 2 has a considerably large friction,
Since the joint torque sensor 3 is attached after the reduction gear 2, that is, on the output side, it is possible to suppress the friction of the reduction gear 2 by feeding back the joint torque value detected by the joint torque sensor 3. .

The joint torque controller K calculates the joint torque value from the target joint torque value generated by the target joint torque value generating means 6 and the joint torque value detected by the joint torque sensor 3 by a calculation described later. The operation amount of the motor 1 for following the joint torque value is calculated and output to the motor 1. FIG. 2 is a block diagram of a model of a control target used in the manipulator control device of the first embodiment. The axis of the manipulator M to be controlled is modeled as a two-inertia system.

Symbols in FIG. 2 are J _m : motor inertia moment, J _a , J _ao , Δ _a : arm inertia moment and its nominal value, and fluctuation amount, D _m : motor viscosity, D _a : arm viscosity, K _s , K _so , Δ _k : Transmission system spring constant and its nominal value and fluctuation amount, ω _m : Motor angular velocity, ω _a : Arm angular velocity, τ: Joint torque, u: Operation amount, d _m : Reduction gear Disturbances such as friction applied to the motor 1, d _{a The} arm 4 from the outside
Is a disturbance added to. However, J _m , ω _m , u, d _m represent values converted so that the reduction ratio becomes 1. The variation of the arm inertia moment J _a is calculated by using the variation δ _1.

[Equation 1] In FIG. 2, ω ₁ , ω ′ ₁ , z ₁ , z ′ ₁ are used to express in a feedback form.

Since the identification error of the transmission system spring constant Ks has a great influence on the control performance, the variation δ ₂ is used in order to take this into account.

[Equation 2] In FIG. 2, ω ₂ , ω ′ ₂ , z ₂ and z ′ ₂ are used to express in a feedback form. The nominal model G _n is
Inputs ω ₁ , ω ₂ , da _, dm _, u and outputs z ₁ , z ₂ , τ.
Here, the generalized plant G _p shown in FIG. 3 is constructed. The generalized plant G _p is calculated from the fluctuations δ ₁ and δ _2.

(Equation 3) Enter

[Equation 4] Are output to the fluctuations δ ₁ and δ ₂ . External input d is

(Equation 5) Is. Here, d ₁ and d ₂ are disturbances d _a applied to the arm 4.
And the disturbance d _m applied to the motor 1 are weight functions W _da , W
It is a signal introduced to weight with _dm ,

(Equation 6) And The controlled variable e is

(Equation 7) Is. Where e ₁ is the deviation e ₀ between the joint torque τ and the target joint torque τ _r

[0016]

(Equation 8) If you, the deviation e ₀ in the weight function W _S

[Equation 9] It is weighted like. e ₂ is the joint torque τ with the weighting function W τ

[Equation 10] It is weighted like. e ₃ is the weighting function W _u

[Equation 11] It is weighted like. The output y to the joint torque controller K is

(Equation 12) Is. W _s is a low-pass filter with a large gain in the low frequency range in order to reduce the deviation e _o in the low frequency range. Wτ has a large gain in the high frequency range in order to reduce the influence of noise of the joint torque sensor 3 in the high frequency range.

(Equation 13) And Here, ω _c represents the cutoff frequency of W τ (s). W _u represents the size of the operation amount u limit and is a constant. W _dm and W _da are the disturbance d _m applied to the motor 1 and the arm 4
Represents the magnitude of the disturbance d _a added to and is a constant. Then the state space representation of the generalized plant G _p is

[0017]

[Equation 14]

(Equation 15)

[Equation 16] However,

[Equation 17]

(Equation 18)

[Formula 19]

(Equation 20)

[Equation 21]

[Equation 22] Is. Here, the joint torque controller K is assumed to satisfy the following equation.

[0018]

(Equation 23) However, Fl (Gp, K) is an LFT representation of Gp and K. Δp is

[Equation 24]

(Equation 25) And Δ _F is a virtual fluctuation. μΔp

[Outside 1] Is under Δp

[Outside 2] Is a structured singular value of, defined by

[Outside 3] Indicates the maximum singular value of Δ. The joint torque controller K satisfying the equations (23), (24) and (25) can be obtained by using DK iteration (see Tsutomu Mita, H∞ control, pp.174-176, Shokoido (1994)). .

FIG. 4 shows the transfer characteristic of the joint torque controller K of the first embodiment having the above structure. With respect to this joint torque controller K, the characteristics of the control system when the arm inertia moment J _a and the transmission system spring constant Ks fluctuate so as to satisfy equations 1 and 2 are shown in FIGS. Figure 5 (a) shows the transfer characteristics e ₀ / τ _r from the target joint torque τ _r to the deviation e _0.
And W _s ^-1 . Transfer characteristic e ₀ / τ _r is W
Since it is kept small in the low frequency range by _s ^-1 ,
The joint torque τ can be made to follow the target joint torque τ _r in the low frequency range specified by W _s .

FIG. 5 (b) shows the transfer characteristics τ / τ _r and W τ ^-1 from the target joint torque τ _r to the joint torque τ. Since the transfer characteristic τ / τ _r is kept small by W τ in the high frequency range, the influence of noise can be reduced in the high frequency range specified by W τ. Further, since it has no resonance point, it has excellent stability. Fig. 5 (c) shows the disturbance applied to the motor.
from d _m illustrates a transfer characteristic tau / d _m up joint torque. Since is suppressed smaller than the transfer characteristic tau / d _m is approximately 0 dB, can be suppressed disturbance d _m applied to the motor.
FIG. 5 (d) is a representation of the transfer characteristic tau / d _a from the disturbance d _a applied to the arm to the joint torque tau. Transfer characteristic τ / d _a
Is less than 0 dB, so the disturbance d _a applied to the arm can be suppressed.

FIG. 6 shows the experimental results of the first embodiment. Figure
6 (a) has the smallest arm inertia moment J _a , that is, J _a
This is the result when = J _a0 −Δ _a . Figure 6 (b) shows the results when the arm inertia moment J _a is maximum, that is, J _a = J _a0 + Δ _a . In any case, the joint torque τ rises quickly and follows the target joint torque τ _r without causing a steady deviation, so that the friction of the speed reducer can be sufficiently suppressed.

(Second Embodiment) In the second embodiment, as shown in FIG. 7, an encoder 70 as an angle detecting means for detecting the rotation angle of the motor 10 on the output shaft of the motor 10.
Is attached. The angle signal output from the encoder is converted into an angular velocity signal by the differential calculator 50. The joint torque controller K calculates the operation amount of the motor 10 from the target joint torque value generated by the target joint torque value generation means 60, the joint torque value detected by the joint torque sensor 30, and the motor angular velocity. The generalized plant Gp is constructed as shown in FIG. There are two differences from the first embodiment. The first point is that the external input d is

(Equation 26) Is to Where d ₄ is a signal introduced to weight the noise n added to the motor angular velocity ω _m with the weighting function W _n ,

[Equation 27] Is. W _n represents the magnitude of the noise n and is a constant.

The second point is the output y to the joint torque controller K.
To

[Equation 28] Is to Where e _n is the motor angular velocity ω _m
And the deviation of noise n

[Equation 29] Is. Then the state space representation of the generalized plant G _P is

[Equation 30]

[Equation 31]

[Equation 32]

However,

[Expression 33]

(Equation 34)

[Equation 35]

[Equation 36]

(37)

(38) xW: State variable. Here, the joint torque controller k is expressed by equations 23 and 2
4 and Equation 25 are satisfied, and it is obtained by using DK iteration.

Also in the second embodiment, since the joint torque controller K satisfies the expressions 23, 24 and 25, even if the arm inertia moment J _a and the transmission system spring constant K _s change,
The joint torque τ can be made to follow the target joint torque τ _r , the friction of the transmission system can be suppressed, and the resonance of the transmission system can be suppressed and stabilized. In addition, since the motor angular velocity is fed back, the second embodiment has an advantage in practical use that it can be sufficiently stabilized and the bandwidth of the control system is widened.

[Brief description of drawings]

FIG. 1 is a block diagram showing a first embodiment.

FIG. 2 is a block diagram of a model of a control target used in the first embodiment.

FIG. 3 is a block diagram of a generalized plant used in the first embodiment.

FIG. 4 is a diagram showing a transfer characteristic of the joint torque controller of the first embodiment.

FIG. 5 is a diagram showing a transfer characteristic of the control system of the first embodiment.

FIG. 6 is a diagram showing a response of joint torque when the control system of the first embodiment is applied to an actual machine.

FIG. 7 is a block diagram showing a second embodiment.

FIG. 8 is a block diagram of a generalized plant used in the second embodiment.

[Explanation of symbols]

 1, 10 Motor 2, 20 Reducer 3, 30 Joint Torque Sensor 4, 40 Arm K Joint Torque Controller 6, 60 Target Joint Torque Value Generating Means 50 Differential Calculator M Manipulator 70 Encoder

Claims (1)

[Claims] 1. A motor for driving a link via a transmission mechanism, torque detection means for detecting output torque after the transmission mechanism, and target joint torque value generation means for generating a target joint torque value. A joint torque control calculating means for calculating an operation amount of the motor from the target joint torque value and the output torque value detected by the torque detecting means, wherein the joint torque control calculating means is adapted to change a predetermined arm inertia moment. On the other hand, the target joint torque value, the disturbance applied to the motor, and the disturbance applied to the link are input, and the transfer characteristics from those inputs to the output torque value detected by the torque detection means and the output torque value from those inputs The manipulator characterized in that it is configured by using a structured singular value so that the transfer characteristic up to the deviation between the target joint torque value and the target joint torque value is not more than a predetermined magnitude. Other control devices.