JPH0726852B2 - Position detection device for unmanned vehicles - Google Patents

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a position detecting device for an unmanned vehicle, and more particularly to a position detecting device for an unmanned vehicle that detects a traveling position, a traveling speed and a moving distance of the unmanned vehicle.

B. Overview of the Invention The present invention is a position detection device for an unmanned vehicle that detects a variety of traveling states by installing a detector that detects a traveling state on the vehicle body of the unmanned vehicle, wherein the vehicle body is provided with an optical system sensor, The detection signal of the optical system sensor is processed by a one-dimensional spatial filter means, and the two-dimensional position of the vehicle body, the speed, and the traveling direction of the vehicle body are detected from the output, thereby detecting the position of the unmanned vehicle excellent in detection accuracy. You get the device.

C. Conventional technology Conventionally, in automatic driving of an unmanned vehicle such as an automated guided vehicle, a method of laying an electromagnetic induction wire or an optical reflection tape on the road to form a traveling guide, an encoder or an axle on the measuring wheel, or the like. There is a method in which a tacho generator is attached to measure the speed and moving distance of an unmanned vehicle from a pulse or analog voltage according to the rotation of the wheels.

D. Problems to be Solved by the Invention In various conventional position detecting devices, it is necessary to process a traveling path on which a guide wire, a reflective tape, or the like is laid on the road surface, and the processing work is troublesome and Accurate measurement could not be performed due to wheel slip due to unevenness, external force, etc. and wheel abrasion.

E. Means for Solving Problems The present invention has been made in view of the above problems, and an optical system arranged on the bottom surface of a vehicle body for capturing a pattern of a traveling road surface, and the optical system An image detection unit that obtains an image pattern signal by sampling a captured image pattern at a predetermined cycle, and the image pattern signal of the image detection unit is multiplied by a preset trigonometric function setting signal, and the multiplication signal is integrated. The position, speed and direction of the unmanned vehicle by the filter means for obtaining the trigonometric function pattern signal and the arithmetic processing section for multiplying and adding based on the trigonometric function pattern signal of the filter means to calculate the travel distance signal of the vehicle body. To detect.

F. Examples Examples of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram of a position detecting device for an unmanned vehicle according to an embodiment of the present invention, in which 10 is a vehicle body of the unmanned vehicle, 12 is a traveling wheel, and 20A.
Is the traveling axis (X axis) of the unmanned vehicle on the bottom surface 11 of the vehicle body 10.
With respect to the first optical system 20B arranged on one side end portion (on the Y axis) of the bottom surface portion 11, 20B is a predetermined distance D from the first optical system 20A.
Is a second optical system disposed on the other side end portion (on the Y axis) of the bottom surface portion 11. 30A is a first image detection unit that receives the image signal of the first optical system 20A, 40A is a first spatial filter that receives the image detection signal of the first image detection unit 30A, and 50A is a first spatial filter. In the arithmetic processing unit, the first spatial filter 40A
The calculation is performed based on the filter output signal of to calculate the moving distance, speed, and current position of the unmanned vehicle. These first optical system 20A, first image detection unit 30A, first spatial filter 40
A and the first arithmetic processing unit 50A, the first detection processing unit
60A is constructed. Similarly, 20B is a second optical system, 30B is a second image detection unit, 40B is a second spatial filter, 50B is a second arithmetic processing unit, and these constitute the second image detection unit 60B. To be done.

The first detection processing unit 60A and the second detection processing unit 60B are composed of the same components as shown in FIG. That is, as shown in FIG. 2, the first and second video detection units 30
A and 30B are composed of a line sensor 31, a reading circuit 32 and an analog / digital converter (A / D converter) 33. The first and second spatial filters 40A, 40B include multiplication units 41a, 41b,
The pattern setting units 42a and 42b and the integration calculation units 43a and 43b are included.

The first and second arithmetic processing units 50A and 50B are low-pass filters 51a and 5a.
1b, trigonometric function calculation unit 52, phase calculation unit 53, summation unit 54, multiplication unit 5
5. The differential calculation unit 56 and the rotation locus calculation unit (m calculation unit) 70 are included. As shown in FIG. 3, the m calculation unit 70 includes sine wave signal calculation units 71a and 71b, multiplication units 72a and 9b, data holding units 73a to 73c that are previous value holding units, and summing units 74a and 74b. Has been done.

FIG. 5 shows a concrete configuration example of the first and second optical systems 20A and 20B. In FIG. 5, 21 is a cylinder lens for receiving the reflected light from the road surface 80, and 22 is a collimating lens. is there. Further, FIG. 6 shows another example of the optical system, in which the reflected light of the road surface 80 is a cylinder lens by the collimator lens 22.
It focuses on 23.

Next, the operation of the position detecting device having the above configuration will be described.

As shown in FIGS. 5 and 6, the optical systems 20A and 20B form images on the line sensor 31 in the direction parallel to the line sensor 31 by the cylinder lenses 21 and 24, and in the direction perpendicular to the line sensor 31. The lens system is configured so that the line sensor 31 becomes the focal point. . That is, the line sensor 31 is formed by continuously coupling a large number of photoelectric conversion elements in the traveling direction of the vehicle body, detects the irradiation distribution on the irradiation surface by each element, and outputs the electric output converted by each element as the first and the first. Image detectors 30A and 30B in Fig. 2
Send to. First, the road surface is irradiated with light to form an irradiation surface. Due to the unevenness of the road surface, an uneven illuminance pattern is formed on the irradiation surface, and this illuminance distribution is projected onto the line sensor through the lens. Lens has length P and width W
Is configured to collect the light. The unevenness pattern is projected on the line sensor, and each photoelectric conversion element forming the unevenness pattern sends out an electrical output corresponding to the intensity of the illuminance to the spatial filter. When the above operation is intermittently performed at fixed time intervals ΔT, the movement amount ΔP of the illuminance distribution before and after that time is obtained. The video signals of the optical systems 20A and 20B are detected by the first and second video detection units 30A and 30B, respectively, and are input to the first and second spatial filters 40A and 40B. The spatial filters 40A and 40B detect a specific spatial frequency component from an optically random pattern on the road surface and measure the temporal behavior thereof to measure the position and speed of the vehicle body relative to the road surface.

The output from the line sensor 31 passes through the read circuit 32,
It will be input to the spatial filter 40A (40B). The spatial filter 40A (40B) extracts an arbitrary frequency component from the reflected light composed of optically random frequencies, and calculates the vector corresponding to the moving distance in the phase space from its time series change. is there. That is, the read circuit 32
The signal P that has passed through is converted into a digital signal by the A / D converter 33, and is expanded into a Fourier series as follows.

However, x ₀ is the position of the vehicle at time t, x is the position in the line sensor read from the reading circuit, and x ₀ is the reference, k = 2π / L, i = 0,1,2 ..., L Is the measurement range of the line sensor.

The Fourier coefficients A _i (x ₀ ) and B _i (x ₀ ) are given by the following equation.

Each of these signals P is multiplied as a spatial filter by a cosine wave and sine waves S ₁ and S ₂ of a specific frequency shown in the following equation, and integrated in a predetermined range.

S ₁ = F ・ cos (n ・ k ・ x) (4) S ₂ = F ・ sin (n ・ k ・ x) (5) Here, the output P from the line sensor should change continuously in a trigonometric function with respect to a certain n-th component if the reflected light has a constant condition such as a road surface or environment. It will be shown by the following formula.

P _n (ΔL) = G · sin (n · k · ΔL + φ) (8) Therefore, the n-th order Fourier coefficients A _n and B _n are obtained as follows.

Similarly, B _n (x ₀ ) = Gcos (nkx ₀ + φ) (10) Equations (9) and (10) are substituted into the above equations (6) and (7) to rearrange them into the following equations.

a (x ₀ ) = (FL / 2) ・ Gcos (nkx ₀ + φ)… (11) b (x ₀ ) ＝ (FL / 2) ・ Gsin (nkx ₀ + φ)… (12) a (x ₀ ), b (x ₀ ) vibrate in a trigonometric function, and are then input to the moving distance calculation unit.

Here, the moving distance calculation unit obtains a rotation angle ρ about the origin on the phase plane shown in FIG. 4 of the vector A having two-dimensional coordinates a (x ₀ ) and b (x ₀ ). . That is, as shown in FIG. 4, the vector A having two-dimensional coordinates of a (x ₀ ) and b (x ₀ ) is
It rotates at a rotation angle nkx ₀ proportional to the moving distance x ₀ . Next, the relational expression holds.

nkx ₀ + φ = 2πm + ρ (13) where m is the number of revolutions around the origin, ρ = arctan (b (x ₀ ) / a (x ₀ )), (0 ≦ ρ ≦ 2π) φ is the initial value ρ _{t = 0} .

Therefore, if the equation (13) is modified, the moving distance x ₀ to be obtained is expressed by the following equation.

x ₀ = m · p + (ρ−φ) p / 2π (14) Schematically, as shown in FIG. 2, the pattern of the road surface 80 is passed through the optical system 20A (20B) and the image detection unit 30A ( 30B), and the output is input to the spatial filter 40A (40B). In the spatial filter, the sum of products is calculated using the input value and a preset pattern that is preliminarily calculated and preset. Here, a sine wave pattern is used. The set pattern has data for a period determined by the number of elements and pits of the line sensor 31 of the image detection unit 30A (30B). The output of the spatial filter is arithmetically processed by the arithmetic processing unit 50A (50B) to calculate the moving distance of the vehicle, and the moving distance is differentiated with respect to time to obtain the speed. When detecting a position in a cycle that is sufficiently smaller than the movement of the vehicle, the change in the direction of the vehicle is calculated from the difference between the movement distance measurement values of the two detection processing units in one cycle, and the average value of the changes is calculated. The position of the vehicle body is calculated assuming that the vehicle has traveled during that period.

More specifically, as shown in FIG. 2, the video signal of the road surface 80 is input to the video detection unit 30A (30B) via the optical system 20A (20B). In the video detection unit 30A (30B), the line sensor 31 detects the video and the read circuit outputs the video detection signal.
32 reads out, A / D converts the video detection signal, and inputs it to the spatial filter 40A (40B). Spatial filter 40A (40B)
In the multiplication unit 41a, the image detection signal from the image detection unit 30A (30B) is multiplied by the sine wave pattern setting signal of the pattern setting unit 42a, and the multiplication unit 41b is used to multiply the image detection signal and the cosine wave of the pattern setting unit 42b. Multiply the pattern setting signals. Further, the multiplication signal of the first multiplication unit 41a is integrated by the integration calculation unit 43a and the integrated signal a is output, and
The multiplication signal of the multiplication unit 41b is integrated by the second integration calculation unit 43b,
The integrated signal b is output.

In the arithmetic processing unit 50A (50B), the integrated signal is passed through the low-pass filter 51a to obtain the signal Sa, and the integrated signal S2
Is passed through a low-pass filter 51b to obtain a signal Sb. Signal Sa
And Sb are the trigonometric function calculation unit 52 and the rotation locus calculation unit (m
It is input to the calculation unit) 70. The trigonometric function calculation unit 52 outputs the signal Sa
The calculation is performed based on Sb, and the output ρ = arctan (Sb / Sa) is calculated. The phase calculation unit 53 calculates (ρ−−
Calculate φ) / 2π.

In the m calculation unit 70, as shown in FIG. 3, the signal Sa is guided to the multiplication unit 72a and the data holding unit 73a through the sine wave signal calculation unit 71a, and the signal Sb is calculated through the sine wave signal calculation unit 71b.
And the data holding unit 73b. The multiplication unit 72b multiplies the previous sampling value from the data holding unit 73a by the current sampling value. The summing unit 74a sums the current sampling value and the previous sampling value of the data holding unit 73b.
The multiplication unit 72b multiplies the multiplication value of the multiplication unit 72a and the addition value of the addition unit 74b, and the addition unit 74a adds the current multiplication value of the multiplication unit 72b and the previous value of the data holding unit 73c to m. Output a signal.

Further, in the arithmetic processing unit 50A (50B), as shown in FIG. 2, the summing unit 54 sums the m signal of the m arithmetic unit 70 and the phase signal of the phase arithmetic unit 53, and the addition of the summing unit 54 is performed. The moving distance is calculated by multiplying the value and the pitch (cycle) P of the spatial filter, and the moving distance signal is input to the differential calculation unit 56 to calculate the speed.

As shown in FIG. 1, the center point of the bottom surface portion 11 of the vehicle body 10 is A
It is assumed that (x, y) and the vehicle is moving in the direction of the arrow at an angle of θ with respect to the X axis on the XY plane. Here, if the outputs of the first and second detection processing units 60A and 60B by one sampling change by Δr, Δl (position), the change Δθ in the traveling direction of the vehicle body 10 is Δθ≈ ( l−r) / D (15), and the current angle θn of the vehicle body 10 becomes θn = θ _n-1 + Δθ (16). Here, θ _n-1 is the angle at the time of the previous sampling. During this time, on average It is possible to obtain (x _n , y _n ) at the position of the point A from the following equations (3) and (4) assuming that the movement has been made by

Here, n is the current coordinate and n-1 is the previous coordinate. Therefore,
The vehicle position (x, y) and direction θ are calculated by calculating equations (15) to (18).
Ask for.

The two outputs of the spatial filter rotate on the phase plane as the vehicle moves, as shown in FIG. Therefore, the traveling distance is calculated by the following equation.

X ₀ = (2πm + ρ−P / nK = m × P + (ρ−φ) / 2πP ...
(19) where m is the number of revolutions of the locus around the origin (the counterclockwise direction is positive), X ₀ is the moving distance, P is the spatial filter pitch (cycle), ρ = arctan (Sb / Sa) , 0 ≦ ρ <2π, P = ρ _t
= 0 (initial value).

7 and 8 show the above-mentioned operation flow.
FIG. 8 is a calculation flow of the spatial filter 40A (40B), and FIG. 8 is a calculation flow of the calculation processing unit 50A (50B).

As shown in FIG. 7, the line sensor (CC
The output of D) is A / D converted and input to the spatial filter. In the spatial filter, as shown in step S2, a sine wave (sin) pattern is read as the window function data 1, and the product-sum operation of this sine wave pattern and the CCD output is executed (step S3). Next, as shown in step S4, a cosine wave (cos) pattern is read as the window function data 2, and the product-sum operation of this cosine wave pattern and the CCD output is performed (step S4).
Five). Then, as shown in step S6, steps S1 to S
The operation of 5 is repeated for the number of CCD elements, for example, 2048 times.

As shown in FIG. 8, in the arithmetic processing section, the output signals a and b of the spatial filter are respectively passed through one-dimensional low-pass filters (steps S7 and S8), and as shown in step S9,
The n operation is performed by counting the number of times the locus on the phase plane moves between the first quadrant and the fourth quadrant, with the direction from the fourth quadrant to the first quadrant being positive. Next, as shown in step S10, ρ = arctan (Sb / Sa) is calculated in the range of 0 ≦ ρ <2π to execute the phase calculation, and as shown in step S11, m
.P + (. Rho.-P) /2.pi..P is calculated and the distance calculation is executed, and the distance is differentiated and the speed calculation is executed as shown in step S12.

FIG. 9 shows another embodiment of the present invention, in which a microcomputer 90 is used as an arithmetic processing section. The microcomputer 90 is composed of a central processing unit (CPU) 91, a random access memory 92 and a read only memory (ROM) 93. As described above, the pattern of the road surface is detected by the CCD through the optical system, and its output is input to the microcomputer 90 through the A / D converter, and its value and the setting calculated in advance and stored in the ROM 93. Based on the pattern, the arithmetic processing as described above is executed.

G. Effect of the Invention As described above, the present invention provides an optical system sensor on a vehicle body, processes a detection signal of the optical system sensor by a one-dimensional filter means, and outputs the two-dimensional position, speed, Since the direction of travel is detected, it is possible to perform non-contact measurement without the need to process the running path such as guide wires or reflective tape, and the effects of slip and wheel abrasion due to road surface irregularities and external forces. It is possible to measure the position and speed accurately without being affected.

[Brief description of drawings]

FIG. 1 is a block diagram of a position detection device according to the present invention, FIG. 2 is a block diagram showing details of a detection processing unit according to an embodiment of the present invention, FIG. 3 is a block diagram of a rotation locus calculation unit, and FIG.
FIG. 7 is a rotation locus diagram of the rotation locus calculation unit, FIGS. 5 and 6 are optical system configuration diagrams, FIG. 7 is a spatial filter calculation flow diagram, and FIG. 8 is a calculation processing unit flow diagram. 9
FIG. 9 is a block diagram showing another embodiment of the detection processing unit. 10 ... Car body, 11 ... Bottom surface of car body, 20A ... First optical system, 20B
... second optical system, 30A ... first image detecting section, 30B ... second image detecting section, 40A ... first spatial filter, 40B ... second spatial filter, 50A ... first arithmetic processing section, 50B ... 2nd arithmetic processing part, 60A ... 1st detection processing part, 60B ... 2nd detection processing part.

Claims (2)

[Claims] 1. An optical system arranged on a bottom surface of a vehicle body for capturing a pattern of a road surface on which the vehicle travels, and an image detecting section for sampling an image pattern captured by the optical system at a predetermined cycle to obtain an image pattern signal. A filter means for multiplying the video pattern signal of the video detection section by a preset trigonometric function setting signal, and integrating the multiplication signal to obtain a trigonometric function pattern signal; and a trigonometric function pattern signal of the filter means. A position detecting device for an unmanned vehicle, which is configured by an arithmetic processing unit that performs multiplication and addition processing to calculate a travel distance signal of the vehicle body. 2. A first optical system arranged on the bottom surface of a vehicle body for taking a pattern of a traveling road surface, and a video pattern obtained by sampling the video pattern taken by the first optical system at a predetermined cycle. A first video detection unit for obtaining a signal, a video pattern signal of the first video detection unit and a preset trigonometric function setting signal are multiplied, and the multiplication signal is integrated to obtain a trigonometric function pattern signal. No. 1 filter means and the trigonometric function pattern signal of the first filter means are multiplied to calculate the moving distance signal of the vehicle body, and the moving distance signal is differentiated to calculate the speed signal. A first detection processing unit including a first arithmetic processing unit; and a traveling road surface pattern arranged on the bottom surface of the vehicle body at a predetermined distance from the first optical system with respect to the traveling direction axis of the vehicle body. Second optical system to take a picture , A second video detection unit that obtains a video pattern signal by sampling a video pattern captured by the second optical system in a predetermined cycle, and a video pattern signal of the second video detection unit is preset. Second vehicle means for multiplying the trigonometric function setting signal and integrating the multiplied signal to obtain a trigonometric function pattern signal; and multiplying based on the trigonometric function pattern signal of the second filter means to perform the vehicle body. And a second detection processing unit configured to calculate a speed signal by differentiating the travel distance signal and calculating the speed signal by the first detection processing unit. Travel distance signal,
It is configured by means for calculating the traveling direction angle of the vehicle body based on the movement distance signal obtained by the second detection processing unit and the installation interval between the first optical system and the second optical system. Position detection device for unmanned vehicles.