JPS6020161A - Running locus detecting device of moving object - Google Patents

Abstract

(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

[Detailed Description of the Invention] [Field of Application of the Invention] The present fJA relates to a device for detecting the current position and current direction of movement of a moving object, and in particular, to a device for detecting by applying the Doppler effect of waves. It is.

[Background of the invention]

For a self-propelled device (hereinafter referred to as a moving object) that moves freely on a plane according to its own will while detecting obstacles, it is extremely important to know its current position and direction of travel.

Conventionally, the methods for determining these factors include determining the rotation speed of the wheels of a moving object, and using wireless transceivers installed at both the reference point and the moving object to transmit signals (e.g., radio waves, light, ultrasonic waves, etc.). There was a way to decide by looking at the options.

However, in the former case, a large error occurred if the wheels were slipping or spinning. Moreover, the latter is not an expensive option, and has the drawback that if there is an obstacle between the reference point and the moving object, the signals cannot be clearly seen, resulting in a blind state.

[Purpose of the invention]

SUMMARY OF THE INVENTION An object of the present invention is to eliminate the above-mentioned drawbacks of the prior art and to provide a highly accurate, relatively low-cost, and highly reliable apparatus for detecting the running trajectory of a moving object.

[Summary of the invention]

In order to achieve the above object, the present invention installs two Doppler radars on a moving object, detects the Doppler frequency obtained from the relative velocity between the floor surface and the moving object, and grasps the current position and direction of movement. In addition.

Preferably, the pair of Doppler radars are arranged on a line perpendicular to the tangential direction of the traveling trajectory of the moving object.

[Embodiments of the invention]

The present invention will be described in detail below using examples.

FIG. 1 shows an embodiment of the present invention.

Assume that a single moving object moves on the floor 5 in the direction of the arrow in the figure at a speed V. Also, S animal object 1 is 12! Since I started traveling on an IT-wheeled vehicle, the two front wheels are drive wheels 2 and 2'(2' is not visible in the diagram), and the two rear wheels are drive wheels 3 and 3'. A Doppler radar 4.4' is installed on the extension of the drive 85°3' car @B and outside the drive wheels 6, 3'. However, the Doppler radars 4 and 4' are fixed regardless of the rotation of the drive wheels,
It emits waves (radio waves or ultrasonic waves) toward the floor at an angle θ with respect to the floor 5, and simultaneously receives reflections from the floor. With such a structure, the current position and traveling direction of the moving object 1 can be known as described below.

Before explaining the traveling trajectory detection method of the present invention with reference to FIG.
I will explain. Doppler radar is originally used as a speedometer, but the method described here can easily measure travel distance.

As shown in Fig. 2, the Doppler radar 4 moves at a speed of V in the direction of the arrow (direction parallel to the floor 5) with an inclination of θ with respect to the floor 5.
``'C While moving, it emits radiation wave 7.

It is a well-known fact that when a reflected wave 8 from the floor 5 is received at the same time, the Doppler frequency (the frequency of the difference between the emitted wave and the reflected wave) Δf is expressed by the following equation.

2vcosθ if=-fo, (1) However, f - the frequency of the radiation wave C = wave propagation speed (1) Rewriting equation 2foCnSθ 9 −°“=λ8(2) However, λ - c (3) 2focosθ becomes. Equation (2) means the propagation velocity V.

This is an equation representing a wave with a frequency Δf and a wavelength λ8. This wave is a special type of wave whose wavelength is always constant regardless of frequency or propagation speed. usually.

For waves, the propagation speed is constant (but the propagation medium is constant) and +
Ij,'' When the wave number changes, the wavelength changes accordingly.

From this, by measuring the wave numbers of the two frequencies If, the moving distance of the Doppler radar 4 shown in FIG. 2 can be measured.

In other words, it becomes possible to measure the distance traveled.

Specifically, by adopting the configuration as shown in FIG. 3, for example, the traveling distance Rfe i'I- can be easily measured. 4#'i The Doppler radar mentioned earlier outputs a Si Doppler signal using the radiated wave 7 and the reflected wave 8. Reference numeral 9 denotes a pulse generating circuit, which generates a pulse at the FI period of the zero cross of the Doppler signal. Reference numeral 10 is an integrating circuit for accumulating the pulses, and reference numeral 11 is a reset circuit for resetting the integrating circuit at the start of measurement. Dimensions.

12 indicates an output terminal of the integrating circuit. next.

The operation of this circuit configuration will be explained with reference to FIG.

If the moving speed V changes with respect to time t as shown in the figure, the frequency of the Doppler signal changes accordingly. Therefore, the pulse output of the pulse generating circuit 9 also becomes pulses of different brightness and ugliness as shown in the figure. When the reset circuit 11 is operated with 1=0, the output of the integrator circuit 10 increases sequentially and becomes V at t=T, and this value corresponds to the distance m over which the integrator moves in 7 seconds.For example, dotsupura radar 4
Assume that f, = 40 KHz ultrasound is used.

Transmission speed of ultrasonic waves in the air Vi544m/se
c, so if we set θ=64.53°, we get (3)
From the formula, λ=10 mm. Then, the pulse generation circuit 9
Since one pulse is emitted at i half wavelength, distance can be measured with a resolution of 5 mm/1 pulse.

Similarly, if the drag radar 4 uses 20 GHz radio waves, the radio wave propagation speed is 5 x 1
08m/sec, so if you set θ = 41.41°, it will be the same (travel distance measurement can be done with a resolution of 5mm/1 pulse. In this case, drive wheels 3 and 6'). Even if idle rotation occurs, no measurement error will occur.

First, the current position and direction of movement of the moving object 1 can be known by incorporating the circuit configuration for measuring the traveling distance as described above into the drag radars 4 and 4' shown in FIG. What can be done will be explained with reference to FIG.

In FIG. 5, A and B are the starting points of drag radars 4 and 4', respectively. 2r is the distance between A and B. C indicates the midpoint of the axle B, and 6. It is located at the origin of the x-y coordinates. When the moving object 1 moves, if the movement trajectory of the 0 point can be grasped, the trajectory of the moving object 1 has been grasped.

Now, assuming that point 0 has followed the path C-C'-C", points A and B are A-A'-A" and B-B'-B, respectively.
If we operate the reset circuit described in Figure 3 with 1=0 and start moving at the same time and reach point C' at t=T, then drag radar 4 at that time (A side ) indicates the distance m, and the output of drag radar 4' (B side) indicates the distance λ.These two data fl,, f1
2 can be calculated immediately. However, +R1 is the distance from the 0 point of the intersection G of the straight line passing through points A and B and the straight line passing through points A' and B', and Gl and C
′ can be considered to be equal to the distance between Also,
φ1 is the angle formed by both straight lines.

Therefore, the arc length λ between C and C'. is also c-c
Since the straight line distance between ' is l-1:, the position of C' (
XI + y, ) is expressed by the following formula.

However, β is the angle that the beam makes with the X axis. Also, the direction of travel at this point is relative to the Y axis.
Φ,=φ, (9).

At the same time as the above-mentioned measurement is performed at t=T, the reset circuit 11 is operated again to reset the integration circuit. Assume that the moving object 1 continues to travel and reaches point C'' at time t=2T.

At this time, the outputs of the drag radars 4 and 4' are each at a distance of m-
J #I12' is shown. From these two data (
4) can be determined by calculation in the same way as Eq. However, R2 is A'
, intersection point G of the straight line passing through point B' and the straight line passing through point A", B"
The distance from point C' of G2 is assumed to be equal to the distance between G2 and C'.

Therefore, the length β.' of the arc between c' and c'' is the same as in equation (5).

Also, since the direct i distance L2 between C' and C" is the same as equation (6), the position of C" (X2 + y2) is Y2=
It is expressed as Yl +L2 sinβ2. However, β2 is the angle that L2 makes with the X axis.

It is. Furthermore, the direction of travel at this point is relative to Y@ Φ2=Φ, 10φ2=φ, 10φ20Q becomes.

By repeating the above measurements n times every T seconds,
The position R (Xn, Y, ,) of the moving object after nTT seconds and the traveling direction Φ at that time. is ""Xn-1+Ln" QOBβ.

=L1cosβ1+L2cosβ2+L3cosβ, ten----ten, cos/nM Yn=Yn-1+Lnsinβ.

= L, sin β1 + L2 sin β2 + Lgsin7 to 10-1111Φ. =φ, +φ2+φ3+---φn(H)
It can be stopped.

Further, the actual cruising distance Itn of the moving object 1 is (5).

09 type.

j2n=i, +z. '10G+---14゜"'='
/2('+,x,/+i,'10---+ILH"1
+I12+f12' +I12 +---+It2")
).

The principle of measuring the current position and direction of movement of a moving object according to the present invention has been described above, but if the measurement time interval T is too short, the error will become large due to the relationship with the performance described in Fig. 4. . Luxurious. If it is too long, the temperature will rise to J2° or °C as shown in Figure 5. ' can no longer be regarded as a circular arc, resulting in a large error. An appropriate value must be selected in consideration of the speed V and resolution of the TVi moving object 1.

The calculations detailed above are executed using a microcomputer installed in the moving object.

- For example, 1 cup of 70 using a microcomputer.
- The chart is shown in Figure 6. The input signal to the microcomputer is an analog-to-digital conversion of the output of the mileage measuring device shown in FIG. Of course, the mileage measuring device is separately connected to the Doppler radars 4 and 4'. Note that the reset circuit 11 in FIG. 3 may be omitted, and the refining circuit 10 may be directly reset by the output from the microcomputer.

In Fig. 6, for convenience of explanation, Drag Radar 4.4
The nth measured value of ' is 21□, and the moving component between β2n and l is, cosβ. Tori,, sinβ. ΔXn
, Δyn. Also, the length of the nth arc drawn by the 0 point is l. n.

The actual distance traveled by boat up to that time. Let it be n.

Next, a case will be described in which the moving object 1 is set at the initial position (XO2''O) in the direction Φ and is caused to travel toward the destination (Xp r ''p). however,
It does not mention methods and means related to travel routes, directions, etc.

First, set the initial position and direction (XO+"O+Φ.) and destination (Xp * Yp).Assuming that n is executed from 1, set the value at n=1.n:=0. .At the same time as the start of running, the integration circuit is reset and starts measuring β1n and J22n.At the same time, the previous measurement value 'In-1+'2n-+ is changed to X, -1, Y
n-, Φn-1 and ri. Calculate and store n-+. When Xn-1 and Yn-1 match the destination, traveling is stopped and the process ends.

If they do not match, continue measuring '+, l(!: i!, 2n. The measurement time is 7 seconds as mentioned earlier, so end the measurement after T seconds and read + I'jnpβ2n and store it in memory. .

The integration circuit is reset again, the same calculation is performed, and the process ends when the destination is reached.

If you want to know the current position, it is good to output the position (Xn-11Yn -1) stored in the memory, and if you want to know the current direction, output the direction stored in the memory (Φ.-4). ) should be output.

Further, in the above explanations, the sign of the drag signal If explained with reference to FIG. 2 was not mentioned.

In other words, from equation (2), if V is positive (forward), +Δf.

If it is negative (backward), it is -If.However, since the 1-word drag radar is configured to mix and detect the reflected wave using the oscillation frequency (frequency of the radiation wave 7) as a local oscillation signal, it is difficult to distinguish between positive and negative. Possible fz u 0 Since the moving object 1 may naturally move backward, it cannot be measured as it is. However, the method previously proposed by the inventor of the present application (Japanese Patent Application No. 57-
See No. 10287.

If you use an ultrasonic moving object detection device), you can use a positive/negative (
It is easy to distinguish between forward and backward movement. Even if this means is added, it was mentioned in the description of the prior art. A method in which wireless transmitters and receivers are installed at both the reference point and the moving object to transmit signals can also be constructed at a much lower cost.

In the embodiment shown in FIG. 1, the drag radar 4.4' is mounted on the shaft of the drive wheel and outside, but it is not limited to this position. For a moving object with the structure shown in Figure 1, this position is the easiest to measure because the line connecting both dog radars (A-BIJ in Figure 5) is always perpendicular to the connecting direction of the movement locus. This is why I chose this location. If the handle wheels 2 and 2' are placed in the same position, this relationship will no longer hold, and this will require correction in measurement and may cause errors. Also, it goes without saying that it is best to install it at an equal distance from the center of the left and right wheels.

〔Effect of the invention〕

As described above, according to the present invention, it is possible to eliminate the drawbacks of the prior art and provide a highly accurate, low-cost, and highly reliable traveling trajectory detection device for a moving object.

[Brief explanation of drawings]

FIG. 1 is a front and side view showing one embodiment of the present invention, and FIG.
The figure is a conceptual diagram of Doppler Ray 4, Figure 3 is a configuration diagram of the mileage measuring device, Figure 4 is a diagram showing the time boolean for explaining the operation of Figure 2, and Figure 5 is the same as Figure 1. An explanatory diagram of the measurement method according to the embodiment, FIG. 6 is a diagram showing a flowchart of the measurement method explained using FIG. 5. 1... Moving object, 2... Nondle wheel, 3.3'...
・Drive wheel. 4.4'...Dragon radar, 5...Floor, 6...
Axle, 7... Radiation wave, 8... Reflected wave, 9... Noculus generation circuit, 10... Precision circuit, 11... Reset circuit, 12... Output terminal. 3rd figure 4th figure 1500

Claims (1)

[Claims] 1. A moving object that freely travels on a plane is provided with a pair of drag radars that transmit and receive signals with the plane, and a pair of Doppler signals obtained from the relative speed of the plane and the moving object are obtained. A traveling trajectory detection device for a moving object, characterized in that the device measures the current position and traveling direction of the moving object. 2. The traveling trajectory detection device for a moving object according to claim 1, wherein the pair of drag radars are arranged on a line perpendicular to a tangential direction of the traveling trajectory of the moving object.