JPH03218468A - Position detector for traveling object - Google Patents

Abstract

PURPOSE:To exactly calculate phase difference and a stop position by providing a filter arithmetic part between a space filter arithmetic part and a phase difference calculation part and executing HPF arithmetic when the phase difference is larger than a prescribed value. CONSTITUTION:Signals Sa and Sb respectively multiplying 32a and 32b and integrating 34a and 34b triangle function setting signals Ci and Si to a road surface pattern signal Di and inputted through changeover switches 51a and 51b to a phase difference calculation part 36 as current signals Sa2 and Sb2 and inputted through delay parts 35a and 35b to the calculation part 36 as signals Sa1 and Sb1 before one sampling, and a phase difference signal DELTAphiis calculated and compared with a reference phase setting signal DELTAphiref through an absolute value circuit 54. At the time of DELTAphi>DELTAphiref, the switches 51a and 52b are connected to HPF 52a and 52b and filter arithmetic is executed. Then, a direct current part to be superimposed to a space filter 31 is removed and the exact phase difference is calculated. At the time of DELTAphi<DELTAphiref, the signals are not passed through the HPF 52a and 52b and therefore, when a traveling object is stopped, phase difference calculation error and position detection error are eliminated as well.

Description

DETAILED DESCRIPTION OF THE INVENTION A. Field of Industrial Application The present invention relates to a position detection device for a moving object, and particularly to a position detection device that is effective for use in detecting the position of an unmanned vehicle or the like.

B. Summary of the Invention The present invention provides a spatial filter calculation unit, a phase difference calculation unit that obtains a phase difference based on a spatial filter calculation signal obtained by the spatial filter calculation, and a phase difference calculation unit that calculates a phase difference based on the spatial filter calculation signal obtained by the spatial filter calculation. A device comprising a calculation unit that calculates the speed and moving distance of a moving object based on a phase difference, wherein a high-pass filter calculation is performed between the spatial filter calculation unit and the phase difference calculation unit when the phase difference is larger than a predetermined value. By providing a filter calculation section, a position detection device for a moving body that eliminates position detection errors can be obtained.

C. BACKGROUND OF THE INVENTION In the automatic operation of vehicles such as automatic guided vehicles, one of the methods for measuring the position and speed of a vehicle is to use a spatial filter. This method has the characteristics of non-contact and highly accurate detection.

Figure 6 shows the distance traveled by this type of conventional spatial filter.
Since this figure shows a position detection device for detecting speed, 10 is an optical system detection section, and 20 is an optical system detection section 10.
A video detection section 30 receives the video signal of the video detection section 20 as an input and is an arithmetic processing section that receives and processes the video detection signal of the video detection section 20, and is composed of a microcomputer.

The optical system detection section 10 is composed of a line sensor 11 and a CCD 12 that receive an image of the road surface 40 as input.

The video detection section 20 is the readout circuit 2! and an analog/digital (A/D) converter 22. The arithmetic processing section 30 includes a spatial filter arithmetic section 31, a delay section 35a, and a delay section 35b.
, a phase calculation section (Δφ calculation section) 36, an accumulation section 37, a multiplication processing section 38a, and a multiplication processing section 38b. Furthermore, the spatial filter calculation unit 3K is
It is composed of a first multiplication operator 32b, pattern setting operators 33a and 33b, an integral operator 34a, and an integral operator 34b.

According to the position detecting device shown in FIG. 6, the road surface pattern signal Di digitally converted by the image detecting section 20 is input to the spatial filter operator 3I of the arithmetic processing section 30. The spatial filter operator 3l performs a sum-of-products operation on the COS load function Ci from the pattern setting unit 33a as shown in FIG. The spatial filter output signals Sa and sb are output as follows. In FIG. 7, P is the pitch of the spatial filter weight function, L is the length of the line sensor, and n is the wave number of the weight function.

Furthermore, the arithmetic processing unit 30 calculates the rotation angle Δφ of the output vector in l sampling period from the spatial filter output at the previous sampling time, and adds P/2π
By multiplying the moving distance X [m”J and the speed V [
m/TS].

Here, S. = S:f (X, t)・COS(2πX/P
)d X,sb=z:r(x,t>・SIN(2πX/
P) dX, L'rS is the sampling time, P11, and n.

FIG. 9 shows the algorithm of the position detection device of FIG. 6. In step St, data from a CCD line sensor consisting of n pixels is read.

Next, in step S2, the SIN weight function Si (SIN numerical value x Hanning function) is read, and in step S3
Clears the spatial filter outputs Sa and sb and executes the calculation of the spatial filter 3I. In the spatial filter calculation, as shown in step S4, i=1, and the product-sum calculation Sb=Sb
+S iXDi is executed (step S5). As a result, i=i+1 is set in step S6. After that, i>n
(step S7), and if i>n, the process returns to step S5, and if inn, the process proceeds to step S8. In step S8, the COS weight function (COS function x Hanning function) is read, and in step S9, it is set to i-1.

After setting i=1, proceed to step SlO. Product-sum operation Sa=
Execute Sa+CiXDf and set i=i+ at step Sll.
Set to 1. When i-i+1, the process proceeds to step Sl21 to determine if i>n, and if it is not in, the process returns to step SIO, and if i>n, the process proceeds to step Sl3. In step S13, the calculation of Δφ is executed, and in step S15, 916
Proceed to. The moving distance is determined in step Sl5, and step 9
In step 16, the speed is calculated.

D. Problems to be Solved by the Invention As mentioned above, spatial filters have the advantage of being able to measure speed and moving distance with a simple configuration. Due to the luminous intensity distribution, as the target object moves, there is a stationary part in the COD signal as shown in Figure 10, and if the spatial frequency component of the stationary part matches the frequency component extracted by the spatial filter, the spatial filter will have a stationary part as shown in Figure 10. The DC components shown in the figure are superimposed. Therefore, if the spatial filter output does not cross zero as shown in Figure 11 and the rotation vector rotates away from the origin as shown in Figure 12, the calculation result of Δφ will no longer be a value proportional to the speed. . The direct current component superimposed on the spatial filter output has been a major problem.

In order to solve this problem, it is possible to add a high-pass filter to the spatial filter. This method is effective as a means for removing the DC component of the spatial filter output, but since the time constant of the high-pass filter is fixed, when the automatic guided vehicle is stopped, the spatial filter output is , converges to zero. In this way, when the spatial filter outputs Sa and Sb both converge to zero,
The output will be only noise components. Therefore, since the automatic guided vehicle is stopped, Δφ should be zero, but this noise component causes an incorrect Δφ to be calculated, resulting in a large position detection error.

The present invention has been made in view of the above-mentioned problems, and its purpose is to provide a position detection device for a moving object that eliminates position detection errors without superimposing a DC component on a spatial filter calculation output signal. be.

E. Means for Solving the Problems In order to achieve the above-mentioned objects, the present invention includes an optical system detection unit that photographs a pattern on a running road surface according to the movement of a moving object, and an optical system detection unit that photographs a pattern of a traveling road surface according to the movement of a moving object. A video detection unit that samples a video pattern to obtain a video pattern signal, and multiplies the video pattern signal of the video detection unit by an orthogonal trigonometric function setting signal with a spatially different period, and integrates the multiplied signal to obtain an orthogonal signal. an arithmetic processing unit that obtains a trigonometric function pattern signal, obtains a phase difference signal of the trigonometric function pattern signal based on the trigonometric function pattern signal, and calculates the speed or moving distance of the moving object based on the phase difference signal; The position detecting device is constructed by providing the arithmetic processing section with filter arithmetic means for performing a high-pass filter arithmetic operation on the trigonometric function pattern signal when the phase difference signal is larger than a predetermined value.

F. Effect: Due to scratches or dust on the COD line sensor or lens, or the luminous intensity distribution of the lighting, stationary parts that do not move with the movement of the target object occur in the COD signal, but this causes the DC component of the spatial filter output to become high. Remove with bus filter.

Furthermore, the Δφ calculation error in zero convergence of the spatial filter output when the moving body is stopped due to the fixed time constant of the high-pass filter is solved by not passing the high-pass filter when Δφ is small.

G. EXAMPLES Examples of the present invention will be described below with reference to FIGS. 1 to 5.

FIG. 1 shows a position detecting device for a moving body according to an embodiment of the present invention, and the same or corresponding parts as those in FIG. 6 are given the same reference numerals.

In this embodiment, a filter operator 50 is provided at the output stage of the spatial filter m operator 31.

That is, the first changeover switch 51a and the first high-pass filter 52a are connected to the output stage of the integral calculation unit 34a of the spatial filter calculation unit 3I, and the second changeover switch 5lb is connected to the output stage of the integral calculation unit 34b. While connecting the second high-pass filter 52b, the first changeover switch 51a
A filter control section 53 is provided between the second changeover switch 52b and the output of the Δφ operator 36. The filter control section 53 includes an absolute value circuit 54 which inputs the output signal Δφ of the Δφ operator 36, and a comparator 55 which receives the output signal of the absolute value circuit 54 and the reference phase setting signal Δφref.

In the moving body position detection device having the above configuration, the optical system detection section 10 detects a road surface pattern, and the readout circuit 21 of the image detection section 20 reads out the road surface pattern signal from the optical system detection section 10. The A/D converter 22 converts the road pattern signal into an analog/digital (A/D) signal and converts it into an arithmetic processing unit 30.
A road surface pattern signal Di is input to. In the arithmetic processing unit 30, the road surface pattern signal Di is
and the COS weighting function signal C of the first pattern setting filter 33a.
Multiply by i to obtain CiXDi, and use S by Kakusanro 32b
SiXDi is obtained by multiplying the IN load function signal Si and the road surface pattern signal St. The integral operator 34a performs an integral operation on Ci
πX/P)・dX is obtained, and the integral operator 34b is SiXDi
is integrated and the output sb=5:r(x,t)・SIN(2π
Obtain X/P)dX. The output signal Sa of the spatial filter operator 31 is changed to the current signal Sa2 via the changeover switch 51a.
is input to the Δφ operator 36 as well as the delay operator 35a.
is inputted to the Δφ operator 36 as the signal Sal (previous value) of one sampling before. Further, the output sb of the spatial filter section 3l is input to the Δφ calculation section 36 as the current signal Sb2 via the changeover switch 5lb, and is also inputted to the Δφ calculation section 36 as the signal Sbl (previous value) of one sampling before passing through the delay gate 35b. is input. Δφ operator 3
In step 6, these signals Sal, Sa2, Sbl, and Sb2 are input and processed to obtain a phase difference signal. The phase difference signal Δφ is input to the comparator 55 through the absolute value circuit 54 and is input to the reference phase setting signal Δφref.
compared to When the phase difference signal Δφ is extremely small and Δφ is less than Δφref, the comparator 55 does not output an output.
Changeover switch 51a. 5lb is in a state where it is not connected to the high bass filter. When the phase difference signal Δφ becomes larger than the reference phase setting signal Δφref, that is, Δφ>Δ
When φref, the comparator 55 outputs an output, which causes the selector switches 51a and 5lb to switch to the high-pass filter 5.
2a and 52b to perform filter calculations.

FIG. 2 shows the algorithm of the apparatus shown in FIG. 1. In step SI7 (Sl), CCD data is read, the process proceeds to step S18 (S2), the SIN weight function is read, and based on this reading, S19 ( S4-S7)
Executes the SIN spatial filter operation. Next step S
20 (S8), the COS weight function is read, and the process proceeds to step S2l. A COS spatial filter calculation is executed in step S21 (S9 to S12), and the process proceeds to step S22, where a flag indicating whether or not to perform a high-pass filter is set HF =
Determine the presence or absence of 1. }{ If F = 1, the process advances to step S23, where high-pass filter calculation is performed, and then step S24. Proceed to (Sl3).

If H F = 1 in step S22, step S
Proceed to step 24. In step S24, a Δφ calculation is performed, and the process proceeds to step S25, where it is determined whether Δφ>Δφref. If Δφ>Δφref, step S 2 6 1.
: Advance HF=1, step S28 (S14.S1
5)+. :-Go forward. Step S251? Δφ〉Δφre
If it is not f, the process proceeds to step S27, where H F = 0, and the process proceeds to step S28. In step 628, the moving distance and speed are calculated.

According to the position detection device shown in FIG. 1, when Δφ>Δφref, high-pass filter calculation is performed on the spatial filter output signals Sa and Sb. As shown in FIG. 4, the direct current component superimposed on the spatial filter is removed, and the output waveform thereby crosses zero, and as shown in FIG. 4, the Lissajous waveform rotates around the origin, making it possible to calculate accurate Δφ. Furthermore, if Δφ is small, by not allowing the high-bus filter to pass through, it is possible to eliminate the Δφ calculation error when a moving object such as an automatic guided vehicle is stopped, as well as the position detection error, as shown in Figure 5. can.

H. Effects of the Invention The present invention, as described above, adds a high-pass filter, and when the phase difference Δφ is extremely small, such as when a moving body is stopped, calculation of the high-pass filter is not performed, and the phase difference Δφ Since the calculation of the high-pass filter is performed only when the Since it rotates, it is possible to calculate an accurate phase difference and also to calculate an accurate stopping position.

[Brief explanation of drawings]

FIG. 1 is a block diagram of a moving object position detection device according to an embodiment of the present invention, FIG. 2 is an operation flow diagram of the device in FIG. 1, and FIG. 3 is a spatial filter output signal waveform diagram of the device in FIG. 1. , FIG. 4 is a spatial filter surge waveform diagram of the device shown in FIG. 1, FIG. 5 is a characteristic diagram of the spatial filter output signal of the device shown in FIG. 1, and FIG. 6 is a block diagram of a conventional position detection device for a moving object. Figure 7 is a spatial filter weight function waveform diagram, Figure 8 is a spatial filter output waveform diagram, Figure 9 is an operation flow diagram of the apparatus in Figure 6, and Figure 1O is an operation waveform of the apparatus in Figure 6. Figure, 11th
The figure is an output waveform diagram of the spatial filter of the device in Figure 6,
The figure is a surge waveform diagram of the device in Figure 6, and Figure 13 is the waveform diagram of the device shown in Figure 6.
FIG. 3 is an operation waveform diagram of the device shown in the figure. 10... Optical system detection unit, l1... Line sensor, 1
2. CCD, 20... image detection unit, 21... successive circuit, 22... A/D converter, 30... arithmetic processing unit,
3l... Spatial filter calculation unit, 32a, 32b...
Multiplication section, 33a, 33b - pattern setting section, 34a, 3
lb...integral calculation section, 35. a, 35b... Delay unit, 36... Phase difference calculation unit, 37... Accumulator, 38a
, 38b... Multiplication processing unit, 40... Road surface, 50...
- Filter operation section, 51a. 5lb...Selector switch, 52a, 52b...High bass filter, 53...
Filter control section, 54... Absolute value circuit, 55... Comparator. Fig. 2 Fig. 3 Fig. 4 Sa Fig. 5 Manufacture Fig. 10 CCD line sensor signal Stationary portion as the object moves Fig. 11 Fig. 12 Fig. Sa Fig. 13

Claims (1)

[Claims] (1) An optical system detection unit that captures a pattern on a running road surface according to the movement of a moving object; an image detection unit that samples a video pattern captured by the optical system detection unit to obtain a video pattern signal; The video pattern signal of the video detection section is multiplied by an orthogonal trigonometric function setting signal with a spatially different period, and the multiplied signal is integrated to obtain an orthogonal trigonometric function pattern signal. It consists of an arithmetic processing unit that obtains a phase difference signal of a trigonometric function pattern signal and calculates the speed or moving distance of the moving body based on the phase difference signal, 1. A position detecting device for a moving body, characterized in that the apparatus further comprises filter calculation means for performing a high-pass filter calculation on the trigonometric function pattern signal.