JPH0861916A - Optical position and posture detection device - Google Patents

Abstract

PURPOSE: To provide an optical position and posture detection device easy to be installed, due to the capability of the linear formation of a stationary side unit, and not requiring light intensity information as the whole of incident light, due to mounting an angle detector on the stationary side unit and a light emitter at a portable side unit as a position detecting object. CONSTITUTION: Regarding the optical position and posture detection device formed out of a stationary side unit 10 laid for equipment to give information on a position and a posture, and a portable side unit 12 movable relative to the unit 10, the unit 10 has two light sources 18 and 20 arranged at the prescribed positions and emitting light while alternately being switched to each other, and an angle detector 22 for detecting the incident angle of light from the light source 26 of the unit 12. Also, this unit 12 has one light source 26, and an angle detector 28 for detecting the angle of incident light from the two light sources 18 and 20 of the unit 10. A position and a posture are calculated on the basis of information obtained from the two angle detectors 22 and 28.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an optical position / orientation detecting device, and more particularly to a device for detecting three-dimensional position / orientation.

[0002]

2. Description of the Related Art An optical position and attitude detector is proposed in Japanese Patent Application No. 5-225252, which is a prior application. In the optical position and orientation detection device of the application of this earlier application,
In order to detect the dimensional position and orientation, three light sources (for example, LEDs) are arranged on the fixed side so as not to be on the same line, and one light receiver is arranged on the portable side (movable side).

[0003]

However, in the invention of the above-mentioned patent application, since it is necessary to arrange the three light sources on the fixed side so as not to be on the same line, it is difficult to install them. There was an inconvenient side above. In addition, since the position is calculated based on the light arrival intensity ratio, in order to improve the calculation accuracy, there are restrictions such as "light must be emitted with uniform intensity in all directions within the detection range" and each light source. When the light from the above is received on the side of the detected unit, there is a constraint that "the change in the light density of the light receiving surface depending on the incident angle of light needs to be corrected in the calculation".

Therefore, the object of the present invention is that the fixed side unit can be formed linearly, so that the unit can be easily installed, and the angle detector is provided on the fixed side so that the fixed side unit can be installed on the portable side which is the object to be detected. An object of the present invention is to provide an optical position / orientation detecting device that does not require light intensity information of the entire incident light by providing a light emitter.

[0005]

In order to achieve the above-mentioned object, the present invention provides a mobile unit that is movable with respect to a fixed unit and a solid unit that are arranged with respect to a device whose position and orientation should be input. In the optical position and orientation detection device including a side unit, the solid-side unit is provided with two light sources arranged at a predetermined distance and alternately switched to emit light, and an incident angle of light from the light source of the portable unit. And an angle detector that detects an incident angle of light from two light sources of the solid-state unit, and the portable side unit includes one of the two light sources. The optical position / orientation detection device is characterized in that the position / orientation is calculated based on the information obtained from the angle detector.

[0006]

Embodiments of the present invention will now be described with reference to the drawings.

(Mechanical Structure) FIG. 1 is a perspective view showing an optical position / orientation detecting apparatus of the present invention. As shown in FIG. 1, an optical position / orientation detecting device of the present invention comprises a fixed side light emitting / receiving (light receiving and emitting) unit (hereinafter referred to as a fixed side unit) 10 and a portable side light receiving / emitting (light receiving and emitting) unit. (Hereinafter referred to as a mobile unit) 12. The fixed-side unit 10 is fixed or arranged at any appropriate position of the main body of an apparatus such as a computer or peripheral equipment (for example, keyboard, CRT, etc.) to which information regarding the position and orientation of the portable-side unit is input. It

The fixed unit 10 comprises, for example, an elongated rectangular parallelepiped fixed unit main body 14 and a support member 16 for supporting the main body 14 in the horizontal direction. Body 14
Two light sources 18 and 20 are arranged at positions separated by a predetermined distance on one vertical surface. Each of the light sources 18 and 20 includes, for example, a plurality of LEDs (light emitting diodes).
) LED array or a single LED.
It should be noted that, here, a plurality of LEDs are used for a single LE.
This is because in D, when the light intensity cannot be obtained sufficiently, the intensity is increased. Further, in the central portion of the vertical surface of the main body 14, a light receiver 22 that acts as a light angle detector is provided.
(Hereinafter, it may be referred to as an angle detector) is arranged, and this light receiver 22 is preferably provided with an opening 4 for limiting the incident range of light, which will be described later with reference to FIG.
It may consist of a split photodetector, or it may consist of a two-dimensional angle of incidence detector capable of detecting any suitable two-dimensional angle of incidence (eg a five-sided angle detector as described below in connection with FIG. 5). In this specification, the “two-dimensional incident angle detector” refers to an element capable of detecting two incident angles such as a horizontal angle and a vertical angle with one element. As will be described later with reference to FIG. 14, electric circuit components for processing information and calculating the position and orientation of the portable unit are arranged inside the main body 14.

The portable unit 12 comprises, for example, a rectangular parallelepiped portable unit body 24 having a substantially square cross section, and one vertical side surface of the unit body 24 has a light receiver 28 acting as an angle detector of light. (Hereinafter, it may be called an angle detector.)
Reference numeral 8 will be described later with reference to FIG. 5, and preferably comprises a five-sided adhesive type angle detector. Further, on the upper surface of the unit body, a light source 26 is arranged with its light emitting surface facing the same side as the light receiver 28. This light receiver 28, like the light sources 18 and 20 of the fixed-side unit 10, has a plurality of LEDs.
LED array consisting of or a single LED.

(Detection Items of Position and Attitude) FIG. 2 is a perspective view for explaining detection items of the position and attitude of the portable unit 12 with respect to the fixed unit 10. As shown in FIG. 2, in the present invention, spatial position coordinates (three-dimensional position coordinates) can be detected as the position of the mobile unit 12 with respect to the fixed unit 10, and vertical azimuth angle, horizontal azimuth angle, and rotation angle can be detected as the attitude. Can be detected.

(Summary of Operation) The position and orientation of the portable unit 12 with respect to the fixed unit 10 can be detected as follows. (A) First, one of the light sources 18 of the fixed-side unit 10 is turned on, and the angle detector 28 (two-dimensional angle detector) of the portable-side unit 12 makes the incident direction of the light from the light source 18 horizontal.
Detect in the vertical direction. (B) Next, after turning off the light source 18 of the fixed-side unit 10,
The other light source 20 of the fixed-side unit 10 is turned on, and similarly, the incident direction of the light from the light source 20 is detected in the horizontal and vertical directions by the angle detector 28 of the mobile-side unit 12. (C) Next, after turning off the light source 20 of the fixed-side unit 10,
This time, the light source 26 of the portable unit 12 is turned on, and the angle detector 22 (two-dimensional angle detector) of the fixed unit 12 is turned on.
The direction of incidence of light from the light source 26 is detected in the horizontal and vertical directions. (D) By the above, all the data necessary for calculating the position and orientation are gathered. After that, calculation is performed using these data to obtain the position and orientation. (E) After that, by repeating the above-mentioned operations (a) to (d), the position and orientation at that time are sequentially obtained.

(Light incident angle calculation method) (1) Four-division photo detector FIG. 3 is a diagram for explaining a four-division photo detector used as an angle detector (two-dimensional angle detector). Four
The split photodetector 22b has, for example, a vertically split left / right divided into four portions (portions as shown by a to d in FIG. 3c) such as a four-divided pin (PIN) photodiode, and each portion of the incident light The amount of light is detected individually (independently). This 4-division photo detector 22b
Is disposed in the case 22a, and a circular opening 22c (or a square opening as shown) having a diaphragm function is formed in front of the case 22a. The light from an external light source, for example, the light source 26, passes through the opening 22c and then strikes each part of the photodetector.

Next, the principle of angle detection will be described with reference to FIG.
As shown in a, when the direction in which the light receiving section faces and the direction of the light source differ due to the positional relationship between the one light source 26 and the light receiving section 22, the light is obliquely incident on the light receiving section. An example of how the light at this time strikes the light receiving portion is shown in FIG. 3c.
Shown in

The center of the image of the light passing through in the shape of the aperture on the four-division photodetector deviates from the center of the aperture, so that an output difference occurs between the respective parts of the photodetectors. For example, if the output values obtained in the respective parts a, b, c, d are a, b, c, d, then in the case of FIG.
If (a + b) <(c + d) holds and the output difference is taken out by a differential circuit or the like, a current or voltage proportional to the light amount difference can be obtained.

FIG. 4 is a graph showing the relationship between the differential output and the light beam incident angle in the above case. Based on this relationship, the light incident angle can be determined from the differential output.

As described above, regarding the incident directions of light, if the incident angles in the horizontal direction are known by comparing the amounts of light in (a + b) and (c + d), the amounts of light in (a + c) and (b + d) are compared. Then, the incident angle of light in the vertical direction can be known.

(2) Five-sided attachment type angle detector FIG. 5 is a diagram for explaining a five-sided attachment type angle detector (two-dimensional angle detector), and FIG. 5a is a five-sided attachment type angle detector. Fig. 5b is a plan view of the device, and Fig. 5b is a cross-sectional view of the five-side attached angle detector. As shown in Figure 5a,
This type of angle detector 28 is, for example, photodiode chips 28a to 28e, which are photodetectors having the same area on each of five surfaces of a base 28f having a truncated pyramid shape.
Is attached. The principle of angle detection when this angle detector is used will be described with reference to FIG. 5b. In a word, the principle is that “the angle on both sides of the apex angle where the light from the light source hits is sandwiched. The angle of incidence of light is calculated based on the difference in the amount of light that enters the photodetector. "

Now, as shown in FIG. 5b, the angle between the line perpendicular to the bisector of the apex angle and the mounting surface of the photodetector is β, and the angle between the light source and the bisector of the apex angle is θ. Putting it another way, the output from each photodetector is proportional to the projected area of the photodetector with respect to the incident direction of light.
The following equation is obtained. Output of photodetector 28a ∝cos (θ−β) Output of photodetector 28b ∝cos (θ + β) Let “D (a−b) / (a + b)” be the differential output between photodetectors 28a and 28b normalized by all outputs. , D = {cos (θ−β) −cos (θ + β)} / {cos (θ−β) + cos (θ + β)} = tan θ · tan β Therefore, θ = arctan (D / tan β). 1)

As described above, the angle formed by the angle detector 28 with respect to the light source 22 can be obtained from the differential output standardized by the total output of the two photodetectors 28a and 28b. As can be seen from FIG. 5a, for example with respect to the horizontal direction, photodetectors 28a, 28b, 28
Depending on the direction in which the light is incident on c, the angle can be detected using either the combination of photodetectors 28a, 28b or the combination of photodetectors 28b, 28c, and thus a wide range of incident angles can be detected. Similarly, in the vertical direction, the angle can be detected using either the combination of the photodetectors 28d and 28b or the combination of the photodetectors 28b and 28e, depending on the direction in which light is incident on the photodetectors 28d, 28b and 28e. , Therefore
Similarly, a wide range of incident angles can be detected.

In the present invention, which of the above two configurations is used for detecting the incident angle of light depends on what purpose the optical position and attitude detecting device is used. As shown in this example,
The fixed-side unit is not limited to using the four-division photodetector, and the mobile-side unit is not limited to using the five-sided attachment type angle detector.

The four-division photodetector is characterized in that the parts are easy to manufacture, the angle detection sensitivity can be easily set by changing the distance between the opening and the photodetector, and the detection range is narrowed, but the sensitivity is easily increased. . Meanwhile, 5
The surface-attached angle detector is characterized in that the angle detection range can be widened while keeping the sensitivity constant. After all, it is only necessary to select an angle detection device suitable for the purpose, including another angle detection device, and it is possible to support various devices by making the best use of their respective characteristics.

(Spatial Coordinate Calculation Method) FIG. 6 is a diagram for explaining spatial coordinates. As shown in FIG. 6a, first, consider a plane including the line connecting the two light sources of the fixed-side unit and the portable-side unit, and let that plane be the X′Z ′ plane. The fixed side unit is placed on the X'axis, and the angle detector 22
It is assumed that the position of is the origin and the two light sources 18 and 20 are at the (+ d, 0) coordinate position and the (-d, 0) coordinate position, respectively. It is assumed that the portable unit is at the (X'p, Z'p) coordinate position. Here, "X'p" and "Z'p"
It should be noted that is not a value on the actual coordinates (X, Y, Z).

Here, the known value and the value obtained by the angle detection are d, θ, φ in the diagram shown in FIG. 6b.
(Φ: a straight line connecting the light source 20 and the angle detector 28 and the light source 18
Angle formed by a straight line connecting the angle detector 28 with the angle detector 28, that is, the light source 1
8 is the difference between the incident angle of light from 8 and the incident angle of light from the light source 20, and the values to be obtained are "X'p" and "Z 'from the origin.
The distance to p "is" L ".

As shown in FIG. 6c, from the origin (X'p,
Z'p) is L and φ is divided by L
If one of them is φ1, the other is φ2, and the complementary angle of θ is α, the following relational expression holds. d · cos α + d · sin α / tan φ1 = L ··· (2) −d · cos α + d · sin α / tan φ2 = L ··· (3) From the equation (2), φ1 = arctan (d · sin α / (L- Similarly, from the equation (3), φ2 = arctan (d · sinα / (L + d · cosα)) Therefore, φ1 + φ2 = φ = arctan (d · sinα / (L−d · cosα)) + arctan ( d · sin α / (L + d · cos α)) (5) Since it is clear from the definition that α = π / 2−θ,
From the equation (5), the relation between L and φ is obtained.

As described above, when L is determined, the coordinate value (X'p, Z'p) on the X'Z 'plane is X'p = L.co
Calculated by the relationship of sθ and Z′p = L · sinθ,
Although the position of the portable unit on the X'Z 'plane can be obtained, it is difficult to directly calculate L from θ and φ as far as the form of the expression (5) is concerned. So, in the actual calculation,
In consideration of what is done by a microcomputer, L is obtained by convergence calculation when accuracy is required, and L is obtained by approximation calculation when accuracy is not required.

(1) Convergence calculation method Without directly using the equation (5), a straight line forming an angle of θ with the Z'axis is considered (L is included in this), and first, at an appropriate position on the straight line. Take an arbitrary point (X't, Z't). In the triangle formed by this point and (+ d, 0), (-d, 0),
The apex angle at the (X't, Z't) position is obtained and compared with φ. The apex angle is given by the following equation. arctan {(d-X't) / Z't} + arcta
n (d + X't) / Z't}

As a result of the comparison, if φ is smaller, (X '
t, Z′t) is set farther from the origin, and when it is larger, it is set closer and the same calculation is performed. This operation is repeated, and if it falls within a predetermined error range, the calculation ends. (X't, Z't) at this point is (X'p,
Z'p). L as the distance from the origin to the calculated position is given by the following formula. L = √ (X'p ² + Z'p ² ) ... (6)

(2) Approximate calculation method Assuming that the absolute values of φ1 and φ2 are small, tanφ
It is assumed that the approximation can be made by 1≈φ1 and tan φ2≈φ2.
From the formula (5), φ1 + φ2 = φ≈d · sin α / (L−d · cos α)
+ D · sin α / (L + d · cos α) = 2 · L · d ·
sin α / {L ² − (d · cos α) ² } Therefore, φ · L ² −2 · d · sin α · L−φ · (d · cos
α) ² = 0 Therefore, L = {d · sin α ± √ [(d · sin α) ² + (φ ·
d · cos α) ² ]} / φ Here, if the value of φ is small to some extent, the second
If the terms are ignored, the following expression (7) is obtained because L≈ (d · sin α ± d · sin α) / φ − sign is meaningless. L≈2 · d · sin α / φ = 2 · d · cos θ / φ (7) In addition, the following results are obtained as coordinate values. X'p = 2 · d · sin θ · cos θ / φ (8) Z′p = 2 · d · (cos θ) ² / φ (9)

Now that the preparation is completed, the coordinates (Xp, Yp, Zp) of the portable unit on the actual coordinate axes to be finally obtained are derived. By this time
It should be noted that the distance L from the origin to the mobile phone side is obtained from the equation (6) or the equation (7).

FIG. 7 is a side view of the optical position / orientation detecting device of the present invention as viewed in the X-axis direction. As can be easily understood from the positional relationship shown in FIG. 7, the portable-side unit 12
As for the coordinate (Xp, Yp, Zp) value of, the Y coordinate value shown in the following equation (10) is first obtained from the angle v in the Y direction (vertical direction) when the distance L is considered as a vector. Yp = L · sinv (10) Since the X, Z coordinate values Xp, Zp are the tip coordinates when the vector L is projected on the XZ plane, the following (11)
Equation (12) is given. Xp = L · cosv · sin θ (11) Zp = L · cosv · cos θ (12) As described above, according to the equations (10) to (12), Three-dimensional space coordinates can be obtained.

(Posture calculation method) It was clarified that the horizontal and vertical directions of incident light can be detected in the above-mentioned (ray beam incident angle calculation method). If the rotation angle of the mobile unit is 0, these angles obtained from the angle detector of the mobile unit directly represent the horizontal and vertical directions of the mobile unit. However, since the fixed unit has two light sources, if the portable unit is arranged in the horizontal direction and the intermediate point is the origin, the horizontal angle (H1, H
The average value (H1 + H2) / 2 of 2) is the horizontal angle with respect to the origin. Regarding the vertical angle, the values obtained from each light source should be the same (V1 = V2) in principle. On the contrary, if they are not the same, it means that the portable unit is rotating, and the difference is used to obtain the rotation angle.

(1) Rotation Angle FIG. 8 is a diagram for explaining a method of calculating the rotation angle. In FIG. 8, two fixed-side unit light sources 1
A plane including 8, 20 and the portable unit is defined as an XZ plane,
The line connecting the two light sources of the fixed unit is the horizontal direction (X-axis direction), and the Y direction is the vertical direction. Further, as shown in FIG. 8, a local coordinate system (X ', Y', Z ') depending on the portable unit is set. Z'of this coordinate system
The axis is the same as the Z axis of the reference coordinate system, and the X ′ and Y ′ axes rotate about the Z axis as the portable unit rotates. Also, as an angle detector in the mobile unit,
In the embodiment shown in FIG. 1, a five-sided attachment type angle detector is used. However, a four-divided photodetector can also be used. Therefore, in FIG. The case of using a four-division photodetector in FIG. 10 will be described.

When the portable unit does not have a rotation angle with respect to the coordinate system, the light receiving patterns of the angle detector are as shown in FIGS. 9a and 10a, and the incident angle with respect to each light source of the fixed unit. There is no difference in angle in the vertical direction even if is calculated. That is, in the example of the five-face attachment type angle detector of FIG. 9, for example, the light receiving amounts of the central portions of the five-face attachment type angle detectors from the two light sources are set to a and a ′, respectively, and the portions above the diagram are shown. Let b and b'be the received light amounts of
b / a = b '/ a'. Further, in the example of the four-division photodetector in FIG. 10, for example, assuming that the light receiving amounts of the respective portions of the two four-division photodetectors are a, b, c, d and a ′, b ′, c ′, d ′, respectively, ( a + c)-
(B + d) = (a '+ c')-(b '+ d').

However, when the angle of rotation is given, it is shown in FIGS.
Since the light receiving pattern is as shown in FIG.
As indicated by 0, there is a difference between the vertical angle with respect to the light source 18 and the vertical angle with respect to the light source 20. That is, in the case of FIG. 9b, b / a <b '/
a ′, and in the case of FIG. 10b, (a + c) − (b +
d)> (a '+ c')-(b '+ d').

The following description will be made only with respect to the four-division photodetector, but the same applies to the case of the five-face-attached type angle detector. If the viewpoint is changed based on the portable unit, it can be considered as if a height difference (h) has occurred between the light sources, as shown in FIG. By advancing this way of thinking, it is possible to judge which region the rotation angle of the mobile unit is currently in, in the region shown in FIG. Become. That is, when the horizontal angle and the vertical angle are calculated for the two light sources, and the difference between the vertical angles is larger than the difference between the detected horizontal angles, the rotation state of the portable unit is as shown in FIG. Region II or IV of
Will be inside. Further, in order to specify which one of the regions II or IV, the determination is made based on the positional relationship between the light source 18 and the light source 20. That is, the light sources 18 and 20 of the fixed-side unit are sequentially turned on, and the incident angles of each of them are obtained by the portable-side unit.
When 1 is larger than the vertical angle V2 with respect to the light source 20, the positional relationship of the light sources viewed from the portable unit is as shown in FIG.
, Means that the light source 18 is coming up. Therefore, in this case, it can be determined that the portable unit is placed in the rotating state within the area II. By determining the magnitude relationship between the angles in this manner, it is possible to specify to which region the rotation status of the portable unit belongs.

Next, as a step, the number of rotation angles in each specified region is calculated. To do this, we first need to redefine the horizontal and vertical angles to match the knowledge of the region. For example,
If there is a rotation state of the mobile unit in the area II, there will be a rotation shift of about 90 ° between the reference coordinate system and the coordinate system belonging to the mobile unit, and the difference between the four-division photo detectors will result. The meaning of horizontal and vertical is
In the reference coordinate system, the opposite meanings occur in the vertical and horizontal directions. In this way, the horizontal and vertical directions must be adjusted according to the area.

Now, each angle (H
1, V1, H2, V2), the rotation angle "rot" within ± 45 ° can be obtained by the following formula. rot = arctan {(V1-V2) / (H1-H2)} (13) The meaning of this equation is that "(H1-H2) is the horizontal component of the vector representing the direction of rotation, and (V1-V2) is By becoming a vertical component, calculate the inclination of the vector from the horizontal direction,
Get the rotational position. "

As described above, since the rotation angle in the specified area is determined, the rotation of the portable side unit is eventually 3 times.
This means that it is possible to calculate which angle is within 60 °.

FIG. 13 is a flowchart of the operation of determining each area and calculating the rotation angle after the area is determined. The determination of each area and the rotation angle calculation after the area determination are performed as follows. First, the rotation angle is set to 0, and the horizontal incident angle and the vertical incident angle from the two light sources are obtained. Then, the obtained horizontal incident angle and vertical incident angle are respectively H1, H2, V1,
When V2, the horizontal incident angle difference CH (= H1-H2)
And a vertical incident angle difference CV (= V1-V2) is obtained (step S1).

Next, it is determined whether or not CV is larger than the absolute value of CH (step S2). If it is larger, the rotation angle is in the area II, and the horizontal and vertical angles are recalculated (step S3). ). When it is determined in step S2 that CV is smaller than the absolute value of CH, it is determined whether -CV is larger than the absolute value of CH (step S4).
The rotation angle is in the area IV, and the horizontal and vertical angles are recalculated (step S5). When it is determined in step S4 that -CV is smaller than the absolute value of CH, it is determined whether CH is smaller than zero (step S6). If smaller, the rotation angle is in the region III and the rotation angle is in the horizontal or vertical direction. Recalculation is performed (step S7). When it is determined in step S6 that CH is greater than zero, the rotation angle is in the region 1. Step S3,
After 5 and 7, and after it is determined to be no in step S6, the rotation angle is calculated (step S8).

(2) Horizontal azimuth angle, vertical azimuth angle The "horizontal angle" and "vertical angle" treated in the calculation of the rotation angle of (1) are basically angles in the coordinate system on the portable unit. Therefore, in order to realize the purpose of expressing the original posture, it is necessary to convert the numbers into “horizontal angle” and “vertical angle” viewed from the reference coordinate system. Since the rotation angle “rot” of the mobile unit with respect to the reference system is obtained from the equation (13), as shown in FIGS. 14a to 14b, Z
Performs rotation conversion of coordinates around the axis. Specifically, as shown in FIG. 14c, the unit direction vector components representing the “horizontal angle” and the “vertical angle” in the coordinate system of the portable unit are re-expressed by the components viewed from the reference coordinate system.

When viewed from the coordinate system of the portable unit, the horizontal angle and vertical angle of the unit are respectively “H ′”,
If “V ′”, the coordinate system unit vector of the mobile unit is [sin (H ′), sin (V ′), √ {1−si
n (H ′) ² −sin (V ′) ² }. Xh '
= Sin (H '), Yh' = sin (V '),
Assuming that “Xh” and “Yh” are the corresponding unit vector components in the reference coordinate system, Xh = Xh ′ · cos (rot) + Yh ′ · sin (r
ot) Yh = -Xh'.sin (rot) + Yh'.cos
(Rot).

Therefore, if the horizontal angle and the vertical angle viewed from the reference system are represented by H and V, then Xh = sin (H), Y
The following equation is obtained from the relationship of h = sin (V). H = arcsin (Xh) ··· (13) V = arcsin (Yh) ··· (14) From the above, the horizontal and vertical directions of the portable unit can be obtained.

(Procedure for calculating three-dimensional position coordinates and posture)
In order to easily understand the entire operation of the optical position / orientation detecting device of the present invention, the three-dimensional position coordinate and attitude calculating method which has been sequentially described so far is summarized as follows. [I] Lighting of each light source and detection of incident angle of light (a) Operation: Lighting of one light source 18 of fixed side unit and angle detection Description: From the light source 18 of the fixed side unit by the angle detector 28 of the portable side unit The angle of incidence of the light is detected in the horizontal and vertical directions. Detected value: H'1, V'1 (b) Operation: Lighting and angle detection of the other light source 20 of the fixed side unit Content: Incident of light from the light source 20 of the fixed side unit by the angle detector 28 of the portable side unit Detects corners horizontally and vertically. Detected value: H'2, V'2 (c) Operation: Lighting of the light source 26 of the mobile unit and angle detection Details: The angle detector 22 of the fixed unit measures the incident angle of light from the light source 26 of the mobile unit. Detection in horizontal and vertical directions Detected values: H3, V3 [II] Coordinates and attitude calculation (a) Calculation of rotation angle of mobile unit Contents: From H'1, V'1, H'2, V'2, reference The rotation angle of the portable unit is calculated with respect to the coordinate system (coordinate system of the fixed unit). Calculated value: rot (b) Correction calculation of horizontal angle and vertical angle for each light source of the stationary unit of the mobile unit Contents: Correction of the rotation angle (rot) by H′1, V′1,
It is performed for H'2 and V'2, and is converted into a horizontal angle and a vertical angle in the reference coordinate system. Calculated values: H1, V1, H2, V2 (c) Horizontal angle of the portable unit in the reference coordinate system,
Calculation of vertical angle Contents: The horizontal angle and vertical angle in the reference system of the portable unit are finally determined by averaging H1, V1, H2, and V2. Calculated values: H, V (d) Coordinate calculation of the portable unit Contents: For the triangles with the three points of the light sources of the angle detector of the portable unit and the fixed unit as the vertices, carry from corrected H1 and H2 The angle φ of the apex angle of the position of the side unit is calculated, and the spatial position coordinate of the portable unit is calculated from the value and the previously detected H3 and V3. Calculated values: X, Y, Z The three-dimensional position coordinates (X, Y, Z) and attitude (horizontal azimuth H, vertical azimuth V, rotation angle rot) of the portable unit can be obtained by the above procedure. it can.

(Circuit Configuration) FIG. 15 is a circuit block diagram of the optical position and orientation detection apparatus of the present invention. FIG.
As shown in FIG.
A buffer amplifier 34 for buffer-amplifying the signal from the central processing unit 56 when turning on 6 and a current-voltage conversion amplifier 30 for current-voltage converting the signal from the photodetector (angle detector) 28 are converted. It has a calculator 32 for calculating a signal.

The fixed unit has the light source 1 inside.
A buffer amplifier 54 for buffer-amplifying a signal from the central processing unit 56 when turning on 8 and 20;
It has a current-voltage conversion amplifier 50 for converting a signal from the photodetector (angle detector) 22 into a current-voltage, and a calculator 52 for calculating the converted signal. The fixed unit further includes a data switch 40 that switches the signals from the arithmetic units 32 and 52, a series of elements that processes the switched signals, a bandpass filter 42, and a gain adjuster 44.
It has a detection circuit 46 and a low-pass filter 48, and has a central processing unit 56 having an A / D converter for A / D converting these signals. Central processing unit 56
Performs the calculation for calculating the position and orientation described so far.

[0047]

As described above, according to the present invention,
Since the fixed side unit can be formed linearly, it can be easily installed, and by providing an angle detector on the fixed side and a light emitter on the portable side, which is the object to be detected at the position, An optical position and orientation detection device is obtained that does not require light intensity information.

[Brief description of drawings]

FIG. 1 is a perspective view showing an optical position and orientation detection device of the present invention.

FIG. 2 is a perspective view for explaining detection items of a position and a posture of a mobile unit with respect to a fixed unit.

FIG. 3 is a diagram for explaining a four-division photo detector used as an angle detector.

FIG. 4 is a graph showing a relationship between a differential output and a light ray incident angle.

FIG. 5 is a diagram for explaining a five-sided attachment type angle detector (two-dimensional angle detector).

FIG. 6 is a diagram for explaining spatial coordinates.

FIG. 7 is a side view of the optical position and orientation detection device of the present invention as viewed in the X-axis direction.

FIG. 8 is a diagram for explaining a method of calculating a rotation angle.

FIG. 9 is a diagram for explaining a method of calculating a rotation angle in a five-sided attachment type angle detector.

FIG. 10 is a diagram for explaining a method of calculating a rotation angle in a 4-division photo detector.

FIG. 11 is a diagram for explaining a method of calculating a rotation angle.

FIG. 12 is a diagram for explaining a method of calculating a rotation angle.

FIG. 13 is a flowchart of an operation in calculating a rotation angle.

FIG. 14 is a diagram for explaining coordinate axis conversion.

FIG. 15 is a circuit block diagram of the optical position and attitude detection device of the present invention.

[Explanation of symbols]

 10 Fixed-side unit 12 Portable-side unit 18 Light source 20 Light source 22 Photodetector (angle detector) 26 Light source 28 Photodetector (angle detector)

Claims (9)

[Claims] 1. An optical position / orientation detecting device comprising a fixed-side unit arranged for a device whose position and orientation should be input and a portable-side unit movable relative to the solid-side unit, wherein the solid-side unit Has two light sources arranged at a predetermined distance and alternately switched to emit light, and an angle detector for detecting an incident angle of light from the light source of the portable unit, and the portable unit includes: It has one light source and an angle detector that detects the incident angle of light from the two light sources of the solid-state unit, and calculates the position and orientation based on the information obtained from the two angle detectors. An optical position and attitude detection device characterized by the above. 2. The optical position and attitude detection apparatus according to claim 1, wherein each of the light sources is composed of a single light emitting element or an array of a plurality of light emitting elements. Detection device. 3. The optical position / orientation detecting device according to claim 1, wherein the angle detector comprises a two-dimensional angle detector. 4. The optical position and attitude detection device according to claim 3, wherein the two-dimensional angle detector detects a horizontal incident angle and a vertical incident angle of light from a light source. And attitude detection device. 5. The optical position / orientation detection device according to claim 4, wherein the angle detector of the fixed-side unit comprises:
A four-divided photodetector having four independent light detecting parts on the upper, lower, left and right sides, and an opening for restricting the incidence of light is arranged in the front, and the angle detector of the portable unit has five four-sided pyramid trapezoids. An optical position / orientation detecting device, which is a five-faced type angle detector having a photodetector on its surface. 6. The optical position and orientation detection device according to claim 1, wherein the position and orientation are three-dimensional positions (spatial coordinates) and orientations (horizontal azimuth angle, vertical azimuth angle, Rotation) is detected, and an optical position and attitude detection device. 7. An optical position / orientation detecting device comprising a fixed-side unit arranged for a device whose position and orientation should be input and a portable-side unit movable with respect to the solid-side unit, wherein the solid-side unit. Has two light sources arranged at a predetermined distance and alternately switched to emit light, and an angle detector for detecting an incident angle of light from the light source of the portable unit, and the portable unit includes: The light source includes one light source and an angle detector that detects an incident angle of light from the two light sources of the solid-side unit, and the angle detector of the fixed-side unit detects the light from the light source of the portable-side unit. The horizontal angle and the vertical angle are detected, and the angle detector of the portable side unit detects the horizontal angle and the vertical angle of light from each of the two light sources of the fixed side unit, and the horizontal angle detected by the angle detector is detected. Horn An optical position / orientation detection device, which calculates a three-dimensional position and orientation based on the vertical angle. 8. The optical position and orientation detection device according to claim 7, further comprising means for converting the coordinate system of the portable unit into the coordinate system of the fixed unit. apparatus. 9. The optical position and orientation detection device according to claim 7, further comprising means for determining the rotation of the portable unit with respect to the fixed unit in regions. Attitude detection device.