JPH11231933A - Device for detecting deviation of stop position of mobile object and unmanned carrier - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a mobile object stop position deviation detecting device capable of finding out an accurate positional deviation even when positional relation between a vehicle body and a floor surface is three-dimensionally changed between teaching time and working time and an unmanned carrier capable of executing always accurate work by utilizing the detecting device. SOLUTION: When a mobile object 1 such as an unmanned carrier is moved while photographing featured points such as punched poles 32 of a floor surface 31 by an image pickup means 23 such as a camera loaded on the mobile object 1 and stopped on a prescribed reference position to carry a baggage by a moving arm 22 previously teached at its operation on the reference stop position, a deviation detection means 2 in the mobile object stop position deviation detecting device A1 finds out the deviation of a three-dimensional(3D) stop position of the mobile object 1 only from the relation of respective featured point positions in an image picked up at working obtained when the mobile object 1 is stopped and a reference pickup image obtained when the mobile object is stopped on the reference stop position to correct the teaching data of a working device such as the moving arm 22.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method of stopping a moving body which is mounted on a moving body used for transporting luggage in a clean room or the like, and detects a deviation between a stop position of the moving body and a reference stop position. The present invention relates to a position shift amount detection device and an automatic guided vehicle equipped with the stop position shift amount detection device.

[0002]

2. Description of the Related Art In a clean room or the like of a semiconductor device factory, an unmanned transfer vehicle that transports a load such as a wafer between devices and a device, a stocker, and the like and transfers the load by a transfer arm or the like is used. . In this type of automatic guided vehicle, the transfer arm and the like perform work according to the teaching data taught in a state where the automatic guided vehicle is stopped at a predetermined work position (teaching position) in advance. Therefore, in order to accurately perform the operation using the transfer arm or the like, the stop position of the automatic guided vehicle or the content of the teaching data is corrected based on the deviation of the stopped position of the automatic guided vehicle from the teaching position. It is necessary, and for that purpose, it is indispensable to accurately determine the amount of deviation of the stop position. By the way, in the above-mentioned clean room and the like, a perforated floor material having through holes arranged in a predetermined pattern on the entire surface, such as a punching material or a grating material, is often used as a floor material. Accordingly, the present applicant has already filed a patent application for a device that detects the amount of deviation of the stop position of the automatic guided vehicle using the pattern formed on the floor surface (for example, Japanese Patent Application No. 09-90, 1993).
No. 338530, Japanese Patent Application No. 9-338529). An apparatus and a method for detecting the amount of displacement using these patterns on the floor will be briefly described.

As shown in FIGS. 4 and 5, near the center of the lower part of the automatic guided vehicle 21 on which an arm 22 for transferring goods is mounted (a position that is not easily affected by lighting or the like), a floor surface 31 is provided. A camera 23 having an illuminating device 24 is fixedly installed so as to face each other.
6, a vehicle body control unit 27 and an arm control unit 28 are provided. Further, punching holes 32 are formed in the floor surface 31 in a predetermined pattern, as is usually performed in a semiconductor clean room or the like. A wafer (not shown) is transferred to the arm 22 in a state where the automatic guided vehicle 21 is stopped at a predetermined reference stop position in advance.
The operation of transferring from 1 to a workbench (not shown) is taught.
During the actual transfer operation, the arm 22 performs the operation according to the teaching data. The camera 23 captures an image of the punching floor 31 (see FIG. 6) when the operation of the arm 22 is taught and when the arm 22 is stopped at a predetermined work position during the transfer operation. The arithmetic unit 25 performs image processing on the captured image captured from the camera 23 after the image capturing by the camera 23, thereby obtaining the position data of each of the punching holes 32 (two-dimensional coordinate values in the local coordinate system of the captured image). ) Is required. The position data obtained at the time of teaching the operation of the arm 22 (hereinafter, referred to as position data at the time of teaching) is stored in the storage device 26.
In the arithmetic unit 26, the automatic guided vehicle 2
1 is stopped and the position data of each of the punching holes 32 at that time (hereinafter referred to as "working position data") is obtained. Then, the working position data and the teaching position data stored in the storage device 26 in advance are obtained. Is performed, and based on the difference between the associated position data, the two-dimensional shift amount (in the floor surface 31) of the stop position of the automatic guided vehicle 21 is determined. dx, dy, dθ) are obtained. Here, as a method of the association, for example, a method in which a mark deviating from the arrangement pattern of the punching holes 32 is provided and the mark is used as a reference may be considered. The obtained deviation amount is taken into the vehicle body control section 27 to correct the vehicle stop position, or taken into the arm control section 28 to correct the teaching data of the arm.

[0004]

However, in the above-described conventional position shift detection method, the two-dimensional position shift amount in a plane (translation direction shifts dx, dy, rotation shift d).
θ) is detected, for example, the wheel
2, the vehicle body 21 (and the camera 23) are taught and operated, such as when the vehicle body leans and stops due to inclination, or when the height position of the camera 23 from the floor 31 changes due to wear of the wheels. ) And the floor surface 31 change three-dimensionally, there is a problem that an accurate positional deviation amount cannot be obtained. For example, the relationship between the captured image at the time of teaching and the captured image at the time of work when the vehicle body is tilted and stopped is as shown in FIG. 7A (the upper image at the time of teaching and the lower image at the time of work). FIG. 7 shows the relationship between the teaching-time captured image and the work-time captured image when the height position changes.
(B) As described above, the accurate deviation cannot be obtained by the above-described conventional method in any case.
The present invention has been made in view of the above circumstances, and an object of the present invention is to provide an accurate positional displacement amount even when the positional relationship with the floor surface changes three-dimensionally between teaching and working. An object of the present invention is to provide an apparatus for detecting a displacement amount of a stop position of a moving body that can be obtained, and an automatic guided vehicle that can always perform work accurately using the apparatus.

[0005]

In order to achieve the above object, a first aspect of the present invention is to fixedly install a moving body moving on a plane having a plurality of characteristic points and to take an image on the plane. An image pickup means, the moving object is stopped, and the characteristic point positions on the work-time picked-up image picked up by the image pickup means, and a reference image obtained by stopping the moving object at a predetermined reference stop position in advance and picking up an image by the image pickup means. A shift amount detection unit for detecting a shift amount of the stop position of the moving body based on a relationship with each of the feature point positions on the captured image; Means for determining a three-dimensional shift amount of a stop position of the moving body based only on a relationship between the respective characteristic point positions in the work-time captured image and the reference captured image. Stop position shift amount It is configured as a detection device. Here, the shift amount detecting means detects a three-dimensional attitude change of the imaging means with respect to the plane between the time of capturing the reference captured image and the time of capturing the work-time captured image on the reference captured image. If it is obtained based only on the relationship between each of the characteristic point positions and each of the characteristic point positions on the work-time picked-up image, this is the shift amount of the stop position of the moving body. Also,
The most common example is to use a floor surface as the flat surface and to use a through hole provided in the floor surface as the feature point, but it is needless to say that the present invention is not limited to this. Further, as a specific example of the use of the above-mentioned stop position shift amount detecting device, the second invention teaches operation at the moving object stop position shift amount detecting device according to the first invention and the reference stop position. It is configured as an automatic guided vehicle equipped with a working device and correcting the teaching data of the working device based on the stop position shift amount obtained by the stop position shift amount detecting device. In this automatic guided vehicle, even if wheels fall into through holes or the like during work and the vehicle body is tilted, the working device can be operated accurately.

[0006]

According to the moving body stop position shift amount detecting apparatus of the present invention, the three-dimensional shift amount of the moving body stop position is obtained, and therefore, the moving body is tilted due to, for example, a wheel coming off. The position relationship between the moving body and the plane is three-dimensionally determined during teaching and work, such as when the vehicle stops and when the height of the imaging means from the plane changes due to wear of the wheels. Even in the case of a change, an accurate displacement can be detected. Further, since only the position data of the feature point on the captured image is used in obtaining the three-dimensional shift amount, the program of the arithmetic unit of the conventional stop position shift amount detecting device for obtaining the two-dimensional shift amount is used. Need only be changed, and it is not necessary to install new instruments for measuring the inclination of the moving body, for example. In the automatic guided vehicle according to the present invention,
Since the teaching data of the working device is corrected based on the three-dimensional stop position shift amount obtained by the stop position shift amount detecting device, the vehicle body is tilted and stopped, for example, when the wheels come off during work. As in the case where the height of the image pickup means from the floor surface changes due to the wear of the wheels or the wear of the wheels, the posture of the vehicle body is 3
Even in the case of a dimensional change, the working device can be operated accurately.

[0007]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Embodiments and examples of the present invention will be described below with reference to the accompanying drawings to facilitate understanding of the present invention. The following embodiments and examples are mere examples embodying the present invention, and do not limit the technical scope of the present invention. Here, FIG. 1 is a schematic diagram showing a schematic configuration of a stop position deviation amount detecting device A1 according to an embodiment of the present invention and an automatic guided vehicle 1 on which it is mounted, and FIG. 2 is a world coordinate system, a camera coordinate system, and an image. Explanatory diagram showing the relationship between coordinate systems,
FIG. 3 is an explanatory diagram showing an example of the definitions of θ, φ, and ψ. The stop position shift amount detecting device A1 according to the present embodiment is mounted on an automatic guided vehicle 1 (an example of an automatic guided vehicle according to claim 4), and as shown in FIG. Is substantially the same as the conventional stop position deviation amount detecting device shown in FIG. That is, an arm 22 (an example of a working device) for transferring cargo is mounted on the automatic guided vehicle 1, and is located near a lower central portion of the automatic guided vehicle 1 (a position that is not easily affected by lighting or the like). A camera 23 (an example of an imaging unit) having an illumination device 24 is fixedly installed so as to face the floor 31 (an example of a plane). Furthermore, the arithmetic unit 2 (an example of a shift amount detecting unit), the storage device 26,
7, and an arm control unit 28 are provided. Further, as is usually performed in a semiconductor clean room or the like, a punching hole 32 is formed on the floor surface 31 in a predetermined pattern.
(An example of a plurality of feature points) is formed. However, in the stop position shift amount detecting device A1, the calculation method of the position shift amount in the arithmetic unit 2 is significantly different from the above-described conventional method, whereby the position shift amount is a three-dimensional amount (translational direction shifts dx, dy, dz). , Rotation deviation dθ, dφ, d
ψ).

First, an outline of the operation of detecting the amount of displacement by the stop position displacement detecting device A1 will be described. The operation of transferring the wafer (not shown) from the automatic guided vehicle 1 to the work table (not shown) while the automatic guided vehicle 1 is stopped at a predetermined reference stop position is taught to the arm 22 in advance. You. During the actual transfer operation, the arm 22 performs the operation according to the teaching data. In the camera 23, an image of the punching floor 31 is displayed when the operation of the arm 22 is taught (hereinafter, referred to as teaching) and when the arm 22 is stopped during transfer (hereinafter, referred to as working) (see FIG. 6). Is imaged. The arithmetic unit 2 performs image processing on the captured image captured from the camera 23 after the image capturing by the camera 23, thereby obtaining the position data of each of the punching holes 32 (two-dimensional coordinate values in the local coordinate system of the captured image in the local coordinate system). ) Is required. Here, it is desirable to use the center position of the punching hole 32 as the position for obtaining the position data. This is because, for example, the vehicle body is tilted and
(A) Even if a captured image as shown in the figure below is obtained and each punching hole 32 becomes elliptical on the image, the center position can be specified accurately regardless of the elliptical shape. is there. The position data obtained at the time of the teaching (hereinafter referred to as position data at the time of teaching) is stored in the storage device 26. The arithmetic unit 26 obtains the position data of each of the punching holes 32 at that time (hereinafter referred to as “work position data”) from the work-time captured image obtained at the time of work, and then obtains the work position data and the storage device 26. One-to-one correspondence with the above-described teaching position data stored in advance is performed, and the three-dimensional stop position shift amount of the automatic guided vehicle 1 is determined based on the associated position data. (Dx, dy, dz, dθ, dφ, dψ) are obtained. Here, as a method of the association, for example, a method in which a mark deviating from the arrangement pattern of the punching holes 32 is provided and the mark is used as a reference may be considered, but a detailed description is omitted in the present embodiment. I do.
The obtained three-dimensional displacement amount is taken into the vehicle body control unit 27 to correct the vehicle stop position, or taken into the arm control unit 28 to correct the teaching data of the arm.

Next, a method of calculating the three-dimensional displacement amount performed by the arithmetic unit 26 will be described in detail. still,
In the following description, as shown in FIG. 2, the coordinates in the world coordinate system set on the floor 31 (hereinafter referred to as world coordinates)
To X _w (x _w , y _w , z _w ), the coordinates in the camera coordinate system set for the camera 23 (hereinafter referred to as camera coordinates).
Is defined as X _c (x _c , y _c , z _c ), and the coordinates (hereinafter referred to as image coordinates) in the image coordinate system set for the image captured by the camera 23 are defined as X _g (x _g , y _g ). . First, the coordinate transformation formula from world coordinates to camera coordinates is
It can be expressed by the following equation.

(Equation 1) The relationship between the world coordinates and the camera coordinates at the time of teaching is defined as follows.

(Equation 2) From the above equation (2), x _c = x _w , y _c = y _w , z _c = t _z0 (3) The projective transformation equation from camera coordinates to image coordinates can be expressed by the following equation.

(Equation 3) If m ₀ = t _z0 / f, the above equation (4) can be transformed into the following equation using the above equation (3).

(Equation 4)

Next, considering the operation,
From equations (1) and (5), the camera coordinates X _c′ (X_c', Y
_c', Z_c') Can be expressed as follows.

(Equation 5) The projective transformation formula from camera coordinates to image coordinates is expressed by the following formula.

(Equation 6) The expression relating to x in the expression (7) can be expressed by the following expression from the expression (6).

(Equation 7) Here, if t _z = t _z0 (1 + δ), (t _z0 δ: the amount of change in camera height),

(Equation 8) From this, the relationship t _z = fm ₀ (1 + δ) (10) is obtained. Using the above equation (10), the above equation (8) becomes as follows.

(Equation 9) Here, r ₁₁ = (1 + δ) R ₁₁ , r ₁₂ = (1 + δ) R ₁₂ r ₂₁ = (1 + δ) R ₂₁ , r ₂₂ = (1 + δ) R ₂₂ r ₃₁ = (1 + δ) fR ₃₁ , r ₃₂ = ( 1 + δ) fR ₃₂ t _x = m ₀ (1 + δ) T _x , t _y = m ₀ (1 + δ) T _y (12) The above equation (11) becomes as follows.

(Equation 10)

Similarly, when the equation relating to y in the above equation (7) is modified, the following equation is obtained.

[Equation 11] For the above equations (13) and (14), a set of corresponding teaching position data and working position data (x _g , y _g ,
x _g ′, y _g ′), four sets of R ₁₁ , R ₁₂ ,
_{R 21, R 22, R 31} , R 32, T x, the value of T _y is determined. Incidentally, in the above (1), since ^{_{r 11 2 + r 21 2 +}} r 31 2 = 1 r 12 2 + r 22 2 + r 32 2 = 1 ... the relationship of (15) holds, equation (12) and the ( 15)
The ^{equation, (1 + δ) 2 (} R 11 2 + R 21 2 + f 2 R 31 2) = 1 (1 + δ) 2 (R 12 2 + R 22 2 + f 2 R 32 2) = 1 ... (16) next to this than f Can be expressed by the following equation.

(Equation 12) Further, when the above equation (17) is substituted into the above equation (16),
δ can be expressed by the following equation.

(Equation 13) Above (17), Substituting (18) into the above equation _{(12), r 11 ~r 32} , t x, can be obtained t _y. T _x and t _y obtained here are x in the plane, respectively.
_Are the positional deviation amounts dx and dy in the direction and the y direction, and t _z0 δ is the positional deviation amount dz in the z direction, that is, the height direction.

Next, an example of a method of calculating the rotational displacement amounts dθ, dφ, dψ with respect to each coordinate axis using r ₁₁ to r ₃₂ obtained above will be described. Figure 3 for θ, φ, and ψ
Defined as shown below. Also,

[Equation 14] Then, the following relationship is obtained.

(Equation 15) From the relationship between the above equation (20) and the above r _{11 to} r ₃₂ , the rotational deviation amounts dθ, dφ, dψ can be obtained as follows.

(Equation 16)From the calculation method of the three-dimensional position shift amount described above,
Calculation processing of the actual position shift amount in the arithmetic unit 26
In other words, the arithmetic unit 26 first determines the corresponding teaching position.
Data and work position data set (x_g, Y_g, X_g′,
y_g') And take out four sets, and use them in the above (13), (1)
4) The above R₁₁, R₁₂, R_{twenty one}, R_{twenty two}, R₃₁,
R₃₂, T_x, T_yFind the value of Then, the above (1)
7), f and δ are obtained from the equations (18), and
Substituting into the equation, r₁₁~ R₃₂, T_x, T _yIs obtained.
T found here_x, T_yAre x directions in the plane respectively
Are the displacement amounts dx and dy in the direction y and the direction t, respectively._z0δ is z
Direction, that is, the amount of positional deviation dz in the height direction. Furthermore,
The above r₁₁~ R₃₂Is substituted into the above equation (21), the rotation shift
The amounts dθ, dφ, dψ are obtained.

As described above, when the displacement amounts (dx, dy, dz, dθ, dφ, dψ) of the three-dimensional stop position of the automatic guided vehicle 1 are obtained by the arithmetic unit 26, these positions are determined. The displacement amount is taken into the vehicle body control unit 27 to correct the vehicle stop position, or is taken into the arm control unit 28 to correct the teaching data of the arm. As described above, the stop position shift amount detecting device A1 according to the present embodiment is configured to shift the three-dimensional stop position of the automatic guided vehicle 1 (dx, dy, dz, dθ, dφ,
dψ) can be obtained, for example, when the wheels fall into the punching holes 32 and the vehicle body leans and stops,
As in the case where the height position of the camera 23 from the floor surface 31 changes due to the wear of the wheels, the positional relationship between the vehicle body 1 (and the camera 23) and the floor surface 31 during the teaching and the work is three-dimensional. , The position displacement amount can be accurately detected. Also, since only the position data of the punched hole on the captured image of the floor surface is used when calculating the three-dimensional shift amount, the calculation unit of the conventional stop position shift amount detecting device for calculating the two-dimensional shift amount is used. It is only necessary to change the program, and for example, there is no need to install new instruments for measuring the inclination of the vehicle body.

[0014]

[Embodiment] In the above embodiment, the description has been made on the assumption that the punching holes are regularly arranged on the floor surface, but it is not always necessary to arrange them regularly. In addition, it is needless to say that the characteristic points are not limited to the above-described punching holes, and a mark that can be specified may be used. Further, in the above embodiment, the case where the automatic guided vehicle (moving body) travels on the floor (plane) has been described, but the moving body and the plane are limited to the automatic guided vehicle and the floor. is not. For example, when a plurality of marks are formed on a worktable (plane) and a camera is attached to the tip (moving body) of a robot arm that moves on the worktable, positioning is performed. It can also be applied to the case of forming a mark and positioning the working device with respect to the work.

[0015]

As described above, the first aspect of the present invention is an imaging means which is fixedly installed on a moving body which moves on a plane having a plurality of feature points, and which captures an image on the plane, The moving object is stopped and the respective characteristic point positions on the work-time picked-up image picked up by the image pickup means and the moving object is stopped in advance at a predetermined reference stop position and the reference picked-up image is picked up by the image pickup means. A shift amount detection device for detecting a shift amount of the stop position of the moving body based on a relationship with each of the feature point positions, wherein the shift amount detection means includes: A displacement amount of a stop position of the moving body, wherein a displacement amount of a three-dimensional stop position of the moving body is obtained based only on a relationship between each of the feature point positions in the work-time captured image and the reference captured image. As a detection device In the case of teaching, as in the case where the moving body is tilted and stopped due to, for example, the wheels coming off, or the height position of the above-mentioned imaging means from the plane is changed due to wear of the wheels. Even when the positional relationship between the moving body and the plane changes three-dimensionally between the time of the operation and the time of the operation, an accurate positional shift amount can be detected. Further, since only the position data of the feature point on the captured image is used in obtaining the three-dimensional shift amount, the program of the arithmetic unit of the conventional stop position shift amount detecting device for obtaining the two-dimensional shift amount is used. Need only be changed, and it is not necessary to install new instruments for measuring the inclination of the moving body, for example. Further, the second invention is provided with the moving body stop position displacement amount detection device according to the first invention and a working device that is taught to operate at the reference stop position, and the stop position displacement amount detection device Is configured as an automatic guided vehicle that corrects the teaching data of the working device based on the stop position shift amount obtained by the method described above. Even when the posture of the vehicle body changes three-dimensionally compared to when the operation of the work device is taught, such as when the height position of the imaging means from the floor surface changes due to wear of the wheels, the work device Can be operated accurately.

[Brief description of the drawings]

FIG. 1 is a schematic diagram showing a schematic configuration of a stop position deviation amount detection device A1 according to an embodiment of the present invention and an automatic guided vehicle 1 on which the device A1 is mounted.

FIG. 2 is an explanatory diagram showing a relationship among a world coordinate system, a camera coordinate system, and an image coordinate system.

FIG. 3 is an explanatory diagram showing an example of definitions of θ, φ, and ψ.

FIG. 4 is a schematic diagram showing a schematic configuration of an automatic guided vehicle 21 equipped with a stop position shift amount detection device according to the related art.

FIG. 5 is a schematic diagram showing a schematic configuration of a stop position shift amount detection device according to the related art and an automatic guided vehicle 21 equipped with the same.

FIG. 6 is a diagram showing an example of a floor image captured by a camera 23.

FIG. 7 is a diagram showing a relationship between a captured image during teaching (upper) and a captured image during work (lower) in a case where the vehicle body tilts and stops (a) and a case where the height position of the camera 23 changes (b). .

[Explanation of symbols]

 Reference numerals 1, 21: automatic guided vehicle 2, 25: arithmetic unit (an example of a deviation detecting unit) 22: arm (an example of a working device) 23: camera (an example of an imaging unit) 24: lighting device 26: storage device 27: body Control unit 28: Arm control unit 31: Punching floor (example of plane) 32: Punching hole (example of feature point)

──────────────────────────────────────────────────続 き Continued on the front page (51) Int.Cl. ⁶ Identification symbol FI G05D 3/12 G05D 3/12 L G06T 7/00 H01L 21/68 F H01L 21/68 G06F 15/62 415

Claims (4)

[Claims] 1. An image pickup means fixedly mounted on a moving body moving on a plane having a plurality of feature points and taking an image on the plane, and an image taken by the imaging means after stopping the moving body. The moving based on the relationship between each of the characteristic point positions on the captured image at the time of work and the respective characteristic point positions on the reference captured image captured by the imaging unit by previously stopping the moving body at a predetermined reference stop position. In a moving body stop position shift amount detecting device, comprising: a shift amount detecting unit configured to detect a shift amount of a body stop position, the shift amount detecting unit includes: An apparatus for detecting a displacement of a stop position of a moving body, wherein the amount of displacement of a three-dimensional stop position of the moving body is obtained based only on a relationship between characteristic point positions. 2. The method according to claim 1, wherein the shift amount detecting means detects a time when the reference captured image is captured based on a relationship between each of the feature point positions on the reference captured image and each of the feature point positions on the work captured image. The stop position shift amount of the moving body according to claim 1, wherein the shift amount of the stop position of the moving body is detected by a three-dimensional posture change of the imaging unit with respect to the plane between the time of the work-time captured image and the time of the work-time captured image. Detection device. 3. The apparatus according to claim 1, wherein the plane is a floor, and the feature point is a through hole provided in the floor. 4. A stop position displacement amount detecting device for a moving body according to claim 3, and a working device for performing operation teaching at the reference stop position, wherein the stop obtained by the stop position displacement amount detection device is provided. An automatic guided vehicle that corrects the teaching data of the working device based on the displacement amount.