JP3154355B2 - Moving object relative distance measuring device and position measuring device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for measuring a relative distance or a position of a moving object with respect to a sign and guiding the moving object along a predetermined traveling path based on a result of the measurement.

[0002]

2. Description of the Related Art In a so-called dead reckoning navigation in which a mobile body is guided and guided on the basis of a result of estimation of a position and a direction of the mobile body, as is well known, an estimated position and an estimated moving direction are intermittently removed in order to remove accumulated errors. It is necessary to correct.
In view of this, the present applicant has disclosed in Japanese Patent Application No. 3-148921 a simple optical system in which two sets of light emitting and receiving devices are provided on a moving body and light reflecting means is provided intermittently on one side of a planned traveling path. A patent application has already been filed for a device that can calculate the corrected position and the corrected moving direction of the moving object by the arrangement of the moving object. In the process of this calculation, it is assumed that the trajectory of the moving body from receiving the light beam on one of the two light receivers of the moving body to receiving the light beam on the other light receiver is a straight line. Below, the relative distance of the moving body to the light reflecting means is calculated. Therefore, the correction position obtained by using the relative distance calculated in this way is also calculated on the assumption that the trajectory is a straight line.

[0003]

When the moving body is traveling on a straight line or on a gentle curve, the above assumption is satisfied and the above calculation does not cause an error, but the moving body turns. Is satisfied, the above assumption does not hold, and the calculation error becomes conspicuous. And
This calculation error increases as the curvature of the turning locus increases.

[0004] Therefore, when the moving body is turning, it is apparent that the guided running of the moving body based on the above calculation is not performed accurately.

The present invention has been made in view of such circumstances, and even when the moving body is turning, the relative distance of the moving body to the light reflecting means can be calculated with high accuracy. A first object is to provide a device capable of accurately performing guided traveling based on a distance, and even when a moving body is turning,
It is a second object of the present invention to provide a device that can accurately calculate the position of a moving body and that can accurately perform guided traveling based on the calculated position.

[0006]

SUMMARY OF THE INVENTION Therefore, the first aspect of the present invention has been described.
According to the present invention, the moving distance of the moving body is detected in the moving body relative distance measuring device configured to guide the moving body along the scheduled traveling path based on the measurement result of the relative distance of the moving body to the sign. Moving distance detecting means, turning angle detecting means for detecting a turning angle of the moving body, and first and second light beams provided on the moving body at different projection angles from the moving body to the side. A light projecting means for projecting light, a light reflecting means as the marker disposed on one side of a planned movement path of the moving body, and reflecting the first and second light beams, provided on the moving body, A light receiving unit for receiving the first and second light beams reflected by the light reflecting unit; and one of the first and second light beams based on an output of the light receiving unit and an output of the moving distance detecting unit. After receiving the other The moving distance of the moving body until the beam is received is determined, and one of the first and second light beams is received based on the output of the light receiving unit and the output of the turning angle detecting unit, and then the other is received. The turning angles of the moving body until the light beam is received are obtained, and the obtained moving distance and turning angle, the arrangement position of the light reflecting means, and the projection angles of the first and second light beams are obtained. And a relative distance calculating means for calculating a relative distance of the moving body to the light reflecting means when the first or second light beam is received by the light receiving means.

Further, according to a second aspect of the present invention, in the position measuring apparatus for a mobile unit, the mobile unit is configured to guide the mobile unit along a predetermined travel path based on the measurement result of the position of the mobile unit. A moving distance detecting means for detecting a moving distance of the moving object, a moving direction detecting means for detecting a moving direction of the moving body, and a first and a second light emitting angles provided at different moving angles from the moving body. A light projecting means for projecting a light beam to the side, a light reflecting means serving as the marker disposed on one side of a predetermined moving path of the moving body and reflecting the first and second light beams, A light receiving unit provided on a moving body for receiving the first and second light beams reflected by the light reflecting unit; and the first and second light beams based on an output of the light receiving unit and an output of the moving distance detecting unit. Receiving one of the two light beams And calculating a moving distance of the moving body until receiving the other light beam, and receiving one of the first and second light beams based on an output of the light receiving means and an output of the moving direction detecting means. And the turning angles of the moving body from when the other light beam is received until the other light beam is received, the obtained moving distance and turning angle, the arrangement position of the light reflecting means, and the projection of the first and second light beams. Relative position calculation for calculating a relative position of a moving body with respect to the light reflecting means at the time when the first or second light beam is received by the light receiving means based on a light angle and a moving direction detected by the moving direction detecting means. Means.

[0008]

According to the first aspect of the present invention, from receiving one of the first and second light beams to receiving the other light beam based on the output of the light receiving means and the output of the moving distance detecting means. The moving distance of the moving object is calculated. On the other hand, based on the output of the light receiving means and the output of the turning angle detecting means, the turning angle of the moving body from receiving one of the first and second light beams to receiving the other light beam is obtained. Then, the first or second light beam is received by the light receiving means based on the determined moving distance and turning angle, the arrangement position of the light reflecting means, and the projection angles of the first and second light beams. The relative distance of the moving body to the light reflecting means at the time is calculated. Based on the calculated relative distance of the moving object to the sign, the moving object is guided along the planned traveling path.

According to the configuration of the second invention, one of the first and second light beams is received based on the output of the light receiving means and the output of the moving distance detecting means, and then the other light beam is received. The moving distance of the moving object until the moving is calculated. On the other hand, based on the output of the light receiving means and the output of the moving direction detecting means, the turning angle of the moving body from receiving one of the first and second light beams to receiving the other light beam is obtained. Then, based on the determined moving distance and turning angle, the arrangement position of the light reflecting means, the projection angles of the first and second light beams, and the detected moving direction of the moving direction detecting means, the first or the second is determined. The relative position of the moving body with respect to the light reflecting means when the light beam is received by the light receiving means is calculated. Based on the calculated position of the moving body, the moving body is guided along the planned traveling path.

[0010]

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a block diagram showing an embodiment of a moving object relative distance measuring apparatus and a position measuring apparatus according to the present invention. The position measurement of the moving body itself is based on the assumption disclosed in Japanese Patent Application No. 3-148921.

That is, FIG. 1 is a block diagram showing the configuration of the apparatus of the embodiment, and it is assumed that the apparatus is mounted on the moving body 2 shown in FIG. The moving body 2 is, for example, an unmanned carrier.

The rate gyro 4 shown in FIG. 1 is provided for detecting a moving direction of the moving body 2 at any time, and outputs a moving direction change amount Δθ per unit time to the CPU 7. On the other hand, a wheel (not shown) of the moving body 2 is provided with a pulse encoder 5 for detecting the rotation speed of the wheel. The encoder 5 outputs a number of pulses corresponding to the rotation amount of the wheel, and adds the pulses to the counter 6. The counter 6 outputs the moving distance (running length) Δl of the moving body 2 per unit time by counting the number of pulses, and adds it to the CPU 7.

The light beam projecting / receiving devices 8 and 9 are respectively shown in FIG.
As shown in FIG. 1, the moving body 2 is disposed at a left position (vehicle left side surface) 3 with respect to the traveling direction F. The light beam projecting / receiving device 8 emits the laser beam L1 to the left in the direction perpendicular to the longitudinal direction of the vehicle body, and intermittently extends along the traveling path 1 on the left side of the planned traveling path 1 of the moving body 2. , Cj,..., Cn-1,.
The reflected light L1 reflected by the light receiving section is received, and a signal indicating the received light is applied to the gate circuit 10. Similarly, the light beam projecting / receiving device 9 projects the laser beam L2 in a direction inclined by an angle α toward the front of the vehicle body with respect to the projecting direction of the laser beam L1, and is reflected by the reflecting mirrors C1. The reflected light L2 is received, and a signal indicating the received light is applied to the gate circuit 10. The reflecting mirrors C1 have a function of reflecting the received light beam in the same direction as the incident direction. Here, the light receiving signal input to the gate circuit 10 is the moving object 2
Is used to calculate the position and direction of

The gate circuit 10 comprises a flip-flop or the like, and outputs a signal for instructing the counter 11 to start counting to the counter 11 when the light receiving signal is applied from the light beam projecting / receiving device 9 to the counter 11. When a light receiving signal is added from the emitter / receiver 8, the counter 1
A signal instructing the end of counting by 1 is output to the counter 11. A pulse signal output from the encoder 5 is added to the counter 11. Therefore, the counter 11 outputs a signal indicating the distance traveled by the moving body 2 during the period between the time when the reflected light L2 is received and the time when the reflected light L1 is received.

The CPU 7 calculates the position and direction of the moving body 2 based on so-called dead reckoning based on the outputs Δθ and Δl of the rate gyro 4 and the counter 6. The calculation itself is described in detail in the above-mentioned prior application, and is not directly related to the gist of the present invention, so that the detailed description is omitted.

The CPU 7 also performs a calculation process for intermittently correcting a calculation position and a calculation direction based on the dead reckoning navigation. That is, FIG. 2 is a diagram for explaining such a correction operation, and the arrangement position of the reflecting mirrors C1 shown in FIG. 2 is measured in advance by surveying or the like, or is known by disposing it at a known position. It is assumed that

The moving body 2 travels as shown by a locus 1, and performs the following calculation each time it passes by the side of the reflecting mirrors C1. It should be noted that 1 'shown by a dashed line in the drawing.
Is a trajectory in which the calculated values based on the dead reckoning are plotted, and 1 indicated by a solid line indicates an actual trajectory of the moving body 2.

That is, the moving body 2 is located at the position (Xc / j, Yc / j).
j)), the light L2 projected from the light beam projecting / receiving device 9 is reflected when the arrangement position 3 of the projecting / receiving devices 8 and 9 reaches the position Rj. The reflected light L2 is reflected by the mirror Cj, and the reflected light L2 is received by the light emitting / receiving device 9. Next, when the arrangement position 3 reaches the position Qj, the light beam is projected.
The light L1 similarly projected from the light receiving device 8 is similarly reflected, and the reflected light L1 is received by the projecting / light receiving device 8. As a result, the counter 11 determines the moving distance aj between the two points Rj and Qj.
Is measured.

Therefore, assuming that the moving direction of the moving body 2 at the position Qj is Θ from the geometrical relationship, the moving distance is calculated from the following equation based on the distance data aj and the position of the reflecting mirror Cj (Xc / j, Yc / j). The corrected position Qj (Xj, Yj) of the body 2 is obtained.

Xj = Xc / j + aj · cotα · sinΘj (1) Yj = Yc / j−aj · cotα · cosΘj (2) Even if the correct value Θj of the moving direction is unknown,
The corresponding position Q'j obtained by dead reckoning operation
Is small, and the distance between the reflecting mirror Cj and the position Qj is not large.
Expression (2) can be approximated as shown in the following (3) and (4) by using the estimated operation value θj instead of Θj.

Xj = Xc / j + aj · cotα · sinθj (3) Yj = Yc / j−aj · cotα · cosθj (4) Further, the correction position Qj of the moving body 2 is calculated by dead reckoning as described above. Then, based on this and the correction position Qj-1 at the time of passing the vicinity of the previous reflecting mirror Cj-1, the estimated calculation value θ of the traveling direction is obtained.
The error Δθj of j is obtained as in the following equation (5).

Δθ j = angle Q j Q j−1 Q ′ j (5) Therefore, the correction value Θ j in the moving direction is obtained by the following equation (6).

Θj = θj + Δθj (6) The scale factor Kl of the encoder 5 is modified from K'l used so far by the following equation.

Kl = K'l * QjQj-1 / (Q'jQj-
1)... (7) As described above, although the measurement error of the moving direction θj of the moving body 2 has a large effect on the calculation by dead reckoning, (1),
Since the influence on the correction calculation shown in equation (2) is small,
As shown in equations (3) and (4), the position can be measured with high accuracy even by using the estimated operation value θj including an error,
Further, based on this position, the moving direction can be measured with high accuracy from equation (6). Here, the correction operation is
Since it is not necessary to directly detect the correct movement direction Θj from the output of the light beam projecting / receiving device as in the prior art, the number of light beam projecting / receiving devices and the number of reflecting mirrors can be reduced to two. For this reason, the number of times of light reception is reduced, and the reliability of the device is improved. Also, there is no need to provide a reflector on both sides of the traveling path as in the related art, so that restrictions on the installation location can be reduced.

The CPU 7 performs an estimation operation based on dead reckoning navigation, performs correction operations (1) to (17), and issues a steering angle command to the steering drive unit (not shown) to cause the vehicle to travel along the planned traveling path. Output to drive and control the steering of the moving body 2.

Here, the equations (3) and (4) for calculating the current position Qj of the moving body 2 are based on the assumption that the moving distance aj is a straight line, that is, whether the moving body 2 is running on a straight line. This equation is established on the assumption that the vehicle is traveling on a gentle curve, and under such an assumption, the relative distance a to the reflecting mirror Cj
j · cotα is calculated, and coordinate positions Xj and Yj are calculated. Certainly, if the moving body 2 is actually traveling straight,
Although no calculation error occurs and there is no problem, if the moving body 2 is turning, it is no longer assumed that the vehicle 2 is traveling straight, and the calculated relative distance aj · cotα and the calculated coordinate positions Xj, Yj are not satisfied. Contains errors that cannot be ignored. For this reason, the guided traveling along the planned traveling path cannot be performed with high accuracy.

Therefore, in the embodiment, the relative distance and the position are calculated accurately on the assumption that the moving body 2 turns.

FIG. 3 shows that the moving body 2 moves between two points Rj and Qj.
It is a figure which shows the geometrical relationship at the time of turning along the circular arc 1 of the turning radius r (positive at the time of left turn, negative at the time of right turn) centering on a point. In the above equations (3) and (4), the position 3 at which the emitter / receiver 8 and 9 are disposed is changed to the position X of the moving body 2.
j, Yj are calculated, but the following description is based on the assumption that the positions X0, Y0 of the moving body 2 are the center positions of the left rear wheel and the right rear wheel of the moving body 2. This is because the center position of the moving body 2 is set as a measurement point for position measurement.

In this case, the light emitting / receiving devices 8 and 9 are arranged at a distance a from the center of the left and right rear wheels in front of the moving body 2 and further a distance b to the left of the moving body 2. It is assumed that In addition, the arrangement position 3 of the projector / light receiver 3 when the moving body 2 is located at the point A is E point, and the arrangement position 3 when the moving body 2 is located at the B point is H. Point. The distance between the center point O and the positions where the emitter / receivers 8 and 9 are disposed is s.

Now, it is assumed that the moving body 2 has turned on the arc 1 from point A to point B. The points A and B are shown in FIG.
At points Rj and Qj.

Then, at the point A, the moving direction θA is obtained based on the output of the rate gyro 4. On the other hand, at the point B, the moving direction θB is obtained based on the output of the rate gyro 4. Therefore, the turning angle φ of the moving body 2 between points A and B (positive when turning left and negative when turning right) is calculated as follows.

Φ = θB−θA (8) At point A, the reflected light L2 from the reflecting mirror Cj disposed at point M is received by the projector / receiver 9, and at point B from the reflecting mirror Cj. Is reflected by the light emitting / receiving device 8, the length of the arcs A to B, that is, the moving distance m of the moving body 2 between the points A and B is output from the counter 11. The moving distance m corresponds to the moving distance aj in FIG.

Therefore, if the angle formed by the line segment connecting the points O and E with respect to the line segment connecting the points O and A is represented by 直, a right-angled triangle ODE (where D is the moving object 2 Is a point on the line segment OA on the left side of
(10) is established.

S ・ cosψ = rb (9) s ・ sinψ = a (10) Further, the following equation (11) holds for the sector OAB.

R = m / φ (11) Next, regarding the triangle OEG, OG / sin (α + ψ) = s / sin ｛π- (α + φ +
ψ)｝… (12) holds. Further, assuming that the distance between M and H, that is, the relative distance between the reflecting mirror Cj and the light emitting / receiving devices 8 and 9 at the second light receiving point B is 1, with respect to the triangle MGH, 1 / sin {π- (α + φ + ψ) ｝ = (S−OG) / s
in (α + φ) (13) holds. From the above equations (9) to (13), the relative distance l
1 = {1 / sin (α + φ)} [(r−b)} sin
(Α + φ) -sinα｝ + a ｛cos (α + φ) -co
sα｝] (14) Here, it is possible to assume that the turning angle φ is very small, and at this time, it is possible to approximate cos φ = 1 and sin φ = 0, and therefore, the above equation (14) gives: l = {1 / (sin α + φ · sin α)} ·｝ m ・ co
sα−φ (a · sinα + b · cosα)｝ (1
5) When the moving body 2 travels straight between the two points A and B, l = m / tanα (15) ′ with the turning angle φ = 0 in the above equation (15). On the other hand, the relative distance l0 between the arrangement point M of the reflecting mirror Cj and the second light receiving point B is as follows: l0 = {(l + b) 2 + a2} (16) Therefore, the measured turning angle φ ((8)
The relative distance 1 of the moving body 2 is obtained by substituting the equation (15) and the measured moving distance m into the equation (15), and by substituting the obtained relative distance 1 into the equation (16), Position M of the reflector Cj at the second light receiving point B in FIG.
Can be obtained. Also,
The coordinate position (X0, Y0) at the point B is represented by l instead of aj · cotα in the above equations (3) and (4).
By using 0, X0 = Xc / j + 10 sin (.theta.B-.infin.) (17) Y0 = Yc / j-10.cos (.theta.B-.infin.) (18) Note that Xc / j and Yc / j are reflecting mirrors Cj.
Is the coordinate position.

It should be noted that the position of the moving body 2 is
In this case, the relative distance 1 of the moving body 2 to the reflecting mirror Cj is obtained by the above (15), and the following (19), (2)
The coordinate positions X and Y of the moving body 2 can be obtained by the equation (0).

X = Xc / j + 1.multidot.sin.theta.B (19) Y = Yc / j-1.multidot.cos.theta.B (20) The CPU 7 calculates the corrected positions X0 and Y0 thus calculated.
Based on (or X, Y), a steering angle command for causing the steering drive to travel along the planned travel path is output to drive and control the steering of the moving body 2. In this case, since the relative distance is calculated accurately and the position of the moving body 2 is accurately calculated on the assumption that the moving body 2 is turning, guided driving along the scheduled traveling path is performed accurately. Will be.

In the embodiment, the relative distance and the position of the moving body 2 are calculated as the relative distance and the position at the second light receiving point B. However, the relative distance and the position at the first light receiving point A are calculated. The calculation may be performed.

The direction in which the two light beams projected from the moving body 2 are projected is not limited to the direction of the embodiment, and the light beams projected in the vertical direction as shown in FIG. The light beam L1 may be projected in a direction inclined at an angle β to the rear of the vehicle body with respect to L2 (FIG. 10A), or the light beam L1 may be emitted at an angle γ in a symmetrical direction (FIG. 10A). (B)), the light may be projected at an angle δ in front of the vehicle body (FIG. (C)), or the light may be projected at an angle ε rearward (FIG. (D)).

In this embodiment, the case where the present invention is applied to the dead reckoning navigation correction calculation has been described. However, the present invention is not limited to this. The present invention can be applied to any device that guides the body. For example, the present invention can be applied to a case where a relative distance or a position of a moving object is calculated and displayed on a display screen, and an operator manually controls the traveling of the moving object based on the display result.

In the embodiment, the moving object is the light receiving point A,
The case where the vehicle is turning along the arc between the points B has been described. However, when the radius of curvature of the traveling locus of the moving body is not small and the distance between the sign and the moving body is not large, the traveling locus is strictly determined. Even if the curve is not a circular arc, it can be sufficiently approximated even if it is assumed to be a circular arc as shown in the embodiment.

[0042]

As described above, according to the present invention, even when the moving body is turning, the relative distance of the moving body to the sign or the position of the moving body can be calculated accurately. Guidance traveling control based on these calculation results is performed with high accuracy.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration of an embodiment of a moving object relative distance measuring device and a position measuring device according to the present invention.

FIG. 2 is a diagram illustrating a state in which a moving object travels, and is a diagram used for explaining a calculation of a position and a direction.

FIG. 3 is a diagram geometrically showing a state in which the moving body is making a turning travel.

FIGS. 4 (a), (b), (c), and (d) are diagrams illustrating examples of light projecting directions of a light beam projecting / receiving device.

[Explanation of symbols]

 2 Moving body 4 Rate gyro 5 Encoder 6 Counter 7 CPU 8 Light beam projector / receiver 9 Light beam projector / receiver 11 Counter

Claims (2)

(57) [Claims] 1. A relative distance measuring device for a mobile unit, which is configured to guide the mobile unit along a scheduled travel path based on a measurement result of the relative distance of the mobile unit to a sign, wherein the moving distance of the mobile unit is Moving distance detecting means for detecting, turning angle detecting means for detecting a turning angle of the moving body, and first and second light beams provided at the moving body at different projection angles from the moving body. Light-projecting means for projecting light toward the vehicle; light-reflecting means serving as the marker disposed on one side of the predetermined moving path of the moving body and reflecting the first and second light beams; Light receiving means for receiving the first and second light beams reflected by the light reflecting means; and the first and second light beams based on an output of the light receiving means and an output of the moving distance detecting means. After receiving one of the While obtaining the moving distance of the moving body until receiving the other light beam, receiving one of the first and second light beams based on the output of the light receiving means and the output of the turning angle detecting means And the turning angles of the moving body until the other light beam is received, the obtained moving distance and the turning angle, the arrangement position of the light reflecting means, and the projection of the first and second light beams. Relative distance calculating means for calculating a relative distance of the moving body with respect to the light reflecting means when the first or second light beam is received by the light receiving means based on the angle. apparatus. 2. A moving object position measuring apparatus which guides a moving object along a predetermined traveling path based on a measurement result of the position of the moving object, the moving distance detecting the moving distance of the moving object. Detecting means; moving direction detecting means for detecting the moving direction of the moving body; and first and second light beams which are provided on the moving body and which are laterally projected from the moving body at different light projecting angles. Light-emitting means disposed on one side of a predetermined movement path of the moving object, and reflecting the first and second light beams as the marker; A light receiving unit for receiving the first and second light beams reflected by the reflecting unit; and receiving one of the first and second light beams based on an output of the light receiving unit and an output of the moving distance detecting unit. And then receive the other light beam A moving distance of the moving body until the first light beam is received, and one of the first and second light beams is received based on the output of the light receiving means and the output of the moving direction detecting means. The turning angles of the moving body until the light is received are obtained, and the obtained moving distance and turning angle, the arrangement position of the light reflecting means, the projection angles of the first and second light beams, and the detection of the moving direction are obtained. A relative position calculating means for calculating a relative position of the moving body with respect to the light reflecting means when the first or second light beam is received by the light receiving means based on a detection moving direction of the means; Position measuring device.