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

Abstract

<P>PROBLEM TO BE SOLVED: To identify a spring constant around zxy axes with high accuracy for each rotary joint shaft of an articulated robot arm. <P>SOLUTION: The robot arm is operated at a plurality of positions, one of two weights is attached to a fore end of the robot arm for each operational position, the rotation angle of each rotary joint shaft is acquired at each operational position, and the position and the attitude at the measuring point for each weight are measured by a measuring means. Further, the position and the attitude at the measuring point when a weight of different mass is attached at each operational position are obtained by the calculation by using the predicted values of the rotation angle of each rotary joint shaft and each spring constant of each rotary joint shaft. For the position and attitude, the error between the measured value and the calculated value in the case of a weight of a large mass, and the difference between the measured value and the calculated value in the case of a small mass are obtained for the position and the attitude, and the position and the attitude of the measuring point. The predicted value of the spring constant is corrected so that the total value of the difference between both differences for each operational position is equal to or less than the predetermined threshold. <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, the robot arm is moved to a plurality of positions, and at least two weights having different masses are attached to the tip of the robot arm one by one at each movement position, and the rotation angle of each rotary joint axis is obtained at each movement position. In addition, every time the weight is changed, at least the position and the posture of the measurement point determined at or near the tip of the robot arm are measured by the measuring means, and the rotation angle of each rotary joint axis and each Calculated using the predicted value of each spring constant of the rotary joint axis, and further, the error between at least the position measured by the measurement with a large mass weight and at least the position by the calculation, and the measurement with a small mass weight The error between at least the position by the above and at least the position by the calculation is obtained, and the difference between these two errors is summed for each operation position. The difference between at least the position of the measurement at the time of the weight of the large mass and the error at least of the position by the calculation, and the difference between the at least the position of the measurement at the time of the small mass weight and the at least the position by the calculation was initially determined. Since this is caused by the difference between the predicted value of each spring constant and the true spring constant, the predicted value of the spring constant is corrected by a mathematical method so that the difference between the two is below a predetermined threshold. In addition to the spring constant around the rotation center axis (z axis) of each rotary joint axis, the spring constant around two axes (xy axes) orthogonal to the rotation center can be obtained, and finally obtained. The predicted value (identified spring constant) of each spring constant is close to the true spring constant with a certain degree of accuracy. In addition, since the spring constant is obtained by utilizing at least the position difference when the weight is changed, when calculating the position of the measurement point by calculating the deflection of the rotary joint shaft when the torque is applied, Even if the torque due to the weight or the like is not taken into consideration, only the torque due to the weight of the weight needs to be considered, and the calculation becomes easy.

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.

  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 of the tip of the unit vector on the coordinate axis of each arm 4-9 in the robot coordinate when the coordinates of each arm 4-9 are moved in parallel 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. Each axis zxy of the three-dimensional coordinates of the spring constant is the same as each coordinate axis of the intrinsic coordinates of the arms 4 to 9 in this embodiment, but 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, the bending in the rotation direction around the x-axis and the y-axis is not limited to the bending of the rotary joint shaft 11 itself, and the bearing 10 is also deformed as shown in FIGS. 7A and 7B. The spring constant around each axis includes the spring constant of the bearing 10. In addition, since the rotary joint shaft 11 is an object shape centering on the z-axis together with the bearing 10, the spring constants around the respective axes of xy usually have the same value.

  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 rearward and the opposite side is frontward, as shown in FIG. 3, the holder 20 is fixed to the outer periphery of the flange 9 constituting the tip of the robot arm 2, Measurement points t are defined at predetermined locations on the front surface of the holder 20 (parallel to the front surface of the flange 9). Therefore, in this embodiment, the measurement point t is determined near the flange 9 that is the tip of the robot arm 2. Note that the measurement point t may be determined on the front surface of the flange 9, and in short, it may be a location that moves together with the flange 9.

  For the measurement point t, measurement point coordinates with the measurement point t as the origin are defined. The measurement point coordinates are constituted by three-dimensional coordinates having coordinate axes Zt, Xt and Yt parallel to the coordinate axes Zf, Xf and Yf of the flange 9, respectively. Unit vectors on the coordinate axes Zt, Xt, and Yt are defined as an orientation vector O, an approach vector A, and a normal vector N, respectively.

  Three light emitting diodes 21 to 23 as indices are fixed to the front end surface of the holder 20 by adhesion or the like. The three light emitting diodes 21 to 23 have different emission colors, and their fixed positions are set to positions that are the vertices of the equilateral triangle when, for example, an equilateral triangle having the measurement point t as the center of gravity is drawn. The fixed positions of the light emitting diodes 21 to 23 are not limited to the above positions.

  The coordinate measuring device 17 includes, 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 formed by three cameras. As a result, the measurement control unit 18 analyzes the captured image and measures the position and orientation of the measurement point t. The position of the measurement point t by the coordinate measuring device 17 is indicated by a position on the three-dimensional coordinates (hereinafter referred to as measurement coordinates) of the coordinate measuring device 17 whose coordinate axes are indicated by Xm, Ym, and Zm in FIG. The posture is indicated by the tip position of the orientation vector O, approach vector A, and normal vector N of the measurement point coordinates in the measurement coordinates. 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, in order to identify each spring constant Kx, Ky, Kz of each rotary joint shaft 11, in this embodiment, the two weights 24, 25 having different masses are exchangeably bonded to the center of the front end surface of the flange 9. Fixed by. The weights 24 and 25 may be fixed by screwing. The weights of the two weights 24 and 25 are lighter than the weight 25 in the weight 24. The two weights 24 and 25 have a centroid position determined in advance, and when fixed at the center of the front end surface of the flange 9, the respective centroid positions can be specified as positions on the flange coordinates. ing.

  When identifying the spring constant, the tip of the robot arm 2 is moved to a plurality of different positions. When moving to the plurality of operating positions, all the rotary joint shafts 11 are rotated so that the rotary joint shafts 11 do not take the same rotation angle at the plurality of operating positions. When the robot arm 2 stops at the set operation position, the rotation angle of each rotary joint shaft 11 at that time is detected and sent to the spring constant identification device 19. Further, at each operation position of the robot arm 2, the two weights 24 and 25 are alternately attached to the flange 9, and each time the weights 24 and 25 are attached, the coordinate measuring device 17 changes the position and posture of the measurement point t. It is measured and sent to the spring constant identification device 19.

The spring constant identification device 19 calculates the measurement point t from the measurement information about the position and orientation of the measurement point t from the coordinate measurement device 17 when the two weights 24 and 25 are attached at each operation position of the robot arm 2. The simultaneous conversion matrix T _mea (Tm in claims 1 and 2) is obtained. In addition, the spring constant identification device 19 has the rotation angle of each rotary joint shaft 11 given from the robot control unit 14, the masses of the weights 24 and 25, the lengths of the arms 4 to 9, and the storage unit 16 as parameters. Two weights 24 and 25 are attached at each operation position of the robot arm 2 using the _stored predicted values K _x0 , K _y0 , and K _z0 of the spring constants Kx, Ky, and Kz of each rotary joint shaft 11. A homogeneous transformation matrix T _rob (Tc in claims 1 and 2) indicating the position and orientation of the measurement point t is calculated. Note that the predicted values K _x0 , K _y0 , and K _z0 are stored in advance in the storage unit (storage unit) 16 of the robot 1, for example.

  When calculating a homogeneous transformation matrix indicating the position and orientation of the measurement point t, the deflection of each rotary joint axis 11 must be taken into account. In addition to the torque generated by the weights 24 and 25, the torque generated by the masses of the arms 4 to 9 acts on the rotary joint shafts 11, and the rotary joint shafts 11 bend upon receiving both torques. However, in this embodiment, when calculating the homogeneous transformation matrix indicating the position and orientation of the measurement point t, only the torque generated by the mass of the weights 24 and 25 is considered, and the torque generated by the mass of each arm 4-9 is calculated. Do not consider.

Instead, in this embodiment, paying attention to the difference between the homogeneous transformation matrix T _mea obtained by measurement for each of the weights 24 and 25 and the homogeneous transformation matrix T _rob obtained by calculation, the weight 25 with a large mass is measured. An error matrix ΔT _highload indicating the difference between the homogeneous transformation matrix T _mea by measurement and the homogeneous transformation matrix T _rob by calculation, and the homogeneous transformation matrix T _mea by measurement and the homogeneous transformation matrix by calculation for the weight 24 having a small mass. An error matrix ΔT _lowload indicating a difference from T _rob is obtained.

In the homogeneous transformation matrix T _mea by this measurement and the homogeneous transformation matrix T _rob by calculation, the torques generated by the respective weights 24 and 25 are the same. Therefore, by calculating the difference between T _mea and T _rob , the arms 4 to 9 are obtained. When the torque generated by the mass of each of the rotary joint shafts 11 acts, the predicted values K _x0 , K _y0 , K _z0 of the spring constants Kx, Ky, Kz of the rotary joint shafts 11 and the true values of the rotary joint shafts 11 are generated. The difference in deflection is obtained. However, the torque acting on each rotary joint shaft 11 due to the mass of the arms 4 to 9 is unknown, and when that torque is applied, the predicted values K _x0 , K _y0 of the spring constants Kx, Ky, Kz of each rotary joint shaft 11 are unknown. , K _z0 and the true value do not know how much the difference in deflection of the rotary joint shafts 11 occurs.

Therefore, in the present embodiment, the difference between the error matrix ΔT _highload for the weight 25 having a large mass and the error matrix ΔT _lowload for the weight 24 having a small mass is calculated at each operation position. By taking the difference between the two error matrices (ΔT _highioad −ΔT _lowload ), when the torque generated by the mass of each arm 4 to 9 acts on each rotary joint shaft 11, the predicted values K of the spring constants Kx, Ky, Kz _x0, the difference in deflection occurs each rotary joint shaft 11 by the difference between K _y0, K _z0 and true value is canceled.

Accordingly, (ΔT _highioad −ΔT _lowload ) is obtained when a torque that is a difference between torques acting on each rotary joint shaft 11 by the weight 25 having a large mass and the weight 24 having a small mass acts on each rotary joint shaft 11. The deflection of each rotary joint shaft 11 when the spring constants Kx, Ky, Kz of the rotary joint shafts 11 are predicted values K _x0 , K _y0 , K _z0 , and the deflection of each rotary joint shaft 11 when it is a true value. It makes a difference. Thus, (ΔT _highioad −ΔT _lowload ) is caused by the difference between the predicted values K _x0 , K _y0 , K _z0 of the spring constants Kx, Ky, Kz of the rotary joint shafts 11 and the true values. Therefore, it is possible to identify a true value based on (ΔT _highioad −ΔT _lowload ). The present invention identifies the spring constant based on the above concept.

  Hereinafter, a method for identifying the spring constant will be described with reference to the flowchart of FIG. First, a plurality of operation positions (including postures) of the robot arm 2 for acquiring information that is a basis for identifying the spring constant are determined (step S1 in FIG. 2). In this case, each position is determined so as to cover the entire operation area of the robot arm 2, and when each position is taken as described above, all the rotary joint axes 11 are rotated so that each rotary joint axis is rotated. 11 is prevented from having the same rotation angle between a plurality of operation positions.

  Next, the robot arm 2 is sequentially operated to the determined plural positions. At each operation position, the two weights 24 and 25 are alternately attached to the front end face of the flange 9, and each time the weights 24 and 25 are attached, the position and orientation of the measurement point t are measured by the coordinate measuring device 17, The position / orientation measurement information as the measurement result is sent to the spring constant identification device 19. Further, each time the robot arm 2 is moved to each operation position, the rotation angle of each rotary joint shaft 11 is detected from the rotation speed of the servo motor 12 that drives each rotary joint shaft 11, and the detected rotation angle information is transmitted to the robot control unit. 14 to the spring constant identification device 19. The rotation angle information of each rotary joint shaft 11 and the position / posture measurement information of the measurement point t sent to the spring constant identification device 19 are stored in a storage unit (not shown) such as a RAM of the spring constant identification device 19. (Repeat steps S2-5: information acquisition means).

When the robot arm 2 has been moved to all the determined positions (“YES” in step S5), the spring constant identification device 19 sets the weights 24 and 25 for each movement position from the position / posture measurement information. A homogeneous transformation matrix T _mea (Tm in claims 1 and 2) of a vector representing the position and orientation of the measurement point t with respect to the measurement coordinates (predetermined unique coordinates) when attached is calculated. Further, the position / posture of the measurement point t with respect to the measurement coordinates when the weights 24, 25 are attached for each operation position (position calculation means), and a homogeneous transformation matrix T _rob ( Tc in the first and second aspects is obtained (step S6: homogeneous transformation matrix calculation means).

A method for obtaining the homogeneous transformation matrix T _rob by calculation is as follows. First, 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 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 these, the position / posture ^flange T _tool of the measurement point with respect to the coordinates of the flange 9 is known, and the relationship between the measurement coordinates and the robot coordinates can be measured.
^meabase T _{robbase is} also known. The ^robbase T _flange can be calculated using the rotation angle information of each rotary joint shaft 11 and the torque acting on each rotary joint shaft 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). Note that i is an integer from 1 to 6 indicating the first axis to the sixth axis (Lc-1 to Lc-6), and each axis Lc-1 to parameter θ, a, b, d, α, β is set. The value of Lc-6 (each rotary joint axis 11) is shown in the parameter table of [Equation 2] (b).

In the definition of the above parameters, ai is an offset in the X-axis direction of coordinates of the i-axis (i-arm) and i + 1-axis (i + 1 arm), bi is an offset in the Y-axis direction, and di is an 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. Rotation angle (deflection angle; rad) due to deflection around each coordinate axis of ZXY of each rotating joint axis _{11 s zi, s xi, s} yi is, Z coordinate axes around each rotary joint axis 11, X axis around around the Y axis The applied torque (Nm) is t _zi , t _xi , t _yi , and the predicted value of the spring constant (Nm / rad) around each coordinate axis of zxy of each rotary joint axis 11 stored as a parameter in the storage unit 16 is 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

Here, if the reciprocal number of the predicted value of the spring constant is set to k _z0i , k _x0i , k _y0i , the rotation angle of each rotary joint axis 11 around each coordinate axis of ZXY is the reciprocal number k _z0i , k of the predicted value of the spring constant. _It is expressed as follows using _x0i and _ky0i .
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 in consideration of the deflection angle is substituted into the equations (3), (5), and (6), and the homogeneous transformation matrix between the axes can be obtained by the equation (2). it can. 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.

The torques t _zi , t _xi , and _ty i are based on the mass of the arms 4 to 9 and on the weights 24 and 25. Since it is difficult to measure the mass of each arm 4-9 and its center of gravity, the torque due to the mass of each arm 4-9 is an unknown value. However, the torque by the weights 24 and 25 can be calculated from the distance from each rotary joint axis 11 to each weight 24 and 25 and the rotation angle of each rotary joint axis 11 detected by the joint angle detector 15. The distance from each rotary joint axis 11 to each of the weights 24 and 25 is calculated from the position vector from each rotary joint axis 11 to the flange coordinates and the flange coordinates that can be calculated using the parameters of [Equation 2] (d). It can be calculated from the position vector up to the center of gravity of the weights 24 and 25.

Therefore, in this embodiment, the deflections s _zi , s _xi, and s _yi of each rotary joint shaft 11 obtained by the equations (10) to (12) are obtained in consideration of only the torque due to the weights 24 and 25. Then, the rotation angles θi, αi and βi of the respective rotary joint shafts 11 taking into account the deflection angles are obtained from the s _zi , s _xi and s _yi by the equations (13) to (15), and θi, αi and βi are obtained. Substituting into the equations (3), (5) and (6), the simultaneous conversion matrix between the axes is obtained by the equation (2). Then, the position / posture of the flange 9 with respect to the robot coordinates of the equation (1) is 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).
T _rob = ^meabase T _robbase / ^robbase T _flange / ^flange T _tool (16)

なお、（１８）式および（１９）式において、右辺の括弧の右肩に小さくｔで示す符号は、この行列が横書きされているが、実際には転置行列であることを示している。 T _rob obtained by calculation as described above is the reciprocal k _all of the predicted values of all spring constants and the torque t by the weights 24 and 25 as shown in the following equations (17) to (19). It can be said that it is a function of _all .

In Equations (18) and (19), a small symbol indicated by t on the right shoulder of the right parenthesis indicates that this matrix is written horizontally but is actually a transposed matrix.

When different weights 24 and 25 are attached at the same operation position, the deflection angle differences Δs _zi , Δs _xi and Δs _yi of the rotary joint shafts 11 are expressed by the following equations (20) to (22). .

In the equations (20) to (22), the torque t _highioad when the weight 25 is large and the torque t _lowload when the weight 24 is small are known as described above. Thus, the difference between the homogeneous transformation matrix T _Highioad by calculation, a homogeneous transformation matrix following T _Lowload by calculating the time of the weight 24 made of a small mass in the case of the above (17) the mass of the large consisting weights 25 represented by Can be said to be a function of the reciprocal number k _all of _all spring constants as in the following equation (23).

  Here, the position / posture error ΔT obtained by measuring the measurement point t at the operating position and the calculated position / posture error ΔT are approximated by a linear combination sum of the minute fluctuations Δk of the reciprocal of the spring constant as shown in the following equation (24). Is done.

When the above equation (24) is changed to an equation paying attention to the difference between the error ΔT _highload in the weight 25 with a large mass and the error ΔT _lowload in the weight 24 with a small mass, the following equation (25) is obtained.

An error matrix ΔT indicating the difference between the simultaneous conversion matrix T _mea of the measurement point t obtained by measurement and the homogeneous conversion matrix T _{rob of} the measurement point t obtained by calculation for each of the weights 24 and 25 at one operating position. _highioad and ΔT _lowload can be obtained by the following equation (26). In the equation (26), ΔT _highioad and ΔT _lowload are indicated by ΔT _matrix .

As described above, the error ΔT between the position / orientation obtained by measuring the measurement point t at the operating position and the calculated value is the linear combination sum of the minute fluctuations Δk of the reciprocal of the spring constant as shown in the equation (24). _Therefore , if the difference between ΔT _highioad and ΔT _lowload is small, it can be said that the initial predicted value of the spring constant is close to the true value.

Therefore, the spring constant identification device 19 _expands the difference between ΔT _highioad and ΔT _lowload to all operating positions and _obtains the total (error sum) ΔT _all by the following equation (29) (step S7: error sum matrix calculation). means).

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 value ε (step S8: determination means). Constant prediction values K _z0i , 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 an appropriate value predicted in advance (step S9). 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 exceeds the threshold value ε (“NO” in step S8), the spring constant identification device 19 increments the estimated number counter x (step S11), and then x is set to a predetermined value. It is determined whether it is less than the upper limit number Xmax (step S12), and if it is less (“YES” in step S12), the operator is prompted to select a spring constant to be identified. 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 (step S13). ). For example, a rotary joint shaft supported at both ends and a rotary joint shaft having a large diameter may be treated as having zero deflection about both axes xy, and the spring constant in that case is the spring constant to be identified. Can be excluded. Of course, you may select as a spring constant which identifies all the spring constants.

Here, the error ΔT between T _mea and T _rob is represented by the 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 of the position vector from the measurement coordinates. (30). Further, when the Jacobian matrix J is expressed using an orientation vector, an approach vector, and a position vector, as shown by the following equation (31), the normal vector, the orientation vector, and the approach vector of the measurement point obtained by calculation are expressed. of e.g. Orient vectors, approach each coordinate axis of the measurement coordinate vector Zm, Xm, the position o _x on _{Ym, o y, o z,} a x, a y, a z, and measuring the coordinates of the position vector of the measuring point each coordinate axis Zm of, Xm, the position p _x on Ym, p _y, represented by the matrix of divided by the reciprocal k ₁ to k _m of spring constant to be identified p _z.

  The normal vector, the orientation vector, and the approach vector have the following relationship (32).

  That is, if two of the three vectors are known, the remaining one can be calculated. From this, the above formulas (30) and (31) can be a combination of the following formulas (33) and (34), formulas (35) and (36).

Note that the _matrix of the subscript of ΔT in equation (26) 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 equation (25), that is, the error (ΔT _highioad −ΔT _lowload ) between ΔT _highioad and ΔT _{lowload at} each operation position is expanded to all the operation positions, the following equation (37) is obtained. When expressed simply, equation (38) is obtained. Note that h indicates the number of operating positions.

  When the above equation (38) is transformed into the following equations (41) and (42), a small variation matrix Δk of the reciprocal of the spring constant to be identified is represented by the Jacobian sum matrix A as shown in equation (43). It is expressed by the pseudo inverse matrix of

Therefore, the spring constant identification device 19 calculates the equation (43) to obtain Δk (step S14: Jacobian sum matrix calculation means, minute variation matrix calculation 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.

(43) .DELTA.k determined by equation, as in the following equation (44), the difference between the reciprocals k ₁ to k _m of reciprocal and the predicted value of the true value of the m spring constant K1~Km to be identified (micro variation) .DELTA.k _1, ~, since a matrix representing the .DELTA.k _m, the sum k _{v + 1} of the reciprocal k _v and .DELTA.k the predicted value of the spring constant is closer to the true value of the spring constant.

Therefore, the spring constant identifying unit 19, after calculating the .DELTA.k (step S16), and the k _v of the following equation (45), the matrix of the inverse k ₁ to k _m of the predicted value of the spring constant K1~Km to be identified 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).

After that, the spring constant identification device 19 uses the reciprocal of the updated predicted value for the spring constants K1 to Km to be identified as the reciprocal of the predicted value of the spring constant in the expressions (10) to (12), that is, in the expression (45). For spring constants that do not wish to be identified using k _{v + 1} , the inverses k _z0i , k _x0i , and k _y0i of the original predicted values are used as _they are, and θi, αi, βi is obtained, and θi, αi, βi are substituted into the equations (3), (5), and (6) to obtain a homogeneous transformation matrix between the axes according to the equation (2). Based on the homogeneous transformation matrix, the ^robbase T _flange for each of the weights 24 and 25 at each operation position is obtained again by the matrix (1) (position recalculation means).

Next, the spring constant identification device 19 re-determines T _rob from the equation (16) (step S6: homogeneous transformation matrix recalculation means) and re-determines the error ΔT with respect to T _mea previously obtained. Then, the sum ΔT _all of the errors ΔT is obtained again by the equation (29) (step S7: error sum matrix recalculation means), and it is determined whether the error sum ΔT _all is equal to or less than a predetermined threshold ε (step S8: Judgment means).

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 the threshold value ε, the spring constant identification device 19 increments the estimated number counter x (step S11), and again obtains a minute variation matrix Δk of the reciprocal number of the spring constant by the equation (43) (minute variation matrix reconstruction). calculating means), by substituting this Δk in equation (45), the (45) equation k _v by substituting the reciprocal k _{v + 1} spring constant updating to previously (45) the spring constant to be re-identified by formula The reciprocal number k _{v + 1} of the predicted value is updated (predicted value reciprocal matrix re-updating means). 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 ε. 45) in the expression of k _v, the operation of updating the inverse k _v predictive value of the spring constant to be further identified by substituting the reciprocal k _{v + 1} predictive value of the spring constant to be identified and updated earlier, the error ΔT Is repeatedly executed until the total sum ΔR becomes equal to or smaller than a predetermined threshold value ε (repetitive execution means).

When the error sum ΔT _all becomes equal to or less than a predetermined threshold, the updated k _{v + 1} is set as the reciprocal of the spring constant to be identified, and the spring constant is obtained from this k _{v + 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, the spring constant of each rotary joint shaft 11 can be identified by being divided into spring constants around each axis of zxy. 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, each time the robot arm 2 is operated, the position of the measurement point t for the two weights 24 and 25 is measured. Since the posture only needs to be measured by the coordinate measuring device 17, the measurement work becomes simple and there is no risk of mistakes.

  In addition, since the spring constant is obtained by using the torque difference resulting from the mass difference between the weights 24 and 25, when obtaining the deflection of each rotary joint shaft 11, the mass, center of gravity position, inertia, etc. of each arm 4-9 are determined. It is not necessary to consider the torque generated by the mass of each arm 4 to 9 that cannot be calculated unless it is known, and the calculation becomes easy.

[Second Embodiment]
In the first embodiment described above, the spring constant is identified using both the position and orientation of the measurement point t for each of the weights 24 and 25 for each operation 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.

  That is, in the equation (37), when attention is paid only to the position, the following equation (46) is obtained.

  The above equation (46) is expressed as the following equation (49), and when this equation (49) is modified, a minute variation of the reciprocal of the m spring constants to be identified represented by the above equation (43) is obtained. The matrix Δk is as shown in Equation (52).

Therefore, when the inverse number of the predicted value of the spring constant to be identified is given, the inverse number of the predicted value of the spring constant to be identified can be updated by the following equation (53), so that the error sum is the same as in the first embodiment. The spring constant can be identified by repeatedly calculating so that the value of Δ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 / orientation, an error ΔT (9 rows and 1 column as shown in equation (62) described later) between the measurement value obtained by the coordinate measuring device 17 and the calculated value is as follows. (54) is approximated by a linear combination sum of minute fluctuations Δa of the parameter a to be identified.

T _{mea in the} above equation (56) is a homogeneous transformation matrix based on measurement values by the coordinate measuring device 17 similar to that described in the first embodiment, and T _{rob in} (57) is a homogeneous transformation matrix based on calculated values. It is. 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 (58) and (59).

  The parameter a to be identified is expressed by the following equation (60), and the minute fluctuation Δa of the parameter a is expressed by the equation (61).

  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 (54) are expressed using the orientation vector and the approach vector, respectively. It becomes like (62) Formula and (63) Formula.

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

  The above expression (64) is expressed as the following expression (65), and it is desired to identify the expression when the expression (65) is transformed into the expressions (42) and (43) in the first embodiment. The minute variation matrix Δa of the parameter a is as shown in the equation (68).

Therefore, when the predicted value a _{0 of the} parameter to be identified is given to a _v in the following equation (69), 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 identification of ^meabase T _robbase and ^flange T _tool in the third embodiment may be performed simultaneously with the identification of the spring constant in the first embodiment.
The number of weights attached to the flange 9 is not limited to two, and may be three or more. In this case, (ΔT _highioad −ΔT _lowload ) may be obtained in the relationship of two weights.
The predicted values K _z0 , K _x0 , K _y0 of the spring constants Kz, Kx, Ky may be stored in the storage unit (storage means) of the spring constant identification device 19.
The target articulated robot is not limited to a six-axis robot.

  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, homogeneous transformation matrix 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 updating means, homogeneous transformation matrix recalculation 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, and the robot arm is moved to a plurality of different positions so that at least two weights having different masses for each operation position one by one. When attached to the tip, at each operating position, the rotation angle of each rotary joint axis is obtained, and each time the weight is changed, at least the position and the posture of the measurement point are measured by the measuring means,
For each weight attached to the tip of the robot arm at each operating position of the robot arm, the weight of the weight, the length of each arm, the predicted value of each spring constant of each rotary joint axis, and the rotary joint axis The position of the measurement point is calculated using the rotation angle of
For each weight at each operating position, the homogeneous transformation matrix Tm based on predetermined fixed coordinates for at least the position of the measurement point measured by the measuring means and the at least the position of the measurement point obtained by the calculation. Obtaining a homogeneous transformation matrix Tc based on predetermined fixed coordinates to obtain an error matrix ΔT of a difference between the two homogeneous transformation matrices Tm and Tc;
Of the error matrix ΔT determined for each weight at each operating position, the difference between the error matrix ΔT _highload for a weight with a large mass and the error matrix ΔT _lowload for a weight with a small mass is the plurality of the errors. An error sum matrix ΔR obtained by summing up the operating 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,
If the value of the error sum matrix ΔR exceeds the predetermined threshold, then
A spring constant to be identified is selected from the spring constants Kx, Ky, Kz of the rotary joint axes;
A Jacobian matrix J is obtained for a matrix k _v of the reciprocal number 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 calculation for each weight at each operating position. In addition, a Jacobian sum matrix A is obtained by summing the difference between the Jacobian matrix J _highload having a large mass and the Jacobian matrix J _lowload having a small mass with respect to each of the operation 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 matrix k _v of the inverse of the predicted value of the spring constant to be the identification by adding the micro fluctuation matrix Δk matrix k _v of the inverse of the predicted value of the spring constant to be the identification,
For the spring constant to be identified, a spring constant obtained from the matrix k _{v + 1} of the reciprocal of the updated predicted value is used, and for the spring constants other than the spring constant to be identified, the plurality of the plurality of Recalculating at least the position of the measurement point for each weight for each of the movement positions, and re-determining the homogeneous transformation matrix Tc for at least the position of the recalculated measurement point,
For the plurality of operating positions, the error matrix ΔT is calculated from the homogeneous transformation matrix Tm of the measurement points obtained by the measuring means for each weight and the homogeneous transformation matrix Tc of the measurement points obtained by the recalculation. _And the difference between the error matrix ΔT _highload for a weight with a large mass and the error matrix ΔT _lowload for a weight with a small mass among the error matrices ΔT for each weight previously obtained again The error sum matrix ΔR summed up for the operation position is obtained again,
Whether each of the spring constants to be identified is identified by repeating whether or not the value of the error sum matrix ΔR obtained again is equal to or smaller than the predetermined threshold value until the error sum matrix ΔR obtained again is equal to or smaller than the predetermined threshold value. A method for identifying a spring constant of a robot. 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 the predicted value of the spring constant;
When the robot arm is moved to a plurality of different positions and at least two weights having different masses for each operation position are attached to the tip of the robot arm one by one, the control unit of the robot arm at each operation position Information acquisition means for acquiring the rotation angle of each rotary joint axis from each time and acquiring at least the position of the measurement point and posture from the measurement means each time the weight is changed;
For each weight attached to the tip of the robot arm at each operating position of the robot arm, the weight of the weight, the length of each arm, the predicted value of each spring constant of each rotary joint axis, and the rotary joint axis Position calculation means for calculating the position of the measurement point using the rotation angle of
For each weight at each operating position, the homogeneous transformation matrix Tm based on predetermined fixed coordinates for at least the position of the measurement point measured by the measuring means and the at least the position of the measurement point obtained by the calculation. Homogeneous transformation matrix calculation means for obtaining a homogeneous transformation matrix Tc based on predetermined fixed coordinates;
An error matrix ΔT of a difference between the both homogenous transformation matrices Tm and Tc is obtained, and the error matrix ΔT _highload and the mass for a weight having a large mass among the error matrix ΔT obtained for each weight at the respective operation positions and the mass. An error sum matrix calculating means for obtaining an error sum matrix ΔR obtained by summing the difference between the error matrix ΔT _lowload with respect to a small weight of 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;
Identification means for identifying the predicted values of the spring constants Kx, Ky, and Kz of the rotary joint axes as the spring constants if the value of the error sum matrix ΔR is equal to or less than the predetermined threshold;
Selecting means for selecting a spring constant to be identified from among the spring constants of the operating positions when the value of the error sum matrix ΔR exceeds the predetermined threshold;
A Jacobian matrix J is obtained for a matrix k _v of the reciprocal number 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 calculation for each weight at each operating position. A Jacobian sum matrix calculating means for obtaining a Jacobian sum matrix A in which the difference between the Jacobian matrix J _highload having a large mass and the Jacobian matrix J _lowload having a small mass is summed for each of the operation 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 matrix k _v of the inverse of the predicted value of the spring constant to be the identification by adding the micro fluctuation matrix Δk matrix k _v of the inverse of the predicted value of the spring constant to be the identification ,
For the spring constant to be identified, a spring constant obtained from the matrix k _{v + 1} of the reciprocal of the updated predicted value is used, and for the spring constants other than the spring constant to be identified, the plurality of the plurality of Simultaneous conversion matrix recalculating means for recalculating at least the position of the measurement point for each weight for each operation position and recalculating the homogeneous transformation matrix Tc for at least the position of the recalculated measurement point;
For the plurality of operating positions, the error matrix ΔT is calculated from the homogeneous transformation matrix Tm of the measurement points obtained by the measuring means for each weight and the homogeneous transformation matrix Tc of the measurement points obtained by the recalculation. _And the difference between the error matrix ΔT _highload for a weight with a large mass and the error matrix ΔT _lowload for a weight with a small mass among the error matrices ΔT for each weight previously obtained again Error sum matrix recalculating means for re-determining the error sum matrix ΔR totaled for the operating position;
Whether each of the spring constants to be identified is identified by repeating whether or not the value of the error sum matrix ΔR obtained again is equal to or smaller than the predetermined threshold value until the error sum matrix ΔR obtained again is equal to or smaller than the predetermined threshold value. An apparatus for identifying a spring constant of a robot, comprising:

Images