JPH08178651A - Azimuth detecting device for moving body - Google Patents

Abstract

PURPOSE: To enable the azimuth of a moving body to be detected on a ground side with high precision while avoiding complicated control configuration by separately providing plural light emitting bodies for emitting light with their different natures, respectively. CONSTITUTION: Plural light emitting bodies T0 to T3 provided with electric bulbs 20 are installed on a work vehicle A (moving body). At its center, light emitting bodies T1, T2, and T3 are installed, respectively, at each vertex of a right triangle. The four light emitting bodies T0 to T3 are thereby separately provided within the installation range against a reference position (car body center). Each light with their different natures (deflecting status) is emitted from the four light emitting bodies T0 to T3. Each light enters into an image pickup part 15 of an image pickup unit S on the ground side passes through the four light transmitting means, respectively, at a transmission rate differing depending on the difference in the nature of each light, and each image is picked up by means of image sensors S0 to S3. The azimuth of the work car A is obtained by means of calculation from position information, image pickup angle information, etc., on these image pickup screens, respectively.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an azimuth detecting device for detecting the azimuth of a moving moving body.

[0002]

2. Description of the Related Art Conventionally, in such an azimuth detecting device for a moving body, on the ground side, while setting the direction of a work process in which an automatic traveling work vehicle or the like as a moving body travels as a reference azimuth, for example, using a geomagnetic field, An azimuth detecting means such as an azimuth sensor is installed on the work vehicle side to detect the azimuth of the moving body (that is, the direction of the vehicle body) with respect to the reference azimuth. Incidentally, this vehicle body orientation information is transmitted, for example, via the communication means from the moving body side to a control device or the like for measuring the position of the moving body and guiding traveling provided on the ground side, and for example, a light bulb provided on the moving body side. The position information of the moving body detected by the control device or the like on the ground side based on the information obtained by imaging the light-emitting body such as The vehicle body direction information transmitted from the body is used as control information for guiding and moving the moving body along the set route.

[0003]

However, in the above-mentioned prior art, when an azimuth sensor or the like is installed on the moving body side and the azimuth information is sent to the ground side, communication means is provided on both the moving body side and the ground side. However, in the case of a azimuth sensor using geomagnetism, for example, it is easy to be affected by surrounding metal bodies, and for example, the detection accuracy as control information for guiding travel is low. There was a problem that it was not enough.

The present invention has been made in view of the above circumstances, and an object thereof is to eliminate the problems of the prior art while avoiding complication of the control configuration (particularly on the moving body side) and on the ground side. It is to be able to detect the direction of a moving body with the highest possible accuracy.

[0005]

According to a first characteristic configuration of an apparatus for detecting a direction of a moving body according to the present invention, a plurality of light emitting bodies, which emit light having different properties from each other, are a reference in plan view. Depending on the difference in the nature of each light from the plurality of light emitters, it is provided in a distributed manner according to the set arrangement relationship with respect to the position, and is provided on the ground side so as to correspond to each light from the plurality of light emitters. , A plurality of light passing means for passing corresponding light among them in a state of having a higher transmission rate than other light, and a plurality of imaging for individually photographing each light passing through each of the plurality of light passing means Means, imaging angle detection means for detecting an angle formed by the imaging directions of the plurality of imaging means with respect to a reference azimuth, and distance detection for detecting a distance from the plurality of imaging means to a reference position of the moving body. Means and the plurality of light emitters Arrangement information, position information in people imaging screen each of said plurality of image pickup means of the plurality of light emitters,
An azimuth calculating unit that calculates the azimuth of the moving body based on the distance information of the distance detecting unit and the image capturing angle information of the image capturing angle detecting unit is provided.

A second characteristic configuration is the same as the first characteristic configuration, wherein each of the plurality of light emitters is provided with a polarization means for polarizing the light emitted from the light emitters in different polarization angle directions. The plurality of light passing means is composed of a plurality of light detecting means for detecting light from the plurality of light emitting bodies polarized in the different polarization angle directions in different detection angle directions. It is in.

A third characteristic configuration is the first or second aspect described above.
In the characteristic configuration of (3), the plurality of image pickup means are configured to perform image pickup operations in a synchronized state.

According to a fourth characteristic configuration, in the first, second, or third characteristic configuration, based on the image information of at least one of the plurality of image pickup means, a predetermined image in the image pickup screen is obtained. A tracking control means for activating the imaging angle changing means of the imaging means is provided so as to catch the corresponding luminous body of the plural luminous bodies at the position.

A fifth characteristic configuration is the same as the fourth characteristic configuration, wherein each of the plurality of light passing means is attached integrally to each of the plurality of image pickup means, and the image pickup is performed. The angle changing means is configured to change the angle of the plurality of image pickup means provided with the plurality of light passing means as one body.

[0010]

According to the first characteristic configuration of the moving body azimuth detecting apparatus according to the present invention, a plurality of light emitting bodies distributed in the moving body in a set arrangement relationship with respect to the reference position in plan view have different properties. Light is emitted, and on the ground side, each light from the plurality of light emitters passes through each of the plurality of light passing means corresponding to each light, and the other light depending on the property of each light. The light that has passed through at a higher pass rate than that of the above, and each of the lights that have passed through at the high pass rate is individually imaged by a plurality of imaging means. Further, on the ground side, the angle formed with respect to the reference azimuth of the plurality of image pickup means and the distance from the plurality of image pickup means to the reference position of the moving body are detected, and the installation information of the plurality of light emitters, The azimuth of the moving body is calculated from the position information of each of the plurality of light emitting bodies on each of the image pickup screens of the plurality of image pickup means, the image pickup angle information, and the distance information.

According to the second characteristic configuration, in the first characteristic configuration, the plurality of light emitting bodies emit light having different properties from each other by the polarizing means provided in each of the plurality of light emitting bodies. Lights polarized in different polarization directions are emitted, and on the ground side, lights from a plurality of light emitters polarized in different polarization directions are provided corresponding to the respective lights. The light passes through a plurality of light detecting means that detect light in different light detecting angle directions with a greater pass rate than other light due to the difference in the polarization angle direction of each light, and a plurality of image capturing means are provided separately. It is imaged. Then, the installation information of the plurality of light emitters, the position information of each of the plurality of image pickup means of the plurality of light emitters on the image pickup screen, and the image pickup angle information,
The azimuth of the moving body is calculated from the distance information.

Further, according to the third characteristic constitution, in the first or second characteristic constitution, the light passage means is made to have a greater passage ratio than the other light among the respective lights from the plurality of light emitters. Each light passing through is imaged separately by a plurality of imaging means in a synchronized state.

According to the fourth characteristic configuration, the first,
In the second or third characteristic configuration, the image capturing angle of the image capturing unit is set so that the light emitting body corresponding to the image capturing unit is captured at a predetermined position in the image capturing screen of at least one of the plurality of image capturing units. The tracking control to change is performed.

According to a fifth characteristic configuration, in the fourth characteristic configuration, each of the plurality of light passing means is attached integrally to each of the plurality of image pickup means. Of at least one of the image pickup means, the angle of the plurality of image pickup means is integrally changed so that the light emitting body corresponding to the image pickup means is captured at a predetermined position in the image pickup screen of the image pickup means. Tracking control for changing the imaging angle is performed.

[0015]

According to the first characteristic construction of the present invention, the difference in the above-mentioned property of light on the ground side can be obtained only by installing a plurality of light-emitters which emit light having different properties in the moving body. Since it is possible to detect the azimuth of a moving body from information such as multiple pieces of imaging information that distinguishes and separates each light, install a direction sensor etc. on the moving body side as in the past, and also obtain the azimuth information from the ground. When sending to the side, the disadvantage that the control configuration becomes complicated because communication means is provided on both the mobile side and the ground side, or in the direction sensor using geomagnetism etc., the detection accuracy is affected by the surroundings. It is possible to detect the azimuth of the moving body with the highest accuracy while avoiding the complication of the control configuration without the problem that the accuracy of the control information for guiding and traveling is insufficient.

Further, according to the second characteristic construction, each light-emitting body is provided by a simple means in which the light-emitting body side is provided with polarization means such as a polarization filter and the image pickup means side is provided with similar light-detection means such as a polarization filter. It is possible to emit light having different properties from each other, and to detect the azimuth of the moving body from information such as a plurality of pieces of imaging information in which each light is distinguished and separated according to the difference in the properties of the light. Suitable means for implementing the characteristic construction of

Further, according to the third characteristic configuration, since the azimuth detection of the moving body is performed by using the plurality of image pickup information having no time error due to the synchronous operation, for example, the plural azimuths obtained by the asynchronous operation are obtained. When the azimuth detection of the moving body is performed using the image pickup information, under the condition that the moving speed of the moving body is high, there is an error in the position of each light emitting body in each image pickup screen due to the movement of the moving body within the image pickup time difference. In addition, the problem that the detection accuracy is lowered does not occur, and therefore, suitable means for implementing the first or second characteristic configuration can be obtained.

Further, according to the fourth characteristic configuration, the light-emitting body corresponding to a predetermined position (for example, the center of the screen) of at least one of the plurality of imaging means is tracked so as to be tracked. In the image pickup screen of other image pickup means, the corresponding light emitters are not located at the screen edges but are located near the center of the screen where the position detection accuracy is relatively high.
It is possible to detect the direction of a moving object with high accuracy.
Suitable means for implementing the first, second or third characteristic configuration can be obtained.

Further, according to the fifth characteristic configuration, since each of the plurality of light passing means is attached integrally to each of the plurality of image pickup means, for example, the plurality of light passing means are provided. In the case where the plurality of image pickup means is not in an integrated state, when the image pickup angle of the image pickup means is changed for the tracking control, the light passing through the plurality of light passage means is properly incident to the plurality of image pickup means. While there is a possibility that the image is not picked up by the image pickup means, each light can be picked up by the image pickup means in a good state without such a problem. Therefore, when the fourth characteristic configuration is performed, Suitable means are available.

[0020]

Embodiments of the present invention will be described below with reference to the drawings. As shown in FIG. 1 and FIG. 2, a plurality of light emitters provided with a circular light bulb 20 as a light emitting unit are installed in an automatic traveling work vehicle A (for example, a work vehicle for rice planting) as a moving body. Has been done. Specifically, one light-emitting body T0 is installed in the upper center of the vehicle body of the work vehicle A, and the light-emitting body T0 is located at the center of the vehicle. Two light emitters T2, T3 are installed in the vehicle body, and one light emitter T1 is installed at each apex of the equilateral triangle on the front side of the vehicle body. As a result, four light emitters T0, T1, T2, T3 are provided.
However, in a plan view, they are distributed in a set arrangement relationship with respect to a reference position (in this example, the center of the light emitting body T0 at the center of the vehicle body is the reference position). The distance between the centers of the light emitter T0 at the center of the vehicle body and the other light emitters T1, T2, T3 is r.

Further, on the outer peripheral surface of the cylinder 19 which supports the light emitter T0 in the center of the vehicle body, the beam light b for distance detection projected from the laser length measuring device 13 on the ground side is reflected in the incident direction. The reflection sheet 19a is formed. The cylinder 19 has three light-emitting bodies T whose outer peripheral surfaces are located around the circumference.
1, T2, T3 are formed in such a diameter that they are inscribed on the respective sides of the equilateral triangle, so that when the four light emitters T0, T1, T2, T3 are viewed from the ground side, Light emitters T0, T1, T located on the front side in the viewing direction
Only the light emitting bodies 2, 2 and T3 are visible, and the light emitting body located on the back side in the viewing direction is hidden by the cylinder 19 and disappears, and the light emitting body located on the front side in the viewing direction is selected as the light emitting body to be selected in the processing for detecting the vehicle body direction described later. The body can be selected from T0, T1, T2, T3 (see FIGS. 6 and 9).

The four light emitters T0, T1, T2, T3
Therefore, in order to emit lights having different properties (polarization states described later), the respective light-emitting members T0, T1, T2,
Cylindrical polarizing filters f0, f1, f2, and f3 as polarizing means for polarizing the respective lights from T3 in different polarization angle directions are provided at the respective outer peripheral positions of T3. Specifically, as shown in FIG. 3, the light from the light emitter T0 in the center of the vehicle body is linearly polarized in the vertical direction (this is 0 °) by the polarization filter f0, and the other three light emitters T0.
Each light from 1, T2, T3 is converted into each polarization filter f1,
At f2 and f3, the directions are sequentially rotated by 45 ° from the polarization direction of the light emitting body T0, that is, 45 °, 90 °, 135.
It is linearly polarized in the direction of °.

On the other hand, on the ground side, the position of the work vehicle A is measured while the work vehicle A is being tracked, and the position of the work vehicle A is automatically tracked for guiding and traveling along a predetermined route. The control device B is installed. In the control device B, an imaging device S for imaging a predetermined range including the light emitters T0, T1, T2, T3, and a state in which the projection direction of the detection light b is set along the imaging direction of the imaging device S. The laser length-measuring device 13, which detects the distance from the image pickup device S to the cylinder 19 on the side of the work vehicle A, is fixedly installed on the installation table 9 as an integral unit. From the above, the laser length measuring device 13 causes the imaging device S (a plurality of imaging means S0,
Distance detection means for detecting the distance from S1, S2, S3) to the reference position of moving vehicle A (that is, cylinder 19) is configured.

A swivel motor 11 for swiveling the installation table 9 around the vertical axis θ and a swing motor 12 for swiveling the installation table 9 around the horizontal axis φ are provided. Imaging angle changing means 11 for changing the imaging angle of the imaging device S by means of 11 and a swinging motor 12.
12 are configured. The turning motor 11 and the swinging motor 12 have encoders 11a and 12a built therein (see FIG. 5).
The imaging direction of the imaging device S is detected based on the information of 12a. Therefore, the encoder 11
a, 12a, an angle formed by the imaging direction of the imaging device S (a plurality of imaging means S0, S1, S2, S3 described later) with respect to a reference azimuth (for example, the direction K of the work process shown in FIG. 11). The imaging angle detection means 11a and 12a for detecting are comprised.

As shown in FIG. 4, the image pickup device S includes
An electric zoom lens 14 forming an optical system, and an image pickup unit 15 including a monochrome CCD sensor equipped with an electronic shutter
Are provided. The electric zoom lens 14 includes a focus drive motor 33, a first potentiometer 34 which is a focus position detection sensor, and a zoom drive motor 3.
5 and a second potentiometer 36, which is a zoom position detection sensor, are provided, and the light passing through the electric zoom lens 14 and the optical filter 24 provided on the rear side of the electric zoom lens 14 is
It is configured to enter the image pickup section 15 and form an image on the CCD element 27 which is an image pickup element. The optical filter 24 has a large transmittance with respect to the emission wavelength of the light bulb 20.

The light incident on the image pickup unit 15 is transmitted and reflected by the first prism 37A for light separation with a transmittance and a reflectance of 50% each, and further, each transmitted light and reflected light is second and third. Part of the light is transmitted and reflected by the third prisms 37B and 37C, respectively, and a part of the light is reflected by the mirror 39 to be finally separated into four lights having the same light intensity. Then, each of the separated lights includes a plurality of image sensors S0, S.
1, S2, S3 passing through the polarization filters p0, p1, p2, p3 for detection provided on the front side of each CCD element 2
7 is configured to be incident on.

Here, the polarizing filters p0, p1,
The p2 and p3 are configured to detect the lights from the respective light emitters T0, T1, T2 and T3 which are polarized in different polarization angle directions on the side of the work vehicle A in different detection angle directions. . Specifically, in order to allow the light from each of the light emitters T0, T1, T2, and T3 to pass through at the maximum transmission rate, the polarization angle directions of the polarization filters p0, p1, p2, and p3 are The polarization angle directions of the corresponding polarization filters f0, f1, f2, and f3 on the side of the work vehicle A shown in FIG.

From the above, the polarization filters p0, p
1, p2, p3, the four light emitters T0, T1, T
2 and T3 are provided so as to correspond to the respective lights, and the corresponding lights among the four light emitters T0, T1, T2, and T3 are different from each other depending on the property of each light, that is, the polarization state. A plurality of light passing means for passing the light in a state where the light passing rate is larger than that of the light are configured, and a plurality of image sensors S are provided.
At 0, S1, S2, S3, a plurality of image pickup means for individually taking an image of each light passing through each of the four light passing means (polarization filters p0, p1, p2, p3) is configured. The plurality of light passing means p0, p1, p2, p3
And a plurality of image sensors S0, S1, S2 and S3 are integrally formed. Then, the imaging angle changing means 11 and 12 are provided with the plurality of light passing means p in the integrated state.
0, p1, p2, p3 and a plurality of image sensors S0,
The angles of S1, S2 and S3 are integrally changed.

Next, referring to FIG. 5, the control configuration of the control device B will be explained.
9, image sensor controller 50, laser length measuring device controller 51, and these controllers 49, 5
A main controller C for controlling 0, 51 is provided. The image pickup angle controller 49 receives the image pickup angle information of the image pickup device S from the encoders 11a and 12a, and outputs drive signals for the turning motor 11 and the swinging motor 12. The image sensor controller 50 includes the image pickup information from each of the image sensors S0, S1, S2, and S3 and the first image information.
And the detection information of the second potentiometers 34 and 36 are input, and a drive signal for operating the image sensors S0, S1, S2 and S3 in a synchronous state, and a drive signal for the focus drive motor 33 and the zoom drive motor 35. And are output. Laser length measuring device controller 51
The distance detection information from the laser length measuring machine 13 is input to the main controller C, and the transceiver 10 for communicating with the work vehicle A is connected to the main controller C.

Using the image sensor controller 50, each image sensor S0, S1, S2 of each light emitter T0, T1, T2, T3 is based on the image pickup information of each image sensor S0, S1, S2, S3. Image processing means T for detecting the position on each of the image pickup screens in S3 is configured. That is, as shown in FIG. 6, with the center point O of the imaging screen 29 of each image sensor as the origin, the horizontal direction of the screen is x ′.
Axis, screen vertical direction is set to y'axis, each light emitter T0, T
The circular regions corresponding to 1, T2, T3 (actually, the light bulb 20) are extracted by the binarization process, and the center of gravity position G thereof is used as the position information on the image pickup screen of each light emitter. Here, the polarization filters f0, f1, f2, f3 on the light emitting side shown in FIG.
And each polarization filter p0, p1, p2, p3 on the light receiving side
From the relationship of the polarization angle direction, the images from T1 and T3 other than T0 appear lighter in the image (a) of the image sensor S0, but they are erased by appropriately setting the threshold value of the binarization process. . Other image sensors S1, S2, S3
In each of the screens (b), (c), and (d), unnecessary image information of the light emitter is deleted in the same manner. In addition, FIG.
By the tracking control described later, the center of gravity G of the light emitter T0 corresponding to the center of the image sensor S0 in the horizontal direction of the screen is positioned,
An ideal state in which there is no tracking error is illustrated.

The main controller C is configured to change and adjust the imaging magnification of the imaging device S based on the distance information to the work vehicle A by the laser length measuring device 13. That is, by moving the lens position of the electric zoom 14 so that the imaging magnification is increased when the distance to the work vehicle A is increased, and the imaging magnification is decreased when the distance to the work vehicle A is shortened, light emission in the imaging screen is performed. Body T0, T
The sizes of 1, T2 and T3 are maintained at appropriate sizes. More specifically, as shown in FIG. 7, (the vertical axis in the figure represents the drive voltage (V) of the zoom drive motor 35).
From the standard characteristic M of the distance to the lens position,
When the distance to 0 is L (m), the drive voltage for setting the proper lens position is d (V), and the drive voltage should be within ± 1 / 2Δd above and below this proper drive voltage d (V). Control. Further, when the moving direction of the light emitting body T0 in the image pickup screen of the image sensor S0 is the lateral direction, the control path 40 along the standard characteristic M is used for control, but when the moving direction is the perspective direction, That is, when the distance changes without changing the position of the light emitter T0 in the imaging screen, the control paths 41 and 42 (41 is moved to the far side) more rapidly than the control path 40 along the standard characteristic M. Time,
42 is a route when moving to the approaching side). This minimizes the delay in tracking control.

Further, by using the main controller C, the arrangement information of the plurality of light emitters T0, T1, T2, T3 and the plurality of light emitters T by the image processing means T.
The plurality of image sensors S of 0, T1, T2, T3
The orientation of the work vehicle A (the direction of the vehicle body) based on the position information of the image pickup screens 0, S1, S2, and S3, the distance information of the laser length measuring device 13, and the image pickup angle information of the encoders 11a and 12a. ), And the plurality of image sensors S0, S1,
Based on the image pickup information of at least one of S2 and S3 (in this example, the image sensor S0), the plurality of the plurality of images are provided at a predetermined position in the image pickup screen, specifically, in the lateral center position of the image pickup screen 29. Individual light emitters T0, T1, T2, T3
The tracking control means 101 for activating the turning motor 11 and the swinging motor 12 is configured so as to catch the corresponding light emitter T0.

The azimuth calculating means 100 will be described with reference to FIG. First, the distances in the lateral direction of the screen between the position of the light emitter T0 at the center of the vehicle body and the positions of the other three light emitters T1, T2, T3 are respectively obtained, and the minimum distance Df of them is calculated.
Select a light emitter T3 (represented by the number of pixels) (see FIG. 6). Here, one of the light emitters T1, T2, T3 is located on the rear side with respect to the imaging direction of the image sensors S0, S1, S2, S3, and the light emitter and the light emitter T0 at the center of the vehicle body are Even if the distance in the horizontal direction of the screen is minimized, as described above, the light emitters on the rear side are actually hidden behind the cylinder 19 and cannot be seen. Therefore, the light emitters T1, T2, T3 located on the front side are not visible. Error will not occur in the calculation process described later. Then, the distance Df in the lateral direction of the screen between the selected light emitter T3 and the light emitter T0 at the center of the vehicle body is converted into an angle difference θg by the following equation. In the formula, G
H is the horizontal field angle of view of the image sensors S0, S1, S2, S3, and RES is the image sensors S0, S1, S2.
It is the resolution (represented by the number of pixels) in the horizontal direction of the screen in S3. In addition, in the following equation, it is replaced with α for later calculation.

[0034]

## EQU1 ## θg = (GH / RES) × Df α = tan (θg)

Next, the above α (angle difference θg), the distance r between the light emitter T0 and the light emitter T3 at the center of the vehicle body, and the image sensor S.
0, S1, S2, S3 from the position of the light emitter T0 in the center of the vehicle body
From each information of the distance Dt to the image sensor S0, S
Rotation center Tc of 1, S2 and S3 and light emitter T0 in the center of the vehicle body
The light emitter T0 and the light emitter T3 in the center of the vehicle body with respect to the straight line connecting
The angle θt formed by the line connecting the lines is calculated by the following formula.

[0036]

[Formula 2] δ = (Dt × α ² + [Dt ² × α ⁴ −β × [α ² × Dt
² −r ² ]] ^1/2 ) / (β × r) where β = α ² +1 θt = cos ⁻¹ (δ)

The angle formed by the line connecting the light emitter T0 in the center of the vehicle body and the light emitter T3 with respect to the front-rear direction of the vehicle body is the angle (1 between the light emitter T0 in the center of the vehicle body and the two light emitters T2, T3).
Since the angle is 60 °, which is a half of 20 °), the angle formed by the vehicle front-rear direction of the work vehicle A with respect to the reference azimuth K on the ground side, that is, the vehicle azimuth is θH. An angle (60−θt) minus θt, an imaging angle θC of the image sensors S0, S1, S2, S3, and a light emitter T0 corresponding to the center of the image sensor S0 in the horizontal direction of the screen.
If the center of gravity G is not located, the angle ΔθC corresponding to the deviation X in the horizontal direction of the screen is obtained as an angle. Note that FIG. 9 shows a case where ΔθC is 0.

[0038]

[Equation 3] θH = θC + (60−θt) + ΔθC

Explaining the tracking control means 101, it takes a predetermined processing time to detect the position in the screen by the image processing means T. Therefore, as shown in FIG. 8, the position of the light emitter T0 (in the figure, (Indicated by the angle around the vertical axis θ) is obtained as the position at discrete detection points at intervals. In the figure, the light emitter T0 indicates that the current angle detection time t
It is located at an angle θ2 around the vertical axis θ at an angle detection time t2 two points before 0, and at an angle θ1 around the vertical axis θ at an angle detection time t1 one before the current angle detection time t0. It is assumed that the vehicle is moving on the locus J so as to be positioned at the angle θ0 around the vertical axis θ at the current angle detection time t0. It should be noted that the angle around the vertical axis θ is positive in the clockwise direction when viewed from above, and the angular position around the horizontal axis φ of the light emitter T0 does not change. From the above, the rate of change θv of the angular position around the vertical axis θ of the light emitting body T0 from the angle detection time t1 immediately before the current angle detection time t0 to the current angle detection time t0 is given by the following equation.

[0040]

## EQU00004 ## .theta.v = (. Theta.0-.theta.1) / (t0-t1).

Therefore, in order to catch the light emitting body T0 at the center position in the horizontal direction of the screen at the angle detection time point next to the current angle detection time point t0, the deviation in the x'-axis direction detected by the image processing means T. The correction amount with respect to X, the rate of change θv of the angular position around the vertical axis θ, and the work vehicle A in order to follow the set route (the route facing the direction K of the work process). The turning amount Δθ around the vertical axis θ is determined by the following equation in consideration of the information on the steering angle θst transmitted to the turning motor 11 with the turning amount Δθ.
To operate. Note that a1, a2, and a3 are predetermined gain coefficients (a1> 0, a2> 0, a3> 0).

[0042]

[Formula 5] Δθ = a1 · X + a2 · θv + a3 · θst

Then, the main controller C informs the information of the distance Dt to the work vehicle A and the information of the imaging angle θC of the imaging device S (of the light emitter T0 corresponding to the center of the image sensor S0 in the horizontal direction of the screen). When the center of gravity G is not located, the angle Δθ corresponding to the deviation X in the horizontal direction of the screen.
The position of the work vehicle A is calculated based on the angle C plus the angle C).

Next, referring to FIG. 10, a control configuration on the side of the work vehicle A will be described. A work vehicle controller H using a microcomputer is provided, and the work vehicle controller H is provided with the transceiver 10 on the ground side. A transmitter / receiver 16 on the mobile body side for transmitting / receiving information to and from is connected. From the work vehicle controller H, the traveling motor 17 for shifting the traveling transmission device 18, the steering motor 21 for operating the steering device 22, and the working device 25 are operated (for example, in a rice transplanter). A drive signal is output to the working device actuator 26 for raising and lowering the planting device, which is the working device in this case). The work vehicle controller H is configured to steer the work vehicle A based on the reception information of the transceiver 16 (that is, the steering control information transmitted from the ground transceiver 10).

On the ground side, as shown in FIG. 11, a work vehicle A is guided to travel while planting seedlings and the like.
A rectangular field F of the section is provided. The work vehicle A is adjacent to each other at the end of each work stroke while running along a plurality of work strokes juxtaposed in the short-side direction (y-axis direction) along the longitudinal direction (x-axis direction) of the field F. 1 in work process
It is guided and controlled so as to travel through the entire range of the field F by repeating the reciprocal traveling in which the vehicle travels by turning 80 degrees and traveling in the opposite direction in the working stroke. In the figure, k0 is the field F
Is the entrance and exit of the work vehicle A.

One end side in the longitudinal direction of the field F (k in the figure)
2 and k3), reference poles P1 and P2 for displaying the reference position are provided at predetermined positions near both corner positions k2 and k3 of the field. As shown in FIG. 12, each of the reference poles P1 and P2 is composed of a columnar light reflecting plate 28 supported by a support 28A at a predetermined height above the ground. Like the reflection sheet 19a provided on the work vehicle A, it has a characteristic of reflecting incident light in the incident direction.

The end of the field F in the longitudinal direction on the side opposite to the side on which the reference poles P1 and P2 are installed (k in the figure).
The controller B is installed on the side indicated by 1, k4). Then, in the control device B, the reference poles P1 and P2 are
The reference position displayed by, that is, the reference position detecting means PK for detecting the arrangement position of the reference poles P1 and P2 set and set in advance, and the ground side information including the arrangement position of the reference poles P1 and P2. Information storage means R for storing information
Are provided. Then, the reference position detection means P
K is the laser length measuring device as an optical distance detecting means for detecting the distance to the light reflecting plate 28 in the state where the projection direction of the detection light is set toward the light reflecting plate 28 of the reference poles P1 and P2. 13 and is configured by using the laser length measuring device controller 51 and the main controller C, and the ground side information storage means R is configured by using the main controller C (FIG. 5). reference).

As the above ground side information, the reference pole P
1, P2 coordinates, each corner k1, k2, k3, k of the field F
The coordinates of No. 4, the position of the entrance / exit k0, the information of the work stroke (stroke width, stroke length, the number of strokes, etc.), the coordinates of the default position kd (standard work start position) for the work vehicle A, and the like are stored. Therefore, based on the detection information of the reference position detection means PK and the storage information of the ground side information storage means R, that is, for the installation position information of the reference poles P1 and P2 and the reference poles P1 and P2 which are known in advance. The installation position of the control device B is discriminated from the distance information thus detected, and the position of the work vehicle A is measured on the basis of the discriminated self position.

Next, based on the flow charts shown in FIGS. 13 and 14, the position measurement of the work vehicle A by the controller B is performed,
A specific configuration of the guided traveling of the work vehicle A will be described.

In the main routine (FIG. 13), first,
The control device B is installed in a field, and its installation position is usually near the midpoint of the sides indicated by k1 and k4 as shown in FIG. Next, the map file of the current field is selected and read from the ground side information (hereinafter referred to as a map file) stored in the ground side information storage means R. Then, each reference pole P is set by the laser length measuring machine 13.
1, the distance to P2 is detected, and based on the distance information and the map file information, the above-mentioned reference position detecting means P
The installation position of the control device B is recognized by K.

Next, the image pickup device S (image sensor S
0) The image pickup screen is operated so as to capture the light emitter T0 at the center of the vehicle body of the work vehicle A at the left and right centers, and the vehicle body direction and the vehicle body position are detected. Read) and calculate the deviation between the track and the car body position. And the orbital deviation is a set value (for example, 1 m)
If it is not within the range, the next orbital data is read, and the orbital data is sequentially read and the orbital displacement is calculated until the orbital displacement falls within the set value. Here, even if all track data in the file is read, if the track deviation does not fall within the set value, the current car body position is judged to be inappropriate, so move it manually to the track you want to guide, or The vehicle body position is changed by moving the vehicle body to the default position kd. And the work vehicle A after the movement
The vehicle body azimuth and vehicle body position are detected, the orbit data is read and the orbit deviation is calculated again, and when the orbit data within which the orbit deviation falls within the set value is found, the orbit, the vehicle body direction and the vehicle position are not shown. Display on TV monitor, etc.
Looking at this display, when the operator or the like commands the start of guidance, the vehicle guidance along the trajectory is started, while if the guidance is not started, the flow from the change of the vehicle position is repeated.

In the vehicle body guidance (FIG. 14), the trajectory data is set in the calculation buffer and then the laser length measuring device 1 is used.
3 (abbreviated as PSR) and data to the image processing means T (abbreviated as VSX). Here, since processing of VSX takes time, after waiting for the time until the processing is completed, the distance data from the PSR and the in-screen position data from the VSX, and the imaging angle controller 49 to the encoders 11a and 12a. And the imaging angle data of. Then, the azimuth angle is discriminated from the in-screen position data and the imaging angle data, the vehicle body position is calculated from the azimuth angle and the distance data, and as described above, the vehicle body azimuth angle θH with respect to the reference azimuth K. Is calculated, a control amount for automatic tracking is obtained, and the tracking operation is performed with the control amount.

Next, it is checked whether or not the calculated vehicle position is located at the end of the track. If it is not the end of the track, the steering angle θst of the vehicle body is calculated from the vehicle body azimuth angle θH and the deviation of the vehicle body position with respect to the track, and the data of the steering angle θst and other gear shifting operations and working machines (planting devices, etc.) are calculated. ) Control information for operation is provided to work vehicle A
Send to. Then, on the vehicle body side, control of steering, gear shifting, and work implement operation is performed according to the received control information. On the other hand, if it is the end of the track, the next track data is read, and the flow from the above steering angle calculation is repeated based on the track data. When the processing for the data in the file has been completed by the reading of the next track data, the processing for guiding the vehicle body is completed, and the process returns to the main routine.

[Other Embodiments] Next, other embodiments will be listed. In the above embodiment, a plurality of light emitters T0, T1, T2, T3
Polarizing means (polarizing filters f0, f1, f2) for causing each light-emitting body to polarize the light from the light-emitting portion such as a light bulb in different polarization angle directions so as to emit light having different polarization states as different properties of the light from the , F3) are shown, but the means for emitting lights of different properties are not limited to this. For example, the wavelengths of the respective lights may be different from each other, and in this case, as a means, a filter that allows only a part of the wavelengths of a wide range of wavelengths included in the light of a light bulb or the like (that is, a color filter) ) With different passing wavelengths (eg red, blue,
A plurality of materials (of colors such as green and yellow) are prepared, and each filter is provided in each light-emitting body instead of the above-mentioned polarizing means (polarizing filter). Even in the case of polarization in polarization directions different from each other, the specific conditions are not limited to those in which the polarization angle directions are shifted by 45 ° as in the above embodiment.
It can be appropriately changed according to various conditions such as the number of light emitters.

Further, in the above embodiment, four light emitters T0,
One of T1, T2, and T3 was placed at the center of the vehicle body to be the reference position, and the other three were distributed around the other. However, the arrangement state of the light emitters depends on the number of light emitters, the shape of the vehicle body, and the like. It can be variously changed according to. Further, in the above-described embodiment, the light emitters to be selected when detecting the vehicle body direction can be selected from the light emitters located on the front side in the viewing direction when the four light emitters are viewed from the ground side. Therefore, the diameter of the cylinder 19 is set such that the cylinder 19 supporting the luminous body T0 at the center of the vehicle body hides the luminous body located on the back side in the above-mentioned viewing direction. You may make it hide the light-emitting body of the back side by a means.

In the above embodiment, a plurality of light emitters T0, T
A plurality of light passing means provided on the ground side corresponding to the light from 1, T2 and T3 are aligned with the polarizing means, and the light polarized in different polarization angle directions is detected in different detection angle directions. Light analyzing means (polarizing filters p0, p1, p2,
However, in the case where each illuminant is provided with a color filter instead of the polarizing means (polarizing filter) as described above, the light passing means is also constituted by the same color filter. Even in the case of being configured by the light detecting means, the specific condition does not need to be in the light detecting angle direction of the same direction as the polarization angle direction by the light polarizing means as in the above embodiment, and the point is, Corresponding luminous bodies T0, T1, T2, T
Any condition may be used as long as the transmittance of the light from No. 3 is greater than that of other lights.

In the above embodiment, the case where the image sensors S0, S1, S2 and S3 using the black and white CCD sensors are exemplified as the plurality of image pickup means provided on the ground side.
Other than the CCD sensor, for example, a PSD sensor or the like may be used, and a color type sensor may be used instead of the monochrome type sensor. Further, in the above embodiment, the plurality of image pickup means are operated synchronously, but they may be operated asynchronously for simplification of control.

In the above embodiment, each of the plurality of light passing means (polarization filters p0, p1, p2, p3) has a plurality of image pickup means (image sensors S0, S1, S2, S3).
Although the image pickup angle is changed by integrally changing the angle of the plurality of image pickup means provided with the plurality of light passing means, the image pickup means is changed. The installation state of the plurality of light passing means with respect to the plurality of image pickup means and the specific configuration of changing the image pickup angle of the plurality of image pickup means are not limited to the above.

In the above embodiment, the image pickup means S0, S1, S
Although the imaging angle changing means for S2 and S3 is composed of the electric motors 11 and 12, other actuators can be used in addition to the motors.

In the above embodiment, the image pickup means S0, S
1, S2, S3 imaging angle detection means, the motor 1
Although the encoders 11a and 12a are incorporated in the motors 1 and 12, it is possible to use an angle detecting means such as a potentiometer which is separate from the motors 11 and 12, for example. Incidentally, the reference azimuth at that time can be appropriately set other than the direction K of the ground-side work stroke as in the above embodiment.

In the above embodiment, the image pickup means S0, S1, S
In order to detect the distance from 2, S3 to the reference position of the moving body A (actually, the reflection sheet 19a provided there), the moving body A is projected by projecting beam-like detection light as the distance detecting means on the ground side. Although it is configured by the laser length measuring device 13 that detects the distance up to, it is not limited to this.

In the above embodiment, the tracking control means 101 is
Based on the image pickup information of one image pickup unit S0 of the plurality of image pickup units S0, S1, S2, S3, the light emitting body T0 corresponding to the screen center point O set as the predetermined position of the image pickup screen is captured. Although the case where the tracking control is performed has been illustrated, in addition to the image pickup information of the image pickup unit S0, another image pickup unit S1,
It is also possible to perform the tracking control based on the imaging information of S2 and S3. Further, as the predetermined position,
It is also possible to capture the light emitter T0 within a predetermined range including the center point O of the screen. Further, the tracking control means 101
As information for tracking, the position of the light emitter T0 in the imaging screen (deviation X from the center point O), the moving speed of the light emitter T0,
Also, although the case where each information of the vehicle body azimuth angle (direction of the vehicle body) of the moving body A is used is shown, simplification of control is achieved by being based only on the positional information of the light emitting body T0 in the imaging screen. You can also

In the above embodiment, the moving body is the work vehicle A of the automatic traveling type, but other than this, the work vehicle may be a work vehicle that the driver rides on and manually travels.
In this case, information on the current position of the vehicle body and the vehicle body azimuth angle is displayed on a display screen or the like, and the vehicle is manually driven based on this display information.

It should be noted that reference numerals are given in the claims for convenience of comparison with the drawings, but the present invention is not limited to the configurations of the accompanying drawings by the entry.

[Brief description of drawings]

FIG. 1 is an external view showing a moving body and an automatic tracking type position measurement guidance control device.

FIG. 2 is a plan view showing the arrangement of light emitters.

FIG. 3 is a diagram showing polarization directions of light from a light emitting body.

FIG. 4 is a cross-sectional side view showing the configuration of the image pickup means.

FIG. 5 is a block diagram showing a control configuration of an automatic tracking type position measurement guidance control device.

FIG. 6 is an explanatory diagram of a process for detecting the position of a light emitter in the image pickup screen.

FIG. 7 is a characteristic diagram illustrating zoom operation.

FIG. 8 is an explanatory diagram of tracking control.

FIG. 9 is an explanatory diagram of a vehicle body direction detection process of a moving body.

FIG. 10 is a block diagram showing a control configuration on the moving body side.

FIG. 11 is a plan view showing a field on the ground side on which a mobile body is guided to travel.

FIG. 12 is a side view showing a reference position display means.

FIG. 13 is a flowchart illustrating a control operation.

FIG. 14 is a flowchart illustrating a control operation.

[Explanation of symbols]

 A moving body T0, T1, T2, T3 light emitting body p0, p1, p2, p3 light passing means S0, S1, S2, S3 image pickup means 11a, 12a image pickup angle detection means 13 distance detection means 100 azimuth calculation means f0, f1, f2, f3 Polarizing means p0, p1, p2, p3 Light detecting means 11, 12 Imaging angle changing means 101 Tracking control means

Claims (5)

[Claims] 1. An azimuth detecting device for a moving body for detecting the azimuth of a moving moving body (A), comprising: a plurality of light emitting bodies (T) emitting light having different properties to the moving body (A).
0, T1, T2, T3) are distributed in a set arrangement relationship with respect to a reference position in a plan view, and each light from the plurality of light emitters (T0, T1, T2, T3) is provided on the ground side. Are provided in correspondence with each other, and the corresponding light among the plurality of light emitters (T0, T1, T2, T3) has a greater transmittance than other lights due to the difference in the property of each light. A plurality of light passing means (p0, p)
1, p2, p3) and a plurality of image pickup means (S0, S1, S2, S3) for individually picking up each light passing through each of the plurality of light passage means (p0, p1, p2, p3). An imaging angle detecting means (11a, 12a) for detecting an angle formed by the imaging directions of the plurality of imaging means (S0, S1, S2, S3) with respect to a reference azimuth, and the plurality of imaging means (S0, S1, S2, S3) to a reference position of the moving body (A), a distance detecting means (13), and arrangement information of the plurality of light emitting bodies (T0, T1, T2, T3), Multiple light emitters (T0, T1, T2, T3)
Position information on each of the image pickup screens of the plurality of image pickup means (S0, S1, S2, S3), and the distance detection means (1
3) distance information and the imaging angle detection means (11)
a, 12a), the azimuth calculating means (100) for calculating the azimuth of the moving body (A) based on the imaging angle information of the moving body (A). 2. The plurality of light emitters (T0, T1, T
2, T3), and polarizing means (f0, f1, f2, f) that polarize the respective lights therefrom in different polarization angle directions.
3) is provided, and the plurality of light passing means (p0, p1, p2, p3) are provided.
However, a plurality of light detecting means (p0, p) for detecting light from the plurality of light emitters (T0, T1, T2, T3) polarized in the different polarization angle directions in different detection angle directions.
1, p2, p3), the azimuth detecting device for a moving body according to claim 1. 3. The plurality of image pickup means (S0, S1, S)
2. The azimuth detecting device for a moving body according to claim 1, wherein the second and S3) are configured to perform image pickup operation in a synchronized state. 4. The plurality of image pickup means (S0, S1, S)
2, S3), the corresponding light emitter (T0) of the plurality of light emitters (T0, T1, T2, T3) is placed at a predetermined position in the image pickup screen based on at least one image pickup information. As can be seen, the imaging means (S0, S1,
4. The azimuth detecting device for a moving body according to claim 1, 2 or 3, further comprising a tracking control means (101) for operating the imaging angle changing means (11, 12) of S2, S3). 5. The plurality of light passing means (p0, p1,
p2, p3), each of the plurality of imaging means (S0,
S1, S2, S3) are attached in an integrated state to each other, and the imaging angle changing means (11, 12) is provided with the plurality of light passing means (p0, p1, p2, p3). The azimuth detecting device for a moving body according to claim 4, wherein the image pickup means (S0, S1, S2, S3) is integrated and the angle is changed.