JP2013039643A - Method of detecting inter-axis offset of six-axis robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To measure a displacement amount of inter-axis offset and correct it in a six-axis robot.SOLUTION: Fingers are tipped with light emitting diodes to make a tip two-axis orthogonal intersecting point (intersecting point of a fifth axis and a sixth axis) move to a plurality of moving target positions (white circles) on the X axis of a robot coordinate. A rotary locus is measured by a three-dimensional measurement unit by rotating the light emitting diode around the sixth axis at respective moving target positions and a rotary locus is measured by the three-dimensional measurement unit by rotating the light emitting diode around the fifth axis. Two rotation center lines are obtained by the two rotary loci and the intersecting point of both the rotation center lines is made to be an actual moving position (an×mark) of the tip two-axis orthogonal intersecting point and an inter-axis offset amount F is detected from an error between the moving target position and the actual moving position. A DH parameter is corrected by the inter-axis offset amount F.

Description

  The present invention relates to a method for detecting an offset between axes of a six-axis robot that detects and corrects an offset between a first rotary joint having rotational axes in directions orthogonal to each other and second, third, and fifth rotary joints.

  When the 6-axis robot is given position data represented by fixed three-dimensional Cartesian coordinates, the 6-axis robot converts the position data into rotational joint angle data and moves the hand to the position indicated by the position data. It is configured. In this case, the link (arm) length, the angle between the rotation joint and the rotation axis of the next rotation joint (hereinafter referred to as the torsion angle), the link origin position and the motor origin due to machining errors or assembly errors of the robot components. If there is an error in the relationship with the position (hereinafter referred to as the motor origin position), or if the drive system bends (hereinafter simply referred to as bend) due to the additional torque acting on the drive system of each rotary joint, the position of the hand And the posture is displaced, and the absolute position accuracy is lowered.

  In order to improve the absolute position accuracy, various methods for correcting the rotation angle of the rotary joint in consideration of the above-described error and deflection have been considered so far. For example, Non-Patent Document 1 discloses a method of detecting an error regarding a link length and a twist angle and correcting the error. Patent Documents 1 to 4 disclose error detection and correction methods for the motor origin position. Regarding the deflection of the drive system, the load torque acting on the motor of each rotary joint is calculated from the mass of each link and the posture of each link, and further, the torsional deformation of the drive system is calculated from this load torque and the spring constant of the drive system. There is known a method of correcting the deflection of the drive system by obtaining an angle, obtaining a deflection angle of each link from the twist deformation angle, and obtaining a motor rotation angle that reduces the deflection angle of each link.

Japanese Patent Laid-Open No. 2003-220587 JP 2009-274186 A JP 2009-274187 A JP 2009-274188 A

Issued by the Japan Robot Industry Association Commissioned by the Ministry of International Trade and Industry, Institute of Industrial Science and Technology, 1997 Survey and research on standardization of intelligent robots for plants Result report Page 52 4.3.2.3 Paired-even identification method

  The inventor obtained the link length error, the torsion angle error, the motor origin position error, the deflection of the drive system using the above method for the 6-axis robot, and after correcting the rotation angle of each rotary joint from the results, An experiment was conducted to determine whether the hand moves to the position indicated in the position data. However, the result of this experiment did not satisfy the absolute position accuracy originally planned by the inventor.

  The present invention has been made in view of the above circumstances, and the object of the present invention is still satisfactory even when the above-described link length error, torsion angle error, motor origin position error, and drive system deflection are corrected in a 6-axis robot. It is an object to further improve the absolute position accuracy of the robot by estimating a factor that could not obtain the absolute position accuracy and providing a method for measuring an error amount due to the estimated factor.

  The present inventor has factors that cause position errors other than link length error, torsion angle error, motor origin position error, and drive system deflection as the reason for the lack of absolute position accuracy, and this is the offset between axes. I guessed it.

Here, when the positions of the second, third, and fifth rotary joints after assembly are shifted from the normal positions in the direction of the rotation center line, this shift is referred to as an inter-axis offset.
That is, if the second, third, and fifth rotary joints deviate from their normal positions due to processing errors and assembly errors of the robot component parts, the coordinates of the second, third, and fifth links are designed. And the rotation axis of each of the second, third, and fifth rotary joints will shift, and as a result, the robot hand moves from the target position specified by the position on the robot coordinates. It will shift by the offset.
Therefore, if the offset between axes is measured and corrected, the absolute position accuracy can be further increased. However, conventionally, no proposal has been made to measure the offset between the axes considering the relationship between the offset between the axes and the absolute position accuracy.

The method of offset between axes according to the present invention is as follows.
(Claim 1)
In the first aspect of the invention, first, a measurement point that operates integrally with the sixth link is provided, and the robot arm is rotated about the rotation axis of the first rotary joint, so that a plurality of three or more on the rotation locus of the measurement point are provided. The position is measured by a three-dimensional measuring means, and the rotation center position of the measurement point and the normal of the plane including a plurality of positions on the rotation locus of the measurement point are obtained from the measurement result, and the rotation center of the measurement point is the origin. , A straight line parallel to the normal passing through the origin is the Z axis, an arbitrary straight line passing through the origin and orthogonal to the Z axis is an X axis, and a straight line passing through the origin and perpendicular to both the Z and X axes is the Y axis. Define the reference coordinates.

  Next, an arbitrary plurality of positions on an arbitrary plane extending from the rotation center line of the rotation axis of the first rotation joint are determined as the movement target position, and the rotation center of the rotation axis of the fifth rotation joint is set to the movement target position. First to fourth rotation axes including the rotation axes of the first to fourth rotation joints, when the tip 2-axis orthogonal point, which is the orthogonal point between the line and the rotation center line of the rotation axis of the sixth rotation joint, is moved. Origin position error of the motor that drives each of the first to fourth links via each of the four drive systems, deflection of each of the first to fourth rotation drive systems, and a length error of each of the first to fourth links Correction processing is performed so that the position error of the tip 2-axis orthogonal point does not occur due to the position error due to the angle and the error in the angle between the rotation joint and the rotation axis of the next rotation joint.

  Then, the tip 2-axis orthogonal point is moved to a plurality of movement target positions, the rotation axis of the sixth rotary joint is rotated at each movement target position, and the measurement point moving on the circular rotation locus by this rotation is measured. Three or more different positions that are different from each other are measured by the three-dimensional measuring means, and from the three or more positions on the rotation locus of the measured measurement point, the center position of the rotation locus of the measurement point and the rotation locus of the measurement point are measured. An operation for obtaining a normal line of a plane including three or more positions and obtaining a first straight line parallel to the normal line through the center of the rotation locus of the measurement point, and rotating a rotation axis of the fifth rotation joint The three or more different positions of the measurement points that move on the circular rotation locus by this rotation are measured by the three-dimensional measuring means, and the measurement points are measured from the three or more positions on the rotation locus of the measured measurement points. The center position of the rotation trajectory and the measurement point Performing the operation for obtaining a second straight line parallel to the normal line through the center of rotation locus of measured points seeking and the normal of the plane including the three or more positions on the rolling path.

Then, the intersection of the first straight line and the second straight line is set as the measurement position of the tip 2-axis orthogonal point, the position of the tip 2-axis orthogonal point is coordinate-converted to a position on the reference coordinate, and the reference of the tip 2-axis orthogonal point The XbYb coordinate value on the coordinates is obtained, the XbYb coordinate value of the obtained biaxial orthogonal point at the tip is plotted on the XbYb plane of the reference coordinates, and the line segment connecting these plotted points is defined as a measurement dependent line segment. The length of the perpendicular line drawn from the origin of the reference coordinates to the straight line obtained by extending the line segment is obtained as a deviation amount. This deviation amount is the total offset amount between the axes of the second, third, and fifth rotary joints.
The absolute position accuracy can be improved by correcting the DH parameter, for example, based on the offset amount between the axes obtained as described above.

(Claim 2)
The invention of claim 2 is different from claim 1 in the way of obtaining the reference coordinates. That is, according to claim 2, the first link is rotated to three or more arbitrary angular positions different from each other while the rotation axes of at least the second to fourth rotational joints are fixed, and the sixth rotational joint at each angular position. The rotation axis is rotated, and three or more different positions of the measurement points that move on the circular rotation locus by this rotation are measured by the three-dimensional measuring means, and the three or more positions on the rotation locus of the measured measurement points are measured. From the plurality of positions, the normal position of the plane including the center position of the rotation locus of the measurement point and the three or more positions on the rotation locus of the measurement point is obtained, and the normal line passes through the center of the rotation locus of the measurement point. An operation for obtaining a parallel first straight line and a rotation axis of a fifth rotary joint are rotated, and three or more different positions of measurement points moving on a circular rotation locus by this rotation are measured by a three-dimensional measuring means. Of the measured measurement points From the three or more positions on the rolling locus, the center position of the rotation locus of the measurement point and the normal of the plane including the three or more positions on the rotation locus of the measurement point are obtained, and the rotation locus of the measurement point is calculated. An operation for obtaining a second straight line that passes through the center and is parallel to the normal line is performed, and the intersection of the obtained first straight line and the second straight line is set as a measurement position of the tip 2-axis orthogonal point.

  Then, from the measurement positions of the three or more tip 2-axis orthogonal points obtained at each angular position, the center position of the arc passing through the measurement positions of the plurality of tip 2-axis orthogonal points, and the plurality of tip 2-axis orthogonal points The plane normal line including the measurement position is obtained, and the center of the arc passing through the measurement positions of the plurality of tip two-axis orthogonal points is the origin, and the plane including the measurement positions of the plurality of tip two-axis orthogonal points through the origin The reference coordinates are defined with the straight line parallel to the normal line of the Z axis, the straight line passing through the origin and perpendicular to the Z axis as the X axis, and the straight line passing through the origin and perpendicular to both the Z and X axes as the Y axis. It is characterized by that.

(Claim 3)
The invention according to claim 3 moves in any of a plurality of positions within the movable range of the tip 2-axis orthogonal point in place of the operation until the amount of offset between the axes is obtained after setting the reference coordinates in claim 1 or 2. When the target position is set and the tip 2-axis orthogonal point is moved to these movement target positions, the origin position error of each of the motors described above, the bending of the first to fourth rotary drive systems, the first to fourth After performing correction processing so that the position error of the two-axis orthogonal point is not generated due to the length error of each link and the angle error between the rotation axes of the rotation joint and the next rotation joint, Rotating the rotational axis of the rotary joint to move the tip 2-axis orthogonal point to each of a plurality of movement target positions, and obtaining the first straight line at each movement target position in the same manner as described above, The operation of obtaining a straight line is performed, and the first straight line and the second straight line The intersection point is the measurement position of the tip 2-axis orthogonal point, the measurement position of the tip 2-axis orthogonal point is coordinate-converted to the position on the reference coordinate, and the XY coordinate value on the reference coordinate is obtained. Plot the XY coordinate value of the movement target position on the XY plane and the XY coordinate value of the measurement position of the tip 2-axis orthogonal point, and calculate the length of the line segment from the reference coordinate origin to the movement target position on the XY plane. Along with the target position radius, the length of the perpendicular drawn from the measurement position of the two-axis orthogonal point on the XY plane to the straight line passing through the origin of the reference coordinates and the movement target position is used as an offset error component, and each movement target position is The relationship between the target position radius and the offset error component is plotted in a graph with the target position radius on one of the two orthogonal axes and the offset error component on the other axis, and a straight line passing through these plotted points is the error. straight In this case, the length of the perpendicular line drawn from the origin of the graph to the error straight line is obtained as a deviation amount, and the obtained deviation amount is set as the total inter-axis offset amount of the second, third, and fifth rotary joints. And
Even if it does in this way, the effect similar to Claim 1 can be acquired.

(Claim 4)
In the invention of claim 4, after obtaining the reference coordinates in the same manner as in claim 1 or claim 2, the position of the measurement point when the hand is in an arbitrary reference posture is measured by the three-dimensional measurement means, and The rotation axis of the sixth rotation joint is rotated, and three or more different positions of the measurement points that move on the circular rotation locus by this rotation are measured by the three-dimensional measuring means, and the measured measurement points on the rotation locus are measured. From the three or more positions, the center position of the rotation locus of the measurement point and the normal of the plane including the three or more positions on the rotation locus of the measurement point are obtained and passed through the center of the rotation locus of the measurement point. The operation for obtaining the first straight line parallel to the normal line and the three-dimensional measurement of three or more different positions of the measurement point that moves on the circular rotation locus by rotating the rotation axis of the fifth rotary joint. Measured by means and the rotation trajectory of the measured measurement point From the three or more positions of the measurement point, the center position of the rotation locus of the measurement point and the normal of the plane including the plurality of positions on the rotation locus of the measurement point are obtained and passed through the center of the rotation locus of the measurement point. The second straight line parallel to the normal is obtained, and a vector from the measurement position of the measurement point in the reference posture of the hand to the intersection of the first straight line and the second straight line is obtained, and the vector is the tip. In addition to the detection vector of the biaxial orthogonal point, an angle formed by the detection vector and the XYZ axes of the reference coordinates is obtained, and the angle is set as a directivity angle.

  Then, a plurality of arbitrary positions on an arbitrary plane extending from the rotation center line of the rotation axis of the first rotation joint are set as movement target positions, and the rotation center line of the rotation axis of the fifth rotation joint is set at these movement target positions. When the tip 2-axis orthogonal point that is orthogonal to the rotation center line of the rotation axis of the sixth rotation joint is moved, the first to fourth axes including the rotation axes of the first to fourth rotation joints are moved. Origin position error of the motor driving each of the first to fourth links via each rotation drive system, deflection of each of the first to fourth rotation drive systems, length error of each of the first to fourth links, In addition, by performing correction processing so that the position error of the two-axis orthogonal point is not caused by the error in the angle between the rotation axis of the rotation joint and the next rotation joint, the second and third rotation joints are rotated. , Move the tip 2-axis orthogonal point to each of the multiple movement target positions, Then, the position of the measurement point is measured by the three-dimensional measuring means with the hand held in the reference posture, the position of the measured measurement point is converted to the position on the reference coordinate, and the position of the specific point on the reference coordinate When the detection vector is created so as to have a directivity angle from the position, the position of the end point of the detection vector is determined as the measurement position on the reference coordinate of the tip biaxial orthogonal point, and on the reference coordinate of the obtained plural tip biaxial orthogonal points The XY coordinate value of the measurement position is plotted on the XY coordinate plane of the reference coordinate, the line segment connecting these plotted points is taken as the measurement dependent line segment, and the measurement dependent line segment is extended from the origin of the reference coordinate to the straight line. The length of the perpendicular is obtained as a deviation amount, and the obtained deviation amount is set as a total inter-axis offset amount of the second, third, and fifth rotary joints.

(Claim 5)
The invention of claim 5 is different from claim 4 in the way of obtaining the reference coordinates. That is, in the fifth aspect, as in the second aspect, the first link is rotated to three or more arbitrary angular positions different from each other, and the operation for obtaining the first straight line at each angular position and the second straight line are The obtained operation is performed, and the intersection of the obtained first straight line and the second straight line is set as the measurement position of the tip 2-axis orthogonal point.

  Then, from the measurement positions of the three or more tip 2-axis orthogonal points obtained at each angular position, the center position of the arc passing through the measurement positions of the plurality of tip 2-axis orthogonal points, and the plurality of tip 2-axis orthogonal points The plane normal line including the measurement position is obtained, and the center of the arc passing through the measurement positions of the plurality of tip two-axis orthogonal points is the origin, and the plane including the measurement positions of the plurality of tip two-axis orthogonal points through the origin The reference coordinates are defined with the straight line parallel to the normal line of the Z axis, the straight line passing through the origin and perpendicular to the Z axis as the X axis, and the straight line passing through the origin and perpendicular to both the Z and X axes as the Y axis. It is characterized by that.

(Claim 6)
The invention according to claim 6 is an arbitrary plurality of positions within the movable range of the tip 2-axis orthogonal point in place of the operation until the amount of offset between the axes is obtained after determining the reference coordinates in claim 4 or claim 5. Is defined as the movement target position, and the tip 2-axis orthogonal point is moved to each movement target position, the first to fourth rotation drive systems including the rotation axes of the first to fourth rotation joints are Origin position error of the motor that drives each of the first to fourth links, the deflection of the first to fourth rotational drive systems, the length error of each of the first to fourth links, and the rotation joint and the next The tip 2 is rotated by rotating the rotation shafts of the first to third rotation joints after performing correction processing so that the position error of the two-axis orthogonal point does not occur due to the error in the angle between the rotation axes of the rotation joints. An axis orthogonal point is moved to each of the plurality of movement target positions, and at each movement target position, a hand is moved. Is held in a reference posture, and the position of the measurement point is measured by the three-dimensional measuring means, the position of the measured measurement point is converted to a position on the reference coordinate, and the directivity angle is calculated from the position on the reference coordinate of the measurement point. When the detection vector is created as described above, the position of the end point of the detection vector is determined as the measurement position on the reference coordinate of the tip 2-axis orthogonal point, and the XY coordinate value of the measurement position on the reference coordinate of the tip 2-axis orthogonal point is determined. The movement target position is converted into a position on the reference coordinate, and each movement target position is measured on the XY plane of the reference coordinate on the measurement position on the reference coordinate of the tip 2-axis orthogonal point and the movement target position. Plotting the position on the reference coordinate, the length of the line segment from the origin of the reference coordinate to the movement target position on the XY plane is set as the target position radius, and from the measurement position of the tip 2-axis orthogonal point on the XY plane Reference coordinate origin and target position The length of the vertical line drawn down to the passing straight line is used as an offset error component, and for each moving target position, the relationship between the target position radius and the offset error component is determined by taking the target position radius on one of the two orthogonal axes and taking the other axis. When the straight line passing through the plotted points is plotted as an error line, the length of the perpendicular line drawn from the origin of the graph to the error line is obtained as the deviation amount, and the obtained deviation amount Is a total offset amount between the axes of the second, third, and fifth rotary joints.

(Claim 7)
The invention of claim 7 is any one of claims 1, 2, 4 and 5, wherein a plurality of positions on one plane extending from the rotation center line of the rotation axis of the first rotary joint are determined as the movement target positions. The operation of moving the tip 2-axis orthogonal point to these movement target positions is performed on two straight lines extending in the opposite directions from the Z-axis of the reference coordinates, and in the opposite direction from the straight line extending in one direction from the Z-axis. When the rotation axis of the rotary joint of the second and third rotary joints is rotated while the rotary axis of the first rotary joint is fixed, the robot arm is moved. It is characterized by being reversed.

A measurement dependency line segment that connects the measurement positions of the tip 2-axis orthogonal point when the tip 2-axis orthogonal point is moved to any position on one plane and the robot arm on the other plane by inverting the robot arm The measurement-dependent line segment formed by connecting the measurement positions of the tip 2-axis orthogonal point when the tip 2-axis orthogonal point is moved to any of a plurality of positions is to be located on a straight line. If it is not positioned on a straight line, it depends on the method of setting the reference coordinates, the origin position error of the motor, the bending of each of the first to fourth rotary drive systems, or the length error of each of the first to fourth links. It is thought that there was an error in the technique for preventing the position error of the hand from occurring.
For this reason, according to the invention of claim 7, it is possible to confirm whether or not the offset amount between the axes is correctly obtained.

(Claim 8)
An eighth aspect of the present invention is the robot according to the third or sixth aspect, wherein the movement of moving the tip biaxial orthogonal point to the movement target position includes the forward bending posture of the robot arm and the backward bending of the robot arm reversed from the forward bending posture. It is characterized by performing both of the postures.

  By doing so, when plotting the relationship between the target position radius and the offset error component for each moving target position on the graph, the plotted points are positioned in a straight line between the forward bending position and the backward bending position. Whether or not the inter-axis offset is correctly obtained can be confirmed in the same manner as in the seventh aspect.

The perspective view of the robot apparatus which shows the 1st Embodiment of this invention Sectional view showing the outline of the rotary joint Side view of the robot with the rotation axis of each rotary joint set to the origin position Perspective view showing coordinates set for each link Table showing DH parameters Table showing specific values of DH parameters Block diagram showing the controller Perspective view of flange part Process diagram showing the procedure of the present invention Perspective view for explaining setting of reference coordinates Explanatory drawing of deflection of rotary drive system Perspective view showing the movement target position Side view showing the forward bending state of the robot Side view showing back-bending state of robot Explanatory drawing for obtaining the first straight line Explanatory drawing for obtaining the second straight line Explanatory drawing for obtaining the tip 2-axis orthogonal point from the first straight line and the second straight line The figure which shows the simultaneous conversion matrix for converting a camera coordinate into a reference coordinate Explanatory drawing for obtaining the offset amount between axes FIG. 10 equivalent view showing the second embodiment of the present invention The figure which shows the 3rd Embodiment of this invention, and shows the movement target position of the front-end | tip biaxial orthogonal point, and an actual movement position Explanatory drawing for obtaining the offset amount between axes The 4th Embodiment of this invention is shown, (a) is a side view of a flange part, (b) is a bottom view. Perspective view showing detection vector

(First embodiment)
Hereinafter, a first embodiment of the present invention will be described with reference to FIGS. FIG. 1 shows a 6-axis vertical multi-rotation joint type robot apparatus 1. The robot apparatus 1 includes a 6-axis vertical articulated robot (hereinafter simply referred to as a robot) 2, a control apparatus (robot control means) 3 that controls the operation of the robot 2, and a robot 2 connected to the control apparatus 3. Is provided with a teaching pendant (manual operation means) 4.

  The robot 2 has a shoulder (first link) 7, a lower arm (second link) 8, and a base (base link) 5 fixed to an installation surface, for example, a horizontal floor surface of a factory. The first upper arm (third link) 9, the second upper arm (fourth link) 10, the wrist arm (fifth link) 11, and the flange (first link) 12 are rotated in the first to sixth rotations. The joints J1 to J6 (not shown in FIG. 1) are sequentially connected.

  That is, the shoulder 7 is connected to the base 5 by the first rotary joint J1 so as to be pivotable in the horizontal direction around the first axis Lc-1, and the lower arm 8 is connected to the tip of the shoulder 7 by the second rotary joint J2. The first upper arm 9 is supported so as to be pivotable in the vertical (vertical) direction around the two axes Lc-2, and the first upper arm 9 is vertically moved around the third axis Lc-3 by the third rotary joint J3 at the tip of the lower arm 8. The second upper arm 10 is supported so as to be rotatable about the fourth axis Lc-4 by the fourth rotary joint J4 at the distal end portion of the first upper arm 9, and the wrist arm 11 is supported. Is supported at the tip of the second upper arm 10 by a fifth rotary joint J5 so as to be pivotable in the vertical direction about the fifth axis Lc-5, and the flange 12 is supported on the wrist arm 11 by a sixth rotary joint J6. Supports rotation by twisting around the axis Lc-6 It is.

  The outline of the first to sixth rotary joints J1 to J6 is shown in FIG. As shown in FIG. 2, the rotary joints J <b> 1 to J <b> 6 of each of the links 7 to 12 include a rotary shaft 14 that is rotatably supported by a previous-stage link via a bearing 13, and the next-stage link is connected to the rotary shaft 14. They are connected directly or via a joint material. The rotary shaft 14 is rotationally driven by a servo motor 15 as a drive source fixed to the previous-stage link via a speed reducer, for example, a gear speed reducer 16, and the rotation of the rotary shaft 14 causes the next-stage link to turn or twist. It is designed to rotate.

  Here, the first axis Lc-1 to the sixth axis Lc-6 correspond to the rotation center line Lc of the rotary shaft 14 of each of the first to sixth rotary joints J1 to J6. The directivity directions of the first axis Lc-1 to the sixth axis Lc-6 are as follows. That is, the first axis Lc-1 that is the rotation center of the shoulder 7 is orthogonal to the installation surface, and the second axis Lc-2 that is the rotation center of the lower arm 8 is parallel to the direction orthogonal to the first axis Lc-1. That is, assuming that the base 5 is installed on the floor of a factory, for example, the first axis Lc-1 is set to be perpendicular (vertical) to the installation floor and the second axis Lc-2 is set to be horizontal.

  The third axis Lc-3 that is the rotation center of the first upper arm 9 is parallel or horizontal to the second axis Lc-2, and the fourth axis Lc-4 that is the rotation center of the second upper arm 10 is the second axis Lc-2. The fifth axis Lc-5, which is parallel to the direction orthogonal to the three axes Lc-3 and is the rotation center of the wrist arm 11, is orthogonal to the fourth axis Lc-4, and the sixth axis Lc-6, which is the rotation center of the flange 12. Is defined in a direction orthogonal to the fifth axis Lc-5. The fourth axis Lc-4 and the fifth axis Lc-5 are orthogonal, and the fifth axis Lc-5 and the sixth axis Lc-6 are also orthogonal points of the fourth axis Lc-4 and the fifth axis Lc-5. Are orthogonal at the same point.

  When the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, and the servo motor 15 that drives each rotary shaft 14 of the flange 12 are at the origin position, the shoulder 7, lower The arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 are at the origin position. Here, when the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 are all at the origin position, the robot arm 6 is vertical as shown in FIG. The posture is upward (the lower arm 8, the first upper arm 9, the second upper arm 10, and the wrist arm 11 are vertical). Then, each servo motor 15 rotates in the positive (plus) direction and the reverse (minus) direction from the origin position, so that the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, and the wrist arm. 11 and the flange 12 also rotate in the positive (plus) direction and the reverse (minus) direction from the origin position.

  As shown in FIG. 4, three-dimensional coordinates R1 to R6 are defined for the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12, respectively. . The origins O1 to O6 of these coordinates R1 to R6 are the first axis Lc that is the rotation center line of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12. -1 to the sixth axis Lc-6 are determined at predetermined positions, and the Z1 to Z6 axes that are the Z axes of the coordinates R1 to R6 coincide with the first axis Lc-1 to the sixth axis Lc-6. . In particular, one of the origin O5 of the coordinate R5 of the wrist arm 11 and the origin O6 of the coordinate R6 of the flange 12, in this embodiment, both the origins O5 and O6 are both of the fifth axis Lc-5 and the sixth axis Lc-6. It is set to be located at an orthogonal point.

  In addition, among the coordinates R1 to R6, the X1 axis that is the X axis of the coordinate R1 of the shoulder 7 is horizontal, and is a common vertical line (indicated by h line in FIG. 1) of the first axis Lc-1 and the second axis Lc-2. .), The Y1 axis, which is the Y axis, is horizontal, and is determined in the direction determined by the outer product of the vectors of the Z1 axis and the Y1 axis (a direction orthogonal to both the Z1 axis and the Y1 axis). ing. The X2 to X6 axes that are the remaining X coordinates of R2 to R6 are coordinates in a state where the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 are moved to the origin position. R1 is defined in a direction parallel to the X1 axis, and Y2 to Y6 axes, which are Y axes, are determined in a direction obtained by an outer product of vectors of Z2 to Z6 axes and Y2 to Y6 axes (see FIG. 4). . In addition, the plus directions of the X1 to X6 axes, the Y1 to Y6 axes, and the Z1 to Z6 axes are directions indicated by arrows in FIG.

  Coordinates R0 and Rf are also defined on the tip surface of the flange 12 which is the tip of the base 5 and the robot arm 6. While the coordinates R1 to R6 change positions and orientations with the rotation of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12, the base A coordinate R0 of 5 is immobile and is a robot coordinate. The origin O0 of the robot coordinate R0 is located on the first axis Lc-1 that is the rotation center line of the shoulder 7, and in this embodiment is determined at a position that coincides with the origin O1 of the coordinate R1. Further, the Z0 axis that is the Z axis coincides with the first axis Lc-1, and the X0 axis that is the X axis and the Y0 axis that is the Y axis are the coordinates R1 of the shoulder 7 when the shoulder 7 is at the origin position. It corresponds to the X1 axis and the Y1 axis.

  The coordinate Rf of the front end surface of the flange 12 is a hand coordinate, and the origin Of is on the sixth axis Lc-6, in this embodiment, the intersection of the sixth axis Lc-6 and the front end surface of the flange 12, that is, FIG. As shown in FIG. 5, the Zf axis that is the Z axis is determined to coincide with the center P of the front end surface forming the circle of the flange 12, the Xf axis that is the X axis, and the Y axis that is the X axis. Yf axis is defined in parallel with the X6 axis and the Y6 axis of the coordinate R6 of the flange 12. The center P of the front end surface of the flange 12 is set to the hand that is the front end of the robot arm 6. The flange 12 has a center hole 12a and a small hole 12b formed at a position separated from the center hole 12a. The center of the center hole 12a is P, and between the center points of the center hole 12a and the small hole 12b. Is defined to be the Xf axis of the hand coordinate Rf.

  In the present embodiment, the origins O1 to O6 and Of of the coordinates R0 to R6 and Rf described above are assumed to be a single plane (vertical Z1X1 plane) including the X1 axis and the Z1 axis of the coordinate R1. Located on the top. When the robot arm 6 is operated, the origins O2 to O6 and Of of the coordinates R2 to R6 and Rf are set so as to move on the Z1X1 plane.

  The mutual relationship between the coordinates R0 to R6 and Rf defined as described above is indicated by a well-known DH parameter. FIG. 5 summarizes the DH parameters in a table. For example, for the J1 row, the coordinate R1 is translated a1 in the X1 axis direction, not translated in the Y1 axis direction, and d1 parallel in the Z1 axis direction. After the movement, if the X1 axis is rotated about the twist angle −π / 2, the coordinate R2 is obtained. For example, for the J6 line, the coordinate R6 is set in the X6 axis direction and the Y6 axis direction. It shows that the hand coordinates Rf can be obtained by performing d6 translation in the Z6 axis direction without translation. The relationship between the robot coordinate R0 and the coordinate R1 of the shoulder 7 is not shown in FIG. 5 because the coordinate R1 coincides with the robot coordinate R0 when the shoulder 7 is at the origin position. FIG. 6 shows specific numerical examples of a, b, and d.

  As shown in FIG. 7, the control device 3 includes a CPU (control means) 17, a drive circuit (drive means) 18, and a position detection circuit (position detection means) 19. The CPU 17 is connected to a ROM 20 for storing various data such as a robot language and DH parameters for creating a system program and an operation program for the entire robot 2, and a RAM 21 for storing the operation program for the robot 2. The teaching pendant 4 used when performing work or the like is connected. As shown in FIG. 1, the teaching pendant 4 includes various operation units 4a and a display 4b made of liquid crystal.

  The position detection circuit 19 is connected to a rotary encoder 22 as a rotation sensor coupled to a rotation shaft (not shown) of each servo motor 15. The position detection circuit 19 detects the rotation angles of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, and the flange 12 based on the rotation angle detection information of each rotary encoder 22. Then, the rotation angle detection information is given to the CPU 17.

  Here, the CPU 17 is given the position of the hand on the robot coordinate R0 (the position of the origin Of of the hand coordinate Rf) and the posture (directions of the Xf axis, the Yf axis, and the Zf axis of the hand coordinate Rf). The rotation angle of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, and the flange 12 that take the position and posture is known in inverse kinematics using a Jacobian matrix. It is calculated by the calculation method.

  Also, given the position of each coordinate R1 to R6 on the robot coordinate R0 (the position of the origins O1 to O6) and the posture (directions of the X axis, Y axis, and Z axis of each coordinate R1 to R6), The rotation angles of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, and the flange 12 such that R1 to R6 take their positions and postures are obtained.

  Conversely, given the rotation angles of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12, forward kinematics using coordinate transformation from these rotation angles. The coordinates R1 to R6 on the robot coordinate R0 and the position and posture of the hand can be obtained by the known calculation method. As is well known, the DH parameter indicating the mutual relationship between the coordinates R0 to R6 and Rf is used for the calculation by the inverse kinematics and forward kinematics calculation methods.

  When the CPU 17 acquires the target position of the hand from the operation program, the target movements of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 are obtained from the target position. The angle is calculated, and an operation command based on the target operation angle is given to the drive circuit 18. The drive circuit 18 drives each servo motor 15 based on the given operation command. At this time, the CPU 17 uses the rotation angle detection information of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 given from the position detection circuit 19 as feedback information. The servo motor 15 is feedback controlled.

  By the way, as described above, the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12, the coordinates R1 to R6, and the origins O1 to O6 of the hand coordinates Rf. , Of are present on a vertical plane including the X1 axis and the Z1 axis of the coordinate R1 of the shoulder 7. Therefore, if the robot 2 is assembled with high accuracy, the center P of the tip surface of the flange 12 that is the hand coincides with the origin Of of the designed hand coordinates Rf, and the shoulder 7, the lower arm 8, and the first upper arm If the arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 are rotated by the target operation angle, the hand is correctly moved to the target position.

  However, the position of each rotary shaft 14 of the rotary joints J2, J3, J5 of the lower arm 8, the first upper arm 9, and the wrist arm 11 due to manufacturing errors and assembly errors of the components of the robot 2 is the second axis Lc. -2, the third axis Lc-3, and the fifth axis Lc-5 are displaced in the direction of the normal position and an error (inter-axis offset) occurs, the actual hand (the center P of the front end surface of the flange 12) ) Is deviated from the origin of the designed hand coordinates Rf.

  Then, even if the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 are rotated by the target operating angle, the actual movement position of the hand is determined as the rotational joint J2. , J3 by the sum of the offsets between the axes 14 (when the fifth axis Lc-5 is vertical), or by the sum of the offsets between the axes 14 of the rotary joints J2, J3, J5 ( When the fifth axis Lc-5 is horizontal), the fifth axis Lc-5 is shifted in the horizontal direction from the target position. Since the rotational axis 14 of the rotary joint J5 is short, the offset between the axes of the rotary joint J5 alone is considered to be small, and the sum of the offsets between the rotary axes 14 of the rotary joints J2 and J3 and the rotary joints J2, J3 and J3. It is considered that there is almost no difference from the sum of the offsets between the axes of the rotary shafts J5.

  The method of the present invention estimates the sum of the offsets between the rotary shafts 14 of the rotary joints J2, J3, and J5, corrects the DH parameter by the estimated offset between axes, and sets the actual movement position of the hand to the target position. Try to get as close as possible.

  In the method of the present invention, the teaching pendant 4 is operated, and the rotation center line of the rotation axis 14 of the rotation axis 14 of the fifth rotation joint J5 is the rotation center line of the rotation axis 14 of the fifth rotation joint J6. When the orthogonal point with respect to the sixth axis Lc-6 (hereinafter referred to as the tip 2-axis orthogonal point C) is moved to a plurality of target positions, the actual movement position of the tip 2-axis orthogonal point C is calculated and The deviation amount is obtained, and the offset between the axes is estimated from the deviation amount.

  In this case, the deviation between the movement target position of the tip 2-axis orthogonal point C and the actual movement position is caused by factors other than the inter-axis offset, that is, the deflection of the rotary drive system of each rotary joint J1 to J6, the origin of each servo motor 15. Since this also occurs due to a position error, a length error of each link 7 to 12, and a twist angle error between the rotary shafts 14 of each rotary joint J1 to J6, a correction process is performed in advance so as not to cause a position error due to these factors. There is a need to do.

  In order to obtain the position of the tip biaxial orthogonal point C by calculation, a support base 23 is attached to an arbitrary position of the flange 12 as shown in FIG. 8, and a light emitting diode 24 as a measurement point is attached to the tip of the support base 23. Is provided. A three-dimensional measuring instrument (three-dimensional measuring means) 25 shown in FIG. 7 is fixed at an appropriate position away from the robot 2 in order to measure the position of the light emitting diode 24.

  The three-dimensional measuring instrument 25 includes, for example, a three-dimensional measuring instrument (for example, Toyo Technica Co., Ltd .; robot calibration ROCAL system) provided with three CCD cameras. Note that the three-dimensional measuring instrument 25 may be a laser tracker or the like.

  The measurement position of the light emitting diode 24 by the three-dimensional measuring instrument 25 is indicated by a position on the three-dimensional coordinate (hereinafter referred to as camera coordinate) Rc preset in the three-dimensional measuring instrument 25 by the measurement control device 26 formed of a personal computer. . The distance represented by one scale of the camera coordinates Rc is determined to be equal to the distance when the hand moves by a distance corresponding to one scale of the robot coordinates R0. Therefore, the length of one scale of the robot coordinates R0 and the length of one scale of the camera coordinates Rc represent the same distance.

  Now, the method of the present invention is performed according to the procedure shown in FIG. First, the position of the robot coordinate R0 on the camera coordinate Rc and the directions of the X0 axis, the Y0 axis, and the Z0 axis of the robot coordinate R0 are unknown. Therefore, in this embodiment, the reference coordinate Rb is set on the camera coordinate Rc as a coordinate to replace the robot coordinate R0 (step S1 in FIG. 9). This reference coordinate Rb is set using the teaching pendant 4.

  Here, the function of the teaching pendant 4 will be described. The teaching pendant 4 includes a shoulder 7, a lower arm 8, a first upper arm 9, a second upper arm 10, a wrist arm 11, and a servo motor 15 for the flange 12. The shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, and the flange 12 are rotated in desired forward and reverse directions from their respective origin positions by individually rotating by a desired angle. It is configured to be able to rotate by an angle.

  Further, the teaching pendant 4 can specify the position and posture of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, the flange 12, and the hand using robot coordinates R0. When the teaching pendant 4 designates the position and orientation on the robot coordinate R0, the CPU 17 of the robot apparatus 1 causes the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, and the wrist. The rotation angles of the arm 11 and the flange 12 are calculated, and each servo motor 15 is driven so as to be the rotation angle. Thereby, it operates so that each coordinate R1-R6, Rf and a hand may take the specified position and posture.

  Furthermore, if you want to know the position and posture of the current shoulder 7, lower arm 8, first upper arm 9, second upper arm 10, wrist arm 11, flange 12, and hand, The CPU 17 detects the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11 and the flange 12 from the rotation angles of the shoulder 7, the lower arm 8, the first upper arm 9, and the second The positions and postures of the upper arm 10, wrist arm 11, flange 12, and hand are calculated, and the display 4a of the teaching pendant 4 displays the position and posture on the robot coordinate R0.

Here, the position and posture of the shoulder 7, the lower arm 8, the first upper arm 9, the second upper arm 10, the wrist arm 11, the flange 12, and the hand are the positions and postures of the coordinates R1 to R6 and Rf. Is synonymous with
In order to set the reference coordinate Rb on the camera coordinate Rc, first, the operation unit 4a of the teaching pendant 4 is operated to bring the robot arm 6 into an arbitrary posture. At this time, for example, in a state where the fifth axis Lc-5 is held parallel to the second axis Lc-2 and the third axis Lc-3, that is, horizontally, the light emitting diode 24 is separated from the first axis Lc-1 in the horizontal direction. In this embodiment, as shown in FIG. 13, the lower arm 8 and the first upper arm 9 are rotated from the vertically upward state shown in FIG. The arm 6 is held in a forward-bending posture where the arm 6 is tilted to the front side (the positive side of the X0 axis) of the shoulder 7. The posture at this time may be the backward-bending posture shown in FIG. When the robot arm 6 is rotated around the first axis Lc-1, the light emitting diode 24 moves on one rotation locus.

  With this posture, the shoulder 7 is centered on the first axis Lc-1 until the shoulder 7 reaches a desired angle, preferably a position of plus 90 °, a position of 0 ° (origin position), and a position of minus 90 °. Rotate 180 degrees or more including In setting the reference coordinate Rb, the fifth axis Lc-5 is not necessarily parallel to the second axis Lc-2 and the third axis Lc-3.

  Then, the light emitting diode 24 as a reference coordinate setting measurement point follows a circular rotation locus E around the first axis Lc-1 as shown in FIG. The positions on the rotation locus E are measured at a plurality of positions d1, d2,. The measurement control device 26 obtains the position Ob of the rotation center of the light emitting diode 24 from the plurality of positions d1, d2,... Dn on the rotation locus E of the light emitting diode 24 measured by the three-dimensional measuring instrument 25 by the least square method. Then, a normal Lv of one plane including the rotation locus E of the light emitting diode 24 is obtained. The rotation center position Ob is defined as the origin of the reference coordinate Rb, and a straight line passing through the origin Ob and parallel to the normal Lv (in the same direction) is defined as the Zb axis (Z axis) of the reference coordinate Rb.

  Further, an arbitrary straight line passing through the origin Ob and orthogonal to the Zb axis, for example, a straight line connecting the position P0 of the light emitting diode 24 and the origin Ob when the shoulder 7 is rotated to the origin position is the Xb axis of the reference coordinate Rb. (X axis), a straight line orthogonal to both axes is obtained from the outer product of the Zb axis and Xb axis vectors, and the straight line is defined as the Yb axis (Y axis) of the reference coordinate Rb. The length of one scale on the reference coordinate Rb is determined to be the same as the length of one scale on the camera coordinate Rc.

  As described above, the reference coordinate Rb is set on the camera coordinate Rc. In the reference coordinate Rb set in this way, the Zb axis is on the first axis Lc-1 and thus on the Z0 axis of the robot coordinate R0, and the XbYb plane is parallel to the X0Y0 plane. When the reference coordinate Rb is set, if the position of the light emitting diode 24 is on the X0Y0 plane, the XbYb plane coincides with the X0Y0 plane.

  If there is no inter-axis offset, the Xb axis and Yb axis are parallel to the X0 axis and Y0 axis of the robot coordinate R0. However, when there is an offset between the axes, the position P0 of the light emitting diode 24 when the shoulder 7 is rotated to the origin position is shifted in the X0Y0 direction by the offset between the axes, so that the Xb axis and Yb axis are the X0 axis and Y0. It is not parallel to the axis but tilts according to the offset between the axes as shown in FIG.

  After setting the reference coordinate Rb, a vertical plane including the first axis Lc-1 extending in an arbitrary direction from the first axis Lc-1 is assumed, and a plurality of positions in the one plane are set as movement target positions. Determine. In the present embodiment, as shown in FIGS. 10 and 12, the Zb axis coaxial with the first axis Lc-1, and thus the arbitrary plane A1 including the Z0 axis extending in an arbitrary direction from the Z0 axis, and the Z0 axis A plane A2 extending in the direction opposite to the plane A1 is set. The one plane A1 and the other plane A2 are one plane A that is flush with each other. In this embodiment, a plurality of such planes A are set (only one plane A is shown in FIGS. 10 and 12), and one of the planes A is a plane including the X0 axis. The plane A is designated by an angle β formed with the X0 axis.

  Then, an arbitrary plurality of positions are selected as the movement target positions on each of the planes A1 and A2. A plurality of arbitrarily selected movement target positions are indicated by small white circles in FIG. As will be described later, for example, the fifth axis Lc-5 is connected to the second axis Lc-2 and the third axis at the plurality of movement target positions arbitrarily selected on the respective planes A1 and A2. The tip biaxial orthogonal point C is moved in a state parallel to Lc-3.

  Here, the plurality of positions selected on the plane A are designated by (X0, Y0, Z0) coordinate values of the robot coordinate R0. Since the Zb axis of the reference coordinate Rb coincides with the Z0 axis and the Xb axis and the Yb axis are parallel to the X0 axis and the Y0 axis when there is no inter-axis offset, respectively, the X0 coordinate value and the Y0 coordinate value of the movement target position Is the same value as the Xb coordinate value and the Yb coordinate value on the reference coordinate Rb. However, the origin Ob of the reference coordinate Rb is separated from the origin O0 of the robot coordinate R0, and the position of the light emitting diode 24 on the Z0 axis when the reference coordinate Rb is obtained is the position on the Z0 axis of the origin Ob of the reference coordinate Rb. Since this position is Za, the Zb coordinate value at the reference coordinate Rb of the movement target position is (Z0−Za). Therefore, if there is no inter-axis offset, the Xb axis and Yb axis are parallel to the X0 axis and Y0 axis. Therefore, when there is no inter-axis offset, the robot coordinate R0 is translated along the Z0 axis to Za. And the reference coordinate Rb. However, although the value of Za is unknown, the axis offset appears in the X0 axis and Y0 axis directions, but does not appear in the Z0 axis direction. Therefore, even when the value of Za is unknown, the axis offset amount is obtained. It will not be a hindrance.

  Then, from the relationship between the reference coordinate Rb and the robot coordinate R0 when there is no offset between the axes, each moving target position is designated by a three-dimensional coordinate value at the robot coordinate R0, and the tip 2-axis orthogonal point C is set. When moved, the actual movement position of the tip 2-axis orthogonal point C is shifted in the XbYb direction from the movement target position on the reference coordinate Rb obtained as described above by an amount corresponding to the inter-axis offset. From this, the deviation amount of the offset between axes can be obtained.

  Here, since the tip 2-axis orthogonal point C coincides with the origins O5 and O6 of the coordinate R5 of the wrist arm 11 and the coordinate R6 of the flange 12, the teaching pendant 4 makes the coordinates R5 of the wrist arm 11 or the flange 12 By performing an operation of moving the coordinate R6, for example, the coordinate R6, to the movement target position, the tip biaxial orthogonal point C can be moved to the movement target position.

  Prior to moving the tip 2-axis orthogonal point C of the robot arm 6 to a plurality of selected positions on the planes A1, A2, for example, the fifth axis Lc-5 is changed to the second axis Lc-2 and the third axis. When the tip 2-axis orthogonal point C is moved to the movement target position by rotating the rotating shafts 14 of the second and third rotating joints J2 and J3 on a state parallel to Lc-3. Then, a process for eliminating the movement position error of the tip 2-axis orthogonal point C due to each error other than the offset between axes (deflection of the rotational drive system, origin position error of the servo motor 15, torsion angle error, link length error) is performed.

  First, the bending of the rotational drive system refers to bending in the twisting direction due to the transmission torque from the rotating shaft of the servo motor 15 to the rotating shaft 14 of the rotary joints J1 to J6 through the gear reduction device 16. It is assumed that the rotating shaft 14 does not bend in an arc shape in the axial direction. The deflection of the rotational drive system can be obtained from the torque acting on the rotational drive system and the spring constant of the rotational drive system. Among them, the spring constant is known in the robot 2, and its value is stored in advance in the ROM 20 of the control device 3. The torque acting on the rotational drive system is considered for the second axis Lc-2, the third axis Lc-3, and the fifth axis Lc-5 (the first axis Lc-2, the fourth axis Lc-3, the sixth axis Lc). -5 does not receive a large torque and does not cause bending that causes an absolute position error.)

  As shown in FIG. 11, the torque acting on the rotary drive system can be obtained by the product of the mass W of the link L supported by the rotary shaft 14 and the distance D from the rotary shaft 14 to the center of the link L. When moving the tip 2-axis orthogonal point C of the robot arm 6 to a plurality of arbitrary positions selected on the plane A, the fifth axis Lc-5 is parallel to the second and third axes Lc-2, Lc-3. The upper arm 10 and the first upper arm 9 are rotated to move the tip biaxial orthogonal point C to any plurality of positions. The rotation angles of the arm 8, the first upper arm 9, the wrist arm 11, and the flange 12 are read from the teaching pendant 4, and the second axis Lc-2, the third axis Lc-3, and the fifth axis Lc- are determined from these rotation angles. 5 is calculated, and the deflection (angle) α of these rotary drive systems is obtained (step S2 in FIG. 9).

  Then, the rotation angle of the servo motor 15 is increased or decreased by the obtained bending α of the rotational drive system so as to be equivalent to the case where no bending occurs. In other words, in FIG. 11, assuming that the position of the link indicated by the solid line is the movement target position when the rotation drive system is not considered to be the deflection, and the position indicated by the broken line is the actual movement position when the rotation drive system is deflected, the servo The rotation angle of the motor 15 is decreased by the deflection α. As a result, the link reaches the movement target position indicated by the solid line in a state where the rotational drive system is bent.

The motor origin position error is detected by, for example, the methods disclosed in Patent Documents 1 to 4. Further, the twist angle error and the link length error are detected by the direction disclosed in Non-Patent Document 1, for example. The method of Non-Patent Document 1 measures the rotation trajectory of the hand when the robot arm 6 is rotated about each of the first axis Lc-1 to the sixth axis Lc-6 with a three-dimensional measuring instrument, The position of the center of rotation and the normal of one plane including the rotation locus are calculated. Since the length of each link is obtained from the position of the rotation center and the direction of each axis is obtained from the direction of the normal line, the link length error and the torsion angle error can be obtained from the length of the link and the axial direction. (Step S3 in FIG. 9).
Then, the DH parameter is corrected based on the obtained motor origin position error, link length error and torsion angle error so that these errors do not appear as absolute position errors (step S4 in FIG. 9).

  After performing the above processing, the teaching pendant 4 is operated, and first, the X1 axis of the coordinate R1 of the shoulder 7 is aligned with one desired plane A1 among the plurality of planes A1. For this purpose, the shoulder 7 is rotated from the origin position by an angle β formed with the X0 axis of the plane A1. Thereby, the tip 2-axis orthogonal point C can be moved to a plurality of movement target positions arranged on the planes A1 and A2 by rotating the rotation shafts 14 of the second rotation joint J2 and the third rotation joint J3.

  Therefore, for example, the fifth axis Lc-5 is held in parallel with the second axis Lc-2 and the third axis Lc-3, and the rotary shafts 14 of the second rotary joint J2 and the third rotary joint J3 are rotated. Thus, the fifth axis Lc-5 remains horizontal, and only the lower arm 8 (second axis Lc-2) and the first upper arm 9 (third axis Lc-3) are rotated to obtain the plane A1, The tip biaxial orthogonal point C is moved to a plurality of movement target positions arbitrarily selected on A2. At this time, the rotation angle of the servo motor 15 is adjusted according to the bending of the rotary drive system previously obtained for each movement target position.

  When the hand is moved to a plurality of movement target positions arbitrarily selected on the planes A1 and A2, the robot arm 6 moves the rotation axes 14 of the second and third rotary joints J2 and J3 as shown in FIG. Is rotated in one direction to move the hand to a movement target position on one plane A1 in a posture (forward bending posture) that falls forward from the shoulder 7 and bends forward, and a movement target on the other plane A2 When the tip 2-axis orthogonal point C is moved to the position, the robot arm 6 rotates the rotating shafts 14 of the second and third rotary joints J2 and J3 in the other direction, as shown in FIG. The tip 2-axis orthogonal point C is moved in a posture that is reversed and falls backward from the shoulder 7 and bends backward (the robot arm 6 falls back to the rear side of the shoulder 7).

  Each time the tip 2-axis orthogonal point C is moved to the movement target position, the following operation is performed to estimate the actual position of the tip 2-axis orthogonal point C. That is, at each movement target position, as shown in FIG. 15, the rotation shaft 14 of the sixth rotation joint Lc-6 is rotated, and three or more different light emitting diodes 24 moving on a circular rotation locus by this rotation are provided. The position is measured by the three-dimensional measuring instrument 25. From the three or more positions on the rotation locus of the light emitting diode 24 thus measured, the plane normal V6 including the center position C6 of the rotation locus of the light emitting diode 24 and the three or more positions on the rotation locus of the light emitting diode 24. And ask. And the 1st straight line L6 parallel to the normal line V6 is calculated | required through the center of the rotation locus | trajectory of the light emitting diode 24 (1st operation | movement).

  Further, as shown in FIG. 16, the rotation shaft 14 of the fifth rotation joint Lc-5 is rotated, and three or more different positions of the light emitting diodes 24 moving on the circular rotation locus by this rotation are measured three-dimensionally. The center position C5 of the rotation trajectory of the device 25 and the normal V5 of the plane including three or more positions on the rotation trajectory of the light emitting diode 24 are obtained, and the normal V5 passes through the center of the rotation trajectory of the light emitting diode 24. A parallel second straight line L5 is obtained (second operation).

The intersection of the first straight line L6 and the second straight line L5 is set as the measurement position of the tip biaxial orthogonal point C. A calculation method for obtaining the measurement position (camera coordinate Rc) of the tip biaxial orthogonal point C will be described.
The position of C6 on the camera coordinates is (X6, Y6, Z6), and V6 (vector) is (l6, m6, n6). The position of C5 on the camera coordinates is (X5, Y5, Z5), and V5 (vector) is (l5, m5, n5). However, V6 and V5 are unit vectors, and l6, m6, n6 and l5, m5, and n5 indicate the lengths of V6 and V5 decomposed in the Xc-axis, Yc-axis, and Zc-axis directions.

The equation P1 of a straight line passing through C6 and parallel to V6 is
(X, y, z) = (X6, Y6, Z6) + t (l6, m6, n6)
P1 = C6 + t · V6 (1)
It becomes.
The equation P2 of a straight line passing through C5 and parallel to V5 is
(X, y, z) = (X5, Y5, Z5) + s (l5, m5, n5)
P1 = C5 + s · V5 (2)
It becomes.
Note that t and s are parameter variables.

Since the straight line connecting the two straight lines P1 and P2 with the shortest distance is perpendicular to P1 and P2, if the vector from P1 to P2 is u, the inner product Dot of u, V6, u, and V5 is 0.
Dot (u, V6) = 0
Dot (u, V5) = 0
It is.

Substituting the two linear equations into the inner product equation above,
Dot (u, V6) = Dot (C5-C6 + s.V5-t.V6, V6)
= Dot (C5-C6, V6) + s.Dot (V5, V6) -t = 0 (3)
Dot (u, V5) = Dot (C5-C6 + s · V5-t · V6, V5)
= Dot (C5-C6, V6) + st.Dot (V5, V6) = 0 (4)
It becomes.

  Since values other than t and s are known values, s and t can be calculated from equations (3) and (4), and if the calculated s and t are substituted into equations (1) and (2), 2 The closest point of two straight lines can be obtained. And as shown in FIG. 17, the position of the front-end | tip biaxial orthogonal point C can be calculated | required by averaging the nearest point N1, N2 of the calculated | required two straight lines. If two straight lines intersect at one point, the closest points N1 and N2 of the two straight lines have the same value.

Since the position of the tip biaxial orthogonal point C obtained in this way is a position on the camera coordinate Rc, it is converted to the position of the reference coordinate Rb. This conversion is performed by the simultaneous conversion matrix shown in FIG. 18 (step S6 in FIG. 9).
Thereafter, the Xb and Yb coordinate values at the reference coordinate Rb of the measurement position of the tip biaxial orthogonal point C at each movement target position are acquired. Then, the position of the tip biaxial orthogonal point C indicated by the Xb and Yb coordinate values is plotted on the XbYb plane of the reference coordinate Rb.

  FIG. 19 shows the XbYb plane of the reference coordinate Rb. Here, for simplification of explanation, it is assumed that the movement target positions are a plurality of positions existing on the X0 axis. In FIG. 19, the white circle is the movement target position of the tip 2-axis orthogonal point C on the robot coordinate R0, and the black circle is the movement target position (Xb, Yb coordinate of the tip 2-axis orthogonal point C on the reference coordinate Rb. The value is equal to the X0 and Y0 coordinate values), and the x mark indicates the measurement position of the tip biaxial orthogonal point C.

  When the tip 2-axis orthogonal point C is moved to the movement target position, pre-processing is performed so that no error occurs in the movement position depending on the bending of the rotation drive system, the motor origin error, the link length error, and the torsion angle error. It is considered that the error between the movement target position and the actual movement position is caused by the offset between the axes. The error caused by this inter-axis offset is that the movement target position is on one vertical plane (here, on the X0 axis), so the measurement position of the tip 2-axis orthogonal point C is the movement target position on the robot coordinate R0. Are aligned on a straight line with an offset between the axes (not distributed accurately on a straight line). Therefore, the line segment connecting the measurement positions is defined as a measurement dependent line segment B, and the shortest distance between the straight line Bb obtained by extending the measurement dependent line segment B and the origin Ob (the length of the perpendicular line extending from Ob to Bb) is obtained. The shortest distance is the inter-axis offset amount F (step S7 in FIG. 9).

  In this case, when the robot arm 6 is bent forward and moved to the movement target position on the plane A1, the measurement dependence line segment B connecting the actual movement positions and the robot arm 6 are bent backward and moved on the plane A2. If the measurement dependence line segment B connecting the actual movement position when moved to the target position does not become a single straight line (including the fact that it does not line up near a single straight line), Since it is considered that there was some inconvenience, in this case, the process starts again from the first step S1. As described above, by determining the movement target positions on the two planes A1 and A2, it is possible to detect a problem in implementation.

Actually, the inter-axis offset amount F is obtained in the same manner as described above for the movement target positions determined on other similar planes other than the planes A1 and A2 including the X0 axis. Also in this case, the actual movement position of the hand should be aligned in a straight line.
The shortest distance from the origin Ob is similarly measured for the measurement dependence line segment B formed by connecting the measurement positions for the movement target positions determined on the other plane A other than the plane A including the X0 axis. The shortest distance between each measurement-dependent line segment B and the origin Ob should be the same for any measurement-dependent line segment B, but in reality, it may not always be the same due to a measurement error or the like. Many. In such a case, the average of the shortest distance between the measurement dependent line segment B and the origin Ob is obtained to obtain the offset between the axes. As a result, the offset between axes can be obtained more accurately.

  The inter-axis offset obtained as described above is the sum of the inter-axis offsets generated in the second axis Lc-2, the third axis Lc-3, and the fifth axis Lc-5, and it is analyzed which axis is the offset. Can not. For this reason, among the HD parameters shown in FIG. 5, the value of the offset between axes is added to any of the d columns of J2 and J3 or added separately to the d columns of J2 and J3. Since the fifth axis Lc-5 can take any angle from horizontal to vertical by the rotation of the fourth axis Lc-4, if the inter-axis offset is distributed to the column d of J5, the absolute position accuracy is reduced. I will let you. For this reason, the offset between axes is not distributed to the column d of J5.

In order to check whether the absolute position accuracy has been improved by adding the value of the offset between axes in either the d column of J2 or J3 or in the d column of J2 or J3, the offset between axes is set. Using the added DH parameter, a plurality of target positions were determined on the Xb axis, and the hand was moved to the target position in the same manner as described above, and an experiment was performed to measure the actual movement position.
As a result, the position error on the XbYb plane decreased from 0.67 mm to 0.17 mm. The position error in the Zb axis direction also decreased from 0.9 mm to 0.06 mm. From this, it can be said that the position error is reduced from 1.12 mm to 0.18 mm in the three-dimensional direction of XbYbZb.

  As described above, according to the present embodiment, the offset between the axes can be quantitatively grasped, and the DH parameter is corrected by the offset between the axes so that there is no error due to the offset between the axes, or the error does not occur as much as possible. The absolute position accuracy can be increased.

(Second Embodiment)
FIG. 20 shows a second embodiment of the present invention. In this embodiment, the reference coordinate Rb is set by a method different from that of the first embodiment, but the subsequent method of obtaining the amount of offset between the axes is the same as that of the first embodiment.

  In this embodiment, the reference coordinate Rb is set using the tip biaxial orthogonal point C. That is, among the first to sixth rotary joints J1 to J6, at least the rotary shafts 14 of the second to fourth rotary joints J2 to J4 are fixed, and the shoulders 7 are connected to each other by the rotary shaft 14 of the first rotary joint J1. Rotate to any three or more different angular positions. At each angular position, the first operation described in the first embodiment is performed to obtain the first straight line L6 (the sixth axis Lc-6, which is the rotation center line of the flange 12), and the second The second straight line L5 (the fifth axis Lc-5 that is the rotation center line of the wrist arm 11) is obtained by performing the above operation. Thereafter, the intersection of the first straight line L6 and the second straight line L5, that is, the position of the tip 2-axis orthogonal point C is calculated, and this position is set as the measurement position of the tip 2-axis orthogonal point.

  The measurement positions of the tip biaxial orthogonal point C exist on one circle E centered on the first axis Lc-1. The measurement control device 26 obtains the center position of the circle E passing through the measurement positions of the tip 2-axis orthogonal point C and the plane normal Lv including the measurement position of the tip 2-axis orthogonal point C. Then, the measurement control device 26 sets the center of the circle E as the origin Ob, the straight line parallel to the normal line Lv of the plane including the measurement positions of the plurality of biaxial perpendicular points C through the origin Ob as the Zb axis, and the origin Ob. A reference coordinate Rb is defined with an arbitrary straight line passing through the Zb axis passing through the Xb axis and a straight line passing through the origin Ob perpendicular to both the Zb and Xb axes as the Y axis b.

(Third embodiment)
21 and 22 show a third embodiment of the present invention. In this embodiment, the reference coordinate Rb is obtained in the same manner as in the first embodiment or the second embodiment, but the method for obtaining the offset amount between axes is obtained by a method different from the method in the first embodiment. It is a thing.

In the present embodiment, a plurality of arbitrary positions within the movable range of the tip 2-axis orthogonal point C are set as movement target positions of the tip 2-axis orthogonal point C, and the tip 2-axis orthogonal point C is set at the plurality of positions. When moved, the offset amount between the axes is obtained based on the difference between the movement target position and the actual movement position of the tip biaxial orthogonal point C.
That is, arbitrary plural positions on the robot coordinate R0 are selected as the movement target positions. Note that the X0 and Y0 coordinate values of the movement target position designated on the robot coordinate R0 have the same values for the Xb and Yb coordinates of the reference coordinate Rb (when there is no inter-axis offset).

In FIG. 21, a plurality of positions selected as movement target positions are indicated by black circles. Among these, the movement of the hand to the movement target position with the Xb coordinate value on the plus side (right side of the Yb axis) is performed in the forward bending posture of the robot arm 6 and to the movement target position on the minus side (left side of the Yb axis). The movement of the hand is assumed to be performed in a backward bending posture of the robot arm 6.
Similar to the first embodiment, prior to moving the tip biaxial orthogonal point C to a plurality of selected movement target positions, each error other than the inter-axis offset (deflection of the rotational drive system, the origin of the servo motor 15). Processing for eliminating the movement position error of the tip biaxial orthogonal point C due to position error, torsion angle error, and link length error is performed.

  Thereafter, the tip biaxial orthogonal point C is moved to a plurality of selected movement target positions. The movement at this time is to rotate the rotary shaft 14 of the first rotary joint J1 and the rotary shafts 14 of the second and third rotary joints J2 and J3 among the second, third and fifth rotary joints J2, J3 and J5. Do by. In order to hold the fifth axis Lc-5 horizontally, the rotary shaft 14 of the fourth rotary joint J4 is not rotated. The rotation axis 14 of the fifth rotation joint J5 is an unrelated rotation axis for moving the tip 2-axis orthogonal point C to the movement target position, so it is not necessary to rotate it.

And in each movement target position, in order to obtain | require the measurement position of the front-end | tip biaxial orthogonal point C demonstrated in 1st Embodiment, 1st operation | movement is performed and a 1st straight line (in the rotation centerline of the flange 12). A sixth axis Lc-6) is obtained, and a second operation is performed to obtain a second straight line (fifth axis Lc-5, which is the rotation center line of the wrist arm 11). Thereafter, the intersection of the first straight line and the second straight line, that is, the position of the tip 2-axis orthogonal point C is obtained, and this position is set as the measurement position of the tip 2-axis orthogonal point.
Since the measurement position of the tip biaxial orthogonal point C is the position of the camera coordinate Rc, it is converted into the position of the reference coordinate Rb. The measurement position of the tip biaxial orthogonal point C at each movement target position is indicated by x in FIG.

  In order to obtain the offset between axes from the movement target position and the measurement position of the tip biaxial orthogonal point C, the XbYb coordinate value (X0Y0 coordinate) of the movement target position is indicated on the XbYb coordinate plane of the reference coordinate Rb as indicated by a black circle in FIG. (Same as the value) and the measurement position are plotted, the length of the line segment from the origin Ob to the movement target position is set as the target position radius D0, and the line is lowered from the measurement position of the light emitting diode 24 to a straight line passing through the origin Ob and the movement target position. Let the length of the vertical line be an offset error component M0. Then, as shown in FIG. 22, a graph is created with the target position radius D0 on the horizontal axis and the offset error component M0 on the vertical axis, and the target position radius D0 and offset error for each moving target position are created on this graph. A relationship with the component M0 is plotted, and a straight line passing through these plotted points is defined as an error straight line G. When a perpendicular is drawn from the origin of the graph to the error straight line G, the length of this perpendicular becomes the total inter-axis offset amount F of the second, third, and fifth rotary joints.

(Fourth embodiment)
23 and 24 show a fourth embodiment of the present invention. In this embodiment, after setting the reference coordinate Rb in the same manner as described in the first embodiment or the second embodiment, the position of the light emitting diode 24 and the tip 2 axes when the hand is in an arbitrary reference posture. When the positional relationship with the orthogonal point C is obtained, and then the tip biaxial orthogonal point C is moved to the movement target position, the position of the light emitting diode 24 is measured with the hand as a reference posture. The measurement position of the tip biaxial orthogonal point C is obtained from the positional relationship between the position of the light emitting diode 24 and the tip biaxial orthogonal point C obtained in the above.

That is, after setting the reference coordinate Rb, the hand is in the reference posture, for example, as shown in FIG. 23, the Zf axis of the hand coordinate Rf is parallel to the Z0 axis (first axis Lc-1) and the positive side of the Zf axis is downward ( Xf axis is parallel to X0 axis and the positive side is opposite to X0 axis, Yf axis is parallel to Y0 axis, and the positive side is the same as the positive side of Y0 axis. Make a posture.
In the reference posture of the hand, the position of the light emitting diode 24 is measured by the three-dimensional measuring instrument 25. In addition, the first operation described in the first embodiment is performed to obtain the first straight line (sixth axis Lc-6, which is the rotation center line of the flange 12), and the second operation is performed to perform the second operation. 2 straight lines (the fifth axis Lc-5, which is the rotation center line of the wrist arm 11) are obtained. Thereafter, the intersection of the first straight line and the second straight line, that is, the position of the tip biaxial orthogonal point C is obtained.

  Since the position of the light emitting diode 24 in the reference posture of the hand and the position of the tip 2-axis orthogonal point C obtained from the first straight line and the second straight line are positions on the camera coordinates Rc, these two positions Is converted to a position on the reference coordinate Rb. Then, as shown in FIG. 24, on the reference coordinate Rb, a vector starting from the position of the light emitting diode 24 and starting from the position of the tip 2-axis intersection C is obtained, and this vector is set as a detection vector V. Angles γ, δ, and ε between the detection vector V and each axis of XbYbZb of the reference coordinate Rb are obtained and set as the directivity angles.

  Thereafter, in order to obtain the inter-axis offset, the movement target position of the tip biaxial orthogonal point C is determined in the same manner as in the first embodiment or the second embodiment. Then, the origin position error of the motor, the bending of each of the first to fourth rotation drive systems, the length error of each of the first to fourth links, and the angle between the rotation axis of the rotation joint and the next rotation joint Correction processing is performed so that the position error of the tip 2-axis orthogonal point does not occur due to the above error, and the tip 2-axis orthogonal point C is moved to the movement target position.

Each time the tip biaxial orthogonal point C is moved to the movement target position, the position of the light emitting diode 24 is measured by the three-dimensional measuring instrument 25. Since the position of the light emitting diode 24 measured by the three-dimensional measuring instrument 25 is a position on the camera coordinate Rc, it is converted into a position on the reference coordinate R. After that, for each movement target position, a detection vector V is created so that the angle formed by the XbYbZb axis of the reference coordinate Rb with the position of the light emitting diode 24 becomes the directivity angles γ, δ, ε. The position of the end point of the detection vector V is set as a measurement position on the reference coordinate Rb of the tip biaxial orthogonal point C.
As described above, based on the measurement position on the reference coordinate Rb of the tip 2-axis orthogonal point C, the inter-axis offset amount is obtained in the same manner as in the first embodiment or the second embodiment.

(Other embodiments)
The present invention is not limited to the embodiments described above and shown in the drawings, and the following modifications or expansions are possible.
When moving the tip 2-axis orthogonal point C to the target position, the rotation center of the rotation shaft of the fifth rotation joint does not necessarily need to be parallel to the rotation center line of the rotation shaft of the second and third rotation joints.
The Xb axis of the reference coordinate Rb is not necessarily determined to be a straight line parallel to the X0 axis (assuming no offset between axes). What is necessary is just to set to the arbitrary straight lines which pass through the origin Ob. In this case, the reference is obtained by obtaining the relationship between the reference coordinate Rb and the robot coordinate R0 based on the angle between the arbitrary straight line (Xb axis) and the X0 axis (rotation angle from the origin position of the shoulder 7) and performing coordinate conversion. The position on the coordinate Rb and the position on the robot coordinate R0 may be converted to each other.

  In the first embodiment, when moving the hand from the plane A1 (plane extending in one direction from the Zb axis) to the movement target position of the plane A2 (plane extending in the opposite direction from the Zb axis), depending on the selected position of the movement target position If the rotary shaft 14 of one of the second, third, and fifth rotary joints J2, J3, and J5 is rotated, the robot arm 6 can be reversed to move the hand to the movement target position. it can.

  The deflection α of the rotational drive system, the motor origin position error, the link length error, and the torsion angle error may be performed only for the first axis Lc-1 to the fourth axis Lc-4 (the link length error is the first to fourth axis). Links 7-10). This is because the error between the fifth axis Lc-5 and the sixth axis Lc-6 does not affect the position of the tip 2-axis orthogonal point C. However, since the deflection α of the rotational drive system is only for the horizontal axis, it is actually performed for the second and third axes Lc-2 and Lc-3.

In the first embodiment, whichever of the first straight line L6 and the second straight line L5 may be obtained first.
In the first and second embodiments, when the robot arm 6 is rotated around the first axis Lc-1 in order to obtain the reference coordinate Rb, the rotation axes 14 of the fifth and sixth rotary joints J5 and J6 are Either fixed or rotating operation may be used.

  In the drawings, 3 is a control device, 5 is a base (base link), 6 is a robot arm, 7 is a shoulder (first link), 8 is a lower arm (second link), and 9 is a first upper arm (third). Link), 10 is a second upper arm (fourth link), 11 is a wrist arm (fifth link), 12 is a flange (sixth link), 24 is a light emitting diode (measurement point), and 25 is a three-dimensional measuring instrument. (Three-dimensional measuring means) is shown.

Claims (8)

A 6-axis robot in which first to sixth links constituting a robot arm are sequentially connected to a base link fixed to a mounting surface by first to sixth rotary joints, The rotation center line of the rotation axis of the second rotation joint connecting the second link to one link is orthogonal to the rotation center line of the rotation axis of the first rotation joint connecting the first link to the base link. The rotation center line of the rotation axis of the third rotation joint connecting the third link to the second link is parallel to the rotation center line of the rotation axis of the second rotation joint, The rotation center line of the rotation axis of the fourth rotation joint connecting the fourth link to the link is parallel to the direction perpendicular to the rotation center line of the rotation axis of the third rotation joint, and the fourth link The fifth rotary joint connecting the fifth link The rotation center of the rotation shaft of the sixth rotation joint that connects the sixth link to the fifth link so that the rotation center line of the rotation shaft is parallel to the direction orthogonal to the rotation center line of the rotation shaft of the fourth rotation joint. The lines are respectively set to be orthogonal to the rotation center line of the rotation axis of the fifth rotation joint,
The robot coordinates are set on the rotation center line of the rotation axis of the first rotation joint, and the coordinates of the first to sixth links are set on the rotation center lines of the rotation axes of the first to sixth rotation joints, respectively. And a six-axis robot in which the coordinates of the hand are set at the same position with the predetermined position existing on the rotation center line of the rotation axis of the sixth rotation joint of the sixth link as the hand,
A measurement point that operates integrally with the sixth link is provided, and a three-dimensional measurement unit that can measure a three-dimensional position of the measurement point is provided,
While the robot arm is held in an arbitrary posture, the robot arm is rotated around the rotation axis of the first rotary joint, and the three or more different measurement points that move on a circular rotation trajectory due to this rotation are rotated. Measure multiple positions with 3D measuring means,
From the measured three or more positions on the rotation locus of the measurement point, the center position of the rotation locus of the measurement point and the normal of the plane including the three or more positions on the rotation locus of the measurement point are obtained. The center of the rotation locus of the measurement point is the origin, the straight line passing through the origin and parallel to the normal is the Z axis, the arbitrary straight line passing through the origin and perpendicular to the Z axis is the X axis, and the origin is Through which a reference coordinate having a Y axis as a straight line perpendicular to both the Z and X axes is defined;
A plurality of positions on any one plane including the rotation center line of the rotation axis of the first rotation joint extending from the rotation center line of the rotation axis of the first rotation joint are set as movement target positions;
By rotating the rotation shafts of the second and third rotation joints, the rotation center line of the rotation shaft of the fifth rotation joint and the rotation center line of the rotation shaft of the sixth rotation joint are located at the movement target position. When the tip biaxial orthogonal point that is the orthogonal point is moved,
Origin position errors of the motors driving the first to fourth links via the first to fourth rotary drive systems including the rotation axes of the first to fourth rotary joints; The position error of the tip 2-axis orthogonal point due to the deflection of each fourth rotational drive system, the length error of each of the first to fourth links, and the angle error between the rotational joint and the rotational axis of the next rotational joint After performing correction processing to prevent
By rotating the rotary shafts of the rotary joints of the second and third rotary joints, the rotary shaft of the first rotary joint is rotated to a position where the tip 2-axis orthogonal point can be moved to the movement target position. After that, by rotating the rotation shafts of the second and third rotation joints with the rotation shaft of the first rotation joint fixed, the tip 2-axis orthogonal point is moved to each of the plurality of movement target positions. Let
At each of the movement target positions, the rotation axis of the sixth rotary joint is rotated, and three or more different positions of the measurement points that move on the circular rotation locus by this rotation are measured by a three-dimensional measuring means, From the measured three or more positions on the rotation locus of the measurement point, the center position of the rotation locus of the measurement point and the normal of the plane including the three or more positions on the rotation locus of the measurement point are obtained. Then, an operation for obtaining a first straight line parallel to the normal line through the center of the rotation locus of the measurement point and a rotation axis of the fifth rotary joint are rotated, and this rotation moves on a circular rotation locus. Three or more different positions of the measurement points are measured by a three-dimensional measuring means, and from the three or more positions on the rotation locus of the measured measurement points, the center position of the rotation locus of the measurement point and the measurement 3 or more on the rotation trajectory of the point Through the center of rotation locus of said measurement point seeking and the normal of the plane performs the act of obtaining a second straight line parallel to the normal line comprising a plurality of positions,
The intersection of the first straight line and the second straight line is taken as the measurement position of the tip 2-axis orthogonal point, and the measurement position of the tip 2-axis orthogonal point is coordinate-converted to a position on the reference coordinate and the reference coordinate on XY coordinate value is obtained, and the obtained XY coordinate value is plotted on the XY coordinate plane of the reference coordinate, and a line segment connecting these plotted points is defined as a measurement dependent line segment, and the measurement dependent line segment is extended to a straight line. Obtain the length of the perpendicular drawn from the origin of the reference coordinates as a deviation amount,
A method for detecting an inter-axis offset of a robot, wherein the obtained deviation amount is set as a total inter-axis offset amount of the second, third, and fifth rotary joints. In the detection method of the offset between axes of the robot according to claim 1, instead of the operation of determining the reference coordinates,
The first link is rotated to three or more arbitrary angular positions different from each other while fixing the rotation axes of at least the second to fourth rotary joints among the first to sixth rotary joints. The measurement point is measured by measuring three or more different positions of the measurement point that move on a circular rotation locus by the rotation of the rotation axis of the sixth rotary joint, using a three-dimensional measurement unit. Rotation of the measurement point by obtaining the center position of the rotation locus of the measurement point and the normal of the plane including the plurality of positions on the rotation locus of the measurement point An operation for obtaining a first straight line that is parallel to the normal line through the center of the trajectory, and a rotation axis of the fifth rotary joint is rotated, and the measurement points that move on a circular rotational trajectory by this rotation are different from each other. 3 or more 3 positions A plane including three or more positions on the rotation locus of the measurement point and three or more positions on the rotation locus of the measurement point from three or more positions on the rotation locus of the measurement point. To obtain a second straight line that passes through the center of the rotation locus of the measurement point and is parallel to the normal, and obtains the intersection of the obtained first straight line and the second straight line. The measurement position of the tip 2 axis orthogonal point,
From the measurement positions of the three or more tip 2-axis orthogonal points obtained at the respective angular positions, the center position of an arc passing through the measurement positions of the plurality of tip 2-axis orthogonal points, and the plurality of tip 2-axis orthogonal points And the normal of the plane including the measurement position, the center of the arc passing through the measurement positions of the plurality of tip biaxial orthogonal points as the origin, and the measurement position of the plurality of tip two axis orthogonal points passing through the origin A straight line parallel to the normal of the plane including the Z axis, an arbitrary straight line passing through the origin and orthogonal to the Z axis, an X axis passing through the origin, and a straight line passing through the origin and perpendicular to both the Z and X axes is the Y axis. A method for detecting an offset between axes of a robot, characterized in that reference coordinates are defined. In the detection method of the inter-axis offset of the robot according to claim 1 or 2, instead of the operation until determining the inter-axis offset amount after determining the reference coordinates,
Arbitrary plural positions within the movable range of the tip 2-axis orthogonal point are determined as the movement target position,
When the tip 2 axis orthogonal point is moved to the plurality of movement target positions by rotating the rotation axes of the first to third rotation joints,
Origin position errors of the motors driving the first to fourth links via the first to fourth rotary drive systems including the rotation axes of the first to fourth rotary joints; The position error of the tip 2-axis orthogonal point due to the deflection of each fourth rotational drive system, the length error of each of the first to fourth links, and the angle error between the rotational joint and the rotational axis of the next rotational joint After performing correction processing to prevent
The tip 2 axis orthogonal point is moved to each of the plurality of movement target positions by rotating the rotation axes of the first to third rotation joints,
At each of the movement target positions, the rotation axis of the sixth rotary joint is rotated, and three or more different positions of the measurement points that move on the circular rotation locus by this rotation are measured by a three-dimensional measuring means, From the measured three or more positions on the rotation locus of the measurement point, the center position of the rotation locus of the measurement point and the normal of the plane including the three or more positions on the rotation locus of the measurement point are obtained. Then, an operation for obtaining a first straight line parallel to the normal line through the center of the rotation locus of the measurement point and a rotation axis of the fifth rotary joint are rotated, and this rotation moves on a circular rotation locus. Three or more different positions of the measurement points are measured by a three-dimensional measuring means, and from the three or more positions on the rotation locus of the measured measurement points, the center position of the rotation locus of the measurement point and the measurement 3 or more on the rotation trajectory of the point Through the center of rotation locus of said measurement point seeking and the normal of the plane performs the act of obtaining a second straight line parallel to the normal line comprising a plurality of positions,
The intersection of the first straight line and the second straight line is taken as the measurement position of the tip 2-axis orthogonal point, and the measurement position of the tip 2-axis orthogonal point is coordinate-converted to a position on the reference coordinate and the reference coordinate on And XY coordinate values of the robot coordinates of the respective movement target positions are converted into XY coordinate values on the reference coordinates to obtain movement target positions on the reference coordinates,
For each movement target position, the XY coordinate value of the movement target position and the XY coordinate value of the measurement position of the tip 2-axis orthogonal point are plotted on the XY plane of the reference coordinates, and the XY from the origin of the reference coordinates is plotted. The length of the line segment to the movement target position on the plane is set as a target position radius, and a straight line passing through the reference coordinate origin and the movement target position from the measurement position of the tip 2-axis orthogonal point on the XY plane The length of the perpendicular line down to the offset error component,
For each moving target position, the relationship between the target position radius and the offset error component is plotted in a graph in which the target position radius is taken on one of two orthogonal axes and the offset error component is taken on the other axis. Then, when a straight line passing through these plotted points is taken as an error straight line, the length of the perpendicular drawn from the origin of the graph to the error straight line is obtained as a deviation amount,
A method for detecting an inter-axis offset of a robot, wherein the obtained deviation amount is set as a total inter-axis offset amount of the second, third, and fifth rotary joints. A 6-axis robot in which first to sixth links constituting a robot arm are sequentially connected to a base link fixed to a mounting surface by first to sixth rotary joints, The rotation center line of the rotation axis of the second rotation joint connecting the second link to one link is orthogonal to the rotation center line of the rotation axis of the first rotation joint connecting the first link to the base link. The rotation center line of the rotation axis of the third rotation joint connecting the third link to the second link is parallel to the rotation center line of the rotation axis of the second rotation joint, The rotation center line of the rotation axis of the fourth rotation joint connecting the fourth link to the link is parallel to the direction perpendicular to the rotation center line of the rotation axis of the third rotation joint, and the fourth link The fifth rotary joint connecting the fifth link The rotation center of the rotation shaft of the sixth rotation joint that connects the sixth link to the fifth link so that the rotation center line of the rotation shaft is parallel to the direction orthogonal to the rotation center line of the rotation shaft of the fourth rotation joint. The lines are respectively set to be orthogonal to the rotation center line of the rotation axis of the fifth rotation joint,
The robot coordinates are set on the rotation center line of the rotation axis of the first rotation joint, and the coordinates of the first to sixth links are set on the rotation center lines of the rotation axes of the first to sixth rotation joints, respectively. And a six-axis robot in which the coordinates of the hand are set at the same position with the predetermined position existing on the rotation center line of the rotation axis of the sixth rotation joint of the sixth link as the hand,
A measurement point that operates integrally with the sixth link is provided, and a three-dimensional measurement unit that can measure a three-dimensional position of the measurement point is provided,
While the robot arm is held in an arbitrary posture, the robot arm is rotated around the rotation axis of the first rotary joint, and the three or more different measurement points that move on a circular rotation trajectory due to this rotation are rotated. Measure multiple positions with 3D measuring means,
From the measured three or more positions on the rotation locus of the measurement point, the center position of the rotation locus of the measurement point and the normal of the plane including the three or more positions on the rotation locus of the measurement point are obtained. The center of the rotation locus of the measurement point is the origin, the straight line passing through the origin and parallel to the normal is the Z axis, the arbitrary straight line passing through the origin and perpendicular to the Z axis is the X axis, and the origin is Through which a reference coordinate having a Y axis as a straight line perpendicular to both the Z and X axes is defined;
Thereafter, at an appropriate time, in a state where the hand is in an arbitrary reference posture, the position of the measurement point is measured by the three-dimensional measuring means, and the rotation axis of the sixth rotary joint is rotated to perform this rotation. The three or more different positions of the measurement points moving on the circular rotation locus are measured by the three-dimensional measuring means, and the measurement points are measured from the three or more positions on the rotation locus of the measured points. A first position parallel to the normal passing through the center of the rotation trajectory of the measurement point is obtained by obtaining a center position of the rotation trajectory and a plane normal including three or more positions on the rotation trajectory of the measurement point. An operation for obtaining a straight line, and rotating a rotation axis of the fifth rotary joint and measuring three or more different positions of the measurement point moving on a circular rotation locus by this rotation by a three-dimensional measuring means Rotation trajectory of the measured point The center of the rotation locus of the measurement point is obtained by obtaining the center position of the rotation locus of the measurement point and the normal of the plane including the plurality of positions on the rotation locus of the measurement point. And a second straight line that is parallel to the normal through
A vector from the measurement position of the measurement point at the reference posture of the hand to the intersection of the first straight line and the second straight line is obtained and the vector is used as a detection vector for the tip 2-axis orthogonal point, An angle formed by the detection vector and the three axes XYZ of the reference coordinates is obtained, and the angle is set as a directivity angle.
A plurality of positions on any one plane including the rotation center line of the rotation axis of the first rotation joint extending from the rotation center line of the rotation axis of the first rotation joint are set as movement target positions;
By rotating the rotation axes of the second and third rotation joints, the rotation center lines of the rotation axes of the fifth rotation joint and the rotation axis of the sixth rotation joint are moved to the plurality of movement target positions. When moving the tip 2-axis orthogonal point that is the intersection of
Origin position errors of the motors driving the first to fourth links via the first to fourth rotary drive systems including the rotation axes of the first to fourth rotary joints; The position error of the tip 2-axis orthogonal point due to the deflection of each fourth rotational drive system, the length error of each of the first to fourth links, and the angle error between the rotational joint and the rotational axis of the next rotational joint After performing correction processing to prevent
After rotating the rotary shaft of the first rotary joint to a position where the two-axis orthogonal point can be moved to the movement target position by rotation of the rotary shaft of the rotary joint of the second and third rotary joints The tip 2 axis orthogonal point is moved to each of the plurality of movement target positions by rotating the rotation axes of the second and third rotation joints in a state where the rotation axis of the first rotation joint is fixed. ,
At each of the movement target positions, the position of the measurement point is measured by the three-dimensional measuring means while the hand is held in the reference posture, and the measured position of the measurement point is converted into a position on the reference coordinate. Then, the position of the end point of the detection vector when the detection vector is created so as to be the directivity angle from the position of the measurement point on the reference coordinate is the measurement position on the reference coordinate of the tip 2-axis orthogonal point. And
Plot the XY coordinate value of the measurement position on the reference coordinate of the two-axis orthogonal point of the tip for a plurality of obtained movement target positions on the XY coordinate plane of the reference coordinate, and measure the line segment connecting these plotted points As a dependence line segment, the length of the perpendicular line drawn from the origin of the reference coordinates to the straight line obtained by extending this measurement dependence line segment is obtained as a deviation amount,
A method for detecting an inter-axis offset of a robot, wherein the obtained deviation amount is set as a total inter-axis offset amount of the second, third, and fifth rotary joints. In the detection method of the offset between axes of the robot according to claim 4, instead of the operation of determining the reference coordinates,
The first link is rotated to three or more arbitrary angular positions different from each other while fixing the rotation axes of at least the second to fourth rotary joints among the first to sixth rotary joints. The measurement point is measured by measuring three or more different positions of the measurement point that move on a circular rotation locus by the rotation of the rotation axis of the sixth rotary joint, using a three-dimensional measurement unit. Rotation of the measurement point by obtaining the center position of the rotation locus of the measurement point and the normal of the plane including the plurality of positions on the rotation locus of the measurement point An operation for obtaining a first straight line that is parallel to the normal line through the center of the trajectory, and a rotation axis of the fifth rotary joint is rotated, and the measurement points that move on a circular rotational trajectory by this rotation are different from each other. 3 or more 3 positions A plane including three or more positions on the rotation locus of the measurement point and three or more positions on the rotation locus of the measurement point from three or more positions on the rotation locus of the measurement point. To obtain a second straight line that passes through the center of the rotation locus of the measurement point and is parallel to the normal, and obtains the intersection of the obtained first straight line and the second straight line. The measurement position of the tip 2 axis orthogonal point,
From the measurement positions of the three or more tip 2-axis orthogonal points obtained at the respective angular positions, the center position of an arc passing through the measurement positions of the plurality of tip 2-axis orthogonal points, and the plurality of tip 2-axis orthogonal points And the normal of the plane including the measurement position, the center of the arc passing through the measurement positions of the plurality of tip biaxial orthogonal points as the origin, and the measurement position of the plurality of tip two axis orthogonal points passing through the origin A straight line parallel to the normal of the plane including the Z axis, an arbitrary straight line passing through the origin and orthogonal to the Z axis, an X axis passing through the origin, and a straight line passing through the origin and perpendicular to both the Z and X axes is the Y axis. A method for detecting an offset between axes of a robot, characterized in that reference coordinates are defined. In the detection method of the inter-axis offset of the robot according to claim 4 or 5, instead of the operation until obtaining the inter-axis offset amount after obtaining the detection vector and the directivity angle,
Arbitrary plural positions within the movable range of the tip 2-axis orthogonal point are determined as the movement target position,
When the tip 2 axis orthogonal point is moved to the plurality of movement target positions by rotating the rotation axes of the first to third rotation joints,
Origin position errors of the motors driving the first to fourth links via the first to fourth rotary drive systems including the rotation axes of the first to fourth rotary joints; The position error of the tip 2-axis orthogonal point due to the deflection of each fourth rotational drive system, the length error of each of the first to fourth links, and the angle error between the rotational joint and the rotational axis of the next rotational joint After performing correction processing to prevent
The tip 2 axis orthogonal point is moved to each of the plurality of movement target positions by rotating the rotation axes of the first to third rotation joints,
At each of the movement target positions, the hand is held in the reference posture, the position of the measurement point is measured by the three-dimensional measuring means, and the measured position of the measurement point is converted to a position on the reference coordinate. The detection vector is created so as to be the directivity angle from the measurement position on the reference coordinate of the measurement point, and the end point position of the detection vector is defined as the measurement position on the reference coordinate of the tip 2-axis orthogonal point. Set
The XY coordinate value on the robot coordinate of the movement target position is coordinate-converted to the XY coordinate on the reference coordinate, and the XY coordinate value on the reference coordinate of the obtained plurality of movement target positions is orthogonal to the tip 2 axes. The XY coordinate value of the measurement position of the point on the reference coordinate is plotted on the XY plane of the reference coordinate, and the length of the line segment from the origin of the reference coordinate to the movement target position on the XY plane is targeted. The offset radius component is the length of a perpendicular drawn from the measurement position of the tip 2-axis orthogonal point on the XY plane to a straight line passing through the origin of the reference coordinates and the movement target position.
For each moving target position, the relationship between the target position radius and the offset error component is plotted in a graph in which the target position radius is taken on one of two orthogonal axes and the offset error component is taken on the other axis. Then, when a straight line passing through these plotted points is taken as an error straight line, the length of the perpendicular drawn from the origin of the graph to the error straight line is obtained as a deviation amount,
A method for detecting an inter-axis offset of a robot, wherein the obtained deviation amount is set as a total inter-axis offset amount of the second, third, and fifth rotary joints. In the detection method of the offset between axes of the robot according to any one of claims 1, 2, 4, and 5,
Arbitrary plural positions on an arbitrary plane including the rotation center line of the rotation axis of the first rotation joint extending from the rotation center line of the rotation axis of the first rotation joint are determined as movement target positions, and these movement target positions The operation of moving the tip 2-axis orthogonal point is performed on two planes extending in opposite directions from the rotation center line of the rotation axis of the first rotation joint, and the rotation of the rotation axis of the first rotation joint is performed. The transition of the two-axis orthogonal point of the tip from a plane extending in one direction from the center line to a plane extending in the opposite direction is performed in a state where the rotation axis of the first rotary joint is fixed. A method for detecting an offset between axes of a robot, wherein the robot arm is reversed by rotating a rotation axis of a rotation joint of the rotation joint. The method for detecting an offset between axes of a robot according to claim 3 or 6,
The operation of moving the tip 2-axis orthogonal point to the plurality of arbitrary movement target positions is as follows:
By rotating the rotation shafts of the second and third rotary joints in one direction and the other direction, the robot arm is tilted to the other side and the forward bending posture where the robot arm is tilted to the one side from the first link. A method for detecting an inter-axis offset of a robot, characterized in that it is performed in both a back-bend posture as a posture.

Images