JPH1148181A - Load parameter estimating method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a method decreasing an error in an estimation calculation value, able to estimate an accurate load parameter. SOLUTION: In a load parameter estimating method, in the case of controlling a servomotor driving a multi-shaft robot having a link mechanism, from a motor torque command value of each shaft, data of a motor position and an arm length of each shaft, estimation calculating weight and a position of the center of gravity of a load mounted in a tip end of a robot arm, first based on the motor torque command value of a shaft including the center of rotation of the shaft having the link mechanism, motor position data, and the arm length, weight of the load is estimation calculated, next based on the motor torque command value of the other shaft, motor position data, and the arm length, a position of the center of gravity of the load is calculated. A characteristic of the link mechanism is effectively utilized, weight of the load in a tip end is calculated, calculation is very simple, an error of each shaft is prevented from being accumulated, as a result, tip end load weight Mg can be accurately calculated.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to control of a servomotor for driving a multi-axis robot including a parallel link mechanism, and more particularly to estimating a load parameter such as a weight of a load attached to a tip of a robot arm and a position of a center of gravity. On how to do it.

[0002]

2. Description of the Related Art When a multi-axis mechanical system is controlled by a plurality of servomotors, a method of improving vibration and response or realizing a desired operation by applying a control having an actual machine model to a control unit. Is often used. In order to obtain the effect of those control methods of sufficient consists parameter of the load attached to the shaft tip, i.e., the weight M _g and the center of gravity position _{(e x, e y, e} z) is necessary to be seen accurately . Conventionally, these values are set at the time of load installation or load replacement. 1. A method in which an operator manually inputs a design value as a parameter. A method has been adopted in which each axis is operated and an estimation calculation is made from the torque value or current value at that time. Hereinafter, an example of the second method will be described. The torque or current value corresponding to the load at the tip of each axis is
(i) (i = 1,2,3 ... : i axis th), the weight of the front end load M _g, the gravity center position _{(e x, e y, e} z) When, T (i)
It is expressed in the form of T (i) = f (i ) (e x, e y, e z, M g) ··· (1). Here, f (i) is a function uniquely obtained from each axis length and each axis angle. Since there are four unknown variables, the tip load parameter is estimated by deriving the above equation (1) for four axes or four attitudes and solving a quaternary simultaneous equation.

[0003]

However, the first method in which the operator manually inputs design values as parameters is used when the load shape is complicated and the position of the center of gravity cannot be calculated accurately, or when the operator inputs the design value. In such a case, there is a possibility that an error from the actual machine may increase due to an erroneous input or the like, and there is a problem that it takes time and labor to accurately calculate the position of the center of gravity and input parameters. In the second method, the posture of the robot, the unknown variables _{(M g, e x, e} y, e
_Since the degree of influence of _z ) on the torque value of a certain axis changes, it is difficult to select four equations. In addition, when there is a variable that does not significantly affect the torque values of all four equations, especially when the torque value T (i) includes an error, the error of the estimated calculation value becomes very large, and an accurate value is obtained. There was a problem that it could not be estimated. Further, even if the four equations can be optimally selected, if the torque value T (i) of each axis includes an error, the error in the estimated calculation value increases as a result of accumulation of errors in the calculation process. There was a problem. Therefore, an object of the present invention is to provide a method capable of reducing an error of an estimated calculation value and estimating an accurate load parameter.

[0004]

In order to solve the above problem, a load parameter estimating method according to the present invention provides a motor torque command for each axis when controlling a servomotor that drives a multi-axis robot having a parallel link mechanism. In the load parameter estimation method for estimating and calculating the weight and the center of gravity of the load attached to the robot arm tip from the values, the motor position data, and the arm length of each axis, first, the axis including the rotation center of the axis having the parallel link mechanism is used. The weight of the load is estimated and calculated based on the motor torque command value, the motor position data, and the arm length, and then, based on the motor torque command values, the motor position data, and the arm length of the other axes, the load of the load is calculated. The position of the center of gravity is calculated.

As a first embodiment, the friction torque and the acceleration / deceleration torque are canceled based on the motor torque command values obtained when the respective axes are rotated forward and backward, as the motor torque command values for each axis. The value calculated only for the gravitational torque is used. As a second embodiment, instead of the motor torque command value, a value obtained by integrating a motor torque command value within a certain operation time for each control cycle and dividing the integrated value by the number of times of integration is used. As a third embodiment, a motor current command value is used instead of the motor torque command value. As a fourth embodiment, as a method of estimating and calculating the position of the center of gravity of the load, the value of each axis motor torque command value during arm operation is monitored, and from each axis position at the peak of the motor torque command value, An angle in the direction of the center of gravity on the axis coordinates is calculated, and the position of the center of gravity of the load is estimated and calculated by using the angle in the direction of the center of gravity and the arm length. As a fifth embodiment,
The operation program of the robot has an instruction for performing an estimation operation and an estimation calculation.

[0006]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram showing a configuration of a robot control device for implementing the method of the present invention. In the figure, a robot controller 30 has a processor board 31, and the processor board 31 has a processor 3.
1a, a ROM 31b, a RAM 31c, and nonvolatile memories 35a and 35b are mounted. Processor 31a
Controls the entire robot controller 30 based on a program preset in the ROM 31b. RAM 31
Various data are stored in c. An operation program of the robot 100 and a program for estimating a load parameter according to the present invention are loaded from the ROM 31b into the nonvolatile memories 35a and 35b. Each operation and each calculation for estimating the load parameter shown in FIGS. 2 and 3 described below are functions of software that the processor 31a reads and executes programs in the nonvolatile memories 35a and 35b. Processor board 31
Is coupled to a bus 37. The digital servo control circuit 32 is connected to the bus 37 and drives the servomotors 51, 52, 53, 54, 55 and 56 via the servo amplifier 33 according to a command from the processor board 31. These servo motors are built in the robot 100 and operate each axis of the robot 100. The serial port 34 is connected to a bus 37 and is connected to a teaching operation panel 57 and other RS232C devices 58. The teaching operation panel 57 is used for inputting teaching points to the robot. Also,
Input and output of data and signals with the outside via the I / O 36 are performed.

FIG. 2 is a flowchart illustrating the method of the present invention. This embodiment will be described using the configuration of the six-axis robot shown in FIG. In FIG. 5, 1 to 6 are the first axes, respectively.
The sixth axis 7 is a load. The third shaft has a parallel link mechanism constituted by 501 and 502. The method of the second embodiment is used as a method of calculating the motor torque command value required for the estimation calculation. Hereinafter, this will be described in detail with reference to the flowchart of FIG. In (STEP 1), first, the second shaft 2 is operated in the positive and negative directions, and the motor torque command value at that time is defined as a positive direction T (+) and a negative direction T (+).
Save (-). In (STEP 2), the acceleration / deceleration torque and the friction torque are canceled from the motor torque command value stored in STEP 1, and only the weight torque T _g2 ′ is calculated. As shown in FIG. 4, the second axis gravitational torque T _g2 ′ can be obtained by dividing the sum of the torques in the positive direction operation and the negative direction operation by 2. T _g2 ′ = ｛[T (+)] + [T (−)]｝ / 2 (2)

Next, the method of the third embodiment will be described.
Motor torque command value positive direction T (+) stored in STEP1
When the negative direction T (-) as a value obtained by accumulating for each control cycle of the motor torque command value while operating the second shaft 2 in the positive direction INT _T (+), are operated in the negative direction The value obtained by integrating the motor torque command value during each control cycle is INT _T (-)
And If the number of times of integration is N, the expression (2) becomes: T _g2 ′ = {[INT _T (+)] + [INT _T (−)]} / N / 2 (2 ′) . By using the method of the third embodiment, when there is a ripple or noise in the motor torque command value, the influence of those errors can be reduced. Here, since T _g2 ′ is the gravitational torque when the attitude angle θ ₂ of the second shaft 2 is inclined from the horizontal, the gravitational moment T _g2 co
s (θ ₂ ) times. T _g2 ′ = T _g2 * cos (θ ₂ ) (3) Therefore, the gravitational moment T _g2 is calculated by T _g2 = T _g2 ′ / cos (θ ₂ ) (4). (STEP3)

In (STEP 4), the weight M of the load 7 at the tip is
Calculate _g . Here, since the third shaft 3 has a parallel link mechanism, the torque of the weight fraction of the load 7 attached to the sixth axis 6 tip, center-of-gravity position _{(e x, e y, e} z) regardless of the value of,
Only the gravitational torque is applied to the joint between the second shaft 2 and the third shaft 3. Thus, the weight M _g of the _{tip, M g = (T g2 -T} g2 ini) / L determined by only calculation formula ₂ (5). L ₂ is a second axis arm length.
T _{g2 ini} is the second value when no load is applied to the tip in advance.
This is a value obtained by calculating or measuring the gravitational moment applied to the axis 2. Above, the weight M _g of the tip load 7 using only the second shaft 2 are One Motomema.

[0010] In (STEP5), the center of gravity position (e _x, e _y,
e _z ). In the present embodiment, one embodiment of the method of estimating and calculating the position of the center of gravity shown in the fifth embodiment will be described. Hereinafter, description will be made with reference to the flowchart of FIG. (STE
P5-1) First, it is confirmed that the rotation axis direction of the sixth shaft 6 is not a vertical direction. If it is oriented vertically, the fourth shaft 5 and the fifth shaft 5 are operated and adjusted to an appropriate angle (45 ° or more in the embodiment). (STEP 5-2) Next, the sixth shaft 6 is operated at a low speed of 90 ° or more in the positive direction. The operation at a low speed is for suppressing the torque during acceleration / deceleration. Monitor the motor torque command value at this time,
The angle θ _p (the center of gravity of the load is located in this direction) between the X-axis of the sixth axis and the sixth coordinate when the peak value (maximum or minimum value) is obtained is stored. (FIG. 6) (STEP 5-3) Next, the sixth axis 6 is operated to move the fifth axis 5
So that the position of the center of gravity of the load is in the rotation direction of. Here, the fifth shaft 5 is operated at a low speed of 90 ° or more, and θ _g5 is determined from the motor position when the motor torque command value reaches a peak value. (FIG. 7) (STEP 5-4) Next, the fifth axis 5 is operated so that the fourth axis 4 and the fifth axis 5 are on a straight line. Here the third
The shaft 3 is operated at 90 ° or more at a low speed, and θ _g3 is obtained from the motor position when the motor torque command value reaches a peak value.
(FIG. 8)

[0011] center of gravity of the tip load _{7 (e x, e y,} e z) , the arm length L ₃ of the third shaft 3, the fifth axis arm length L _5, theta _p,
By solving the geometric problem of FIG. 8 using θ _g3 and θ _g5 , it can be obtained as follows. The L ₆ in FIG. 8, holds two expressions. _{L 6 = (L 3 + L} 5 + e z) * tan (θ g3) ··· (6) L 6 = (L 5 + e z) * tan (θ g5) ··· (7) (6) (7) Solving the equation for e _z yields equation (8). _{e z = {(L 3 +} L 5) * tan (θ g3) -L 5 * tan (θ g5)} / {tan (θ g5) -tan (θ g3)} · ·· also (8), e _x , e _y from the L ₆ θ _p, can be determined as follows. _{e x = L 6 * cos (} θ p) = {(L 5 + e z) * tan (θ g5)} * cos (θ p) ··· (9) e y = L 6 * sin (θ p) = {(L ₅ + e _z ) * tan (θ _g5 )} * sin (θ _p ) (10) The above is the method of estimating and calculating the position of the center of gravity of the tip load 7.

Further, in this _{embodiment, (e x, e y,} e z) as the estimated calculation method has been determined that all geometrically, theta
After calculating the _p and theta _g5, fourth shaft 4 or to operate the fifth shaft 5, a motor torque command value T _g4 at that time, a moment length L _g4 from the weight M _g of the tip load 7 calculated in the previous ( (Fig. 7)
_{Look, L g4 = T g4 / M} g ··· (11) with them, it is also possible to determine the barycentric position _{(e x, e y, e} z). Only the calculation results are shown below. _{e z = L g4 * cos (} θ g5) -L 5 ··· (12) e x = {L g4 * sin (θ g5)} * cos (θ p) ··· (13) e y = {L _g4 * sin (θ _g5 )｝ * sin (θ _p ) (14) Also in this case, unlike the conventional method, errors in the motor torque command values of the fourth, fifth, and sixth axes are accumulated. Therefore, the position of the center of gravity can be obtained with high accuracy.

Finally, the method of the fourth embodiment will be described. An operation program is created to teach and execute the operation to the robot. The operation program is described in a specific programming language. The method according to the fourth embodiment has, as one of the instructions of the programming language, an instruction for executing the processing from STEP 1 to STEP 5 and executes the program including the instruction to execute each of the axes. The operation and a series of processing in each STEP are automatically executed.

[0014]

As described above, according to the present invention,
Effective use of the parallel link mechanism's characteristic that only the load weight value can be calculated using the motor torque command value of the axis including the rotation center of the axis with the parallel link mechanism, regardless of the position of the center of gravity of the tip load. However, since the weight of the load at the tip is calculated, the calculation is very simple, no error is accumulated for each axis, and as a result, the weight M of the tip load is accurately determined.
There is an effect that _g can be calculated. Also, if a method of geometrically solving from the arm position at the peak of the motor torque command value is used to calculate the load center of gravity position, if the motor torque command value or the previously calculated load weight Mg includes an error, There is an effect that the position of the center of gravity can be accurately estimated and calculated without being affected by these errors.
Further, if the method of the fourth embodiment is used, the operator simply attaches a load to the end of the robot arm and executes the program, and all the rest is automatically operated and calculated on the control software. Thus, the load parameter can be easily and accurately estimated with high accuracy, and the operator's labor and working time can be minimized.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration of a robot control device for implementing a method of the present invention.

FIG. 2 is a flowchart illustrating a method of the present invention.

FIG. 3 is a flowchart illustrating a method of estimating and calculating a load center of gravity position according to the present invention;

FIG. 4 is an explanatory diagram of an equation for calculating the value of the gravity component torque.

FIG. 5 is a configuration diagram of a robot showing an embodiment of the present invention.

FIG. 6 is a diagram illustrating STEP 5-2 according to the embodiment of the present invention.

FIG. 7 is a diagram illustrating STEP5-3 according to the embodiment of the present invention.

FIG. 8 is a diagram illustrating STEP5-4 of the embodiment of the present invention.

[Explanation of symbols]

1 1st axis, 2nd axis, 3rd axis, 4th axis, 5
5th axis, 6th axis, 7 load, 30 robot controller, 31 processor board, 31a processor,
31b ROM, 31c RAM, 32 digital servo control circuit, 33 servo amplifier, 34 serial port, 35a, 35b nonvolatile memory, 36 I /
O, 37 bus, 51-56 servo motor, 57 teaching operation panel, 58 RS232C device, 100 robot

Claims (6)

[Claims] When controlling a servomotor for driving a multi-axis robot having a parallel link mechanism, a load attached to the tip of a robot arm is determined from a motor torque command value and motor position data of each axis and an arm length of each axis. A load parameter estimating method for estimating and calculating a weight and a center of gravity of the load. First, the weight of the load is determined based on a motor torque command value of a shaft including a rotation center of the shaft having the parallel link mechanism, motor position data, and an arm length. And then calculating the position of the center of gravity of the load based on the motor torque command values of the other axes, the motor position data, and the arm length. 2. A friction torque component and an acceleration / deceleration torque component are canceled from the motor torque command values when the respective shafts perform forward rotation and reverse rotation as the motor torque command values for each axis,
2. The load parameter estimating method according to claim 1, wherein a value obtained by calculating only the gravity torque is used. 3. The method according to claim 2, wherein a motor torque command value within a certain operation time is integrated for each control cycle, and a value obtained by dividing the integrated value by the number of times of integration is used instead of the motor torque command value. 3. The load parameter estimation method according to 1 or 2. 4. The method according to claim 1, wherein a motor current command value is used instead of the motor torque command value.
4. The load parameter estimation method according to 2 or 3. 5. A method of estimating and calculating the position of the center of gravity of the load, wherein a value of a motor torque command value of each axis during arm operation is monitored, and each axis coordinate is calculated from each axis position at the peak of the motor torque command value. The angle in the direction of the center of gravity above is calculated, and the position of the center of gravity of the load is estimated and calculated by using the angle in the direction of the center of gravity and the arm length. The described load parameter estimation method. 6. The load parameter estimating method according to claim 1, wherein an instruction for performing an estimating operation and an estimating calculation is included in the operation program of the robot.