JPH06266425A - Controller of robot - Google Patents

Abstract

PURPOSE:To improve the locus accuracy by calculating the feedforward compensation quantity by target speed and target acceleration on the basis of a prescribed relation formed between a calculated driving controller and a transfer function of a feedforward compensation device. CONSTITUTION:In a target actuation arithmetic part 1 of a target arithmetic part 20, a target actuation rd(n) by which the tool tip of a robot is to move, is calculated. In a reverse conversion arithmetic part 2, a sequential position on the target actuation is converted to a target position thetad(n) of each shaft of the robot. In a distribution arithmetic part 4 of a servo-arithmetic part 30, a processing for distributing each shaft target position thetad of every sampling time DELTAT to a target value thetad(k) of every sampling time DELTAt of the servo- arithmetic part 30 is executed. In a proportional control arithmetic part 5, a position deviation (e) obtained by subtracting the present position theta of an arm 15 detected by a position detector 13 from the calculated target position thetad is inputted, and this deviation (e) is multiplied by proportional gain kp, and it is outputted as the feedback compensation quantity.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a controller for an industrial robot, and more particularly, when controlling the robot by feedforward control, it improves the response to a motion command of the robot and improves the trajectory accuracy of a tool such as a hand at the tip of the robot. The present invention relates to a control device that can improve the.

[0002]

2. Description of the Related Art Conventionally, feedforward control of each axis of a robot has been generally performed, and a control block diagram thereof is shown in FIG.

When a target position θd of each axis of the robot is given as shown in FIG.
First, the target speed θ. d is obtained by the difference method or the like.
In the following, θ. d is the first derivative of θd, and θ. ． Let d be the second derivative of θd.

Next, the target speed θ. d is multiplied by the feedforward gain KF, and the feedforward compensation amount is obtained via the D / A converter and the filter. On the other hand, the deviation e between the target position θd and the current position θ is multiplied by the transfer function K (s) of the position feedback compensation element 70, and the feedback compensation amount is output via the D / A converter. The feedforward compensation amount and the feedback compensation amount thus obtained are added, and the added value is used as the speed command value in the servo amplifier 81.
Added to. Then, the robot arm is driven via the motor 82 and the speed reducer in the latter stage. Reference numeral 83 is a position detector attached to the motor 82 for feeding back the arm position θ.

Generally, if the calculation cycle of the feedback compensating element 70 is large and the calculation cycle of the feedforward compensating element 80, which is greatly affected by the speed command, is the same, vibration and other inconveniences occur. A smooth signal is obtained by adding the above filter to the feedforward compensation element 80. This type of technique is disclosed in, for example, Japanese Patent Application Laid-Open No. 1-108607.

[0006]

As described above, in the conventional feedforward control device, the feedforward control is generally performed by calculating the feedforward amount according to the target speed, which improves the responsiveness. When the PTP control is performed, the steady speed deviation becomes small, and the effect of shortening the cycle time can be obtained. As is well known, the steady speed deviation means a steady position deviation (= target position-current position) that occurs with respect to a constant speed command value, and the PTP control is the teaching 2 When moving between points, it is the control that the position of the tool tip attached to the robot tip is specified only at the start point and the end point, and the trajectory of the tool tip does not matter during movement. The amount of movement of
It is common to determine the required speed for each axis individually and control so as to move the two points at a constant speed.

On the other hand, in CP control, when moving between two taught points, not only the start point and the end point but also the trajectory of the tool tip is specified, and the trajectory of the tool tip is obtained from the taught speed. The target position of each axis is obtained by inverse conversion, and acceleration is generated for each axis. Therefore, even if the feedforward amount is calculated according to the target speed and the feedforward control is performed as described above, in the CP control, the speed constantly changes at each axis level, so the deviation should be reduced. Therefore, it is not possible to expect improvement in trajectory accuracy. The present invention has been made in view of such circumstances, and an object of the present invention is to provide a device capable of improving the trajectory accuracy of a robot even when CP control is performed.

[0008]

Therefore, in the main invention of the present invention, the target position of each axis of the robot is calculated based on the target trajectory of the tool tip attached to the robot tip, and the feedback compensation amount is calculated based on the target position. And a feedforward compensation amount is calculated in the feedforward compensation device based on the target position, and the added value of the feedforward compensation amount and the feedback compensation amount is added as a speed command value to the drive control device, In a controller of a robot configured to drive-control each axis of the robot by a controller, the drive controller is regarded as a first-order lag element, a transfer function of the drive controller is calculated, and the calculated drive controller Based on a predetermined relationship established between the transfer function and the transfer function of the feedforward compensator Obtains an equation for calculating the feedforward compensation amount by the target speed and the target acceleration of the robot axes, so that computing the feedforward compensation amount using the 該演 formula.

[0009]

In other words, according to this configuration, the robot drive control device including, for example, the servo amplifier, the motor, the speed reducer, and the arm is regarded as the first-order lag element, and the transfer function 1 / (1 + s.Tm) of the drive control device is considered. ) Is calculated, and a predetermined relationship F (s) = 1 / G (s) is established between the calculated transfer function G (s) of the drive controller and the transfer function F (s) of the feedforward compensator. ) Based on the target velocity θ. d and target acceleration θ. ． Calculation formula for calculating the feedforward compensation amount VFF from d and VFF
(K) = KF · {θ. d (k) + Tm · θ. ． d (k)} is obtained, and the feedforward compensation amount VFF is calculated using this calculation formula. Here, the feedforward compensation amount is calculated not only on the basis of the target speed but also on the basis of the target acceleration, so that even in the case of CP control in which the speed constantly changes for each axis, The deviation due to the influence of acceleration can be suppressed to a small value, and the trajectory accuracy is dramatically improved.

[0010]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of a robot controller according to the present invention will be described below with reference to the drawings.

In the embodiment, it is premised that feedforward control as shown in the control block diagram of FIG. 3 is performed, and hereinafter, the transfer function G (s) of the controlled object 10 and the feedforward compensation will be referred to with reference to this figure. The relationship established between the transfer function F (s) of the element 50 will be examined. Note that FIG. 3 is a control system similar to that of FIG. 4, in which outputs of the feedback compensation element 40 and the feedforward compensation element 50 are added, and the added value is used as a speed command value in the servo amplifier 11, the motor 12, and the speed reducer 14. , The arm 15 is added to the controlled object 10. Reference numeral 13 is a position detector attached to the motor 12 for detecting the feedback amount θ.

The relationship between the current rotational position θ of the arm 15 and the target position θd is expressed by the following equation (1) when expressed by using each transfer function.

Θ (s) = G (s) · {K (s) + F (s)} / (1 + G (s) · K (s)) · θd (s) (1) Here, K (S) is the feedback compensation element 40
Is the transfer function of.

Now, if the condition that the arm 15 completely follows the target value θd is set, it means that the transfer function in the above equation (1) is equal to 1, and G (s) · {K (s) + F (s)} / (1 + G (s) · K (s)) = 1 (2) When the transfer function F (s) is obtained from the equation (2), F (s) = 1 / G (s) (3) Therefore, if the transfer function G (s) of the controlled object 10 can be obtained as described later, the transfer function F (s) of the feedforward compensation element 50 can be obtained from the above equation (3).

Now, FIG. 1 is an embodiment apparatus constructed by adding various elements to the control apparatus shown in FIG. 3, and mainly comprises a target value computing section 20 and a servo computing section 30. There is. The same parts as those in FIG. 3 are designated by the same reference numerals.

The target value calculation unit 20 is a target trajectory rd of the tool tip attached to the arm tip of a robot (not shown).
Target value θd of the position of each axis of the robot based on (n)
The process of calculating (n) is mainly performed, while the servo calculator 30 mainly performs the servo calculation for each axis of the robot so as to follow the target value. In the drawings, for convenience of description, the servo calculation unit 30
In the above description, the servo calculation for one axis is performed, but the same servo calculation is actually performed for the number of control axes. In the target value calculation unit 20, the sampling time Δ
The target value is calculated for each T, and the servo calculation unit 30 performs the servo calculation for each sampling time Δt.
Since the calculation amount of the target value is larger than that of the servo calculation, the sampling time ΔT of the target value is shorter than the sampling time Δt of the servo, for example, ΔT / Δt = 10.
It is a relatively large value.

First, in the target trajectory calculation unit 1 of the target value calculation unit 20, the target trajectory r to which the tool tip of the robot should move.
d (n) is calculated. Here, n is the target value calculation unit 20.
Is the number of times of calculation of, that is, the number of times of sampling.

In the inverse transformation computing unit 2, the computed successive positions on the target trajectory rd (n) are transformed into the target positions θd (n) of the robot axes. However, conversion is performed in the case of CP control, and in the case of PTP control, the conversion processing in the inverse conversion calculation unit 2 is omitted. In the distribution calculation unit 4 of the servo calculation unit 30, each axis target position θd (n) for each sampling time ΔT calculated by the inverse conversion calculation unit 2 is
Target value θ for each sampling time Δt of the servo calculation unit 30
The process of distributing to d (k) is performed. Here, k is the number of calculations of the servo calculator 30, that is, the number of samplings, and is an integer value within the range of 1 ≦ k ≦ ΔT / Δt. In the proportional control calculation unit 5, the calculated target position θ
A position deviation e (k) obtained by subtracting the current position θ (k) of the arm 15 detected by the position detector 13 from d (k) is input, and this deviation e (k) is multiplied by a proportional gain Kp. , This is output as the feedback compensation amount.

This feedback compensation amount is D / not shown.
It is output through the A converter, added with the feedforward compensation amount VFF (k) as described later, and this added value is added to the servo amplifier 11 as the speed command value VCMD (k). The servo amplifier 11 uses the above speed command value VCMD (k)
The motor 12 is driven according to the
The arm 15 is driven via. On the other hand, the position detector 13
Then, the position of the motor 12 is detected as the arm position, and the detected value θ (k) is fed back.

Now, the controlled object 10 including the arm 15 and the like can be modeled by considering it as a first-order lag system (element) with respect to the speed command value VCMD (s).
This is because the frequency characteristics of the servo amplifier can be ignored in the frequency range that controls the robot, the motor can be regarded as a first-order lag system with respect to the speed command, and if the reducer and arm are inertia terms only, the inertia term of the motor is Since it can be included, it can be assumed that the controlled object 10 is a first-order lag system with respect to the speed command value.

Therefore, the controlled object 10 can be modeled as shown in the characteristic of FIG. In the figure, Tm represents a time constant, and in the case of a mechanical system, it is a mechanical time constant. This mechanical time constant changes depending on the moment of inertia, and the two have a proportional relationship. Since the moment of inertia changes depending on the posture of the robot, the time constant T
As will be described later, m will be calculated according to the posture state of the robot.

When modeled as shown in FIG. 2, the arm speed θ. Is expressed by the following equation.

Θ. (S) = G (s) · VCMD (s) = (1 / KF) · {1 / (1 + s · Tm)} · VCMD (s) (4) where KF is a conversion coefficient , Tm are time constants. Therefore, the transfer function F (s) in the feedforward compensation element 50 is obtained from the above equation (3) and the above equation (4) as shown in the following equation (5).

F (s) = KF · (1 + s · Tm) (5) Therefore, the target velocity θ. The relationship between d and the feedforward compensation amount VFF is: VFF (s) = KF · (1 + s · Tm) · θ. d (s) (6), and eventually, the feedforward compensation amount VFF is the target velocity θ. d
And the target acceleration θ. ． It will be required by d and.

VFF (k) = KF · {θ. d (n) + Tm · θ. ． d (n)} (7) According to this arithmetic expression, the target velocity and acceleration of each axis are required, and therefore these are obtained from the target position θd (n) by the difference as in the following equation.

Θ. d (n) = {θd (n) −θd (n−1)} / ΔT (8) θ. ． d (n) = {θ. d (n) −θ. d (n−1)} / ΔT (9) The above equations (8) and (9) are calculated by the target value calculation unit 2
It is performed by the velocity / acceleration calculation unit 3 of 0. Alternatively, the servo calculator 30 may perform this calculation. Now, assuming that the target speed / acceleration obtained by the speed / acceleration calculation unit 3 is used as it is, the servo calculation unit 30 performs the feedforward control calculation of the equation (7), the calculation cycle of the target value calculation unit 20. ΔT is the calculation cycle Δ of the servo calculation unit 30
It is larger than t (the left side of equation (7) is the number of operations k
And the right side is the number of calculations n), the motor 12
This causes the inconvenience that the machine vibrates. Therefore, the straight-line approximation calculation unit 6 obtains a target value for each shorter cycle for each servo calculation period based on the target value for each target value calculation period, and then the feedforward control calculation unit 7 calculates the target value of the above formula (7). The feedforward control calculation is performed.

The contents of the calculation in the calculation unit 6 are described in the following (1
Expressions (0) and (11) show the contents of calculation in the calculation unit 7 based on the calculation result of the calculation unit 6 in the following formula (12).

Θ. d (k) = {(k0-k) .theta .. d (n-1) + k.theta .. d (n)} / k0 (10) θ. ． d (k) = {(k0-k) .theta .. ． d (n−1) + k · θ. ． d (n)} / k0 (11) VFF (k) = KF · {θ. d (k) + Tm · θ. ． d (k)} (12) where k0 = ΔT / Δt.

Thus, the feedforward compensation amount VFF (k) obtained by the equation (12) is added to the feedback compensation amount, and the added value is output to the servo amplifier 11 as the speed command value VCMD (k). .

The above equations (10) and (11) are simple approximation equations, and can be easily realized as an arithmetic operation in a computer, and there is a merit that the smoothing effect is great.

The load converted to the motor shaft 12 of the arm 15 may fluctuate significantly due to the change in the posture of the arm. In this case, the transfer function G ( It is necessary to successively calculate the parameters of s) online. Among the parameters of the controlled object 10, KF does not change because it is a conversion coefficient, and the time constant Tm fluctuates according to the change of the posture of the robot, so this can be obtained online as in the following equation.

Tm = tm · (Jm + J (θ)) / Jm (13) where tm is the mechanical time constant of the motor alone, Jm is the moment of inertia of the motor alone, and J (θ) is the motor shaft of the arm 15. It is a converted moment of inertia and changes depending on the posture of the arm 15. Note that J (θ) is a predetermined function that differs depending on the robot structure.

The calculation of the inertia moment J (θ) is carried out by the moment of inertia calculation unit 8, the calculation of the time constant Tm by the equation (13) is carried out by the time constant calculation unit 9, and the obtained time constant Tm is fed. In addition to the forward control calculation unit 7, the feedforward compensation amount VFF (k) of the above formula (12) is calculated.

As described above, according to this embodiment, the velocity / acceleration feedforward control computing unit 7 is modeled by modeling the controlled object 10 as a first-order lag system.
Since the feedforward compensation amount VFF is calculated from the target speed and target acceleration of each axis and the feedforward control is performed based on this, the trajectory accuracy is dramatically improved even in CP control in which there is constant speed fluctuation at each axis level. Improve to.

Even when the calculation cycle in the target value calculation unit 20 and the calculation cycle in the servo calculation unit 30 are different, the target value for each servo calculation cycle is obtained in the linear approximation calculation unit 6, and the feedforward is performed from this. Since the compensation amount VFF (k) is calculated, accurate control is performed without vibration of the motor 12.

Further, the moment of inertia calculation unit 8 and the time constant calculation unit 9 successively calculate the time constant Tm according to the posture state of the robot online to sequentially obtain the transfer function G (s) of the controlled object 10. Therefore, in the case of controlling a machine such as a robot that has large load fluctuations, improvement of trajectory accuracy can be achieved.

[0037]

As described above, according to the present invention,
By considering the drive control device of the robot as a first-order lag element, the feedforward compensation amount is calculated not only based on the target speed but also based on the target acceleration, so that the speed is constantly calculated for each axis. Even in the case of the CP control in which the fluctuation of 1 occurs, the deviation due to the influence of the acceleration can be suppressed to a small value, and the trajectory accuracy is dramatically improved.

[Brief description of drawings]

FIG. 1 is a control block diagram showing a configuration of an embodiment of a robot controller according to the present invention.

FIG. 2 is a characteristic diagram showing a relationship between a frequency and an amplitude ratio when the controlled object shown in FIG. 1 is regarded as a first-order delay system and modeled.

FIG. 3 is a control block diagram showing a simplified control system which is a premise of the embodiment apparatus shown in FIG.

FIG. 4 is a control block diagram showing a conventional general feedforward control system.

[Explanation of symbols]

 7 Feedforward control calculation unit 10 Control target 20 Target value calculation unit 30 Servo calculation unit

Claims (4)

[Claims] 1. A target position of each axis of the robot is calculated based on a target trajectory of a tool tip attached to the robot tip, a feedback compensation amount is calculated based on the target position, and a feedforward compensation is performed based on the target position. Calculate the feedforward compensation amount in the device,
The addition value of the feedforward compensation amount and the feedback compensation amount is added to the drive control device as a speed command value,
A robot controller configured to drive-control each axis of the robot by the drive controller, wherein the drive controller is regarded as a first-order lag element, a transfer function of the drive controller is calculated, and the calculated drive control is performed. Based on a predetermined relationship established between the transfer function of the device and the transfer function of the feedforward compensator, an arithmetic expression for calculating the feedforward compensation amount by the target velocity and target acceleration of each axis of the robot is obtained. A control device for a robot, wherein the feedforward compensation amount is calculated using the calculation formula. 2. The robot controller according to claim 1, wherein a time constant, which is a parameter of the transfer function indicating the first-order lag, is sequentially calculated according to the current posture of the robot. 3. The transfer function of the drive controller is G
The control device for a robot according to claim 1, wherein a predetermined relationship that a transfer function of the feedforward compensator is 1 / G (s) when (s) is satisfied. 4. A target of each robot axis for each target position calculation cycle when a speed command value calculation cycle for calculating the speed command value is smaller than a target position calculation cycle for calculating a target position of each axis of the robot. The target velocity and target acceleration of each axis of the robot for each speed command value calculation cycle are calculated based on the speed and the target acceleration, and the feedforward compensation amount is calculated for each speed command value calculation cycle based on the calculated value. The control device for the robot according to claim 1, configured to do so.