JP2010112731A - Joining method of coordinate of robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a method acquiring the relation between flange coordinate F and camera coordinate C with a camera in a visual inspecting device which is constituted by attaching the camera to the flange at the distal end of a robot arm. <P>SOLUTION: The moving vector of a camera 12 on picked-up images is acquired when moving a flange 11 along the XYZ axis of the flange coordinate F. The inclination of the camera coordinate C with respect to the flange coordinate F is operated based on this. The position of the camera coordinate C on the flange coordinate F is acquired by a photograph screen of a marker M when the flange 11 is rotated around the Zc axis of the camera coordinate C and a photograph screen of the marker M when the flange 11 is rotated around a straight line, passing through the origin of the flange coordinate F, parallel to the Zc axis of the camera coordinate C. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

  The present invention relates to a robot coordinate combining method for obtaining a position and an inclination of a camera coordinate relative to a coordinate of a hand part in a robot having a camera attached to the hand part.

In the factory production line, for example, a visual inspection device composed of a combination of a robot and a camera is installed in the final process of assembly, and a desired part of the workpiece is photographed with the camera, and the photographed image is displayed on a display for visual inspection. Has been done. This visual inspection apparatus attaches a camera to a hand portion of a robot and moves the hand portion to a preset position to acquire a photographed image by the camera. In this visual inspection apparatus, in order to set the movement position of the hand portion, it is necessary to obtain the relationship between the coordinates of the hand portion of the robot and the coordinates of the camera in advance.
In Patent Document 1, a three-dimensional camera is used to obtain the relationship between the coordinates of the robot's hand and the coordinates of the camera.
Japanese Patent Laid-Open No. 10-63317

  However, 3D cameras are expensive. Since the camera used in the visual inspection apparatus is usually a two-dimensional camera, it is very convenient if the relationship between the coordinates of the hand portion and the coordinates of the camera can be obtained with the two-dimensional camera. For this reason, there is a strong demand for the appearance of a method for obtaining the relationship between the coordinates of the hand portion and the coordinates of the camera using a two-dimensional camera attached to the hand portion.

  SUMMARY OF THE INVENTION An object of the present invention is to provide a robot coordinate combining method capable of obtaining the relationship between the coordinates of the hand portion and the coordinates of the camera by a two-dimensional camera attached to the hand portion.

  In order to achieve the above object, in the present invention, the hand portion is provided at a fixed position before and after the movement by moving the hand portion by a desired distance in parallel with the respective axes Xf, Yf, Zf of the coordinates of the hand portion. The captured marker is photographed, and the tilt of the camera coordinate relative to the hand coordinate is obtained using the photographed image. Next, of the camera coordinate axes Xc, Yc, and Zc, the optical axis of the lens is the Zc axis, and the hand is moved by a desired amount parallel to the Xc axis and / or Yc axis of the camera coordinates before and after the movement. The marker is photographed in this, and the amount of camera movement per pixel of the camera is calculated from the photographed image, and it is assumed that the Zc axis of the camera coordinates passes through the origin of the coordinates of the hand portion. Then, the hand part is rotated by a desired amount, and the marker is photographed at at least three positions from the start to the end of the rotation, and the Zc axis of the camera coordinates and the hand part are obtained from the taken image and the length of one pixel. The distance between the Zc axis and the axis parallel to the camera coordinate passing through the origin of the coordinate is obtained. Further, assuming that the plane including the Xc axis and Yc axis of the camera passes through the origin of the coordinates of the hand part, the hand part is moved by a desired amount in parallel with the Xc axis or Yc axis of the camera coordinates, and before and after the movement The marker is photographed, the distance between the hand part and the marker is calculated from this photographed image, the amount of movement of the camera per pixel and the focal length of the lens, and a plane including the Xc axis and the Yc axis of the camera coordinates is the hand side. Assuming that it passes through the origin of the coordinates of the part, the hand part is rotated by a desired angle around the Xc axis or Yc axis of the camera coordinates, and the marker is photographed before and after the rotation. Then, the relative movement distance of the marker before and after rotation is obtained, and the distance from the rotation center axis passing through the origin of the coordinates of the hand part to the marker is calculated from the movement distance and rotation angle, and the distance between the rotation center axis and the marker is calculated. To the origin of the camera coordinates and the marker The distance from the origin of the coordinates of the hand to the origin of the coordinates of the camera is obtained by subtracting the distance of the camera, and the position and inclination of the coordinates of the camera relative to the coordinates of the hand of the robot are obtained by the camera attached to the hand. be able to.

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 1 shows a visual inspection apparatus. A robot 1 used in this visual inspection apparatus includes a robot main body 2 and a control device 3 for controlling the robot main body 2, and further includes a teaching pendant 4 and a display device (display means) 5 mainly including a liquid crystal display, for example. It is configured with. The teaching pendant 4 functions as an input means for inputting various data and an operating means for remotely operating the robot body 1 and includes various operation switches 4a and a display unit 4b.

The robot body 2 is configured, for example, as a six-axis vertical articulated type, and includes a base 6, a shoulder portion 7 that is supported by the base 6 in a swiveling manner in a horizontal direction, and a vertical turn on the shoulder portion 7. The lower arm 8 supported so as to be able to rotate, the upper arm 9 supported by the lower arm 8 so as to be rotatable in the vertical direction and capable of rotating (twisting), and supported by the upper arm 9 so as to be capable of rotating in the vertical direction. It comprises a wrist 10 and a flange 11 that is rotatably supported by the wrist 10. The shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10 and the flange 11 constitute a robot arm, and a camera 12 is attached to the flange 11 which is a tip portion of the arm tip via a stay 12a. Yes.
The teaching pendant 4 for remotely operating the robot body 2 moves the flange 11 in parallel with the coordinate axes of the flange coordinates described later, or moves (translates) while maintaining the flange 11 in a desired posture. It is configured to be able to.

  On the other hand, as shown in FIG. 2, the control device 3 includes a CPU 13, a drive circuit 14, and a position detection circuit 15 as position detection means. The CPU (control means) 13 includes a ROM (storage means) 16 for storing a system program for the entire robot 1, a RAM (storage means) for storing an operation program for the robot body 2 created by the teaching pendant 4, etc. ) 17 is connected, and the teaching pendant 4, the display device 5, and the camera 12 are connected.

  The position detection circuit 15 is for detecting the relative position of each joint, that is, the shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10, and the flange 11. The position detection circuit 15 includes A rotary encoder 19 as a position sensor provided in the drive motor 18 is connected. In FIG. 2, only one drive motor 18 and one rotary encoder 19 are shown, but actually, a plurality of drive motors 18 and rotary encoders 19 are provided in a one-to-one relationship with respect to each joint.

  Then, the position detection circuit 15 detects the rotation angle of the shoulder portion 7 with respect to the base 6 based on the detection signal of the rotary encoder 19, the turning angle of the lower arm 8 with respect to the shoulder portion 7, the turning angle and the twisting angle of the upper arm 9 with respect to the lower arm 8, The turning angle of the wrist 10 with respect to the upper arm 9 and the rotation angle of the flange 11 with respect to the wrist 10 are detected, and the operation angle information is given to the CPU 13. When the CPU 13 operates the shoulder unit 7, the lower arm 8, the upper arm 9, the wrist 10 and the flange 11 based on the operation program, the input signal from the position detection circuit 15 is used as a feedback signal for those operations (drive motor 18). To control.

  Here, three-dimensional coordinates are fixed to the base 6, the shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10, and the flange 11, respectively. Among these, the coordinates of the stationary base 6 are the robot coordinates (indicated by S in FIG. 1), and the other coordinate systems are the shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10 and the flange 11. The position and orientation (orientation) on the robot coordinate S change due to the operation. For example, the robot coordinate S has an origin P at the intersection of the bottom surface of the base 6 and the turning center of the shoulder portion 7, the Z-axis is defined as a coordinate axis in the height direction (vertical direction), and the X-axis and Y-axis are vertically and horizontally in a horizontal plane. Two coordinate axes are defined.

  Then, the control device 3 (CPU 13) receives position detection information of each joint of the shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10 and the flange 11 input from the position detection circuit 15 and a ROM (arm length storage means) in advance. ) According to the arm length information of the shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10 and the flange 11 stored in 16, each of the shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10 and the flange 11 is stored. The coordinate origin position and orientation can be recognized by converting the position and orientation on the robot coordinate S by the coordinate conversion calculation function.

  Now, among the shoulder portion 7, the lower arm 8, the upper arm 9, the wrist 10 and the flange 11, the coordinates of the flange 11 are as shown in FIG. 3, with the center Pf of the front end surface (surface) of the flange 11 as the origin, Two coordinate axes are defined on the front end surface of the flange 11 and one coordinate axis is defined on the rotation axis of the flange 11. Specifically, a straight line passing through the origin Pf and the center of the small hole 11a formed eccentric to the flange 11 is defined as the Y axis, a straight line passing through the origin Pf and orthogonal to the Y axis is defined as the X axis, and passing through the origin Pf , Y perpendicular to both axes, that is, a straight line that coincides with the rotation center line of the flange 11 is defined as the Z axis.

  Of the position and posture of the robot 1, that is, the position and posture of the flange 11, the position is indicated by the position on the robot coordinate S occupied by the center of the front end surface of the flange 11, which is the origin Pf of the coordinates of the flange 11. . In addition, the posture of the flange 11 is a unit length “1” vector extending in the negative direction along the Z axis from the origin Pf of the coordinates of the flange 11 and a unit extending in the positive direction along the X axis from the origin Pf. When a vector of length “1” is defined and the coordinates of the flange 11 are translated so that the origin Pf coincides with the origin P of the robot coordinate S, it is represented by both unit vectors on the robot coordinate S. .

  On the other hand, as shown in FIG. 4, the camera 12 has a fixed-focus lens 20 and a two-dimensional CCD imaging screen 21 as an imaging sensor installed at a position separated from the lens 20 by the focal length L of the lens 20. I have. The CCD imaging screen 21 (hereinafter simply referred to as the imaging screen 21) is formed by arranging a large number of CCDs vertically and horizontally, and one CCD constitutes one pixel. The camera 12 is also set with three-dimensional coordinates. As shown in FIGS. 4 and 5, the coordinates of the camera 12 are coordinated with an X axis representing a straight line that bisects the imaging screen 21 in the vertical direction through the origin Pc with the center point Pc of the imaging screen 21 as the origin. A straight line that bisects the imaging screen 21 in the left-right direction through the origin Pc is the Y axis, and a straight line that passes through the origin Pc and the center of the lens 20 (matches the optical axis) is the Z axis.

  In the following, the coordinates of the flange 11 and the coordinates of the camera 12 are referred to as the flange coordinates F and the camera coordinates C, respectively, and in order to distinguish the coordinate axes of the flange coordinates F and the camera coordinates C from each other, X, Y and Z are indicated by subscripts f and c.

By the way, in order to determine the shooting point of the camera 12, it is necessary to know in advance the relationship of the camera coordinates C to the flange coordinates F. In other words, in order to photograph the part to be photographed of the workpiece by the camera 12, the optical axis (Zc axis) of the camera 12 faces the part to be photographed for the workpiece, and the whole part to be photographed is in the field of view of the camera 12. The position and posture of the flange 11 must be determined so as to be in focus in the state. For this purpose, as shown in FIG. 5, the position and inclination (posture) of the camera coordinates C with respect to the flange coordinates F must be known.
The ROM 16 of the control device 3 stores a program for obtaining the position and inclination of the camera coordinates C with respect to the flange coordinates F. Hereinafter, a method for obtaining the relationship between the position and inclination of the camera coordinate C with respect to the flange coordinate F by this program will be described with reference to the flowchart shown in FIG.

FIG. 7 shows the robot body 2 with a skeleton, and also shows a workpiece W that is a visual inspection object. A small dot-like marker M is attached to the workpiece W. The marker M may be formed by drawing with a paint, or may be substituted by, for example, a head of a small screw attached to the workpiece W.
First, the inclination of the camera coordinate C with respect to the flange coordinate F is obtained. Therefore, the user uses the teaching pendant 4 to move the flange 11 of the robot body 2 by a desired distance in parallel with the Xf axis of the flange coordinates F, in parallel with the Yf axis, and in parallel with the Zf axis. .

  At this time, in order to focus on the marker M, the marker M is not moved out of the field of view by the movement, and the camera 12 and thus the posture of the flange 11 are not changed before and after the movement. Images are taken once (step S1: first step in the flowchart of FIG. 6). Then, the CPU 13 calculates the movement distances of the flange 11 parallel to the Xf axis, the Yf axis, and the Zf axis from the positions before and after the movement of the flange coordinates F on the robot coordinates S, and stores them in the RAM 17.

At this time, the flange 11 may be moved in parallel with one axis and then moved back in parallel to the next axis after returning to the original position. Then, it may be moved parallel to the next axis from that position.
When the flange 11 is moved by a desired distance in parallel to the Xf axis, parallel to the Yf axis, and parallel to the Zf axis, the CPU 13 of the control device 3 thereafter tilts the camera coordinates C with respect to the flange coordinates F. Is estimated (step S2).

  Before explaining the method of estimating the inclination of the camera coordinate C with respect to the flange coordinate F, as a prior knowledge, a vector (first vector) from the origin P of the robot coordinate S to the marker M of the workpiece W, the origin of the robot coordinate S A transformation matrix (second matrix) from P to the origin Pf of the flange coordinate F, a vector (third vector) from the origin Pf of the flange coordinate F to the marker M of the workpiece W, the origin Pf of the flange coordinate F to the camera coordinate C The relationship between the transformation matrix (fourth matrix) to the origin Pc and the vector (fifth vector) from the origin Pc of the camera coordinates C to the marker M of the workpiece W will be described, and the inclination of the camera coordinates C will be based on these relationships. Let us derive a formula to find

  When the robot body 2 is at a position indicated by a solid line in FIG. 7 (hereinafter referred to as i position), the first vector to the fifth vector have the following relationship (1).

The first vector, the third vector, and the fifth vector are represented by vectors of 1 row and 4 columns (the fourth element is 0). The second matrix and the fourth matrix are shown as a 4 × 4 matrix.
Further, when the robot body 2 is at a position indicated by a broken line in FIG. 7 (hereinafter referred to as j position), the first vector to the fifth vector have the relationship of the following equation (2).

The first vector and the fifth vector are represented by vectors of 1 row and 4 columns (the fourth element is 0). The second matrix, the third matrix, and the fourth matrix are shown as a 4 × 4 matrix.
When the robot body 2 is moved from the i position to the j position, the movement vector of the flange 11 is equal to the difference between the third vector at the j position and the third vector at the i position. Can be expressed as Similarly, when the robot body 2 is moved from the i position to the j position, the movement vector of the camera 12 is equal to the difference between the fifth vector at the j position and the fifth vector at the i position. It can be expressed by a formula.

Note that the movement vector of the flange coordinates and the movement vector of the camera coordinates are represented by vectors of one row and four columns (the fourth element is 0).
When it is considered that the robot body 2 has moved from the i position to the j position, the expression (2) replaces the movement vector of the expression (3) with the third vector of the expression (1), and the fifth expression of the expression (1) Since this is the same as the case where the movement vector of equation (4) is replaced with the vector, the following equation (5) is established.

  In the equation (5), when the left side and the right side are divided by the second vector at the common i position, the following equation (6) is obtained.

  In step S1, the flange 11 is moved to a desired distance in parallel with the Xf axis, Yf axis, and Zf axis of the flange coordinate F. In FIG. 7, the flange 11 is moved from the i position to the j position. It corresponds to the case. At this time, the distance that the flange 11 moved in parallel with the Xf axis, Yf axis, and Zf axis of the flange coordinate F is Δx, Δy, and Δz, respectively, and the movement vector in the direction of each coordinate axis from the i position to the j position is the i position. Is expressed by the following equations (7) to (9).

  Further, when the flange 11 is moved by a desired distance parallel to the Xf axis, Yf axis, and Zf axis of the flange coordinate F, the movement vector of the camera coordinate C by the movement is used as a reference with respect to the camera coordinate C at the i position. If the movement coordinates parallel to the Xc axis, Yc axis, and Zc axis of the camera coordinate C are decomposed and expressed, the following equations (10) to (12) are obtained.

  On the other hand, when the movement vector from the i position to the j position of the flange coordinate F is expressed using the expressions (7) to (9), the following expression (13) is obtained.

  Since the left side of the equation (6) is equal to the movement vector from the i position to the j position of the flange coordinate F in the equation (13), the equation (6) can be expressed as the following equation (14). .

  And if the right side of (14) type | formula is shown using the said (10)-(13) type | formula, it will become the following (15) type | formula. Therefore, equation (14) can be expressed as equation (16).

  By the way, in order to obtain the inclination of the camera coordinate C with respect to the flange coordinate F, it is only necessary to consider the rotation component on the assumption that the origin Pc of the camera coordinate C coincides with the origin Pf of the flange coordinate F (offset is 0). Therefore, the equation (16) becomes the following equation (17).

Note that Δpcx, Δpcy, Δpcz in parentheses are indicated by vectors of 1 row and 3 columns.
Now, the movement vector on the camera coordinate C of the camera 12 when the flange 11 is moved in each of the Xf axis, Yf axis, and Zf axis directions is an Xc axis direction component, a Yc axis direction component, and a Zc axis direction component. Can be represented. Among these, the Xc-axis direction component and the Yc-axis direction component are the Xc-axis direction component from the image position of the marker M before movement and the image position of the marker M after movement, which are captured on the imaging screen 21 of the camera 12, and the Yc axis. It can be obtained from the direction component.

  For example, as shown in FIG. 8, when the flange 11 is moved in the Xf-axis direction, the image position of the marker M before the movement is the position indicated by Ixa, and the image position of the marker M after the movement is Ixb. The movement vector is indicated by an arrow from Ixa to Ixb. Incidentally, the marker M appears on the imaging screen 21 upside down and left and right with respect to the actual movement direction of the camera 12, so the movement vector is converted into a vector that is upside down and left and right opposite to the vector on the imaging screen 12. There is a need. In addition, when the marker M is not in focus at the moved position and the image is blurred, the center point of the blurred range is set as the video position of the marker M. Similarly, even when the marker M is in focus, if the image is enlarged, the graphic center of the image of the mark M is set as the video position.

  The length of the Xc-axis direction component and the Yc-axis direction component of this movement vector is to draw a right triangle whose hypotenuse is a straight line connecting Ixa and Ixb, and a side parallel to the Xc axis and a side parallel to the Yc axis. Indicated by the length of The length of each side can be obtained by the number of pixels (squares having the same length and width) that each side passes through. Here, since only the inclination of the camera coordinates C with respect to the flange coordinates F is to be obtained, it is not necessary to obtain the actual movement amounts in the Xc-axis direction and the Yc-axis direction, and it is only necessary to know the direction. For this reason, since it is sufficient to know the relative lengths of the Xc-axis direction component and the Yc-axis direction component of the movement vector, the magnitudes of the Xc-axis direction component and the Yc-axis direction component are expressed by the number of pixels. When the flange 11 is moved in the Yf-axis direction and the Zf-axis direction, the magnitudes of the Xc-axis direction component and the Yc-axis direction component of the movement vector in the Xc-axis, Yc-axis, and Zc-axis directions on the camera coordinate C on the camera 12 Similarly, it can be obtained from the photographed image.

However, in the method of detecting the movement vector from the captured image, the Zc-axis direction component of the movement vector cannot be acquired because the shooting screen 21 of the camera 12 is two-dimensional.
Therefore, taking the inverse matrix of the rotation matrix of equation (17), the following equation (18) is obtained.

  Further, when the inverse matrix of the movement vector of the flange 11 in the equation (18) is taken, the following equation (19) is obtained.

  Since the right side of the equation (19) is a movement vector of the camera 12 on the camera coordinates C, the equation (19) further becomes the following equation (20).

  Here, ax, ay, and az are decomposed into the Xc axis direction, the Yc axis direction, and the Zc axis direction of the movement vector on the camera coordinate C of the camera 12 when the flange 11 is moved in parallel with the Xf axis. Each component is shown. bx, by, and bz are obtained when the movement vector on the camera coordinate C of the camera 12 when the flange 11 is moved in parallel with the Yf axis is decomposed in the Xc axis direction, the Yc axis direction, and the Zc axis direction. Each component is shown. cx, cy, and cz are obtained when the moving vector on the camera coordinate C of the camera 12 when the flange 11 is moved in parallel with the Zf axis is decomposed in the Xc axis direction, the Yc axis direction, and the Zc axis direction. Each component is shown.

  However, as described above, az, bz, and cz are unknown. However, since the inverse determinant of the middle side of the equation (20) is the amount of movement when the flange 11 is moved in parallel to the Xf axis, the Yf axis, and the Zf axis, the CPU 13 solves the inverse matrix equation stored in the ROM 16. The inverse determinant can be solved by a program. Since az, bz, and cz satisfy the conditions shown in the following equations (21) to (24), solving the equation (20), that is, obtaining the inclination of the camera coordinates C with respect to the flange coordinates F. Yes (third step).

Equation (21) is based on the condition that the rotation matrix component of 3 columns and 3 rows of the simultaneous conversion matrix of 4 columns and 4 rows is 1, and the magnitude (norm) of each vector of the rotation matrix components is 1. Is. In the equations (22) to (24), the components of the movement vector in the Xc axis, Yc axis, and Zc axis directions are orthogonal to each other, and therefore the inner product is based on the condition that it is zero.
From the above, when the flange 11 is moved in parallel to the Xf axis, the Yf axis, and the Zf axis, the CPU 13 determines the inclination of the camera coordinates C with respect to the flange coordinates F. The Xc-axis direction component and the Yc-axis direction component of the movement vector are obtained as the number of pixels. Next, the CPU 13 obtains (estimates) the inclination of the camera coordinates C with respect to the flange coordinates F by solving the equation (20).

  When the inclination of the camera coordinate C with respect to the flange coordinate F is obtained in this way, the user then uses the teaching pendant 4 to move the flange 11 of the robot body 2 along the Xc axis and the Yc axis of the camera coordinate C. At least one of them, in this embodiment, each is moved by a desired distance in parallel to the Xc axis and the Yc axis. At this time, the marker M is in a photographing field of view of the camera 12 so that the posture of the camera 12 does not change before and after the movement, and the marker M is photographed twice before and after the movement by the imaging screen 21. (Step S3: 4th process).

Then, the CPU 13 obtains the movement distance of the flange 11 along the Xc axis and the Yc axis from the position before and after the movement of the flange coordinate F on the robot coordinate S (movement distance calculation means) and stores it in the RAM 17. Next, for each of the movement parallel to the Xc axis of the camera coordinates C and the movement parallel to the Yc axis, the CPU 13 displays the image position of the marker M before the movement and the image after the movement, which are captured on the imaging screen 21 of the camera 12. The number of pixels in the Xc-axis direction and the number of pixels in the Yc-axis direction between the positions are counted. FIG. 9 illustrates image positions before and after the movement of the marker M on the imaging screen 21 when the flange 11 moves in the Yc axis direction. Then, the CPU 13 calculates the movement distance per pixel by dividing the obtained movement distance in the Xc and Yc axis directions by the number of pixels in the Xc and Yc axis directions (step S4: fifth step). The amount of movement per pixel is under the condition that the marker M and the camera 12 are separated by a distance that allows the marker M to be accurately focused.
In order to calculate the movement distance per pixel, it can be obtained (estimated) by moving the flange 11 only in one of the Xc axis direction and the Yc axis direction. When the movement distance per pixel is obtained in the two directions of the Xc axis direction and the Yc axis direction, a more accurate movement amount per pixel can be obtained by taking the average.

  Next, assuming that the Zc axis of the camera 12 passes through the origin Pf of the flange coordinate F, the user operates the teaching pendant 4 to rotate the flange 11 about the assumed Zc axis by a desired angle. The hypothetical Zc axis is indicated by Zcf in FIG. This rotation operation about the Zcf axis is performed with the marker M in the shooting field of view of the camera 12 and in focus with the marker M so that the posture of the camera 12 does not change, including before and after rotation. During the rotation, photographing is performed at least three times (step S5: sixth step). FIGS. 10A, 10B, and 10C illustrate the image position of the marker M when photographing is performed once at a desired time during the rotation, once at a desired time during the rotation, and a total of three times at the end of the rotation.

  Subsequently, the CPU 13 calculates at least two straight lines connecting the video positions P1 to P3 of the three markers M, and calculates orthogonal straight lines L1 and L2 passing through the midpoints of the two straight lines. The intersection of these two orthogonal straight lines L1 and L2 is calculated. And CPU13 determines the intersection of two orthogonal straight lines as the rotation center Tc (7th process). Next, the CPU 13 obtains the distance and direction between the rotation center Tc and the origin Pc of the camera coordinates C (step S6).

  In other words, the rotation center Tc is a straight line passing through the origin Pf of the flange coordinate F and is on a straight line (Zcf axis) parallel to the Zc axis of the camera coordinate C. Therefore, the camera is detected from the position of the rotation center Tc on the imaging screen 21. A vector V1 up to the origin Pc of the coordinate C is obtained. Then, the distance between the rotation center Tc and the origin Pc is obtained as the number of pixels for the Xc-axis direction component and the Yc-axis direction component of the vector V1, and the number of pixels of these two-axis direction components is multiplied by the distance per pixel. The lengths of the Xc-axis direction component and the Yc-axis direction component are obtained, and further, the distance between the origin Pc of the camera coordinates C and the rotation center Tc is obtained from the lengths of the Xc-axis direction component and the Yc-axis direction component. As described above, the straight line (Zcf axis) parallel to the Zc axis of the camera coordinate C passing through the origin Pf of the flange coordinate F, and the direction and distance from the origin Pf of the flange coordinate F to the Zc axis of the camera coordinate C are obtained. (Eighth process). A vector indicating the direction and distance from the origin Pf of the flange coordinate F to the Zc axis of the camera coordinate C is represented by V2 in FIG.

  In this way, the inclination of the camera coordinate C with respect to the flange coordinate F and the distance in which direction the Zc axis of the camera coordinate C passes from the origin Pf of the flange coordinate F are obtained. The origin Pc of the camera coordinates C is on a straight line passing through the Zc axis, but it is not yet determined how far away from the origin of the origin Pf of the flange coordinates F. In the following, it is determined where the origin Pc of the camera coordinates C is on the straight line through which the Zc axis passes.

  That is, the user assumes that a plane including the Xc axis and the Yc axis of the camera coordinate C passes through the origin Pf of the flange coordinate F, and is parallel to the Xc axis or the Yc axis of the camera coordinate C, here, parallel to the Xc axis. Is moved by a desired distance G (step S7: ninth stroke). Also at this time, the marker M is in the imaging field of view of the camera 12 and is in focus so that the posture of the camera 12 does not change before and after the movement, and the marker M is moved before and after the movement. Take two shots.

  Then, the CPU 13 calculates the movement distance G of the flange 11 from the position of the flange coordinate F before and after the movement on the robot coordinate S, and the image position and movement of the marker M before the movement, which is captured on the imaging screen 21 of the camera 12. The number of pixels in the Xc-axis direction between the subsequent video positions is counted. Then, the CPU 13 calculates the movement distance on the imaging screen 21 of the marker M before and after movement by multiplying the number of pixels by the length of one pixel (step S4). The length of one pixel is stored in advance in the ROM 16 (pixel length storage means). Then, the CPU 13 determines from the origin Pc of the camera coordinates C to the marker M based on the moving distance G of the flange 11 parallel to the Xc axis, the moving distance g on the imaging screen 21, and the focal length L of the lens 20 of the camera 12. Distance D is calculated (tenth step).

  Here, as shown in FIG. 11, the center of the lens 20 is O, the position of the marker M before the movement is P1, the position after the movement is P2, and the image position of the marker M on the imaging screen 21 before the movement is the image position. If I1 and the image position after movement are I2, the triangles O, P1, P2 and the triangles O, I1, I2 are similar. Further, since the distance from the center O of the lens 21 to the imaging screen 21 is equal to the focal length L of the lens 20, the distance d from the center O of the lens 21 to the straight line connecting P1 and P2 is L · G / g. Become. Since the distance from the straight line connecting P1 and P2 to the center of the imaging screen 21, that is, the origin Pc of the camera coordinates C is d + L, D = (L · G / g) + L.

Next, the user assumes that a plane including the Xc axis and the Yc axis of the camera coordinate C passes through the origin Pf of the flange coordinate F, and the Xc axis or Yc axis of the camera coordinate C at this assumed position, for example, the Xc axis. Is rotated by a desired angle θ. Also at this time, the marker M is in the imaging field of view of the camera 12 and in focus, and the marker M is rotated by the imaging screen 21 so that the posture of the camera 12 does not change before and after the movement. Take two shots before and after rotation (11th step).
Then, the CPU 13 obtains a distance m on the imaging screen 21 between the video position I3 of the marker M before rotation and the video position I4 of the marker M after rotation on the imaging screen 21, as shown in FIG. This distance m can be calculated by counting the number of pixels in the Yc-axis direction between I3 and I4 and multiplying this number of pixels by the length per pixel.

    Next, the CPU 13 calculates a relative movement distance N of the marker M with respect to the camera 12 for generating two positions of the distance n from the distance n and the rotation angle θ. As shown in FIG. 11, N is taken on the imaging screen 21 with the length of G being g, so N = n · G / g. Subsequently, the CPU 13 calculates a distance Q from the straight line passing through the origin Pf of the flange coordinate F and parallel to the Xc axis of the camera coordinate C (Xcf axis with the rotation center) to the marker M (step S10: 12th process). This Q can be obtained by Q = N / tan θ.

Then, the CPU 13 calculates a distance R from the rotation center axis Xcf axis to a plane including the Xc axis and the Yc axis of the camera coordinates C (step S11: 13th step). This R can be obtained by R = Q−D.
The origin Pc of the camera coordinate C is an intersection of the Zc axis and a plane including the Xc axis and the Yc axis, and the Zc axis of the camera coordinate C is a point indicated by the vector V2 in FIG. 5 from the origin Pf of the flange coordinate F. It is going through. From this, the origin Pc of the camera coordinate C has a radius centered on the Zcf axis passing through the point indicated by the vector V2 from the origin Pf of the flange coordinate F and the Xcf axis parallel to the Xc axis passing through the origin Pf of the flange coordinate. Is the origin Pc of the camera coordinates C. In this case, there are two intersections between the Zc axis and the cylindrical surface with the radius R centered on the Xcf axis, but the intersection on the marker M side of the plane including the Xf axis and the Yf axis of the flange coordinates F is the camera. The origin is Pc of the coordinate C. Then, the CPU 13 calculates the distance between the obtained origin Pc of the camera coordinates C and the origin Pf of the flange coordinates F.

As described above, since the inclination of the camera coordinates C with respect to the flange coordinates F and the origin Pc position of the camera coordinates C on the flange coordinates F are obtained, the CPU 13 stores the inclination and position in the RAM 17 (step S12: 14th). Process).
Thus, according to this embodiment, the relationship between the flange coordinates F and the camera coordinates C can be obtained by the two-dimensional camera 12 attached to the flange 11.

1 is a perspective view of a visual inspection device according to an embodiment of the present invention. Block diagram showing the control configuration of the robot Perspective view showing flange coordinates Schematic cross section of camera Perspective view showing relationship between flange coordinates and camera coordinates Flow chart for determining the relationship of camera coordinates to flange coordinates Robot body skeleton diagram The figure which shows the photography screen for detecting the movement vector The figure which shows the imaging | photography screen for detecting the movement amount per pixel. The figure which shows the imaging | photography screen for detecting the shift | offset | difference of the Zc axis from the Zf axis. Explanatory diagram for detecting the distance between the camera and the marker Principle diagram for detecting the distance of the camera coordinates from the flange coordinates

Explanation of symbols

  In the drawings, 2 is a robot body, 3 is a control device, 4 is a teaching pendant, 11 is a flange (hand portion), 12 is a camera, 13 is a CPU, 16 is a ROM, 20 is a lens, and 21 is a CCD imaging screen (imaging sensor). ).

Claims (1)

In a robot attached with a camera having a lens and an image sensor on the hand,
A method for determining the position and inclination of the coordinates of the camera relative to the coordinates of the hand portion,
With the marker provided at a fixed position within the field of view of the camera, the hand portion is moved by a desired distance in parallel with the axes Xf, Yf, Zf of the coordinates of the hand portion, and the axes Xf, A first step of acquiring captured images of the camera before and after parallel movement with respect to Yf and Zf;
A second step for obtaining a movement vector of the marker generated by the parallel movement with respect to each of the axes Xf, Yf, Zf of the coordinates of the hand from the photographed image acquired in the first step;
The coordinates of the hand portion are obtained from the parallel movement distance of the coordinates of the hand portion moved in the first stroke with respect to the respective axes Xf, Yf, Zf and the movement vector of the marker obtained in the second stroke. A third step of determining the inclination of each axis Xc, Yc, Zc of the camera coordinates with respect to
In the state where the optical axis of the lens is the Zc axis of the axes Xc, Yc, Zc of the camera coordinates, and the marker provided at the fixed position is within the photographing field of view of the camera, A fourth step of acquiring a photographed image before and after the translation about the Xc axis and / or the Yc axis by moving the camera coordinates by a desired amount in parallel with the Xc axis and / or the Yc axis, respectively.
Based on the captured image of the marker acquired in the fourth step, the image position of the marker before the translation about the Xc axis and / or the Yc axis of the coordinates of the camera and the image of the marker after the translation The number of pixels between the positions is obtained, and the amount of movement of the camera per pixel is calculated from the number of pixels and the distance of the translation about the Xc axis and / or Yc axis of the camera coordinates. The fifth step,
Assuming that the Zc axis of the camera coordinates passes through the origin of the coordinates of the hand portion in a state where the marker provided at the fixed position is within the photographing field of view of the camera, the Zc axis is set as the center. A sixth step of rotating the hand portion by a desired amount and acquiring captured images at at least three positions from the start to the end of the rotation;
The seventh position is obtained by obtaining at least three video positions of the marker from the captured images at the at least three positions acquired in the sixth stroke, and obtaining the actual rotation center of the rotation in the sixth stroke from these video positions. And the process of
The minimum number of pixels between the rotation center obtained in the seventh step and the Xc axis and Yc axis of the camera coordinates is obtained, and this number of pixels and the movement per pixel obtained in the fifth step And an eighth step for obtaining a distance from an axis parallel to the Zc axis of the camera coordinates and passing through an origin of the coordinates of the hand portion from the quantity to the Zc axis of the camera coordinates;
Assuming that the plane containing the Xc axis and Yc axis of the camera passes through the origin of the coordinates of the hand portion, the hand portion is moved to the position of the camera coordinate in a state where the marker is in the field of view of the camera. A ninth step of acquiring a photographed image before and after the movement by moving a desired amount parallel to the Xc axis or the Yc axis;
The number of pixels between the image position of the marker before moving in parallel with the Xc axis or the Yc axis of the camera coordinates and the image position of the marker after moving according to the captured image acquired in the ninth step And calculating the movement distance of the marker on the photographed image from the number of pixels and the length of one pixel obtained in advance, and the movement distance of the marker on the photographed image and the movement of the hand portion A tenth step of calculating the distance from the origin of the camera coordinates to the marker from the distance and the focal length of the lens of the camera;
Assuming that the plane including the Xc axis and Yc axis of the coordinates of the camera passes through the origin of the coordinates of the hand portion in a state where the marker provided at the fixed position falls within the field of view of the camera. An eleventh step of rotating the hand portion by a desired angle around the Xc axis or the Yc axis of the coordinates of the camera, and acquiring captured images before and after the rotation;
The number of pixels between the image position of the marker before rotation and the image position of the marker after rotation is obtained from the captured image acquired in the eleventh step, and the imaging is performed from the number of pixels and the length of the one pixel. The movement distance of the marker on the image is calculated, and the coordinates of the camera serving as the rotation center of the eleventh stroke are calculated based on the movement distance of the marker on the captured image and the rotation angle in the eleventh stroke. A twelfth step of calculating a distance from the axis that is parallel to the Xc axis or the Yc axis and passing through the origin of the coordinates of the hand portion to the marker;
From the distance between the marker and the axis that is parallel to the Xc axis or the Yc axis of the coordinates of the camera that becomes the rotation center of the eleventh stroke and passes through the origin of the coordinates of the hand portion, Subtracting the distance between the origin of the coordinates and the marker, the origin of the coordinates of the hand portion is an axis parallel to the Xc axis or the Yc axis of the camera coordinates serving as the rotation center of the eleventh stroke. A thirteenth step of obtaining a distance from an axis passing through the origin of the camera coordinates;
The distance from the front axis passing through the origin of the coordinates of the hand portion parallel to the Zc axis of the camera coordinates obtained in the eighth step to the Zc axis of the camera coordinates, and the thirteenth step According to the distance from the axis passing through the origin of the coordinates of the hand portion to the origin of the coordinates of the camera on the coordinate of the hand portion on the Xc axis or the Yc axis of the obtained camera coordinates A fourteenth step of obtaining the position of the origin of the camera coordinates;
A method for combining robot coordinates.

Images