JP2676777B2 - Position detection device for unmanned vehicles - Google Patents

Description

The present invention relates to a position detection device for an unmanned vehicle, and more particularly to a position detection device for an unmanned vehicle that detects displacements of the unmanned vehicle with respect to a traveling position, a moving distance, and a traveling direction. Regarding

B. SUMMARY OF THE INVENTION The present invention is a position detecting 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 the one-dimensional spatial filter means, and the displacement of the vehicle body in the direction perpendicular to the traveling direction of the two-dimensional vehicle body is detected from the output of the one-dimensional spatial filter means. A position detecting device is obtained.

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 of attaching a tachogenerator and measuring the speed and moving distance of an unmanned vehicle from a pulse or analog voltage according to the rotation of the wheel.

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, etc. are laid on the road surface, which is a troublesome work, and unevenness of the road surface is required. , Accurate measurement was not possible due to wheel slip and wheel wear due to external force, etc., and the displacement of the vehicle body in the direction perpendicular to the traveling direction could not be detected.

E. Means for Solving the Problems The present invention has been made in view of the above-mentioned problems, and is provided on a bottom surface portion of a vehicle body so as to be substantially perpendicular to a traveling axis direction of the vehicle body and a traveling road surface. Optical system for capturing the image pattern, an image detection unit that obtains an image pattern signal by sampling the image pattern captured by the optical system at a predetermined cycle, and the image pattern signal of the image detection unit is set in advance. A filter means for multiplying a trigonometric function setting signal and performing an integral operation on the multiplied signal to obtain a trigonometric function pattern signal; and a multiplying and adding process based on the trigonometric function pattern signal of the filter means to obtain a moving distance signal of the vehicle body. The position, speed and direction of the unmanned vehicle are detected by the calculation processing unit for calculation.

F. Examples Examples of the present invention will be described below with reference to the drawings. First
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 traveling wheels, and 20 is a traveling wheel.
A is the traveling axis of the unmanned vehicle (X
The first axis disposed at one side end of the bottom surface portion 11 with respect to the (axis)
The second optical system 20B is disposed at the other side end portion (on the Y-axis) of the bottom surface portion 11 with a predetermined distance D from the first optical system 20A.
The optical system 20C is a third optical system arranged so that the center point of the bottom surface portion 11 of the vehicle body 10 coincides with the center point of the optical system and the axis is in the width direction of the vehicle body 10. 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. The arithmetic processing unit performs arithmetic processing based on the filter output signal of the first spatial filter 40A to calculate the moving distance, speed and current position of the unmanned vehicle. The first optical system 20A, the first image detecting section 30A, the first spatial filter 40A and the first arithmetic processing section 50A constitute a first detection processing section 60A. Similarly, 20B is the second optical system,
30B is a second image detector, 40B is a second spatial filter, 50
B is a second arithmetic processing unit, which constitutes a second detection processing unit 60B. 20C is the third optical system,
30C is a third video detection unit, 40C is a third spatial filter, 50
C is a third arithmetic processing unit, which constitutes a third detection processing unit 60C.

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

The first, second, and third arithmetic processing units 50A, 50B, 50C are low-pass filters 51a, 51b, trigonometric function arithmetic unit 52, phase arithmetic unit 53, summing unit
54, a multiplication unit 55, a differential calculation unit 56, and a rotation locus calculation unit (m calculation unit) 70. The m calculation unit 70 is the third
As shown in the figure, the sine wave signal calculation units 71a, 71b, the multiplication unit 72a,
72b, data holding unit 73a to 73c which is the previous value holding unit, summing unit 7
It is composed of 4a and 74b.

FIG. 5 shows a concrete configuration example of the first, second and third optical systems 20A, 20B and 20C. In FIG. 5, 21 is a cylinder lens for receiving the reflected light from the road surface 80, 22 is a collimating lens, and 31 is a CCD line sensor. Further, FIG. 6 shows another example of the optical system, in which the reflected light of the road surface 80 is focused on the cylinder lens 23 by the collimator lens 22. Next, the operation of the position detecting device having the above-mentioned configuration will be described. .

The optical systems 20A, 20B, 20C are, as shown in FIGS. 5 and 6,
A lens system is configured such that the cylinder lenses 21 and 24 form an image on the line sensor 31 in a direction parallel to the line sensor 31, and the line sensor 31 is a focal point in a vertical direction. The video signals of the optical systems 20A, 20B, 20C are respectively detected by the first, second, and third video detection units 30A, 30B, 30C, and the first, second, and third spatial filters 40A, 40B, 40
Entered in C. 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. Further, the spatial filter 40C also detects a specific spatial frequency component from an optically random pattern on the road surface and examines its temporal behavior to move (slip) almost vertically to the traveling direction of the vehicle body 11. The displacement when it does is detected.

Schematically, as shown in FIG. 2, the pattern of the road surface 80 is transferred through the optical system 20A (20B, 20C) to the image detecting unit 30A (30B, 3B).
The output is detected by the spatial filter 40A (40C).
B, 40C). 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 the pitch of the line sensor 31 of the video detection unit 30A (30B, 30C). The output of the spatial filter is the processing unit 50A
(50B, 50C) is calculated to calculate the moving distance of the vehicle, and the moving distance is differentiated with respect to time to obtain the speed or displacement. 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 transmitted through the optical system 20A (20B, 20C) to the video detection unit 30A (3
0B, 30C). In the video detection unit 30A (30B, 30C), the line sensor 31 detects the video, the reading circuit 32 reads the video detection signal, and the video detection signal is A / D.
It is converted and input to the spatial filter 40A (40B, 40C). In the spatial filter 40A (40B, 40C), the multiplication unit 41a multiplies the image detection signal from the image detection unit 30A (30B, 30C) by the sine wave pattern setting signal of the pattern setting unit 42a, and the multiplication unit 41b generates an image. The detection signal and the cosine wave pattern setting signal of the pattern setting unit 41b are multiplied. Furthermore, 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 and the integration signal b is calculated. Output.

In the arithmetic processing unit 50A (50B, 50C), the integrated signal is passed through the low-pass filter 51a to obtain the signal Sa, and the integrated signal is passed through the low-pass filter 51b to obtain the signal Sb. The signals Sa and Sb are input to the trigonometric function calculation unit 52 and the rotation locus calculation unit (m calculation unit) 70, respectively. The trigonometric function calculator 52 is a signal
Calculate based on Sa and Sb to calculate the output ρ = arctan (Sb / Sa). The phase calculator 53 calculates (ρ−φ) / 2π based on the input ρ and the initial phase φ.

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.
4b and the data holding unit 73b. The multiplication unit 72a multiplies the previous sampling value from the data holding unit 73a by the current sampling value. The summing unit 74b 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 section 50A (50B), as shown in FIG. 2, the summing section 54 is provided with the m signal of the m arithmetic section 70 and the phase arithmetic section 53.
The sum of the phase signals is calculated and the sum value of the summing unit 54 is multiplied by the pitch (cycle) P of the spatial filter to calculate the moving distance, and the moving distance signal is input to the differential calculating unit 56 to calculate the speed. To calculate.

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 (1) and the current angle θn of the vehicle body 10 becomes θn = θ _n-1 + Δθ (2). 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 formulas (1) to (4) are calculated and the vehicle position (x, y) and direction θ
Ask for.

The two outputs a and b of the spatial filter are, as shown in FIG.
It rotates according to the movement of the car on the phase plane. Therefore, the traveling distance is calculated by the following equation.

X ₀ = (2πm + ρ−P) / nK = m × P + (ρ−φ) / 2πP (5) However, 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 pitch (period) of the spatial filter, ρ = arctan (Sb / Sa), 0 ≦ ρ <2π, P =
ρ _t = 0 (initial value).

Further, by providing a third optical system 20C for detecting the vertical direction with respect to the first and second optical systems 20A and 20B, the vehicle body 1
When 0 moves (shifts) from the traveling course in a direction substantially orthogonal to this traveling course, the current position (X ₀ , Y ₀ ) of the center point A of the bottom surface portion 11 is set, and the third arithmetic processing unit 50C When the calculation result is Y ₁ , the current A (X, Y) point is (X, Y) = (X ₀ , Y ₀ ) + (O, Y ₁ ) ... (6). Therefore, from the relationship between equations (4) and (6),
It is possible to correct the position error that occurs when the vehicle body 10 is displaced in the direction perpendicular to the traveling direction.

7 and 8 show the above-mentioned operation flow. FIG. 7 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 S2). 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 locus on the phase plane is first The number of times of transition between the quadrant and the fourth quadrant is counted with the positive direction of the transition from the fourth quadrant to the first quadrant, and the m operation is executed. 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, the output is input to the microcomputer 90 through the A / D converter, and its value,
Based on the setting pattern calculated in advance and stored in the ROM 93, the arithmetic processing as described above is executed.

G. Effect of the Invention As described above, the present invention provides the vehicle body with the optical system sensor,
The detection signal of the optical system sensor is processed by a one-dimensional filter means, and the two-dimensional position, speed and traveling direction of the vehicle body are detected from the output, so that the running path such as the guide wire and the reflection tape is processed. It is possible to perform non-contact measurement without the need for, and to measure the position and speed accurately without being affected by slippage and wheel wear due to unevenness of the road surface or external force. Furthermore, since the displacement of the vehicle body in a direction substantially perpendicular to the traveling direction can be detected, the current position of the vehicle body can be recognized more accurately.

[Brief description of the 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, 20C ... Third optical system, 30A
...... First video detector, 30B …… Second video detector, 30C
...... Third image detector, 40A ...... First spatial filter, 4
0B: second spatial filter, 40C: third spatial filter, 50A: first arithmetic processing unit, 50B: second arithmetic processing unit, 50C: third arithmetic processing unit, 60A ... ... 1st detection processing part, 60B ... 2nd detection processing part, 60C ... 3rd detection processing part.

 ─────────────────────────────────────────────────── ─── Continuation of front page (56) References JP-A-54-61972 (JP, A) JP-A-61-283873 (JP, A) JP-A-62-105206 (JP, A)

Claims (2)

(57) [Claims] 1. An optical system which is arranged on a bottom surface of a vehicle body so as to be substantially perpendicular to a traveling axis direction of the vehicle body and which captures a pattern of a traveling road surface, and an image pattern which is captured by the optical system. An image detection unit that obtains an image pattern signal by sampling at a predetermined cycle, a video pattern signal of the image detection unit is multiplied by a preset trigonometric function setting signal, and the multiplication signal is integrated to perform trigonometric pattern signal. The position detecting device for an unmanned vehicle, comprising: a filter means for obtaining the above-mentioned information; and an arithmetic processing section for calculating a moving distance signal of the vehicle body by multiplying and adding based on a trigonometric function pattern signal of the filter means. . 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 And a second video detection unit that obtains a video pattern signal by sampling a video pattern captured by the second optical system at a predetermined cycle, and a video pattern signal of the second video detection unit that is set in advance. A second filter means for multiplying the trigonometric function setting signal and performing an integral operation on the multiplied signal to obtain a trigonometric function pattern signal; and a multiplication processing based on the trigonometric function pattern signal of the second filter means While calculating the travel distance signal of the car body,
A second for calculating a velocity signal by differentiating the moving distance signal
Second detection processing means including an arithmetic processing unit, a third optical system that captures a displacement of the vehicle body in a direction substantially perpendicular to the traveling direction of the vehicle body, and a third optical system that captures the displacement. A third video detection unit that obtains a video pattern signal by sampling a video pattern at a predetermined cycle, a video pattern signal of the third video detection unit, and a preset trigonometric function setting signal, and the multiplication signal Is integrated to obtain a trigonometric function pattern signal, and multiplication processing is performed based on the trigonometric function pattern signal of the third filter means to calculate the travel distance signal of the vehicle body and the travel distance. A position detecting device for an unmanned vehicle, comprising a third arithmetic processing unit for obtaining a signal.