JPH11184512A - Control gain determining method for robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To switch a gain corresponding to the state of a load applied to a shaft to operate a robot at high speed within the range not to saturate an output torque. SOLUTION: This method has a step for finding the required acceleration/ deceleration time in the case of executing a moving command from the set acceleration/deceleration, step (S4) for proximately estimating the output torque of each axis motor at the time of acceleration and deceleration completion concerning the operating command generated based on this required acceleration/deceleration time and defining that value as an estimate torque and step for comparing (S5) this estimate torque with the previously stored torque parameter for comparison of each axis, setting low (S6) the gain of feed forward control when the estimate torque exceeds the torque parameter for comparison, setting high (S7) the gain of feed forward control when the estimate torque does not exceed the torque parameter for comparison and applying a command to the motor.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

The present invention relates to a method for determining a control gain of a robot.

[0002]

2. Description of the Related Art Generally, a robot performing a path operation performs acceleration / deceleration control to obtain a smooth motion. At the time of command generation, linear acceleration / deceleration control for performing constant acceleration and deceleration is often used. This command is smoothed by the servo control unit. FIG. 2 is a graph showing a relationship between a moving distance of each axis of the robot, a reaching speed, and an output torque. FIG.
When the movement amount is sufficiently large as in (a), the operation speed reaches the command speed. Normally, the acceleration / deceleration time constant is determined so that the generated output torque does not exceed the allowable value. However, as shown in FIG. 2B, when the movement distance of the designated movement command is short, the generated command does not reach the taught command speed. In this case, since the actual speed of the motor does not reach the speed at which the operation command is reached due to the servo delay, there is a margin in the output torque of the motor. Therefore,
When operating at such a short moving distance, FIG.
As shown in (c), a method is generally adopted in which the gain of the feedforward control is set high, the response of the motor is accelerated, and the operation speed is increased. Conventionally, in the determination of the gain switching, the moving distance and the moving amount of each axis are compared with a moving amount determining value stored in advance, and the gain is set to be higher when the determining value is smaller than the moving amount determining value. Alternatively, it is determined whether or not the arrival speed of the command has reached the instruction speed at the time of teaching, and if not, the gain is set high.

[0003]

However, when the robot actually operates, the load applied to each axis differs depending on the posture and operation speed of the robot, interference torque from other axes, and the like. Therefore, as shown in FIG. 2D, even when the moving amount is small or the speed does not reach, if the static load is large or the interference torque from another axis is large, a high gain is applied. , Output torque is saturated. On the other hand, if an attempt is made to set the movement amount determination value so that the output torque does not saturate when the load is large,
Since the number of patterns to which a high gain is applied decreases, the effect of speeding up cannot be obtained. Therefore, an object of the present invention is to provide a robot control method that switches a gain according to the state of a load applied to an axis in order to operate the robot at high speed within a range where output torque is not saturated.

[0004]

SUMMARY OF THE INVENTION In order to solve the above-mentioned problems, the present invention provides a gravitational moment, inertia due to acceleration, and movement of other axes, depending on the position and posture of a teaching point of a robot, the movement direction and movement speed of each axis. Determining a required acceleration / deceleration time required for executing a movement command from a set acceleration / deceleration in a method for determining a control gain of a robot having a drive shaft affected by interference torque or the like, In the generated operation command, the output torque of each axis motor at the time of completion of acceleration and at the time of completion of deceleration is approximately estimated, and the value is used as the estimated torque. If the estimated torque exceeds the comparison torque parameter, the feedforward control gain is set low and the If the torque does not exceed the torque parameter for comparison, and setting a high gain of the feedforward control, it is characterized in that a step of providing a command to the motor. By the above means, the gain of the feedforward control is switched in accordance with the arrival speed of each axis and the state of the load, so that the robot can be operated at high speed within a range where the output torque is not saturated.

[0005]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS One embodiment of the present invention will be described below with reference to the drawings. FIG. 3 is a skeleton diagram showing the teaching points and the form of the robot. The teaching point (S) is the operation start point, and the teaching point (E) is the operation end point. FIG. 4 is a skeleton diagram showing the mechanism of the robot used in the present embodiment, and is composed of three axes. 1 is a first axis having a degree of freedom about an axis parallel to the ground, 2 is a first arm driven around the first axis 1, and 3 is provided at the tip of the first arm 2 and is parallel to the first axis 1. A second axis 4 having a degree of freedom around the second axis, a second arm driven around the second axis 3,
5 is provided at the tip of the second arm 4 and has a degree of freedom around an axis parallel to the second axis, and 6 is driven around the third axis 5,
It is a third arm having a mass at its tip. Each axis has one degree of freedom and has a total of three degrees of freedom, and is a robot operating in the XY plane. Each axis is affected by a gravitational moment, inertia due to acceleration, interference torque due to operation of another axis, and the like. The acceleration / deceleration control of the robot usually has a maximum acceleration / deceleration as a parameter for each axis, and a required acceleration / deceleration time is obtained from the acceleration / deceleration and the command speed.
The acceleration / deceleration control is performed by aligning all axes during the maximum acceleration / deceleration time. In some cases, Japanese Patent Application Laid-Open No. 5-46
As disclosed in Japanese Patent Publication No. 234, a method is employed in which load inertia is calculated from a position and a posture, and the acceleration / deceleration is varied based on the calculated load inertia. When operating the robot based on the acceleration / deceleration times obtained by these methods, consider estimating the load torque applied to each axis.

[0006] The model shown in FIG.
The load torque on the first shaft 1 from the equation of motion
Then, it is known that it is expressed in the form of equation (1). τ_L1 = M ｛｛2cos (θ_Two+ Θ_Three) r₁r_Three+2 cos θ_Twor₁r_Two+2 cos θ_Three r_Twor_Three+ R₁ ^Two+ R_Two ^Two+ R_Three ^Two｝ Θ₁"+ ｛Cos (θ_Two+ Θ_Three) r₁r_Three+ Cos θ_Twor₁r_Two+2 cos θ_Threer_Twor_Three+ R_Two ^Two + R_Three ^Two｝ Θ_Two"+ ｛Cos (θ_Two+ Θ_Three) r₁r_Three+ Cos θ_Threer_Twor_Three+ R_Three ^Two｝ Θ_Three"-2r₁｛Sin (θ_Two+ Θ_Three) r_Three+ Sin θ_Twor_Two｝ Θ₁'θ_Two'-2r_Three｛Sin (θ_Two+ Θ_Three) r₁+ Sin θ_Threer_Two｝ Θ₁'θ_Three'-2r_Three｛Sin (θ_Two+ Θ_Three) r₁+ Sin θ_Threer_Two｝ Θ_Two'θ_Three'-R₁｛Sin (θ_Two+ Θ_Three) r_Three+ Sin θ_Twor_Two｝ Θ_Two'^Two -R_Three｛Sin (θ_Two+ Θ_Three) r₁+ Sin θ_Threer_Two｝ Θ_Three'^Two −g ｛cos (θ₁+ Θ_Two+ Θ_Three) r_Three+ Cos (θ₁+ Θ_Two) r_Two+ Cos θ₁r₁｝｝ (1) where τ_L1Is the load torque generated on the first shaft 1,
θ₁', θ_Two', θ_Three'Is the command speed of each axis, θ₁", θ_Two", θ_Three"
Represents acceleration of each axis, and g represents gravitational acceleration. Distance r₁, r_Two,
r_Three, Mass m is known. Command speed θ₁', θ_Two', θ
_Three', Angle θ₁, θ_Two, θ_ThreeGiven that
Equation (1) is obtained by calculating acceleration θ as shown in equation (2).₁", θ_Two",
θ_ThreeCan be rewritten as an expression that uses only "._L1= A₁・ Θ₁"+ A_Two・ Θ_Two"+ A_Three・ Θ_Three"+ Τ_Lv1+ Τ_Lg1…… (2) where a₁= M ｛2cos (θ_Two+ Θ_Three) r₁r_Three+2 cos θ_Twor₁r_Two+2 cos θ_Threer_Twor _Three + R₁ ^Two+ R_Two ^Two+ R_Three ^TwoＡ a_Two= M ｛cos (θ_Two+ Θ_Three) r₁r_Three+ Cos θ_Twor₁r_Two+2 cos θ_Threer_Twor_Three+ R_Two ^Two+ R_Three ^TwoＡ a_Three= M ｛cos (θ_Two+ Θ_Three) r₁r_Three+ Cos θ_Threer_Twor_Three+ R_Three ^Two｝ Τ_Lv1= M ｛-2r₁｛Sin (θ_Two+ Θ_Three) r_Three+ Sin θ_Twor_Two｝ Θ₁'θ_Two'-2r_Three｛Sin (θ_Two+ Θ_Three) r₁+ Sin θ_Threer_Two｝ Θ₁'θ_Three'-2r_Three｛Sin (θ_Two+ Θ_Three) r₁+ Sin θ_Threer_Two｝ Θ_Two'θ_Three'-R₁｛Sin (θ_Two+ Θ_Three) r_Three+ Sin θ_Twor_Two｝ Θ_Two'^Two -R_Three｛Sin (θ_Two+ Θ_Three) r₁+ Sin θ_Threer_Two｝ Θ_Three'^Two｝ Τ_Lg1= M ｛-g ｛cos (θ₁+ Θ_Two+ Θ_Three) r_Three+ Cos (θ₁+ Θ_Two) r_Two+ Cos θ ₁ r₁｝｝

Since acceleration = velocity / time, if the acceleration time is set to t, θ ″ = θ ′ / t, and the equation can be modified as follows: τ _L1 = ・ a ₁ · θ ₁ ′ + A ₂ · θ ₂ '+ a ₃ · θ ₃ '｝ / t + τ _Lv1 + τ _Lg1 ... (3) Since the command speed θ * 'is known, the inside of ｛｝ can be regarded as a coefficient. The equation can be transformed as follows: τ _L1 = τ _La1 / t + τ _Lv1 + τ _Lg1 where τ _La1 = a ₁ · θ ₁ '+ a ₂ · θ ₂ ' + a ₃ · θ ₃ '2nd axis 3rd, 3rd the shaft 5 also, the equation for the load torque is derived from the equation of motion of Lagrange only differs by the same method as described above, respectively (4) can be obtained an equation corresponding to equation. that is, tau _li = Τ _Lai / t
+ Τ _Lvi + τ _Lgi (4) The above is considered about the load torque acting on the load shaft. This is converted into a motor shaft and considered. The torque at the motor shaft is (torque given from the load (arm side) via the speed reducer) + (moment of inertia due to rotation of the motor shaft itself). _{That, τ m = τ L / (} η · N) + J m · θ m " However, tau _m: motor shaft torque, tau _L: Load shaft torque eta: Efficiency, N: reduction ratio J _m: motor shaft inertia, theta _m ": motor-axis acceleration _{θ m" = θ 'm /} t than _{τ mi = (τ Lai / t} + τ Lvi + τ Lgi) / (η · N)
+ J _mi · θ ′ _mi / t = ｛τ _Lai / (η · N) + J _mi ·
θ ′ _mi ｝ / t + (τ _Lvi + τ _Lgi ) / (η · N) where τ _ai = ｛τ _Lai / (η · N) + J _mi · θ ′ _mi ｝ τ _vi = τ _Lvi / (η · N N) τ _gi = τ _Lgi / (η · N).

In this way, from the Lagrange's equation of motion, the load torque applied to the motor shaft can be calculated as follows: torque generated by acceleration (moment of inertia): τ _ai , torque generated by speed (centrifugal force, Coriolis force): τ _vi , torque generated by gravity (gravitational moment): τ _gi can be separated into three. Hereinafter, these are referred to as acceleration term torque, velocity term torque, and gravity term torque, respectively. These are functions of each axis speed and angle as follows. τ _ai = f _ai ｛θ ′ *, θ * τ τ _vi = f _vi ｛θ '*, θ *｝ τ _gi = f _gi ｛θ *｝ * * = 1 to 3 axes …… (5)

In this case, Lagrange's equation of motion is solved by considering only the mass point at the tip of the robot.
Regarding the mass and the like of the robot arm itself, a model of a concentrated mass is considered, the load torque generated by each mass is obtained, and finally the sum of the individual load torques is the load torque applied to each axis. Therefore, for each term of the equation (5), the value for each cell may be calculated in the same manner to obtain the sum. Although the series drive type robot has been described above, the parallel drive type (parallel link type) robot is also similar except that the form of the load torque calculation formula derived from Lagrange's equation of motion is different. Can be applied.

The load torque obtained in this way is:
This is an estimated value when the speed of each axis reaches the command speed.
However, if the movement amount of the actual operation command is small, FIG.
As shown in (b), the shaft speed may not reach the command speed: _Vref . Here, the rate actually operation command reaches: V _r, acceleration time required to reach V _r: consider t _r. If the amount of movement is large enough and there is a steady speed portion, then V _r = V _ref . The operation of the servo-controlled motor in response to a command given from the host
This includes the servo delay. Taking these into consideration, the load torque applied to each axis is obtained as an estimated torque: _τei .

First, considering the upper command, (4)
From the equation, τ _ei = (τ _ai (V _r / V _ref )) / t _r + τ _vi (V _r / V _ref ) ² + τ _gi (6) Further, a servo delay is taken into consideration. Is approximated by first-order _{lag, τ ei = {(τ ai} · (V r / V ref)) / t r + τ vi · (V r / V ref) 2} · (1-exp (-kp · t r)) + Τ _gi …… (7)

Here, kp is an estimated time constant, which is prepared in advance as a parameter. Strictly, each axis angle at the time of completion of acceleration has moved from the starting point. However, _assuming that the difference is sufficiently small, each axis angle when obtaining τ _gi may be approximated by the starting point position. Thus, the load torque actually applied to each axis can be approximately estimated.
Allowable torque: T _pi , efficiency coefficient:
η _i is prepared. And the estimated torque τ _ei ,
The allowable torque T _pi × the efficiency coefficient η _i are compared. If τ _ei <(τ _pi · η _i ) as a result of the comparison, there is still room for the load torque. Therefore, the responsiveness can be improved by setting the gain of the FF control high. On the other hand, τ _ei > τ
when a _pi · eta _i is the load torque, such that sufficiently large, the gain of the FF control is set low, to avoid saturation of the torque.

Hereinafter, a state in which the gain of the FF control is set to a high level is referred to as “HIGH gain”, and a state in which the gain is set to a low level is referred to as “L”.
This is referred to as “OW gain”. As one of the methods of setting the acceleration / deceleration time at the time of command generation, there is a method disclosed in JP-A-7-261822. Japanese Patent Application Laid-Open No. Hei 7-261822 describes a method for obtaining the shortest acceleration / deceleration time within a range where each axis does not exceed the allowable torque, based on the above equation (4). According to this, the following equation (8) can be obtained by solving the equation (4) for the acceleration time t.

(Acceleration) t _ai = τ _ai / (T _i − (τ _vi + τ _gi )) (τ _ai > 0: T _i = τ _pi ) (τ _ai <0: T _i = −τ _pi ) ( (During deceleration) t _di = −τ _ai / (T _i − (τ _vi + τ _gi )) (τ _ai > 0: T _i = −τ _pi ) (τ _ai <0: T _i = τ _pi ) (8) )

The acceleration / deceleration time thus obtained is obtained by assuming a load torque applied when the shaft speed reaches the command speed. When the acceleration / deceleration time is obtained in this manner, the command generation unit can perform the linear acceleration / deceleration control based on the acceleration / deceleration time. Therefore, the acceleration / deceleration time is determined according to Japanese Unexamined Patent Application Publication No.
822, the acceleration term torque τ _ai , the velocity term torque τ _vi , and the gravity term torque τ _gi are stored in the local data storage area. When calculating the estimated torque τ _ei , by using the previously stored τ _ai , τ _vi , and τ _gi , more efficient acceleration / deceleration control can be performed without spending unnecessary calculation time. You can do it. FIG. 5 is a block diagram schematically showing a robot control device for performing the method. In the figure, 11 is a teaching unit, 12 is a preprocessing unit, 13 is a command generation unit, 14 is a servo control unit, 15 is a driving unit, 21 is a teaching data storage area, 22 is a parameter storage area, and 23 is an acceleration / deceleration time storage area. , 24 are designated FF gain value storage areas.
The pre-processing unit 12 reads the start point and end point positions, the command speed, and the like from the teaching data storage area 21, determines the acceleration / deceleration time using these and various parameters stored in the parameter storage area 22, and stores the acceleration / deceleration time. Store in area 23. Further, FF gain switching determination is performed, and the result is stored in the specified FF gain value storage area 24. The command generation unit 13 generates a command that is executed at predetermined intervals and performs acceleration / deceleration based on the acceleration / deceleration time stored in the acceleration / deceleration time storage area 23.
Output command to The servo control unit 14 drives the driving unit 15 according to the command from the command generation unit 13.
The FF control at that time is performed in the FF gain designated value storage area 24.
This is executed according to the specified gain value stored in. FIG.
5 is a flowchart relating to processing of a control gain switching method in the preprocessing unit 12 according to the present invention. In the figure, a numerical value following S indicates a step number. The suffix i of each variable represents the axis number. The processing in each step is performed for each axis of the robot.

[S1] The posture at the starting point of the robot,
The load torque component of each axis is obtained from each axis command speed θ ′ _i . What is obtained is each component in the equation (5), τ _ai : acceleration term torque τ _vi : velocity term torque τ _gi : gravity moment.

[S2] Each axis allowable acceleration _Api is obtained from the load torque component obtained in [S1]. Than the allowable torque tau _pi of each axis are stored in advance in the parameter storage area 22, from equation (8), determine the respective permissible axis acceleration time t _ai, t _ai
And the maximum value t _Amax of from the t _Amax and each axis command speed theta _'i, obtaining the respective permissible axis acceleration A _pi. A _pi = θ ' _i / t _aMAX

[S3] Based on the amount of movement from the start point to the end point and the permissible acceleration A _{pi of} each axis, the arrival speed V _r that can be actually reached,
And obtaining the acceleration time t _r to reach the V _r. The calculated _tr is stored in the acceleration / deceleration time storage area 23.
If the movement amount is sufficiently large, _Vr = _Vref .

[S4] For each axis, the estimated torque τ _ei at the time of completion of acceleration is obtained from equation (7).

[S5] The estimated torque τ _ei is compared with (allowable torque τ _pi × efficiency coefficient η _i ) for all axes. If there is at least one axis _satisfying (τ _ei <τ _pi · η _i ), the flow shifts to [S6]. If there is no axis that satisfies (τ _ei <τ _pi · η _i ),
Shift to [S7].

[S6] FF gain designated value storage area 24
"HIGH gain".

[S7] FF gain designated value storage area 24
In the table, “LOW gain” is stored.

Thus, the FF gain for each step is determined in the preprocessing section. The command generation unit 13 generates an operation command at predetermined intervals and sends it to the servo control unit 14.
At this time, the designated FF gain value is read from the designated FF gain value storage area 24 and the FF gain is designated to the servo control unit 14. The servo control unit 14 includes the command generation unit 13
The drive unit 15 is driven by executing the feedforward control with the FF gain designated by the command generation unit 13 in response to the operation command sent from the control unit 13. In the above flowchart,
Although the description is made only when the acceleration is reached, the deceleration can be similarly performed by reversing the sign of the acceleration term torque.

[0023]

As described above, according to the present invention, it is possible to determine the FF gain so that the output torque does not saturate. Therefore, it is possible to operate the robot at high speed without causing vibration due to insufficient torque. This has the effect of shortening the cycle time at low loads and improving the life at high loads.

[Brief description of the drawings]

FIG. 1 is a flowchart showing an embodiment of the present invention.

FIG. 2 is a graph showing a relationship between a moving distance of each axis of a robot, a reaching speed, and an output torque for explaining a problem of a conventional example.

FIG. 3 is a skeleton diagram of a teaching position and a robot for explaining the embodiment.

FIG. 4 is a skeleton diagram showing a configuration of a robot used in the present embodiment.

FIG. 5 is a block diagram showing one embodiment of the present invention.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 1st axis 2 1st arm 3 2nd axis 4 2nd arm 5 3rd axis 6 3rd arm 11 Teaching unit 12 Preprocessing unit 13 Command generation unit 14 Servo control unit 15 Drive unit 21 Teaching data storage area 22 Parameter storage Area 23 Acceleration / deceleration time storage area 24 FF gain specified value storage area

Claims (2)

[Claims] 1. Control of a robot having a drive shaft which is influenced by a gravitational moment, inertia due to acceleration, interference torque due to movement of another axis, etc., depending on the position and posture of a teaching point of the robot, the operation direction and operation speed of each axis. In the gain determination method, a step of obtaining a required acceleration / deceleration time necessary for executing a movement command from a set acceleration / deceleration; and an operation command generated based on the required acceleration / deceleration time, when acceleration and deceleration are completed. Estimating the output torque of each axis motor approximately, and using the value as an estimated torque; comparing the estimated torque with a previously stored comparison torque parameter for each axis, and comparing the estimated torque If the estimated torque does not exceed the comparative torque parameter, the gain of the feedforward control is set low. Setting the gain of the feedforward control high and giving a command to the motor. 2. The set acceleration / deceleration is based on teaching data consisting of the position, operating direction, and operating speed of each drive shaft taught at each teaching point, and the mass and center of gravity of each part of the robot stored as parameters in advance. The load torque component generated on each axis is divided into an acceleration term torque which is a load torque component affected by acceleration / deceleration, and a speed term torque and a gravity term torque which are load torque components not affected by acceleration / deceleration. In the step of calculating and temporarily storing, calculating an allowable acceleration / deceleration such that the torque generated on each axis does not exceed the allowable torque when rising to the command speed, and setting this as the set acceleration / deceleration, in the step of obtaining the estimated torque The acceleration term torque and the speed term torque are approximated as a first-order lag function from the time constant given as a parameter in advance and the required acceleration / deceleration time, Control gain determination method according to claim 1, wherein the robot and obtaining a plus gravity term torque LES as an estimated torque.