JPH047716A - Steering control device for autonomous traveling vehicle - Google Patents

Abstract

PURPOSE:To prevent the steering control device from lossing the sight of a reference point for a long time by changing a vertical scanning angle only by a proposed angle in each scanning cycle at the time of lossing the sight of a light reflecting means arranged on the reference point, and detecting reflected light from the undetected light reflecting means again. CONSTITUTION:Plural reference points are set up around a working area and reflectors 6a to 6d consisting of light reflecting means provided with reflecting faces for reflecting incident light to the incident direction are arranged on respective reference points. An optical beam 2E scanned from an optical scanner 2 in the direction of the arrow 29 is successively reflected by the reflectors 6a to 6d. When the reference point can not be detected in a forecasting azimuth to be detected, the projection angle of the beam 2E is changed in each proposed angle with short intervals in the forecasting azimuth for detecting the reference point in the succeeding scanning to search the undetected reference point, so that even when the reference point is lost sight of, the reference point can be automatically searched. Consequently, the reference point can be prevented from being lost sight of it for a long period.

Description

DETAILED DESCRIPTION OF THE INVENTION (Industrial Application Field) The present invention relates to a steering control device for self-propelled vehicles, and particularly to a steering control device for self-propelled vehicles such as unmanned mobile conveyance devices in factories, agricultural and civil engineering machinery, etc. Regarding a control device.

(Prior Art) Conventionally, as a device for detecting the current position of a moving object such as the above-mentioned self-propelled vehicle, a device for scanning a light beam generated by the moving object in a circumferential direction centering on the moving object; A device has been proposed that is equipped with a light reflecting means that is fixed at at least three locations apart from the light reflecting means in the direction of incidence, and a light receiving means that receives the reflected light from the light reflecting means (Japanese Patent Laid-Open No. 5967
Publication No. 476).

This device detects the opening angle between the three light reflecting means as seen from the moving body based on the light receiving output of the light receiving means, and the detected opening angle and the preset position of each light reflecting means. The position of the moving object is calculated based on the information.

In the above system, the light beam may not be able to be irradiated to the light reflecting means due to the tilting or shaking of the self-propelled vehicle, or the light receiving means may receive reflected light from a reflective object other than the light reflecting means. there were. If the scheduled reflected light from the light reflecting means cannot be received, the position of the self-propelled vehicle cannot be calculated, and as a result, the self-propelled vehicle may not be able to travel along the scheduled course.

In contrast, the applicant predicts the azimuth at which the same light reflecting means should be detected in the next scan based on the azimuth of the light reflecting means detected in the previous scan, and A control device has been proposed that is configured to determine that light is normal reflected light from a predetermined reflecting means (Japanese Patent Application No. 63-262192). The control device is configured to stop the self-propelled vehicle if the light reflecting means is repeatedly not detected in the predicted direction.

In addition, the applicant has determined that if no light reflecting means is detected in the predicted direction, as an emergency measure, at least the light reflecting means should have been detected in the vicinity of the predicted direction. A control device was also proposed that uses predicted azimuth data in place of the actual azimuth to detect the position of a self-propelled vehicle (Japanese Patent Application No. 12424/1999).

(Three problems to be solved by the invention) With the above control device, if the light reflection means is lost or temporarily lost, the self-propelled vehicle will not be stopped, and the predicted direction will not be reflected in the actual light reflection. Even if this predicted direction data is used to detect the position of the self-propelled vehicle, the error in detecting the position of the self-propelled vehicle is small, so there is no problem.

However, depending on the condition of the running surface, the light reflecting means may be lost for a long time. In this case, the light reflecting means is lost at each detection timing, resulting in a state where the light reflecting means is frequently lost. In such a state of frequent loss of sight, if the predicted azimuth angle is repeatedly used to calculate the position of the self-propelled vehicle, errors in the calculation results will accumulate, making it impossible to accurately guide the self-propelled vehicle. If the error in the calculation results becomes large, the self-propelled vehicle must be stopped, and if the self-propelled vehicle stops frequently, there is a problem in that work efficiency is significantly reduced.

As a measure to prevent frequent loss of sight caused by the condition of the running surface, a device that scans in the circumferential direction while swinging the light beam in the vertical direction (for example, Japanese Patent Laid-Open No. 60-242313 The device described in the above publication) has been proposed.

By the way, in this device, unless the light beam swing speed in the vertical direction is increased and the light beam scanning density in the circumferential direction is increased, the occurrence of frequent loss of sight cannot be significantly improved. In order to increase the scanning density of the light beam, for example, 20.00
This is a high-class rocking mechanism that is durable enough to withstand continuous use for long periods of time, using a high-speed motor that rotates at higher than Orpm and bearings that are highly wear-resistant. Is required.

On the other hand, Japanese Patent Application Laid-Open No. 59-126275 discloses that a plurality of light receiving means are arranged in a vertical line, and by detecting which light receiving means receives the reflected light from the light reflecting means, a light beam emitting point is detected. This document describes a method of determining the deviation between the incident point of the reflected light and the incident point of the reflected light, and correcting the light beam emission angle based on the deviation so that the light beam can be irradiated onto the center of the light reflecting means in the vertical direction.

This method requires a plurality of light-receiving means, and there is a problem that if the light-reflecting means is lost because no reflected light is received, it becomes difficult to search for the lost light-reflecting means again. .

An object of the present invention is to solve the above-mentioned problems of the prior art, and to prevent losing sight of the light reflecting means over a long period of time by searching for the lost light reflecting means, and as a result, reducing work efficiency and self-propelling. An object of the present invention is to provide a steering control device for a self-propelled vehicle that can avoid an increase in vehicle position detection errors.

(Means and Effects for Solving the Problems) In order to solve the above-mentioned problems and achieve the objects, the present invention provides a beam scanning means for scanning a light beam in a circumferential direction around a self-propelled vehicle. , receiving the reflected light of the light beam from the light reflecting means installed at a plurality of reference points distant from the self-propelled vehicle, measuring the azimuth angle of the light reflecting means as seen from the self-propelled vehicle, and based on the result. means for calculating the position of the self-propelled vehicle based on the vertical scanning angle of the light beam; means for storing the vertical scanning angle of the light beam at the time of receiving the reflected light; projection angle setting means for setting the projection angle for each direction in which each reference point should be detected in the next scan based on the direction scanning angle; and a light reflection set at the scheduled reference point depending on the presence or absence of an output from the light receiving means. a reference point lost determination means for determining whether or not the reference point has been detected; and if it is determined based on the output of the determination means that the reference point has been lost, the reference point determined to have been lost has been detected in the previous scan; The present invention is characterized in that it includes means for changing the vertical scanning angle by a predetermined amount when the azimuth angle is set.

In the present invention having the above configuration, when the light reflecting means disposed at the reference point is lost, the vertical scanning angle is changed by a predetermined amount in each scanning cycle, thereby preventing the reflection from the lost light reflecting means. Light can be detected again. As a result, it is possible to avoid a situation where the reference point is lost for a long time.

(Example) An embodiment of the present invention will be described below with reference to the drawings.

FIG. 2 is a perspective view showing the arrangement of a self-propelled vehicle equipped with the control device of the present invention and a light reflector disposed in the travel area of the self-propelled vehicle. In the figure, a self-propelled vehicle 1 is, for example, a self-propelled vehicle for agricultural work such as a lawn mower.

The self-propelled vehicle 1 is equipped with an optical scanner 2. The optical scanner 2 consists of a fixed part 2a and a rotating part 2b, and the rotating part 2b is connected to a motor 5 by a belt.
It is driven by the motor 5 and rotates, for example, counterclockwise.

The light beam 2E generated by the light emitter of the optical scanner 2 is scanned in the direction of the arrow 29 as the rotating part 2b rotates.

Further, the rotating section 2b incorporates a mechanism described below that changes the vertical scanning angle (hereinafter referred to as the projection angle) of the generated light beam.

The rotating part 2b is provided with a slit plate 7a, and the rotating part 2b is provided with a slit plate 7a.
rotates with. By providing an angle sensor that outputs a pulse signal every time the slit of the slit plate 7a is detected, the rotation angle of the rotating portion 2b can be detected based on the counted value of the pulse.

For the sake of explanation, the optical scanner 2 is shown in an exposed state, and it goes without saying that it is provided with a dust-proof/splash-proof cover.

A plurality of reference points are set around the work area, and reflectors 68 to 6d made of well-known light reflecting means such as a corner cube prism having a reflective surface that reflects incident light in the direction of incidence are installed at the reference points. will be installed. A light beam 2E scanned from the optical scanner 2 in the direction of the arrow 29 is sequentially reflected by these reflectors 6a to 6d, and the reflected light 2R is sequentially received by the light receiver of the optical scanner 2. Then, based on the received light signal, the self-propelled vehicle 1 responds to the reflectors 68 to 6d.
Steering control is performed by detecting the self-position of the vehicle.

Next, the structure of the optical scanner 2 will be explained.

FIG. 9 is a sectional view of the optical scanner 2, and the same reference numerals as in FIG. 2 indicate the same or equivalent parts.

In the figure, light generated by a light emitter (light emitting diode) 3a passes through lenses 4a and 4b and is reflected by a mirror 30. The reflection direction of the light, that is, the projection angle is determined according to the inclination of the mirror 30. The light beam 2E reflected by the mirror 30 and emitted to the outside is reflected by the reflectors 6a to 6d, and the reflected light 2R is sent to the optical scanner 2.
Return to

The reflected light 2R is direction-changed by a mirror 30 and then directed to a lens 4.
b and 4a, and is further refracted by a reflecting prism 4C and enters a light receiver (photodiode) 3b. Receiver 3
The light entering b is converted into an electrical signal, and based on the electrical signal, it is detected that the reflected light from the reflectors 6a to 6d has been received.

The mirror 30 is tilted in the direction of an arrow 33 about a shaft 32 by rotation of a pin 31 fitted into a notch provided at one end thereof. That is, the pin 31 is eccentrically attached to the shaft of the step motor 34, and the amount of tilting of the mirror 30 is determined according to the amount of rotation of the step motor 34. As a result, the projection angle of the light beam 2E is changed in the vertical direction (arrow 35 direction) according to the tilting of the mirror 30.

The angle sensor 7b detects the slit of the slit plate 7a, and based on the detection signal, the rotation angle of the slit plate 7a, that is, the rotation angle of the rotating part 2b is detected. A pulley 37 of the rotating part 2b is attached to the fixed part 2a via a bearing 36, and the rotation of the motor 5 is transmitted to the pulley 37 by a belt 38.

The optical scanner shown in FIG. 9 is an example, and a well-known scanner equivalent to this scanner can be used.

For example, the projection angle of the light beam may be changed by tilting the optical scanner 2 itself while keeping the movement of the mirror 30 in the direction of the arrow 33 fixed.

Further, the rotation angle of the rotating part 2b is detected by using the slit plate 7a.
The detection is not limited to the combination of the angle sensor 7b and the angle sensor 7b, and any known angle detection means may be used, such as a rotary encoder directly connected to the motor 5.

Next, a method for identifying the reflector placed at the reference point and other reflective objects will be described. FIG. 8 is an explanatory diagram of the reference point identification process. In the figure, the reflectors 6a to 6d are arranged at reference points A to D around the working area 22, respectively. An arrow 29 indicates the scanning direction of the light beam emitted from the self-propelled vehicle 1.

In the illustrated arrangement, the azimuth of each reference point is calculated based on the detection signal of the light receiver 3b, and the same reference point is detected in the next scan based on the azimuth detected up to the present time. The azimuth angle to be used (predicted azimuth angle) is calculated. The predicted azimuth angle of each reference point is the angle θpa~θ
Indicated by pd.

Light beam scanning direction 29 from each predicted azimuth θpa to θpd
Reference point identification directions pa to pd are provided as timings for identifying reference points in directions in which scanning has advanced by an angle θh. In the reference point identification azimuths pa to pd, among the lights detected between each reference point identification azimuth pa to pd and the immediately preceding reference point identification azimuths, incident light from the direction closest to the predetermined M1 azimuth is detected. , it is determined that the light is from a reflector installed at a scheduled reference point.

For example, assume that light from the noise source NlN2 and reflected light from the reflector 6a installed at the reference point A are detected between the reference point identification direction pa and the reference point identification direction pd immediately before it. In this case, among these lights, the light from the direction closest to the predicted azimuth θpa, that is, the light reflected from the reference point A, can be distinguished from the light from the other light sources Nl and N2.

Furthermore, the following process can be added to improve the accuracy of identifying reference points. In other words, if a predetermined range (an angle equal to or smaller than the angle θh) is set before and after the predicted azimuth, and even light from the direction closest to the predicted azimuth θpa to θpd deviates from the range. It is determined that the planned reference point has been lost.

If it is determined that the scheduled reference point has been lost because no light is detected between the two reference point identification azimuths or within the scheduled range set to improve identification accuracy, the processing cycle is started using the predicted azimuth angle. The position of the self-propelled vehicle 1 is calculated, and the predicted azimuth is stored as it is for further use as the next predicted azimuth. Note that the next predicted azimuth may be a value obtained by adding a planned minute angle to the current predicted azimuth.

Further, in the reference point identification azimuths p a - p d, the projection angle is set so that the light beam can be reliably irradiated onto the intended reflector at the predicted azimuth angle immediately after that.

Next, a calculation procedure for detecting the position and traveling direction of the self-propelled vehicle 1 will be explained. 10 and 11 show the positions of the self-propelled vehicle 1 and the reflector 6 in a coordinate system for indicating the working range of the self-propelled vehicle 1. FIG.

In FIGS. 10 and 11, reference points A, B, and C, where reflectors 6a to 6c are arranged, and the position of self-propelled vehicle 1 are expressed by a straight line connecting reference points B and C with reference point B as the origin. It is expressed in an x-y coordinate system with .

As can be understood from the figure, the position T of the self-propelled vehicle 1 is on the circumcircle of the triangle ATB and at the same time
Exists on the circumcircle of Therefore, the position of the self-propelled vehicle 1 is the circumcircle Q of each of triangle ATB and triangle BTC.
It is obtained by calculating the two intersection points of and P.

As shown in the figure, the position of the self-propelled vehicle 1 can be determined by setting the reference point B, which is the intersection of one of the circumscribed circles Q and P, as the origin, and calculating the other intersection T of the circumscribed circles Q and P according to the following procedure. .

The calculation formula for determining the position of the self-propelled vehicle 1 is disclosed in Japanese Patent Application Laid-open No. 1-287.
415 and Japanese Unexamined Patent Publication No. 1-316808, the description thereof will be omitted.

The traveling direction of the self-propelled vehicle 1 is calculated using the following formula. 1st
In Figure 1, the angle between the traveling direction of the self-propelled vehicle 1 and the
, the azimuthal angles are θa and θb.

If θC, θf- 360°-tan' (y/ (x c-x) 1
-θC...(1)

The position and traveling direction of the self-propelled vehicle 1 are calculated by the position and traveling direction calculating section 13, which will be described later, using the above-mentioned calculation formula and the above-mentioned calculation formula (1).

Next, steering control of the self-propelled vehicle 1 based on the position information of the self-propelled vehicle 1 calculated by the above procedure will be explained. FIG. 6 is a diagram showing the positional relationship between the traveling course of the self-propelled vehicle 1 and the reference points. The location of the vehicle 1 and the work area 22 by it are shown.

Point R (Xret, Yret) indicates the return position of the self-propelled vehicle 1, and the coordinates (Xs t, Ys t), (XstYe),
The area connected by the points (Xe, Yst) and (Xe, Ye) is the work area 22.

Here, the position T of the self-propelled vehicle 1 is indicated by (Xp, Yp).

The self-propelled vehicle 1 starts traveling from a work start position S, sequentially travels through a straight stroke and a turning stroke for transitioning from one straight stroke to the next straight stroke to perform scheduled work such as mowing the lawn.

The self-propelled vehicle 1 starts turning when the y-coordinate becomes Ye in the forward direction (upward on the drawing) and when the y-coordinate becomes Yst in the return direction (downward on the drawing). .

The y-coordinate of the turning end point is also the same as the y-coordinate of the turning start point, but since the coordinates of the self-propelled vehicle 1 are not accurately detected during the turn, it is difficult to determine whether the self-propelled vehicle 1 has reached the turning end point. is self-propelled car 1
Judgment is made based on the azimuth data of the reference point as seen from.

Specific examples of steering control during the turning process and judgment of turning completion are as follows:
Japanese Patent Application Publication No. 1-316808 and Japanese Patent Application No. 19293-1993
listed in the number.

In addition, in FIG. 6, in order to simplify the explanation, an example is shown in which the work area 22 is arranged on the y-axis or parallel to the y-axis.
Reference points A around the work area 22.

As long as B, C, and D are arranged, the shape of the work area 22 and the orientation of the four sides of the work area 22 are arbitrary.

Next, the functional configuration of the control device of this embodiment will be explained according to the block diagram shown in FIG. In the figure, the part surrounded by chain lines can be constructed by a microcomputer.

In FIG. 1, a light beam 2 emitted from a light emitter 3a
E is scanned in the rotating direction of the rotating section 2b, and the reflector 6
(68-6d). The reflectors 6a-6
The reflected light 2R of d is received by the light receiver 3b.

The counter 9 counts pulses output from the angle sensor 7b as the slit plate 7a of the rotating section 2b rotates. The count value of the pulse is transferred to the azimuth detecting section 11 in response to a detection signal from the light receiver 3b. Azimuth detection unit 1
1, reflectors 68-6 based on the number of pulses supplied.
The azimuth angle of d is calculated.

The azimuth detected by the azimuth detection unit 11 is stored in the azimuth storage unit 1.
The data stored in the azimuth storage unit 12 is transferred to the azimuth storage unit 12 and stored in the azimuth storage unit 12 in response to the identification timing signal supplied from the identification timing generation unit 23 for each of the identification azimuths pa to pd. The information is transferred to the identification unit 24.

In order to output the identification timing signal when the scanning of the light beam progresses to each reference point identification direction pa-pd, the identification timing generating section 23 receives the output pulse of the angle sensor 7b. Then, at the time when the output pulses are taken in a number corresponding to the angle θh added to the predicted azimuth calculated by the azimuth angle prediction/7X calculating section 27, an identification timing signal is output.

The azimuth identification unit 24 arranges the light detected in the direction closest to the predicted azimuth calculated by the azimuth prediction calculation unit 27 from among the azimuths supplied from the azimuth storage unit 12 at a predetermined reference point. It is determined that the light is reflected from a reflected reflector. The azimuth angle data of the reflector determined by this judgment is input to the aperture angle calculation unit 10, and the reflector 6a to 6a as seen from the self-propelled vehicle 1
The opening angle between 6d is calculated. The azimuth angle data is also supplied to the azimuth angle prediction calculation unit 27 and used to predict the azimuth angle of the reflector to be detected in the next scan.

That is, in the azimuth hook 11$1 calculation unit 27, the predicted azimuth is determined by a function of the azimuth determined by the azimuth data 24, which is expected to be obtained experimentally.

The predicted azimuth is not limited to the method of finding it based on a predetermined function, but may be found by adding the difference between the current and previous azimuths obtained by the azimuth angle identification unit 24 to the current azimuth.

The position/progressing direction calculating section 13 calculates the current position coordinates of the self-propelled vehicle 1 based on the aperture angle, and also calculates the traveling direction of the self-propelled vehicle 1 based on the azimuth angle. This calculation result is input to the position/direction comparison section 25. The position/travel direction ratio soft section 25 stores data representing the travel course set in the travel course setting section 16 and the coordinates and travel direction of the self-propelled vehicle 1 obtained by the position/direction calculation section 13. be compared.

The comparison result is input to the steering section 14, and the steering motor 28 connected to the front wheels 17 of the self-propelled vehicle is driven based on the comparison result. The steering angle of the front wheels 17 by the steering motor 28 is detected by a steering angle sensor 15 provided on the front wheels of the self-propelled vehicle 1 and fed back to the steering section 14 . Drive control section 1
8 starts and stops the engine 19, and the engine 19
The operation of the clutch 20 that transmits the power to the rear wheels 21 is controlled.

Furthermore, the following functions are added to improve the accuracy of identifying reference points. That is, a value indicating the angular range set in the range setting section 26 is supplied to the azimuth angle identification section 24, and it is determined whether the light is detected in the direction closest to the predicted azimuth or in the planned angular range. A function is added to identify whether or not the

As a result of the identification by the identification function, if the light detected in the direction closest to the predicted azimuth is within the planned range, the aperture angle is calculated using the azimuth, and if it is outside the planned range. , the aperture angle is calculated using the predicted azimuth calculated by the azimuth angle prediction calculation unit 27.

Either the light from the angular range set in the range setting unit 26 is determined to be reflected light from the scheduled reference point and the aperture angle is calculated using the azimuth of the light, or the azimuth of the incident light is within the range. Whether the opening angle is calculated using the azimuth determined without determining whether it is within the range set in the setting unit 26 is necessary depending on the type and type of work performed by the self-propelled vehicle 1. It may be arbitrarily selected depending on the degree of accuracy required.

Note that the configuration of the lost-of-sight determination means is not limited to that shown in this embodiment, and the detection range of the predicted azimuth angle may be set narrowly, and loss of sight of the reference point may be determined based on whether or not a received light signal is detected within the range. Simple method for determining loss of sight (patent application 1986-26)
2192) can also be applied to the present invention.

As shown in FIG. 1 (part 2), when the reference point appearance signals are continuous, the consecutive appearance counting unit 8 adds up the number of times the reference point is lost in succession for all the reference points. The continuous appearance counting section 8 is provided with counters corresponding to all reference points set in the work area. In this example, the reference point A
-D, four counters M1 to M4 respectively corresponding to the counters M1 to M4 are provided. The counts of the counters M1 to M4 are incremented by the lost sight signal supplied from the azimuth recognition unit 24, and are cleared by the signal indicating that the scheduled reference point has been detected.

The projection angle in the vertical direction for each reference point in the reference point initial recognition process performed before the self-propelled vehicle 1 runs, that is, the angle setting value of the step motor 34 at the time of detecting each reference point, or the initial projection angle storage section 40. In the reference point initial recognition process, the mirror 30 is swung by a predetermined amount in the direction of the arrow 33 for each scan until the reference point is detected, and the mirror 30 is automatically set.

Note that this step may be configured such that the initial projection angle is memorized by manual adjustment by the operator.

In the process immediately after the self-propelled vehicle 1 starts running, the projection angle setting section 39 uses the data stored in the initial projection angle storage section 40.
The projection angle is determined by The determined projection angle is stored in the projection angle storage section 41.

- After the data is stored in the projection angle storage unit 41,
In each reference point identification direction pa-pd, the projection angle at the time of the previous scan of the reference point to be detected in the immediately following predicted direction is read from the projection angle storage section 41 and the projection angle setting section 3
9 will be powered by humans. The projection angle setting section 39 determines the projection angle according to the number of continuous appearances stored in the continuous appearance counting section 8 by calculations to be described later.

That is, if it is determined that the reference point was detected in the previous scan based on the number of consecutive appearances, the projection angle in the previous scan is determined as the projection angle in the next scan.

On the other hand, if it is determined that the reference point has been continuously lost based on the number of consecutive appearances, the projection angle setting unit 39
The projection angle from the previous scan is read in, the scheduled calculation is performed according to the number of consecutive appearances, and the projection angle for the next scan is set.

The step motor control section 42 determines the step angle of the step motor 34 for changing the projection angle based on the projection angle supplied from the projection angle setting section 39, and drives the step motor 34 according to the step angle.

The control procedure will be explained according to the flowchart in FIG.

First, in step S1, the self-propelled vehicle 1 is moved from point R to the vicinity of the work start position by radio control. Step S
In step 2, Xst is set as the X coordinate Xn of the running course, and the running course is determined.

In step S3, the light beam is scanned while the self-propelled vehicle 1 remains stopped, and initial recognition processing of the reference point is performed. In the initial approval process, the vertical projection angle at the azimuth angle detected from the reference point is stored in the initial projection angle storage section 40.

In step S4, the self-propelled vehicle 1 starts running.

In step S5, it is determined whether the light receiver 3b has received light from the reference point or another light source. If light is detected, the process proceeds to step S6, where light reception processing to be described later is performed, and if no light is detected, the process proceeds to step S7.

In step S7, it is determined whether or not it is time to perform a reference point identification process to determine which of the received reflected lights is from the scheduled reference point. This judgment is based on the predicted azimuth angle θp calculated by the azimuth angle prediction calculation unit 27.
This is performed depending on whether the scanning has progressed from a to θpd to the reference point identification process pa-pd, which is a predetermined angle θh.

Steps 85 to S until the judgment in step S7 is affirmative
Step 7 is repeated, and if the determination is affirmative, the process proceeds to step S8, where a reference point identification process is executed.

If the reference point is detected, the process proceeds to step S9, where projection angle control is performed to determine the projection angle in order to accurately project the light beam to the next reference point.

In step S10, the position T (Xp, Yp) and traveling direction θf of the self-propelled vehicle 1 are calculated based on the azimuth of the detected reference point.

In step Sll, the amount of deviation from the driving course (ΔXm
Xp-Xn, Δθf) is calculated, and in step S12, steering angle control is performed in the steering section 14 according to the calculated deviation amount.

In step S13, it is determined whether the self-propelled vehicle 1 is traveling in a direction away from the origin (going direction) or in a direction approaching the origin (returning direction) in the y-axis direction.

If it is in the forward direction, it is determined in step S14 whether the -stroke has ended (Yp>Ye). If it is in the return direction, in step 515, -stroke or end (Y
p<Ys t), but it is determined whether or not. Step S1
4 or S15, if it is determined that the -stroke has not been completed, the process returns to step S5.

If it is determined in step S14 or S15 that the -stroke has been completed, then in step s16 it is determined whether all the strokes have been completed (Xn>Xe-L).

If the entire stroke has not been completed, the process moves from step 816 to step S17, and the U-turn control of the self-propelled vehicle 1 is performed. The U-turn control is different from the straight-line steering control performed by the processing in steps 510 to 512 in which the position information of the self-propelled vehicle 1 calculated by the position/direction calculation unit 13 is fed back to the steering unit 14. It is done by method.

That is, in the turning stroke, the steering section 14 determines a steering angle based on an angle previously set for traveling in the turning stroke, and the vehicle travels with the steering angle fixed at the determined angle.

Then, when at least one of the azimuth angles of each of the reference points A to D relative to the self-propelled vehicle 1 matches a planned angle but falls within the planned angle range, the fixation of the steering angle for turning is released.

In step 31g, Xn is set to Xn+L, and the travel course for the next stroke is set. If the driving course is set, the process returns to step s5 and the above process is repeated.

When the entire stroke is completed, the return position R (Xret.

Yret) (step 519), and the traveling is stopped (step 520).

In this embodiment, since the self-propelled vehicle 1 was moved to the work start position by radio control, the initial approval process was performed after the movement. When the self-position of the self-propelled vehicle 1 is detected and the self-propelled vehicle 1 is moved by automatic steering based on the result, an initial recognition process is performed before the movement.

Next, the light reception process in step S6 and step S8
The reference point identification process will be explained. A flowchart of the light reception process is shown in FIG.

In the figure, in step S50, the light reception flag is set to "1°" in order to memorize that light has been detected.

In step S51, the azimuth of the detected light source is stored in the azimuth storage section 12.

A flowchart of the reference point identification process is shown in FIG. This flowchart shows an example of a procedure for further narrowing down the detected azimuth angle closest to the predicted azimuth angle using a scheduled angle set in the range setting section 26 to identify a reference point.

In the figure, in step S70, a value n of a pole counter (hereinafter simply referred to as a pole counter) for distinguishing a reference point to be identified is incremented. The pole counter is associated with each reference point. In other words, pole counter "1" is at reference point A, pole counter "2" is at reference point A, and pole counter "2" is at reference point A.
corresponds to the reference point B, pole counter "3" corresponds to the reference point C, and pole counter "4" corresponds to the reference point.

If the initial value of the poll counter is “0”, step S
As a result of the process 70, the poll counter becomes "1", and the reference point corresponding to this becomes "A". In this embodiment, the initial value is set to "0".

In step S71, the light reception flag is determined, and if the light reception flag is "1 degree", the process proceeds to step S72, and if the light reception flag is "0", the process jumps to step S79.

In step S72, the predicted azimuth θpn (the pole counter is “1°, so the predicted azimuth θp
Assume that the one closest to a) is the azimuth angle of the scheduled reference point, and store that value as the angle θS.

In step S73, the presence or absence of noise is determined by determining whether the number of received lights is 2 degrees or more, that is, whether a plurality of azimuths are stored in the azimuth storage section 12.

If the determination in step S73 is affirmative, it is assumed that one or more noises have been detected, and the process moves to step S74, in which the fact that noise has been detected is stored as noise processing. This stored data can later provide clues about the state of the work environment, making it easier to take measures such as eliminating noise sources.

If the judgment in step S73 is negative, the process proceeds to step S75, where the temporarily determined azimuth θS and the predicted azimuth θp are
It is determined whether the difference between the angle θh and the predicted azimuth θpa is smaller than the angle θh. If the error is larger than the angle θh, it is determined that the temporarily determined azimuth θS is not the azimuth of the planned reference point but the azimuth of the noise source, and the process proceeds to step 378, where the same noise as in step S74 is detected. Perform processing.

After the noise processing, the process proceeds to step S79, where the predicted azimuth θpn (predicted azimuth θpa
) is set as the azimuth angle θn (θa) of the scheduled reference point.

In step S80, the number of times Mn (Ml
) is incremented.

On the other hand, if the difference is smaller than the angle θh, the assumed azimuth θS is determined to indicate the azimuth of the scheduled reference point, and the process proceeds to step S76.

In step 576, the azimuth θ determined in the previous process is
Based on n and the azimuth θS determined in the current process, a predicted azimuth that should be detected at the same reference point in the next process is calculated using the calculation formula (θs+(θS−θn)).

In step S77, the azimuth angle θn is updated by the angle θS.

In step S81, since the reference point has been detected, the number Mn (~11) of continuous appearance of the reference point is cleared.

In step S82, the next reference point identification direction pn+1 (
That is, the predicted azimuth θpn+1 calculated when the reference point B was detected during the previous scan as the reference point identification azimuth pb)
The angle obtained by adding the planned angle θh to is set.

In step S83, the light reception flag is cleared.

In step S84, the data stored in the azimuth storage section 12 is erased.

In step S85, it is determined whether the poll counter is "4" or not. The error value "4" is the number of installed reference points, and is set in accordance with the number n of installed reference points.

If the number of installed reference points and the pole counter match, that is, if it is determined that the identification process has been completed for all reference points, the pole counter is set to "0" in step 386, and the pole counter is set to "0". Returning to the process shown in FIG. 3.

When the pole counter is "1", in the next process, the pole counter is incremented to "2" in step S70, and the reference point B corresponding to the pole counter "2" is identified.

Thereafter, identification processing for reference points C and D is also performed in the same manner.

Next, projection angle control for determining the projection angle will be explained. FIG. 7 is a flowchart of projection angle control.

In the same figure, in step S30, it is determined whether or not the number of consecutive appearances M(n+1) of the reference point to be identified next is "0", and it is determined whether or not the reference point was detected in the previous scan. That is, when the current pole counter is "1", it is determined whether the reference point B corresponding to the pole counter "2" was detected in the previous scan.

If the scheduled reference point has been detected in the previous scan, the judgment is affirmative and the process jumps to step S34, where the projection angle θ in the previous scan is set as the projection angle θ~1 in the current scan.
Set M(n+1).

In step S35, the step angle of the step motor 34 for obtaining the projection angle θN1 is determined, and the step motor 34 is driven by a predetermined amount according to the step angle.

On the other hand, if the determination in step 530 is negative, that is, if it is determined that the next reference point to be identified could not be detected in the previous scan, the process advances to step S31, and the number of times M(n+1) or "1" is determined. Depending on whether or not this is the case, it is determined whether or not this process is the first process after losing sight of the reference point.

If the determination in step S31 is affirmative, the process proceeds to step S36, in which the projection angle is directed downward by a predetermined angle.
The angle obtained by subtracting the planned angle ΔθM from the projection angle θM (n+1) during the previous scan is stored in the projection angle storage unit 41 as the current projection angle θM (n+1) of the reference point.

In addition, if the judgment in step S31 is negative, that is, if the reference point is overlooked two or more times in a row, step S31 is determined.
Proceeding to step 32, it is determined whether the number of consecutive appearances M(n+1) has reached the scheduled number Z or not, or whether the number of consecutive appearances has been lost or has occurred frequently.

If the limit has not been reached, the process proceeds to step S33, where the projection angle change amount Δθt for searching for a reference point that could not be detected is added to the projection angle θM (nil) during the previous scan, and that angle is used for the current scan. The projection angle is stored in the projection angle storage unit 41 as the projection angle θM (n+1).

As a result, the projection angle is directed upward by the projection angle change amount Δθt from the angle set during the previous scan.

Note that if it is determined in step S32 that loss of sight occurs frequently, an alarm is generated in step S37, and the self-propelled vehicle 1 is stopped in step S538.

As described above, in this embodiment, when the planned reference point is lost, the projection angle is first directed downward by the planned angle, and is gradually corrected upward with each scan. Then, when the loss of the reference point is resolved, the operation of changing the projection angle is stopped, and the next detection of the reference point is performed at the projection angle.

In this way, if the reference point is lost or the reference point is lost, the projection angle should be adjusted downward based on the premise that it is the self-propelled vehicle 1 or the work vehicle that travels on the ground or floor. This condition was determined from the viewpoint that it is possible to eliminate loss of sight in a shorter time than by pointing upward. Therefore, the projection angle may be once directed upward and then gradually changed downward. In addition to these procedures, for example,
The reference point may be searched for by changing the projection angle alternately in the vertical direction and gradually changing the amount of change.

In short, if a reference point is not detected in the predicted direction in which it should be detected, the projection angle of the light beam is changed in small increments by a predetermined amount in the predicted direction in which the reference point should be detected in the next scan. What is necessary is to perform a search operation for the reference point.

(Effects of the Invention) As is clear from the above description, according to the present invention, the following effects can be obtained.

(1) Even if a reference point is temporarily ostentatious, the ostentatious reference point can be automatically searched for, thereby preventing the ostentatious reference point from being ostentatious for a long period of time.

(2) While searching for a pretentious reference point, the azimuth of the pretentious reference point is estimated based on the predicted azimuth, and the self-propelled vehicle uses the estimated azimuth angle with relative accuracy. can be induced.

(3) Unlike scanning a light beam by combining vertical swinging and circumferential rotation, no driving means is required to scan the light beam in the vertical direction at high speed.

(4) Since only one set of light receiving means is required, the cost of the optical scanner can be reduced and control can be simplified compared to conventional devices that require a plurality of light receiving means to adjust the projection angle.

[Brief explanation of drawings]

FIG. 1 is a block diagram showing the functions of an embodiment of the present invention, FIG. 2 is a perspective view showing the traveling course of a self-propelled vehicle and the arrangement of reflectors, FIG. 3 is a flowchart of steering control, and FIG. Figure 5 is a flowchart of reference point identification processing, Figure 5 is a flowchart of light reception processing, Figure 6 is a diagram showing the traveling course of a self-propelled vehicle and the arrangement of reflectors, Figure 7 is a flowchart of projection angle control, and Figure 8 is a flowchart of control point identification. The figure is an explanatory diagram of reference point identification processing, Fig. 9 is a cross-sectional view of the optical scanner, Fig. 10 is an explanatory diagram of the principle of position detection of a self-propelled vehicle, and Fig. 11 is an explanatory diagram of the principle of detection of the traveling direction of a self-propelled vehicle. It is. DESCRIPTION OF SYMBOLS 1... Self-propelled vehicle, 2... Optical scanner, 3a... Emitter, 3b... Light receiver, 6, 6a-6d... Reflector, 8... Continuous appearance count unit , 11...Azimuth angle detection section, 12...Azimuth angle second storage section, 13...Position.
Traveling direction calculation unit, 23...Identification timing generation unit, 2
4...Azimuth angle identification unit, 30...Mirror, 34...
Step motor, 39... Projection angle setting section, 40...
- Initial projection angle storage section, 41... Projection angle storage section, 4
2...Step motor control section Fig. 2 Fig. Fig. 8 Fig. 5 Fig. 6 Fig. <CO)! Figure 9 Figure 11

Claims (5)

[Claims] (1) A light beam is scanned in the circumferential direction centering on the self-propelled vehicle, and the reflected light of the light beam is received from light reflecting means installed at a plurality of reference points distant from the self-propelled vehicle. In a steering control device for a self-propelled vehicle that measures an azimuth angle of the light reflecting means as seen from the vehicle and runs the self-propelled vehicle based on the result, the vertical projection angle changing means for the light beam is provided. and means for storing the projection angle of the light beam when the reflected light is received; and projection for setting the projection angle for each direction in which each reference point is to be detected in the next scan based on the stored projection angle. an angle setting means; a reference point loss determination means for determining whether or not the light reflection means installed at a scheduled reference point has been detected based on the presence or absence of an output from the light receiving means; and a reference point loss determination means based on the output of the determination means. If it is determined that the reference point has been lost, in the direction in which the lost reference point should be detected in the next scan, the light beam projection angle changing means changes the projection angle by a predetermined amount from the previous scan. What is claimed is: 1. A steering control device for a self-propelled vehicle, comprising means for outputting a signal for controlling the vehicle. (2) The reference point loss determining means includes: means for detecting the azimuth of each light reflecting means as seen from the self-propelled vehicle based on the reflected light; and based on the azimuth detected by the azimuth detecting means. , azimuth prediction means for calculating the azimuth at which each light reflection means should be detected in the next scan; It is characterized by comprising a reference point identification means for identifying whether the light is reflected from the light reflecting means or not, and a means for outputting a lost signal when the light reflected from the intended light reflecting means is not detected by the identification means. The steering control device for a self-propelled vehicle according to claim 1. (3) If no light that can be determined to be from a light reflecting means placed at a scheduled reference point is detected, the position of the self-propelled vehicle is detected based on the predicted azimuth. The steering control device for a self-propelled vehicle according to claim 1 or 2, characterized in that: (4) If it is determined that the reference point has been lost, the projection angle is changed either up or down by the planned angle, and in the direction in which the reference point determined to have been lost should be detected in the next scan, the reference point The self-propelled vehicle according to any one of claims 1 to 3, characterized in that the projection angle is changed by a predetermined amount in a direction opposite to the changing direction for each scan until the loss of sight is resolved. Steering control device. (5) If it is determined that the reference point has been lost, the direction in which the lost reference point should be detected in the next scan will be up and down with respect to the projection angle of the previous scan until the loss of the reference point is resolved. 5. The self-propelled vehicle according to claim 1, wherein the projection angle is alternately changed by a predetermined amount in either direction, and the predetermined amount is updated for each scan. Steering control device.