JP3172272B2 - Mobile steering control device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for detecting the position of a moving object, and more particularly, to the position of a moving object such as a self-propelled machine used for agriculture and civil engineering work and an automatic transfer device used in a factory. It relates to a detection device.

[0002]

2. Description of the Related Art Heretofore, as a device for detecting the current position of a moving body, means for scanning a light beam generated by the moving body in a circumferential direction around the moving body, and at least three locations distant from the moving body. And a light-reflecting means for receiving light reflected by the light-reflecting means, and a light-reflecting means for receiving light reflected by the light-reflecting means, has been proposed (JP-A-59-67476). .

In this apparatus, an opening angle between the three light reflecting means as viewed from a moving body, that is, an observation point, is detected based on an output signal of the light receiving means. Then, the position of the moving body is calculated from the detected opening angle and information (position information) indicating the position of each light reflecting means set in advance.

[0004] In the above-described apparatus, there is a case where the light beam emitted from the self-propelled vehicle cannot be applied to the light reflecting means because the moving body, that is, the self-propelled vehicle runs while being tilted or shakes during running. In some cases, the light reflected by the light reflecting means cannot be detected by the light receiving means. Also,
In some cases, light from a reflecting object other than the intended light reflecting means is detected by the light receiving means. If the reflected light from the planned light reflecting means cannot be detected, or if light from other parts is erroneously detected as reflected light from the planned light reflecting means, the position of the self-propelled vehicle cannot be calculated correctly. As a result, the vehicle may not be able to run along the planned traveling course.

On the other hand, the present applicant has proposed the following apparatus (Japanese Patent Application No. 3-105099). This apparatus includes means for rotationally scanning a light beam projected from a light beam generating means mounted on a moving body in a substantially horizontal plane around the moving body, and a long cycle that is twice or more the rotation scanning cycle. Swing means for swinging the rotationally scanned light beam in the up-down direction, wherein at least three of the light beams projected from the moving body are separated from the moving body.
It is configured to receive the reflected light reflected back by the light reflecting means provided at a fixed position of the place and to detect the position of the moving body based on the light receiving direction and the position information of the light reflecting means. Have been. That is, in this apparatus, the light beam is reciprocally scanned also in the vertical direction so as to cope with the inclination traveling of the self-propelled vehicle and the shaking during traveling.

[0006]

By the way, as in the above-mentioned apparatus, the light receiving direction of the reflected light, that is, the light receiving azimuth is detected.
When the position of the moving body is detected based on the result, the reference direction of the detecting means for detecting the azimuth and the straight traveling azimuth of the moving body must be matched in advance.
If the reference direction and the rectilinear azimuth of the moving body are displaced from each other, the deviation directly affects the calculation of position detection of the moving body, causing a detection error, and significantly reducing the guidance accuracy of the moving body. There is a point.

In order to solve this problem, a signal indicating a reference direction (azimuth reference signal) is output from an encoder serving as an azimuth angle detection means when the projection direction of the light beam coincides with the straight azimuth of the moving body. The position is adjusted so that the encoder is attached, and the reference direction for detecting the azimuth and the straight traveling azimuth of the moving body are matched.

However, there is a problem in that it is extremely difficult to accurately mount the encoder in consideration of the dispersion of the components and the like, and a highly accurate and expensive dedicated jig is required.

An object of the present invention is to solve the above problems,
It is an object of the present invention to provide a position detecting device for a moving body that can prevent the variation in encoder mounting accuracy or component characteristics from adversely affecting the moving body guidance accuracy.

[0010]

SUMMARY OF THE INVENTION To solve the above problems,
In order to achieve the object, the present invention provides a state in which only one of the light reflecting means arranged for detecting the position of a moving body can be detected, and the moving azimuth of the moving body is the one light. In a state of being set to face the reflecting means, the light beam is rotationally scanned in a circumferential direction around the moving body, and the azimuth of the light reflecting means and the straight running azimuth of the moving body which are detected by the rotational scanning. Is calculated and stored as an azimuth deviation correction value in the non-volatile storage means. When detecting the position of the moving body, the light beam is emitted in the circumferential direction around the moving body. The light beam reflected by the light reflecting means installed at at least three reference positions distant from the moving body is received by the light receiving means mounted on the moving body, and the light receiving azimuth signal is received. The azimuth deviation Corrected by positive, it is characterized in that configured to detect the position of the moving object based on the output signal of the signal means of the corrected.

[0011]

The present invention having the above features operates as follows. First, the movable body is set so that only one of the light reflecting means can be detected, and the moving body is set so that its straight traveling direction is directed to the light reflecting means. The azimuth angle of the light reflecting means, that is, the rotational scanning angle based on the rectilinear azimuth of the moving body should be 0 °.

However, if the reference direction of the encoder for detecting the azimuth angle, ie, the azimuth indicated by the azimuth reference signal, does not match the straight traveling azimuth of the moving body, the detected azimuth angle is shifted accordingly.

Therefore, this deviation is stored as a correction value, and the azimuth angle detected thereafter is corrected with this correction value, whereby the azimuth indicated by the azimuth reference signal output from the encoder and the moving object It can match with the straight running direction. As a result, the detection error when the position of the moving body is detected based on the detected azimuth is not adversely affected by the mounting accuracy of the encoder, the variation in the characteristics of the components, and the like, so that high guidance accuracy can be maintained.

[0014]

An embodiment of the present invention will be described below with reference to the drawings. FIG. 2 is a perspective view showing a self-propelled vehicle equipped with the position detecting device of the present invention and traveling in a predetermined area. In FIG. 2, light reflectors (hereinafter, simply referred to as reflectors) 6 a to 6 having a reflecting surface for reflecting incident light in the incident direction are provided around an area where the vehicle 1 as a moving body is traveling. 6d
Are arranged. Well-known light reflecting means such as a corner cube prism is used for the reflecting surfaces of the reflectors 6a to 6d. The self-propelled vehicle 1 is, for example, a lawn mower having a cutter blade (not shown) for lawn mowing work on a lower surface thereof. A light beam scanning device (hereinafter, simply referred to as a scanning device) 2 is mounted on the upper part of the vehicle 1. The scanning device 2 includes a light emitting device that generates a light beam 2E, and the reflectors 6a to 6e.
It has a light receiver for receiving the reflected light 2R of the light beam 2E reflected by 6d. The light emitting device has a light emitting diode, and the light receiving device has a photodiode for converting incident light into an electric signal. The light emitter and the light receiver are housed in the casing 3.

The light beam emitted upward from the light emitter in the casing 3 is a rotating mirror (hereinafter simply referred to as a mirror).
At 4, the light is changed in direction by being refracted and reflected in a right angle direction, and is projected from the scanning device 2 to the outside. The mirror 4 is rotated by the motor 5 around the rotation center axis 8 in the direction of arrow 17, and the rotation of the mirror 4 causes the light beam 2E to rotate and scan about the rotation center axis 8 in the direction of arrow R. The rotation angle of the light beam 2E from the projection direction, that is, the reference direction, which is determined by the rotation position of the mirror 4, is detected by an encoder 7 attached to the motor 5.

The scanning device 2 has a gimbal oscillating mechanism (hereinafter, simply referred to as an oscillating mechanism) for oscillating scanning while continuously changing the vertical projection angle of the light beam 2E.
The swing mechanism includes an outer ring member 11 pivotally supported on a shaft 12 of the bracket 9 and a shaft (not shown) of the bracket 10, and an inner ring member 14 provided inside the outer ring member 11. And The inner ring member 14 includes a shaft 13 provided on the outer ring member 11 on a line orthogonal to an extension of the support shaft of the outer ring member 11 and the other shaft 20 provided at a position facing the shaft 13 ( 1 (shown in FIG. 1).

The rotation center axis 8 of the mirror 4 is perpendicular to the angle φ.
And the inclination direction (hereinafter referred to as the swing direction) changes continuously when the swing mechanism is driven by the swing drive motor 15, and the arrow 17a Rotate as shown by. As a result, the angle of the scanning surface by the rotation scanning of the light beam 2E, that is, the projection direction of the light beam 2E continuously changes in the vertical direction, and the swing scanning is performed.

Next, the scanning device and the swing driving device of the swing mechanism will be described in detail. FIG. 3 is a sectional view of a main part of the scanning device 2 mounted on the self-propelled vehicle 1, and the same reference numerals as those in FIG. 2 indicate the same or equivalent parts. The mirror 4 is attached to one end 5a of a shaft of the motor 5 via a base 4a. on the other hand,
The other end 5 b of the shaft of the motor 5 is connected to the shaft 7 a of the encoder 7 by a connection fitting 19. The output pulse of the encoder 7 is transmitted to a control device (not shown), and is used for calculating the rotation angle and the number of rotations of the mirror 4.

A suction plate 34 is provided on the pedestal 4a of the mirror 4. The suction plate 34 is made of a magnetic material, for example, iron. The attraction plate 34 is attracted to the electromagnet 16 by the energization of the electromagnet 16, and the stop position of the mirror 4 is fixed at an arbitrary timing when the electromagnet 16 is energized.

The casing 3 is attached to the lower surface of the inner ring member 14. The means for attaching the casing 3 is not shown, but a known fastening means such as bolting may be used as appropriate.

A shaft 22 is inserted through a bearing 21 mounted on the upper surface of the vehicle 1. A small disk 23 is fixed to one end of the shaft 22, and a large disk 24 is fixed to the other end. The small disk 23 has an eccentric shaft 23a protruding at a position eccentric with respect to the shaft 22, and the large disk 24 has an eccentric shaft 24a protruding at a position eccentric with respect to the shaft 22. The eccentric directions of the eccentric shaft 23a and the eccentric shaft 24a are shifted from each other by 90 degrees.

The shaft 15a of the oscillating motor 15 is
The L-shaped block 32 is fixed to the shaft 15a. That is, the eccentric shaft 23
A and 24a are also eccentric with respect to the shaft 15a by the same amount as the eccentricity with respect to the shaft 22, and the shaft 15a, the eccentric shaft 23a, the shaft 22, and the eccentric shaft 24a of the motor 15 form a crankshaft. The rotation of the rotation shaft 15a by the swing motor 15 is transmitted to the eccentric shaft 23a by the block 32,
The shaft 22 rotates. As a result, the eccentric shaft 24a also rotates about the shaft 22.

The eccentric shaft 23a is rotatably fitted to a circumscribed ring 23b, and a block 25 is pivotally supported on the circumscribed ring 23b. The block 25 is connected to a spherical bearing 27 that receives a shaft (not shown) protruding from the inner ring member 14 by a connecting bolt 26.

As described above, since the small disk 23 and the inner ring member 14 are connected to each other, the rotational movement of the eccentric shaft 23a with respect to the small disk 23 is caused by the vertical movement of the inner ring member 14 about the shafts 13 and 20. Is converted into a rocking motion.

On the other hand, the eccentric shaft 24 protruding from the large disc 24
a is received by the spherical bearing 28. Outer ring member 1
A shaft 29 protrudes from 1 and a spherical bearing 30 is supported by the shaft 29. The spherical bearing 28 and the spherical bearing 30 are connected by a connecting bolt 31. With such a configuration, the outer ring member 11 is also connected to the inner ring member 1.
Like FIG. 4, it is swung about the shaft 12 (see FIG. 2) and a shaft (not shown) at a position facing the shaft 12 (see FIG. 2).

When the swing of the outer ring member 11 and the inner ring member 14 is combined, the rotation center axis 8 of the mirror 4 of the scanning device 2 attached to the inner ring member 14
Turns around the intersection of the swing center axes of the two ring members 11 and 14 with the inclination angle φ. The trajectory of the rotation center axis 8 due to the turning is a side surface of a cone having the intersection point as a vertex (hereinafter, simply referred to as a cone). Since the casing 3 containing the light emitting device and the light receiving device is also attached to the lower surface of the inner ring member 14, the casing 3 swings integrally with the inner ring member 14.

The connecting bolt 26 is threaded in opposite directions at both ends. When the connecting bolt 26 is rotated,
The connecting bolt 26 is connected to the block 25 and the spherical bearing 2.
7, the connection length between the spherical bearing 27 and the block 25 can be adjusted. Like the connection bolt 26, the connection bolt 31 adjusts the connection length with the spherical bearings 28 and 30 into which the connection bolt 31 is screwed.

The large disk 24 is provided with a thin disk 24b, and the thin disk 24b is provided with a sensor 33 for detecting a swing reference across the thin disk 24b. For example, the sensor 33 is a metal detection sensor or a light transmission type sensor, and the thin disk 2
By forming a slit at a predetermined position on the circumference of 4b, a swing reference position can be detected based on the slit detection signal output from the sensor 33.

An encoder 35 for detecting the rotational position of the motor 15 is attached to the rear of the motor 15. From the output signal of the encoder 35 and the output signal of the sensor 33, the tilt direction of the rotation center axis 8 of the mirror 4, that is, the swing direction can be detected.

The means for detecting the swing direction of the rotation center shaft 8 is not limited to the one using the encoder 35 and the sensor 33. For example, a slit for detecting the amount of rotation of the thin disk 24b may be formed in the thin disk 24b separately from the slit for detecting the reference position, and these two types of slits may be respectively detected by two sensors. . Also,
The encoder 35 may be configured to take out both signals indicating the amount of rotation of the motor 15 and the rotation reference position.

In order to scan the light beam evenly in the vertical direction and to simplify the process of receiving the reflected light, it is desirable that the swing trajectory of the rotation center axis 8 be a cone, but not necessarily a cone. In both cases, the bottom surface may be a cone other than a circle. For example, if the eccentric amounts of the eccentric shafts 23a and 24a are changed so that the maximum inclination angles of the outer ring member 11 and the inner ring member 14 are different, the rotation center shaft 8
The trajectory drawn by the swing of is an elliptical cone.

In the present embodiment, the amount of eccentricity of the eccentric shafts 23a and 24a is adjusted so that the swing locus is substantially conical, that is, the maximum inclination angle of each of the outer ring member 11 and the inner ring member 14 is the same. You have set.

In this embodiment, the outer ring member 11 and the inner ring member 14 are driven by one motor, but each ring member may be driven by a separate motor. In this case, it is needless to say that the respective motors are synchronously rotated so that the rotation center axis 8 draws a desired conical shape.

Next, the light trace of the light beam by the scanning device of this embodiment will be described with reference to FIG. FIG. 4 shows a modeled light trace drawn on a virtual cylindrical surface having a constant radius centered on the mirror 4.

As shown in the figure, the light beam 2E projected from the scanning device 2 forms a net-like light trace on the assumed cylindrical surface by the concentric movement of the rotation center axis 8 of the mirror 4. Draw. In this embodiment, the rotation speed of the mirror 4 is set to 27
Since 00 rpm and the number of swings of the rotation center shaft 8, that is, the rotation speed of the shaft 22, are 90 rpm, the mirror 4 itself rotates 30 times while the rotation center shaft 8 makes one conical rotation. That is, while the rotation center axis 8 makes one rotation of drawing a cone, 30 light traces cross any vertical line 18 on the cylindrical surface. In this way, by making the cycle of the mirror 4 make one rotation sufficiently shorter than the cycle of the rotation center axis 8 making a cone and making one rotation, a rotation scanning trajectory with a fine pitch can be drawn. By this fine rotation scanning, the light trace crosses the reflector 6 at least once while the rotation center axis 8 performs one conical movement, and the light reflected by the reflector 6 can be reliably detected. In FIG. 4, the number of light traces is smaller than the actual number in order to avoid complication and facilitate drawing.

Next, a basic principle for detecting the position and traveling direction of the vehicle 1 in the traveling area will be described. FIG. 5 and FIG. 6 are diagrams showing the positions of the self-propelled vehicle 1 and the reflectors 6a to 6d in the coordinate system indicating the traveling area of the self-propelled vehicle 1. In the figure, the arrangement positions of the reflectors 6a to 6d, that is, the reference points A, B, C, D, and the position T (Xp, Yp) of the self-propelled vehicle 1 have the reference point B as the origin and the reference point B
Is represented by an xy coordinate system in which a straight line connecting C and C is an x-axis.

As shown in the figure, the position T of the self-propelled vehicle 1 exists on the circumscribed circle of the triangle ATB, and at the same time, the triangle BTC
Exists on the circumcircle of. Therefore, the position of the self-propelled vehicle 1 is obtained by calculating the intersection of the circumscribed circles Q and P of these two triangles. 2 of circumscribed circles Q and P
Since one of the intersections is the reference point B, that is, the origin, the other intersection is the position of the vehicle 1. The calculation formula for calculating the position of the self-propelled vehicle 1 according to such a principle is disclosed in Japanese Patent Application Laid-Open No. 1-28-28, filed by the present applicant.
7415 and JP-A-1-316808.

The traveling direction of the self-propelled vehicle 1 is calculated using the following equation. 6, the angle between the traveling direction of the self-propelled vehicle 1 and the x-axis is θf, the azimuth of the reference point C with respect to the traveling direction is θc, the x coordinate of the reference point C is xc, y
When the coordinates are Yp, θf = 360 ° −tan ⁻¹ {Yp / (xc−x)} − θc (1)

Next, the position of the self-propelled vehicle 1 obtained using the calculation formula described in the above publication and the traveling direction of the self-propelled vehicle 1 obtained based on the traveling direction obtained using the calculation formula (1) are described. The steering control will be described with reference to the drawings. Fig. 7 is a self-propelled vehicle 1
FIG. 6 is a diagram showing a positional relationship between the reference points A to D.

The self-propelled vehicle 1 starts traveling from a start position near the reference point B, travels on a planned traveling course 36, and returns to the home position 63. The traveling course includes a straight traveling process set in parallel with the interval L and a turning process connecting the straight traveling processes. After the self-propelled vehicle 1 has traveled the straight path, the y-coordinate reaches Ytn or Ytf,
The vehicle travels on the turning stroke with the steering angle fixed at a constant value, and shifts to the next adjacent straight traveling stroke. If the x-coordinate of the straight travel exceeds the final x-coordinate Xend, the vehicle returns to the home position 63 via the final turning stroke after traveling the straight travel.

In FIG. 7, for simplicity of description, the reference points A, B, C, and D are arranged at the vertices of a rectangle, and the straight travel distance is determined by the reference points A and B. The connecting straight line, that is, the y-axis is parallel, but the traveling course 36 can be set arbitrarily as long as the reference points A to D are arranged around the traveling course.

Next, a control procedure will be described with reference to a flowchart. The meanings of various parameters (symbols) used in the following flowcharts are as follows. θ (n): azimuth angle determined based on the received light signal, θ
q (n): predicted azimuth angle, Cg (i): number of light receptions by detection block, Am (i): detection azimuth angle by detection block, C
p (n): number of light receptions at reference point n, Ap [n, I]: light receiving azimuth angle of reference point n, As [n, I]: swing direction when reference point n is detected, Cm [n, I] ... A mirror rotation counter value at the time of detecting the reference point n, Aps (k).
Azimuths in which ps (k) is set to n = 1 to 4 in ascending order, i: number of the detection block, j: number of detection blocks whose number of light receptions is equal to or greater than the first threshold value, k: number of detection blocks is second The number of detection blocks equal to or greater than the threshold value, I: a number indicating the order of storage in the swing direction when the mirror 4 is rotated a predetermined number of times and the reference point n is detected, J: the mirror 4 is rotated a predetermined number of times , The maximum value of the number of continuous detections of the reference point n when the mirror 4 is rotated a predetermined number of times, and the storage of the swing direction when the maximum value of the number of continuous detections occurs. Asc (n): swinging direction that can capture reference point n with high probability, Ac (n): azimuth angle determined based on the received light signal in the straight traveling process, θt (n) …
First, the entire steering control will be described with reference to the flowchart of FIG. 8. In FIG. 5, in step S1, the motor 5 is started to rotate the mirror 4, and the motor 15 is rotated.
To rotate the rotation center axis 8 of the mirror 4.
Here, the reflectors 6a to 6d installed at the reference points A to D
Motor 15 so that the light beam can be reliably irradiated.
Rotate at low speed.

In step S2, it is determined whether or not the mode is the encoder output correction mode. This determination is made based on whether the correction mode selection switch is on or off. The self-propelled vehicle 1
In the operation performed for the first time, it is necessary to select a correction mode to correct the encoder output, so the selection switch is set to the ON side.

When the selection switch is set to the ON side, a separate measure is taken so that reflected light can be received from only one of the reflectors 6a to 6d. The selected one is hereinafter referred to as a correction pole. For example, a light beam is emitted only to the correction pole by covering or tilting the three reflectors out of the four reflectors except the correction pole, and the reflected light is received by the light receiver. To be detected by.

Further, of the light beam projected from the scanning device 2, not a whole of the vertical scanning width (= both ends) but a predetermined range of the upper limit or lower limit (= only one end) of the correction pole. Scanning device 2 to irradiate the reflecting surface
And the correction pole in the height direction are set in advance. Specifically, the upper half of the correction pole is hidden, or the lower half of the light receiver is hidden. In addition, self-propelled car 1
Is arranged so that the straight running azimuth is directed to the correction pole.

When it is detected that the switch is set to the ON side, the determination in step S2 becomes affirmative, and the process proceeds to step S3. Hereinafter, steps S3 to S
The process of No. 9 is called a correction mode process.

In the processing of the correction mode, step S
At 3, the memory area for storing the output correction value of the rotation detecting encoder 7 of the mirror 4, that is, the azimuth correction value, is cleared. In step S4, the memory area for storing the output correction value of the encoder 35 for detecting the tilt direction of the rotation center shaft 8, ie, the swing direction, as the swing correction value is cleared.

In step S5, an initial pole identification process is performed on the correction pole. Since light noise from a light-emitting object other than the correction pole may be detected, in this initial pole identification processing, the reflected light reception signal of the correction pole is specified from among the reception signals, and the incident azimuth angle is determined. The detected azimuth angle Aps of the correction pole is detected. The details of the initial pole identification processing will be described later with reference to FIG. In step S6, the azimuth Aps is stored in the memory as an azimuth correction value.

In step S7, a swing correction amount measuring process is performed. In this process, a correction amount for obtaining a reference signal of the encoder for detecting the swing direction is measured when the swing direction and the straight traveling azimuth of the self-propelled vehicle 1 match. That is, in the pole position measurement processing described later, each reference point 6a
In order to measure the distance between .about.6d and the self-propelled vehicle 1, it is necessary to easily and surely irradiate the light beam to the reference points 6a to 6d. For this purpose, it is necessary to obtain a reference signal of the swing direction detecting encoder 35 when the swing direction coincides with the traveling direction of the self-propelled vehicle 1, that is, the output signal of the sensor 33 in the configuration shown in FIG. There is. Because of this necessity, the process of step S7 is performed. The details of the swing correction amount measurement processing will be described later with reference to FIG.

In step S8, the correction amount obtained in the above-described swing correction amount measurement processing is stored in the memory as a swing correction value. In step S9, an LED as a display unit is turned on to indicate that all the processes in the correction mode have been completed.

After recognizing the LED display, the operator switches the correction mode selection switch to the off side. At the same time, measures are taken so that reflected light can be obtained from reflectors other than the correction pole. That is, the cover is removed, or the reflector that has been turned down is put back up.

After the correction mode selection switch has been turned off, the determination in step S2 is negative and the routine proceeds to step S10, where processing in the normal running mode is started.

First, in step S10, initial pole identification is performed. Here, unlike the initial pole identification processing for the correction pole described above, the initial pole identification processing for determining the initial azimuth angle is performed for all the reflectors 6a to 6d installed around the traveling area. Details of this processing will be described later with reference to FIGS.

In step S11, a pole position measurement process is performed to measure each distance from the self-propelled vehicle 1 to the reference points A to D and calculate the position of each reference point, that is, the reference coordinate value in the xy coordinate system. . This process will be described later with reference to FIGS. 15, 16 and 17.

In step S12, the current position coordinates (Xp, Yp) of the self-propelled vehicle 1 are calculated based on the azimuths and coordinate values of the respective reference points calculated in steps S11 and S12.
Is calculated. In step S13, x of the current self-propelled vehicle 1
The coordinate Xp is set as the x coordinate Xref of the first straight travel. However, this set is an operation when the self-propelled vehicle 1 is at the traveling work start position.

In step S14, the motors 5 and 15
Is rotated at a predetermined speed to rotate and swing the mirror 4. In step S15, the engine rotation of the self-propelled vehicle 1 is connected to the drive wheels to start traveling.

After the self-propelled vehicle 1 starts running, the self-position (Xp, Yp) and the traveling direction θf of the self-propelled vehicle 1 are calculated, and the difference between the calculation result and the running course 36 is corrected. The steering control for changing the steering angle of the steered wheels of the self-propelled vehicle 1 is performed as described above (step S16). Since this steering control is not directly related to the present invention, a detailed description is omitted. This steering control is described in detail in Japanese Patent Application No. 3-126511.

Next, the reflected light receiving process common to the operation of this embodiment will be described. In the reflected light receiving process, a process for specifying the optical signals from the reflectors 6a to 6d among the optical signals detected by the light receiver is performed. For this purpose, first, the detection data of the light receiver is stored. In storing the detection data, the light signals obtained based on the output signal of the encoder 7 having substantially the same light receiving angle, that is, the azimuth angle are regarded as the detection signals of the light from the same reflector or the same light emitting object. Then, the light receiving data of the optical signals incident from the same or almost the same azimuth is stored in one group. Hereinafter, this group is referred to as a detection block. The number of the detection blocks is the sum of the number of reflectors and the number of other reflective objects. If no light signal from a light emitting object other than the reflectors 6a to 6d is detected, the number of detection blocks is reduced to four. Should be.

FIG. 9 is a flowchart of the reflected light receiving process started in response to the detection of the reflected light. In the preceding stage, that is, the process up to step S106, a detection block is set for each light receiving signal at substantially the same azimuth. The number of times of light reception Cg (i) in the detection block is counted and the detected azimuth is stored. Further, the latter part, that is, step S10
In steps 7 to S111, processing for storing various conditions when the reflector arranged at the reference point is detected is performed.

In FIG. 9, in step S99, the azimuth and the swing direction obtained from the output signals of the encoders 7 and 35 are read, and the parameters Mi and W are respectively read.
e.

In step S100, the parameter M
i and We are corrected with the azimuth correction value and the swing correction value, respectively, and set as new parameters Mi and We.

In step S101, it is determined whether or not the signal read in step S99 is due to chattering. If the mirror 4 has rotated only a small angle, for example, 1 ° from the previous processing, that is, if the read azimuth angles are almost the same, it is determined that chattering has occurred, and the optical signal detected later is ignored. If not due to chattering, the process proceeds to step S102.

In step S102, "0" is set to a variable i indicating the detection block number. In this embodiment, the mirror 4 makes 30 rotations while the rotation center axis 8 of the mirror makes one rotation along a conical trajectory. That is, while the rotation center axis 8 makes one rotation along a conical trajectory, the rotation scanning is performed 30 times. There is a possibility that the reflected light from the same reflector will be received many times by these 30 rotation scans. Light reception data relating to a plurality of light signals incident on the light receiver from substantially the same direction is stored in one detection block as detection data of light signals from the same reflector or reflecting object. At this time, the latest data is stored as representative data of the detected block.

In step S103, it is determined whether or not the number of light receptions Cg (i) for each detection block is "0". Since “0” is set in the parameter i in step S102, first, whether the number of times of light reception in the detection block of the detection block number “0” is “0”, that is, the optical signal first detected in this detection block Is determined.

In the first process, this determination is affirmative and the process proceeds to step S106, where the mirror angle, that is, the azimuth angle when the light is detected, is stored. The azimuth detected this time is stored as the azimuth Am (i) representing the detection block i,
The value of the number of times Cg (i) of receiving the optical signal in the detection block i is incremented.

In step S107, the counter value n for identifying the reference point is cleared. In this embodiment, the counter value “1” corresponds to the reference point A, the counter value “2” corresponds to the reference point B, the counter value “3” corresponds to the reference point C, and the counter value “4” corresponds to the reference point D. Let me do it. In step S108, the value n of the counter is incremented.

In step S109, it is determined whether or not the azimuth detected this time is substantially the same as the predicted azimuth θq (n). Regarding the reference point corresponding to the counter value n set in step S108, for example, when the counter value n is "1", it is determined whether or not the predicted azimuth and the detected azimuth substantially match with respect to the reference point A. You. The predicted azimuth θq (n) is
For example, a value obtained by adding the predicted change amount α to the azimuth at the time of detection this time may be used, but since the light receiving interval of the reflected light is shorter than the movement amount of the self-propelled vehicle 1, the current value is used as it is as the predicted azimuth. However, there is no practical problem and the processing is simple. This predicted azimuth angle θ (n) is set in an initial pole identification process described later.

If the determination in step S109 is negative,
In step S110, whether or not the comparison between all the predicted azimuths of the reference points A to D and the azimuth detected this time is completed is determined based on whether or not the counter value n is “4”. Until the determination in step S110 is affirmative, step S108,
The process of S109 is repeated.

If the predicted azimuth θq (n) substantially matches the detected azimuth, steps S109 to S11
Proceed to 1. In step S111, the received light data when the predetermined reference point is detected is stored. That is, the number of light receptions Cp (n) at the reference point n is incremented, and the detected azimuth angle Ap [n, Cp (n)] at the reference point n is tilted, ie, the swing direction As [n, of the rotation center axis 8 of the mirror 4. , Cp
(N)], and the rotation counter value Cm of the mirror 4
[N, Cp (n)] is stored. Rotation counter value Cm
[N, Cp (n)] is a value indicating the number of rotations of the mirror 4 counted from the reference swing direction determined by the output signal of the sensor 33.

If it is determined in step S103 that the number of light receptions Cg (i) in the detection block i is not "0", that is, if it is determined that light is not received for the first time, step S103 is executed.
Proceed to 104. In step S104, the detected azimuth is
Azimuth angle Am of the optical signal previously received by detection block i
It is determined whether or not they substantially match (i). If they are almost the same, the process proceeds to step S106, and the azimuth Am (i) of the detection block i is updated with the current detected azimuth.

If the determination in step S104 is negative, that is, if the azimuth Am (i) of the optical signal previously received in the detection block i does not match the azimuth detected this time, the other Judge as the light from the detection block,
In step S105, the detection block number (i) is incremented. In step S103, it is determined whether or not the first detection is performed for the incremented detection block number (i).

FIG. 10 shows an example of the received light data stored in step S106 of the reflected light receiving process. In the figure,
The vertical axis is the number of light receptions Cg (i) of the detection block i, and the horizontal axis is the azimuth Am (i) of each detection block i. As shown in the figure, if the number of the detection blocks is seven (i = 0 to 6), this means that the rotation center shaft 8 swings once.
This indicates that optical signals have been received from seven different directions. When the number of times of light reception Cg (i) is a plurality of detection blocks, the azimuth Am (i) is the latest detection data as described above. The reason why the numbers i of the detection blocks are not always arranged in ascending order of the azimuth angle is that the numbers are assigned in the order of receiving light.

Step S111 of the reflected light receiving process
FIG. 11 shows an example of the swing direction As [n, Cp (n)] and the rotation counter value Cm [n, Cp (n)] of the mirror 4 accumulated in the above. FIG. 3 shows a part of the received light data detected in one swing cycle. Swing direction As [n, Cp (n)]
Represents the rotation amount of the encoder 35 connected to the swing motor 15.

The rotation counter value Cm [n, Cp (n)]
Indicates a value obtained by measuring the number of rotations of the mirror 4 from the position where the swing direction is “0 °”, that is, from when the reference signal is output from the sensor 33 to when the reference point n is detected. The parameter I is a number indicating the storage order of the swing direction when the reference point n is detected. That is, the maximum value of the storage number I is the number of light receptions Cp (n) at the reference point n.

Next, the initial pole identification processing will be described in detail. In the initial pole identification process, the reflector located at the reference point is identified based on the received light data stored while the rotation center axis 8 swings once.

The initial pole discriminating process differs between the process in the correction mode and the process in the normal traveling mode in the following point. That is, in the correction mode processing (step S5), the azimuth angle is determined for one reflector selected as the correction pole, whereas in the normal traveling mode processing (step S10), the reflectors 6a to 6d are determined. The processing for determining the azimuth angle is performed for all of them.

The initial pole identification process in the process in the correction mode will be described in detail with reference to the flowchart in FIG. In the figure, in step S120, it is determined whether or not the swing direction has become “0 °”, that is, whether or not a predetermined reference position has been detected by the reference swing direction signal output sensor 33. If it is determined that the swing direction has become “0 °”, the process proceeds to step S121 to clear the data obtained by the reflected light receiving process.

In step S122, a reflected light receiving process is performed. Here, the former part of the reflected light receiving processing described with reference to FIG. 9, that is, the steps S99 to S106 is executed, and the process proceeds to step S123 in FIG. Also, in the initial pole discrimination processing in the correction mode processing, the azimuth correction value and the swing correction value are both not determined and are set to "0". An uncorrected value is used for the moving direction.

In step S123, the swing direction is set to "0".
° ”is determined again.
It is determined whether the cycle has ended. When one cycle of scanning is completed, the process proceeds to step S124.

In step S124, reference point selection processing (pole selection processing) is performed. In this selection processing, one of the detection blocks detected in the reflected light reception processing is selected from among the detection blocks having a large number of light receptions Cg (i), and a detection azimuth angle representing the detection block, that is, the latest data in the detection block is selected. Am (i) is set as the azimuth Aps of the correction pole.

In this pole selection process, the pole selection modes "1" to "3" which are the basis of the determination in the next step S125 are also determined. The pole selection process is further described in detail with respect to FIG.

In step S125, it is determined whether the pole selection mode determined in the pole selection processing is any one of "1" to "3", and the subsequent processing is determined. When the pole selection mode is “1”, the correction pole is identified, and the azimuth thereof is detected, and the processing shown in this flowchart is ended.

When the pole selection mode is "2", the number of light receptions Cg (i) has not reached the predetermined value.
Returning to step S122, the reflected light receiving process is continued.
Further, the case where the pole selection mode is “3” is a case where there is an unexpected reflecting object and there are five or more detection blocks whose light receiving frequency Cg (i) is equal to or more than a predetermined value. In this case, it is determined that the reference point cannot be specified from the detection block, the process returns to step S121, and the initial pole identification process is restarted from the beginning.

FIG. 13 is a flowchart of the pole selection process. Also in this pole selection processing, there is a difference between the processing in the correction mode and the processing in the normal traveling mode whether the number of reference points, that is, the number of reflectors is "1" or "4". Here, the processing in the correction mode will be described.

In this pole selection process, the number of light receptions Cg
It is determined whether or not there is a detection block in which (i) has reached a predetermined threshold value, and a pole selection mode used for the determination in the initial pole identification processing is determined. If the number of detection blocks reaching the predetermined threshold value is 1, the light reception data of the detection block is determined to be data relating to the correction pole, and the pole selection mode is set to "1".
If there is no detected block that has reached the predetermined threshold, the poll selection mode is set to "2" in order to continue collecting data. When a large number of detection blocks have reached the predetermined threshold value, the correction pole cannot be specified. Therefore, the data is temporarily cleared and the data is collected again, so that the pole selection mode is set to “3”.

The threshold value of the number of times of light reception Cg (i) is “3”
And “5” were set. And the number of light receptions Cg
The number of detected blocks for which (i) has reached the first threshold “3” is stored as a parameter j, and the second threshold “5” is stored.
Is stored as a parameter k. Then, based on the parameters j and k, it is determined whether or not the reference point can be identified.

In FIG. 13, first, in step S130, the data obtained in the reflected light receiving process, that is, the detected azimuth angle Am (i) and the number of times Cg (i) of receiving the reflected light are read.
In step S131, the parameters i, j, and k are cleared. In step S132, the detection block (i)
It is determined whether or not the number of times of light reception Cg (i) is more than three times. If the number of light receptions Cg (i) is more than three, step S
Proceeding to 133, the parameter j indicating the number of detection blocks for which the number of light receptions Cg (i) is 3 or more is incremented.

In step S134, the number of light receptions Cg
It is determined whether (i) is more than five times. Number of received light is 5
If the number is greater than the number of times, the process proceeds to step S135, and a parameter k indicating the number of detection blocks for which the number of light receptions is 5 or more is set.
Is incremented.

In step S136, it is determined whether or not the value of parameter k is "1" or more. As a result, it is determined whether or not a detection block in which the number of light receptions Cg (i) exceeds five, that is, a detection block that is considered to include an optical signal from the correction pole is detected. Number of received light C
If one detection block has g (i) exceeding 5 times,
In step S137, the detected azimuth Am (i) representing the detection block is changed to the azimuth Aps of the correction pole.
To be stored.

In step S138, the number i indicating the detected block is incremented. In step S139, it is determined whether or not the number of light receptions Cg (i) is “0”. If the number of light receptions Cg (i) is not “0”, it is determined that there is still a detection block in which the number of light receptions is stored, and the process returns to step S132. If the number of light receptions Cg (i) is “0”, it is determined that there is no detection block for which the check has not been completed, and the process proceeds to step S140.

In step S140, parameters j and k
Are both “1”, that is, whether the number of detection blocks having received light more than three times and the number of detection blocks having more than five light receptions are both the number of correction poles, that is, “1”. to decide.

If step S140 is affirmative, the flow advances to step S141 to set the pole selection mode to "1".

If the determination in step S140 is negative, the flow advances to step S142 to detect whether or not the parameter j is "1" or more, that is, whether or not there is one or more detection blocks in which the number of light receptions is three or more. I do. Step S14
When 2 is negative, the pole selection mode is set to "2" in step S143, and when affirmative, the pole selection mode is set to "3" in step S144. According to these determined pole selection modes, the determination in step S125 in the initial pole identification processing of FIG. 12 is performed.

In the correction mode processing, the azimuth Aps determined in the pole selection processing is stored as an azimuth correction value in step S6.

Next, step S shown in FIG.
7 is described. In the swing correction amount measurement process, a correction amount for matching the reference in the swing direction with the reference in the azimuth is determined. For this purpose, the present embodiment executes the following processing.

When the swinging direction coincides with the straight running direction of the self-propelled vehicle 1, that is, when the rotation center axis 8 is inclined in the straight running direction of the self-propelled vehicle 1, the light beam is directed downward. Therefore, if the value output from the encoder 35 indicating the swing direction when the light beam is directed to the lowermost position is detected, the value is a deviation from the reference of the azimuth angle, and the vehicle 1 travels straight. It means that it is out of azimuth.

FIG. 14 is a diagram showing the relationship between the correction pole and the vertical scanning width of the light beam at the position of the correction pole, where the vertical axis represents the height of the light beam at the position of the correction pole and the horizontal axis. Indicates the output of the encoder 35, that is, the swing direction.

In the figure, the upper half of the correction pole 6x is covered by an opaque cover 44. Therefore, of the light beams projected on the correction pole 6x,
Only the reflected light at the lower end of the vertical scanning width is detected by the light receiver of the scanning device 2.

When the light beam is projected along the trajectory as shown, the reflected light starts to be detected in the swing direction Wa, and the swing direction W
At b, the reflected light is no longer detected. In other words, the swing direction W
A range between a and Wb is a range (capture range) in which the correction pole 6x can be detected. Therefore, an intermediate point Wc {Wc = (Wa + Wb) / 2} between the swing directions Wa and Wb, that is, a point where the light beam is directed to the bottom is detected, and this point Wc and the output signal Ws of the sensor 33 are detected. Difference (Wc-W
s) is determined as the swing correction amount.

The swing correction amount measurement processing based on the concept described with reference to FIG. 14 will be described with reference to the flowchart in FIG. In FIG. 15, step S150
In order to store the swing direction when the light incident from the straight traveling azimuth of the self-propelled vehicle 1 is detected in the reflected light receiving processing, 0 degree is set as the predicted azimuth angle θq (n).

In step S151, the swing direction is set to "0".
° ”. If it is determined that the swing direction has become“ 0 ° ”, the process proceeds to step S152, where the data obtained by the reflected light receiving process is cleared to collect the reflected light data. The reflected light receiving process is performed in step S153.This process is as described with reference to FIG. 9, and data as shown in FIGS. 10 and 11 is collected.

In step S154, the swing direction is "0".
It is determined whether or not one cycle of swing has ended based on whether or not the angle has become “°”. The received light data until this determination is affirmative, that is, while the swing of the rotation center shaft 8 in a cone is performed once. Is accumulated.

When it is determined that the rotation of the rotation center shaft 8 for one cycle has been completed, the flow proceeds to step S155. In Step S155, Step S11 of the reflected light receiving process of FIG.
Read the data obtained in step 1. In step S156,
The number of reflectors that will reflect the light beam is only one correction pole, and the variable n is set to “1” in accordance with this.

In step S157, a parameter Cp indicating the number of times a reference point (here, a correction pole) is detected is set.
It is determined whether or not (1) is "0", and it is determined whether or not the reference point has been detected one or more times. If this determination is affirmative, it is determined that no light is received from the direction in which the predicted azimuth angle θq is 0 degrees, that is, the direction in which the correction pole is installed, and the process returns to step S151 to collect light reception data again.

If step S157 is negative, the flow advances to step S158 to set "1" to the parameter I. A plurality of rotation scans are performed in one swing cycle, and a correction pole is also detected a plurality of times in the plurality of rotation scans. This parameter I is a number (storage number) for identifying the order in which the swing direction and the counter value for counting the number of rotations of the mirror 4 when the correction pole is detected a plurality of times during one swing cycle are stored. .

In step S159, parameters K and e are cleared. When a light-receiving signal from the same direction is detected each time the mirror 4 makes one rotation, that is, when an optical signal from the same direction is continuously detected each time, the maximum value of the number of consecutive times, that is, the maximum number of consecutive detections is determined by this parameter. Indicated by K. The parameter e is the storage number I of the swing direction.
Of these, the memory number of the last swing direction when an optical signal is continuously detected is shown.

In step S160 in FIG. 16, "1" is set to a value J of a counter for counting the number of consecutive light receiving signals. In step S161, it is determined whether or not the number of detections Cp (n) of the correction pole is the same as the number I indicating the stored number in the swinging direction, so that the received light data detected and stored in the reflected light receiving processing is determined. Determine if everything has been checked. Normally, there are a plurality of received light data, and the storage number I is set to “1” in step S158. In this case, the determination in step S161 is negative, and
It is determined that all the light reception data has not been checked, and the process proceeds to step S162.

In step S162, the rotation counter value Cm of the mirror 4 is detected at the time of the present detection.
(N, I + 1) is the counter value Cm (n,
It is determined whether or not the value obtained by adding “1” to I) is obtained. That is, by determining the continuation of the counter value Cm, it is determined whether or not the light receiving signal is continuously detected every rotation of the mirror 4. If this judgment is affirmative,
Proceeding to step S163, the value of the counter value J and the value of the parameter I are incremented.

On the other hand, if step S162 is negative,
Proceeding to step S164, the counter value J becomes equal to the parameter K
It is determined whether or not the number of consecutive detections is greater than the number of consecutive detections stored so far. If this determination is affirmative, the process proceeds to step S165,
The maximum number of continuous detections K is updated with the number of continuous detections J this time,
The parameter e indicating the last stored number in the swing direction upon detection of the continuous light receiving signal is updated with the storage number I in the current swing direction. In step S166, the number I indicating the stored number in the swing direction is incremented.

On the other hand, if step S161 is affirmative, that is, if it is determined that all the light reception data checks have been completed, the flow advances to step S167. Step S16
7 and S168 correspond to steps S164 and S1 described above.
Since the processing is the same as that of step 65, the description is omitted.

In step S169, the storage number of the first swing direction when the maximum number of consecutive detections has occurred is calculated and set as the parameter I. In step S170, the swing direction As detected in the reflected light receiving process as the swing direction Wa
(N, I), that is, the data in the swing direction stored corresponding to the first storage number when the maximum number of consecutive detections has occurred is set. As the swing direction Wb, As
(N, e), that is, the data in the swing direction stored corresponding to the last storage number at which the maximum number of consecutive detections has occurred is set.

In step S171, the swing direction Wc in which the light beam is directed to the lowermost position is determined by finding the midpoint between the swing directions Wa and Wb. ｛Wc = (Wa + Wb)
/ 2｝. The calculated swing direction Wc is determined in step S in FIG.
The swing correction value of 8 is set in a storage unit constituted by a nonvolatile memory such as an EEPROM.

Next, details of the initial pole discriminating process (step S10) and the pole position measuring process (step S11) in the normal traveling mode will be described. In these processes, the values corrected by the azimuth correction value and the swing correction value determined by the above-described correction mode process are used as the azimuth and the swing direction.

First, the initial pole discrimination in step S10 is performed in the same manner as in FIG. 12, but there are some different processes and additional processes in the contents of the pole selection process (step S124). .

That is, in the process of the normal traveling mode, 4
Since it is necessary to identify the four reflectors 6a to 6d arranged at the reference points, steps S136, S140, and S140 are performed in the flowchart of the pole selection processing.
At 142, the value to be compared with the parameters j and k is "1".
Instead of "4".

Further, in the processing in the normal traveling mode, the number of detection blocks whose light receiving frequency Cg (i) is 5 or more is detected as many as the number of reference points, that is, “4”. Representative azimuth angle value Aps (k)
Are set in Aps (1) to (4) in ascending order of their values, between steps S140 and S141.

Only the above differences and the processing to be added are performed, and the rest is performed in the same manner as the initial pole identification processing in the correction mode processing, and the azimuth initial values θ (n) of the four reflectors 6a to 6d are set. Is obtained.

In the processing in the normal traveling mode, after the initial pole identification processing, with respect to each reference point (n),
The azimuth θ (n) and the predicted azimuth θq (n) are set.
Aps is used as the azimuth angle θ (n) and the predicted azimuth angle θq (n).
(1) to Aps (4) can be determined by directly substituting them.

Next, the pole position measurement processing (step S11) for determining the coordinates of each of the reference points A to D will be described. In order to determine the coordinates of the reference point, first,
And the distance between each of the reference points is measured. Then, the coordinates of the reference point are calculated based on the measured distance and the azimuth of each reference point viewed from the self-propelled vehicle 1. In this embodiment, the distances to the reference points are calculated based on the phase difference between the phase of the optical signal projected from the light emitting unit and the phase of the optical signal detected by the light receiving unit.

In measuring the distance, it is necessary to determine a swing direction in which the reflector can be irradiated with the light beam with high probability. In the present embodiment, the processing for this, that is, the pole capture swing direction determination processing is performed based on the following idea.

First, when a reference point is detected continuously every one rotation during a plurality of rotation scans of the mirror 4, the value of the swing direction in the middle of the continuous range is set to the reference point with a high probability. It is determined that the swing direction is such that the beam can be irradiated. If the continuous detection of the reference points is distributed to a plurality of groups, the determination is made in the group having the largest number of continuous detections.

For example, when there is the data shown in FIG. 11, the counter value Cm (n) when the reference point is detected
Are continuous when the number I is "1" to "2",
This is the case of “3” to “6”, which are distributed into two groups. Of these, when the storage number I is “3” to “6”, the number of consecutive times is larger. Therefore, in this example, an intermediate point in the swing direction when the storage number I is “3” and “6” is obtained. That is, (108.6 + 144.4 °) / 2
= 126.5 ° is determined as the swing direction in which the reference point can be irradiated with the light beam with a high probability, that is, the swing direction in which the reference point (pole) is captured.

The same process as the swing correction amount measurement process (FIG. 15) can be used for the pole capture swing direction determination process. That is, in the swing correction amount measurement processing, the midpoint of the range in which the light beam can be continuously irradiated on one reflector set as the correction pole is determined. In this pole capture swing direction determination processing, a continuous light receiving range may be detected for the four reflectors 6a to 6b, and a swing direction value at an intermediate point between the continuous light receiving ranges may be detected.

Referring to FIG. 17, a description will be given of a pole position measurement process performed using the detected pole swing direction. In FIG. 17, in step S180, the pole-capturing swing direction determination processing is performed to detect the pole-capturing swing direction Asc (n) at each reference point. In step S181, a counter value n for identifying a reference point
Is cleared, and in step S182, the value n is incremented.

In step S183, the pole capture swing direction Asc (n) is set to the reference point n capture swing direction TG-SW.
And the azimuth θ (n) obtained in the initial pole discriminating process is set as a reference point and a reference point n capture azimuth TG
-Set as ML.

In step S184, swing position control is performed to match the swing direction with the reference point n capturing swing direction TG-SW. In this swing position control, when the output value of the encoder 35 becomes the reference point n capture swing direction TG-SW, the rotation of the rotation center shaft 8 is stopped, and the inclination direction of the rotation center shaft 8, that is, the swing direction is fixed. I do.

In the step S185, it is determined whether or not the reference point n is continuously detected in a state where the inclination direction of the rotation center shaft 8 is fixed. If the determination is negative, an error is displayed or an alarm is issued to perform sensor failure processing and stop the processing.

On the other hand, if the reference point n has been continuously detected, the flow advances to step S187 to perform mirror position control. In this mirror position control, the motor 5 is rotated based on the output value of the encoder 7 until the value substantially matches the reference point n capture azimuth angle TG-ML. When the value of the encoder 7 substantially matches the reference point n capture azimuth angle TG-ML, the motor 5 is stopped, and the electromagnet 16 is energized so that the stopped state can be maintained. Thereby, the suction plate 3
4 is attracted to the electromagnet 16, and the stop position of the motor 5, that is, the stop position of the mirror 4, is fixed.

In the step S188, the reflected light from the reference point n is detected for 3 seconds or more. Of course, this time is not limited to 3 seconds, and it is sufficient if it is possible to confirm whether the orientation of the mirror 4 has been securely fixed.

In step S189, the distance between the self-propelled vehicle 1 and the reference point n is measured based on the detection result of the reflected light from the reference point n. This is calculated based on, for example, the phase difference between the light beam emitted from the light emitting device and the reflected light detected by the light receiving device.
In step S190, it is determined whether or not the counter value n for identifying the reference point is "4", that is, whether or not the measurement of the distance from the self-propelled vehicle 1 has been completed for all the reference points. In step S191, based on the detected distance between each reference point and the self-propelled vehicle 1 and the azimuth obtained by the initial pole identification processing, an arbitrary coordinate system having the self-propelled vehicle 1 as an origin, for example, a self-propelled vehicle With car 1 as the origin, reference point A
Each reference point A to D on a coordinate system whose direction is the positive direction of the x-axis
Is calculated [X (n), Y (n)]. Step S
At 192, the coordinates are converted into coordinates [x (n), y (n)] on a coordinate system having the reference point B as the origin.

As described above, in this embodiment, the azimuth angle detecting encoder 7 and the oscillating direction detecting encoder 35 are set so that the reference of the azimuth angle and the oscillating direction coincides with the straight running azimuth of the vehicle 1. Output is corrected. The output data after this correction is used for initial pole identification processing, pole position measurement processing, and steering control.

The method of selecting the correction pole is as follows.
A method is used in which any one of the reflectors 6a to 6d is left, and the other reflecting surface is covered or turned down. In the swing correction amount measurement, the upper half of the correction pole is covered. However, in addition to such a treatment, it is also possible to irradiate only the lower half of one reflector with a light beam by covering a predetermined range of the mirror 4 housing portion of the scanning device 2 and blocking the projection light. In effect, the same operation as when one correction pole is set can be performed. That is, a predetermined portion in the traveling direction of the scanning device 2 may be left as a transparent portion in the form of a slit in the vertical direction, and may be covered with another cover.

Although the lower end of the vertical scanning width of the light beam corresponds to the reflecting surface of the correction pole, the upper end of the vertical scanning width of the light beam corresponds to the reflecting surface of the correcting pole. As described above, the lower half of the correction pole may be covered with the cover, or a predetermined range of the scanning device 2 corresponding to the upper half or the lower half of the projection range of the light beam may be covered.

In this case, the swinging direction in which the light beam is directed to the lowermost side is Wc = {(Wa + Wb) / 2} + 180 °.

Next, the function of the operation described with reference to the flowchart will be described. FIG. 18 is a block diagram showing main functions of the pole position measurement processing unit. In the figure, a scanning device 2 projects a light beam 2E, and detects a reflected light 2R of the light beam 2E at a reflector 6. The scanning device 2 supplies a signal indicating the phase of the light beam 2R and the phase of the reflected light 2E to the distance detection unit 50. Each time the scanning device 2 detects the reflected light 2R, the scanning device 2 outputs the values of the azimuth angle detection encoder 7 and the swing direction detection encoder 35 to the light receiving processing unit 4.
6 is output.

In the light receiving processing unit 46, based on the received light data obtained by the reflected light receiving processing, the azimuth of the reflector (the correction pole in the correction mode processing) and the reflector (the correction mode) In the process (1), a swing direction at the time of detection of the correction pole) and a mirror rotation counter value indicating continuous / non-continuous of the received light data are output.

The switches SW1 and SW2 are switched to the illustrated side in the correction mode processing, and are switched to the opposite side in the normal traveling mode processing. First, switch S
In the processing in the correction mode in which W1 and SW2 are switched to the illustrated side, the oscillating direction correction value determination unit 45 determines the light beam based on the oscillating direction and the mirror rotation counter value supplied via the switch SW1. Determines the swing direction Wc when is projected to the bottom, and the swing direction W
c is stored as a swing correction value for correcting the deviation of the self-propelled vehicle 1 from the straight traveling direction in the correction value storage unit 47 constituted by a nonvolatile memory such as an EEPROM. The swing correction value is input to the light receiving processing unit 46, and is used for correcting the swing direction output from the light receiving processing unit 46.

On the other hand, the azimuth correction value determination unit 52 determines the azimuth Aps based on the azimuth supplied via the switch SW2, and this azimuth Aps deviates from the straight running azimuth of the self-propelled vehicle 1. Is stored in the azimuth correction value storage unit 52 as an azimuth correction value for correcting. The azimuth correction value is input to the light receiving processing unit 46 and used for correcting the azimuth output from the light receiving processing unit 46.

In the normal traveling mode processing, when the switch SW1 is switched to the side opposite to that shown in the figure, the swing direction and the counter value used to determine the continuous / non-continuous of the received light data are within the swing direction range. It is supplied to the calculation unit 51. The swing direction range calculation unit 51 determines the number of times of continuous detection of an optical signal for each rotation of the mirror 4, that is, the light receiving range where the number of continuous light receiving is the largest, based on the supplied data. The maximum value and the minimum value of the direction are output to the reference point capturing swing direction calculation unit 48. The reference point capturing / oscillating direction calculation unit 48 calculates the midpoint between the maximum value and the minimum value in the aforementioned oscillating direction, and determines a reference point at which the light beam can be irradiated with a high probability, that is, the reference point capturing / oscillating direction. The reference point capturing swing direction is input to the projection direction fixing unit 49.

On the other hand, when the switch SW2 is switched to the side opposite to that shown in the processing in the normal traveling mode, the azimuth is changed to the reference point position calculation section 53 and the projection direction fixing section 49.
Supplied to The projection direction fixing unit 49 adjusts the direction of the mirror 4, that is, the swing direction and the azimuth, with the supplied azimuth angle and the reference point capturing swing direction as target values, and adjusts the direction to the direction indicated by these target values. The mirror 4 is fixed.

When the mirror 4 is fixed and the light beam projection direction is fixed to the reflector 6, the distance between the self-propelled vehicle 1 and each reference point is measured by the distance detection unit 50. The distance measurement is
The calculation is performed based on the difference between the phase of the light beam 2R output from the scanning device 2 and the phase of the reflected light 2R.

The calculated distance data is supplied to a reference point position calculation section 53. The reference point position calculation section 53 calculates the position coordinates [X of the reference point based on the distance data and the azimuth.
(N), Y (n)] is calculated.

As described above, data corrected by the correction values supplied from the swing correction value storage unit 47 and the azimuth correction value storage unit 54 are output from the light reception processing unit 46.

Next, the function of the steering control unit of the self-propelled vehicle 1 will be described with reference to the functional block diagram of FIG. In the figure,
18 denote the same or equivalent parts. In FIG. 1, the position coordinates [X (n), Y (n)] of the reference point calculated by the reference point position calculation unit 53 are calculated by the position / traveling direction calculation unit 5.
5 is input. The position / traveling azimuth calculation unit 5 calculates the current position and traveling direction of the self-propelled vehicle 1 based on the position coordinates of the reference point and the corrected azimuth data input from the light receiving processing unit 46.

[0145] The traveling course setting section 56 stores data indicating the traveling course of the self-propelled vehicle 1. Then, the steering unit 57 compares the position and the traveling direction of the self-propelled vehicle 1 with the traveling course, and based on the comparison result, the self-propelled vehicle 1
Steering.

The steering control of the self-propelled vehicle 1 is described in detail in JP-A-1-287415, JP-A-1-316808, and Japanese Patent Application No. 3-126511.

[0147]

As is apparent from the above description, according to the present invention, the following effects can be achieved. (1) It is possible to greatly reduce a guiding error caused by a deviation between a reference direction for measuring an azimuth angle and a traveling direction of a moving body due to an assembly error or a variation in parts. (2) Since the mounting accuracy of the encoder for detecting the azimuth of the reference point and the variation of the components can all be handled by software processing, this does not adversely affect the azimuth detection accuracy. Therefore, no skill is required for mounting the encoder, and no special jig is required. (3) Since the reflector used for the work can be used for determining the correction value, and the correction value can be stored in the writable nonvolatile storage means, not at the production site of the self-propelled vehicle,
After disassembly and assembly for maintenance, a correction value can be set at the work site. After setting the correction value, no adjustment is required regardless of the change of the work area.

[Brief description of the drawings]

FIG. 1 is a block diagram showing main functions of a self-propelled vehicle steering control.

FIG. 2 is a perspective view showing a traveling state of the self-propelled vehicle.

FIG. 3 is a sectional view of a main part of the light beam scanning device.

FIG. 4 is a perspective view showing a light trace of a light beam.

FIG. 5 is an explanatory diagram of the principle of calculating the position of the self-propelled vehicle.

FIG. 6 is a diagram illustrating the principle of calculating the traveling direction of the self-propelled vehicle.

FIG. 7 is a diagram showing a traveling course of a self-propelled vehicle and an arrangement state of reflectors.

FIG. 8 is a general flowchart of the embodiment.

FIG. 9 is a flowchart of a reflected light receiving process.

FIG. 10 is a diagram illustrating an azimuth angle and the number of times of light reception for each detection block.

FIG. 11 is a diagram showing data on a mirror rotation speed and a swing direction when a reference point is detected.

FIG. 12 is a flowchart of an initial pole identification process.

FIG. 13 is a flowchart of a pole selection process.

FIG. 14 is a diagram showing the height of a light beam at a reflector position.

FIG. 15 is a first flowchart of a swing correction amount measurement process.

FIG. 16 is a second flowchart of the swing correction amount measurement process.

FIG. 17 is a flowchart of a pole position measurement process.

FIG. 18 is a block diagram illustrating main functions of a reference point detection process.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 ... Self-propelled vehicle, 2 ... Light beam scanning device, 4 ... Rotating mirror, 5 ... Mirror drive motor, 6, 6a-6d ... Light reflector, 7 ... Encoder, 8 ... Rotation center axis, 15 ... Swing motor 33: swing reference detection sensor, 34 ...
Suction plate, 35 ... Encoder, 36 ... Traveling course,
45: swing correction value determination unit; 46: light reception processing unit;
... Swing correction value storage unit, 52 ... Azimuth correction value determination unit, 5
4. Azimuth correction value storage unit

──────────────────────────────────────────────────続 き Continuation of the front page (56) References JP-A-4-3209 (JP, A) JP-A-62-295121 (JP, A) JP-A-4-315085 (JP, A) 53608 (JP, U) (58) Fields investigated (Int. Cl. ⁷ , DB name) G05D 1/00-1/12 G01S 5/00-5/14

Claims (3)

(57) [Claims] 1. A rotary scanning means mounted on a moving body for rotating and scanning a light beam in a circumferential direction about the moving body, and reflected by a light reflecting means provided at a position distant from the moving body. A light receiving unit for receiving the reflected light of the light beam; calculating a self-position of the moving body based on a light receiving signal of the reflected light; and calculating the calculated position detection result along a scheduled traveling course. In a steering control device for a moving body that travels a moving body, in a state where only one of the light reflecting means can be detected and a straight azimuth of the moving body is set so as to face the one light reflecting means. Means for detecting an azimuth of the one light reflecting means detected when the rotary scanning means is rotationally scanned; and a difference between the detected azimuth of the one light reflecting means and the straight azimuth. As the deviation azimuth correction amount Non-volatile storage means for storing the azimuth signal when the light beam reflected by the light reflection means provided at a position distant from the moving body is received by the light reception means, the storage means And a self-position of the moving body is calculated using the corrected azimuth angle signal, the self-position of the moving body. 2. The method according to claim 1, wherein the rotating scanning unit is configured to scan the light beam in a circumferential direction while swinging the light beam in a vertical direction, and to scan the light beam prior to traveling of the moving body. Means for storing the azimuth detected by the light receiving means and the number of incident light detections at the azimuth angle; and that the number of incident light detections at the azimuth stored in the storage means has reached the predetermined number. Means for detecting, when detecting incident light more than a predetermined number of times from substantially the same azimuth, means for detecting this azimuth as an azimuth angle of a predetermined light reflecting means as viewed from the moving body. 1
A steering control device for a moving object according to the above. 3. The steering control apparatus for a moving body according to claim 1, wherein the rotation scanning of the light beam is performed a plurality of times during one cycle of the vertical swing.