JP3120028B2 - Control method for machine having link mechanism - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION The present invention relates to a machine control how having a link mechanism, particularly, has a link mechanism, a dynamic position control method and a system in which each link is coupled by a movable portion It concerns the device.

[0002]

2. Description of the Related Art Conventionally, various techniques relating to dynamic position control of a system having a link mechanism and each link connected by a movable part have been developed. All of these are related to systems, and all report the lack of rigidity of reduction gears in the power transmission system. For example, Japanese Patent Application Laid-Open No. 61-2013
No. 04, JP-A-62-204307, and JP-A-62-157790,
Deflection (elastic deformation) caused by insufficient rigidity of the reduction gear is calculated from the joint angle target value and the like, and the amount of elastic deformation is added to the angle command value to the motor so as to compensate for the elastic deformation. We are improving. Of these, the previous two
One technique is a control method of adding an elastic deformation amount in a control loop, and the other technique is a teaching method of adding an elastic deformation amount to teaching data. Also, JP-A-63-033389 and JP-A-6-245570.
In Japanese Patent Application Laid-Open Publication No. H08-27139, due to insufficient rigidity of a power transmission system such as a speed reducer, they act as a spring element, and the vibration generated thereby is estimated by an observer, and the estimated elastic deformation speed and the like are fed back to suppress the vibration. Is being planned.

[0003]

In the above-described method of controlling a machine having a link mechanism, etc., only the elastic deformation of the power transmission system, particularly the elastic deformation of the speed reducer, is dealt with. The deformation is based on only a conventional model (M
Based on ( ₀ ), attempts are being made to improve 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,
Only the reduction gears of the power transmission system are not elastically deformed.
A movable portion such as a power transmission system and the entire supporting portion supporting the movable portion are elastically deformed. For this reason, the actual machine cannot be accurately represented only by the elastic deformation in the rotational direction of the joint (axial torsion of the rotating shaft), and the position accuracy and vibration suppression performance of the target machine cannot be improved. In particular, in the case of the vertical articulated robot, the displacement of the hand position due to the elastic deformation accompanying the turning (rotation of the first joint) and the change due to the posture of the natural vibration frequency cannot be explained by the conventional model. Also for other joints, the effect of the shaft torsion (difference between the motor rotation angle and the joint angle) handled in the conventional model is small, and the elastic deformation of the movable part and the support part in the rotation axis direction is equivalently reduced. Unless it is approximated by torsion, the actual machine cannot be described accurately. That is,
The present invention has been made in view of the above circumstances, and has as its object to reduce the elastic deformation of a movable portion including a joint and the like, a support portion for supporting the movable portion, and a portion where mechanical strength is insufficient. By using a more accurate model considered,
An object of the present invention is to provide a control method and a device for a machine having a link mechanism capable of improving positional accuracy and vibration suppression performance.

[0004]

[MEANS FOR SOLVING THE PROBLEMS] To achieve the above object
One of the three inventions is the moving part and its supporting part.
The movable direction of the movable part,
A predetermined direction other than the movable direction of the movable part, and the support part
Is modeled taking into account the elastic deformation of the
A control method for controlling the machine by using the
Machine to have a link mechanism, for at least the base
First joint that supports the leg so that it can rotate about the vertical axis
And around the first horizontal axis on the leg with respect to the axis
A second joint rotatably supporting the right-angled first arm,
Around the first horizontal axis on the first arm section,
Joint that rotatably supports the right-angled second arm part
A vertical articulated robot having:
The elastic deformation component in the rotation axis direction of the third joint is
Elastic deformation components in the same direction as the rotation axis near the three joints
Elastic deformation component of the pivot axis of the first joint
In the same direction as the turning axis direction near the first or second joint.
Of a machine with a link mechanism included in the elastic deformation component of
It is configured as a control method. Achieve the above objectives
Another one of the three inventions to achieve
A movable mechanism having a link mechanism comprising
Of the movable part, a predetermined direction other than the movable direction of the movable part, and
Model considering the elastic deformation of the support in the predetermined direction.
And a control method that controls the machine using the model
And the machine with the above link mechanism is at least
The legs are pivotally supported around the vertical axis with respect to the base.
A first joint and a first horizontal axis on the leg.
The first arm that rotatably supports the first arm perpendicular to the center
Around two joints and a first horizontal axis on the first arm.
A second arm portion perpendicular to the axis is rotatably supported.
Is a vertical articulated robot with a third joint
Perpendicular to the rotation axis direction of the second or third joint, and
Elastic deformation in the direction orthogonal to the pivot axis direction of the first joint
A bullet in the same direction as the above direction near the first or second joint.
Of a machine with a link mechanism included in the elastic deformation component
Is configured as a method. Also achieve the above objectives
The remaining one of the three inventions for
A machine having a link mechanism comprising
A model is created in consideration of elastic deformation in the movable direction.
In the control method for controlling the machine using
In the case of Delling, a predetermined direction other than the movable direction of the movable part
Considering the elastic deformation of the support in the predetermined direction,
In addition, the elastic deformation in the predetermined direction
Components in the same direction.
Elastic deformation in the movable direction of the movable part is included
Control method for a machine having a link mechanism, characterized by:
It is configured.

[0005]

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.
The following embodiments and examples are mere examples embodying the present invention, and do not limit the technical scope of the present invention.

Here, FIG. 1 strictly considers elastic deformation .
Construction of rigorous model M _1, FIG. 2 is a schematic structural diagram of a vertical articulated robot, Figure 3 against the articulated robot of FIG. 2
Construction of that rigorous model M _2, FIG. 4 is comparison diagram between the measured value and the theoretical value of the hand position displacement, FIG. 5 is compared view between the measured value and the theoretical value of the deformation amount that by the twisting, Fig. 6 Simplify exact model
Construction of the simplified model M _3, easy to FIG strictly Model
Ryakuka the other simplified structure diagram of a model M _4, Fig. 8 is an explanatory diagram showing the integration of the spring elements, FIG. 9 is a simplified exact model
Further structural diagram of another simplified model M ₅ was, 10 strictly
Structure view of a further simplified model obtained by simplifying the model M _6, 11 further structural diagram of another simplified <br/> model M ₇ a simplified exact model, Figure 12 is a simplified strict model did
FIG. 13 is a structural diagram of still another simplified model M _{8 according to} the present invention.
Control method for a machine having a link mechanism according to an embodiment of the present invention
Construction of simplified model M ₉ according to the FIG. 14 the present invention
Control method for a machine having a link mechanism according to an embodiment of the present invention
Structure view of another simplified model M ₁₀ according to the FIG. 15 view comparing between the actual measurement value and the theoretical value of the deformation amount, 16 other comparison diagram between the actual measurement value and the theoretical value of the deformation amount, FIG. 17 yet another comparison diagram between the actual measurement value and the theoretical value of the amount of deformation, Figure 18 is the braking block diagram showing a control system, Figure 19 is another block diagram of the control system, Figure 20 the vibration response of the hand position amplitude Comparative Figure 21 shows comparison views of the theoretical value of the frequency and the amount of deformation of, FIG. 22 shows a control system
Still another block diagram, yet another 23 illustrating a control system
It is a block diagram of.

[0007] only performs moving parts 1 and model the machine with a link mechanism consisting of the support unit 2 which in view of the elastic deformation of the movable direction of the movable portion 1, using the model
Sufficient positional accuracy and vibration suppression performance even when the above machines are controlled
It was confirmed that it could not be secured. Therefore, as shown in FIG. 1, per above SL model, and the predetermined direction other than the moving direction of the movable portion 1, it is necessary to consider also the elastic deformation in a predetermined direction of the support 2. Has the above link mechanism
As a specific example of the machine, a first joint for supporting at least a leg pivotally about a vertical axis with respect to a base, and a first joint perpendicular to the axis about a first horizontal axis on the leg. 1
A second joint rotatably supporting the arm and a third joint rotatably supporting a second arm perpendicular to the first horizontal axis about the first horizontal axis on the first arm; The equipped vertical joint robot is used. For example, in the case of a three-axis vertical articulated robot , as shown in FIG. 24, in the prior art, spring constants k ₁ θ to k ₃ θ exist only in the rotating joint, and the spring element is also in the direction of the rotating axis of the rotating joint. Elastic deformation ε ₁ θ of
~ε ₃ θ allows only, was based on a model that does not allow other direction bending deformation and stretching direction of the elastic deformation at all. In contrast, as shown above Symbol Figure 1, in addition to the elastic deformation of the movable portion such as those rotating joint, also considering supporting elastic deformation of the supporting them movable portion, and optionally direction elastic deformation thereof adopted a model that allows the elastic deformation (exact model), and more accurate modeling. However, the model M ₁ in FIG. 1, k _{iR, iS} represents the side of the spring for supporting the side and it is driven by the i-th joint, X fixed to the link, Y, the elastic deformation of the Z-direction ε _{iRX, Y, Z, iSX, Y, Z} , spring constant k _{iRX, Y, Z, iSX, Y, Z} , bending moment M
_{iRX, Y, Z, iSX, Y, Z.} Further, since the load inertia of the wrist (the fourth to sixth axes in the case of a normal vertical articulated robot) is usually very small, the effect of their elastic deformation 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 axis is immediately behind the third axis, and the entire arm is rotated to rotate the wrist. Even with such a structure, the fourth
Since the inertia in the rotation direction of this axis does not become so large, the elastic deformation of the fourth axis 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, inertia other than the joint angle direction becomes extremely large, and the amount of elastic deformation in those directions cannot be ignored. In this case, by describing the model M ₂ to handle the amount of elastic deformation of the portion of the movable portion and supporting the fourth axis as shown in FIG. 3, to model better actual it can.

The results of modeling based on these models are shown in FIGS. Figure 4 shows the exact model (M ₂ ) for the displacement of the hand position caused by elastic deformation.
And a theoretical value and a measured value by the conventional model (M ₀ ). In the conventional model, only the elastic deformation in the joint rotation direction is handled, and therefore it is impossible to describe the elastic deformation. Therefore, the theoretical value and the measured value greatly vary. On the other hand, in the strict model, other elastic deformations can be described, and it can be seen that the theoretical values and the measured values are in good agreement. Therefore, when the actual control is performed, the control performance can be improved by employing the elastic deformation amount or the like calculated by the strict model. On the other hand, in the conventional model, since there is a large difference between the measured value and the theoretical value, improvement in control performance cannot be expected. FIG.
Shows the relationship between the theoretical value of the strict model and the conventional model and the actual amount of torsion in the hand position displacement due to elastic deformation in the joint rotation direction (torsion in the direction of the rotation axis). Is shown. Similar to FIG. 4 above, the exact model and the measured values are in good agreement, but in the conventional model, all the elastic deformations are to be expressed by this torsion. . Furthermore, FIGS. 4 and 5 are based on exactly the same measurement data, but the hand position displacement due to elastic deformation shown in FIG. The displacement is only about 0.05 mm. This shows that the displacement of the hand position due to the shaft torsion is less than 10% of the displacement of the hand position due to all the elastic deformations. That is, in the conventional model, all the elastic deformations are to be described using only the displacement having an effect of less than 10%, which indicates that it is impossible. Such a phenomenon is
In particular, it appears remarkably in the joint (the third or fourth joint in FIG. 5) adjacent to the pivot axis or a place with low rigidity.

In the above strict model , the amount of elastic deformation to be considered is large, and the amount of calculation is enormous. For this reason,
The following describes a simplified against strictly model.
As a method of simplifying the model, many reductions in the frequency domain have been proposed. However, in a link mechanism system such as a robot, the transfer function changes depending on the posture, and in the conventional simplification method by reducing the dimension, it is necessary to have a transfer function model for each posture, and the amount of memory is enormous. Therefore, in this case, by simplifying the model itself, it was decided to reduce the dimensions in consideration of the change in posture. After describing some simplification techniques, the present invention is described.
Will be explained. One simplification approach, when the upper liver Del reduction, among the elastic deformation, is to ignore the elastic deformation of the movable portion or the support portion exceeds the predetermined rigidity value. For example, if the rigidity in the Z direction of the support portion of the first axis (turn axis) of the robot is high, the model can be simplified by omitting elastic deformation in that direction. Thereby, reduction in control calculation can be achieved. Another one of the simplification methods is to express a plurality of integral elastic deformations by one spring element among the elastic deformations in the modeling. For example, in the models M ₁ and M ₂ shown in FIG. 1 or FIG. 3, the elastically deformable spring elements k ₁ θ, k _{1 R} or k ₂ θ, k _{2 R,} k ₃ θ, k _{3 R} or k ₄
For θ and k _4R, it can be considered that there is no mass between them (actually, there is a very small mass, but since their influence is very small, the figure assumes that these masses can be ignored. ). Therefore, the position of the center of elastic deformation is only slightly shifted, and a plurality of elastic deformations can be represented by a single spring element. The model M ₃ of the case shown in FIG. Further, the inertia by the output of the third axis and the fourth axis is very small, the model of Figure 6 Neglecting it can further simplified as model M ₄ in FIG.
Further, the center of the elastic deformation of the one spring element is provided in each region including the centers of the plurality of elastic deformations represented by the spring element, or the center of the elastic deformation of the one spring element is It can also be determined by a weighting process for each center of the plurality of elastic deformations represented by the spring elements.

For example, when a plurality of spring elements are represented by one spring element and the model M ₂ in FIG. 3 is simplified to the model M ₄ in FIG. 7, the spring elements k ₃ θ, k _3R , k 3 in FIG. _{Although 4S} is represented by a spring element k ′ _3R in FIG. 7, the spring elements k ₃ θ, k
_3R, the position p ₃ theta elastic deformation center of k _4S, p _3R, if p _4S is shown as in FIG. 8, the _3R 'position p of the elastic deformation center of the _3R' spring element k p ₃ theta, Take in the area enclosed by p _3R and p _4S . This makes it possible to simplify the model without losing the accuracy of the model. Alternatively, the position p ′ _3R of the center of the elastic deformation is obtained more mathematically. That is,
Spring element k ₃ θ, k _3R, X fixed to the link k _4S, Y, and Z-direction spring coefficient _{K 3 θ X, Y, Z} , k 3RX, Y, Z,
k _{4SX, Y, Z} and their elastic deformation center position p ₃ θ,
X, Y, and Z components p ₃ θ fixed to the links of p _3R and p _{4S
X, Y, Z, p 3RX} , Y, Z, p 4SX, Y, _{if Z} is given, the elastic deformation center position _{p '= 3R [p' 3RX} , p '3RY, p'
_3RZ ] ^T is _calculated by the following equation.

(Equation 1) The elastic deformation center position p ′ _3R obtained by the above equation also 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 amount of control calculation can be reduced accordingly. Furthermore, in the above modeling, there are a plurality of spring elements having a plurality of elastic deformations, and when the values are close to each other, a plurality of spring elements are assumed to have the same elastic deformation spring coefficient in the predetermined direction. Elements may be further aggregated. For example, in the model M ₄ of FIG. 7, the movable portion and the spring constant k _1SX and k _1SX for elastic deformation of the non-movable direction of the supporting portion, 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'
Just assuming that the _3RZs are the same, the number of parameters (such as spring coefficients) required for the model is reduced by six. Furthermore, if the minute influence of the Coriolis force due to the coordinate change is ignored, the spring elements k _1S and k ′ _1R ,
k _2S and k _'2R, k _3S and k' _3R can are expressed as spring elements one each, can be simplified as model M ₆ model M ₅ or 10 of FIG. Similarly, for the fourth joint, the spring elements k _4RX and k _4RX , k _4RY and k _4RX
Assuming _4RY are identical respectively, it can be further simplified as model M ₇ in FIG. 9 to 11
The same simplification can be performed by combining the above models. For example, as model M ₈ in FIG. This can further simplify the model and reduce the amount of control calculation.

Another one of the [0012] simplification approach, hit on liver Dell reduction is separated into components according to the elastic deformation in the deformation direction, of the same deformation direction component, than a predetermined stiffness value component Also includes higher components in lower components. In particular, the three inventions
Two of the first and second joints are configured such that the machine having the link mechanism supports a leg at least with respect to the base so as to be pivotable about a vertical axis, and a first joint about the first horizontal axis on the leg. A second joint rotatably supporting a first arm perpendicular to the axis, and a second arm perpendicular to the axis about a first horizontal axis on the first arm; A vertical joint robot having a third joint rotatably supported.
The case applies. One of the two inventions relates to an elastic deformation component in the rotation axis direction of the second or third joint in the same direction as the rotation axis direction near the second or third joint. And the elastic deformation component of the pivot axis of the first joint is included in the elastic deformation component in the same direction as the direction of the pivot axis near the first or second joint . Ma
The other of the two inventions uses the first or the second joint to generate a component of elastic deformation in a direction orthogonal to the rotation axis direction of the second joint and the rotation axis direction of the first joint. Or included in the elastic deformation component in the same direction as the above direction near the second joint.
Things. For example, taking the model M ₈ of articulated robot of three axes shown in FIG. 12 as an example, X, Y, Z-axis direction of the elastic deformation _{ε "1SX, Y, Z,} ε" 2SX, Y, _Z , ε ″
_Think separately for _{3RX, 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, the elastic deformation ε ″ _1Sz in the Z direction was measured with ε ″ _2Sz and ε ″ _3Rz , and as a result of actual measurement, it was _{found that} the rigidity of the elastic deformation ε ″ _2Sz was smaller than that of the other elastic deformations. The model is simplified by expressing ε ″ _{2Sz in the} X-axis direction. Similarly, in the X-axis direction, as a result of actual measurement, it was _{found that} the rigidity of the elastic deformation ε ″ _2SX was smaller than that of the others. simplified model by representing the high elastic deformation epsilon _"2SX. This model M ₁₀ obtained is as shown in FIG. 14. Further, 3 in the axes of the articulated robot, generally in the X-direction deformation Cannot be expressed by joint angles,
In the model M ₁₀ of FIG. 14, omitted deformation in the X-axis direction, represented as model M ₉ in Figure 13. Naturally, FIG.
Is the model with higher accuracy. In particular, when the rigidity of the support portion of the second shaft is low as in this case, the model of FIG. 14 needs to be adopted. However, when the rigidity of the support portion is sufficiently high, the simplified model of FIG. But it is good. FIG. 15 shows the theoretical values of the amounts of deformation for the models subjected to these simplifications (indicated by Δ in the figure). Compared with the case without simplification (marked with 〇 in the figure), when these simplifications are performed, there is a slight variation, but the difference between the measured value of the deformation amount is better than that of the conventional model (marked with ■ in the figure). I do.

[0013] Two of the three inventions as described above,
The above rigorous model is simplified. However, it can also be based on conventional models. The rest of the three inventions
One of those focused on this point. Three inventions
One remaining Chino strikes on liver Dell reduction, not only the movable direction of the movable portion, considering the elastic deformation of the a predetermined direction other than the moving direction of the movable portion, the predetermined direction of the support portion At the same time, the elastic deformation in the predetermined direction is separated into components corresponding to the movable direction of the movable portion, and the elastic deformation in the predetermined direction for the component in the same direction is included in the elastic deformation in the movable direction of the movable portion. is there. Immediately Chi, in the conventional model shown in FIG. 2 4, although the conventional torsional model base, where are those is a forced you try approximated by also torsional other elastic deformation. Here, including the case where only the actual shaft torsion is represented by the shaft torsion model, the theoretical value and the measured value by the present model and the conventional model are shown in FIG. Compared. As can be seen from the figure, the measured value and the theoretical value agree with each other very well when the present model is used (the symbol in the figure). On the other hand, when the shaft torsion model represents only the shaft torsion without changing the conventional method (x in the figure)
Mark), no displacement due to the elastic deformation of the hand position can be expressed at all. However, based on the conventional model, when approximation is performed by forcible axis torsion including other elastic deformations (marked in the figure, this is the same as the conventional model in FIG. 4), some of the results are smaller than the previous model. The displacement of the position can be approximated. However, as described above, the theoretical value and the actually measured value may be quite different depending on the posture or the like, and the variation is large. Therefore, in order to reduce the variation due to the posture based on the conventional model which is simpler than the present model, it is necessary to change the inertia or the spring coefficient according to the posture. A comparison between the present model and the conventional model shows that the elastic deformation amount is small in the contracted posture and large in the extended posture in the conventional model. Therefore, the spring coefficient may be reduced or the inertia may be reduced in the contracted position, or the spring coefficient may be increased or the inertia may be changed in the extended position. Further, in order to change the spring coefficient or the inertia, a table of the spring coefficient or the inertia according to the posture may be provided, and the values of the table may be derived so that the elastic deformation amount matches the present model. FIG.
FIG. 7 shows theoretical values of the hand position deformation amount when the inertia and the spring constant are changed depending on the posture. In the figure, the deformation amounts are calculated for the three postures, but the theoretical values can be made to match the actually measured values by forcibly changing the inertia and the spring constant.

As described above, based on the modeling results of the present model (for example, M ₉ and M ₁₀ ), a simple conventional model (M ₀ ) that considers only the rotation axis torsion is modified, and the modeling results are approximated. it is possible to obtain substantially equivalent approximate model more detailed the model (M ₉ and M _10), it is possible to less modeled amount of calculation with high accuracy. Further, in the models M _{1 to} M ₁₀ , equations of motion and state equations are derived, and these equations are strictly derived when performing control. However, this requires extremely complicated calculations. Therefore, it is assumed that the amount of elastic deformation is very small and that the equations are simplified. In particular,
By minimizing the amount of elastic deformation, it is possible to greatly reduce the amount of calculation simply by replacing sin and cos of the amount of elastic deformation that often appear in the equations with 0 or 1. In addition, it is possible to omit the interference terms between the respective elastic deformation amounts and to omit them. Furthermore, in the derived equations, the off-diagonal term of the inertia matrix or the off-diagonal term of the inverse matrix represents the influence of the interference of each state variable, which generally has an effect on the model characteristics. Since it is smaller than the diagonal term, the model can be simplified by omitting the off-diagonal term.

Subsequently, a case where a machine is controlled using the above model will be described. That is, based on liver Dell, it calculates the amount of elastic deformation, modifying each joint or the tip unit position command value so as to correct the amount of elastic deformation. that way,
Even though the robot or the like is elastically deformed, the command value is corrected in anticipation of the amount of elastic deformation, so that high positional accuracy can be realized. Specifically, FIG.
8 or FIG. That is, FIG. 18 is corrected command value to estimate the amount of elastic deformation on the basis of the model (elastic deformation model) from the command value, it corrects the estimated deformation amount. Here, the amount of elastic deformation that will occur when the robot operates according to the command value is corrected. Further, FIG. 19 is to correct the command value so as to estimate the amount of elastic deformation based on the elastic deformation model from the output value of the robot (actual movement of the robot), to correct the estimated deformation amount, the current The amount of elastic deformation that may occur when the robot operates is corrected. In general, as an input of an elastic deformation model, it is generally the simplest model description to give a joint angle or a motor rotation angle. If the target value in FIG. 18 or the robot output value in FIG. 19 is the position of the hand, an inverse transformation is performed in the preprocessing in the figure, and the hand position is converted into a joint angle or a motor rotation angle. Must be entered. If the target value in FIG. 18 or the output value of the robot in FIG. 19 is a joint angle or a 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 for converting a given input to an 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 amount of elastic deformation to the desired hand position or posture is calculated as
Elastic deformation vector E having the calculated elastic deformation amount as an element
Subjected to, calculates the displacement amount X _E of the position or attitude of the end. By adding this to the command value D, the effect of elastic deformation can be corrected. This can be formulated as follows.

[0016]

(Equation 2) Further, if the command value is joint angle, the displacement vector X _E to displacement of the elements of the position or attitude of the end determined earlier, the inverse of the Jacobian matrix J _X theta from the joint angle to the position or attitude of the end A required angle θ _C for correcting the displacement is calculated by applying a matrix. By adding this to the command value D, the effect of elastic deformation can be corrected.

[0017]

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

[0018]

(Equation 4) It is. Furthermore, taking advantage of the redundancy of these pseudoinverses,
Pseudo inverse matrix with weight

[0019]

(Equation 5) By using the above, the position, posture, or joint angle to be emphasized can be corrected with emphasis. However,
W is a weight matrix. When corrected as described above,
FIG. 20 shows the case where no correction is made. Here, FIG.
(A) and (b) show the case where the hand position is sin-moved at 5-1 Hz and 1.5 mm amplitude.
In the case of no correction (a), the amplitude increases as the frequency increases due to the influence of elastic deformation. On the other hand, in the case of the correction (b), the elastic deformation is compensated, and
The amplitude is kept at 1.5 mm as intended. In addition, when the amount of elastic deformation is to be corrected based on a conventional model, not based on the above model, the result is as shown in FIG. That is, since the elastic deformation amount is not correctly calculated in the conventional model, the elastic deformation is correctly corrected or adversely affected depending on the posture.

[0020] Thus, to calculate the amount of elastic deformation based on the exact model than slave come model, by correcting the command value for the robot or the like, it is possible to greatly improve the control accuracy. Further, by configuring an observer or performing robust control, the effect of the above model can be maximized . That is, the model <br/> Dell as a control object, the elastic deformation amount of the control target is the you estimated using the differential values or a disturbance observer of the elastic deformation. In here, large targets constitute observer considering the elastic deformation is to suppress the vibration caused by the elastic deformation. In this sense, the model required when configuring the observer needs to describe at least the primary mode. FIG. 21 shows a gain diagram from the torque of the turning axis of the robot to the elastic deformation. The vibration mode that can be described by the conventional model is the vibration mode in the direction of the turning axis, which is indicated by a symbol in the figure. Have been. When the robot is in an extended state, the vibration mode in the direction of the turning axis appears as a natural vibration primary mode, but in a contracted posture, it becomes a secondary mode. Conversely, in the contracted posture, the vibration mode in the X-axis direction in the model of FIG.
Mark) becomes low, and this is the primary mode. That is, in the robot, the primary mode vibration changes depending on the posture. Therefore, the primary mode cannot be described in the conventional model that only considers the vibration in the joint rotation direction. That is , this means that the observer based on the conventional model cannot describe the primary mode and cannot suppress the vibration. On the other hand, when the observer is configured based on the above 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 primary mode but also the secondary mode can be accurately represented, the situation of vibration can be more accurately estimated by the observer, and the vibration can be suppressed. Also, in the observer based on the conventional model, elastic deformation in three directions was considered, but in the strict model as shown in FIG. 1, a considerable amount of elastic deformation had to be considered, and the order of the observer was growing. However, by using a simplified model obtained by simplifying the strict model, the elastic deformation to be considered can be reduced to a minimum of four, and the order of the observer does not increase so much.

The observer is constructed by 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 configured. However, extending the observer in the prior art to the above-described model only requires replacing the model, and the details thereof are easily understood, so that the description is omitted. Thus, than conventional models based on exact upper liver Dell, by configuring the observer for estimating the elastic deformation or the like, it is possible to obtain a more accurate estimate, and feeds back the estimated values This can significantly improve the vibration suppression performance. Furthermore, it is important to match the natural vibration between the actual machine and the model when suppressing vibration by the observer. Therefore, by determining such first mode or the second mode of at least the natural vibration parameters such as coefficients that are added to the above liver Dell spring constant and the inertia and they match, it exhibits a higher vibration control performance it can.

Further, robust control in consideration of modeling errors may be performed with the above model as a control object .
No. For example, in robust control such as H∞ control and μ analysis, the control performance is affected by the magnitude of the modeling error. Therefore, controlling the modeling error as small as possible improves the control performance. In that sense, above
In this model, the effect of robust control can be expected. As shown in FIG. 21 described above, the first-order mode cannot be described in the conventional model, and the modeling error is large.
In addition, the frequency region where the modeling error is large becomes low (that is, becomes large near the first-order mode frequency). For this reason, when robust control is directly applied to the conventional model, the control performance is reduced, and the control performance particularly in a low frequency region is significantly deteriorated. On the other hand, when robust control was performed based on the above model, the modeling error was small because the model and the actual machine matched well.
Therefore, the control performance is improved. Furthermore, in the above-described 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 the high-frequency region. Therefore, high control performance can be maintained up to a high frequency range.

[0023] To design a robust controller based on SL model, first, in the conventional design method, the conventional replacing the model to the model, to calculate or measure the modeling error. Then, the frequency weight transfer function W (s) is determined so as to suppress the modeling error, and at least the transfer function W (s) and the transfer function P (s) of the present model are determined by a control system design tool (for example, MATLAB or MATLAB). MATRI
XX), the controller can be designed. still,
The control system design support tool is the most commonly used support tool among controller designers at present. Thus, by suppressing the modeling error to a small extent, the effect of the robust control can be maximized and the control performance can be greatly improved. By calculating or adding the required torque based on the above model, it is possible to instruct only the really required torque to each motor, thereby enabling highly accurate control. When the conventional model is used, the value of the required torque near the natural vibration greatly deviates from the calculated value, and the natural vibration is excited. Specific examples of the configuration at this time are shown in FIGS. Therefore, the above control method is particularly effective when high-speed and dynamic movement is required. As shown in FIG. 20, when the high-speed operation of the robot causes elastic deformation and the vibration mode is excited, Very effective.

As described above, in the three embodiments of the invention,
According to the control method of a machine having such a link mechanism, an accurate model (strictly speaking) is taken into consideration by considering elastic deformation of a movable part including a joint and the like, a support part supporting the same, and a part having mechanically insufficient strength. By using a model) or a simplified model thereof, the position accuracy and vibration suppression performance can be greatly improved. The three inventions
The simplification based on the conventional model is also effective for a robot other than the vertical articulated robot or a machine having another link mechanism .

[0025]

Control how the machine having a link mechanism according to the present invention, because they are configured as described above, insufficient supporting portion and mechanical strength for supporting the movable portion and it includes a joint or the like By using a more accurate model that takes into account the elastic deformation of the part where the vibration occurs, the position accuracy and the vibration suppression performance can be improved.

[Brief description of the drawings]

FIG. _{1 is} a structural diagram of a strict model M1 in which elastic deformation is strictly considered .

Figure 2 is a schematic structural diagram of a vertical articulated robot.

FIG. 3 is a structural diagram of an exact model M2 for the vertical articulated robot of FIG. ₂ ;

FIG. 4 is a graph comparing a measured value and a theoretical value of a hand position displacement amount.

[5] Comparison diagram between the measured value and the theoretical value of I that deformation of the torsion.

[6] Construction of a rigorous model simplified by simplifying the model M _3.

FIG. 7 shows another simplified model M obtained by simplifying an exact model .
_{4 is} a structural view.

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

[9] structure view of a further simplification strict model simplified model <br/> del M _5.

[10] Further structural diagram of another simplified <br/> model M ₆ a rigorous model simplified.

[11] Further structural diagram of another simplified <br/> model M ₇ to the exact model was simplified.

[12] Further structural diagram of another simplified <br/> model M ₈ the exact model was simplified.

FIG. 13 has a link mechanism according to the embodiment of the present invention .
Construction of simplified model M ₉ to which the control method of the machine to be.

FIG. 14 has a link mechanism according to an embodiment of the present invention .
Structure view of another simplified model M ₁₀ to which the control method of the machine to be.

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

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

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

FIG. 18 is a system block diagram showing the control system.

[19] system other block diagram showing the control system.

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

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

[Figure 22] system is yet another block diagram showing the control system.

[23] system is yet another block diagram showing the control system.

Construction of a model M ₀ indicating a case of applying the control method of the machine [24] with the conventional link mechanism.

[Explanation of symbols]

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

──────────────────────────────────────────────────続 き Continuation of the front page (56) References JP-A-1-191209 (JP, A) JP-A-7-244513 (JP, A) JP-A-59-103106 (JP, A) JP-A-6-106 270079 (JP, A) (58) Fields investigated (Int. Cl. ⁷ , DB name) B25J 9/10 B25J 9/16 G05B 17/00

Claims (3)

(57) [Claims] 1. A link machine comprising a movable part and a supporting part thereof.
Moving the machine having the frame in the moving direction of the movable part,
In a predetermined direction other than the moving direction, and
A model is created in consideration of elastic deformation.
A control method for controlling a machine, wherein the machine having the above link mechanism is provided at least with a base.
The first function of supporting the leg portion so as to be pivotable about the vertical axis
Knots and around the first horizontal axis on the legs
Joint for rotatably supporting a right-angled first arm portion
And an axis about a first horizontal axis on the first arm portion.
A third arm rotatably supports a second arm portion perpendicular to the third arm portion.
A vertical articulated robot having a joint and an elastic deformation component of the second or third joint in the rotation axis direction.
A bullet in the same direction as the rotation axis direction near the second or third joint.
Of the pivot axis of the first joint
The sexual deformation component in the direction of the pivot axis near the first or second joint.
Link mechanism included in the elastic deformation component in the same direction as
How to control a machine. 2. A link machine comprising a movable part and a supporting part thereof.
Moving the machine having the frame in the moving direction of the movable part,
In a predetermined direction other than the moving direction, and
A model is created in consideration of elastic deformation.
A control method for controlling a machine, wherein the machine having the above link mechanism is provided at least with a base.
The first function of supporting the leg portion so as to be pivotable about the vertical axis
Knots and around the first horizontal axis on the legs
Joint for rotatably supporting a right-angled first arm portion
And an axis about a first horizontal axis on the first arm portion.
A third arm rotatably supports a second arm portion perpendicular to the third arm portion.
And a vertical articulated robot having a joint, which is orthogonal to the rotation axis direction of the second or third joint, and
The elastic deformation of the first joint in the direction orthogonal to the turning axis direction
Component in the same direction as the above direction near the first or second joint.
Control of machine with link mechanism included in elastic deformation component
Your way. 3. A control method for modeling a machine having a link mechanism including a movable part and a support part thereof in consideration of elastic deformation of the movable part in a movable direction, and controlling the machine using the model. In the modeling, the elastic deformation in the predetermined direction other than the movable direction of the movable portion and the predetermined direction of the support portion is also taken into consideration, and the elastic deformation in the predetermined direction is determined according to the movable direction of the movable portion. A method of controlling a machine having a link mechanism, wherein the elastic deformation in the predetermined direction for the component in the same direction is included in the elastic deformation in the movable direction of the movable part for the component in the same direction.