JPH08190433A - Load weight estimating method - Google Patents

Abstract

PURPOSE: To estimate the load weight and perform proper feedforward control as to a body attached additionally to a controlled system. CONSTITUTION: The load weight is estimated as to a tool fitted to the tip of the arm of a six-axes robot. The robot begins to be moved first according to a proper operation program. Adaptive control is performed for 4th-6th axes (step S1). At adaptive control convergence time (step S2), estimated moments gr4-gr6 of the 4th-6th axes and the estimated load inertia J5 of the 5th axis are stored (step S3). An additional weight estimation equation is solved as to m6, x6g , y6g , and z6g by using those data and the respective axis values θ3 -θ6 of the robot and parameters m5 , s5z , l5z , J5m, g, etc., and the result is stored (step S4). When the robot reaches an end point position (step S5), the robot is stopped and the processing ends. The found parameters of the load weight are used to calculated feedforward terms when subsequent feedforward control is performed.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control technique for an automatic machine driven by a servomotor such as a robot or a machine tool, and more particularly, to a load weight value required for feedforward control of the automatic machine. Regarding the method for estimating.

[0002]

2. Description of the Related Art Conventionally, feedforward control has been widely used as a control method for reducing a tracking delay of a servo system when a servomotor is used to drive a robot arm or a feed axis of a machine tool. For example, if feedforward is applied to the position loop and velocity loop, the position command (movement command) is differentiated for each position / speed processing cycle, and this differentiated value becomes the speed command value calculated in the position loop process. While the speed command value is obtained by addition, the feedforward term determined based on the parameter related to the inertia is added to the torque command value calculated in the speed loop process, and the process to make the torque command during the feedforward control is performed. Be done.

Therefore, in order for the feedforward control to be properly performed, the calculation of the feedforward term must be performed correctly. The feedforward term naturally changes if there is a change in the system to be controlled. Therefore, conventionally, when a user attaches an additional object (for example, a hand, a processing tool, etc.) to an automatic machine such as a robot, the user inputs parameters such as the mass and the position of the center of gravity of the object, and the parameter is input. Was adopted in the calculation of the feedforward term.

However, when a user attaches a commercially available hand, tool or the like to an automatic machine, the mass and the position of the center of gravity are often not easily known, and the user's burden has to be investigated to find out. Further, if the value of the parameter input by the user is inaccurate, the operating performance of the automatic machine to be controlled may deteriorate.

Adaptive control is known as a method for approximating a controlled object by a model and estimating parameters such as the mass and the position of the center of gravity of the model to achieve a nearly optimal feedforward control. However, this adaptive control method also has the reason that accurate modeling is difficult and calculation becomes complicated when a system having complicated mechanical characteristics such as a 6-axis robot is to be controlled. Therefore, it is difficult to actually cause the controlled object to perform an appropriate operation.

[0006]

SUMMARY OF THE INVENTION Therefore, the object of the present invention is to control an object driven by a servomotor (automatic machine).
For an object additionally attached to the vehicle, the load weight can be estimated without externally inputting parameters related to the load weight of the object (parameters representing mass, center of gravity, etc.), and appropriate feedforward control can be executed. To do so.

Further, the present invention aims to reduce the work load when a user attaches an object such as a hand or a tool to a robot or a machine tool to perform the work.

[0008]

In order to solve the above technical problems, the present invention provides a method for controlling acceleration, speed or position of each axis of a controlled object having a plurality of axes driven by a servomotor. A load weight estimation method for estimating the load weight of an object additionally attached to a controlled object is provided. In the present invention, first, data necessary for calculating a parameter representing the load weight of the additionally attached object is obtained through a process of performing adaptive control on at least some of the plurality of axes. To do. Then, the obtained data is used to calculate a parameter representing the load weight of the additionally attached object.

The calculated parameters can be used for the calculation of the feedforward term when the feedforward control is executed for the controlled object.

When the controlled object having a plurality of axes driven by the servomotor is a robot, a plurality of axes on the hand side of the robot (for example, 6 axes) are used as at least some of the plurality of axes. 4th axis to 6th in robot
Axis) is selected. In this case, the data necessary for calculating the parameter representing the load weight of the additionally attached object includes estimated moments (for example, the fourth axis to the sixth axis in the 6-axis robot) regarding the plurality of axes on the hand side. Data representing the estimated moment of the axis) and data representing the estimated load inertia of at least one of the plurality of axes on the hand side (for example, the estimated load inertia of the fifth axis in a 6-axis robot).

Parameters (mass, center of gravity position, etc.) representing the load weight of the additionally attached object are calculated using these data, and are used for calculation of the feedforward term in the feedforward control executed thereafter. To be done.

[0012]

According to the present invention, with respect to a hand, a tool, a work, etc. additionally attached to a controlled object driven by a servomotor such as a robot or a machine tool, the parameters (mass, center of gravity position, etc.) related to the load weight of the object. The load weight can be estimated by software processing without externally inputting the parameter (representing the parameter). Therefore,
The calculation of the feedforward term at the time of executing the feedforward control thereafter will be properly performed.

When the controlled object is a multi-axis robot such as a 6-axis robot, adaptive control is executed for, for example, the 3 axes on the hand side among the 6 axes, and the estimated moments (eg, 6 axes) associated with these axes are executed through the process. Data representing the fourth to sixth axes of the axis robot and the estimated load inertia of at least one of the plurality of axes on the hand side (for example, the estimated load inertia of the fifth axis in the six-axis robot). ) Is obtained.

Generally, the parameters (mass, center of gravity position, etc.) representing the load weight of the additionally mounted object are expressed by an approximate relational expression using the data obtained through the process of the adaptive control. If these are solved by numerical calculation as an equation in which the parameter representing the load weight of the additionally attached object is an unknown value, the values of those parameters can be obtained. If the feedforward term is calculated using these parameters, appropriate feedforward control is executed.

[0015]

EXAMPLE As an application example of the method of the present invention, here,
When the tool 2 is attached to the hand of the robot 1 having the 6-axis configuration as shown in FIG. 1, the adaptive control is performed for the fourth axis, the fifth axis, and the sixth axis of the robot, and the tool 2 is caused. A method of estimating the load weight will be described. In FIG. 1, θ _{1 to} θ ₆ are variables representing displacement amounts of the first axis to sixth axis of the robot 1, m ₅ , s _5z , and l _5z are the mass, center of gravity position, and length of the arm of the fifth axis, respectively. Is represented. Also, the estimated moments of the 4th, 5th and 6th axes are respectively g
r4, gr5, gr6, the estimated load inertia of the fifth axis is J5, the rotor inertia of the fifth axis motor is Jm5, and the gravitational acceleration is g. Then, the following four equations approximately hold. Hereinafter, these four formulas will be collectively referred to as "load weight estimation equation".

[0016] _{J5 = Jm5 + m 5 s 5z} 2 + m6 {(l 5z +
z _6g ) ² + (x _6g cos θ ₆ + y _6g sin θ ₆ ) ² } gr4 = m5 g (l _5z -s _5z ) cos θ ₃ sin θ ₄ sin θ ₅
+ m6 g {-x _6g cos θ ₃ (sin θ ₄ cos θ ₅ cos θ ₆
+ cos θ ₄ sin θ ₆ ) -y _6g cos θ ₃ (sin θ ₄ cos θ ₅ sin
θ ₆ -cos θ ₄ cos θ ₆ )-(z _6g + l _5z ) cos θ ₃ sin θ ₄
sin θ ₅ } gr5 = m5 g (l _5z -s _5z ) (-cos θ ₃ cos θ ₄ cos
θ ₅ + sin θ ₃ sin θ ₅ ) + m 6 g (-x _6g cos θ ₆ (cos θ ₃
cos θ ₄ sin θ ₅ + sin θ ₃ cos θ ₅ ) -y _6g sin θ ₆ (cos
θ ₃ cos θ ₄ sin θ ₅ + sin θ ₃ cos θ ₅ ) + (z _6g + l _5z )
(cos θ ₃ cos θ ₄ cos θ ₅ -sin θ ₃ sin θ ₅ )} gr6 = m6 g [-x _6g {sin θ ₆ (cos θ ₃ cos θ ₄ cos
θ ₅ -sin θ ₃ sin θ ₅ ) + cos θ ₃ sin θ ₄ cos θ ₆ } + y
_6g (cos θ ₆ (cos θ ₃ cos θ ₄ cos θ ₅ -sin θ ₃ sin θ
₅ ) -cos θ ₃ sin θ ₄ sin θ ₆ }] where m6 is the mass of the tool 2, (x _6g , y _6g , z _6g )
Are coordinates of the barycentric position of the tool 2 as seen from the origin of the coordinate system set at the hand of the robot 1. Therefore, the load weight estimation equation consisting of the above four equations is calculated as m ₆ , x _6g , y _6g , z _6g
Solving sees equations, parameters m6 representing the load weight of these tools _{2, x 6g, y 6g,} z 6g is required.

Here, the axis positions θ _{1 to} θ ₆ are quantities by which the robot can always know their values. The arm of the mass of the fifth axis, the center of gravity position, length m _5, s _5z, for l _5z and fifth shaft of the motor inertia J5m can be set in advance the design value.

The value of the gravitational acceleration can be set in the same manner. Estimated moment gr4 of axis 4, axis 5, axis 6
, Gr5, gr6 and the estimated load inertia J5 of the fifth axis
With respect to, the estimated value can be obtained in the process of causing the robot to perform an appropriate motion while adaptively controlling the fourth axis, the fifth axis, and the sixth axis.

Therefore, an example of processing for teaching the robot the estimated value of the load weight of the tool 2 is shown in FIG. The mass of the arm of the fifth axis, the position of the center of gravity, and the length m
₅ , s _5z , l _5z and setting of the value of the inertia J5m of the motor of the 5th axis, setting of the parameter (time or movement amount) that determines the timing considered as adaptive control convergence, and adaptation for the 4th to 6th axes It is assumed that the teaching of the operation program that causes the robot to perform an appropriate motion while controlling is already executed.

It is preferable that the operation program is such that the hand of the robot changes the position / orientation within a proper range with a certain speed and acceleration so that the operation by the adaptive control is sufficiently converged.

First, the operation program is read block by block, and the movement of the robot is started under the condition that adaptive control is performed on the fourth to sixth axes (step S).
1). When the timing considered as adaptive control convergence has come (step S2), the estimated moment values gr4, g for the fourth to sixth axes at that time point.
r5, gr6 and value of estimated load inertia of axis 5 J5
Is temporarily stored in the buffer memory in the robot controller (step S3). Then, these data, the respective axis values θ _{3 to} θ ₆ of the robot at the time of fixing them, and the preset parameters m ₅ , s _5z , l _5z , J 5m, g
Using the above data, the above equation for estimating weight addition is calculated as m6,
The _solution is solved for x _6g , y _6g , and z _6g , and the result is stored (step S4). When the robot reaches the end point position designated by the operation program (step S5), the robot is stopped (step S6), and the process ends.

The parameters relating to the load weight of the tool (object) 2 thus obtained are used for the calculation of the feedforward term in the subsequent feedforward control. Hereinafter, the outline of the feedforward control will be described for a simple case as shown in FIG.

In FIG. 2, the controlled object driven by the motor 3 (for example, the motor of the first axis) is the arm 4 in the illustrated relationship.
It shows a case where it can be regarded as a system in which the tool (object) 2 is attached to the hand of the. As also shown in the figure, each parameter value Jm, m1, s1, I1z, l1, m6
, S6 have the following meanings.

Jm; Rotor inertia of motor 3 m1; Mass of arm s1; Position of center of gravity of arm 4 I1z ;; Inertia around center of gravity of arm 4 with respect to z axis (z component of inertia around center of gravity) l1; Length of arm M6; mass of the tool 2 s6; distance from the tip of the arm 4 to the center of gravity of the tool 2, where Jm is a parameter preset in the robot,
m1, s1, I1z and l1 are parameters calculated from the posture of the robot. The value obtained in the above process is used as m6. The value of s6, the value obtained by the above process x _{6 g,} y _{6 g,} is calculated from z _{6 g} and the robot posture.

The feedforward term in this case is defined as follows. First, the equation of motion of the controlled object is established. If the output torque and the position of the motor 3 are τ and θ, respectively, the equation of motion is given by the following equation (1). Where J = Jm + m1.s1 + I1z, L = I1
When expressed as + s6, the equation (1) is represented by the equation (2).

[0026]

[Equation 1] This right side is directly adopted as the feedforward term and added to the speed command value. FIG. 3 shows a block diagram of feedforward control for adding this feedforward term to the position loop. As is clear from the figure, the position command output from the position creation unit (CNC in the case of machine tool) of the robot controller at a predetermined cycle is compared with the position detector (pulse coder) of the motor and the deviation is calculated. To be done. Gain K based on the calculated position deviation
The position loop processing is executed at p, and the speed command before addition of the feedforward term is created.

On the other hand, the position command (movement command) is differentiated for each position / speed processing cycle, and this differential value is added to the speed command obtained by the position loop processing executed by the gain Kp. Further, regarding the torque command value calculated by the velocity loop process executed with the gain Kv, the right side of the above equation (2), that is, (J + m6.L) multiplied by the second derivative of the position command is fed-forward term. Is added to the torque command value. A current is supplied from the servo amplifier to the motor 3 based on the torque command thus calculated, and the motor 3 is driven. It should be noted that 1 / (J + m6.L) is a transfer function representing the load applied to the motor 3, and 1 / s represents that the integrated motor speed is the rotational position of the motor.

Although the present invention has been described above by taking a 6-axis robot as an example, the above description is given even if the controlled object is changed to a robot having a different number of axes (for example, a 4-axis robot) or a machine tool (for example, a 3-axis configuration). It is obvious that the above idea can be applied to calculate the load weight on an additionally mounted object.

[0029]

According to the present invention, parameters such as a hand and a tool, which are additionally attached to a controlled object driven by a servomotor such as a robot or a machine tool, such as a mass and a position of the center of gravity are externally set. The load weight is properly estimated without input from. Then, based on that, the feedforward term is automatically calculated and the feedforward control is performed, and the appropriate feedforward control is realized without burdening the user.
It is possible to improve the operation accuracy of the automatic machine.

[Brief description of drawings]

FIG. 1 is a diagram showing an axis configuration of a robot to which a method of the present invention is applied in an embodiment.

FIG. 2 is a diagram illustrating an application example of feedforward control in which the parameters obtained in the embodiment are used.

FIG. 3 is a block diagram for explaining an outline of feedforward control in the case shown in FIG.

FIG. 4 is a flowchart showing an example of a process for obtaining an estimated value of a load weight, which is executed in the embodiment.

[Explanation of symbols]

 1 robot 2 tool (object attached additionally) 3 motor 4 arm

Claims (3)

[Claims] 1. A load for estimating a load weight of an object additionally attached to the controlled object when controlling the acceleration, velocity or position of each axis of the controlled object having a plurality of axes driven by a servo motor. A method for estimating weight, wherein data necessary for calculating a parameter representing a load weight of the additionally attached object is obtained by performing adaptive control on at least a part of the plurality of axes. And a step of calculating a parameter representing a load weight of the additionally attached object using the acquired data. 2. A load for estimating a load weight of an object additionally attached to the controlled object when controlling acceleration, velocity or position of each axis of the controlled object having a plurality of axes driven by a servo motor. A method for estimating weight, wherein data necessary for calculating a parameter representing a load weight of the additionally attached object is obtained by performing adaptive control on at least a part of the plurality of axes. And a step of calculating a parameter representing a load weight of the additionally attached object using the acquired data, and a step of executing the feedforward control for the control target by the calculated parameter. A method for estimating a load weight, the method including the step of calculating a feedforward term. 3. A control target having a plurality of axes driven by the servomotor is a robot, and at least a part of the plurality of axes is a plurality of axes on the hand side of the robot, The data necessary for calculating the parameter representing the load weight of the additionally mounted object is at least one of the data representing the estimated moment about the plurality of axes on the hand side and the at least one of the plurality of axes on the hand side. The load weight estimation method according to claim 1, further comprising data representing an estimated load inertia of the shaft.