JP4289169B2 - Relative position measurement method between multiple robots - Google Patents

Description

  The present invention relates to a method for measuring a relative positional relationship between robots when a plurality of robots work in cooperation.

An apparatus is known in which two robots are placed face to face, one robot holds a workpiece, the other robot has an arc welding torch, and performs an arc welding operation on the workpiece. Yes. In addition, there are an increasing number of cases where multiple robots work together, such as handling heavy objects and spot welding of automobile bodies.
When a plurality of robots collaborate in this manner, the relative positional relationship between the robots (position and posture between the robot base coordinates) is acquired in advance, and the relative positional relationship is set in the robot controller. There is a need. Various methods for obtaining the relative position for this purpose have been proposed.
For example, in Patent Document 1, the tool tip of the other robot is matched against one point on the rotating part of one robot, and the teaching data at that time is obtained three times with different angles of the rotating part. A method for obtaining the relative position of the coordinate system of the one robot as viewed from the coordinate system of the other robot based on the three types of teaching data obtained in the above manner is disclosed.
In Patent Document 2, a force sensor and a planar tool are attached to the wrist flange of one robot, a needle-like tool is attached to the wrist flange of the other robot, and the planar tool and the needle are not at a straight line. A method is disclosed in which a tool-like tool is brought into contact and the relative position between the robots is determined from the position of each contact point in the base coordinate system of one robot and the position of the needle-like tool in the base coordinate system of the other robot. .
Also, in Non-Patent Document 1, a marker is attached to the tool tip of two robots, two cameras are arranged at appropriate locations, and a plurality of different marker positions when each robot is moved are indicated by two A method of calculating the relative positional relationship between robots by measuring with a camera has been proposed. This system basically uses the principle of camera stereo viewing (see Chapter 4 of Non-Patent Document 2).
Japanese Patent Laid-Open No. 6-278063 (page 9, FIG. 1, FIG. 3, FIG. 5) Japanese Patent Laid-Open No. 5-1111897 (6th page, FIG. 1) Kikuchi et al., “Inter-Robot Calibration for“ Plug & Produce ””, Proceedings of the 19th Annual Conference of the Robotics Society of Japan, pp.1065-1066, 2001 Koichiro Deguchi, “Basics of Robot Vision”, Corona, July 12, 2000

However, in the method of Patent Document 1 or Patent Document 2, since the tools of the robot are abutted with each other, there is a risk of damaging the tool or the robot, which causes a safety problem. In addition, since the robot is operated so as not to be damaged, there is a problem in that the worker is stressed.
The method of Non-Patent Document 1 has the advantage that it is not necessary to match the robot tool tips with each other because the marker is observed by the camera. However, since it is based on the principle of stereo vision, two cameras are required. There is a problem that the cost of the measuring device increases. There is also a problem that camera parameter identification (calibration) is required.
The present invention has been made in view of such problems, and it is not necessary to abut the tool tips of the robots. It is not necessary to identify the parameters of the measuring device, and the cost of the measuring device is low. An object of the present invention is to provide a relative position measurement system.

In order to solve the above-mentioned problem, the invention of claim 1 operates a plurality of robots (101a, 101b) so that the feature points (302a, 302b) defined in the movable parts of the robots are moved to the plurality of robots (101a, 101a). , 101b) are located at two or more points on each of three or more straight lines (301) that pass through a reachable three-dimensional space and are not parallel to each other, and the feature points (302a, 302b) are located on the straight lines (301). The position of the feature point (302a, 302b) based on each robot coordinate system in a state of being positioned above is measured by forward kinematics calculation from the encoder value of each joint axis of the robot (101a, 101b) , and the robot coordinate system the characteristic point from the origin of the coordinate system which is provided separately from the (302a, 302b) and the distance or the distance to the physical quantity having a correlation Constant, and the derived for the feature points based on the robot coordinate system (302a, 302b) said expression representing the relationship between the position and the distance or physical quantity having a correlation with the length of each straight line (301) with simultaneous and equations, and requests the plurality robots (101a, 101b) relative positional relationship between by calculating a position vector between the origin of each robot coordinate system by solving the simultaneous equations.

According to a second aspect of the present invention, the feature points (302a, 302b) are picked up by the image pickup device (106) , the image is displayed on the image display device (108) in a plane, and the image display device (108) is displayed. three or more reference points (206) is provided, said robot (101a, 101b) the feature point by manipulating the (302a, 302b) each reference point position in the image display device (108) on the (206 ) So that the feature points (302a, 302b) are positioned on the straight lines (301), and the two or more points on the straight lines (301) are based on the robot coordinate systems. The position of (302a, 302b) is measured, and the physical quantity having a correlation with the distance or the distance is measured .

According to a third aspect of the present invention, a target object (105) is provided on the movable part of the robot, the feature points (302a, 302b) are defined on the target object (105) , and the image display device (108) is defined. The area or brightness of the image of the target object (105) displayed on the screen is a physical quantity having a correlation with the distance.

According to a fourth aspect of the present invention, a target object (105) is provided in the movable part of the robot, the feature points (302a, 302b) are defined in the target object (105), and the imaging device (106) is provided. A zoom lens is attached, and the zoom lens is moved so that the area of the image of the target object (105) projected on the image display device (108) is constant. The physical quantity has a correlation with the distance.

According to a fifth aspect of the present invention, the target object (105) is caused to emit light.

According to a sixth aspect of the present invention, a plurality of the target objects (105) having different sizes or brightness are provided in a movable part of the robot, and the target objects are picked up according to a distance from the imaging device (106). (105) is switched.

According to a seventh aspect of the present invention, the feature points (302a, 302b) are picked up by an image pickup device (106) , the image is displayed on a plane on the image display device (108) , and the robot (101a, 101b) is displayed. operation to the feature point (302a, 302b) the feature points by matching the laser beam oscillated position from the laser distance laser oscillator of the sensor (501) in said image display device (108) on the (302a, 302b ) Is positioned on each straight line (301), and the positions of the feature points (302a, 302b) based on each robot coordinate system at two or more points on each straight line (301) are measured. By changing the direction of the laser beam, the laser beam is made into the three or more straight lines .

In the invention according to claim 8, a target object (105) is provided in a movable part of the robot, the feature points (302a, 302b) are defined in the target object (105), and the laser beam is used as the feature points. (302a, 302b) is irradiated to measure the distance.

According to a ninth aspect of the present invention, the imaging device (106) and the laser oscillation device (501) are integrated, and the optical axis of the imaging device (106) and the laser optical axis of the laser oscillation device (501) are set. It will be parallel.

According to a tenth aspect of the present invention, a needle-like tool (705a, 705b) is provided on the movable part of the robot (101a, 101b) , and the feature point is defined at the tip of the needle-like tool (705a, 705b). , the position of the robot (101a, 101b) the operation to the feature point, a state of positioning the characteristic point on the straight line by located on the rod-shaped jig (701) which forms a straight line, the rod-shaped jig ( 701) Measure the positions of the feature points based on each robot coordinate system for two or more points on the above, and change the arrangement of the bar-shaped jigs (701) to change the bar-shaped jigs (701) to the three or more points. It is a straight line .

According to an eleventh aspect of the present invention, a distance scale is attached to the rod-shaped jig (701) , and the distance is measured using the distance scale and the position of the feature point based on the robot coordinate system.

The invention of claim 12, wherein the plurality of robots (101a, 101b) for each intersect with the three or more straight lines, and define the two parallel or more planes (904a, 904b), wherein said characteristic points each plane (904a, 904b) and positioning the characteristic point at the intersection (905, 906) between said by constrained on each straight line between the respective planes (904a, 904b), wherein each intersection (905 906), the position of the feature point based on the robot coordinate system is measured.

The invention according to claim 13 is to measure a physical quantity having a correlation with the distance based on a position of the feature point at each intersection (905, 906) based on a robot coordinate system.

The invention of claim 14 is characterized in that, of the two or more parallel planes (904a, 904b) , the ratio of the distance between the intersection points on a certain plane and the distance between the intersection points on another plane is calculated as follows. The physical quantity has a correlation with the distance.

According to the first to sixth aspects of the present invention, the feature point can be constrained on a straight line by moving the robot so that the feature point of the target on the robot tool matches the reference point of the imaging plane of one camera. In addition, since the distance to the target can be estimated by measuring the area or brightness of the target image on the imaging plane, the relative position between the robots without matching the tips of the robot tools at low cost Measure relationships. Furthermore, the relative positional relationship between robots can be measured without identifying camera parameters.
In addition, according to the seventh to ninth aspects, the feature points can be constrained on a straight line by moving the robot so that the laser beam of the laser distance sensor is irradiated to the feature points of the target on the robot tool. In addition, since the distance to the target can be directly measured using the laser distance sensor, the relative positional relationship between the robots can be accurately measured without matching the tips of the robot tools.
Further, according to the invention described in claims 10 to 11, since the feature points of the robot tool are constrained on a straight line using an elongated bar-shaped jig, the relative positional relationship between the robots without matching the tips of the robot tools Can be measured easily.
According to the invention described in claims 12 to 14, the robot is moved so that the feature point of the marker on the robot tool coincides with the reference point of the imaging plane of one camera, and the feature point is on the preset plane. By moving the robot to be constrained, the feature point is positioned at the intersection of each plane and each straight line, and by measuring the robot coordinate value of the feature point there, the target (feature Since the distance to the point) can be estimated, the relative positional relationship between the robots can be accurately measured without matching the tips of the robot tools at low cost. Furthermore, the relative positional relationship between robots can be measured without identifying camera parameters.

  Hereinafter, specific embodiments of the present invention will be described with reference to the drawings.

FIG. 1 is a configuration diagram of a relative position measuring apparatus showing a first embodiment of the present invention. In the figure, reference numerals 101a and 101b denote a first robot and a second robot that are targets for relative position measurement. Reference numerals 102a and 102b denote base coordinate systems fixed to the first and second robots. 103a and 103b are control devices for the first and second robots, and 104a and 104b are operation interfaces for the first and second robots. Reference numerals 105a and 105b denote spherical targets attached to the tool tips of the first and second robots. Here, the geometric gravity center (sphere center) of the spherical targets 105a and 105b is defined as a feature point of the target.
Reference numeral 106 denotes an imaging device (CCD camera). Reference numeral 107 denotes an image processing apparatus that processes a video signal output from the imaging apparatus 106. Reference numeral 108 denotes an image display apparatus that presents (displays) image data output from the image processing apparatus 107 to an operator. Reference numeral 109 denotes a relative for calculating the relative positional relationship between the robots (the positional relationship between the base coordinate systems 102a and 102b) based on data obtained from the control devices 102a and 102b and the image processing device 107 of the first and second robots. It is a position calculation device. Reference numeral 110 denotes a support unit of the imaging device 106.
The installation location of the imaging device 106 may be anywhere as long as the spherical targets 105a and 105b of the first robot 101a and the second robot 101b can be imaged together, and there is no need to measure the position (unknown It is not necessary to perform some kind of calibration on the imaging device 106 in advance.

FIG. 2 is an explanatory diagram of image display showing the first embodiment of the present invention. In the figure, 201 is an imaging plane displayed on the image display device 108, 202 is a coordinate system ΣI (U axis, V axis, W axis) of the imaging plane 201, and 203 is a camera coordinate system Σc of the imaging device 106. (Xc axis, Yc axis, Zc axis). The Zc axis of the camera coordinate system 203 coincides with the optical axis of the imaging device 106 and is perpendicular to the imaging surface 201 (the W axis and the Zc axis coincide). Here, the distance f from the origin of the camera coordinate system 203 to the imaging plane 201 coincides with the focal length of the lens. Reference numeral 204 denotes an image (image) of the spherical target 105 a formed on the imaging plane 201. Reference numeral 205 denotes the center of gravity of the image 204, the coordinate value of which is calculated by the image processing device 107 and overlaid on the image 204. For example, the image processing device 107 binarizes the video signal from the imaging device 106 to extract a circular region, and calculates the position of the center of gravity of the extracted region. Reference numeral 206 denotes a reference point on the imaging plane designated by the operator, and the marker of the reference point 206 is overlaid on the imaging plane 201 by the image processing apparatus 107. Here, the coordinate value of the reference point 206 on the imaging plane 201 is represented as (ua, va).
While viewing the state of the imaging plane 201 with the image display device 108, the operator uses the operation interface 104a to move the spherical target 105a of the first robot 101a so that the center of gravity 205 of the image 204 coincides with the reference point 206. Move. Alternatively, an image-based visual servo technique (see Chapter 6 of Non-Patent Document 2) is applied, and the position deviation on the imaging plane 201 between the reference point 206 and the center of gravity 205 is determined by the image processing apparatus 107 using an image Jacobian matrix. By converting the change amount into a minute position change amount in the coordinate system and providing the change amount to the control device 103a of the first robot 101a to constitute position feedback, the reference point 206 and the center of gravity 205 are made to coincide with each other. The spherical target 105a of the robot 101a can be automatically moved.

FIG. 3 shows a state where the center of gravity 205 and the reference point 206 coincide with each other on the imaging plane 201 by the above operation. In this state, the feature point (the center of the sphere) of the spherical target 105a of the first robot is at a position 302a on the straight line 301 passing through the origin Oc of the camera coordinate system 203 and the reference point 206 (the shape of the target and the lens). It is clear from the imaging principle and the imaging principle of the camera).
In this state, the operator measures the position vector 303a to the first feature point 302a relative to the base coordinate system 102a of the robot _^(r1 P t1_a1). In a general industrial robot, the TCP position is calculated by the controller 103a by forward kinematics calculation from the encoder (internal sensor) value incorporated in each joint axis of the first robot 101a, and the result is obtained as the operation interface 104a. You can check above. The position vector 303a may be measured using this general function. Further, the area (S _{t1 — a1} ) of the image 204 on the imaging plane 201 is measured using the image processing device 107 and the image display device 108. The area of the circular region extracted in the image processing apparatus 107 may be automatically calculated, or the manual cursor may be overlaid on the image plane 201 and the area may be read from the cursor on the image display apparatus 108.

Next, as shown in FIG. 4, the operator moves the spherical target 105a in the depth direction (Zc axis direction) with respect to the imaging plane 201 using the operation interface 104a of the first robot 101a, and then again. The spherical target 105 a is moved so that the center of gravity 205 of the image 204 coincides with the reference point 206. At this time, the characteristic point of the spherical target 105a is on the straight line 301, but is different from 302a shown in FIG. The feature point position in FIG. 4 is represented by 302b. As in the case of FIG. 3, the operator measures the area of the first position vector 303b of the coordinate system 102a of the robot to the feature point 302b which is the reference _^(r1 P t1_a2) an image 204 _(S t1_a2).
Next, a second reference point and a third reference point that are different from the reference point 206 are defined on the imaging plane 201, and the origin 0c of the camera coordinate system 203 and the second and third reference points are defined as in the case of the reference point 206. The feature point of the spherical target 105a is positioned on each straight line connecting the third reference points, and the position vector of the feature point and the area of the image of the spherical target 105a are obtained. That is, for the second reference point, position vectors ^r1 P _{t1_b1} and ^r1 P _{t1_b2} and areas S _{t1_b1} and S _{t1_b2} for the third reference point, position vectors ^r1 P _{t1_c1} and ^r1 P _{t1_c2} , area S _{t1_c1} and S _{t1_c2} is obtained.
Then, the same measurement process is performed for the second robot 101b (the same applies when there are three or more robots).
In addition, the measurement order of the position vector and the target area on each straight line of each robot is not limited to the above description, and may be measured in an arbitrary order.

Next, a procedure for obtaining the relative position between the first robot 101a and the second robot 101b based on the measurement data described above will be described.
When the center (feature point) of the spherical target 105a of the first robot 101a is placed on the straight line 301 (see FIGS. 3 and 4), the following equation is established.

^c P _OcOr1 + ^c R _r1・^r1 P _{t1_a1} = w _{t1_a1} (ua, va, f) ^T (1)

^c P _OcOr1 + ^c R _r1・^r1 P _{t1_a2} = w _{t1_a2} (ua, va, f) ^T (2)

Here, ^c P _OcOr1 represents a position vector (size 3 × 1) from the origin Oc of the camera coordinate system 203 to the origin Or1 of the base coordinate system 102a of the first robot 101a with reference to the camera coordinate system 203. (The subscript on the upper left indicates the reference coordinate system). ^c R _r1 is a rotation matrix (size 3 × 3) from the base coordinate system 102a of the first robot 101a to the camera coordinate system 203. (ua, va, f) ^T is a position vector from the origin Oc of the camera coordinate system 203 to the reference point 206 on the imaging plane 201 (based on the camera coordinate system 203). Further, w _{t1 — a1} and w _{t1 — a2} are scale factors in the perspective transformation, and become positive numbers proportional to the distance from the origin Oc of the camera coordinate system 203 to the feature points 302a and 302b, respectively. ^r1 P _{t1_a1} and ^r1 P _{t1_a2} are position vectors (base coordinate system 102a) from the origin Or1 of the base coordinate system 102a of the first robot 101a to the feature points 302a and 302b, respectively, and are directly obtained by the measurement process described above. Data.

  Subtracting both sides of equation (1) from both sides of equation (2) gives the following equation.

^c R _r1・^r1 ΔP _{t1_a} = Δw _{t1_a} (ua, va, f) ^T (3)

However, in equation (3)

^r1 ΔP _{t1_a} = ^r1 P _{t1_a2} − ^r1 P _{t1_a1} (4)

Δw _{t1_a} = w _{t1_a2} − w _{t1_a1} (5)

It is. In addition, the following constraint condition is attached to Expression (3).

^r1 ΔP _{t1_a} | = | Δw _{t1_a} | sqrt (ua ² + va ² + f ² ) (6)

  For the second robot 101b, the following equation holds in the same manner.

^c R _r2・^r2 ΔP _{t2_a} = Δw _{t2_a} (ua, va, f) ^T (7)

^r2 ΔP _{t2_a} | = | Δw _{t2_a} | sqrt (ua ² + va ² + f ² ) (8)

Note that the position vectors ^r2 P _{t2_a1} and ^r2 P _{t2_a2} measured to obtain ^r2 ΔP _{t2_a} are the position vectors ^r1 P _{t1_a1} and ^r1 P _{t1_a2} of the feature points 302a and 302b measured for the first robot 101a. It doesn't have to match.
From equations (3) and (7), (ua, va, f) ^T is eliminated, and the following equations are obtained by using the constraints of equations (6) and (8).

^r1 R _r2・^r2 ΔP _{t2_a} = (Δw _{t2_a} / Δw _{t1_a} ) ^r1 ΔP _{t1_a} (9)

| Δw _{t2_a} | / | Δw _{t1_a} | = | ^r2 ΔP _{t2_a} | / | ^r1 ΔP _{t1_a} | (10)

Here, the signs of Δw _{t2_a} and Δw _{t1_a} can be determined according to whether the TCP of the robot is moved toward or away from the imaging device on the straight line 301 (because the scale factor is determined). Therefore, (Δw _{t2 — a} / Δw _{t1 — a} ) on the right side of Equation (9) can be calculated from Equation (10) and the respective codes.
Similarly, for each straight line connecting the origin 0c of the camera coordinate system 203 and the second and third reference points, the following simultaneous equations are derived by deriving equations corresponding to equations (9) and (10). can get.

^r1 R _r2・ ( ^r2 ΔP _{t2_a} , ^r2 ΔP _{t2_b} , ^r2 ΔP _{t2_c} ) =
(α ^r1 ΔP _{t1_a} , β ^r1 ΔP _{t1_b} , γ ^r1 ΔP _{t1_c} ) (11)

However, the coefficients α, β, γ are

α = Δw _{t2_a} / Δw _{t1_a} (12a)
β = Δw _{t2_b} / Δw _{t1_b} (12b)
γ = Δw _{t2_c} / Δw _{t1_c} (12c)

It is.

To both sides of the equation (11), right ^{_{(r2 ΔP t2_a, r2 ΔP t2_b}} , r2 ΔP t2_c) by multiplying an inverse matrix of (solving the simultaneous equations), the base coordinate system 102a and a second first robot 101a A rotation matrix ^r1 R _r2 representing the posture between the base coordinate systems 102b of the robot 101b can be calculated. The inverse matrix of ( ^r2 ΔP _{t2 — a} , ^r2 ΔP _{t2 — b} , ^r2 ΔP _{t2 — c} ) always exists if each straight line (reference point) is independent. In addition, by making the number of straight lines (reference points) to be measured 4 or more and making the simultaneous equation (11) redundant, it is possible to obtain a rotation matrix with the smallest variation due to measurement error (to the least squares method) Equivalent).

Next, a method for deriving a position vector between the origin Or1 of the base coordinate system 102a of the first robot 101a and the origin Or2 of the base coordinate system 102b of the second robot 101b will be described.
When ^r1 R _c is applied to both sides of the equations (1) and (2) from the left, the following equation is established.

^r1 P _OcOr1 + ^r1 P _{t1_a1} = w _{t1_a1} ^r1 R _c (ua, va, f) ^T (13)

^r1 P _OcOr1 + ^r1 P _{t1_a2} = w _{t1_a2} ^r1 R _c (ua, va, f) ^T (14)

From the equations (13) and (14), when ^r1 R _c (ua, va, f) ^T is eliminated, the following equation is established.

^r1 P _OcOr1 + ^r1 P _{t1_a1} = (w _{t1_a1} / w _{t1_a2} ) ( ^r1 P _OcOr1 + ^r1 P _{t1_a2} ) (15)

Equation (15) can be transformed into the following equation.
^r1 P _OcOr1 = (w _{t1_a1} / w _{t1_a2} ) / (1 − w _{t1_a1} / w _{t1_a2} ) ^r1 P _{t1_a2}
− 1 / (1 − w _{t1_a1} / w _{t1_a2} ) ^r1 P _{t1_a1} (16)

Here, since w _{t1_a1} / w _{t1_a2} is the ratio of the scale factor, it matches the distance ratio from the origin Oc of the camera coordinate system 203 to the feature points 302a (end point of ^r1 P _{t1_a1} ) and 302b (end point of ^r1 P _{t1_a2} ). To do. From the perspective transformation geometry, the area of the image 204 on the imaging plane 201 is inversely proportional to the square of the distance from the origin Oc of the camera coordinate system 203 to the feature point (the center of the sphere) of the spherical target 105a. Therefore, the following approximation holds.

w _{t1_a1} / w _{t1_a2} ≒ sqrt (S _{t1_a2} / S _{t1_a1} ) (17)

Similarly, the following equation holds for the second robot 101b.

^r2 P _OcOr2 = (w _{t2_a1} / w _{t2_a2} ) / (1 − w _{t2_a1} / w _{t2_a2} ) ^r2 P _{t2_a2}
− 1 / (1 − w _{t2_a1} / w _{t2_a2} ) ^r2 P _{t2_a1} (18)

w _{t2_a1} / w _{t2_a2} ≒ sqrt (S _{t2_a2} / S _{t2_a1} ) (19)

Depending on the ambient lighting conditions, the area of the target image may not be accurately measured. In such a case, the target may emit light directly or indirectly so that the area can be measured without depending on the surrounding illumination state. At this time, it is more effective to emit light with light of a specific wavelength and filter the light other than the light of the wavelength with the image pickup apparatus.
Eventually, from the position vectors ^r1 P _OcOr1 and ^r2 P _OcOr2 calculated by the equations (16) and (18) and the rotation matrix ^r1 R _r2 , the origin Or1 of the base coordinate system 102a of the first robot 101a and the second robot 101b A position vector between the origins Or2 of the base coordinate system 102b is calculated by the following equation.

^r1 P _Or1Or2 = ^r1 R _r2・^r2 P _OcOr2 − ^r1 P _OcOr1 (20)

Further, ^r1 P _Or1Or2 may be obtained for the second reference point and the third reference point by the same procedure, and the average (center of gravity) of them may be used as the final position vector.
Instead of measuring the area change of the image 204 when the spherical target 105a is moved in the depth direction to obtain the distance ratio to the target (ratio of scale factors) (equations (17) and (19)), the imaging device A zoom lens may be attached to 106, the zoom lens may be moved so that the area of the image 204 is constant, and the distance ratio from the movement amount to the target may be obtained.
Further, instead of measuring the area of the target image on the imaging plane, the distance ratio of equations (17) and (19) may be estimated by measuring the brightness (light quantity) of the target image.

FIG. 5 is an explanatory diagram of a robot showing a modification of the first embodiment of the present invention. This modification is characterized in that two large and small spherical targets 105a are attached to the tool tip of the first robot 101a. Instead of changing the size of the spherical target, the light emission brightness of the target may be varied.
As shown in FIG. 5, when a plurality of target objects having different sizes (or brightness) are attached to the tool portion of the robot, the distance ratio is increased by switching to a larger (brighter) target as the distance from the imaging device 106 increases. The estimation accuracy can be increased. However, when calculating the distance ratio using the equations (17) and (19), it is necessary to consider the difference in target size (brightness). For example, when measuring S _{t1 — a2} in equation (17), _assuming that a target having a size k times larger than the target used when measuring S _{t1 — a1} is used, the distance ratio is estimated by the following equation.

w _{t1_a1} / w _{t1_a2} ≒ sqrt (S _{t1_a2} / k ² × S _{t1_a1} ) (21)

  As described above, according to the first embodiment, the feature points can be constrained on a straight line by moving the robot so that the feature points of the robot tool coincide with the reference points of the imaging plane of one camera. In addition, since the distance ratio to the target can be estimated by measuring the area ratio (brightness ratio) of the target on the imaging plane, the robot can be used at low cost without matching the tips of the robot tools. The relative positional relationship between them can be measured. In addition, the relative positional relationship between the robots can be measured without identifying camera parameters. By making the target object emit light, the target area can be measured without depending on the surrounding illumination state, and further, the distance ratio can be estimated more accurately by switching to a larger (brighter) target as the distance from the imaging device increases. As a result, the relative positional relationship between the robots can be accurately measured.

FIG. 6 is a configuration diagram of a relative position measuring apparatus showing a second embodiment of the present invention. In the figure, reference numeral 501 denotes a laser oscillation device of a laser distance sensor. Reference numeral 502 denotes a support portion of the laser oscillation device, which has a rotation mechanism with two degrees of freedom (pan and tilt). Reference numeral 503 denotes a distance calculation unit of the laser distance sensor. In addition, since the same code | symbol is attached | subjected to the component which is common in 1st Example, description is abbreviate | omitted.
In the first embodiment described above, the feature point of the target is constrained on a straight line by matching the center of gravity 205 of the image 204 with the reference point 206 on the imaging plane in the imaging plane 201 of the camera. On the other hand, in the second embodiment, the characteristic points of the target are constrained on a straight line by using a laser beam oscillated from the laser oscillation device 501. That is, the straight laser beam itself is a straight line that should constrain the feature points of the target.
In the second embodiment, the vicinity of the spherical target at the tip of the robot tool is photographed with a camera (imaging device 106), and the center of gravity of the target image coincides with the laser light spot while viewing the imaging plane presented on the image display device 108. As described above, the operator moves the robot target using the operation interfaces 104a and 104b (this process may be automated in the same manner as in the first embodiment). By doing so, the feature point (center of the sphere) of the target can be constrained on a straight line (laser beam).

  In FIG. 6, the imaging device 106 (camera) and the laser oscillation device 501 are installed separately, but both may be integrated by a jig. FIG. 7 is an explanatory diagram of an apparatus in which an imaging apparatus and a laser oscillation apparatus are integrated. In this apparatus, the spherical target at the tip of the robot tool is irradiated with the laser beam by fixing the image pickup apparatus 106 on the support unit 502 so that the optical axis of the imaging apparatus 106 and the laser optical axis of the laser oscillation apparatus 501 are parallel. Easy to do. That is, the robot may be moved so that the target is projected near the center of the imaging plane 201 while adjusting the zoom of the camera. In the figure, reference numeral 601 denotes an image of laser light on the imaging plane 201.

In the second embodiment, as in the first embodiment, the position vector from the origin of the robot coordinate system to the feature point of the spherical target is measured by the robot's internal sensor at two points on a certain straight line. At the same time, the distance to the feature point along the straight line is measured by the distance calculation unit 503 of the laser distance sensor (in the first embodiment, the direct distance is not measured, but the area of the target image having a correlation with the distance is measured. ).
The position vector and distance are measured in the same manner for each robot with respect to three or more straight lines. In order to change the direction of the straight line, the pan axis and tilt axis of the support portion 502 may be rotated.

The principle of deriving the relative positional relationship between the robots from the above measurement data is almost the same as that of the first embodiment, but the description of the medium parameter is slightly different.
The three straight lines are represented as straight lines A, B, and C, and the position vectors to the target feature points of the robot 1 constrained on the straight line A are similarly represented as ^r1 P _{t1_a1} and ^r1 P _{t1_a2} . Here, the straight line A (unit) direction vector La, the distance to the end point of the end point and ^r1 P _{T1_a2} laser distance sensor ^r1 was measured by P _{T1_a1} each d _{T1_a1,} expressed as d _{T1_a2,} equation (1) to The relative positions may be similarly derived by replacing (ua, va, f) ^T described in (21) with La, w _{t1_a1} with d _{t1_a1} , and w _{t1_a2} with d _{t1_a2} . However, in the second embodiment, the equations (6), (8), (10), (17), (19), and (21) are not required. This is because, unlike w _{t1_a1} and w _{t1_a2} , d _{t1_a1} and d _{t1_a2} are physical quantities that can be directly and accurately measured by the laser distance sensor.

  As described above, according to the second embodiment, the feature point can be constrained on a straight line by moving the robot so that the laser beam of the laser distance sensor is applied to the feature point of the robot tool. Since the distance to the target can be accurately measured directly using the laser distance sensor, the relative positional relationship between the robots can be accurately measured without matching the tips of the robot tools.

FIG. 8 is a block diagram of a relative position measuring apparatus showing a third embodiment of the present invention. In the figure, reference numeral 701 denotes a rod-shaped jig, and a distance scale is provided on the surface thereof. Reference numerals 702 a and 702 b denote support portions for the rod-shaped jig 701. Linear motion joints 703a and 703b and universal joints 704a and 704b are attached to the support portions 702a and 702b, and the direction of the bar-shaped jig 701 can be freely changed and fixed in an arbitrary direction.
In the third embodiment, not the spherical target but the needle tools 705a and 705b are attached as the robot tools. Then, by constraining (matching) the tips of the needle-like tools 705a and 705b on the bar-shaped jig 701, the feature points (tips) of the robot tool are constrained on a straight line, and the tool tip position vector at that time is determined by the robot tool. Measured with an internal sensor. Further, the distance from the end point of the bar-shaped jig 701 is measured by reading the distance scale in a state where the tips of the needle-shaped tools 705a and 705b are constrained on the bar-shaped jig 701.
The position vector and distance are measured in the same manner for each robot with respect to three or more straight lines (changing the arrangement of rod-shaped jigs). Further, the principle of deriving the relative position between the robots from the measured data is as described in the second embodiment.
As described above, in the third embodiment, since the feature points of the robot tool are constrained on a straight line using the elongated rod-shaped jig 701, an expensive apparatus is used without matching the tips of the robot tools. Even without it, the relative positional relationship between robots can be easily measured.

FIG. 9 shows the principle of the relative position measurement method of the fourth embodiment of the present invention. In the figure, reference numeral 901 denotes a marker (mark) attached to the tool tip of the first robot 101a. In the first and second embodiments, the spherical target (105a, 105b) is attached to the tip of the robot tool. However, the marker 901 in the fourth embodiment does not need to be as large as the spherical target. A reflective marker, LED marker, infrared marker or the like coated with a fluorescent paint is used. The configuration of the apparatus used for relative position measurement in Example 4 is the same as that of FIG. 1 except for the marker (in the apparatus configuration of FIG. 1, the spherical target is replaced with a marker).
In FIG. 9, reference numeral 902 denotes the origin Or1 of the base coordinate system 102a of the robot 101a, and 903 denotes the origin Oc of the camera coordinate system 203. Reference numerals 904a and 904b denote two parallel planes set for the robot 101a, which are represented as plane G and plane H, respectively. Reference numeral 206 denotes three or more reference points set on the imaging plane 201, and these are represented as I ₁ , I ₂ ,..., I _N (N is an integer of 3 or more). Reference numeral 301 denotes straight lines that pass through the camera coordinate origin Oc and the reference points I ₁ , I ₂ ,..., I _N , and are represented as straight lines 1, 2,. 905, linear 1, a point of intersection between the straight line 2 ... linear N and the plane G, G _1, G ₂ respectively, ..., represented as G _N. 906, linear 1, a point of intersection between the straight line 2 ... linear N and the plane H, respectively H _1, H _2, ..., expressed as H _N. Reference numeral 907 denotes an image (image) of the marker 901 shown on the imaging plane 201.

Here, the center of the marker 901 is used as a feature point, and control for restricting the feature point to the plane G is applied to the robot 101a so that the feature point operates on the plane G. In this state (state where the feature point is constrained to the plane G), the feature point is set to the intersection point G ₁ by operating the robot 101a on the imaging plane 201 so that the center of gravity of the marker image 907 matches the reference point I _1. Can be positioned. After positioning the feature point at the intersection point G ₁ , the position vector 908 from the robot coordinate origin Or 1 (902) to the intersection point G ₁ (feature point) is determined from the value of the internal sensor (eg, encoder of each axis) of the robot 101 a and the robot coordinates. It is measured as a value (coordinate value of the base coordinate system 102a). Similarly, intersection G ₂ feature points, ..., positioned to G _N, measures the position vector to each intersection (feature points). Intersection G _1, G ₂ measured in this manner, ..., a position vector ^r1 P _{g_1} to G _N, ^r1 P _{g_2,} ..., expressed as ^r1 P _{G - n.}

Next, feature points of the robot 101a (center of the marker) in a state of being constrained to a plane H, in a similar manner, the center of gravity is I _1, I ₂ of the marker images 907, ..., the robot 101a to match the I _N By operating, the feature points are positioned at the intersections H ₁ , H ₂ ,..., H _N on the plane H, and the position vector 909 from the robot coordinate origin Or1 to each intersection (feature point) is measured. Intersection H _1, H ₂ as measured in this manner, ..., a position vector ^r1 P _{h_1} to H _N, ^r1 P _{h_2,} ..., expressed as ^r1 P _{h_N.}
In the imaging plane 201, the robot operation to match the center of gravity of the marker image 907 with the reference point 206 applies the image-based visual servo technology (see Chapter 6 of Non-Patent Document 2) as described in the first embodiment. Can be automated.
Further, the position vector measurement order may be arbitrarily changed regardless of the above. Furthermore, the robot may be moved so that the feature points are on each plane while the feature points of the robot are constrained to the straight line 301.
The above is the measurement process performed on the first robot 101a.

Next, a similar marker (901) is also attached to the tool tip of the second robot 101b. Also, two parallel planes are set for the robot 101b. These are represented as plane S and plane T. Here, the plane S (plane T) need not be the same as the plane G (plane H) (regardless of whether it is the same or different). Straight line 1, straight line 2 ... The intersections of straight line N and plane S are represented as S ₁ , S ₂ , ..., S _N , respectively, and straight lines 1, straight line 2 ... The intersections of straight line N and plane T are respectively T ₁ , T ₂ ,…, T _N
A similar measurement process is performed on the robot 101b using the same reference points I ₁ , I ₂ ,..., I _N used in the measurement process of the robot 101a. That is, in a state of being restrained marker center of a robot 101b (the feature points) to the plane S (plane T), the center of gravity is the reference point of the marker image 907 in imaging plane _{201 I 1, I 2, ...} , match the I _N By operating the robot 101b as described above, the feature points are positioned at the intersections S ₁ , S ₂ ,..., S _N (intersections T ₁ , T ₂ ,..., T _N ), and the base coordinate system to each intersection 102b position vector ^r2 P _{s_1} referenced ^{_{to, r2 P s_2, ..., r2}} P s_N (r2 P t_1, r2 P t_2, ..., r2 P t_N) is measured.

Next, a procedure for obtaining the relative position between the first robot 101a and the second robot 101b based on the measurement data described above will be described.
First, a rotation matrix ^r1 R _r2 between the base coordinates 102a and the base coordinates 102b is obtained.
Here, the direction vector of the straight line k is expressed as n _k (k = 1, 2,..., N). However, the direction from the camera coordinate origin Oc toward the marker 901 is positive. As can be seen from FIG. 9, the base of the robot 101a is obtained by normalizing the difference between the position vectors ^r1 P _{g_k} and ^r1 P _{h_k} up to the intersections G _k and H _k of the straight line k and the plane G and the plane H. A direction vector ^r1 n _{k based} on the coordinates 102a can be calculated. That is,

^r1 n _k = δ (G, H) ・ ( ^r1 P _{g_k} − ^r1 P _{h_k} ) / | ^r1 P _{g_k} − ^r1 P _{h_k} | (22)
(k = 1, 2,…, N)

It is. Here, δ (G, H) is a code coefficient (1 or −1) determined by the positional relationship between the plane G and the plane H.
The direction vector of Expression (22) and the direction vector ^C n _k based on the camera coordinate system 203 are related as follows.

^r1 R _C・^C n _k = ^r1 n _k , (k = 1, 2,…, N) (23)

  The following equation holds for the robot 101b as well.

_{^{r2 n k = δ (S,}} T) · (r2 P s_k - r2 P t_k) / | r2 P s_k - r2 P t_k | (24)
^r2 R _C・^C n _k = ^r2 n _k , (k = 1, 2,…, N) (25)

Now, since the camera parameter is unknown (because the positional relationship of the imaging plane 201 with respect to the camera coordinate system 203 is not known), the value (coordinate value) of the direction vector ^C n _k is not known, but from the equations (23) and (25), ^C If n _k is deleted, the following direct relational expression between the robot 101a and the robot 101b is derived.

^r1 R _r2・^r2 n _k = ^r1 n _k , (k = 1, 2,…, N) (26)

The rotation matrix ^r1 R _r2 is obtained by solving the simultaneous equations of Equation (26). In an ideal state where there is no position vector measurement error, it is sufficient if N = 3 (that is, three reference points and three straight lines). However, since measurement error is actually included, N is set to 3 Increase to the above to minimize the variation (standard deviation) in the calculation results.

Next, determine the position vectors ^r1 Po _r1 o _r2 between the base coordinate 102a and the base coordinate 102b.
In FIG. 9, since the plane G and the plane H are parallel, the line segment G ₁ G ₂ and the line segment H ₁ H ₂ are also parallel. Therefore, the triangle OcG ₁ G ₂ and the triangle OcH ₁ H ₂ are similar,

G ₁ G ₂ : H ₁ H ₂ = OcG ₁ : OcH ₁ (27)

Holds. Furthermore, with respect to the intersection points G _i , G _j arbitrarily selected from the intersection points G ₁ , G ₂ ,..., G _N on the plane G and the intersection points H _i , H _j on the corresponding plane H,

G _i G _j : H _i H _j = OcG _k : OcH _k (28)
OcG _k / OcH _k = const. (G _i G _j / H _i H _j = const.) (29)
(i = 1,2,…, N, j = 1,2,…, N, i ≠ j, k = 1,2,…, N)

Holds. OcG _k / OcH _k is set as L _g / L _h , and an average value is obtained from the measured position vector as follows.

Here, _N C ₂ is a combination when two are selected from N intersections.

Since the distance ratio from the camera coordinate origin Oc to the intersection point G _k and the intersection point H _k can be obtained by Equation (30), using this ratio and the position vectors ^r1 P _{g_k} and ^r1 P _{h_k} , the robot can be used to obtain the outer _dividing point. it can be determined the position vector from the coordinate origin Or1 to the camera coordinate origin Oc _^r1 Po ^r1 o _C a (910 in FIG. 9) as follows.

^r1 Po _r1 o _C = (−L _h ) / (L _g −L _h ) ・^r1 P _{g_k} + L _g / (L _g −L _h ) ・^r1 P _{h_k} (31)
(k = 1, 2,…, N)

  Since the position vector includes a measurement error, the average value may be obtained as follows based on Equation (31).

Eventually, based on the measurement values of the position vector, from equation ^{(30) (32) r1 Po} r1 o C is obtained.

Next, for the second robot 101b, in the same manner to obtain the position vector ^r2 Po _r2 o _C from the origin Or2 of the base coordinate system 102b to the origin Oc of the camera coordinate system 203 as follows.

Thus, in the same manner as in Example 1, it is obtained the position vector ^r1 Po _r1 o _r2 by the following equation.

^r1 Po _r1 o _r2 = ^r1 R _r2・^r2 Po _C o _r2 − ^r1 Po _C o _r1
= ^-R1 R _r2 · ^r2 Po _r2 o _C + ^r1 Po _r1 o _C (35)

Further, δ (G, H) and δ (S, T) in the equations (22) and (24) can be calculated as follows using L _g / L _h and L _s / L _t .

δ (G, H) = sgn (L _g / L _h − 1) (36)
δ (S, T) = sgn (L _s / L _t − 1) (37)

  Here, sgn (x) is a sign function, and is 1 when x ≧ 0 and -1 when x <0.

  As described above, according to the fourth embodiment, the feature points of the robot tool are operated on two parallel planes, and the feature points coincide with the reference points on the imaging plane of one camera. By moving the robot, the feature point is positioned at the intersection of each straight line passing through the reference point and each plane, and the robot coordinate value of the feature point at each intersection is measured, so the size and brightness of the target image on the imaging plane Without measuring the distance information, the distance information in the depth direction can be accurately obtained with only one camera. Therefore, the relative positional relationship between the robots can be measured more accurately without matching the tips of the robot tools at low cost. In addition, the relative positional relationship between robots can be easily measured without identifying camera parameters.

  The present invention is useful as a method for measuring the relative positional relationship between robots when a plurality of robots work in cooperation.

1 is a configuration diagram of a relative position measuring apparatus showing a first embodiment of the present invention. FIG. FIG. 3 is an explanatory diagram of image display showing a first embodiment of the present invention. FIG. 3 is an explanatory diagram of image display showing a first embodiment of the present invention. FIG. 3 is an explanatory diagram of image display showing a first embodiment of the present invention. It is explanatory drawing of the robot which shows the modification of 1st Example of this invention. It is a block diagram of the relative position measuring apparatus which shows 2nd Example of this invention. It is explanatory drawing of the apparatus which integrated the imaging device and laser oscillation apparatus which are used in 2nd Example of this invention. It is a block diagram of the relative position measuring apparatus which shows 3rd Example of this invention. It is explanatory drawing of the measurement principle in 4th Example of this invention.

Explanation of symbols

101a First robot, 101b Second robot, 102a, 102b Base coordinate system, 103a, 103b Control device, 104a, 104b Operation interface, 105a, 105b Spherical target, 106 Imaging device, 107 Image processing device, 108 Image display device , 109 relative position calculation device, 110 support unit, 201 imaging plane, 202 imaging plane coordinate system, 203 camera coordinate system, 204 spherical target image, 205 spherical target image centroid, 206 reference point, 301 straight line, 302a, 302b Position of feature point (sphere center) of spherical target, 303a, 303b Position vector from origin of robot coordinate system to feature point of spherical target, 501 Laser oscillation device, 502 Support unit, 503 Distance measurement device, 601 Spot of laser beam irradiated to target, 701 Rod-shaped jig, 702a, 702b Rod-shaped jig support, 703a, 703b Linear motion joint, 704a, 704b Universal joint, 705a, 705b Needle-shaped tool, 901 marker, 902 Base coordinate system 102a origin, 903 origin of camera coordinate system 203, 904a plane G, 904b plane H, 905 intersection of plane G and line 301, 906 intersection of plane H and line 301, 907 image of marker, 908 intersection from robot coordinate origin 902 Position vector from 905, 909 Position vector from robot coordinate origin 902 to intersection 906, 910 Position vector from robot coordinate origin 902 to camera coordinate origin 903

Claims (14)

By operating a plurality of robots (101a, 101b), the feature points (302a, 302b) defined in the movable part of each robot pass through a three-dimensional space that can be reached by the plurality of robots (101a, 101b) together, and Located at two or more points on each of three or more straight lines (301) that are not parallel,
With the feature points (302a, 302b) positioned on the straight lines (301), the positions of the feature points (302a, 302b) based on the robot coordinate systems are set to the joint axes of the robot (101a, 101b). With forward kinematics calculation from the encoder value of
A distance from the origin of a coordinate system provided separately from the robot coordinate system to the feature point (302a, 302b) or a physical quantity correlated with the distance;
An equation representing the relationship between the position of the feature point (302a, 302b) based on each robot coordinate system and the distance or a physical quantity having a correlation with the distance is derived for each straight line (301) to be a simultaneous equation,
A relative position measurement method between a plurality of robots, wherein a relative positional relationship between the plurality of robots (101a, 101b) is obtained by solving the simultaneous equations and calculating a position vector between the origins of the robot coordinate systems. . The feature points (302a, 302b) are picked up by the image pickup device (106), and the image is displayed in a plane on the image display device (108).
Three or more reference points (206) are provided on the image display device (108), and the robot (101a, 101b) is operated to set the position of the feature points (302a, 302b) to the image display device (108). The feature points (302a, 302b) are positioned on the straight lines (301) by matching with the reference points (206) above,
Measuring the position of the feature point (302a, 302b) based on each robot coordinate system at two or more points on each straight line (301) and measuring a physical quantity having a correlation with the distance or the distance. The relative position measuring method between a plurality of robots according to claim 1, wherein:   A target object (105) is provided on the movable part of the robot, the feature points (302a, 302b) are defined in the target object (105), and the target object (105) displayed on the image display device (108) is defined. 3. The relative position measuring method between a plurality of robots according to claim 2, wherein the area or brightness of the image is a physical quantity having a correlation with the distance.   A target object (105) is provided in the movable part of the robot, the feature points (302a, 302b) are defined in the target object (105), and a zoom lens is attached to the imaging device (106), and the image display device The zoom lens is moved so that the area of the image of the target object (105) displayed in (108) is constant, and the movement amount of the zoom lens at that time is a physical quantity having a correlation with the distance. The relative position measuring method between a plurality of robots according to claim 2.   The relative position measuring method between a plurality of robots according to claim 3 or 4, wherein the target object (105) is caused to emit light. A plurality of the target objects (105) having different sizes or brightness are provided in a movable part of the robot, and the target object (105) to be imaged is switched according to the distance from the imaging device (106). The relative position measuring method between a plurality of robots according to any one of claims 3 to 5. The feature points (302a, 302b) are picked up by the image pickup device (106), and the image is displayed in a plane on the image display device (108),
The robot (101a, 101b) is operated to make the position of the feature point (302a, 302b) coincide with the laser beam oscillated from the laser oscillation device (501) of the laser distance sensor on the image display device (108). The feature points (302a, 302b) are positioned on the straight lines (301) by
The position of the feature point (302a, 302b) based on each robot coordinate system is measured for two or more points on each straight line (301), and the laser beam is changed to 3 by changing the direction of the laser beam. The relative position measuring method between a plurality of robots according to claim 1, wherein the number of straight lines is more than one.   A target object (105) is provided on the movable part of the robot, the feature points (302a, 302b) are defined on the target object (105), and the feature points (302a, 302b) are irradiated with the laser light. The method for measuring a relative position between a plurality of robots according to claim 7, wherein the distance is measured.   The imaging device (106) and the laser oscillation device (501) are integrated, and the optical axis of the imaging device (106) and the laser optical axis of the laser oscillation device (501) are parallel. Item 9. A method for measuring a relative position between a plurality of robots according to Item 8. A needle-like tool (705a, 705b) is provided on the movable part of the robot (101a, 101b), the feature point is defined at the tip of the needle-like tool (705a, 705b), and
By operating the robot (101a, 101b) to position the feature point on a bar-shaped jig (701) forming a straight line, the feature point is positioned on the straight line.
The position of the feature point based on each robot coordinate system is measured for two or more points on the bar-shaped jig (701), and the bar-shaped jig (701) is changed by changing the arrangement of the bar-shaped jig (701). The relative position measuring method between a plurality of robots according to claim 1, wherein there are three or more straight lines.   11. A distance scale is attached to the rod-shaped jig (701), and the distance is measured using the distance scale and the position of the feature point based on the robot coordinate system. Relative position measurement method.   For each of the plurality of robots (101a, 101b), two or more planes (904a, 904b) intersecting with the three or more straight lines and parallel to each other are defined, and the feature points are defined as the planes (904a). 904b), the feature points are positioned at the intersections (905, 906) of the straight lines and the planes (904a, 904b), and the robot of the feature points at the intersections (905, 906). 3. A relative position measuring method between a plurality of robots according to claim 1, wherein a position based on a coordinate system is measured.   The physical quantity having a correlation with the distance is measured based on the position of the feature point at each intersection (905, 906) based on the robot coordinate system. Position measurement method.   Of the two or more parallel planes (904a, 904b), the ratio of the distance between the intersections on one plane and the distance between the intersections on another plane is a physical quantity having a correlation with the distance. The relative position measuring method between a plurality of robots according to claim 13.

Images