JPH09123075A - Control method of machine having link mechanism and its device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To improve positional accuracy and vibration restraining performance by a precise model or a model by simplifying it. SOLUTION: A machine having a link mechanism composed of a movable part 1 and its support part 2 is turned into a model by considering elastic deformation of the movable part 1 in the movable direction, and in a device to control the machine by using the model, when it is turned into the model M1, the prescribed direction of the movable direction of the movable part 1 and elastic deformation of the support part 2 in the prescribed direction are also considered. By the constitution, positional accuracy and vibration restraining performance can be improved by using an accurate model.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method of controlling a machine having a link mechanism and an apparatus therefor, and more particularly, to a dynamic position control method of a system having a link mechanism and each link being connected by a movable part. And its device.

[0002]

2. Description of the Related Art Conventionally, various techniques relating to dynamic position control of a system having a link mechanism, in which each link is connected by a movable part, have been developed, but these are a robot having a rotary joint or a motor drive. This is related to the system, and all of them deal with the insufficient rigidity of the reducer in the power transmission system. For example, JP-A-61-2013
In the technology disclosed in JP-A-04-04, JP-A-62-204307, and JP-A-62-157790,
Deflection (elastic deformation) caused by insufficient rigidity of the reducer is calculated from the joint angle target value, etc., and the elastic deformation amount is added to the angle command value to the motor so as to compensate for the elastic deformation. We are making improvements. Of these, the previous two
One technique is a control method of adding an elastic deformation amount in a control loop, and the other one technique is a teaching method of adding an elastic deformation amount to teaching data. Further, JP-A-63-033389 and JP-A-6-245570.
In the publication, due to insufficient rigidity of a power transmission system such as a speed reducer, they act as spring elements, and the vibrations caused by them are estimated by an observer, and the estimated elastic deformation speed is fed back to suppress the vibrations. I am trying to

[0003]

In the above-mentioned conventional method for controlling a machine having a link mechanism, only the elastic deformation of the power transmission system, particularly the elastic deformation of the speed reducer, is dealt with. The deformation considers only the twist of the rotating shaft in the rotating direction, for example, a conventional model (M
Based on ₀ ), we are trying to improve the position accuracy and vibration suppression performance by correcting the amount of elastic deformation. However, in a machine with a link mechanism such as an actual robot,
Not only the speed reducer of the power transmission system is elastically deformed,
The movable portion such as the power transmission system and the entire supporting portion that supports the movable portion are elastically deformed. For this reason, the actual machine cannot be accurately represented only by elastic deformation of the joint in the rotational direction (axial twist of the rotary shaft), and the position accuracy and vibration suppression performance of the target machine cannot be improved. Especially, in the vertical articulated robot, the conventional model cannot explain the hand position displacement due to the elastic deformation accompanying the turning (the rotation of the first joint) and the change in the natural vibration frequency due to the posture. Also, in other joints, the effect of shaft twist (difference between motor rotation angle and joint angle) handled in the conventional model is small, and elastic deformation in the rotation axis direction of the movable part and the support part is equivalent to the axial deformation. The actual machine cannot be accurately described unless it is approximated by torsion. That is,
The present invention has been made in view of the above circumstances, and an object of the present invention is to prevent elastic deformation of a movable part including joints and the like, a support part that supports the movable part, and a mechanically insufficient portion. By using a more accurate model that takes into account
(EN) Provided are a machine control method and a machine having a link mechanism capable of improving position accuracy and vibration suppression performance.

[0004]

In order to achieve the above object, a first aspect of the present invention provides a machine having a link mechanism composed of a movable portion and a supporting portion thereof in consideration of elastic deformation of the movable portion in the movable direction. In the control method for controlling the machine by using the model, elastic deformation in a predetermined direction other than the movable direction of the movable part and a predetermined direction of the support part is also considered in the modeling. It is configured as a control method of a machine having a link mechanism characterized by the following. Second
In the above modeling, the invention of is a control method for a machine having a link mechanism that ignores elastic deformation of a movable part or a supporting part that exceeds a predetermined rigidity value among the elastic deformations. The third invention is This is a control method for a machine having a link mechanism in which a plurality of elastic deformations, which are one of the elastic deformations, are represented by one spring element in modeling. Further, it is a method of controlling a machine having a link mechanism in which the center of elastic deformation of one spring element is provided in a region including the centers of a plurality of elastic deformations represented by the spring element.

Further, it is a method of controlling a machine having a link mechanism for determining the center of elastic deformation of one spring element by weighting processing of each center of a plurality of elastic deformations represented by the spring element. Furthermore, when there are a plurality of certain one spring elements representing the plurality of elastic deformations and the values are close to each other, the spring coefficients of the elastic deformations in the predetermined direction are assumed to be the same, so that a plurality of springs are obtained. It is a method of controlling a machine having a link mechanism that further integrates elements. In the fourth invention, in the above modeling, each elastic deformation is separated into components according to the deformation direction, and among the components in the same deformation direction, a component higher than a predetermined stiffness value component is included in a lower component. A method of controlling a machine having a mechanism. Further, in the machine having the link mechanism, at least a first joint that supports the leg portion with respect to the base so as to be rotatable about a vertical axis center, and a first joint that is provided around the first horizontal axis center on the leg portion. A second joint that rotatably supports the first arm portion that is perpendicular to the first arm portion, and a second joint that rotatably supports the second arm portion that is perpendicular to the first horizontal axis center on the first arm portion. A method of controlling a machine having a link mechanism which is a vertical multi-joint robot having a third joint.

Further, the elastic deformation component in the rotational axis direction of the second or third joint is included in the elastic deformation component in the same direction as the rotational axis direction near the second or third joint, and at the same time, the first A method of controlling a machine having a link mechanism, wherein the elastic deformation component of the joint pivot axis is included in the elastic deformation component in the same direction as the pivot axis direction near the first or second joint. Furthermore,
A component of elastic deformation in a direction orthogonal to the rotation axis direction of the second or third joint and also orthogonal to the rotation axis direction of the first joint is in the same direction as the direction in the vicinity of the first or second joint. Is a method of controlling a machine having a link mechanism included in the elastic deformation component of. A fifth invention is a control method for modeling a machine having a link mechanism composed of a movable part and its support part in consideration of elastic deformation of the movable part in the movable direction, and controlling the machine using the model. In the above modeling,
Considering elastic deformation in a predetermined direction other than the movable direction of the movable part and in a predetermined direction of the support part, the elastic deformation in the predetermined direction is separated into components corresponding to the movable direction of the movable part, and the same A method of controlling a machine having a link mechanism, characterized in that elastic deformation in a predetermined direction is included in elastic deformation in a movable direction of a movable part with respect to a component of a direction. A sixth invention is a control method for a machine having the above model as a control target and having a link mechanism for estimating an elastic deformation amount, a differential value of the elastic deformation amount or a disturbance of the control target by using an observer.

A seventh aspect of the present invention is a method of controlling a machine having a link mechanism for subjecting the above model to control and performing robust control in consideration of modeling error. An eighth invention is a modeling means for modeling a machine having a link mechanism composed of a movable part and a supporting part thereof in consideration of elastic deformation of the movable part in the movable direction, and the machine using the model. In the control device including control means for controlling the above, the modeling means considers elastic deformation in a predetermined direction other than the movable direction of the movable part and in a predetermined direction of the support part in the modeling. A machine control device having a link mechanism characterized by the above. Further, in the machine having the link mechanism, at least a first joint that supports the leg portion with respect to the base so as to be rotatable about a vertical axis center, and a first joint that is provided around the first horizontal axis center on the leg portion. A second joint that rotatably supports the first arm portion that is perpendicular to the first arm portion, and a first joint on the first arm portion.
A control device for a machine having a link mechanism that is a vertical articulated robot having a third joint that rotatably supports a second arm portion that is orthogonal to the horizontal axis about the horizontal axis.

[0008]

DETAILED DESCRIPTION OF THE INVENTION AND

DESCRIPTION OF THE PREFERRED EMBODIMENTS Embodiments and examples of the present invention will be described below with reference to the accompanying drawings to provide an understanding of the present invention.
It should be noted that the following embodiments and examples are merely examples embodying the present invention and are not of the nature to limit the technical scope of the present invention.

FIG. 1 is a structural diagram of a _first model M ₁ when the method of controlling a machine having a link mechanism according to the first embodiment of the invention and the embodiment (first embodiment) is applied. 2, FIG. 2 is a schematic structural diagram of a vertical articulated robot to which the present method can be applied, and FIG. 3 is a control of a machine having a link mechanism according to an embodiment and an example (first example) of the first invention. Structure diagram of the second model M ₂ when the method is applied, FIG.
Is a comparison diagram between the actual measurement value and the theoretical value of the hand position displacement, and FIG. 5 is a comparison chart between the actual measurement value and the theoretical value of the deformation amount due to twisting, FIG.
Embodiments and examples of the second invention (second example)
FIG. 7 is a structural diagram of a _third model M ₃ when a control method for a machine having a link mechanism according to the present invention is applied, and FIG. 7 is a link mechanism according to an embodiment and an example (second example) of the second invention. Fig. 8 is a structural diagram of a _fourth model M ₄ when the control method for a machine having
FIG. 9 is a structural diagram of a _fifth model M ₅ when the method of controlling a machine having a link mechanism according to the embodiment and example (third example) of the third invention is applied, and FIG. 3 is a structural diagram of a _sixth model M ₆ when the method of controlling a machine having a link mechanism according to an embodiment and an example (third example) of the third invention is applied, and FIG. 11 shows the third invention. FIG. 12 is a structural diagram of a _seventh model M ₇ when the method of controlling a machine having a link mechanism according to the embodiment and the example (third example) is applied, and FIG. FIG. 13 is a structural diagram of an _eighth model M _{8 in} the case of having a control method for a machine having a link mechanism according to an example (third example), and FIG. 13 shows an embodiment and an example (fourth example) of the fourth invention. Fourth embodiment) A structure of a _ninth model M ₉ when the control method of the machine having the link mechanism according to the fourth embodiment is applied. Concrete Figure 14 shows a tenth structure diagram of a model M ₁₀ in the case of applying the mechanical control method having a link mechanism according to the embodiments and examples of the fourth invention (fourth embodiment), Fig. 15 is a comparison diagram of the measured value of the deformation amount and the theoretical value, FIG. 16 is a comparison diagram of the measured value of the deformation amount and the theoretical value, FIG. 17 is a comparison diagram of the measured value of the deformation amount and the theoretical value, and FIG. 20 is a block diagram showing a control system to which the present control method is applied. FIG. 19 is a block diagram showing a control system to which the present control method is applied.
Is a comparison diagram of vibration response of hand position amplitude, FIG. 21 is a comparison diagram of frequency and theoretical value of deformation amount, FIG. 22 is a block diagram showing a control system to which this control method is applied, and FIG. 23 is this control method It is a block diagram which shows the control system which did.

<First Invention> As shown in FIG. 1, a method of controlling a machine having a link mechanism according to an embodiment and an embodiment (first embodiment) of the first invention is a movable part 1 and its This is the same as the conventional example in that a machine having a link mechanism including the support portion 2 is modeled in consideration of elastic deformation of the movable portion in the movable direction and the machine is controlled using the model. However, the first embodiment is different from the conventional example in that the elastic deformation in the predetermined direction other than the movable direction of the movable portion 1 and the predetermined direction of the support portion 2 is also taken into consideration in the modeling. different. A controller of a machine having a link mechanism according to an embodiment and an example (eighth example) of the eighth invention is an apparatus to which the method of the first example can be applied, and the above modeling is performed. It comprises a modeling means for performing the above and a control means for performing the above control. Therefore, in the following, the eighth embodiment will be described by including it in the first embodiment. It should be noted that the machine having the link mechanism has at least a first joint for supporting the leg portion around the vertical axis about the base and a first joint about the first horizontal axis on the leg portion with respect to the axis. Second joint that rotatably supports the first arm portion that is perpendicular to the first arm portion, and a second joint that rotatably supports the second arm portion that is perpendicular to the first horizontal axis center on the first arm portion. It may be a vertical joint robot having a third joint.

A three-axis vertical articulated robot will be described in detail below as an example. In the conventional technique, for example, as shown in FIG. 24, the spring constants k ₁ θ to k ₃ θ exist only in the rotary joint portion, and the spring elements also have elastic deformations ε ₁ θ to ε _{3 in} the rotational axis direction of the rotary joint. It was based on a model that allows only θ and does not allow bending deformation in other directions or elastic deformation in the extension direction at all. On the other hand, in the first embodiment (eighth embodiment, the same applies hereinafter), as shown in FIG. 1, in addition to elastic deformation of the movable parts such as the rotary joints, the movable parts are supported. Considering the elastic deformation of the support, and by adopting a model that allows elastic deformation in any direction,
Higher precision modeling is possible. However, in the model M ₁ of FIG. 1, k _{iR, iS} represent springs on the side driven by and the side supporting the i-th joint, and elastic deformations in the X, Y, and Z directions fixed to the link are represented. ε
_{iRX, Y, Z, iSX, Y, Z} , spring constant is k _{iRX, Y, Z, iSX, Y, Z} , and bending moment is M _{iRX, Y, Z, iSX, Y, Z.}

In addition, since the load inertia of the wrist portion (the fourth to sixth axes in a normal vertical articulated robot) is usually very small, the influence of those elastic deformations can be ignored. However, even if the load inertia is small, if the rigidity is lower than that, the amount of elastic deformation increases, and in that case, the load inertia of the wrist must be considered. FIG. 2 shows a general industrial robot, in which the rotating part of the wrist shaft is located just behind the third shaft, and the entire arm is rotated to rotate the wrist part. Even with such a structure, the inertia in the rotation direction of the fourth shaft does not increase so much, so the elastic deformation of the fourth shaft in the joint angle direction is not large. Therefore, in the conventional model, it is not necessary to consider the elastic deformation of the fourth axis. However, the inertia in directions other than the joint angle direction becomes very large, and the amount of elastic deformation in those directions cannot be ignored. In such a case, in the model according to the first aspect of the present invention, by describing the model M ₂ so as to handle the elastic deformation amount of the movable portion of the fourth shaft and the portion supporting it, as shown in FIG. ， The real machine can be better modeled.

The results of modeling based on these models are shown in FIGS. Of these, FIG. 4 shows theoretical values and actual measured values by the present model (M ₂ ) and the conventional model (M ₀ ) in the displacement of the hand position caused by elastic deformation. Since the conventional model deals only with elastic deformation in the joint rotation direction, it is difficult to describe elastic deformation, and therefore the theoretical value and the measured value vary widely. On the other hand, in this model, other elastic deformations can also be described, and it can be seen that the theoretical values and the measured values are in good agreement. Therefore, when performing actual control,
The control performance can be improved by adopting the amount of elastic deformation calculated by this model. On the other hand, in the conventional model, there is a large difference between the measured value and the theoretical value, so improvement in control performance cannot be expected. In addition, Fig. 5 shows the theoretical value of this model and the conventional model for the hand position displacement due to elastic deformation in the joint rotation direction (twist in the rotation axis direction) and the hand position displacement caused by actually measuring the amount of twist. Shows the relationship. Similar to Fig. 4 above, this model and the measured values are in good agreement, but in the conventional model, all the elastic deformations are to be represented by this twist, so the values are larger than those of the actual machine and the variations are large. .
Further, although FIG. 4 and FIG. 5 are based on exactly the same measurement data, the hand position displacement due to the elastic deformation shown in FIG. 4 reaches 0.5 mm, whereas in FIG. The displacement is only about 0.05 mm. From this, it can be seen that the displacement of the hand position due to the axial twist is less than 10% in the displacement of the hand position due to all elastic deformation. That is, the conventional model tries to describe all elastic deformations only with displacements having an effect of less than 10%, which is unreasonable. Such a phenomenon remarkably appears particularly in the joints (the third and fourth joints in FIG. 5) adjacent to the turning axis and the portion having low rigidity.

By the way, in the first embodiment, the amount of elastic deformation to be considered is large, and the amount of calculation becomes enormous. Therefore, the simplification in this model is described below. As a method for simplifying the model, many reductions in the frequency domain have been proposed. However, in link mechanism systems such as robots, the transfer function changes with posture,
In the conventional simplification method based on the reduction of dimensions, it is necessary to have a transfer function model for each posture, which results in an enormous amount of memory. Therefore, here, the model itself is simplified to reduce the dimension in consideration of changes in posture. The second to fourth inventions were created with this point in mind, and will be described in detail below. <Second Invention> A method of controlling a machine having a link mechanism according to an embodiment and an embodiment (second embodiment) of the second invention is based on the modeling in the first embodiment. Among the deformations, the elastic deformation of the movable portion or the support portion that exceeds a predetermined rigidity value is ignored. For example, if the rigidity of the support portion for the first axis (turn axis) of the robot in the Z direction is low, the model can be simplified by omitting elastic deformation in that direction. This makes it possible to reduce the control calculation.

<Third Invention> Further, the third invention is embodied.
With the link mechanism according to the embodiment and the example (third example)
The machine control method is based on the above model.
Of the shapes, one with multiple elastic deformations with unity
It is represented by a spring element. For example, as shown in Fig. 1 or
Model M shown in FIG.₁, M_TwoThen, the spring that elastically deforms is required.
Elementary k₁θ, k _1ROr k_Twoθ, k_2ROr k_Threeθ,
k_3ROr k_Fourθ, k_4RFor
Can be considered nonexistent (actually, there is a small mass
However, since their effects are minute,
I assume that their mass can be ignored). Therefore, the bullet
Only the position of the center of deformation is slightly deviated.
Therefore, it is possible to represent multiple elastic deformations with a single spring element.
it can. Model M in that case_ThreeIs shown in FIG. In addition,
The inertia due to the output of the 3rd axis and the 4th axis is very small.
6 is ignored, the model of FIG. 6 becomes the model M of FIG._Fourlike
It can be further simplified. Furthermore, the bullet of one spring element mentioned above
Of a plurality of elastic deformations in which the center of elastic deformation is represented by the spring element
Provided in each area including the center, or one of the above
The center of elastic deformation of the spring element of
The number of elastic deformations can be determined by weighting each center.
Can also be.

For example, a plurality of spring elements can be replaced by a single spring element.
The model M shown in FIG._TwoIs the model M of FIG._FourTo simplify
The spring element k in FIG._Threeθ, k_3R, K_4SIn Figure 7
Is the spring element k ′_3R, The spring element k_Threeθ, k
_3R, K_4SPosition p of elastic deformation center of_Threeθ, p_3R, P_4SIs a figure
8, the spring element k ′_3RElastic change of
Position p'of the shape center_3RTo p_Threeθ, p_3R, P_4SSurrounded by
Take in the rear. As a result, the accuracy of the model is not lost and the model is
It is possible to simplify Dell. Or more
Mathematically the position p ′ of the center of elastic deformation_3RAsk for. That is,
Spring element k_Threeθ, k_3R, K_4S Fixed on the link
Spring coefficient K in X, Y, Z directions_Threeθ _{X, Y, Z,}k_{3RX, Y, Z},
k_{4SX, Y, Z}And their elastic deformation center position p_Threeθ,
p_3R, P_4SX, Y, Z components p fixed to the link_Threeθ
_{X, Y, Z}, P_{3RX, Y, Z}, P_{4SX, Y, Z}Where is given
, The elastic deformation center position p ′_3R= [P '_3RX,p '_3RY,p '
_{3R Z}]^TIs calculated by the following equation.

(Equation 1) The elastic deformation center position p ′ _3R obtained by the above equation exists in the area surrounded by the center positions p ₃ θ, p _3R and p _{4S shown} in FIG.

Thus, while maintaining the accuracy of the model,
The model can be simplified and the control calculation amount can be reduced accordingly. Further, in the above modeling, one spring element having a plurality of elastic deformations exists, and when they are close to each other, it is assumed that the spring coefficient of elastic deformation in the above-mentioned predetermined direction is the same. The elements may be further aggregated. For example, in the model M ₄ of FIG. 7, the spring constants for elastic deformation of the movable part and its supporting part other than the movable direction are k _1SX and k _1SX , k
_1SY and k _1SY , k ′ _1RX and k ′ _1RX , k ′ _1RY and k ′
_1RY , k _2SX and k _2SX , k _2sz and k _2sz , k ′ _2RX and k ′ _2RX , k ′ _2RZ and k ′ _2RZ , k _3SX and k _3SX , k
_3sz and k _3sz, k _'3RX and k' _3RX, k _'3RZ and k'
_By just assuming that the _3RZ are the same, the number of parameters (spring coefficient etc.) required for the model is reduced by 6. Further, ignoring the minute effect of the Coriolis force due to the coordinate change, the spring elements k _1S and k ′ _1R ,
Each of k _2S and k ′ _2R and k _3S and k ′ _3R can be represented by one spring element, and can be simplified as the model M _{5 of} FIG. 9 or the model M ₆ of FIG. Further, similarly for the fourth joint, the spring elements k _4RX and k _4RX , k _4RY and k
_Assuming that _4RY is the same, it can be further simplified as the model M ₇ in FIG. 9 to 11
The same simplification can be performed by combining the models. For example, it becomes like the model M _{8 in} FIG. This further simplifies the model and reduces the control calculation amount.

<Fourth Invention> A method of controlling a machine having a link mechanism according to an embodiment and an embodiment (fourth embodiment) of the fourth invention is the model in the first to third embodiments. At the time of realization, each elastic deformation is separated into components according to the deformation direction, and among the components in the same deformation direction, a component higher than a predetermined stiffness value component is included in the lower component. Here, again, in the machine having the link mechanism, at least the first joint that supports the leg portion around the vertical axis about the base and the first axis around the first horizontal axis on the leg is used. A second joint that rotatably supports a first arm portion that is perpendicular to the first arm portion, and a second arm portion that is orthogonal to the first arm portion and is orthogonal to the first horizontal axis center on the first arm portion. Third to support
It may be a vertical joint robot having joints. In that case, the elastic deformation component in the rotational axis direction of the second or third joint is included in the elastic deformation component in the same direction as the rotational axis direction near the second or third joint, and
The elastic deformation component of the joint rotation axis can be included in the elastic deformation component in the same direction as the direction of the rotation axis near the first or second joint. Further, a component of elastic deformation in a direction orthogonal to the rotation axis direction of the second or third joint and also orthogonal to the rotation axis direction of the first joint is compared with the direction in the vicinity of the first or second joint. It can also be included in the elastic deformation components in the same direction.

For example, taking the model M ₈ of the 3-axis vertical articulated robot shown in FIG. 12 as an example, elastic deformations in the X, Y and Z axis directions ε ″ _{1SX, Y, Z} , ε ″ _{2SX, Y, Z} , ε ″
Separately consider 3 _{RX, Y, Z.} Normally, Y-axis direction of the elastic deformation _{ε "1SY, ε" 2SY,} ε " of the _{3RY, ε" 1SY} is an elastic deformation of the support portion for supporting the first shaft, its rigidity is greater than the other axes. Further, ε _"2SY, ε" _3RY is an elastic deformation of the support portion and the joint portion comprising a reduction gear, epsilon "stiffness _1SY is ε" _2SY, ε "greater than _3RY. Therefore, the elastic deformation epsilon" the _{1SY ε "2SY, ε"} by representing the _3RY, simplified models. Furthermore, when the elastic deformation ε ″ _1Sz in the Z direction was _measured for ε ″ _2Sz and ε ″ _3Rz , it was found that the elastic deformation ε ″ _2Sz had a smaller rigidity than other elastic deformations. The model is simplified by expressing ε ″ _2Sz . Similarly, in the X-axis direction, as a result of actual measurement, it was _{found that} the rigidity of elastic deformation ε ″ _2SX is smaller than the other ones. The model is simplified by expressing high elastic deformation by ε ″ _2SX . The model M ₁₀ obtained by this is as shown in Fig. 14. In addition, in a 3-axis vertical articulated robot, deformation in the X direction is usually _observed . Cannot be expressed as a joint angle,
In the model M ₁₀ of FIG. 14 described above, the deformation in the X-axis direction is omitted, and the model M ₉ is represented as the model M ₉ of FIG. Of course, FIG.
The model of is the model with higher accuracy. In particular, when the rigidity of the support portion of the second shaft is low as in this time, it is necessary to adopt the model of FIG. 14, but when the rigidity of the support portion is sufficiently high, the simplified model of FIG. 13 is used. But good.

FIG. 15 shows theoretical values of the deformation amount for these simplified models (Δ in the figure).
mark). Compared to the case without simplification (marked with ◯ in the figure), when these simplifications are performed, there is some variation, but it is better than the measured value of the deformation amount compared with the conventional model (marked with ■ in the figure). I am doing it. In each of the above second to fourth embodiments, the so-called strict model of the first embodiment is simplified. However, it can also be based on the conventional model.
The fifth invention focuses on this point and will be described below. <Fifth Invention> A method of controlling a machine having a link mechanism according to the fifth embodiment of the invention and the embodiment (fifth embodiment) corresponds to the modeling in the first to fourth embodiments. ，
Considering elastic deformation in a predetermined direction other than the movable direction of the movable part and in a predetermined direction of the support part, the elastic deformation in the predetermined direction is separated into components corresponding to the movable direction of the movable part, and the same Regarding the component of the direction, the elastic deformation in the predetermined direction is included in the elastic deformation in the movable direction of the movable portion.

That is, in the conventional model shown in FIG.
It is based on the conventional shaft twist model, but here it is forcible, but other elastic deformations are also attempted to be approximated by shaft twist. Here, including the case where only the actual shaft twist is expressed by the shaft twist model, the theoretical value and the actual measured value by this model and the conventional model are shown in FIG. 16 in the displacement of the hand position caused by the elastic deformation. Compared. As can be seen from the figure, when the model is used (marked with a circle in the figure), the measured value and the theoretical value agree very well. On the other hand, when the shaft twist model represents only the shaft twist with the conventional method as it is (X mark in the figure), the displacement due to the elastic deformation of the hand position cannot be expressed at all. However, even though it is based on the conventional model, when it is approximated by axial twist including other elastic deformations (■ in the figure, this is the same as the conventional model in Fig. 4), it is somewhat more handy than before. The displacement of the position can be approximated. However, as described above, the theoretical value and the actually measured value may be quite different from each other depending on the posture and the like, and thus the variation is large. Therefore, in order to reduce the variation due to the posture while using the conventional model, which is simpler than this model, it is necessary to change the inertia or the spring coefficient according to the posture. Comparing this model with the conventional model, the amount of elastic deformation is small in the contracted posture and large in the extended posture in the conventional model. Therefore, the spring coefficient may be decreased or the inertia may be increased in the contracted posture, or the spring coefficient may be increased or the inertia may be decreased in the extended posture. Further, in order to change the spring coefficient or the inertia, it is sufficient to have a table of the spring coefficient and the inertia according to the posture, and the value of the table may be derived so that the elastic deformation amount matches the present model. FIG.
Figure 7 shows the theoretical values of the hand position deformation when the inertia and spring constant are changed depending on the posture. In the figure, the deformation amount is calculated for three postures, but the theoretical value can be made to match the measured value by forcibly changing the inertia and the spring constant.

As described above, based on the modeling results of this model (for example, M ₉ and M ₁₀ ), the simple conventional model (M ₀ ) which only considers the torsion of the rotating shaft is corrected and the modeling results are approximated, It is possible to obtain an approximate model that is almost equivalent to the more detailed model (M ₉ or M ₁₀ ), and it is possible to perform modeling with high accuracy and small calculation amount. Further, in the above models M _{1 to} M ₁₀ , the equations of motion and the state equations are derived, and when the control is performed, these equations are derived exactly, but this requires very complicated calculation. Therefore, let us consider that the amount of elastic deformation is minute and the equations are simplified. Especially,
By making the elastic deformation amount very small, the calculation amount can be greatly reduced only by replacing the elastic deformation amounts sin and cos, which often appear in the equation, with 0 or 1. Further, it is possible to omit the interference terms between the elastic deformation amounts and omit them. Furthermore, in the derived equation, the off-diagonal terms of the inertial matrix and the off-diagonal terms of its inverse represent the effects of the interference of each state variable, and they generally affect the characteristics of the model. Since it is smaller than the diagonal term, the model can be simplified by omitting the off-diagonal term.

Next, a case of controlling the machine using the above model will be described. That is, the elastic deformation amount is calculated based on the model obtained in any of the first to fifth embodiments, and each joint or hand position command value is corrected so as to correct the elastic deformation amount. Then, even if the robot or the like is elastically deformed, the command value is corrected in anticipation of the elastic deformation amount, so that high position accuracy can be realized. Specifically, FIG. 18 or FIG.
As shown in FIG. That is, in FIG. 18, the elastic deformation amount is estimated from the command value based on this model (elastic deformation model), and the command value is corrected so as to correct the estimated deformation amount.
Here, the amount of elastic deformation that would occur if the robot operates according to the command value is corrected. Also, FIG.
Reference numeral 9 is for estimating the elastic deformation amount from the output value of the robot (actual movement of the robot) based on the elastic deformation model, and correcting the command value so as to correct the estimated deformation amount. The amount of elastic deformation that may occur during operation is corrected.

Further, it is generally the simplest model description to give a joint angle or a motor rotation angle as an input of the elastic deformation model. If the target value in FIG. 18 or the robot output value in FIG. 19 is the hand position, the hand position is converted to the joint angle or the motor rotation angle by performing the inverse conversion in the preprocessing in the figure, and then the elastic deformation model is obtained. Need to enter. Target value of FIG. 18 or FIG.
If the robot output value of 9 is the joint angle or the motor rotation angle, the pre-processing in the figure may be directly input to the elastic deformation model without any processing. As described above, the preprocessing in the figure is to transform the given input into the elastic deformation model so as to match the input. Next, a method of calculating the correction amount will be described. When the command value D is the position or posture of the hand, the Jacobian matrix J _XE from the elastic deformation amount to the desired hand position or posture is multiplied by the elastic deformation vector E whose element is the calculated elastic deformation amount, and the position of the hand is calculated. Alternatively, the posture displacement X _E is calculated. By adding this to the command value D, the effect of elastic deformation can be corrected. This can be expressed as follows.

[0025]

(Equation 2) When the command value is the joint angle, the inverse of the Jacobian matrix J _X θ from the joint angle to the position or posture of the hand is added to the displacement vector X _E having the displacement amount of the position or posture of the hand obtained earlier as an element. Multiply the matrix to calculate the required angle θ _C to correct the displacement. By adding this to the command value D, the effect of elastic deformation can be corrected.

[0026]

(Equation 3) However, since the Jacobian matrix J _X θ is not always regular, there may be no inverse matrix in general. in this case,
A pseudo inverse matrix may be used instead of the inverse matrix. The most common inverse pseudomatrix is

[0027]

(Equation 4) It is. Furthermore, taking advantage of the redundancy of these pseudo inverse matrices,
Weighted pseudo-inverse

[0028]

(Equation 5) By using, for example, the position, posture, or joint angle to be emphasized can be corrected intensively. However,
W is a weight matrix. When corrected as above,
FIG. 20 shows the case where no correction is made. Here, FIG.
Each of (a) and (b) shows a case where the hand position is sin-moved at an amplitude of 1.5 mm at 5 to 1 Hz.
In the case where no correction is made (a), the amplitude becomes larger at higher frequencies due to the effect of elastic deformation. On the other hand, in the case of correction (b), elastic deformation is compensated,
The amplitude is kept at 1.5 mm as desired. Further, when the elastic deformation amount is to be corrected based on the conventional model, not based on the present model, the result is as shown in FIG. That is, since the amount of elastic deformation is not calculated correctly in the conventional model, the elastic deformation is correctly corrected depending on the posture, or adversely, it is adversely affected.

As described above, in the first to fifth embodiments, the elastic deformation amount is calculated based on a model more accurate than the conventional model and the command value to the robot or the like is corrected, so that the control accuracy is significantly increased. Can be improved. Furthermore, by configuring an observer or performing robust control, it is possible to maximize the effects of the first to fifth embodiments. The sixth and seventh inventions have focused on this point. <Sixth Invention> A method of controlling a machine having a link mechanism according to an embodiment and an embodiment (sixth embodiment) of the sixth invention is to control the models of the first to fifth embodiments as control targets. age,
The amount of elastic deformation of the controlled object, the differential value of the amount of elastic deformation, or the disturbance observer is used for estimation.

Here, a major goal of constructing an observer in consideration of elastic deformation is to suppress vibration caused by elastic deformation. In this sense, the model required for constructing the observer needs to describe at least the first-order mode. Fig. 21 shows a gain diagram from torque to elastic deformation of the robot's swivel axis. The vibration mode that can be described by the conventional model is the vibration mode in the swivel axis direction, which is indicated by the circles in the figure. Has been done. When the robot takes the extended state, this vibration mode in the direction of the turning axis appears as the natural vibration first mode, but becomes the second mode in the contracted posture. Conversely, in the contracted posture, the vibration mode in the X-axis direction (marked by □ in the figure) in the model of FIG. 6 becomes low, and this is the first-order mode. That is,
In the robot, since the vibration of the primary mode changes depending on the posture, the conventional model that only considers the vibration in the joint rotation direction cannot describe the primary mode. That is, this is
This means that the observer based on the conventional model cannot describe the first-order mode and cannot suppress vibration. On the other hand, when the observer is configured based on this model, both the vibration mode in the turning direction and the vibration mode in the X-axis direction can be described. Therefore, not only the first-order mode but also the second-order mode can be represented accurately, the situation of vibration can be more accurately estimated by the observer, and the vibration can be suppressed. Further, in the observer based on the conventional model, elastic deformations in three directions are considered, but in the present model as shown in FIG. 1, a considerable amount of elastic deformations must be considered, and the order of the observer is growing.
However, by using the simplification means shown in the second to fourth embodiments, the elastic deformations to be considered can be reduced to a minimum of four, and the order of the observer does not become so large.

As a method of constructing the observer, a model and a state to be estimated (elastic deformation amount, its differential value, or a state including them, or disturbance)
However, the minimum dimensional observer and the same dimensional observer can be easily constructed. However, extending the observer in the prior art to this model requires only replacing the model, and the details are easily understood, so this description is omitted. As described above, a more accurate estimated value can be obtained by configuring the observer for estimating the elastic deformation amount and the like based on the models of the first to fifth embodiments, which are more accurate than the conventional model. ，
By feeding back the estimated value, it is possible to significantly improve the vibration suppressing performance. Furthermore, in order to suppress vibrations by the observer, it is important that the natural vibrations of the actual machine and model match. Therefore, by determining the parameters such as the spring coefficient and inertia of the models of the first to fifth embodiments and the coefficient added to them, at least the first-order mode or the second-order mode of the natural vibration is determined, Higher vibration suppression performance can be demonstrated.

<Seventh Invention> A method for controlling a machine having a link mechanism according to an embodiment and an embodiment (seventh embodiment) of the seventh invention is the model in the first to fifth embodiments. As a control target, robust control is performed in consideration of modeling errors. For example, in robust control such as H ∞ control or μ analysis, the control performance is affected by the magnitude of modeling error. Therefore, the control performance is improved by keeping the modeling error as small as possible. In that sense, robust control effects can be expected in this model. As shown in FIG. 21, in the conventional model, the first-order mode cannot be described, the modeling error is large, and the frequency range where the modeling error is large becomes low (that is, near the first-order mode frequency). Will grow bigger). For this reason, if robust control is applied to the conventional model as it is, the control performance will be low, and the control performance will be significantly deteriorated especially in the low frequency region. On the other hand, when robust control is performed based on this model, the model and the actual machine match well, so the modeling error is small, and therefore the control performance is high. Furthermore,
In this model, at least the first-order mode and the second-order mode can be described with high accuracy, and therefore the frequency region where the modeling error is large is only in the high-frequency region. Therefore, high control performance can be maintained up to a high frequency range.

To design a robust controller based on this model, the conventional model is first replaced with this model in the conventional design method, and the modeling error is calculated or measured. Then, the frequency weight transfer function W (s) is determined so as to suppress the modeling error, and at least this transfer function W (s) and the transfer function P (s) of this model are controlled by a control system design tool (for example, MATLAB or MATRIX
The controller can be designed just by giving it to X). The control system design support tool is the most commonly used support tool among controller designers. As described above, by suppressing the modeling error to be small, the effect of robust control can be maximized and the control performance can be significantly improved. By the way, by calculating or adding the required torque based on this model, it becomes possible to command only the actually required torque to each motor, and high-precision control becomes possible. When based on the conventional model, the required torque value in the vicinity of the natural vibration deviates greatly from the calculated value, and the natural vibration is excited. Specific configuration examples at this time are shown in FIGS. 22 and 23. Therefore, the above control method is particularly effective in a machine having a link mechanism when high-speed dynamic movement is required, and as shown in FIG. 20, elastic deformation occurs due to high-speed movement of the robot, It is very effective when vibration modes are excited.

As described above, in any of the embodiments, an accurate model is obtained by considering the elastic deformation of the movable part including joints and the like, the supporting part for supporting the movable part, and the part where mechanical strength is insufficient. By using a (strict model) or a simplified model, it is possible to greatly improve the position accuracy and vibration suppression performance. Of course, the present invention is also effective for robots other than vertical articulated robots or machines having other link mechanisms.

[0035]

Since the control method and the apparatus for the machine having the link mechanism according to the present invention are configured as described above, the movable part including the joint and the like, the supporting part for supporting the movable part and the mechanical strength. By using a more accurate model that takes into account the elastic deformation of the location where the gap is insufficient, it is possible to improve the position accuracy and vibration suppression performance.

[Brief description of the drawings]

FIG. _{1 is} a structural diagram of a _first model M ₁ when a control method for a machine having a link mechanism according to an embodiment and an example (first example) of the first invention is applied.

FIG. 2 is a schematic structural diagram of a vertical articulated robot to which the present method can be applied.

FIG. 3 is a structural diagram of a _second model M ₂ when the method of controlling a machine having a link mechanism according to the embodiment and example (first example) of the first invention is applied.

FIG. 4 is a comparison diagram of an actually measured value and a theoretical value of a hand position displacement amount.

FIG. 5 is a comparison diagram of a measured value and a theoretical value of a deformation amount due to twisting.

FIG. 6 is a structural diagram of a _third model M ₃ when a method of controlling a machine having a link mechanism according to an embodiment and an example (second example) of the second invention is applied.

FIG. 7 is a structural diagram of a _fourth model M ₄ when a method of controlling a machine having a link mechanism according to an embodiment and an example (second example) of the second invention is applied.

FIG. 8 is an explanatory diagram showing integration of spring elements.

FIG. 9 is a structural diagram of a _fifth model M ₅ when a method of controlling a machine having a link mechanism according to an embodiment and an example (third example) of the third invention is applied.

FIG. 10 shows an embodiment and an example (third embodiment) of the third invention.
FIG. _{6 is} a structural diagram of a _sixth model M ₆ in the case where the control method for the machine having the link mechanism according to the example) is applied.

FIG. 11 shows an embodiment and an example (third embodiment) of the third invention.
FIG. _{7 is} a structural diagram of a _seventh model M ₇ when the method of controlling a machine having a link mechanism according to the embodiment) is applied.

FIG. 12 shows an embodiment and an example (third embodiment) of the third invention.
FIG. _{8 is} a structural diagram of an _eighth model M ₈ when the control method for the machine having the link mechanism according to the example) is applied.

FIG. 13 shows an embodiment and an example (fourth embodiment) of the fourth invention.
FIG. _{9 is} a structural diagram of a _ninth model M ₉ when the control method for the machine having the link mechanism according to the example) is applied.

FIG. 14 shows an embodiment and an example (fourth embodiment) of the fourth invention.
FIG. 13 is a structural diagram of a _tenth model M ₁₀ in the case where the method of controlling a machine having a link mechanism according to the embodiment) is applied.

FIG. 15 is a comparison diagram of the actually measured value and the theoretical value of the deformation amount.

FIG. 16 is a comparison diagram of the measured value and the theoretical value of the deformation amount.

FIG. 17 is a comparison diagram of the measured value and the theoretical value of the deformation amount.

FIG. 18 is a block diagram showing a control system to which the present control method is applied.

FIG. 19 is a block diagram showing a control system to which the present control method is applied.

FIG. 20 is a comparison diagram of vibration response of hand position amplitude.

FIG. 21 is a comparison diagram of the frequency and the theoretical value of the deformation amount.

FIG. 22 is a block diagram showing a control system to which the present control method is applied.

FIG. 23 is a block diagram showing a control system to which the present control method is applied.

FIG. 24 is a structural diagram of a model M ₀ showing a case where a conventional method for controlling a machine having a link mechanism is applied.

[Explanation of symbols]

M _{1 to} M ₁₀ ... Model 1 ... Movable part 2 ... Support part

Claims (15)

[Claims] 1. A control method for modeling a machine having a link mechanism composed of a movable part and its support part in consideration of elastic deformation of the movable part in the movable direction, and controlling the machine using the model. A method of controlling a machine having a link mechanism, characterized in that, in the modeling, elastic deformation of the movable portion in a predetermined direction other than the movable direction and the support portion in a predetermined direction is also taken into consideration. 2. The method for controlling a machine having a link mechanism according to claim 1, wherein, in the modeling, elastic deformation of a movable portion or a support portion that exceeds a predetermined rigidity value among the elastic deformations is ignored. 3. The method of controlling a machine having a link mechanism according to claim 1, wherein, in the modeling, a plurality of elastic deformations having unity among the elastic deformations are represented by one spring element. 4. A method of controlling a machine having a link mechanism according to claim 3, wherein the center of elastic deformation of said one spring element is provided in a region including each center of a plurality of elastic deformations represented by said spring element. . 5. The control of a machine having a link mechanism according to claim 3, wherein the center of elastic deformation of said one spring element is determined by weighting processing of each center of elastic deformation represented by said spring element. Method. 6. A plurality of one spring elements representing the plurality of elastic deformations are present, and when the spring elements have close values, a plurality of spring elements are assumed to have the same spring coefficient of elastic deformation in the predetermined direction. The spring element of claim 3 is further integrated.
A method for controlling a machine having the link mechanism according to any one of 1. 7. In the modeling, each elastic deformation is separated into components according to the deformation direction, and among the components in the same deformation direction, a component higher than a predetermined rigidity value component is included in a lower component. A method for controlling a machine having the link mechanism according to any one of 1 to 6. 8. A machine having the link mechanism, wherein at least a first joint for supporting the leg portion with respect to the base so as to be rotatable about a vertical axis, and a shaft about the first horizontal axis on the leg portion. A second joint that rotatably supports a first arm portion that is orthogonal to the center, and a second arm portion that is orthogonal to the axis and is rotatable about a first horizontal axis on the first arm. A method for controlling a machine having a link mechanism according to any one of claims 1 to 7, wherein the robot is a vertical articulated robot having a third joint supported by the robot. 9. The elastic deformation component in the rotational axis direction of the second or third joint is included in the elastic deformation component in the same direction as the rotational axis direction near the second or third joint, and the first joint is also included. 9. The method of controlling a machine having a link mechanism according to claim 8, wherein the elastic deformation component of the pivot axis is included in the elastic deformation component in the same direction as the pivot axis direction near the first or second joint. 10. A component of elastic deformation in a direction orthogonal to the rotation axis direction of the second or third joint and also orthogonal to the turning axis direction of the first joint is calculated in the vicinity of the first or second joint. The elastic deformation component in the same direction as the above direction is included in the elastic deformation component.
A method for controlling a machine having the described link mechanism. 11. A control method for modeling a machine having a link mechanism composed of a movable part and its support part in consideration of elastic deformation of the movable part in the movable direction, and controlling the machine using the model. In the above modeling, elastic deformation in a predetermined direction other than the movable direction of the movable part and a predetermined direction of the support part is also taken into consideration, and the elastic deformation in the predetermined direction is determined according to the movable direction of the movable part. A method of controlling a machine having a link mechanism, characterized in that the elastic deformation in the predetermined direction is included in the elastic deformation in the movable direction for the components in the same direction. 12. The link mechanism according to claim 1, wherein the model is used as a control target, and an elastic deformation amount of the control target, a differential value of the elastic deformation amount, or a disturbance is estimated by using an observer. Machine control method. 13. A method of controlling a machine having a link mechanism according to claim 1, wherein the model is a control target, and robust control is performed in consideration of modeling errors. 14. Modeling means for modeling a machine having a link mechanism composed of a movable part and its support part in consideration of elastic deformation of the movable part in the movable direction, and the machine using the model. In the control device including control means for controlling, in the modeling, the modeling means also considers elastic deformation in a predetermined direction other than the movable direction of the movable part and a predetermined direction of the support part. A control device for a machine having a link mechanism. 15. A machine having the link mechanism, wherein at least a first joint for supporting the leg portion with respect to the base so as to be rotatable about a vertical axis, and a shaft about the first horizontal axis on the leg portion. A second joint that rotatably supports a first arm portion that is orthogonal to the center, and a second arm portion that is orthogonal to the axis and is rotatable about a first horizontal axis on the first arm. 15. The control device for a machine having a link mechanism according to claim 14, wherein the robot is a vertical articulated robot having a third joint supported by.