JPH0726854B2 - 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. SUMMARY OF THE INVENTION The present invention provides 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, A position detecting device for an unmanned vehicle excellent in detection accuracy by processing a detection signal of an optical system sensor by a one-dimensional spatial filter means and detecting a two-dimensional position of the vehicle body, a speed and a traveling direction of the vehicle body from the output thereof. Is what you get.

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 the Problems The present invention has been made in view of the above-mentioned problems, and is a first structure that is arranged on the bottom surface of a vehicle body and takes a pattern of a traveling road surface.
An optical system, a first image detecting section for obtaining an image pattern signal by sampling an image pattern photographed by the first optical system at a predetermined cycle, and an image pattern signal of the first image detecting section. First filter calculating means for multiplying a preset trigonometric function setting signal and performing integral calculation of the multiplied signal to obtain a trigonometric function pattern signal; and a predetermined optical axis from the first optical system with respect to the axial direction of the vehicle body. A second optical system which is arranged on the bottom surface of the vehicle body at a distance to capture a pattern of a traveling road surface, and a video pattern obtained by sampling the video pattern captured by the second optical system at a predetermined cycle. Based on a second image detecting unit for obtaining a signal, each moving angle of the first image detecting unit and the second image detecting unit with respect to the traveling road surface of the first optical system and the second optical system, and an arc radius. Image pattern signal and preset Second filter calculating means for multiplying the generated trigonometric function setting signal and integrating the multiplied signal to obtain a trigonometric function pattern signal, and the trigonometric function patterns of the first filter calculating means and the second filter calculating means. The moving distance signal of the vehicle body is calculated by performing multiplication processing based on the signal, and the moving distance signal is differentiated to calculate the speed signal, and the moving distance signal, the first optical system and the second The position, speed, and direction of the unmanned vehicle are detected by means for calculating the traveling direction angle of the vehicle body based on the installation interval with the optical system.

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, 20B is a second optical system, 30B is a second image detection unit, and 100 is a processing unit. A detection processing unit is configured. As shown in FIG. 2, the first and second video detection units 30A and 30B are composed of a line sensor 31, a readout circuit 32 and an analog / digital converter (A / D converter) 33.

A microcomputer 90 is used as the processing unit 100, and this microcomputer 90 has a central processing unit (CPU) 9
1, a random access memory (RAM) 92, a read only memory (ROM) 93 and a bus 94. 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 and the setting pattern calculated in advance and stored in the ROM 93 are also stored. Then, the arithmetic processing is executed.

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

As shown in FIGS. 8 and 9, 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.

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 input to the microcomputer 90.

That is, the line sensor 31 is formed by continuously coupling a large number of photoelectric conversion elements in the vehicle traveling direction, detects the illuminance distribution on the irradiation surface by each element, and outputs the electrical output converted by each element as the first and first 2 to the image detectors 30A and 30B.

First, the road surface is irradiated with light to form an irradiation surface. The unevenness of the illuminance is formed on the irradiation surface due to the unevenness of the road surface, and this illuminance distribution is projected on the line sensor through the lens. The lens is configured to collect light having a length P and a width W. The unevenness pattern is projected on the line sensor, and each optical photoelectric conversion element forming the unevenness pattern sends out an electrical output according 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.

Schematically, as shown in FIG. 2, the pattern of the road surface 80 is detected by the image detection unit 30A (30B) via the optical system 20A (20B), and the output thereof is input to the microcomputer 90.

In the microcomputer 90, a product-sum operation is performed using an input value and a setting pattern which is calculated in advance and stored in the ROM 93 in advance. The setting pattern is 30A (30
It has data for a period determined by the number of elements and the pitch of the line sensor 31 in B). The CPU 91 performs arithmetic processing on the basis of this data to calculate the moving distance of the vehicle, and differentiates this moving distance 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 obtained from the difference between the measured travel distances in one cycle of the detection processing unit, and the average value of the change is used for that period. The position of the car body is calculated as if the vehicle was traveling.

As shown in FIG. 3, when the vehicle moves from position F to position G on the XY plane during one sampling, it is considered that the car moves on an arc, and points B and C move during this period. Distance
Letting r and l be the radius R of the circle and the change Δθ in the traveling direction, Δθ = (l−r) / D (3) R = (l + r) · D / (l−r) (4) However, when r> l, the center of the circle comes to the side opposite to that of FIG. 3, and R <0. Further, the current angle θ _n of the vehicle body 10 is θ _n = θ _n-1 + Δθ (5). Here, θ _n-1 is the angle at the time of the previous sampling. The position (x _n , y _n ) of the point A is X _n = X _n-1 + R · sin Δθ · cos θ _n-1 (6) Y _n = Y _n-1 + R · (1-cos θ) · sin θ _{n -1} (7), the moving distance is calculated for each of B and C points,
The position of the vehicle body is calculated by the equations (1) to (7), and the locus of the vehicle is obtained as shown in FIG.

FIG. 5 shows a second embodiment of the present invention, in which 30A is a first image detecting section which receives the image signal of the first optical system 20A as input, and 40A is an image detecting signal of the first image detecting section 30A. First input
50A is a first arithmetic processing unit for performing 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 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 a second detection processing 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 72b, data holding units 73a to 73c that are previous value holding units, and summing units 74a and 74b. Has been done.

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

As shown in FIGS. 8 and 9, 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. The video signals of the optical systems 20A and 20b are respectively detected by the first and second video detection units 30A and 30B and input to the first and second spatial filters 40A and 40B. Spatial filter 40A, 4
0B detects a specific spatial frequency component from an optically random pattern on the road surface and examines its temporal behavior 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) (11) S ₂ = F ・ sin (n ・ k ・ x) (12) 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 + φ) (15) Therefore, the n-th order Fourier coefficients A _n and B _n are obtained as follows.

Similarly, B _n (x ₀ ) = Gcos (nkx ₀ + φ) (17) (16) (17) is substituted into the above equations (13) and (14) and rearranged to obtain the following equation.

a (x ₀ ) = (FL / 2) ・ Gcos (nkx ₀ + φ)… (18) b (x ₀ ) ＝ (FL / 2) ・ Gsin (nkx ₀ + φ)… (19) 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 + ρ (20) where m is the number of revolutions around the origin, ρ = arctan (b (x ₀ ) / a (x ₀ )), (0 ≦ ρ ≦ 2π) φ is the initial value, that is, ρ _t = ₀ .

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π (21) Schematically, as shown in FIG. 6, 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 the pitch of the line sensor 31 of the video 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. 6, 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, the reading circuit 32 reads the video detection signal, A / D converts the video detection signal, and inputs the signal to the spatial filter 40A (40B). Spatial filter 40A (40
In B), the multiplication unit 41a multiplies the video detection signal from the video detection unit 30A (30B) and the sine wave pattern setting signal of the pattern setting unit 42a, and the multiplication unit 41b multiplies the video detection signal and the pattern setting unit 42b. Multiply the cosine wave pattern setting signal. 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), 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 via the sine wave signal calculation unit 71a, and the signal Sb is added to the summation unit 74b via 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 74 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. 6, 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 FIGS. 5, 3, and 4, the center point of the bottom surface portion 11 of the vehicle body 10 is A (x, y), and when the vehicle is traveling in the arrow direction, the vehicle is XY. It is assumed that the object moves on the plane at an angle of θ with respect to the X axis. Here, if the outputs of the first and second detection processing units 60A and 60B by 1 sampling change by Δr, Δl (position), the change Δθ in the traveling direction of the vehicle body 10 is
Assuming that the sampling cycle is short, the above equations (15) to (2
1) Execute the calculation of expression.

10 and 11 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. 10, in step S1, 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. 11, 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.

G. Effect of the Invention As described above, according to the present invention, the vehicle body is provided with the optical system sensor, and the detection signal of the optical system sensor based on the movement angle and the arc radius of the vehicle body is processed by the one-dimensional filter calculation means, and the output thereof is output. Since the two-dimensional position, speed, and direction of travel of the vehicle body are detected from the vehicle, it is possible to perform non-contact measurement without the need to process the running path such as guide wires or reflective tape, and to measure road surface irregularities and external forces. It is possible to measure the position and speed with high accuracy without being affected by slips and wheel abrasion due to factors such as.

[Brief description of drawings]

1 is a block diagram of a position detecting device according to a first embodiment of the present invention, FIG. 2 is a block diagram showing details of a detection processing unit according to an embodiment of the present invention, and FIGS. 3 and 4 are calculation methods. FIG. 5, FIG. 5 is a block diagram of a position detecting device according to a second embodiment of the present invention, FIG. 6 is a block diagram showing details of a detection processing unit, and FIG. 7 is a block diagram of a rotation locus calculation unit. ,
8 and 9 are block diagrams of the optical system, FIG. 10 is a calculation flow chart of the spatial filter, and FIG. 11 is a calculation flow chart of the calculation 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, 90 ... Microcomputer, 100 ... Processing unit.

Claims (1)

[Claims] 1. A first optical system arranged on the bottom surface of a vehicle body for capturing a pattern of a traveling road surface, and a video pattern obtained by sampling a video pattern captured 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. A first filter calculation means, and a second optical system arranged on the bottom surface of the vehicle body at a predetermined distance from the first optical system with respect to the axis of the vehicle body in the traveling direction, and capturing a pattern of a traveling road surface. 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 the first video detection unit and the second video detection unit. Of the first optical system and the second optical system Second filter calculation means for multiplying a video pattern signal based on each movement angle and arc radius with respect to the road surface by a preset trigonometric function setting signal, and integrating the multiplication signal to obtain a trigonometric function pattern signal; Multiplication processing is performed based on the trigonometric function pattern signals of the first filter calculation means and the second filter calculation means to calculate the travel distance signal of the vehicle body, and the travel distance signal is differentiated to calculate the speed signal. In addition, the position detection of the unmanned vehicle is configured by means for calculating the traveling direction angle of the vehicle body based on the movement distance signal and the installation interval between the first optical system and the second optical system. apparatus.