JP2009270893A - Position measuring device, position measuring method, and program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a position measuring device or the like for measuring a position of an object in a simple configuration and high accuracy. <P>SOLUTION: The position measurement device 300 includes: a camera 30 for setting an exposure time so as to have a saturation exposure amount or more of an imaging element and photographing four point LEDs 11-14 provided on the object 100; a storing part for storing position relationship of LEDs 11-14; and a calculation part for obtaining the position of the object 100 from the position relationship of images of LEDs 11-14 photographed with the camera 30 based on the position relationship by reading the position relationship of LEDs 11-14 stored to the storing part. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

  The present invention relates to a position measuring device, a position measuring method, and a program.

  As a means for measuring the position of a predetermined object in a three-dimensional space, a method of photographing the object with two cameras and calculating the position coordinates of the object by triangulation using the distance between the cameras as a baseline. Are known.

  For example, in Patent Document 1, a reference coordinate system is defined on a reference body by obtaining three-dimensional coordinates of at least three points on the reference body, and a predetermined point is obtained by obtaining three-dimensional coordinates of the predetermined point on the subject. A three-dimensional coordinate measurement method for obtaining a position on a reference coordinate system is disclosed.

  In Patent Document 2, a camera posture matrix T is obtained by the least square method or the like, and n camera rotation angles (φn, θn, φn) are obtained from this matrix, and rotated from this rotation angle by Euler angles or the like. A camera posture detection device for obtaining a matrix T ′, obtaining a sum of absolute values of differences between the elements of the matrices T and T ′ as a difference S, and outputting a rotation angle of the difference S as a camera posture. Is disclosed.

JP-A-5-164517 JP-A-10-62145

  The present invention captures a point light source provided in an object and measures the position of the point light source in the captured image with high accuracy, thereby enabling the position measurement of the object to be performed with high accuracy. The purpose is to provide.

  According to a first aspect of the present invention, there is provided a positional relationship between an imaging unit configured to shoot four or more point light sources provided on an object by setting an exposure time so as to be equal to or greater than a saturation exposure amount of an image sensor, and the point light source. And the positional relationship between the point light sources stored in the storage unit, and the position of the object is obtained from the positional relationship and the positional relationship of the image of the point light source captured by the imaging unit. And a calculation unit.

The invention according to claim 2 is characterized in that the photographing unit sets, as the exposure time, a time when the radius of the image of the point light source is at least twice the radius of the image of the point light source during the saturation exposure time. The position measuring device according to claim 1.
The invention according to claim 3 is the position measurement apparatus according to claim 1, wherein the photographing unit sets a time that is four times or more of a saturation exposure time as the exposure time.
The invention according to claim 4 is characterized in that the photographing unit changes the exposure time for each frame photographed by the photographing unit, and sets the exposure time according to the size of the photographed point light source image. The position measuring device according to claim 1.
The invention according to claim 5 is the position measurement apparatus according to claim 1, wherein the photographing unit includes an optical system in which an image of the point light source is formed in a ring shape due to spherical aberration.
The invention according to claim 6 is the position measuring apparatus according to claim 1, wherein the photographing unit photographs infrared light emitted from the point light source.
According to a seventh aspect of the present invention, there is provided a point light source for obtaining a position of the object by the calculation unit according to a size of the point light source image from the image of the point light source photographed by the photographing unit. The position measurement apparatus according to claim 1, further comprising a selection unit that selects an image.

  According to an eighth aspect of the present invention, four or more point light sources provided on an object are photographed by setting an exposure time so as to be equal to or greater than the saturation exposure amount of the image sensor, and the position of the point light source on the object The position measurement method is characterized in that the position of the object is obtained from the relation and the positional relation of the image of the photographed point light source.

The invention according to claim 9 is characterized in that the exposure time is changed for each frame to be photographed and is selected and set according to the size of the photographed point light source image. This is a position measurement method.
According to a tenth aspect of the present invention, a point light source for determining the position of the object is further selected from the captured image of the point light source according to the size of the point light source image. Item 9. The position measurement method according to Item 8.

  According to an eleventh aspect of the present invention, there is provided a computer having a function of setting an exposure time for photographing at least four point light sources provided on an object at a saturation exposure amount of an image sensor, and from a memory, the point light source A program that reads a positional relationship and realizes the function of obtaining the position of the object from the positional relationship and the positional relationship of the image of the photographed point light source.

According to the first aspect of the present invention, since the position of the image of the point light source can be obtained with higher accuracy in an image photographed with a simple configuration as compared with the case where the present configuration is not adopted, the position of the object Can be obtained with higher accuracy.
According to the second aspect of the present invention, the position of the image of the point light source can be obtained with higher accuracy in the photographed image as compared with the case where this configuration is not adopted.
According to the invention of claim 3, it is possible to set the exposure time of the photographing unit by a simpler method as compared with the case where this configuration is not adopted.
According to the fourth aspect of the present invention, compared to the case where the present configuration is not adopted, even when the object moves, it is less likely to be affected by the accuracy of the position measurement of the object.
According to the invention of claim 5, the position of the image of the point light source can be obtained more easily in the photographed image as compared with the case where this configuration is not adopted.
According to the sixth aspect of the present invention, since a difference in contrast between the background and the point light source image is more likely to occur in the captured image as compared with the case where the present configuration is not adopted, the point light source image is further extracted. It becomes easy to do.
According to the seventh aspect of the present invention, it is possible to select a point light source for obtaining the position of the object, and to obtain the position of the object with higher accuracy than in the case where this configuration is not adopted.
According to the eighth aspect of the present invention, since the position of the image of the point light source captured with a simple configuration can be obtained with higher accuracy than in the case where the present configuration is not adopted, the position of the object is further increased. A position measurement method that can be performed with high accuracy can be obtained.
According to invention of Claim 9, compared with the case where this structure is not employ | adopted, even when a target object moves, it can make it difficult to be influenced by the precision of the position measurement of a target object.
According to the tenth aspect of the present invention, it is possible to select a point light source for obtaining the position of the object, and to obtain the position of the object with higher accuracy than in the case where this configuration is not adopted.
According to the eleventh aspect of the invention, the function of measuring the position of the object with high accuracy can be realized by a computer.

Hereinafter, embodiments of the present invention will be described in detail with reference to the accompanying drawings.
<Device configuration>
FIG. 1 is a diagram illustrating an overall configuration including a position measurement device to which the present embodiment is applied and an object for performing position measurement.
The overall configuration illustrated in FIG. 1 includes a target object 100 that is a position measurement target, and a position measurement device 300 that calculates a three-dimensional position and a three-axis angle of the target object 100. The object 100 is provided with four or more LEDs (Light Emitting Diodes) as an example of a point light source. In the form shown in FIG. 1, the object 100 includes LEDs 11 to 14 that are four-point LEDs. In addition, the position measuring device 300 is a camera 30 as an example of a photographing unit that photographs the object 100, and an arithmetic device that calculates a three-dimensional position and a three-axis angle of the object 100 based on an image photographed by the camera 30. 40.

FIG. 2 is a diagram illustrating a configuration example of the camera 30.
The camera 30 includes an optical system 31 that converges the light emitted from the LEDs 11 to 14 and an image sensor 32 that is an example of an image sensor that detects the light converged by the optical system 31.

  The optical system 31 is configured by combining a single lens or a plurality of lenses. For example, a twin lens is used in which two hemispherical lenses are used and their spherical surfaces are combined face to face.

  The image sensor 32 is configured by arranging a charge coupled device (CCD), a complementary metal oxide semiconductor (CMOS), and the like. The surface of the image sensor 32 becomes an imaging surface of the camera 30. By installing a filter according to the emission wavelength of the LEDs 11 to 14 on the front surface of the image sensor 32, unnecessary light is eliminated and only images of the LEDs 11 to 14 are taken. In the case of the present embodiment, it is preferable to use a filter that transmits infrared light as a filter, and image the infrared light with the image sensor 32. By doing so, a difference in contrast between the background and the images of the LEDs 11 to 14 is likely to occur in the captured image, and it becomes easier to extract the LED image. Specifically, for example, when the emission wavelength of the LEDs 11 to 14 is 900 nm, a filter that transmits only a wavelength longer than 850 nm is installed on the front surface of the image sensor 32.

Further, the camera 30 can set the exposure time, and sets an appropriate exposure time in order to obtain the position of the LED image in the photographed image.
In the present embodiment, the exposure time is set to be equal to or greater than the saturation exposure amount of the image sensor 32. By setting the exposure time in this manner, a larger image of the LED on the image sensor 32 is taken, and the position of the LED image in the image can be obtained with higher accuracy.

FIGS. 3A and 3B are diagrams comparing the LED images when the exposure time is set to be equal to or greater than the saturation exposure amount of the image sensor 32 and when the exposure time is not set.
Among these, FIG. 3A shows an image of the LED when the exposure time is not set to be equal to or greater than the saturation exposure amount of the image sensor 32. FIG. 3B shows an LED image when the exposure time is set to be equal to or greater than the saturation exposure amount of the image sensor 32. In addition, as a photographing condition, a LED having a light source size of 1 mm was used, and the camera 30 was photographed with a distance of 1 m from the LED. As the image sensor 32, an image sensor having 1 million pixels was used.

3A and 3B, each block divided in a lattice shape corresponds to one pixel of the image sensor 32. As shown in FIGS. 3A to 3B, the LED image has a double circular shape. Of these, the inner circular portion is a portion with high luminance. Then, a weak luminance portion is photographed as a circular image on the outside. In the LED image in FIG. 3A, the size of the inner circular portion is the size of one pixel of the image sensor 32. However, such image information of about one pixel can only provide information that an LED image is taken at the pixel position.
On the other hand, in FIG. 3B, the inner circular portion of the LED image has a size of 4 pixels. That is, the radius is twice that of the LED image in FIG. By photographing the LED image in this manner, the position of the LED image in the image can be extracted more accurately. Moreover, since the image of the LED in the image is captured more clearly, the difference in contrast with the background becomes larger, and it is easier to extract the LED image from the image.

Next, specific means for photographing such an LED image will be described in more detail.
FIG. 4A illustrates the relationship between the exposure time and the output voltage of the image sensor 32. FIG. 4B is a diagram illustrating the relationship between the exposure time and the radius of the LED image taken by the image sensor 32.
In FIG. 4A, the output voltage of the image sensor 32 increases proportionally as the exposure time elapses from 0. However, when the fixed exposure time (t ₁ ) elapses, the output voltage is saturated at the upper limit value. It can be seen that the output voltage saturates and thereafter the output voltage remains constant at the saturation output voltage. In this exposure time (t ₁ ), the image sensor 32 reaches the saturation exposure amount. Here, this exposure time (t ₁ ) is hereinafter referred to as “saturation exposure time”.

In FIG. 4B, the radius of the LED image taken by the image sensor 32 from 0 to t ₁ is constant at r ₁ , but after the saturation exposure time has elapsed, It can be seen that the radius increases proportionally. Here, the charge accumulated in the image sensor 32 during the saturation exposure time is πr ₁ ² D _{s when} the charge per unit area is D _s, and increases at I _r = πr ₁ ² D _s / t ₁ per second. It will be. Here, in order to double the radius of the image of the LED with that of the LED at the time of the saturation exposure time, an electric charge of π (2r ₁ ) ² D _{s is} required. The exposure time (t ₂ ) for accumulating this charge is π (2r ₁ ) ² D _s / I _r = 4t ₁ . That is, by setting the exposure time to 4 times or more of the saturation exposure time, the radius can be set to 2 times or more compared to the LED image at the saturation exposure time.

In addition, the image | photographed LED image is not restricted to the thing of a double circle shape as shown to Fig.3 (a)-(b).
FIG. 5A is a diagram showing a ring-shaped image as another example of a captured LED image. In FIG. 5A, it can be seen that five LED images are taken in a ring shape. In order to capture such an image, the hemispherical lens shown in FIG. 5B may be used as the optical system 31 (see FIG. 2). Since the hemispherical lens has spherical aberration, the image of the LED imaged on the image sensor 32 by the spherical aberration becomes a ring shape. In order to actually obtain the position of the LED image, the center position of the ring-shaped LED image may be set as the LED image position.

The exposure time is set by changing the exposure time for each frame taken by the camera 30 (see FIG. 2), and selecting and setting the optimum exposure time according to the size of the taken LED image. Is preferred.
FIG. 6 is a diagram illustrating a specific method of changing the exposure time for each frame captured by the camera 30 and selecting and setting an optimal exposure time according to the size of the captured LED image. .
In FIG. 6, the horizontal axis represents the number of frames, and the vertical axis represents the exposure time. The relationship between the number of frames taken by the camera 30 and the corresponding exposure time is shown.
Here, four frames are set as one set, and in this one set, the exposure time is changed in four stages for each frame, and shooting is performed. In other words, shooting is performed for each frame with an exposure time corresponding to the black circle in FIG. Then, the LED image taken for each set is selected to have an appropriate size.

  By extracting an LED image from an image captured in such a form with an optimal exposure time as appropriate, a calculation for obtaining the position of an object 100 (see FIG. 1) described later can be accurately performed. For example, when the object 100 moves, the position and brightness of the LED image provided on the object 100 change from time to time. Even in such a case, the LED imaged with an optimal exposure time as appropriate Because it is possible to select the image, it is difficult to be affected. Therefore, calculation fluctuations are less likely to occur when calculating the position of the object 100 by determining the position of the LED described later.

FIG. 7 is a diagram illustrating the configuration of the arithmetic device 40.
As shown in FIG. 7, the arithmetic device 40 includes a storage unit 41 that stores the positional relationship of LEDs provided on the object 100 (see FIG. 1), and an LED image as an input image captured by the camera 30. The selection unit 42 that selects four or more LED images and the positional relationship of the LEDs from the storage unit 41 are read, and the three-dimensional position of the object 100 and the positional relationship of the LED images in the image And an arithmetic unit 43 for obtaining a triaxial angle.

FIG. 8 is a flowchart for explaining the operation of the arithmetic unit 40.
Hereinafter, an outline of the operation of the arithmetic unit 40 will be described with reference to FIGS. 7 and 8.
The arithmetic device 40 is realized by, for example, a personal computer. The selection part 42 acquires the image of LED image | photographed with the camera 30 (step 101). Then, four or more LED images satisfying a predetermined condition are selected from the LED images (step 102). For this selection, for example, as described above, an LED image whose radius is twice or more compared to the LED image at the saturation exposure time of the image sensor 32 (see FIG. 2) may be selected. The image of the LED may be selected such that the calculation variation is reduced in the calculation in the calculation unit 43. Next, the calculation unit 43 reads the positional relationship of the LEDs corresponding to the selected LED image from the storage unit 41 (step 103). And the calculating part 43 calculates | requires the three-dimensional position and triaxial angle of the target object 100 in which LED was provided based on this positional information and the positional relationship of the image of LED selected by the selection part 42, and the target object 100 position measurement results are output (step 104).

Next, a method of calculating the three-dimensional position and the three-axis angle of the object 100 will be described by taking as an example the case where the selected LED images are on the same plane with four points. Here, it is assumed that the object 100 includes LEDs 11 to 14 that are four-point LEDs as in the case described with reference to FIG.
For the position of the LED 11, the coordinates in the coordinate system with the object 100 as a reference are [X ₁ , Y ₁ , Z ₁ ] ^T. Further, the coordinates of the image of the point of the LED 11 in the coordinate system on the image photographed by the camera 30 (on the image sensor 32) (see FIG. 2) are defined as [u ₁ , v ₁ ]. In the following description, the coordinate system with the position of the camera 30 as the origin is taken as the camera coordinate system, and the coordinates of the LED 11 in this camera coordinate system are taken as [X _c1 , Y _c1 , Z _c1 ] ^T.
In this case, using the 3 × 4 matrix as the projection matrix P, the following relational expression is established.

The projection matrix P in Formula 1 has a constant multiple indefiniteness, and this is absorbed by the coefficient h on the left side of Formula 1. Accordingly, P ₃₄ = 1 can be set. Further, since the LEDs 11 to 14 are on the same plane, the Z coordinate can be set to zero. Therefore, if Z ₁ = 0, the third column of the projection matrix P corresponding to Z ₁ in Equation ₁ can be omitted. Therefore, Formula 1 can be modified as follows. In this case, there are 8 unknown projection matrices.

  The following two constraint equations are obtained from this equation (2).

The relationship represented by Equation 3 holds true for the LEDs 12 to 14 as well. That is, the following relational expressions hold for the LEDs 12 to 14. Here, the coordinates in the coordinate system based on the object 100 of the LEDs 12 to 14 are [X ₂ , Y ₂ , Z ₂ ] ^T , [X ₃ , Y ₃ , Z ₃ ] ^T , [X _{4, respectively.} , Y ₄ , Z ₄ ] ^T, and the coordinates of the images of the points of the LEDs 12 to 14 in the coordinate system on the image taken by the camera 30 are [u ₂ , v ₂ ], [u ₃ , v ₃ ], [U ₄ , v ₄ ]. Further, similarly to the LED 11, Z ₂ = Z ₃ = Z ₄ = 0.

  The following formula is obtained by rearranging formula 3 and formula 4.

数５式において左辺第１項をＡとし、また左辺第２項は射影行列Ｐの３列目がない行列の要素を表していることからＰ３とし、右辺をＢとして書き表すと、以下の式となる。

In equation (5), the first term on the left side is A, and the second term on the left side is P3 because it represents an element of the matrix that does not have the third column of the projection matrix P. Become.

  When the inverse matrix of A is applied to both sides of Equation 6 from the left,

Thus, P3 can be obtained.
The projection matrix P can be represented by the product of the camera inner matrix C and the camera outer matrix [r1 r2 r3 t].

  Here, r1, r2, and r3 are rotation vectors for converting the coordinate system on the object 100 to the camera coordinate system, and t is a translation vector. These vectors are all represented as column vectors having three elements. Since P3 is a matrix without the third column of the projection matrix P,

  It becomes. Therefore,

  Thus, the column vectors r1, r2, and t can be calculated. Since r3 is orthogonal to r1 and r2,

It can be expressed as. Therefore, r3 can be calculated from Equation (11). As described above, all the rotation vectors r1, r2, and r3 and the translation vector t can be calculated. Therefore, when R = [r1 r2 r3], the three-dimensional positions of the LEDs 11 to 14 can be expressed as follows using the camera coordinate system. Note here, the LED12~14 coordinates in the camera coordinate system, respectively ^{_{[X c2, Y c2, Z}} c2] T, [X c3, Y c3, Z c3] T, [X c4, Y c4, Z c4] T It is said.

Since the camera coordinates of the LEDs 11 to 14 can be specified from the relational expression of Formula 12, the three-dimensional position and the three-axis angle of the object 100 are obtained. Therefore, the position of the object 100 can be measured as described above.
In the above description, the case where the number of selected LEDs is four has been described, but the calculation can be performed using the same method when the number of selected LEDs is five or more.

  In addition, in this Embodiment explained in full detail above, although the point light source was comprised by LED, it is not limited to this. For example, a retroreflecting plate may be provided instead of the LED as a point light source, light may be emitted from an illumination device provided near the camera 30, and reflected light from the retroreflecting plate may be captured by the camera 30.

It is a figure explaining the whole structure containing the position measuring apparatus with which this embodiment is applied, and the target object which performs position measurement. It is a figure which shows the structural example of a camera. It is the figure which compared the image of LED with the case where it does not perform when the exposure time is set so that it may become more than the saturated exposure amount of an image sensor. (A) is the figure explaining the relationship between exposure time and the output voltage of an image sensor. Further, (b) is a diagram illustrating the relationship between the exposure time and the radius of the LED image taken by the image sensor. (A) is the figure which showed the ring-shaped thing as another example of the image of LED image | photographed. (B) is a diagram illustrating a case where a hemispherical lens is used as an optical system. It is a figure explaining the specific method which changes exposure time for every flame | frame which a camera image | photographs, and selects and sets optimal exposure time according to the magnitude | size of the image | photographed LED. It is a figure explaining the structure of the arithmetic unit. It is the flowchart explaining operation | movement of the arithmetic unit.

Explanation of symbols

DESCRIPTION OF SYMBOLS 11-14 ... LED, 30 ... Camera, 31 ... Optical system, 32 ... Image sensor, 40 ... Calculation device, 41 ... Storage part, 42 ... Selection part, 43 ... Calculation part, 100 ... Object, 300 ... Position measuring device

Claims (11)

An imaging unit that sets and shoots an exposure time such that four or more point light sources provided on the object are equal to or greater than the saturation exposure amount of the image sensor;
A storage unit for storing the positional relationship of the point light sources;
A calculation unit that reads the positional relationship of the point light sources stored in the storage unit, and obtains the position of the object from the positional relationship and the positional relationship of the image of the point light source captured by the imaging unit;
A position measuring device comprising:   The said imaging | photography part sets the time when the radius of the image of the said point light source becomes 2 times or more of the radius of the image of a point light source in saturation exposure time as said exposure time. Position measuring device.   The position measurement apparatus according to claim 1, wherein the photographing unit sets a time that is four times or more of a saturation exposure time as the exposure time.   The said imaging | photography part changes the said exposure time for every flame | frame which the said imaging | photography part image | photographs, and sets exposure time according to the magnitude | size of the image | photographed point light source image. Position measuring device.   The position measurement apparatus according to claim 1, wherein the photographing unit includes an optical system in which an image of the point light source is ring-shaped due to spherical aberration.   The position measurement apparatus according to claim 1, wherein the photographing unit photographs infrared light emitted from the point light source.   A selection unit that selects, from the image of the point light source captured by the imaging unit, a point light source image for determining the position of the object by the calculation unit according to the size of the image of the point light source; The position measuring device according to claim 1, comprising: Shoot with 4 or more point light sources provided on the object set with an exposure time so that it is equal to or greater than the saturation exposure amount of the image sensor,
A position measurement method characterized in that the position of the target object is obtained from the positional relation of the point light source in the target object and the positional relation of a photographed point light source image.   9. The position measurement method according to claim 8, wherein the exposure time is changed for each frame to be photographed, and is selected and set according to the size of the photographed point light source image.   9. The position measuring method according to claim 8, further comprising selecting a point light source for obtaining the position of the object from the photographed image of the point light source in accordance with the size of the image of the point light source. . On the computer,
A function for setting an exposure time for photographing four or more point light sources provided on an object at a saturation exposure amount or more of the image sensor;
A function of reading the positional relationship of the point light source from the memory, and obtaining the position of the object from the positional relationship and the positional relationship of the image of the captured point light source;
A program that realizes

Images