JPH0823492B2 - Position and orientation angle measurement device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial field of use] The present invention relates to a moving body, particularly an underground moving body such as a tunnel excavator (TBM, shield, semi-shield) or a small diameter propulsion machine (iron mall). The present invention relates to a position / orientation angle measuring device for measuring a position and an attitude angle.

[Conventional technology]

For underground excavators that perform tunnel excavation to bury water pipes, gas pipes, etc. in the ground, in order to promote according to the execution plan line, the movement position in the underground of the underground excavator, that is, the execution plan line Relative horizontal position (horizontal displacement) and relative vertical position (vertical displacement) with respect to the line, and posture angle of the underground excavator in the ground, that is, horizontal posture angle with respect to the execution planning line (yawing angle) and execution planning line Vertical posture angle (pitching angle)
Must be measured so that the deviation between each of these measured data and the corresponding target value (horizontal displacement, vertical displacement, yawing angle and pitching angle is zero) becomes zero.

Therefore, the underground excavator is provided with a position / orientation measuring device capable of measuring these position and attitude angle. FIG. 16 conceptually shows the structure of the sensor section of this position / orientation angle measuring apparatus. As shown in the figure, from the light projector 6 ′ arranged at a predetermined position outside the underground excavator, its optical axis is along the inner longitudinal direction of the underground excavator, and its optical axis is the planned movement path of the underground excavator ( Light beam 3'to match the implementation plan line)
(Laser beam) is projected as a reference light beam. on the other hand,
The inside of the underground excavator is separated by a predetermined distance L in the longitudinal direction, and PSD (position sensitive detector)
1'and 2'are provided. This PSD 1 ', 2'
In general, a-Si (amorphous silicon) PSD is used.

Then, the light beam 3 'receiving surface 1a of the' PSD1 'is irradiated, together with the two-dimensional irradiation position of incident light (x _1, y ₁₎ is detected, PSD2 even, PSD1' light beam transmitted through the 3 '
Is irradiated, and the two-dimensional irradiation position (x ₂ , y ₂ ) is detected. Here, the optical axis of the light beam 3'is aligned with the central axis CT of the sensor section when the underground excavator is at the position according to the execution planning line and neither yawing nor pitching is performed with respect to the execution planning line. , And the origins O ₁ and O ₂ (0, 0) of the light receiving surfaces of both PSDs 1 ′ and 2 ′, respectively. Therefore, if the irradiation position of PSD1 ′ is (x ₁ , y ₁ ) and the irradiation position of PSD 2 ′ is (x ₂ , y ₂ ) as described above, the relative horizontal position (left and right) with respect to the execution planning line Displacement) X and the relative vertical position (vertical displacement) Y with respect to the line, and the attitude angle of the underground excavator in the ground, that is, the horizontal attitude angle (yaw angle) θx with respect to the execution planning line and the vertical attitude with respect to the execution planning line The angle (pitching angle) θy can obviously be calculated as in the following equations (1) to (4).

X = −x ₁ (1) Y = −y ₁ (2) θx = arctan {(x ₁ −x ₂ ) / L} (3) θy = arctan {(y ₁ −y ₂ ) / L} (4) Conventionally, the position and attitude angle of the underground excavator, and by extension, all moving bodies have been obtained as described above.

Further, in the underground excavator, the position and attitude angle data obtained as described above are transmitted as information for correcting the direction to the operating room of the underground excavator provided in the starting shaft or the like. There is a need.

Heretofore, in the past, position and attitude angle data are analog-transmitted through individual signal lines.

In addition, as shown in Fig. 17, the underground excavator is located at the position according to the implementation plan line in the front stage of PSD1 ', and the light beam 3'becomes under the condition that neither yawing nor pitching is performed with respect to the implementation plan line. There is provided a scale plate 4'where the light spot is displayed at the origin position. Then, the scale plate 4 '
By visually recognizing the two-dimensional position coordinates of the light spot of the light beam 3 ′ displayed on the
Vertical shift) is performed. In this case, in order to facilitate the reading of the position by the operator, illumination lamps 5 'are provided around the PSD 1' to ensure sufficient brightness.

[Problems to be Solved by the Invention]

The above-described conventional position and orientation angle detection device has a merit that the structure of the device itself can be simplified because the detection principle is simple, but it has the following problems in practical use.

1) First, generally, the irradiation position detection accuracy is generally high (1
In order to obtain a PSD, it is essential to deposit a uniform resistive film. However, it becomes more difficult to secure a uniform resistive film as the light receiving surface of the PSD increases in area, and the manufacturing yield also decreases. In this regard, it is easier to increase the area of the a-SiPSD than the single-crystal silicon PSD, but it is still difficult to secure a uniform resistance film as the light receiving surface of the PSD increases.

On the other hand, when the a-SiPSD is used for the position / orientation angle measuring device, two pieces having a size larger than the necessary measurement range due to the irradiation position detection range, the structure of the a-SiPSD, and the position / orientation angle measurement principle are used. A-Si PSD is required. When such a large-sized a-SiPSD is used, it has been impossible to bring the position and posture angle measurement accuracy to a sufficient level because of the difficulty in securing a uniform resistance film as described above. As described above, the conventional device has a drawback in that it cannot satisfy the planned range and the measurement accuracy required for practical use at the same time.

2) Secondly, since PSDs 1'and 2'are mounted on an underground excavator, it is necessary to shorten the interval L between both PSDs due to space restrictions.

On the other hand, the measurement accuracy of the attitude angle can be clearly understood from the above equations (3) and (4), and the irradiation positions (x ₁ ,
It can be seen that it depends on the detection accuracy of (y ₁ ) and (x ₂ , y ₂ ) and the size of the distance L between PSDs.

It is possible to improve the accuracy of detecting the irradiation position by improving the accuracy of posture angle measurement.
It is difficult to achieve high accuracy due to the influence of the variation of the irradiation position detection accuracy of 2 'and the problem 1).

In addition, by increasing the PSD interval L, the practically required ± 1
It is possible to make the posture angle measurement accuracy on the order of / 1000 radians, but in order to ensure this, it is necessary to make the PSD interval L sufficiently large. However, in practice, L cannot often be made sufficiently large due to the above-mentioned space restrictions.

As described above, the conventional apparatus has a drawback that it is not possible to simultaneously satisfy the miniaturization of the apparatus and the required accuracy of measuring the attitude angle.

3) In addition, the irradiation position of PSD1 'and PSD
The range of posture angles that can be measured changes according to the distance L. That is, as shown in FIG. 18 (a), the light beam 3'has a distance l ₁ When irradiating to the separated positions, if the incident angle of the light beam 3'with respect to PSD1 'is not within the range of θ ₁ or less, the light beam 3'will receive the light receiving surface 2a' of PSD2 '(within the position detection range). Yes, it is not possible to irradiate inside (indicated by a broken line in the figure), and the posture angle cannot be measured. On the other hand, when the light beam 3 ′ is applied to a position separated by a distance l ₂ (> l ₁ ) from the end of the light receiving surface 1 a ′ of the PSD 1 ′, light having an incident angle in the range of θ ₂ or less that is larger than θ _{1 and} smaller than θ _1. Even the beam 3 'can be irradiated into the light receiving surface 2a' of the PSD 2 ', and the attitude angle can be measured.
It can be seen that the upper limit of the incident angle of the light beam 3, that is, the upper limit of the measurable attitude angle decreases as the irradiation position of the PSD 1 ′ deviates from the center.

When the PSD interval L is L ₁ as shown in FIG. 7B, the light beam 3 ′ is separated from the end of the light receiving surface 1 a ′ of PSD 1 ′ by a distance l ₁
When irradiating to a distant position, the light beam 3 ′ within the range of the incident angle θ or less can be received by the PSD 2 ′.
When the distance L is L ₂ (> L ₁ ), the light beam 3 ′ is PSD1 ′.
The light-receiving surface 1a 'distance l ₂ from the end of the (> l ₁₎ spaced light beams also incident angle θ the range beginning when irradiated to a position 3' can be received by PSD2 '. in this way
It can be seen that the larger the PSD interval L, the smaller the upper limit of the incident angle of the light beam 3 ', that is, the upper limit of the measurable attitude angle.

As described above, in the conventional device, the attitude angle that can be measured when the light beam 3 ′ irradiates the end of the light receiving surface 1 a ′ of the PSD 1 ′ is the upper limit value of the attitude angle that can be actually measured, and is extremely small. It turns out that it becomes. Also, as mentioned above,
If the D-interval L is increased to increase the posture angle measurement accuracy, the measurable upper limit of the posture angle is reduced.

Due to the problems 1) to 3) as described above, the conventional one is not so practical and has not been adopted or implemented at present.

Furthermore, the conventional method of analog transmission of position and orientation data is vulnerable to noise due to its nature,
There is a problem that the performance of the transmission path is affected. Also, due to its nature, each data position X, Y, attitude angle θ
It is necessary to transmit x and θy through individual transmission paths, which increases the number of transmission cables (the number of lines) and raises the device cost. In addition, there is a demand to suppress the transmission cable (rotation speed) as much as possible due to space constraints.

Further, in the conventional system in which the illumination lamps 5 '... Are provided around the PSD 1'to illuminate the scale plate 4', light is scattered, so that in order to obtain sufficient brightness for visual recognition Are required for the illumination lamps 5 '..., which leads to an increase in device cost. In addition, since the illumination lamps 5 'are provided around the PSD 1', the illumination lamp 5 'positionally obstructs the light receiving area of the PSD 1', thereby narrowing the measurement range of the position and the posture angle. was there.

The present invention has been made in view of the above circumstances, and aims to increase the measurement range of the position and posture angle and to improve the measurement accuracy of the position and posture angle,
The first purpose is to downsize the device, and the second purpose is to transmit measurement data with high reliability and efficiency, low cost, and small space. The third object is to make the display of the scale plate visible without narrowing the measurement range of the attitude angle, and the common object thereof is to provide a highly reliable position / orientation angle measuring apparatus.

[Means and Actions for Solving the Problems]

Therefore, according to a first aspect of the present invention, there is provided a light projector for projecting light so that an optical axis coincides with a predetermined movement path of a moving body, and receives, transmits, and irradiates light projected from the light projector. A first position detector that two-dimensionally detects a position, and a second position detector that receives light transmitted by the first position detector and two-dimensionally detects an irradiation position. A position which is disposed at a predetermined distance from the moving body, and calculates the position and the posture angle of the moving body based on the two-dimensional irradiation position of the light detected by the first and second position detectors. An attitude angle measuring device, wherein a lens disposed between the light projector and the first position detector or between the first position detector and the second position detector; Moving body based on the two-dimensional irradiation position of the light detected by the position detector of Position as well as calculating and so that comprise a calculating means for calculating an attitude angle of the mobile body based on the two-dimensional irradiation position of the detected by a second position detector the light.

According to this configuration, since the lens is disposed between the light projector and the first position detector or between the first position detector and the second position detector, the projected light is transmitted through the lens. After being converged, the first position detector can detect the deviation of the light from the reference position (origin) according to the irradiation position, and the second position detector can control the lens relative to the irradiation position according to the irradiation position. The incident angle can be detected. Therefore, the position and the posture angle of the moving body can be calculated for each detection result of the first and second position detectors.

In addition, in the second invention of the present invention, in addition to the configuration of the first invention, a transmission means for multiplexing the position data and the attitude angle data of the moving body calculated by the calculation means and serially transmitting the multiplexed data is provided. ing.

That is, the position data and attitude angle data of the moving body are not individually and parallel analog-transmitted, but all data are digitally transmitted at one time through one line.

In addition, in the third invention of the present invention, in addition to the configuration of the first invention, the first position detector is provided between the light projecting device and the sensor portion including the first and second position detectors and the lens. Of 2
A position display plate that displays the three-dimensional irradiation position is provided
An illumination lamp for illuminating the position display plate is provided near the focal point of the lens.

That is, the illumination lamp is not provided around the first position detector, but provided near the focal point of the lens.
Positionally, it does not disturb the light receiving area of the first position detector. Since the illumination light emitted from the illumination lamp is near the focal point of the lens, it is refracted by the lens and condensed into parallel rays. The directivity of the illumination light becomes stronger and the degree of scattering becomes smaller. As a result, the position display plate can be illuminated extremely brightly without providing a large number of illumination lamps.

〔Example〕

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

In addition, in the embodiment, it is assumed that the position / orientation angle measuring device according to the present invention is applied to an underground excavator such as a tunnel excavator or a small diameter propulsion machine. However, the present invention is not limited to the underground excavator and can be arbitrarily applied to an underground moving body that can move in the ground, and can be applied to any moving body.

The position / orientation angle measuring apparatus according to the embodiment is roughly composed of a sensor unit and an arithmetic processing unit, and FIG. 1 conceptually shows the structure of the sensor unit.

As shown in FIG. 1, the sensor unit of the embodiment apparatus is arranged at a predetermined position (starting shaft, etc.) outside the underground excavator, and the optical axis is the planned movement route of the underground excavator, that is, the pointing plan line. The laser transmitter 6 that projects the laser beam 3 as a reference beam toward the inside of the underground excavator and the underground excavator so that the longitudinal direction of the underground excavator and the light receiving surface 1a are perpendicular to each other. It is arranged inside the excavator and receives the laser beam 3 projected from the laser transmission unit 6, and the two-dimensional irradiation position of the laser beam 3 (x ₁ ,
y ₁ ), which outputs an electric signal indicating
And (Position Sensitive Detector) 1, arranged inside the underground excavator spaced so that its light receiving surface 2a is parallel to the light receiving surface 1a of the PSD1 from PSD1 predetermined distance L _2,
When the laser beam 3 that has passed through PSD1 is received,
An electric signal indicating the dimensional irradiation position (x ₂ , y ₂ ) is output. It is configured mainly with a PSD2 having a light transmitting property, and further, as shown in the figure (a) or the figure (b), inside the underground excavator, between the laser transmission part 6 and the PSD1, or PSD1 and
A lens 7 for converging the laser beam 3 projected from the laser transmission unit 6 is arranged between the PSD 2 and the PSD 2 such that a plane portion 7a thereof is parallel to the light receiving surfaces 1a and 2a of the PSDs 1 and 2.

Here, as the PSDs 1 and 2, a-Si (amorphous silicon) PSD described later is used. Further, the electric signals output from the PSDs 1 and 2 are input to the arithmetic processing unit 8 or 9 described later, and the arithmetic processing unit 8 or 9 calculates the position and attitude angle of the underground excavator. The lens 7 may be an aspherical lens or a spherical lens. However, when an aspherical lens is used as the lens 7, it is necessary to arrange the lens 7 so that its focal point is located on the light receiving surface 2a of the PSD 2. However, if a spherical lens is used as the lens 7, it is not necessary to do so. Here, the underground excavator is at the position according to the implementation plan line, and neither yawing nor pitching is performed on the implementation plan line. Then, the optical axis of the laser light 3 coincides with the central axis CT of the sensor unit, and the origins O ₁ and O ₂ of the light receiving surfaces 1a and 2a of both PSDs ₁ and _{2 are}
Each of them passes through (0, 0).

With the above configuration, the laser beam 3 is projected from the laser emitting unit 6, the PSD ₁ detects the two-dimensional irradiation position (x ₁ , y ₁ ) of the incident light, and the PSD 2 detects the two-dimensional irradiation position of the incident light. When the three-dimensional irradiation position (x ₂ , y ₂ ) is detected, the relative horizontal position (lateral displacement) X with respect to the execution plan line, the relative vertical position (vertical displacement) Y with respect to the line, and the underground of the underground excavator , That is, the horizontal posture angle (yaw angle) θx with respect to the execution plan line and the vertical posture angle (pitching angle) θ with respect to the execution plan line
In the case of the sensor configuration of FIG. 1 (a), y can be calculated as follows.

_{X = - (F / L 2} ) · x 1 ... (5-a) Y = - (F / L 2) · y 1 ... (6-a) θx = arctan (-x 2 / F) ... (7) θy = arctan (−y ₂ / F) (8) where F is the focal length of the lens 7. In these equations (5-a), (6-a), (7), and (8), equality is established on both sides when an aspherical lens is used.
When a spherical lens is used, the left and right sides are substantially equal, and it is necessary to perform aberration correction described later. On the other hand, in the case of the sensor configuration of FIG.
Although x and θy can be obtained from the above equations (7) and (8), the positions X and Y are obtained from the following equations (5-b) and (6-b).

X = −x ₁ (5-b) Y = −y ₁ (6-b) Here, these (5-a), (6-a), (5-b),
The equations (6-b), (7), and (8) are equations that hold when the inclination angle of the underground excavator with respect to the execution planning line (reference ray) is sufficiently smaller than 1 (radian). For example, in a small-caliber propulsion machine, -2 ° <θ <2 ° (2
Since θ = 0.035 radians), θ << 1 is true, and it can be seen that these equations can be applied sufficiently. Note that, as shown in FIG. 1, the x-coordinates of the two-dimensional irradiation positions of PSDs 1 and 2 are in the direction perpendicular to the paper surface, and the y-coordinates are in the vertical direction in the paper surface. The polarities of the positions X and Y are, as is apparent from the above formula, the detection positions x ₁ and y ₁ of the PSD _1.
The opposite of the polarity. Similarly, the polarities of the posture angles θx and θy are opposite to the polarities of the detection positions x ₂ and y ₂ of the PSD ₂ .

Here, the structure of the a-SiPSD used as the PSDs 1 and 2 and the principle of position detection will be briefly described.

The schematic diagram of a-SiPSD is shown in FIG. a
-SiPSD is a three-layer structure by a thin film, the transparent resistive layer 50 (ITO or SnO ₂₎ for x-coordinate detection from the upper surface, then a-Si
Layer 51 (This layer has the same structure as used for a-Si solar cells
In a pin structure, when light is irradiated to this layer, a photoelectromotive force is generated), and a transparent resistance layer 52 for y-coordinate detection is provided on the lower surface. When the He-Ne laser irradiates the spot light 56 such as a light emitting diode (LED) to the a-Si PSD, only the portion irradiated with the spot light operates as a solar cell, and the voltage (0.6 to 0.8) is applied.
V) occurs, and a current resulting from this occurs in the photovoltaic unit 53. As shown in FIG.
Serves as a current outlet, and the y-coordinate detection resistance layer 52 serves as a suction inlet.

First, paying attention to the x-coordinate detection resistance layer 50, the current that has flowed out flows through the resistance layer 50, and flows from the current collecting electrodes 54 attached to both ends of the resistance layer 50 (shown by oblique lines) to an external circuit. Are extracted as Ix ₁ and Ix ₂ . Further, focusing on the y-coordinate detecting resistance layer 52, the current supplied from the external circuit as Iy ₁ and Iy ₂ is represented by a current collecting electrode 55 (shown by oblique lines).
The more it flows into the resistance layer 52, the photovoltaic part 53 from the suction port.
Reach

Furthermore, the position detection principle of the a-Si PSD will be described with reference to FIG.

As shown in the figure, the position of the collecting electrode 54a is X ₁ , the collecting electrode 5
The position of 4b is X ₂ , the distance between the collecting electrodes 54a and 54b is K, and the positions X _{1 and}
The distance to X ₂ and the corresponding resistance are l ₁ , R _{1 and} I ₂ ,
If R ₂ and the current generated by the incident light 56 are I ₀ ,
The currents Ix ₁ and Ix ₂ flowing out from the current collecting electrodes 54a and 54b are obtained by the following equation (9).

Ix ₁ = {R ₂ / (R ₁ + R ₂ )} ・ I ₀ Ix ₂ = {R ₁ / (R ₁ + R ₂ )} ・ I ₀ (9) Here, the resistance layer 50 is uniform and the distance l ₁ , Assuming that l ₂ is proportional to the resistance values R ₁ and R ₂ , respectively, the above equation (9) is Ix ₁ = (l ₂ / K) · I ₀ Ix ₂ = (l ₁ / K) · I ₀ … ( 10), and if the distance K and the current I ₀ are constant, the currents Ix ₁ and Ix ₂ are functions of only the distances l ₂ and l ₁ , and the difference or ratio between Ix ₁ and Ix ₂ is calculated. By doing so, the relative position (x coordinate position) in the sensor plane can be obtained. However, in reality, there is a change in I ₀ due to a change in light intensity, so the relative position in the sensor plane is calculated by the following equation (11) without using the simple difference or ratio between Ix ₁ and Ix _2. (X
Coordinate position). (11) x in formula represents the relative position of the sensor surface, for example, x = 1 at position X _1, x = -1 at the position X _2, a x = 0 at the center position X _1, X _2.

x = (Ix ₁ −Ix ₂ ) / (Ix ₁ + Ix ₂ ) = (l ₂ −l ₁ ) / K = 1− (2l ₁ / K) (11) Note that the direction of the current is also the y coordinate. A formula is similarly obtained only by changing the x coordinate to the y coordinate.

Hereinafter, a-SiPSD as described above is used as PSDs 1 and 2, and an aspherical lens is used as the lens 7, and underground excavation is performed using the above formula (11) with the sensor configuration of FIG. 1 (b). A calculation processing mode in the case of calculating the position and attitude angle of the machine will be described.

The positions X and Y of the underground excavator and the attitude angles θx and θy are calculated based on the equations (5-b), (6-b), (8) and (9), but in practice Angle θx, θ
y is sufficiently smaller than 1 radian, and tan θx and θy are θ
x and θy are approximately equal, so the calculations are (8) and (9)
Instead, the following equations (8) 'and (9)' are used.

θx = −x ₂ / F (7) ′ θy = −y ₂ / F (8) ′ These (5-b), (6-b), (7) ′, (8) ′
The calculation based on is performed in the calculation processing unit 8 shown in the block diagram of FIG.

As will be described below with reference to FIG. 1 (b) and FIG. 4 together, as described above, when the laser beam 3 is projected from the laser transmission unit 6 and is irradiated on the light receiving surface 1a of the PSD 1, Currents Ix ₁ and Ix ₂ corresponding to the incident position x ₁ of the incident light from PSD ₁ are output to the amplifiers 10 and 11, and currents Iy ₁ and Iy ₂ corresponding to the irradiation position y ₁ of the incident light in the y direction. Is output to the amplifiers 12 and 13. Then, the currents Ix ₁ , Ix ₂ , Iy ₁ and Iy ₂ are respectively subjected to current-voltage conversion in the amplifiers 10, 11, 12 and 13, and are also amplified to a required level, so that the current Ix
₁ , the voltages Vx ₁ and Vx ₂ corresponding to Ix ₂ are applied to the adder 14 and the subtractor 15 in the next stage, and the voltages Vy ₁ and Vy ₂ corresponding to the currents Iy ₁ and Iy ₂ are added to the next stage. 16 and subtractor 17.

After passing through the PSD 1, the laser light 3 is refracted by the aspherical lens 7 and converged on the light receiving surface 2 a of the PSD 2 arranged at the focal point. Then, similarly, currents Jx ₁ and Jx ₂ corresponding to the irradiation position x ₂ of the incident light from PSD ₂ and current J corresponding to the irradiation position y ₂
y ₁ and Jy ₂ are output to the amplifiers 18 and 19 and the amplifiers 20 and 21, respectively. And the output voltages Ux ₁ and Ux _{2 of the} amplifiers 18 and 19
Is added to the adder 22 and the subtractor 23 in the next stage, and the output voltages Uy ₁ and Uy ₂ of the amplifiers 20 and 21 are added to the adder 24 and the subtractor 25 in the next stage, respectively.

In the adder 14, the addition operation of the input voltages Vx ₁ and Vx ₂ Vx ₁ + Vx ₂
There is executed, the subtracter 15, the subtraction operation Vx ₁ -Vx ₂ input voltage Vx _1, Vx ₂ is executed. These calculation results are applied to a divider 26, first (11) division operation corresponding to the right-hand side of _{(Vx 1 -Vx 2) / (} Vx 1 + Vx 2) is performed in該除adder 26. Division result of the divider 26 shows the x-direction irradiation position x ₁ of the incident light PSD1, added to the next stage of the offset-gain adjustment circuit 28, in the offset-gain adjustment circuit 28 (5- b) calculation of the equation is performed, lateral displacement X of the underground Excavators proportional to the x-direction irradiation position x ₁ is output.

Similarly, the adder 16 executes the addition operation of the input voltage, and the subtractor 17 executes the subtraction operation of the input voltage. Then, these operation results are added to the divider 26,
In the divider 26, the division operation corresponding to the right side of the equation (11) is executed. Here, the division result of the divider 26 indicates the y-direction irradiation position y ₁ of the incident light of PSD ₁ , which is added to the offset / gain adjustment circuit 29 in the next stage, and the offset / gain adjustment circuit 29 (6 The calculation of equation (b) is executed, and the vertical displacement Y of the underground excavator that is proportional to the y-direction irradiation position y ₁ is output.

On the other hand, for PSD2, the adder 22 similarly performs the addition operation of the input voltage, and the subtractor 23 performs the subtraction operation of the input voltage. Then, these calculation results are added to the divider 27, and the divider 27 executes the division operation corresponding to the right side of the equation (11). Division result here to the divider 27, which shows the x-direction irradiation position x ₂ of the incident light PSD2, added to the next stage of the offset gain adjustment circuit 30,
In the offset-gain adjustment circuit 30 (8) 'type operation is performed in the left-right direction posture angle θx of the underground Excavators proportional to the x-direction irradiation position x ₂ is output.

The adder 24 performs an addition operation on the input voltage, and the subtractor 25 performs a subtraction operation on the input voltage.
Then, these operation results are added to a divider 27,
At 27, the division operation corresponding to the right side of the equation (11) is executed. Division result of the divider 27 here is for indicating the y-direction irradiation position y ₂ of the incident light PSD2, added to the next stage of the offset-gain adjustment circuit 31, in the offset-gain adjustment circuit 31 (9 ) 'Expression is executed, and y
The vertical attitude angle θy of the underground excavator is output in proportion to the direction irradiation position y ₂ . Hereinafter, the signals indicating the positions X and Y and the attitude angles θx and θy are transmitted to a control panel arranged in a starting shaft or the like via a required transmission cable. Then, these transmitted position and attitude angle data are used for, for example, CRT.
Is displayed on a display device mainly composed of, and these data are used as a feedback amount for control of moving the underground excavator according to the execution plan line.

The offset / gain adjustment circuits 28, 29, 30 and
At 31, the output range is freely set according to the required planning range of the position and the posture angle. Further, the offset / gain adjustment circuits 28 to 31 are adapted to correct the displacement of the irradiation positions of the PSDs 1 and 2, that is, to perform the zero point offset.
Now, a displacement Δd of the irradiation position y of PSD2 will be described as an example.
As shown in FIG. 6, assuming that the laser light 3 is incident on the lens 7 at the incident height H and the PSD 2 is vertically displaced with respect to the light receiving surface 2a by ΔZ like PSD2b and PSD2c, these and the focal point are assumed. Apparently, Δd / ΔZ = H / F (13) ′ is established between the distance F and this, and this is Δd = ΔZ · H / F (13), and the irradiation position deviation Δd is PSD2. It is understood that the correction can be easily performed if the deviation ΔZ of is known. However, the effect of the irradiation position deviation Δd on the attitude angles θx and θy is the cos of Δd.
Component and the sine component, and correcting this with an offset / gain adjustment circuit leads to a complicated circuit configuration. It is desirable to secure mechanically.

The arithmetic processing unit 8 shown in FIG. 4 does not need to perform correction arithmetic for complicated spherical aberration using an aspherical lens, and thus can be easily configured with an analog circuit.
Of course, it is also possible to implement it by using a digital circuit.

It is also possible to use a spherical lens as the lens 7.

In addition, although the sensor configuration of FIG. 1 (b) has been described in this embodiment, it goes without saying that the sensor configuration of FIG. 1 (a) also uses a processing circuit similar to the arithmetic processing unit 8 shown in FIG. Can perform operations.

By the way, in the above-described embodiment, since the arithmetic processing is simplified by using the aspherical lens, there is a merit that the analog circuit can be easily configured, but the aspherical lens has a demerit that it is expensive. Therefore, an embodiment of a digital circuit configuration in which an inexpensive spherical lens is used and the correction calculation for the complicated spherical aberration due to this can be easily processed by software will be described.

Hereinafter, a spherical lens is used as the lens 7, and the first
A calculation processing mode in the case where the position and the attitude angle of the underground excavator are calculated using the above equation (10) with the sensor configuration of FIG.

The calculation of the positions X and Y and the posture angles θx and θyn is (5-
This is performed in the arithmetic processing unit 9 shown in the block diagram of FIG. 5 based on the equations (a), (6-a), (7) and (8).

That is, as will be described with reference to FIG. 1 (a) and FIG. 5 together, when the laser light 3 is projected from the laser transmission unit 6, the laser light 3 is first refracted by the lens 7 and is reflected by the PSD 1. The light-receiving surface 1a is irradiated. Then, the currents Ix ₁ and Ix ₂ corresponding to the incident position x ₁ of the incident light from PSD ₁ are output to the amplifiers 32 and 33, and the current Iy ₁ corresponding to the incident position y ₁ of the incident light to the y direction ₁ , Iy ₂ is output to the amplifiers 34 and 35. Then, in these amplifiers 32, 33, 34 and 35, the currents Ix ₁ , Ix ₂ ,
Iy ₁ and Iy ₂ are respectively current-voltage converted and amplified to a required level, and the voltages Vx ₁ and Vx ₂ corresponding to the currents Ix ₁ and Ix ₂ are added to the adder 36 of the next stage, and The voltages Vy ₁ and Vy ₂ corresponding to the currents Iy ₁ and Iy ₂ are applied to the adder 37 in the next stage.

The laser light 3 is disposed at the lens focal point after passing through the PSD 1 (although it is not always necessary to dispose the laser light 3 at the lens focal point).
The light is converged on the light receiving surface 2a of the PSD2. Then, similarly, the currents Jx ₁ and Jx ₂ corresponding to the irradiation position x ₂ of the incident light from PSD ₂ are changed to the amplifier 38,
In addition to being output to 39, the currents Jy ₁ and Jy ₂ corresponding to the irradiation position y ₂ of the incident light are output to the amplifiers 40 and 41. Then
The output voltages Ux ₁ and Ux _{2 of the} amplifiers 38 and 39 are added to the adder 42 of the next stage, and the output voltages Uy ₁ and Uy _{2 of the} amplifiers 40 and 41 are added.
Is added to the adder 43 in the next stage.

In the adder 36, the addition operation of the input voltages Vx ₁ and Vx ₂ is Vx ₁ + Vx ₂
Is executed and the addition result is added to the multiplexer 44 at the next stage. Similarly, the adders 37, 42, and 43 also perform the addition operation of the input voltages, and the addition results are added to the multiplexer 44. In addition, the output voltages Vx ₁ , Vy ₁ , Ux ₁ and Uy _{1 of the} amplifiers 32, 34, 38 and 40 are also multiplexed 44
Is added to

The circuit operation of the subsequent stage from the multiplexer 44 is managed by the CPU 45 according to the program written in the ROM 45a. Below, C
The processing procedure performed by the PU 45 will be described with reference to the flowchart of FIG.

Focusing on PSD1, first, the CPU 45 is initialized (step 100), and the channel of the A / D converter 46 is PSD.
Set to the channel assigned for signal input.
Next, the gain of the AGC circuit 47 is set to 1. Then, the channel of the multiplexer 44 is set to the channel of the signal voltage (for example, Vx ₁ + Vx ₂ ) corresponding to the total current of PSD1. Then, when the gain of the AGC circuit 47 is 1, Vx ₁ + Vx ₂ (corresponding to the total current of PSD1) is A /
Input from the D converter 46.

After that, the AGC is based on the magnitude of the signal voltage input with gain 1 so that the input signal voltage becomes a constant value near the full scale of the data length that can be handled by the CPU45 (for example, 0 to 255 bits for an 8-bit CPU). The process of setting the gain of the circuit 47 is executed. The reason for doing this is that the irradiation position (x ₁ ) of the incident light can be calculated from the modified formula l ₂ / L = Ix ₁ / I ₀ … (10) ′ of the above formula (10), but the light intensity is actually If the total current I ₀ fluctuates due to the change of or the total current I ₀ becomes small due to the decrease of the light intensity, the calculation accuracy of the division (current ratio Ix ₁ / I ₀ ) performed in the CPU45 will deteriorate. This is to prevent the accuracy of calculation from deteriorating by making the total current of the AGC circuit 47 always be a certain value.

When the gain of the AGC circuit 47 is set as described above, the channels of the multiplexer 44 are sequentially switched under these set gains, and the voltages Vx ₁ + Vx ₂ , Vx ₁ , Vy ₁ + Vy ₂ and Vy ₁ are A / D. Converted and input sequentially (Step 10)
1). Then, the calculation corresponding to the above (10) ′, that is, the division Vx ₁ / I ₀ corresponding to Ix ₁ / I ₀ based on the input voltages Vx ₁ + Vx ₂ , Vx ₁
(Vx ₁ + Vx ₂ ) is executed, and the x-direction irradiation position x ₁ of the incident light of PSD ₁ is obtained. Similarly, the division Vy corresponding to the equation (10) ′
₁ / (Vy ₁ + Vy ₂ ) is executed to obtain the y-direction irradiation position y ₁ of the incident light of PSD ₁ (step 102).

When the two-dimensional irradiation position (x ₁ , y ₁ ) of incident light is calculated for PSD1 as described above, the same processing as steps 101 to 102 above is repeatedly executed, and the two-dimensional irradiation position of incident light for PSD2 is also repeated. (X ₂ , y ₂ ) is calculated.

In this way, the incident light two-dimensional irradiation position (x ₁ , y ₁ ), (x ₂ ,
y ₂ ) is calculated, then (5-a), (6-a),
The processing corresponding to the operations of the expressions (7) and (8), that is, 2
The position (X, Y) and posture angle (θx, θy) corresponding to the three-dimensional irradiation positions (x ₁ , y ₁ ) and (x ₂ , y ₂ ) are selected from a data table not shown, Position (X, Y) and posture angle (θx, θy) are obtained (step 103; this processing will be described later). Then, data checking and averaging processing is performed on these calculation results (step 104).

The position data and the attitude angle data of the underground excavator thus obtained are output to the data communication device 48 via an interface of RS-232-C standard, for example. Then, the data communication device 48 multiplexes the position data and the attitude angle data, and serially transmits them via a single cable to a host computer (not shown) arranged in the starting shaft or the like.

That is, FIG. 15 is a schematic diagram showing a protocol of a transmission signal transmitted from the data communication device 48. As shown in FIG. 15, the transmission signal includes a start code indicating the beginning of a data string and a position and orientation following the start code. And a corner data string. The position / attitude angle data string is a data string in which each data of the horizontal position X, the vertical position Y, the horizontal attitude angle θx, and the vertical attitude angle θy is time-division multiplexed, and the transmission having such contents is performed. The signal is serially transmitted repeatedly at a cycle corresponding to the data length of the start code and position / orientation angle data string. The protocol of the transmission signal is not limited to that shown in the figure, and an error code may be added to detect an error by a method such as a parity check. If there is data to be transmitted other than the position / orientation angle data, this data can be added together after the position / orientation angle data string so that it can be transmitted together with one transmission cable ( Step 105).

The above-described processing is executed in the configuration shown in FIG. 5, but the contents of the data table will be described here.

FIG. 9 is a diagram showing a geometrical relationship of the sensor unit,
Here are shown the lens (spherical lens) 7 the radius of curvature R, the thickness of the center of the thickness D, PSD1 lens 7 D1, the thickness of PSD2 of D2, the refractive index N ₀ of the air, the refractive index N ₁ of the lens 7, Refractive index of PSD1 and PSD2
It is assumed that N ₂ is known and determined in the process of designing the sensor unit.

Hereinafter, the incident height H ₀ and the incident angle θ ₀ of the laser light 3 on the lens 7 are obtained, and then the exit height and the exit angle of the lens 7 and the entrance height, the entrance angle and the exit height and the exit height on each surface of PSD1 and PSD2 are determined. The angle is calculated sequentially (this is called ray tracing method).

First, the relationship between the incident height H ₀ on the lens 7 and the incident angle θ ₀ and the exit height Hi on the lens 7 and the exit angle θd is obtained.

In the same figure, when the laser light 3 parallel to the longitudinal center axis CT of the sensor portion is incident on the lens 7 at the incident height H ₀ , the incident angle θ ₀ is expressed by the following equation (14).

θ ₀ = arcsin (H ₀ / R) (14) On the other hand, when the incident angle θ ₀ , the refraction angle θb on the sphere is θb = arcsin {(N ₀ / N ₁ ) ・ sin θb} from the law of reflection and refraction. … (15) holds. Also, the incident angle θc and the exit angle θ on the plane side
Since the relation of θc = θ ₀ −θc (16) holds with d, the injection height Hi can be expressed by the following equation.
That is, assuming that the distance from the incident point CP of the lens 7 to the lens plane portion 7a is T, T = DR (1-cosθ ₀ ) ... (17) and Hi = H ₀ −Ttan θc = H ₀ − { D-R (1-cos θ ₀ )} · tan θc (18) The exit angle θd is expressed by the following equation (19) from the law of reflection and refraction.

θd = arcsin {(N ₁ / N ₀ ) · sin θc} (19) Considering the case where the incident angle θi is changed by θ from the incident angle θ ₀ , if θ ₀ is θi in equation (14), then The expression of can be used as it is. However, θi = θ ₀ ± θ, where θi is an unknown number and θ ₀ is known,
θ is given.

Next, the relationship between the incident height S1 on PSD1 and the incident angle θ ₀ and the exit height Hj on PSD1 and the exit angle θd ′ will be obtained.

In the same figure, if the lens flat portion 7a and the light receiving surface 1a of the PSD1 are parallel, the exit height θd on the lens 7 and the incident angle θd 'on the PSD1 are equal. On the other hand, the relationship between the incident angle θd ′ and the exit angle θd ″ at PSD1 is shown in FIG. 10 and equation (20).
Required from.

N ₀ sin θd ′ = N ₂ sin θe = N ₀ sin θd ″ (20) θd = θd ′ = θd ″ (21) Then, the relationship between the incident height S1 and the exit height Hj on PSD1 is expressed by the following equation. .

S1 = Hi−X1tanθd (22) Hi = S1−D1tanθe (23) Next, the incident height Y2 on PSD2 is obtained.

If the exit surface of PSD1 and the entrance surface of PSD2 are parallel, the entrance height S2 on PSD2 can be expressed by the following equation (24).

S2 = Hj-X2tan θd (24) The light receiving surface of PSD1 and PSD2 corresponding to the positions X and Y and the attitude angles θx and θy in the required position measurement range and angle measurement range of the underground excavator by the above calculation method. The irradiation position in the inside is calculated, and these are stored and stored in the data table.

The above is the calculation method by the ray tracing method. However, as described below, the deviations due to aberrations and incident angles from the irradiation positions of PSDs 1 and 2 are corrected to calculate the positions X and Y and the posture angles θx and θy. May be.

In other words, AB / CD = BE / DE (25) due to the similarity between ΔABE and ΔCDE in Fig. 11. Therefore, the incident height H ₀ on the lens 7 can be expressed by the following equation.

H ₀ = (F / L ₂ ) S1 (26) However, in reality, in the case of a spherical lens, the point where the light 3 transmitted through the lens 7 crosses the central axis CT due to the incident height at the lens 7. different.

-The laser light 3 is refracted by the lens 7 and PSD1.

Equation (26) does not hold rigorously because of the above reasons.
When the ratio H ₀ / S1 at each incident height is calculated by the ray tracing method (14) to (24), it becomes a very gradual monotone decreasing function as shown in FIG. In this case, if H ₀ / S1 = K1, then K
1 = K1 (H), but there is no problem if you think that K1 = K1 (S1). Generally, it can be approximated by the average value of H ₀ / S1 at each incident height, but when this is a problem in terms of measurement accuracy, K1 = K1 (S
Considering 1), the linear approximation or the curve approximation may be performed.

H ₀ = K1 (S1) · S1 (27) The above is the case where the laser light 3 is incident parallel to the central axis CT, but the incident angle θ (0
If the light enters the lens 7 at an angle other than that, it is necessary to consider its effect. S1 by incident angle θ at each incident height
When the correction amount ΔS1 of is calculated by the ray tracing method, it can be seen that it can be approximated by ΔS1 = −L ₁ tan θ (28) in the range of θ << 1. Therefore, the incident height H ₀ can be expressed by the following equation.

_{H 0 = K1 (S1) ·} (S1 + ΔS1) ... (29-1) ΔS1 = -L 1 · tanθ ... (29-2) On the other hand, the calculation of the incident angle is performed as follows.

As shown in FIG. 13, when the laser light 3 is incident on the central axis CT at an incident angle θ, and when θ << 1 holds, the following equation holds.

S2 = Ltan θ (30) Actually, there is an influence of aberration. Considering this,
Equation (30) is as follows.

S2 (H ₀ , θ) −S2 (H ₀ , 0) = Ltan θ (31) where S2 (H ₀ , θ) is the irradiation position on PSD2 when the incident height is H ₀ and the incident angle is θ. And calculated from the output of PSD2. Further, S2 (H ₀ , 0) is the irradiation position on PSD2 when the incident height is H ₀ and the incident angle is 0, and it is calculated in advance by the ray tracing method and put in the program as a data table. After all, the incident angle θ is given by the following equation.

θ = arctan [{S2 (H ₀ , θ) −S2 (H ₀ , 0)} / L] ...
(32) Since this equation (32) originally holds for θ << 1, θ
In << 1, it may be considered that tan θ and θ are almost equal, and (3
Equation (2) can be expressed as the following equation.

θ = {S2 (H ₀ , θ) −S2 (H ₀ , 0)} / L (33) Further, the distance L is determined according to the following two criteria.

・ Minimize the range of S2 (H ₀ , 0) depending on the incident height.

• Make sure that S2 (H ₀ , θ) is within the PSD2 light receiving range within the required measurement range.

The incident height and the incident angle of the laser beam 3 can be obtained from the above equations (29) and (33). Then, by taking these cos components and sin components, the positions X and Y and the posture angles θx and θy can be calculated.

FIG. 8 shows a procedure for obtaining the positions X and Y and the posture angles θx and θy by the calculation method described above.

As shown in the figure, first, the correction amount ΔS1 is set to zero (step 201), and the process proceeds to the subroutine of the figure (b) (step 202) to calculate the incident height H ₀ (formula (29-1)). Step 205), aberration data selection (step 206), aberration is canceled and incident angle is calculated (equation (33); step 20).
7), the positions X and Y and the posture angles θx and θy are calculated (step 208).

Next, from θ calculated with the correction amount ΔS1 = 0, the correction amount Δ
Calculate S1 (step 203) and repeat steps 205 to 20 above
The process of 8 is performed (step 204).

The CPU 45 of the arithmetic processing unit 9 configured as shown in FIG. 5 performs the above processing.

By the way, in the position / orientation angle measuring apparatus of the embodiment, as shown in FIG. 14, the operator should perform visual position measurement between the laser oscillator 6 and the sensor portion composed of PSDs 1 and 2 and the lens 7. A scale plate 4 is provided.
On the other hand, in order to illuminate the scale plate 4, the focus FP of the lens 7
The illumination lamp 5 is arranged in the vicinity.

Since the light projected from the illumination lamp 5 is near the lens focus FP, it is refracted by the lens 7 and condensed into parallel rays.
Therefore, the illuminating light has a stronger directivity than the conventional one in which the lens 7 is not provided, so that the light is less scattered and the scale plate 4 can be visually recognized from a distance farther than the conventional one. In addition, the entire light receiving range of PSDs 1 and 2 glows brightly,
The visibility of the scale plate 4 is greatly improved.

If the light reflector 5a is provided behind the illumination lamp 5 as shown in FIG. 3B, the amount of light output from the lens 7 increases, and the visibility of the scale plate 4 is further improved. be able to.

As described above, according to the embodiment, the lens 7 converges the laser light 3 and the PSD is arranged on the side where the laser light 3 converges (in the case of FIG. PS
D2, PSD2 in the case of FIG. 3B), the PSD provided on the convergence side can be reduced in size, and the cost can be significantly reduced. In addition, PSD
The smaller the light receiving area, the higher the irradiation position detection accuracy. Therefore, it is possible to improve the measurement accuracy of the position and the posture angle while reducing the apparatus cost. Further, if the light receiving area is the same, it is possible to measure the position and the posture angle in a wider range than the conventional one. Furthermore, (5-a), (6-a), (5-
b) and (6-b) and the expressions (7) and (8), as is clear from the comparison between the expressions, the PS is less than 1 when the attitude angle is sufficiently smaller than 1.
The D2 can be made smaller than the PSD1, resulting in higher accuracy and lower cost.

Also, as is apparent from the above equations (5-a) to (8),
Position X, Y of the underground excavator based on the detected irradiation position of PSD1
Is the attitude angle θ of the underground excavator based on the detected irradiation position of PSD2.
x and θy can be obtained separately (however, strictly speaking, if there is aberration, based on the output of PSD1,
Although the output of PSD2 needs to be slightly corrected), the range of posture angles that can be measured according to the irradiation position of PSD1 does not change as in the past. For this reason, the angle measurement range required in the entire position measurement range can be secured.

Further, the accuracy of the attitude angle measurement is determined by the distance F (see FIG. 1) from the lens principal point RP to the light receiving surface 2a of PSD2, as is clear from the above equations (7) and (8). Here, in the conventional case, the accuracy of the attitude angle is to be determined by the PSD interval L (see FIG. 16) as is clear from the above equations (3) and (4), but according to the embodiment, , About 1/3 of PSD interval L
It is possible to achieve the same attitude angle measurement accuracy with the distance F having a length of about 1/2. Therefore, the length of the sensor unit in the longitudinal direction can be shortened, and the size of the device can be reduced and the measurement accuracy can be improved.

In addition, since the position and attitude angle data are digitally processed, these data are multiplexed and serially transmitted, they are resistant to noise and transmitted over a single transmission cable without being affected by the performance of the transmission path. can do. For this reason, accurate data can be transmitted, and the reliability of the device can be improved, and at the same time, the cost of the device and the space can be saved by reducing the number of transmission cables.

Further, in the embodiment, the illumination lamp 5 is provided in the vicinity of the lens focal point to illuminate the scale plate 4, so that the scale plate 4 can be sufficiently bright without scattering light and without providing a large number of lamps. It is illuminated with this. Moreover, the lamp does not positionally erode the measurement range of the position and attitude angle of the underground excavator.

In the embodiment, the a-SiPSD is used as a sensor for detecting the two-dimensional irradiation position of the laser beam 3. However, the present invention is not limited to this. Any sensor can be used as long as it can perform the above.

Further, in the embodiment, laser light is used as light to be irradiated to the PSD. However, the present invention is not limited to this, and any light having linearity can be used.

〔The invention's effect〕

As described above, according to the present invention, it is possible to increase the measurement range of the position and the posture angle and to improve the measurement accuracy of the position and the posture angle. At the same time, the size of the device can be reduced.

In addition, the measurement data can be transmitted with high reliability and efficiency, low cost, and small space.

Further, it becomes possible to visually recognize the display on the scale plate with low-cost equipment and without narrowing the measurement range of the position and the posture angle.

BRIEF DESCRIPTION OF THE DRAWINGS FIGS. 1 (a) and 1 (b) are views conceptually showing the configuration of a sensor unit in an embodiment of a position / orientation angle measuring apparatus according to the present invention, and FIG. FIG. 3 is a perspective view showing the structure of a-SiPSD used as the PSD shown in FIG. 1, and FIG.
The drawings used for explaining the position detection principle of the a-SiPSD shown in the drawings, FIGS. 4 and 5 are conceptual views of the configuration of the arithmetic processing unit in the embodiment of the position and orientation angle measuring apparatus according to the present invention. 6 is a block diagram schematically showing the geometrical relationship of the sensor portion when the PSD 2 shown in FIG. 1 (b) is displaced in the longitudinal direction of the apparatus, and FIG. 7 is shown in FIG. FIG. 8 is a flowchart illustrating the processing procedure performed by the CPU, and FIG. 8 is a calculation of the position and attitude angle of the underground excavator assumed in the embodiment based on the detection outputs of PSDs 1 and 2 shown in FIG. FIG. 9 is a flow chart showing the procedure, FIG. 9 is a view showing the geometrical relationship of each constituent element of the sensor section shown in FIG. 1 (a), and FIG. 10 is a partially enlarged view of PSD1 shown in FIG.
FIG. 11 and FIG. 13 are diagrams showing the geometrical relationship of each constituent element of the sensor section shown in FIG. 1 (a) used for explaining the calculation procedure shown in FIG. 8, and FIG. FIG. 14 is a graph used to explain the calculation procedure shown in FIG. 8 and is a graph showing the relationship between the incident height of the lens and the ratio of PSD1 to the incident height of the lens shown in FIG. (a) and (b) are diagrams exemplifying a configuration in which a scale plate and an illumination lamp are added to the sensor unit of the embodiment, and FIG. 15 shows a protocol of a signal transmitted from the data communication device shown in FIG. Illustrated figure, No.
FIG. 16 is a perspective view conceptually showing the structure of a sensor unit of a conventional position / orientation angle measuring device, and FIG. 17 is a view showing a conventional illumination lamp arrangement, FIGS. 18 (a), (b). ) Are each
FIG. 17 is a diagram used for explaining a posture angle measurement range based on the sensor unit shown in FIG. 16. 1, 2,..., PSD, 3,..., Laser light, 4,.
... lighting lamp, 6 ... laser transmitter, 7 ... lens,
8, 9 ... Arithmetic processing unit, 48 ... Data communication device.

Claims (1)

[Claims] 1. A light projector for projecting light so that an optical axis thereof coincides with a planned movement path of a moving body, and the light projected from the light projector is received and transmitted to two-dimensionally irradiate a position. And a second position detector that detects the irradiation position two-dimensionally by receiving the light transmitted by the first position detector and a second position detector that detects the irradiation position two-dimensionally. A position display plate for displaying a two-dimensional irradiation position of the first position detector is provided between the light projector and the first position detector, the position display plates being arranged apart from each other by a distance. A position / orientation angle measuring device configured to calculate a position and an attitude angle of a moving body based on a two-dimensional irradiation position of the light detected by a second position detector, wherein the light projector and the first position detector are provided. Or between the first position detector and the second position detector The position of the moving body is calculated based on the arranged lens and the two-dimensional irradiation position of the light detected by the first position detector, and the light detected by the second position detector is calculated. Between the position detecting unit configured by the first and second position detectors and the lens and the light projector. A position / orientation angle measuring device, characterized in that the position display plate is provided in the lens, and an illumination lamp for illuminating the position display plate is provided near the focal point of the lens.