JP5316395B2 - Robot spring constant identification method and robot spring constant identification apparatus - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To identify the spring constant around the zxy axes with high accuracy for each rotary joint shaft of an articulated robot arm. <P>SOLUTION: At least the position out of the position and the attitude of a point of measurement determined on a fore end of a robot arm or close to the fore end thereof is measured by a measuring means, and calculated by using the predicted value of the spring constant around each axis of zxy of each rotary hinge shaft. The difference between the measured value and the calculated value is defined as the difference between the true value of the spring constant around each axis of zxy of each rotary hinge shaft and the artificially determined value as the predicted value, and the predicted value of the spring constant is corrected by the mathematical method so that the difference between the two is equal to or less than the predetermined threshold. The finally obtained predicted value of each spring constant (the identified spring constant) becomes the value close to the true spring constant with certain level of correctness. <P>COPYRIGHT: (C)2011,JPO&amp;INPIT

Description

  The present invention relates to a method and apparatus for identifying a spring constant of a rotary joint axis of a robot arm.

For example, in a vertical articulated robot, the robot arm is configured by connecting a plurality of arms by a plurality of rotary joint axes, and each arm is turned or twisted in the horizontal direction or the vertical direction by the rotation of the rotary joint axes. Rotate and do what you want.
Each rotary joint axis of the robot arm is rotationally driven by a servomotor via a gear reduction device, but receives a rotational torque to cause bending in the torsional direction, or receives a mass of the arm or workpiece to cause bending in the bending direction. Or For this reason, if the spring constant of each rotary joint axis is accurately grasped and the degree to which each rotary joint axis is twisted or bent cannot be accurately calculated, the tip of the robot arm is correctly set to the taught position / posture. It becomes difficult to control.

One of the conventional methods used to determine the spring constant of the rotary joint axis is to attach two weights with different masses alternately to the tip of the robot arm, and each weight is connected to each rotary joint axis. The amount of vertical deflection of the arm is measured by an external measuring device such as a laser displacement meter, and the spring constant of each rotary joint shaft is determined from the difference in mass between the two weights and the difference in deflection between the arms. There is something called.
Although Patent Document 1 is not a robot, it models a structure as a spring / mass system and measures the excitation force when one end of the spring / mass system is excited and the response to the excitation force at an arbitrary point. A method of identifying the spring constant and the mass of the mass point from this excitation force and response is disclosed.

JP-A-5-209805

  In addition to the both-end support type in which both ends are supported by bearings, there are many cantilever types in which only one end side is supported by a bearing. When the same axis as the rotation center axis of the rotary joint axis is the z-axis and the two axes orthogonal to the z-axis and the two axes orthogonal to each other are the x-axis and the y-axis, the rotary joint axis is a double-end supported rotary joint axis. However, even a cantilever rotary joint axis does not bend in the direction of rotation (twisting direction) about the z axis, but of course in the direction of rotation (bending direction) about each axis of xy. Therefore, it is necessary to consider a spring constant (hereinafter, simply referred to as a spring constant around the z-axis, x-axis, and y-axis) for bending in the rotational direction about each axis of zxy.

However, the above conventional method, that is, when the two weights are attached to the tip of the robot arm, the vertical deflection amount of each arm is measured, and the mass difference between the two weights and the vertical deflection difference between the arms. In the method of identifying the spring constant from the above, it is not known how much the spring constant around the z-axis, the spring constant around the x-axis, and the spring constant around the y-axis are involved in the measured deflection amount. For this reason, the degree of involvement of the spring constant around each axis of zxy can only be estimated by experience and intuition, and as a result, the spring constant around each axis of zxy is accurately set to a value close to the true spring constant. It becomes difficult to identify well.
Further, the fact that the spring constant around each axis zxy of each rotary joint axis cannot be identified with high accuracy is that even if each rotary joint axis is correctly rotated by the servo motor, the deflection of the rotary joint axis that cannot be predicted in advance. It means that the taught position / posture cannot be controlled with high accuracy.

  The present invention has been made in view of the above circumstances, and its object is to provide not only a spring constant around the z axis but also a spring constant around the x axis and a circumference around the y axis for each rotary joint axis of the articulated robot arm. It is an object of the present invention to provide a robot spring constant identification method and a robot spring constant identification apparatus that can identify the spring constant of the robot with high accuracy.

  In the present invention, at least the position and the position of the measurement point determined at or near the tip of the robot arm are measured by the measuring means, and the spring constant around each axis of zxy of each rotary joint axis is predicted. The difference between the measured value and the calculated value is due to the difference between the true value of the spring constant of each rotary joint axis around each axis and the value artificially determined as the predicted value. As the predicted value of the spring constant is corrected by a mathematical method so that the difference between the two becomes a predetermined threshold value or less, not only the spring constant around the rotation center axis (z axis) of each rotary joint axis, Spring constants about two axes (xy axes) orthogonal to the center of rotation can also be obtained, and each finally obtained spring constant is a value close to the true spring constant with a certain degree of accuracy.

The 1st Embodiment of this invention is a figure which shows the system configuration for identifying the spring constant of a rotating joint axis in model. Perspective view of vertical articulated robot Perspective view showing measurement points near the tip of the robot arm flowchart The rotary joint axis of the arm that performs torsional rotation is shown. (A) is a cross-sectional view schematically showing the support structure of the rotary joint axis and the torque transmission structure of the servo motor, and (b) is a three-direction view of the rotary joint axis. Perspective view showing the central axis of deflection The rotary joint axis of the arm which performs turning is shown, (a) is a sectional view schematically showing the support structure of the rotary joint axis and the torque transmission structure of the servo motor, and (b) is a bending of the rotary joint axis in three directions. The perspective view which shows the central axis of FIG. 4A shows the bending of the rotary joint axis, (a) is a cross-sectional view showing the bending in the rotational direction around the x-axis, (b) is a cross-sectional view showing the bending in the rotational direction around the y-axis, (c) ) Is a perspective view showing bending (twisting) in the rotational direction around the z-axis.

Hereinafter, the present invention will be described with reference to a plurality of embodiments. Each embodiment is for a 6-axis vertical articulated robot, for example.
[First Embodiment]
The drawings show a first embodiment of the present invention. FIG. 2 shows a six-axis vertical articulated robot 1. The robot arm 2 of the robot 1 includes a base 3, a shoulder portion 4 supported by the base 3 so as to be pivotable in a horizontal direction around the first axis Lc- 1, and a second axis Lc− on the shoulder portion 4. A lower arm 5 supported so as to be pivotable in the vertical direction about the second axis, a first upper arm 6 supported on the lower arm 5 so as to be pivotable in the vertical direction about the third axis Lc-3, A second upper arm 7 is supported at the tip of the first upper arm 6 so as to be rotatable about the fourth axis Lc-4, and a fifth axis Lc-5 is centered on the second upper arm 7. The wrist 8 is supported so as to be pivotable in the vertical direction, and the flange 9 is supported on the wrist 8 so as to be able to rotate by twisting about the sixth axis Lc-6.

  The base 3, the shoulder portion 4, the lower arm 5, the first upper arm 6, the second upper arm 7, the wrist 8, and the flange 9 function as the arms in the robot arm 2. As shown in FIG. 5 and FIG. 6, the rotary joint shaft 11 is fixedly connected to the front arm via a bearing 10 so as to be rotatable. Each rotary joint axis 11 corresponds to the first axis Lc-1 to the sixth axis Lc-6.

  Each rotary joint shaft 11 is rotationally driven by a servo motor 12 as a drive source via a speed reducer, for example, a gear speed reducer 13, and each arm 4 to 9 turns with the rotation of the rotary joint shaft 11. An operation or a twisting operation is performed. 5 and 6, the same reference numeral 11 is used to indicate the rotary joint axis. However, the rotary joint axes 11 of the arms 4 to 9 have different diameters and lengths, and the servo motor 12 is used. Also, the capacity corresponding to the arm to be driven is provided on each rotary joint shaft 11 in a one-to-one relationship.

The robot 1 includes a control unit (hereinafter referred to as a robot control unit) 14 that controls the robot arm 2 by controlling the servo motor 12 according to an operation program. As shown in FIG. 1, a joint angle detector (axis angle detecting means) 15 for detecting the rotation angle of the rotary joint shaft 11 and a storage unit 16 are connected to the robot control unit 14. The joint angle detector 15 uses a rotary encoder (not shown) as rotation detecting means connected to a rotating shaft (not shown) of the servo motor 12 as a sensor, and rotates based on rotation angle detection information of the rotary encoder. The rotation angle of the joint shaft 11 from the reference position with a rotation angle of 0 degrees is detected. That is, the joint angle detector 15 does not detect the actual rotation angle of the rotary joint shaft 11, and when the rotation shaft of the servo motor 12 is rotated by a rotation command from the robot control unit 14, the rotation of the servo motor 12 is rotated. The rotation angle of the shaft is detected, and the detected angle is divided by the reduction ratio of the gear reduction device 13 to be converted into the rotation angle of the rotary joint shaft 11 and output to the robot control unit 14.
Various kinds of software for robot control are stored in the storage unit 16 in advance, and a teaching operation program is stored in actual robot work. Furthermore, the storage unit 16 stores various parameters described later.

  Three-dimensional coordinates are defined for the base 3 and the arms 4 to 9. Among these, the coordinate system of the base 3 installed on the floor is a robot coordinate (reference coordinate) as a stationary coordinate system. The coordinate system of each arm 4-9 is defined at the base end of each arm 4-9 (predetermined position on the rotation center axis of the rotary joint axis 11), and on the robot coordinates by the rotation of each arm 4-9. The posture (orientation) or position and posture of the camera changes. In FIG. 1, among the coordinates of the base 3 and the arms 4 to 9, the robot coordinates are indicated by the coordinate axes of Zr, Xr, and Yr, and the coordinates of the flange 9 (hereinafter referred to as flange coordinates) are Zf, Xf, It is indicated by the coordinate axis of Yf.

  The storage unit 16 stores the coordinate position of the shoulder unit 4 on the robot coordinates, the coordinate position of the lower arm 5 on the coordinates of the shoulder unit 4, the coordinate position of the first upper arm 6 on the coordinates of the lower arm 5, The coordinate position of the second upper arm 7 on the coordinates of the upper arm 6, the coordinate position of the wrist 8 on the coordinates of the second upper arm 7, the coordinate position of the flange 9 on the coordinates of the wrist 8, and each arm 6 A numerical value representing the length of ˜9 is stored as a parameter.

  Then, the robot control unit 14 calculates the coordinate positions and postures of the arms 4 to 9 from the mutual positional relationship between the rotation positions of the arms 6 to 9 and the coordinates of the arms 4 to 9 by a coordinate conversion calculation function. It can be recognized by converting the position and posture on the robot coordinates. The posture of each arm 4-9 is the position on the robot coordinate where the tip of the unit vector on the coordinate axis of each arm 4-9 is located when the coordinate of each arm 4-9 is translated to the robot coordinate. Represented by

  FIG. 1 shows a model of the system configuration for identifying the spring constant of each rotary joint shaft 11 of the robot arm 2. The spring constant to be identified is as follows. That is, as shown in FIG. 5B, FIG. 6B, and FIG. 7, the same axis as the rotation center axis Lc of each rotary joint shaft 11 in the state without bending is the z axis, and is orthogonal to the z axis. Assuming three-dimensional coordinates with two axes that are perpendicular to each other and the x-axis and y-axis being orthogonal to each other, the spring constant around the z-axis is Kz, the spring constant around the x-axis is the spring constant around Kx and the y-axis Is Ky. Among the spring constants Kz, Kx, Ky of each rotary joint shaft 11, all of the spring constants to be identified can be targeted, and a desired one can be selected. The three-dimensional coordinates of the spring constant may be the same as the unique coordinates of the arms 4 to 9, or may be different.

  Here, the spring constant Kz around the z-axis is not only the bending (twisting) of the rotary joint shaft 11 alone shown in FIG. 7C but also the rotary joint shaft from the rotary shaft of the servo motor 12 through the gear reduction device 13. 11 to bend (torsion) including a torque transmission path to the arms 4 to 9. Further, as shown in FIGS. 7A and 7B, most of the bending in the rotational direction about the x-axis and the y-axis occurs in the bearing 10, and the bending of the rotary joint shaft 11 hardly occurs. Therefore, regarding the spring constant Kx around the x axis and the spring constant Ky around the y axis, the rotary joint axis 11 is treated as a rigid body.

  In FIG. 1, the robot arm 2 installed on the floor surface is shown by a skeleton. A three-dimensional coordinate measuring device (measuring means) 17 is installed on the floor so as to face the robot arm 2. Both the robot control unit 14 and the control unit (hereinafter referred to as a measurement control unit) 18 of the coordinate measurement device 17 are connected to a spring constant identification device 19. This spring constant identification device 19 is composed of a personal computer, and includes a control unit mainly composed of a CPU, a storage unit storing software for spring constant identification, an input unit such as a keyboard and a mouse, and an output unit such as a liquid crystal display. I have.

  The coordinate measuring device 17 is for measuring the position and posture of a measurement point defined at or near the tip of the robot arm 2. In this embodiment, when the wrist 8 side of the flange 9 is the base end and the side opposite to the wrist 8 is the front end, as shown in FIG. 3, the spring constant is applied to the front end surface of the flange 9 (tip of the robot arm 2). A weight 20 used for identification is fixed by adhesion or the like. The position of the center of gravity of the tip surface of the weight 20 is determined as the measurement point t. Therefore, the measurement point t is determined near the tip of the robot arm 2 in this embodiment. The weight 20 may be attached to the flange 9 with a screw.

  For the measurement point t, measurement point coordinates with the measurement point t as the origin are defined. The measurement point coordinates are 3 with the axis perpendicular to the tip surface of the weight 20 as the Zt coordinate axis, and the two axes perpendicular to the Zt coordinate axis and existing on the tip surface of the weight 20 as the Xt coordinate axis and the Yt coordinate axis. It is composed of dimensional coordinates. The unit vectors on the coordinate axes of Xt, Yt, and Zt are defined as approach vector A, normal vector N, and orientation vector O, respectively.

  FIG. 3 also shows flange 9 coordinates represented by Zf, Xf, and Yf. In this case, by making the tip surface of the weight 20 parallel to the tip surface of the flange 9 and making the measurement point t of the tip surface coincide with the Zf coordinate axis perpendicular to the tip surface of the flange 9, The posture is known. That is, the measurement point t is separated from the origin of the flange coordinate by the thickness of the weight 20, the Zt coordinate axis of the measurement point coordinate overlaps the Zf coordinate axis of the flange coordinate, and each coordinate axis of the measurement point coordinates Xt and Yt is the flange coordinate Xf, It is parallel to each coordinate axis of Yf.

  Three light emitting diodes 21 to 23 are fixed to the front end surface of the weight 20 by bonding or the like. The light emitting colors of the three light emitting diodes 21 to 23 are different from each other, and the fixed position thereof is, for example, an equilateral triangle when a regular triangle with the measurement point t as the center of gravity and one vertex located on, for example, the Yt coordinate axis is drawn. It is determined at the position that becomes the vertex of.

  The coordinate measuring device 17 is composed of, for example, a three-dimensional measuring instrument (for example, Toyo Technica Co., Ltd .; robot calibration ROCAL system) provided with three CCD cameras. Three light emitting diodes 21 are composed of three cameras. As a result, the measurement control unit 18 analyzes the captured image and measures the position and orientation of the measurement point. The position / orientation of the measurement point t analyzed by the measurement control unit 18 is the position / orientation at the coordinates of the coordinate measuring device 17 (hereinafter referred to as measurement coordinates) indicated by the three-dimensional coordinates Xm, Ym, and Zm in FIG. Indicated by The coordinate measuring device 17 may be a laser tracker or the like. The reason why the light emitting colors of the three light emitting diodes 21 to 23 are different is to make the coordinate measuring device 17 recognize each of them individually. For that purpose, the colors of the three light emitting diodes 21 to 23 are made the same. Thus, the timing of light emission may be made different from each other.

The basic concept of the spring constant identification method of the present invention is as follows. That is, when each servo motor 12 is rotated by the rotation command from the robot control unit 14 to operate the robot arm 2, the rotation angle of each rotary joint shaft 11 is given from the robot control unit 14 to the spring constant identification device 19. Measurement information about the position and orientation of the measurement point t is supplied from the measurement control unit 18 to the spring constant identification device 19. The spring constant identification device 19 is provided with a rotation angle of each rotary joint shaft 11 given from the robot control unit 14, a torque acting around each zxy axis of each rotary joint shaft 11, and a spring constant of each rotary joint shaft 11. The position / orientation of the measurement point t is calculated using the predicted values K _z0 , K _x0 , K _y0 of Kx, Ky, Kz.

Each arm 3 to 9 can be treated as a rigid body because it hardly bends due to its structure. Therefore, between the measured position / posture of the measurement point t and the position / posture of the measurement point t obtained by calculation. _Are different from each other in the predicted values K _z0 , K _x0 , K _y0 of the spring constants Kx, Ky, Kz of the rotary joint shafts 11 and the spring constants Kx, Ky, Kz of the rotary joint shafts 11. This is considered to be caused by the difference from the true value of. Accordingly, the predicted values K _z0 , K _x0 , and K _y0 are changed so that there is no difference between the position / posture of the measured measurement point t and the position / posture of the measurement point t obtained by calculation. The spring constants Kx, Ky, Kz can be identified as true values or values close thereto. The initial predicted values K _z0 , K _x0 , K _y0 are stored in advance in the storage unit 16 of the robot 1, for example.

  An identification method for identifying the spring constants Kx, Ky, Kz of each rotary joint shaft 11 based on such a concept will be described with reference to the flowchart of FIG. First, in order to identify the spring constant, a plurality of operation positions (including postures) of the robot arm 2 for acquiring information as a basis thereof are determined (step S1 in FIG. 4). In this case, the operation position is determined so as to cover the entire operation area of the robot arm 2, and when each operation position is taken, all the rotation joint axes 11 rotate so that a plurality of each rotation joint axis 11 is provided. It is preferable not to have the same rotation angle between the operating positions.

  Next, the robot arm 2 is sequentially moved to the determined plurality of operation positions (step S2). At this time, the position and orientation of the measurement point t of the robot arm 2 are measured by the coordinate measuring device 17 every time the position is moved to each operation position. Then, the position / posture measurement information, which is the measurement result, is sent from the measurement control unit 18 to the spring constant identification device 19 and at the same time, the rotational joint shaft 11 detected from the rotational speed of the servo motor 12 that drives each rotational joint shaft 11. The rotation angle information is sent from the robot control unit 14 to the spring constant identification device 19. The position / posture measurement information sent to the spring constant identification device 19 and the rotation angle information of each rotary joint shaft 11 are stored in a storage unit (not shown) such as a RAM of the spring constant identification device 19 (above). In step S2-5, “NO” is repeated; information acquisition means).

When the robot arm 2 has been moved to all of the plurality of operation positions (“YES” in step S5), the spring constant identification device 19 uses the same order of vectors representing the position / posture of the measurement point t from the position / posture measurement information. A transformation matrix T _mea (Tm in the claims) is obtained. Of course, the homogeneous transformation matrix T _mea of the position and orientation of the measurement point t obtained from this measurement information is for the measurement coordinates (predetermined fixed coordinates).

Further, the spring constant identification device 19 obtains the position / posture of the measurement point t by calculation using the rotation angle information of each rotary joint shaft 11 (measurement point calculation means). Here, in FIG. 1, ^meabase T _robbase is a homogeneous transformation matrix indicating the position / orientation of the robot coordinate with respect to the measurement coordinate, ^robbase T _flange is a homogeneous transformation matrix indicating the position / orientation of the flange coordinate with respect to the robot coordinate, ^flange T _tool is a homogeneous transformation matrix indicating the position and orientation of the measurement point t with respect to the flange coordinates.

Therefore, the homogeneous transformation matrix T _rob (Tc in the claims) indicating the position and coordinates of the measurement point t with respect to the measurement coordinates is
It can be obtained by the product of ^meabase T _robbase , ^robbase T _flange and ^flange T _tool . Among them, as described above, the ^flange T _tool is known, and since the relationship between the measurement coordinates and the robot coordinates can be measured, the ^meabase T _robbase is also known. And
^{The robbase} T _flange can be calculated using the rotation angle information of each rotary joint axis 11 and the torque acting on each rotary joint axis 11.

Now, a homogeneous transformation matrix indicating the position and orientation of the flange 9 with respect to the robot coordinates.
^{The robbase} T _flange is expressed by a product of homogeneous transformation matrices between the rotary joint axes 11 (first axis to sixth axis) as shown in the following equation (1).

なお、（１）式において、０はベース（ロボット座標）、１〜６は第１軸〜第６軸を示す。

In equation (1), 0 indicates the base (robot coordinates), and 1-6 indicate the first to sixth axes.

  The homogenous transformation matrix between axes in the equation (1) is expressed by a product of matrices represented by parameters θ, a, b, d, α, and β, as shown in the following equation (2). In addition, i is an integer from 1 to 6 indicating the first axis to the sixth axis, and each axis Lc-1 to Lc-6 of the parameters θ, a, b, d, α, and β (each rotary joint axis 11). ) Values are shown in the parameter table.

  In the definition of the above parameters, ai is the offset in the X coordinate axis direction of the coordinates of the i axis (i arm) and the i + 1 axis (i + 1 arm), bi is the offset in the Y coordinate axis direction, and di is the offset in the Z coordinate axis direction. Θi is a rotation angle around the Z coordinate axis, αi is a rotation angle around the X coordinate axis, and βi is a rotation angle around the Y coordinate axis.

By the way, the actual rotation angle around each coordinate axis of ZXY is obtained by adding the rotation angle due to the bending of the rotary joint shaft 11 to each rotation by the servo motor 12. The rotation angles (deflection angles; rads) s _zi , s _xi , and s _yi due to the bending of each rotary joint axis 11 about each axis are about the z axis, the x axis, and the y axis of each rotary joint axis 11. acting torque _{(Nm) t zi, t xi} , t yi, the predicted value of the spring constant around each axis of zxy of each rotating joint shaft 11 stored in the storage section 16 as a parameter _(Nm / rad) K z0i , K _x0i , K _y0i ,
s _zi = t _zi / K _z0i (7)
s _xi = t _xi / K _x0i (8)
s _yi = t _yi / K _y0i (9)
It is represented by

The torques t _zi , t _xi , and t _yi are the rotation angles obtained from the rotation angle information of the respective rotary joint shafts 11 detected by the joint angle detector 15, the masses of the arms 4 to 9, and the arms 4 to 9. Calculation is performed by the Newton Euler method based on the position of the center of gravity, the inertia of each arm 4 to 9, the mass of the weight 20, and the position of the center of gravity. The Newton Euler method requires information on angular velocity and angular acceleration, but since the robot arm 2 is stopped, there is no need for information on angular velocity and angular acceleration.

If the reciprocal of the predicted value of the spring constant is set to k _z0i , k _x0i , k _y0i , the rotation angle of each rotary joint shaft 11 about each axis of xxy is the reciprocal of the predicted value of the spring constant k _z0i , k _x0i. , K _y0i are expressed as follows.
s _zi = t _zi · k _z0i (10)
s _xi = t _xi · k _x0i (11)
s _yi = t _yi · k _y0i (12)
Therefore, the actual rotation angle around each coordinate axis of zxy is
θi = θi + t _zi · k _z0i (13)
αi = αi + t _xi · k _x0i (14)
_βi = _βi + t _yi · k _y0i (15)
Therefore, the rotation angle considering the deflection angle is substituted into the equations (3), (5) and (6), and the homogeneous transformation matrix between the axes is obtained by the equation (2). is there. Then, a homogeneous transformation matrix ^robbase T _flange indicating the position / orientation of the flange 9 with respect to the robot coordinates of the equation (1) can be obtained by the product of the obtained homogeneous transformation matrix between the respective axes.

After ^{obtaining the homogeneous} transformation matrix ^robbase T _flange by calculation, as described above, the homogeneous transformation matrix T _rob indicating the position and orientation of the measurement point t with respect to the measurement coordinates is obtained by the following equation (16) (step S6: Homogeneous transformation matrix calculation means).
T _rob = ^meabase T _robbase / ^robbase T _flange / ^flange T _tool (16)

When T _rob is obtained by calculation as described above, the spring constant identification device 19 then obtains the simultaneous conversion matrix T _mea of the measurement points t obtained by measurement and calculation for each operation position of the robot arm 2. An error matrix ΔT indicating a difference between the measurement point t and the homogeneous transformation matrix T _rob is obtained (step S7: error matrix calculation means), and then the total (error sum) ΔT _all (error sum) of the error matrix ΔT at each operation position. The error sum matrix ΔR in the claims is calculated by the following equation (17) (error sum matrix calculation means).

なお、（１７）式のｈは、動作位置の数を示す。

In the equation (17), h represents the number of operating positions.

Subsequently, the spring constant identification device 19 determines whether or not the error sum ΔT _all is equal to or less than a predetermined threshold ε (step S8: determination means). If the error sum ΔT _all is equal to or less than the threshold ε, the spring constant predicted value K _{z0i is determined.} , K _x0i , and K _y0i are identified as spring constants (identification means), and it is determined whether or not the identified spring constants are within appropriate values predicted in advance. If it is within an appropriate value, the identified spring constant is stored in the storage unit 16 (“YES” in step S9, step S10), and the identification of the spring constant is terminated.

If the identified spring constant is not within an appropriate value (“NO” in step S9), the identification operation is repeated from step S1 described above.
On the other hand, if the error sum ΔT _all exceeds the threshold ε (“NO” in step S8), the spring constant identification device 19 increments the estimated number counter x (step S11), and then x is predetermined. It is determined whether or not it is less than the predetermined upper limit number Xmax (step S12), and if it is less, the operator is prompted to select a spring constant to be identified ("YES" in step S12, step S13). In response to this, the operator operates the input unit (selection means) of the spring constant identification device 19 to select _m spring constants K _{1 to} K _m to be identified from all the spring constants K _all . For example, in a rotary joint shaft that is supported at both ends, the bending around both axes of xy is considered to be zero, so that the spring constant need not be identified, and is excluded from the spring constant to be identified. Of course, all spring constants may be selected.

By the way, as understood from the above description, the homogeneous transformation matrix T _rob indicating the position / orientation of the measurement point t obtained by calculation is a function of the unknown deflection angle s of each rotary joint axis 11, and the deflection. Since the angle s is a function of the reciprocal k of the spring constants Kx, Ky, Kz of each axis of zxy, it can be expressed by the following equation (18), and the reciprocals k of all the spring constants are expressed by equation (19). It can be expressed as a matrix of In addition, the code | symbol shown with small t on the right shoulder of the parenthesis of (19) formula has shown that this matrix is transposed matrix, although this matrix is written horizontally. Further, n is the number of the rotary joint shafts 11, which is 6 in the present embodiment.

  The reciprocals k1 to km of the m spring constants K1 to Km to be identified are represented by the following matrix (20).

The error matrix ΔT between T _mea and T _rob for each operation position taken by the robot arm 2 is the reciprocal k ₁ to m spring constants K1 to Km to be identified, as shown by the following equation (21). It is approximated by linear combination the sum of the slight change Δk of k _m, therefore, the homogeneous transformation matrix T _rob and identification of m spring constant K1~Km to be the measurement point t obtained by calculation reciprocal k ₁ for to k _m of It becomes Jacobian matrix J.

Here, the difference between T _mea and T _rob is expressed by the following difference (Δn, Δo and Δa between the normal vector, the orientation vector and the approach vector of the measurement point coordinates, and the difference Δp between the position vectors from the measurement coordinates: 22).

Therefore, the difference between T _mea and T _rob is expressed by the following equation (25) when expressed using Δo, Δa, and Δp.

Further, when the Jacobian matrix J is expressed by using an orientation vector, an approach vector, and a position vector, as shown by the following equation (26), the normal vector, the orientation vector, and the approach vector of the measurement point obtained by calculation are expressed. Of these, for example, the position of the orientation coordinates of the orientation vector and the approach vector on the coordinate axes Zm, Xm, and Ym and the position of the measurement point position vector on the coordinate axes Zm, Xm, and Ym are to be identified. represented by matrix divided by the reciprocal k ₁ to k _m constant.

  Here, the normal vector, the orientation vector, and the approach vector have the following relationship (27).

  That is, if two of the three vectors are known, the remaining one can be calculated. From this, the above formulas (25) and (26) can be a combination of the following formulas (28) and (29), formulas (30) and (31)

Note that the _matrix of the subscript of ΔT in equation (22) means that ΔT is expressed using all of the normal vector, the orientation vector, and the approach vector, and (25), (28), (30) The absence of a _matrix suffix in ΔT in each equation means that any two vectors of a normal vector, an orientation vector, and an approach vector are used.

Now, when the above equation (21), that is, the error (T _mea −T _rob ) between T _mea and T _{rob at} each position / posture is extended to all motion positions and the total ΔR (error sum matrix) is obtained. The following equations (32) and (33) are obtained.

  When the above equation (33) is transformed into the following equations (34) and (35), the small variation matrix Δk of the reciprocal of the spring constant to be identified is represented by the Jacobian sum matrix A as represented by equation (36). It is expressed by the pseudo inverse matrix of

Therefore, Δk is obtained by calculating the equation (36) (step S14: Jacobian sum matrix calculating means, minute variation matrix calculating means). At this time, if the determinant S of the Jacobian sum matrix A ^T A is 0 (where S = | A ^T A |), the pseudo inverse matrix of the Jacobian sum matrix A cannot be obtained. In “NO” in S15, the estimated number counter x is set to 0 (step S18), and the process returns to step S1 to determine the operation position again, and the identification operation is repeated.

.DELTA.k determined by expression (36) is very small differences in the reciprocal k ₁ to k _m of reciprocal and the predicted value of the true value of the m spring constant K1~Km to be identified (slight change) .DELTA.k _1, ~, .DELTA.k _Since it is a matrix representing _m , Δk is expressed by the following equation (37).

Then, the spring constant identifying unit 19, after calculating the .DELTA.k (step S16), and the k _v of the following equation (38), the matrix of the inverse k ₁ to k _m of the predicted value of the spring constant K1~Km to be identified (K0 in the claims) is substituted, and the matrix of the reciprocal of the predicted value of the spring constant to be identified is updated (step S17: predicted value reciprocal matrix updating means).

Thereafter, the spring constant identification device 19 uses the reciprocal number k _{v + 1} of the updated predicted value for the spring constants K1 to Km to be identified, as the reciprocal number of the predicted value of the spring constant in the expressions (13) to (15). For the spring constant not desired to be identified, the reciprocal of the initial predicted value is used as it is, and the ^robbase T _flange for each operating position is obtained again using the matrix (1) and (2). Next, the spring constant identification device 19 re-determines T _rob from the equation (16) (step S6: simultaneous conversion matrix recalculation means) and re-determines the error ΔT from T _mea previously obtained (step S7). : Error matrix recalculation means), then, the total ΔT _all (ΔR) of the errors ΔT is obtained again by the equation (17) (error sum matrix recalculation means), and the error sum ΔT _all (ΔR) is a predetermined threshold ε It is determined whether the following is true (step S8).

If the error sum ΔT _all is less than or equal to the predetermined threshold ε, the spring constant identification device 19 initially uses the spring constant obtained from the reciprocal k _{v + 1} of the predicted value of the updated spring constant to be identified and the spring constant not desired to be identified. Is identified as each spring constant, and if the identified value is appropriate (“YES” in step S9), this is stored in the storage unit 16 (step S10).

If it is not less than or equal to the threshold ε, the spring constant identification device 19 increments the estimated number counter x (step S11), obtains a minute variation matrix Δk of the reciprocal of the spring constant again by the equation (36), and formula (38). Thus, the inverse number of the predicted value of the spring constant to be identified again is updated by adding the inverse number k _{v + 1} of the previously updated spring constant to Δk. When the estimated number counter x is 2 or more, it is not always necessary to select the spring constant to be identified. Of course, the same spring constant as when x = 1 may be selected even when x is 2 or more, and further, a spring constant to be identified is added within a range that does not affect the estimation of the spring constant. Accordingly, the selection operation may be performed when x is 2 or more.

Then, using the inverse k k _{+ 1} of the predicted value of the spring constant to be identified again, the homogeneous transformation matrix T _rob for each position / posture is obtained by matrix (2), and T _mea obtained previously Next, it is determined whether the total sum (error sum) ΔT _all (ΔR) is equal to or less than a predetermined threshold value ε. 38) The operation of updating the inverse k k _{+ 1} of the predicted value of the spring constant to be identified again by adding the inverse k _v of the predicted value of the spring constant to be identified previously updated to It is repeatedly executed until the total sum [Delta] R becomes equal to or less than a predetermined threshold [epsilon] (repetitive execution means).

When the error sum ΔT _all becomes equal to or less than a predetermined threshold value, k _{v + 1} is set as the reciprocal of the spring constant to be identified, and the spring constant is obtained from k k _{+ 1} and stored in the storage unit 16.
When the robot arm 2 is actually operated, the bending of the rotary joint shaft 11 in the zxy directions is obtained using the spring constant identified as described above, and the rotation speed of each servo motor 12 is calculated using this bending. By correcting, the tip of the robot arm 2 can be accurately operated to the taught position / posture.

  As described above, according to the present embodiment, not only the spring constant around the z axis but also the spring constant around each xy axis can be identified. In addition, since the torque around each axis of zxy acting on each rotary joint axis 11 is analyzed to identify the spring constant around each axis of zxy, the identified spring constant is matched with the true spring constant with high accuracy. be able to.

  Further, in the case of the conventional spring constant identification method, it is necessary to measure the deflection of each rotary joint shaft 11 in order from the one closest to the base 3, and if the rotary joint shaft is mistakenly skipped, the measurement cannot be performed correctly. However, according to the identification method of this embodiment, every time the robot arm 2 is operated, the position / posture of the measurement point t is measured by the coordinate measuring device 17 according to the identification method of this embodiment. Since only measurement is required, the measurement work becomes simple and there is no risk of mistakes.

[Second Embodiment]
In the first embodiment described above, the spring constant is identified using both the position and orientation for each operating position. In this embodiment, the spring constant can be identified only by the position so as to cope with the case where the posture cannot be measured.
In other words, in the equation (32), when attention is paid only to the position, the following equation (39) is obtained.

  The above equation (39) is expressed as the following equation (40), and when this equation (40) is transformed, a minute variation of the reciprocal of the m spring constants to be identified represented by the above equation (36) is obtained. The matrix Δk is expressed by the following equation (45).

Therefore, when the reciprocal of the predicted values K _{01 to} K _0m of the spring constant to be identified is given, the reciprocal of the predicted value of the spring constant to be identified can be updated by the following equation (46). Similarly, the spring constant can be identified by repeatedly calculating so that the error sum ΔR _p is equal to or less than the threshold value ε.

[Third Embodiment]
In this embodiment, when ^meabase T _robbase or ^flange T _tool is unknown, these are to be identified (estimated) before the spring constant is identified.

  In general, when the robot arm 2 takes one position / posture, an error ΔT (9 rows and 1 column) between the measured value by the coordinate measuring device 17 and the calculated value is a parameter to be identified as the following equation (47): It is approximated by a linear combination sum of a minute fluctuation Δa of a.

The T _mea is a homogeneous transformation matrix based on measurement values obtained by the coordinate measuring device 17 similar to that described in the first embodiment, and T _rob is a homogeneous transformation matrix based on calculated values. However, in the present embodiment, when _obtaining T _rob , the deflection of each rotary joint shaft 11 is not considered, and only the rotation angle of each rotary joint shaft 11 is used as a basis.

^Homogeneous transformation matrices representing ^meabase T _robbase and ^flange T _tool are expressed by the following equations (51) and (52).

  The parameter a to be identified is represented by the following equation (53), and the minute fluctuation Δa of the parameter a is represented by the equation (54).

  Here, since the normal vector, the approach vector, and the orientation vector have the relationship shown in the first embodiment, when the error ΔT and the Jacobian matrix J in the equation (47) are expressed using the orientation vector and the approach vector, respectively. Equations (55) and (56) are obtained.

  When the error ΔT is expanded to all the operation positions of the plurality of operation positions, the equation (57) is obtained.

  The above formula (57) is expressed as the following formula (58), and it is desired to identify the formula (58) by transforming it into the formulas (34) and (35) in the first embodiment. The minute variation matrix Δa of the parameter a is as shown in Equation (61).

Therefore, when the predicted value a _{0 of the} parameter to be identified is given to a _v in the following equation (62), the predicted value of the parameter a can be updated.

The parameter a to be identified can be obtained by repeating the above calculation until ΔR becomes equal to or less than a predetermined threshold value.
[Other Embodiments]
^{The meabase} T _robbase and ^flange T _tool may be identified simultaneously with the spring constant.
It is not necessary to attach a weight to the tip of the robot arm. However, since the deflection of the rotary joint shaft 11 increases when the weight is added, the spring constant can be easily identified.
The predicted values K _z0 , K _x0 , and K _y0 of the spring constants Kz, Kx, and Ky may be stored in the storage unit of the spring constant identification device 19.
As can be understood from the equation (22), since nine simultaneous equations can be generated for one operation position, theoretically, when the spring constant to be identified is 9 or less, the operation position of the robot arm 2 is 1 If the number is 18 or less, two operation positions are sufficient.

  In the drawings, 2 is a robot arm, 9 is a flange (tip of the robot arm), 11 is a rotary joint axis, 14 is a robot control unit, 15 is a joint angle detector, 16 is a storage unit (storage means), and 17 is coordinate measurement. Device (measuring means) 18 is a spring constant identification device (information acquisition means, measurement point calculation means, error sum matrix calculation means, identification means, selection means, Jacobian sum matrix calculation means, minute variation matrix calculation means, predicted value reciprocal matrix Update means, simultaneous conversion matrix recalculation means, and repeat execution means), 19 is a measurement control unit, 20 is a weight, and t is a measurement point.

Claims (2)

In a multi-joint robot arm configured by connecting a plurality of arms by a plurality of rotary joint axes, the same axis as the rotation center axis of each rotary joint axis is a z-axis, and two axes orthogonal to the z-axis. The two axes perpendicular to each other are the x-axis and the y-axis, the spring constant of the deflection in the rotational direction about the z-axis including the torque transmission path to each rotary joint axis is Kz, and the x-axis is the center When the spring constant of deflection in the rotational direction is Kx and the spring constant of deflection in the rotational direction around the y-axis is Ky,
A measurement point is set near the tip of the robot arm or near the tip of the robot arm, a weight is attached to the tip of the robot arm, or the weight is not attached, and the robot arm is moved to a plurality of different positions, For each operation position, at least the position and the posture of the measurement point are measured by the measuring means, and the rotation angle of each rotary joint axis and the respective axes of zxy acting on each rotary joint axis are centered. And at least a position of the position and orientation of the measurement point is obtained by calculation using a predicted torque and a predetermined value of each spring constant Kx, Ky, Kz of each rotary joint axis,
For each of the plurality of operation positions, a simultaneous conversion matrix Tm based on a predetermined fixed coordinate for at least the position of the measurement point measured by the measuring means and the predetermined point for at least the position of the measurement point obtained by the calculation An error matrix ΔT with a simultaneous conversion matrix Tc based on fixed coordinates is obtained, and an error sum matrix ΔR obtained by summing the error matrix ΔT with respect to the plurality of operation positions is obtained.
Determining whether the value of the error sum matrix ΔR is less than or equal to a predetermined threshold;
If the value of the error sum matrix ΔR is equal to or less than the predetermined threshold, the predicted values of the spring constants Kx, Ky, Kz of the rotary joint axes are identified as the spring constants Kx, Ky, Kz,
If the value of the error sum matrix ΔR exceeds the predetermined threshold, then
A spring constant to be identified is selected from among the spring constants Kx, Ky, Kz for each of the zxy axes of the rotary joint axes;
For each of the plurality of operating positions, a predicted value reciprocal matrix k _v that is a matrix of the reciprocal of the predicted value of the spring constant to be identified and the homogeneous transformation matrix Tc for at least the position of the measurement point obtained by the calculation. And a Jacobian sum matrix A that sums up the obtained Jacobian matrix J at each of the motion positions.
Obtaining a reciprocal minute variation matrix Δk for the predicted value of the spring constant to be identified from the product of the pseudo inverse matrix of the Jacobian sum matrix A and the error sum matrix ΔR;
Update the predicted value inverse matrix k _v spring constant to be the identification by adding the micro fluctuation matrix Δk to the predicted value inverse matrix k _v spring constant to be the identification,
For the spring constants to be identified, the spring constants obtained from the updated predicted value reciprocal matrix k _{v + 1} are used, and for the spring constants other than the spring constants to be identified, initial predicted values are used for the plurality of operating positions. Recalculating at least the position of the measurement point and re-determining the homogeneous transformation matrix Tc for at least the position of the recalculated measurement point;
The error matrix ΔT is obtained again from the homogeneous transformation matrix Tm of the measurement point obtained by the measuring means and the homogeneous transformation matrix Tc of the measurement point obtained by the calculation again for the plurality of operation positions, The error matrix ΔT obtained again is summed up for the plurality of operation positions to obtain the error sum matrix ΔR again, and whether or not the value of the error sum matrix ΔR is less than or equal to the predetermined threshold value is determined. A robot spring constant identification method for repeatedly identifying each spring constant to be identified until a sum matrix ΔR becomes a predetermined threshold value or less. Of the articulated robot arm controller configured by connecting a plurality of arms with a plurality of rotary joint axes, and the position and posture of the measurement point determined near the tip of the robot arm or near the tip of the robot arm Connected to a control unit of a measuring means for measuring at least the position of
The same axis as the rotation center axis of each rotary joint axis is the z-axis, and the two axes orthogonal to the z-axis and perpendicular to each other are the x-axis and the y-axis, and torque is transmitted to each rotary joint axis. A spring constant of deflection in the rotation direction centered on the z axis including the path is Kz, a spring constant of deflection in the rotation direction around the x axis is Kx, and a spring constant of deflection in the rotation direction around the y axis When the constant is Ky, storage means for storing predicted values of the spring constants Kx, Ky, Kz;
When the robot arm is moved to a plurality of different positions with or without a weight at the tip of the robot arm, each rotation from the control unit of the robot arm is performed for each of a plurality of movement positions. Information acquisition means for acquiring rotation angle information of the joint axis, and acquiring information on at least the position of the position and orientation of the measurement point from the control unit of the measurement means;
For each of the plurality of operation positions, at least a position of the position and posture of the measurement point is set to a rotation angle of each rotary joint axis, a torque about each axis of the zxy acting on each rotary joint axis, and Measuring point calculation means for calculating by using predicted values of the spring constants Kx, Ky, Kz of the rotary joint axes determined in advance;
For each of the plurality of operation positions, a simultaneous conversion matrix Tm based on predetermined fixed coordinates is obtained for at least the position of the measurement point from the information acquired from the measurement means, and at least the position of the measurement point obtained by the calculation is obtained. An error sum matrix calculation means for obtaining an error matrix ΔT with respect to the simultaneous conversion matrix Tc based on the predetermined fixed coordinates and obtaining an error sum matrix ΔR by summing the error matrix ΔT for the plurality of operation positions;
Determining means for determining whether or not the value of the error sum matrix ΔR is equal to or less than a predetermined threshold;
If the value of the error sum matrix ΔR is equal to or less than the predetermined threshold, identification means for identifying the predicted value as each spring constant;
Selecting means for selecting a spring constant to be identified from the spring constants Kx, Ky, Kz of the rotary joint axes when the value of the error sum matrix ΔR exceeds the predetermined threshold;
Obtaining a Jacobian matrix J for a predicted value reciprocal matrix k _v , which is a matrix of the reciprocal of the predicted value of the spring constant to be identified and the homogeneous transformation matrix Tc determined by the calculation, for each of the plurality of operation positions; A Jacobian sum matrix calculation means for obtaining a Jacobian sum matrix A that sums up the Jacobian matrix J of each of the obtained motion positions;
A minute variation matrix calculating means for obtaining a minute variation matrix Δk of an inverse of the predicted value of the spring constant to be identified from a product of the pseudo inverse matrix of the Jacobian sum matrix A and the error sum matrix ΔR;
A predicted value inverse matrix updating means for updating the predictive value inverse matrix k _v for the spring constant to be the identification by adding the micro fluctuation matrix Δk to the predicted value inverse matrix k _v spring constant to be the identification,
For the spring constants to be identified, the spring constants obtained from the updated predicted value reciprocal matrix k _{v + 1} are used, and for the spring constants other than the spring constants to be identified, initial predicted values are used, and the plurality of operating positions A simultaneous conversion matrix recalculation means for recalculating at least the position of the measurement point and recalculating the homogeneous transformation matrix Tc for at least the position of the recalculated measurement point;
Re-determining the error matrix ΔT from the homogeneous transformation matrix Tm of the measurement points obtained by the measurement means and the homogeneous transformation matrix Tc of the measurement points obtained again for the plurality of operation positions; and The operation of determining whether or not the value of the error sum matrix ΔR is equal to or less than the predetermined threshold by resuming the error sum matrix ΔR by summing the error matrix ΔT obtained again for the plurality of operation positions, An apparatus for identifying a spring constant of a robot, comprising: repetitive execution means for repetitively executing the error sum matrix ΔR until the error sum matrix ΔR becomes equal to or less than the predetermined threshold value to identify each spring constant to be identified.

Images