JPH05248862A - Position detecting apparatus - Google Patents

Abstract

PURPOSE:To detect the position of reflector based on the reflected light of an optical beam, which is emitted from an observing point and reflected from the reflector, even if the providing surfaces of the observing point and the reflector are deviated. CONSTITUTION:The optical beam, which is emitted from an optical scanning device 2, is turned and made to scan in the direction of an arrow R. At the same time, the optical beam is turned around an arrow 17a under the state wherein a central turning axis 8 is inclined by an angle phi. Reflected lights 2R from optical reflectors 6a-6d are received in high frequencies. The projecting angle of the optical beam in the vertical direction is detected by the scanning of the optical beam. The intermediate point is detected out of the range, wherein the projecting angles that are detected in every individual optical deflectors are distributed. The optical beam is directed toward the intermediate point, and the position of each of the reflectors 6a-6d is detected based on the received optical signal of the reflected light. When the projecting angle of the optical beam is deviated to one side of the height direction of the optical reflector, the range of the detected projecting angle is corrected.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a position detecting device,
In particular, self-propelled machines used in agriculture and civil engineering,
The present invention relates to a reference point position detection device used for steering control of an automatic transfer device used in a factory.

[0002]

2. Description of the Related Art Conventionally, as a device for detecting the current position of a moving body, means for scanning a light beam generated by the moving body in the circumferential direction around the moving body, and at least three locations apart from the moving body. An apparatus has been proposed which is fixed to the light source and has a light reflecting means for reflecting light in the incident direction and a light receiving means for receiving the light reflected by the light reflecting means (Japanese Patent Laid-Open No. 59-67476). .

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

In the above apparatus, even a slight error in the position information of the light reflecting means as the reference point adversely affects the control accuracy of the entire system. Therefore, for example, in work on farmland, the position information of the light reflecting means, that is, the interval and relative angle of each light reflecting means is accurately measured prior to the work each time the work area changes, and the result is input to the control device. I had to do it. Accurately measuring the intervals and relative angles of the respective light reflecting means installed in a wide work area such as farmland and inputting them has been extremely difficult work.

On the other hand, the present applicant has proposed a device capable of simplifying the work of measuring and inputting the position information (Japanese Patent Laid-Open No. 1-287415). In this device, the light beam emitted from the self-propelled vehicle is scanned, and the reflected light from the light reflecting means of this light beam is detected. The distance between the self-propelled vehicle and the light-reflecting means calculated based on the reflected-light detection signal and the azimuth angle of the light-reflecting means as viewed from the self-propelling vehicle are used to accurately determine the intervals and relative angles of the respective light-reflecting means. Measured to
And the work to input it is done automatically.

[0006]

By the way, a place where a device for measuring a distance or a relative angle between the light reflecting means is installed,
For example, the work area in which a vehicle equipped with this device travels is not always flat. Therefore, depending on the terrain where the vehicle is placed to measure the distance and azimuth, even if the light beam is rotationally scanned only in one direction, that is, in the horizontal direction, the light beam is emitted to the light reflecting means. It may not be possible to make it happen.

As a measure for solving such a problem, it is conceivable that the light beam is oscillated in the vertical direction in addition to the rotational scan in the horizontal direction. However, with such a method of swinging the light beam in the vertical direction as well,
In distance measurement for position detection prior to the start of driving,
In order to accurately irradiate the light beam on the light reflecting means, it is necessary to first perform accurate positioning in the horizontal direction and the vertical direction and fix the projection direction of the light beam to that position.

Therefore, it has been attempted to previously calculate a direction in which the reflected light from the reflecting means can be detected, and then fix the projection direction in that direction to measure the distance and the like. However, in this case, the irradiation position of the light beam may deviate from the upper end or the upper end of the light reflecting means due to a mechanical hysteresis error or the like.

Further, a sign such as a columnar corner cube prism is generally used as the light reflecting means arranged at the above-mentioned reference point. It may be installed with a large inclination. Therefore, there may be a difference of several tens of centimeters in the rotational scanning direction between the case of using the position near the upper end of the sign as a reference and the case of using the position near the lower end as a reference. If such measured data near the end is used as it is as the reference point data prior to traveling, it may adversely affect the control of the entire system.

With respect to the above problems, the object of the present invention is to provide a relatively central portion of the light reflecting means whose position is to be calculated, which is not extremely close to the upper end or the lower end of the vertical range in which the light beam is irradiated. By calculating the projection azimuth and swing angle that can reliably irradiate the light beam, and projecting the light beam in that direction and measuring the distance, etc., the position of the reference point can be detected at the preparatory stage of operation start, etc. It is to provide a position detection device that can be easily performed.

[0011]

[Means for Solving the Problems] By solving the above problems,
In order to achieve the object, the present invention provides a rotary scanning means for scanning a light beam emitted from a light beam generating means installed at an observation point in a circumferential direction around the observation point, and a rotation of the rotary scanning means. Oscillation in which the central axis is swung so as to draw a substantially conical locus, and the light beam is scanned around the observation point a plurality of times in the circumferential direction during one vertical swing cycle. Scanning means, light receiving means for receiving the light beam reflected by a plurality of light reflecting means provided at a position distant from the observation point, and azimuth angle in the rotational scanning direction when an optical signal is detected by the light receiving means. And means for detecting and storing the swing angle of the swing scanning means and reflection from the light reflecting means in the same direction based on the swing angle when the reflected light from the light reflecting means in the same direction is received. Receiving swing range that can receive light When the light receiving swing range is within the light beam projection range of the same azimuth, a substantially central value of the light receiving swing range is set to the light reflecting means. When a reference point capture projection direction suitable for projection is determined and one end of the light beam projection range overlaps the light reception swing range, the other end of the light reception swing range with the overlapping point as one end. A reference point capture swing direction determining means for determining a substantially central value up to the point as a reference point capture projection direction suitable for projecting a light beam on the light reflecting means, and a light in the reference point capture swing direction. It is characterized in that the beam is projected, the reflected light thereof is received, and the distance between the observation point and the light reflecting means is measured based on the received light signal.

[0012]

According to the present invention having the above characteristics, the rotary scanning is performed a plurality of times during one cycle of the vertical swing of the light beam so that the light beam can be easily irradiated to the light reflecting means. Therefore, assuming a cylindrical surface centered at the observation point, a mesh-like light trace by the light beam is drawn on the cylindrical surface.

When calculating the position of the reference point, that is, the position of the light reflecting means, which is performed prior to the start of operation, the light reflecting means is not extremely close to the upper end or the lower end of the vertical range in which the light beam is irradiated, It is possible to calculate the projection azimuth and the swing angle that can reliably irradiate the light beam to the relatively central portion, and project the light beam in that direction. Therefore, even if there is a mechanical play such as a hysteresis error, the light beam is less likely to deviate from the end of the light reflecting means.

Furthermore, when the light reflecting means is installed at an angle, it is possible to detect the position in a state where the influence of the error caused thereby is small.

[0015]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS 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 detection device of the present invention and traveling in a predetermined area. In FIG. 2, a light reflector (hereinafter, simply referred to as a reflector) 6a having a reflection surface that reflects incident light in the incident direction around the area where the mobile 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 not-shown cutter blade for lawn mowing work on its lower surface. A light beam scanning device (hereinafter, simply referred to as a scanning device) 2 is mounted on the top of the self-propelled vehicle 1. This scanning device 2 comprises a light emitter for generating a light beam 2E and the reflectors 6a to 6a.
It has a light receiver which receives the reflected light 2R of the light beam 2E reflected by d. The light emitter has a light emitting diode, and the light receiver 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 from the light emitter is refracted and reflected at a right angle by a rotating mirror (hereinafter, simply referred to as a mirror) 4 to be redirected and projected from the scanning device 2 to the outside. The mirror 4 is rotated around the rotation center axis 8 by the motor 5 in the direction of the arrow 17, and the rotation of the mirror 4 causes the light beam 2E to be rotationally scanned around the rotation center axis 8 in the direction of the arrow R. The projection direction of the light beam 2E, which is determined by the rotation position of the mirror 4, that is, the rotation angle of the motor 5 is detected by the encoder 7.

The scanning device 2 has a gimbal swing mechanism for continuously changing (oscillating scan) the angle of the rotary scanning surface drawn by the light trace of the light beam 2E. This swing mechanism
An outer ring member 11 swingably supported by 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. The inner ring member 14 is
The outer ring member 11 is swayed by a shaft 13 provided on the outer ring member 11 on a line orthogonal to the extension line of the support shaft and the other shaft 20 (shown in FIG. 1) provided at a position facing the shaft 13. It is rotatably supported.

The gimbal swing mechanism is a motor 1 for swing drive.
Driven by five. With this gimbal swing mechanism, the rotation center axis 8 of the mirror 4 is mounted so as to be tilted from the vertical by an angle φ, and the tilt direction (hereinafter, referred to as the swing direction) is continuously changed, and 17a
Rotate in the direction of. By the conical rotation of the rotation center shaft 8 as described above, the angle of the scanning surface due to the rotational scanning of the light beam 2E continuously changes. That is, the projection direction of the light beam 2E continuously changes in the vertical direction, and the scanning is performed by swinging.

Next, the scanning device and the swing drive device for the gimbal swing mechanism will be described in detail. FIG. 1 is a cross-sectional view of a main part of a scanning device 2 mounted on a self-propelled vehicle 1, and the same reference numerals as those in FIG. 2 denote the same or equivalent parts. First, the scanning device 2 will be described. The mirror 4 is attached to one end 5a of the shaft of the motor 5 via the pedestal 4a. On the other hand, the motor 5
The other end 5b of the shaft is connected to the shaft 7a of the encoder 7 by a connecting fitting 19. The output pulse of the encoder 7 is transmitted to a control device (not shown) and is used to calculate the rotation angle and the rotation speed of the mirror 4.

A suction plate 34 is provided on the pedestal 4a of the mirror 4. The attraction plate 34 is made of a magnetic material, such as iron, and is attracted to the electromagnet 16 when the electromagnet 16 is biased. By this attraction operation, the stop position of the mirror 4 is fixed at an arbitrary timing when the electromagnet 16 is urged.

A casing 3 is attached below the inner ring member 14. Although the mounting means of the casing 3 is not shown, well-known fastening means such as bolt tightening may be appropriately used.

Next, the swing drive device of the gimbal swing mechanism will be described. The swing drive device is provided on the upper surface of the vehicle 1. Bearing 21 mounted on the upper surface of self-propelled vehicle 1
A shaft 22 is inserted through the shaft 22, a small disc 23 is fixed to one end of the shaft 22, and a large disc 24 is fixed to the other end. The small disc 23 is provided with an eccentric shaft 23a at a position eccentric to the shaft 22, and the large disc 24 is similarly provided with an eccentric shaft 2a.
4a is projected. Eccentric shaft 23a and eccentric shaft 24
The eccentric directions of a are offset from each other by 90 degrees.

The shaft 15a of the swinging motor 15 is the shaft 22
And an L-shaped block 32 is fixed to the shaft 15a. That is, the eccentric shaft 23
The a and 24a are eccentric to the shaft 15a by the same amount as the eccentric amount to the shaft 22, and the shaft 15a of the motor 15, the eccentric shaft 23a, the shaft 22, and the eccentric shaft 24a form a crankshaft. Rotating shaft 15a by swinging motor 15
When is rotated, this rotation is transmitted to the eccentric shaft 23a by the block 32, and the shaft 22 rotates. As a result, the eccentric shaft 24a also rotates around the shaft 22.

The eccentric shaft 23a is rotatably fitted in the circumscribing ring 23b, and the block 25 is swingably supported by the circumscribing ring 23b. The block 25 is connected by a connecting bolt 26 to a spherical bearing 27 that receives a shaft (not shown) provided on the inner ring member 14 so as to project therefrom.

Since the small disc 23 and the inner ring member 14 are connected in this manner, the rotational movement of the eccentric shaft 23a with respect to the small disc 23 causes the inner ring member 14 to move in the vertical direction about the shafts 13 and 20. Is converted into a swinging 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 projects from the shaft 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 and the inner ring member 1 also
Similar to 4, the shaft 12 is swung about the shaft 12 and a shaft (not shown) at a position facing the shaft 12.

When the swings of the outer ring member 11 and the inner ring member 14 are combined, the rotation center shaft 8 of the mirror 4 of the scanning device 2 mounted on the inner ring member 14 is rotated.
, However, turns with a predetermined inclination angle about the intersection of the swing center axes of both ring members 11 and 14. In other words, the locus of the rotation center axis 8 due to this turning is the side surface of a cone having the intersection as the apex (hereinafter, simply referred to as a cone). The casing 3 containing the light emitter and the light receiver is also attached to the lower surface of the inner ring member 14, and therefore swings integrally with the inner ring member 14.

Both ends of the connecting bolt 26 are threaded in opposite directions, and when the connecting bolt 26 is rotated,
The connecting bolt 26 is used for the block 25 and the spherical bearing 2.
It is possible to adjust the connection length between the spherical bearing 27 and the block 25 by moving forward and backward with respect to 7. Like the connecting bolt 26, the connecting bolt 31 also adjusts the connecting length with the spherical bearings 28 and 30 into which the connecting bolt 31 is screwed.

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

Behind the motor 15, an encoder 35 for detecting the rotational position of the motor 15 is attached. With the output signal of the encoder 35 and the output signal of the sensor 33, the inclination of the rotation center axis 8 of the mirror 4, that is, the swing direction can be detected. The means for detecting the swinging direction of the rotation center shaft 8 is not limited to the one using the encoder 35 and the sensor 33. For example, the thin disk 24b may be provided with a slit for detecting the rotation amount of the thin disk 24b in addition to the slit for detecting the reference position, and two sensors may respectively detect these two kinds of slits. . Alternatively, both signals indicating the rotation amount of the motor 15 and the rotation reference position may be extracted from the encoder 35.

In order to scan the light beam in the vertical direction evenly and simplify the process of receiving the reflected light, it is desirable that the swing locus of the rotation center shaft 8 is a cone, but it is not necessarily a cone. Both sides may have a pyramid 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 locus drawn by the swing of is an elliptical cone.

In the present embodiment, the eccentric amounts of the eccentric shafts 23a and 24a are adjusted so that the swing locus becomes substantially conical, that is, the maximum inclination angles of the outer ring member 11 and the inner ring member 14 are the same. It is set.

Although the outer ring member 11 and the inner ring member 14 are driven by one motor in this embodiment, each ring member may be driven by a separate motor. In that case, it goes without saying that each motor rotates in synchronization so that the rotation center axis 8 draws a desired conical shape.

When the swinging mechanism described above is driven to project a light beam, swinging scanning is performed in which the rotation center axis 8 of the mirror 4 rotates in a conical shape, and the rotation of the mirror 4 causes a light trace. The surface (rotational scanning surface) drawn by is not fixed to one plane and constantly changes during one cycle of rocking.

The rotation scanning locus at a fine pitch as will be described later is set by sufficiently shortening the cycle in which the mirror 4 makes one rotation as compared with the cycle in which the rotation center axis 8 draws a cone and makes one rotation. Can be drawn. In this embodiment, the mirror 4 is rotated at 2700 rpm, and the shaft 22 for swinging the rotation center shaft 8 is rotated at 90 rpm.

Next, the light trace of the light beam by the scanning device of the present embodiment will be described with reference to the drawings. FIG. 3 shows a model of a light trace drawn on an imaginary 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 mesh-like light trace on the assumed cylindrical surface due to the conical movement of the rotation center axis 8 of the mirror 4. Draw. In this embodiment, the rotation number of the mirror 4 is set to 27.
The rotation number of the rotation center shaft 8 is 00 rpm, that is, the rotation number of the shaft 22 is 90 rpm. Therefore, the mirror 4 rotates 30 times while the rotation center shaft 8 makes one conical rotation. That is, 30 light traces cross an arbitrary vertical line 18 on the cylindrical surface while the rotation center axis 8 makes one rotation in a conical shape.

Next, how the light beam is easily irradiated to the reflector in one cycle of oscillation when the reflector is arranged on the vertical line 18 will be described. FIG. 4 is an enlarged view of a part of the light trace. In the figure, when the vehicle 1 and the reflector 6 are close to each other as indicated by reference numeral 6H and the dimension of the reflector 6 in the height direction is sufficiently longer than the swing width BB of the light trace, 30 All light trails cross this reflector 6. On the other hand, when the distance between the self-propelled vehicle 1 and the reflector 6 is very long as indicated by reference numeral 6L, the size of the reflector 6 in the height direction is relative to the swing width BB of the light trace. Becomes shorter.
However, even if the size of the reflector 6 in the height direction is relatively short as described above, if the maximum vertical distance H of the light traces is relatively smaller than the size of the reflector 6 in the height direction. The light trace crosses the reflector 6 at least once while the central axis of rotation 8 makes one conical movement. 3 and 4 are
The number of light traces is less than the actual number in order to avoid complexity and facilitate drawing.

Next, the basic principle for detecting at which position in the traveling area and in which direction the traveling vehicle 1 equipped with the scanning device 2 having the above configuration is traveling. Will be explained. 5 and 6 are a self-propelled vehicle 1 and a reflector 6a in a coordinate system showing a traveling area of the self-propelled vehicle 1.
It is a figure which shows the position of ~ 6d. In the figure, the reflector 6
The arrangement positions of a 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 straight line connecting the reference points B and C is x. X- as axis
It is expressed in the y coordinate system.

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 position of the triangle BTC.
Exists on the circumscribed circle 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. Circumscribed circles Q and P 2
Since one of the two intersections is the reference point B, that is, the origin, the other intersection is the position of the vehicle 1. The calculation formula for obtaining the position of the self-propelled vehicle 1 according to the principle as described above has been already filed by the present applicant.
The details are disclosed in Japanese Patent No. 7415 and Japanese Patent Laid-Open No. 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 angle of the reference point C relative to the traveling direction is θc, the x-coordinate of the reference point C is xc, and y
When the coordinates are Yp, θf = 360 ° −tan ⁻¹ {Yp / (xc−x)} − θc ... (1)

Next, steering control for controlling the traveling direction of the self-propelled vehicle 1 on the basis of the position information obtained by the calculation formula described in the above publication and the calculation formula (1) will be described with reference to the drawings. To do. FIG. 7 is a diagram showing a positional relationship between the self-propelled vehicle 1 and 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 is composed of straight travel paths that are set in parallel with an interval L and turning strokes that connect the respective straight travel paths. After traveling straight ahead, the self-propelled vehicle 1 is at a position where the y coordinate reaches Ytn or Ytf,
The steering angle is fixed to a fixed value and the vehicle travels in the turning stroke, and then shifts to the next adjacent straight travel distance. When the x-coordinate of the straight travel distance exceeds the final x-coordinate Xend, after traveling the straight travel distance, the vehicle returns to the home position 63 after the final turning stroke.

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

Next, the control procedure will be described with reference to the flow chart. The meanings of various parameters (symbols) used in the flow charts referred to for this description are as follows. θ (n) ... Azimuth determined based on the received light signal, θq
(N) ... Predicted azimuth angle, Cg (i) ... Number of received light by detection block, Am (i) ... Detected azimuth angle by detection block, Cp (n) ...
Number of times light is received at the reference point n, Ap [n, I] ... Azimuth of light reception at the reference point n, As [n, I] ... Oscillation direction when the reference point n is detected, Cm [n, I] ... Reference point n detection Mirror rotation speed counter value at time, Aps (k) ... Azimuth representing a detection block in which the number of received light is greater than or equal to a threshold, Aps (n) ... Aps (k) are set to n = 1 to 4 in ascending order. Corner, i ... number of detection block, j ... number of detection blocks whose number of received light is greater than or equal to a first threshold, k ... number of detection blocks whose number of received light is greater than or equal to a second threshold, I ... mirror 4 A number indicating the storage order of the swinging direction when the reference point n is detected by rotating a predetermined number of times, J ... The number of consecutive detections of the reference point n when the mirror 4 is rotated by a predetermined number, K ... The number of mirrors 4 is expected The maximum value of the number of continuous detections of the reference point n when rotating, e ... The maximum value of the number of continuous detections occurs. The last number of the number indicating the storing order of the swinging direction, Asc (n) ... The swinging direction capable of capturing the reference point n with high probability, Ac (n) ... Determined based on the light receiving signal in the straight-ahead processing. Azimuth angle, θt (n) ... Turn release angle of reference point n for turning release First, the reflected light receiving process, which is the basis of the steering control, will be described. The light beam emitted from the scanning device 2 and reflected by the reflectors 6a to 6d, that is, the reflected light, is received as follows.

FIG. 8 is a flow chart showing the control procedure of the reflected light receiving process. In step S100, it is determined whether or not an optical signal is detected by the light receiver. If the optical signal is detected, the process proceeds to step S101. However,
At this point, it is not possible to identify whether the detected optical signal is the reflected light from the reflectors 6a to 6d.

In step S101, it is determined whether or not the detection signal is due to chattering, based on the angle of rotation of the mirror 4. That is, if a plurality of optical signals are detected while the mirror 4 has rotated only a small angle from the previous process, it is determined that chattering occurs, and the optical signals detected after that are ignored. If not due to chattering, the process proceeds to step S102.

In step S102, "0" is set to the variable i indicating the detected block number. In this embodiment, the mirror 4 rotates 30 times while the rotation center axis 8 of the mirror draws a conical locus and makes one rotation. That is, the rotational scanning is performed 30 times while the rotation center axis 8 draws a conical locus and makes one rotation. There is a possibility that reflected light from the same reflector may be received many times by the 30 times of rotational scanning. Detection data relating to a plurality of optical signals that have entered the light receiver from substantially the same direction are collectively stored in one group as data of the same reflector. This group is called a detection block. Therefore, the planned reflectors 6a-6d
If only the light from is detected, then the number of detection blocks is four, which corresponds to the number of reflectors installed.

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

In the first processing, this judgment becomes affirmative and the routine proceeds to step S106 where the mirror angle, that is, the azimuth angle at which the light is detected is stored. Azimuth angle A representing the detection block i
The azimuth angle detected this time is stored as m (i), and the value of the number of times Cg (i) of light reception of the optical signal in the detection block i is incremented.

In step S107, the value n of the counter for identifying the reference point is cleared. In this embodiment, the counter value "1" is at the reference point A and the counter value "2" is at the reference point B.
The counter value “3” is at the reference point C and the counter value is “4”.
Corresponds to the reference point D, respectively. Step S10
At 8, the value n of the counter is incremented.

In step S109, it is determined whether or not the azimuth angle detected this time is substantially the same as the predicted azimuth angle θq (n) set in the initial pole identification processing or the forward straight traveling processing, which will be described later. That is, since the counter value n is "1" in step S108, it is determined whether or not the predicted azimuth angle of the reference point A corresponding to the counter value "1" substantially coincides with the detected azimuth angle. To be judged. Predicted azimuth θ
q (n) may be, for example, a value obtained by adding the predicted change amount α to the azimuth angle at the time of this detection, but since the light reception interval of the reflected light is short with respect to the movement amount of the self-propelled vehicle 1, it is the same value as this time. Even if is set as the predicted azimuth, there is no practical problem and the processing is simple.

If the determination in step S109 is negative,
In step S110, it is determined whether the counter value n is "4". The processes of steps S108 and S109 are repeated until the determination of step S110 becomes positive, and the reference point A to
For all the predicted azimuth angles θq (n) of D, it is determined whether or not this and the detected azimuth angle substantially match.

If the predicted azimuth angle θq (n) substantially matches the detected azimuth angle, steps S109 to S11.
Go to 1. In step S111, it is determined that the predetermined reference point is detected, and the number of times of light reception Cp (n) at the reference point indicated by the counter value n is incremented. Further, the detected azimuth angle Ap [n, Cp (n)] of the reference point, the tilt direction of the rotation center axis 8 of the mirror 4, that is, the swing direction As [n, Cp].
(N)], and the rotation counter value Cm of the mirror 4
Store [n, Cp (n)]. The rotation counter value is a value indicating the number of rotations of the rotation of the mirror 4 counted from the time when the swing direction is in the predetermined direction based on the output signal of the sensor 33.

If it is determined in step S103 that the number of times of light reception Cg (i) in the detection block i is not "0", that is, it is not the first light reception, step S104.
Proceed to. In step S104, it is determined whether or not the detected azimuth angle substantially matches the azimuth angle Am (i) of the optical signal previously received by the detection block i. If they match, 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 angle Am (i) of the optical signal previously received by the detection block i does not match the azimuth angle detected this time, another When it is determined that the light is from the detection block, the process proceeds to step S105, and the detection block number (i) is incremented. After incrementing the detection block number (i), it is determined in step S103 whether or not the incremented detection block number (i) is the first light reception.

Based on the azimuth angle of the received light signal stored by the reflected light receiving process, that is, the azimuth angle of the reference point, the position and traveling direction of the self-propelled vehicle 1 are calculated and steering control is performed as described later. .. 9 and 10 are general flowcharts showing the entire steering control. In FIG. 9, in step S1, the motors 5 and 15 are activated to rotate the mirror 4 about the rotation center axis 8 and rotate the rotation center axis 8 so as to draw a conical locus. Here, the motor 15 is rotated at a low speed so that the reflectors 6a to 6d set at the reference points A to D can be reliably irradiated with the light beam.

In step S2, an initial pole identification process for determining the initial azimuth angles of the reference points A to D, that is, the reflectors 6a to 6d is performed. Details of this processing will be described later with reference to FIGS. 11 and 12.

In step S3, the reference point A is calculated from the self-propelled vehicle 1.
A pole position measuring process is performed to measure the distances to D and calculate the position of each reference point, that is, the reference coordinate value in the xy coordinate system. This process is shown in FIG. 13, FIG. 14 and FIG.
5 will be described later. In step S4, step S2
And the current position coordinates (Xp, Yp) of the self-propelled vehicle 1 based on the azimuth and coordinate values of the reference point calculated in step S3.
To calculate. In step S5, the current x coordinate Xp of the vehicle 1 is set as the x coordinate Xref of the first straight travel distance. However, this set is an operation when the self-propelled vehicle 1 is at the traveling work start position. In step S6, the motors 5 and 15 are rotated at a predetermined speed to rotate and swing the mirror 4. In step S7, the engine rotation of the self-propelled vehicle 1 is connected to the drive wheels to start traveling.

In step S8 of FIG. 10, forward traveling straight traveling processing is performed in which the self-propelled vehicle 1 travels in a straight traveling range in a direction in which the y coordinate value increases. In this forward path straight-ahead processing, based on the azimuth angle obtained by the reflected light receiving processing, the self-position (Xp, Y
p) and the traveling direction θf are calculated. Then, the difference between these values and the set traveling course is calculated, and control is performed to change the steering angle of the steered wheels of the self-propelled vehicle 1 so as to correct this difference. Since this processing is not directly related to the present invention, the detailed flowchart is omitted.

In step S9, the y coordinate Yp of the vehicle 1 is set.
Is determined to be greater than the planned y-coordinate Ytf, it is determined whether or not the first straight travel has been completed. When it is determined that the self-propelled vehicle 1 has finished traveling straight ahead, the process proceeds to step S10. In step S10, the distance L is added to the x coordinate Xref of the straight travel distance to set the next straight travel distance.

In step S11, right turn release angle setting processing for setting the azimuth angle at which the traveling of the turning stroke is finished is performed. In the right turn release angle setting process, the azimuth angle for each reference point for ending the U turn process performed in the next step, that is, the right turn release angle is calculated. In step S12, a U-turn process is performed in which the steering angle of the self-propelled vehicle 1 is fixed to a predetermined value and the self-propelled vehicle 1 is caused to travel in a turning stroke of turning to the right with a constant turning radius. Since the processes of steps S11 and S12 are not directly related to the present invention, the detailed flowchart is omitted.

In step S13, a cancellation counter (counted during the U-turn processing) for counting the number of reference points at which the azimuth viewed from the vehicle 1 has reached the planned right-turn cancellation angle.
It is determined whether or not the value of exceeds "1". When this determination is affirmative, it is determined that the traveling of the turning stroke is completed, and the process proceeds to step S14. In step S14, a return straight traveling process is performed in which the self-propelled vehicle 1 travels in the straight traveling direction in the direction in which the y coordinate value decreases. This return straight traveling process is similar to the forward straight traveling process of step S8.

In step S15, the y coordinate Y of the self-propelled vehicle 1
second depending on whether p is smaller than the planned y coordinate Ytn
It is determined whether or not the traveling of the second straight travel is completed. In step S16, it is determined whether or not the x-coordinate Xref of the straight travel distance exceeds the x-coordinate Xend of the planned traveling end point. If the determination in step S16 is negative, step S
Proceed to 17 to set the next straight travel distance.

In step S18, a left turn release angle setting process for setting an azimuth angle at which the traveling of the leftward turning stroke is finished is performed. This process is the same as the right turn release angle setting process except that the value of the release angle to be set is different. In step S19, U-turn processing is performed. Step S2
At 0, it is determined whether or not the value of the release counter exceeds "1". If this determination is affirmative, it is determined that the traveling of the turning stroke has been completed, and the process returns to step S8.

If the determination in step S16 is affirmative, the process proceeds to step S21. If the determination in step S16 is affirmative, it means that the traveling of all the straight travel strokes has been completed, and in step S21, processing for setting the release angle in the final turning stroke is performed. This process is performed in the same manner as the right turn release angle set and the left turn release angle.

In step S22, U-turn processing is performed,
In step S23, it is determined whether or not the value of the release counter exceeds "1". In step S24, a process of traveling straight ahead to return to the home position 63 is performed. This processing is the same as the forward straight processing.

In step S25, the x coordinate X of the self-propelled vehicle 1
It is determined whether p is smaller than the x coordinate Xhome of the home position 63. If this determination is affirmative, it is determined that the self-propelled vehicle 1 has returned to the home position 63, and the processing ends.

Next, the initial pole identification processing in step S2 will be described in detail. FIG. 11 is a flowchart of the initial pole identification process. In the figure, in step S120, it is determined whether or not the swing direction is "0 °", that is, whether or not the predetermined reference position is detected by the sensor 33 for detecting the swing reference. When the predetermined reference position is detected and it is determined that the swing direction is "0 °", the process proceeds to step S121. In step S121, the data obtained by the reflected light receiving process is cleared.

In step S122, the reflected light receiving process shown in FIG. 8 is performed. In the reflected light receiving process in the initial pole identification process, since the predicted azimuth angle is not determined before this process, only the process corresponding to steps S100 to S106 in the reflected light receiving process described with reference to FIG. 8 is performed. Be seen.

In step S123, it is determined again whether the swing direction is "0 °". That is, it is determined whether or not one cycle of the swing scan in which the rotation center axis 8 draws a cone is completed. The reflected light receiving process is continued until the scanning of one cycle is completed, and when one cycle is completed, step S124.
Proceed to.

In step S124, reference point selection processing (pole selection processing) is performed. In this selection processing, among the detection blocks detected in the reflected light reception processing, four detection blocks having a large number of light reception times Cg (i) are selected, and the detected azimuth angle Am (i) representing the detection block is set as the azimuth angle. Set to Aps (n) in ascending order. In this embodiment, the reference points are four points A, B, C and D, so that n = 1 to 4. As shown in the flowchart of the reflected light receiving process in FIG. 8, the detected azimuth angle Am (i) representing the detection block
Is the latest data of the azimuth detected by the detection block. By using the latest data like this,
It is possible to reduce the storage capacity.

Further, in step S124, the pole selection modes "1" to "3", which are materials for the determination in the next step S125, are also determined. The pole selection process is
Further details are provided with respect to FIG.

In step S125, it is determined whether the poll selection mode determined in the poll selection processing is "1" to "3". When the pole selection mode is "1", it is determined that all four reference points can be identified and their azimuth angles can be detected, and the process proceeds to step S126. Step S1
26, the azimuth θ (n) of the reference point and the azimuth (predicted azimuth) θq predicted to detect the same reference point next time.
As (n), the azimuth angle Aps (n) obtained in the process of step S124 is set.

When the pole selection mode is "2", the number of times of light reception Cg (i) has not reached the predetermined value.
Returning to step S122, the reflected light receiving process is continued.
Furthermore, when the pole selection mode is "3", there are five or more detection blocks in which the number of times of light reception Cg (i) is equal to or more than a predetermined value due to an unplanned reflecting object. 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 performed again from the beginning.

In the initial pole identification processing, reference point identification processing is performed based on the received light data stored while the rotation center axis 8 swings once.

FIG. 18 is a diagram showing an example of received light data accumulated by the reflected light receiving process. In the figure, the vertical axis represents the number of light receptions Cg (i) of the detection block i, and the horizontal axis represents the azimuth angle Am (i) of each detection block i. As shown in the figure, if the number of detection blocks is seven (i = 0 to 6), this means that the optical signal is received from seven directions while the rotation center shaft 8 swings once. .. Number of received light Cg
In the detection block in which (i) is multiple times, the azimuth angle Am (i)
Is the latest detection data as described above. The numbers i of the detection blocks are not necessarily arranged in the ascending order of the azimuth angle because the numbers are given in the order in which they are received.

Details of the poll selection process will be described with reference to this data. FIG. 12 is a flowchart of the poll selection process. In this pole selection processing, the detection blocks in which the number of times of light reception Cg (i) has reached the predetermined threshold value are extracted, and whether the number of the extracted detection blocks matches the predetermined number of reference points, that is, "4". Determine whether
If they match, the light reception data of the detection block is determined to be the data regarding the predetermined reference points A to D. Also,
When the number of extracted detection blocks is large, it is determined that the reference point cannot be specified and data is collected again. When the number of extracted detection blocks is small, data collection is continued.

In this embodiment, the threshold values are "3" and "5".
It was set in two stages. The number of times of light reception Cg (i) is the first
The number of detected blocks that has reached the threshold value "3" is stored in the parameter j, and the number of detected blocks that has reached the second threshold value "5" is stored in the parameter k. Then, based on the parameters j and k, it is determined whether or not the reference point may be identified.

First, in step S130, the data obtained by the reflected light receiving processing, that is, the light detection azimuth angle Am (i) and the number of times Cg (i) of receiving the reflected light are read. Step S1
At 31, the parameters i, j, k are cleared. In step S132, it is determined whether the detection block (i) is a detection block in which the number of times of light reception Cg (i) is more than three times. If the detection block has the number of times of light reception Cg (i) more than 3, the process proceeds to step S133, and the number of times of light reception Cg (g)
The parameter j indicating the number of detected blocks in which (i) is three or more times is incremented.

In step S134, the number of times light is received Cg
It is determined whether (i) is a detection block more than five times.
If the detection block receives more than 5 times,
In step S135, the parameter k indicating the number of detection blocks for which the number of times of light reception is 5 or more is incremented.

In step S136, it is determined whether the value of the parameter k is "4" or more. It is determined whether or not there are four detection blocks in which the number of times of light reception Cg (i) exceeds 5, that is, the total number of reference points is greater than or equal to the predetermined number. If the number of detection blocks in which the number of times of light reception Cg (i) exceeds 5 is 4 or less, in step S137, the number of times of light reception Cg
The detected azimuth angle Am (i) representative of the detection block in which (i) is five times or more is one azimuth angle A of the predetermined reference points.
Store as ps (k).

In step S138, the number i indicating the detection block is incremented. In step S139, it is determined whether the number of times of light reception Cg (i) is "0". If the number of times of light reception Cg (i) is not "0", it is determined that there is a detection block in which the number of times of light reception 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 is not completed, and the process proceeds to step S140.

In step S140, the parameters j, k
Are both “4”, that is, whether the number of detection blocks having the number of times of light reception 3 times or more and the number of detection blocks having the number of times of light reception 5 times or more are 4 respectively.

If step S140 is positive, the process proceeds to step S141, and the detected azimuth angles Aps (k) stored in step S137 are set in ascending order of azimuth angles Aps (1).
~ Set as Aps (4). In step S142, the pole selection mode is set to "1".

If the determination in step S140 is negative, the process proceeds to step S143 to detect whether or not the parameter j is "4" or more, that is, whether or not there are four or more detection blocks in which the number of times of light reception is three or more. To do. Step S14
If 3 is negative, the pole selection mode is set to "2" in step S144, and if positive, the pole selection mode is set to "3" in step S145. According to these determined pole selection modes, the determination of step S125 in the initial pole identification processing of FIG. 11 is performed.

For example, in the example of the received light data shown in FIG. 18, the azimuth angle of the detection block (number i = 0, 2, 3, 5) in which the number of times Cg (i) of received light exceeds 5 is the predetermined reference. Identified as the azimuth of the point.

Next, the process of determining the coordinates of the reference point necessary for the pole position measuring process shown in step S3 of FIG. 9 will be described. To determine the coordinates of the reference point,
First, the distance between the self-propelled vehicle 1 and each reference point is measured. Then, the coordinates of the reference point are calculated based on the measured distance and the azimuth angle of each reference point viewed from the mobile vehicle 1. In this embodiment, the distance to each reference point is calculated based on the phase difference between the phase of the optical signal projected from the light emitting means and the optical signal detected by the light receiving means.

In measuring the distance, an azimuth angle for surely irradiating the reflector at the reference point with the light beam, and
The projection angle in the vertical direction, that is, the inclination of the rotation center axis 8 must be determined.

By the way, it is a matter of course that a reflector having a sufficient size in the height direction is preferably prepared, but the deviation of the projection angle due to the mechanical play of the rotation center axis 8 is preferable. It is also necessary to consider the above-mentioned problems due to the fact that the reflector is erected with a large inclination due to the sloped state of the terrain and the unevenness.

In consideration of the above-mentioned problems, in order to irradiate the reflector with the light beam with a high probability in order to measure and calculate the reference data before traveling, the reflector is actually irradiated with the light beam. It is desirable to set the irradiation angle of the light beam in the vertical direction, that is, the swing angle near the center in the height direction of the range.

In order to set the projection angle of the light beam near the center in the height direction of the reflector, the following method for determining the pole trapping swing direction is adopted in this embodiment.

First, the relationship between the swing angle and the light beam projection angle will be described with reference to the schematic diagram of FIG. The swing angle is detected as the angle of the conical swing of the rotation center shaft 8, that is, the swing direction due to the configuration of the detection means. In the figure, arrow WD
Indicates the swing direction.

FIG. 20A shows the case where the swing direction is 0 °. The light beam 2E emitted from the scanning device 2 is directed in the same direction as the swing direction, and the reflector 6 is located in that direction.
In this way, when the light beam 2E is directed in the same direction as the swinging direction, the light beam 2E is emitted downward. From this state, the swing direction changes to 180 ° as shown in FIG.
When E is directed, the light beam 2E is emitted toward the top. That is, the light beam 2 is moved in the direction opposite to the swing direction.
When E is directed, the light beam 2E is emitted in the uppermost direction. A reflector will be caught between this bottom and top.

When the distance between the scanning device 2 and the reflector 6 is large, the reflector 6 is positioned within the range of angles of incidence between the light beam projected on the lowermost side and the light beam projected on the uppermost side as described above. I will enter. Therefore, when the light beam is rotationally scanned while changing the swing direction, the reflected light starts to be detected from the lower end (or the upper end) of the reflector and the reflected light at the upper end (or the lower end) is completely detected. If the midpoint of the swinging direction is calculated, the direction indicated by the calculation result will reflect the light beam to the reflector 6
That is, it is the swinging direction in which the irradiation can be performed with the highest probability and the average value of the data detected as the position of the reflector 6 can be detected.

However, it is not always possible to irradiate the light beam over the entire area from the upper end to the lower end of the reflector 6 between the lowermost part and the uppermost part. When the projection range of the light beam projected from the scanning device 2 and the position of the reflector 6 do not match well, or the distance between the scanning device 2 and the reflector 6 is too short, the upper end of the reflector 6 Alternatively, the light beam may not reach the lower end side. In this case, start detecting the reflected light from the bottom edge (or top edge),
The reflected light from the lower end (or the upper end) is detected again without detecting the reflected light at the upper end (or the lower end), and the light beam can be emitted to the reflector 6 until the reflected light is detected. It becomes the range of the moving direction. Therefore, the intermediate point in the swing direction in this case does not necessarily correspond to the average value of the data detected as the position of the reflector 6, as will be described later with reference to FIG.

In this embodiment, even in such a case, a portion of the data detected as the position of the reflector 6 that can be detected as a substantially average value, that is, at least a portion that is not a deviated position such as both ends. A method that reliably irradiates the light beam, that is, a pole trapping swing direction determination method is adopted.

The outline of the pole trapping swing direction determination method will be described with reference to the relationship diagrams between the reflector and the light beam scanning locus shown in FIGS. FIG. 21 is a relationship diagram when the reflector 6 is provided between the lowermost part and the uppermost part in the light beam irradiation direction. In the figure, the vertical axis represents the height of the light beam at the reference point position, that is, the position where the reflector 6 is installed, and the horizontal direction represents the swinging direction. As can be seen from the figure, the reflector 6 can be continuously irradiated with the light beam by each rotational scan while the swing direction changes in the range of W1 to W2. Therefore, the height of the light beam at the intermediate position M1 in the swing directions W1 and W2 corresponds to the height-direction intermediate position PC of the reflector 6.

On the other hand, in FIG. 22, even when the light beam is directed to the uppermost side, it does not reach the height beyond the uppermost end of the reflector 6. Therefore, the detection of the light beam is not completed on the upper end side of the reflector 6, but is continued to be detected on the lower end side again. Therefore, in this case, the reflector 6
The irradiation height position of the light beam at the intermediate position M2 of the swing direction range W3 to W4 corresponding to the range in which the light beam can be irradiated does not correspond to the height direction intermediate position PC of the reflector 6.

Therefore, in such a case, the range of the swing direction in which the reflector 6 can be irradiated with the light beam is between the uppermost end where the light beam can be detected and the position where the detection of the light beam ends. Change to a certain W4 to W5. Then, the intermediate position M3 of the changed range is determined as a portion that can be detected as a substantially average value of the data detected as the position of the reflector 6, and the direction indicated by the value is most surely determined. It is determined as the swinging direction in which the light beam can be emitted, that is, the pole capturing swinging direction.

FIG. 19 shows an example of received light data regarding the reference point n detected by the reflected light receiving process of the pole trapping swing direction determination process. The figure shows the received light data detected in one cycle of oscillation. The swing direction is represented by the rotation amount of the encoder 35 connected to the swing motor 15.

As shown in the flow chart of the reflected light receiving process of FIG. 8, the parameter Cm indicates the rotation counter value when the counting is started from the position where the swing direction is “0 °” and the reference point n is detected. It is a thing. Parameter I is the reference point n
Is a number indicating the storage order of the swing direction when is detected.
That is, the maximum value of the number I indicating the storage order is the number of times of light reception Cp (n) at the reference point n.

In this pole trapping swing direction determination processing, a group in which the reference point n is continuously detected is searched for each rotation of the rotational scanning of the mirror 4, and the first and last groups in the group having the largest number of consecutive detections are searched. The midpoint of the values indicating the swing direction of is calculated. Then, the swinging direction represented by the intermediate point is determined as the swinging direction with which the reference point n can be irradiated with the light beam with high probability.

For example, in FIG. 19, the counter value Cm (n) is continuous when the reference point n is detected when the number I is "1" to "2" and when the number "3" to "6". Among these, when the number I is “3” to “6”, the number of consecutive times is large. Therefore, in this example, the intermediate point in the swinging direction when the memory number I is "3" and "6" is obtained.
That is, (108.6 + 144.4 °) / 2 = 12
6.5 ° is determined as the reference point (pole) capture swing direction.

13 and 14 are flowcharts of the pole trapping swing direction determination processing. In FIG. 13, in step S150, it is determined whether or not the swing direction is “0 °”. If it is determined that the swing direction is “0 °”, the process proceeds to step S151, and the collection of reflected light data is started from that point.

First, in step S151, the data obtained by the reflected light receiving process is cleared, and step S15
At 2, the reflected light receiving process is performed. This process is as described with reference to FIG. 8, and here, FIG. 18 and FIG.
Collect data as shown in 9.

In step S153, the swing direction is "0".
It is determined whether or not one cycle of oscillation has ended depending on whether or not the angle becomes "°". Until the determination is affirmative, that is, the received light data during one oscillation of the rotation center axis 8 drawing a cone. Is accumulated.

When it is determined that the one-cycle swing of the rotation center shaft 8 is completed, the process proceeds to step S154. In step S154, step S11 of the reflected light receiving process of FIG.
Read the data obtained in 1. In step S155,
The reference point identification counter n is cleared. Step S15
At 6, the counter n is incremented.

In step S157, it is determined whether or not the parameter Cp (n) indicating the number of times the reference point (reference point n) indicated by the counter n is detected is "0", and the reference point n is detected. Determine whether or not. If this determination is affirmative, it is determined that the reference point n has not been detected, and step S1
Returning to 50, the received light data is collected again.

If step S157 is negative, the process proceeds to step S158 and the parameter I is set to "1". The parameter I is a number for identifying the order in which the swing direction in which the reference point n can be detected is stored as described above.

In step S159, the parameters K and e are cleared. This parameter K is the maximum value of the number of consecutive times, that is, the maximum continuous detection, when the light receiving signal from the same direction is detected every time the mirror 4 makes one rotation, that is, when the optical signal from the same direction is continuously detected each time. Indicates the number of times. Further, the parameter e indicates, among the memory numbers I in the swing direction, particularly the last memory number in the swing direction when the optical signal is continuously detected.

In step S160 of FIG. 14, "1" is set to the value J of the counter that counts the number of consecutive light reception signals. In step S161, the number of detections Cp of the reference point n is calculated.
By determining whether or not (n) is the same as the number I indicating the stored number in the swing direction, it is determined whether or not all the received light data detected and stored in the reflected light receiving process have been checked. Normally, there are a plurality of received light data, and the number I is set to "1" in step S158. In such a case, the determination in step S161 is negative, and it is determined that all the received light data have not been checked. And proceeds to step S162.

In step S162, with respect to the rotation counter value Cm of the mirror 4, the counter value Cm at the time of the current detection is detected.
(N, I + 1) is the counter value Cm (n,
It is determined whether or not the value obtained by adding "1" to I). That is, by determining the continuity of the counter value Cm, it is determined whether or not the light reception signal is continuously detected each time the mirror 4 rotates. If this decision is positive,
In step S163, the values of the counter J and the parameter I are incremented.

On the other hand, if step S162 is negative,
In step S164, the counter value J is the parameter K.
It is determined whether or not the number of consecutive detections this time is greater than the number of consecutive detections stored up to that time. If this determination is positive, the process proceeds to step S165,
The maximum continuous detection count K is updated with the current continuous detection count J,
The parameter e indicating the last stored number in the rocking direction when the continuous light receiving signal is detected is updated with the memory number I in the current rocking 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 the checking of all received light data has been completed, the process proceeds to step S167. Step S16
7 and S168 correspond to steps S164 and S1.
Since the processing is the same as that of 65, the description is omitted.

In step S169, the first memory number in the swing direction when the maximum number of consecutive detections occurs is calculated and set as the parameter I. In step S170, the swing direction As (n, I) detected by the reflected light receiving process is stored as the minimum value min in the swing direction, that is, the first storage number when the maximum number of consecutive detections occurs is stored. Set the data of the swing direction. Further, as the maximum value max, As (n, e), that is, the data in the swing direction stored corresponding to the last memory number at which the maximum number of consecutive detections has occurred is set.

In step S171, the lowermost check is performed. This process includes a process of detecting whether or not the detected azimuth angle θ (n) of the reference point n is between the maximum value max and the minimum value min in the swing direction. As described with reference to FIG. 20, when the light beam is irradiated in the same direction as the swinging direction, the light beam should be directed to the lowermost side, and at this time, the light beam is applied to the reflector 6. Whether or not it is possible is to detect the azimuth angle θ corresponding to this swing direction.
(N) is determined, and the swing range is changed as described with reference to FIG. The details of this lowermost check will be described later with reference to FIG.

In step S172, the uppermost check is similarly performed. This processing is performed by detecting the azimuth angle θ (n) + of the reference point n between the maximum value max and the minimum value min in the swing direction.
A process of detecting whether there is 180 ° is included. That is, as described with reference to FIG. 20, when the light beam is emitted in the direction opposite to the swinging direction, the light beam should be directed to the uppermost side, and therefore, the same as in the lowermost check described above. Depending on the concept, the swing range is changed as described with reference to FIG. The details of this uppermost check will be described later with reference to FIG.

In step S173, an intermediate point between the maximum value max and the minimum value min in the swing direction is calculated and stored as the reference point n trapping swing direction Asc (n). here,
When the swing range is changed, the reference point n is changed according to the maximum value max and the minimum value min accompanying the change.
The trapping swing direction Asc (n) is calculated.

In step S174, it is determined whether or not the processing for determining the reference point n capture swing direction with respect to all the reference points has been completed depending on whether the counter value n is "4". If step S174 is negative, the process proceeds to step S156 in FIG.

Next, details of the lowermost check and the uppermost check will be described with reference to the flowcharts of FIGS. 23 and 24. 23, in step S200, the detected azimuth angle θ (n) of the reference point n detected by the initial pole identification processing is set as the parameter a.

In step S201, it is determined whether the parameter a is larger than the minimum value min in the swing direction. If the parameter a is larger than the minimum value min,
It proceeds to step S202. In step S202, it is determined whether the parameter a is smaller than the maximum value max in the swing direction. If the parameter a is smaller than the maximum value max, the process proceeds to step S203. That is, in steps S201 and S202, it is determined whether or not the detected azimuth angle θ (n) is within the range of the swing direction in which the reference point n can be detected. Then, if the determinations in step S201 and step S202 are affirmative, step S20
Go to 3.

In step S203, the difference between the parameter a and the maximum value max, and the parameter a and the minimum value mi.
Compare the magnitude with the difference of n. If the difference between the parameter a and the maximum value max is larger, the process proceeds to step S204, and the value of the parameter a is set to the minimum value min in the swing direction. If the difference between the parameter a and the minimum value min is large, the process proceeds to step S205 and the value of the parameter a is set to the maximum value max in the swing direction.

Steps S201 and S202
If either of the judgments is negative,
This is the same state as above, and since there is no need to change the range in the swinging direction, this processing is ended as it is.

In the uppermost check of FIG. 24, in step S206, the detected azimuth angle θ (n) + 180 ° of the reference point n detected by the initial pole identification processing is set as the parameter a.

In step S207, it is determined whether the parameter a is larger than the minimum value min in the swing direction. If the parameter a is larger than the minimum value min,
It proceeds to step S208. In step S208, it is determined whether the parameter a is smaller than the maximum value max in the swing direction. If the parameter a is smaller than the maximum value max, the process proceeds to step S209. That is, in steps S207 and S208, it is determined whether or not the detected azimuth angle θ (n) + 180 ° is within the range of the swing direction in which the reference point n can be detected. And step S
If the determinations at 207 and step S208 are affirmative, the process proceeds to step S209.

In step S209, the difference between the parameter a and the maximum value max, and the parameter a and the minimum value mi.
Compare the magnitude with the difference of n. If the difference between the parameter a and the maximum value max is greater, the process proceeds to step S210 and the value of the parameter a is set to the minimum value min in the swing direction. If the difference between the parameter a and the minimum value min is large, the process proceeds to step S211, and the value of the parameter a is set to the maximum value max in the swing direction.

Steps S207 and S208
If either of the judgments is negative,
This is the same state as above, and since there is no need to change the range in the swinging direction, this processing is ended as it is.

Next, the process for determining the coordinates of the reference point according to the reference point n capture swing direction (reference point capture swing direction) will be described. FIG. 15 is a flowchart of the pole position measuring process for determining the coordinates of the reference point.

In the figure, in step S180, the pole trapping swing direction determination process is performed. Step S18
In 1, the value of the counter n for identifying the reference point is cleared, and in step S182, the counter value is incremented.

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

In step S184, swing position control is performed to adjust the swing direction to the reference point n trapping swing direction TG-SW. This swing position control is performed based on the value of the encoder 35 that detects the rotation of the swing motor 15, and the inclination of the rotation center shaft 8 is fixed in the reference point n capture swing direction TG-SW. As a result of fixing the inclination of the rotation center axis 8, the rotation scanning plane drawn by the light trace is fixed to one plane.

In step S185, it is determined whether or not the reference point n is continuously detected as a result of fixing the rotary scanning surface. If this determination is negative, it is determined that there is an abnormality such as a sensor failure, and a display is sent to notify the abnormality or an alarm is issued (sensor fail processing in step S186) and the processing is stopped.

On the other hand, if the reference point n is continuously detected every time and the determination in step S185 is affirmative, the process proceeds to step S187. In step S187, a mirror position control process for adjusting the reference point azimuth angle to the reference point n capture azimuth angle TG-ML is performed. This mirror position control processing is performed based on the value of the encoder 7 that detects the rotation of the mirror rotation motor 5, energizes the electromagnet 16 and stops the rotation of the mirror 4 that rotates about the rotation center axis 8 to stop the rotation. Fix the direction.

In 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, but may be any time as long as it is possible to confirm whether or not the mirror azimuth is securely fixed. In step S189, the distance between the 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 by the phase difference between the light beam emitted from the light emitter and the reflected light detected by the light receiver.

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 distance from the self-propelled vehicle 1 has been measured for all the reference points. If step S190 is negative, the process returns to step S182, the counter value n is incremented, and the same processing is performed for other reference points.

In step S191, based on the detected distances between the reference points and the self-propelled vehicle 1 and the azimuth angle obtained by the initial pole identification processing, an arbitrary coordinate system having the self-propelled vehicle 1 as the origin, For example, the coordinates [X (n), Y (n)] of the reference points A to D on the coordinate system in which the moving vehicle 1 is the origin and the direction of the reference point A is the positive direction of the x-axis are calculated. In step S192, the coordinates are converted into coordinates [x (n), y (n)] on the coordinate system having the reference point B as the origin.

Next, the control function for performing the above operation will be described. First, the function of the reflected light receiving process will be described. FIG. 16 is a block diagram showing the main functions of the reflected light reception processing section. The function of the reflected light reception processing unit shown in the same drawing corresponds to the contents of the reflected light reception processing, the initial pole identification processing, and the pole selection processing shown in the flowcharts of FIGS.

In the figure, an optical signal from the outside is input to the azimuth angle detecting section 37 and the swing direction detecting section 38. The azimuth angle detection unit 37 has a counter that counts the number of pulse signals input from the encoder 7, and based on the pulse count value when the optical signal is input, the optical signal incident direction viewed from the self-propelled vehicle 1 (= Azimuth) is detected. The detected azimuth angle is stored in the block-by-block azimuth angle storage unit 39.

For example, the first detected azimuth angle is stored in the storage area Am (0) as the azimuth angle of the first detection block. If the azimuth detected at the second time is substantially the same as the azimuth detected at the first detection block, the stored data of Am (0) is the azimuth detected at the second time. If it is updated and does not match, it is newly stored in the storage area Am (1) as the azimuth angle of the second detection block. In this way, the optical signals incident from the same direction are stored as the data Am (i) of the same detection block.

The block-by-block light-reception number storage section 40 stores the number of light-receptions by the detection block. The light-reception-count determining unit 41 determines a detection block having a large number of light-receptions and extracts four detection blocks in the descending order of the number of light-receptions. Then, the azimuth angles representative of those detection blocks are read from the block-by-block azimuth angle storage unit 39 to read the azimuth angle storage unit 4.
Store in 2. At this time, the storage area A in ascending order of azimuth angle
Store in ps (1) to Aps (4). The predicted azimuth storage unit 43 stores the azimuth that should be detected in the next scan based on the azimuth detected this time, that is, the predicted azimuth. As described above, this predicted azimuth may be the same value as the azimuth detected this time, or may be a value obtained by adding a planned value.

The azimuth angle comparison section 44 compares the predicted azimuth angle with the detected azimuth angle this time, and outputs a coincidence signal s1 when the both angles substantially match. In response to the coincidence signal s1, the normally open switches SW1 and SW2 are closed, and various data are input and stored in the reference point data storage unit 45. The various data are the azimuth angle Ap, the swing direction As, the number of rotations Cm of the mirror 4 based on the output of the sensor 33, and the number of light reception times Cp (n) for each reference point. Here, the values Ap, As, Cm are stored as a function of the reference point n and the number of light reception times Cp (n) for each reference point.

Next, the function of the pole (reference point) position measurement processing section will be described. FIG. 17 is a block diagram showing the main functions of the pole position measurement processing unit. In the figure, the continuous light receiving detector 46 determines whether or not an optical signal is continuously detected for each rotation of the mirror 4, based on the mirror rotation speed Cm when the reflected light is received. Then, all the values in the swing direction in the continuous period when the most optical signals are continuously received are read from the reference point-specific data storage unit 45 to the swing direction range calculation unit 47. The swing direction range calculator 47 calculates the range of the swing direction based on the maximum value and the minimum value of the supplied swing direction data.

In the swing angle confirmation section 54, based on the detected azimuth angle θ (n) regarding the reference point n and the value indicating the swing direction range calculated by the swing direction range calculation section 47, the light in the horizontal direction is detected. It is confirmed whether the beam irradiation direction, that is, the lowermost angle and the uppermost angle of the swing angle are within the swing direction range. If either the lowermost angle or the uppermost angle of the swing angle is included in the swing direction range, the switching unit 56 switches to the side opposite to that shown in the figure according to an instruction from the swing angle confirmation unit 54. Be done.

By this switching, the swing direction range calculation unit 4
The data calculated in 7 is supplied to the swing range changing unit 55, and the swing range changing unit 55 changes the maximum value max or the minimum value min in the swing direction as described with reference to FIGS. 23 and 24. The data of the swing direction in which the range is changed is supplied to the capture swing direction calculation unit 48 for each reference point.

On the other hand, if both the lowermost angle and the uppermost angle of the swing angle are out of the swing direction range,
The switching unit 56 remains in the state shown in the figure, and the data representing the swinging range is captured as it is by the reference point-based capture swinging direction computing unit 4.
8 are supplied.

The reference swing-by-reference-point-swing-movement-direction calculator 48 calculates the midpoint of this range based on the data indicating the range of the swing direction. The midpoint value Asc (n) is supplied to the mirror direction fixing unit 49. On the other hand, the azimuth angle θ (n) is supplied from the azimuth angle storage unit 42 to the mirror fixing unit 49. In the mirror direction fixing unit 49, these data θ (n) and As
The direction of the mirror 4, that is, the swinging direction and the azimuth, are adjusted with c (n) as the target value, and the mirror 4 is fixed in the direction indicated by these target values.

When the mirror 4 is fixed, the distance measuring unit 50
The distance between the self-propelled vehicle 1 and each reference point is measured at. The distance is calculated, for example, based on the difference between the phase of the optical signal emitted from the light emitting unit 51 and the phase of the optical signal detected by the light receiving unit 52. The reference point position calculation unit 53 calculates the position coordinates [X (n), Y (n)] of the reference point based on the measured distance and the azimuth angle θ (n).

[0149]

As is apparent from the above description, according to the present invention, while rotating the light beam in the horizontal direction, the light beam is vertically scanned at a speed lower than the scanning speed, Even if the topography of the installation location of the position detection device, that is, the moving area of the moving body is not flat, it is performed before the start of operation when the light beam is applied to the reference point, that is, the light reflecting means with high probability. When calculating the position of the reference point, calculate the projection azimuth and angle that are most likely to irradiate the light beam, project the light beam in that projection direction, measure the distance, and calculate the position based on the result. It is possible to reliably detect the position of the reference point, that is, the position of the light reflecting means, which is performed by the above-mentioned method.

Further, even if the irradiation range of the light beam is shifted to one side in the height direction of the light reflecting means due to the distance between the reference point and the position detecting device being too short, or the vertical swing angle being small, Of the vertical range (height direction) range where the light beam is actually irradiated, the projection azimuth and swing angle that can reliably irradiate the light beam to the relatively central part, which is neither the upper end nor the lower end, are set. It is possible to calculate and project the light beam in that direction.

Therefore, even if the vertical projection angle of the light beam is slightly shifted due to mechanical play, the light reflecting means can be reliably irradiated with the light beam. Further, by this, even when the light reflecting means is installed at an angle, it is possible to perform the position detection in a state in which the adverse effect of the error caused by that is small.

[Brief description of drawings]

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

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

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

FIG. 4 is a diagram showing a relationship between a light trace and a light reflector.

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

FIG. 6 is an explanatory diagram of the principle of calculating the traveling direction of a 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 flowchart of a reflected light receiving process.

FIG. 9 is a flowchart showing steering control of a self-propelled vehicle.

FIG. 10 is a flowchart showing steering control of a self-propelled vehicle.

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

FIG. 12 is a flowchart of a poll selection process.

FIG. 13 is a flowchart of a pole trapping swing direction determination process.

FIG. 14 is a flowchart of a pole trapping swing direction determination process.

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

FIG. 16 is a block diagram showing a main function of reflected light receiving processing.

FIG. 17 is a block diagram showing main functions of reference point detection processing.

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

FIG. 19 is a diagram showing data on the number of rotations of the mirror and the swing direction when the reference point is detected.

FIG. 20 is a diagram showing a relationship between a swing direction and a light beam projection direction.

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

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

FIG. 23 is a flowchart for checking the lowermost light beam.

FIG. 24 is a flowchart of the light beam topmost check.

[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 ... Optical reflector, 7 ... Encoder, 8 ... Rotation center axis, 9, 10
... bracket, 11 ... outer ring member, 14 ... inner ring member, 15 ... swing motor, 16 ... electromagnet,
19 ... Connection metal fittings, 23 ... Large disk, 24 ... Small disk, 2
6, 31 ... Connection bolts, 33 ... Swing reference detection sensor, 34 ... Adsorption plate, 35 ... Encoder, 36 ... Travel course, 37 ... Azimuth angle detection unit, 38 ... Swing direction detection unit, 39 ... Block-oriented Corner storage unit, 40 ... Block-by-block light-reception-times storage unit, 41 ... Light-reception-times determination unit, 42
... azimuth storage unit, 43 ... predicted azimuth storage unit, 44 ...
Azimuth angle comparison unit, 45 ... Reference point data storage unit, 46 ...
Continuous light receiving detection unit, 47 ... Oscillation direction range calculation unit, 48
... Reference point-by-reference capture swing direction calculation unit, 49 ... Mirror direction fixing unit, 50 ... Distance measuring unit, 51 ... Light emitting unit, 52 ...
Light receiving part, 54 ... Swing angle confirmation part, 55 ... Swing direction range changing part

Claims (4)

[Claims] 1. A rotary scanning means for rotationally scanning a light beam in a circumferential direction around an observation point, and a reflected light of the light beam reflected by a light reflecting means installed at a position distant from the observation point. In a position detecting device having a light receiving means for receiving light, and detecting the position of the light reflecting means with respect to the observation point based on a light reception signal of the reflected light, a rotation center axis of the rotary scanning is a substantially conical locus. A swing scanning unit configured to scan the light beam a plurality of times in the circumferential direction during one swing cycle in the vertical direction by swinging as drawn, and a light receiving signal is detected by the light receiving unit. Based on the swing angle when the reflected light from the same light reflecting means is received, and the means for detecting and storing the azimuth angle in the rotational scanning direction and the swing angle of the swing scanning means. Same A swing range calculation means for calculating a light receiving swing range capable of receiving the reflected light from the light reflecting means in the azimuth, and the light receiving swing when the light receiving swing range is within the light beam projection range in the same direction. When a substantially central value of the swing range is determined as a reference point capturing / projecting direction suitable for projecting a light beam on the light reflecting means, and when one end of the light beam projection range overlaps the light receiving swing range. , A reference point capture that determines a substantially central value between the overlapping point and one end of the light receiving swing range to the other end as a reference point capture projection direction suitable for projecting a light beam on the light reflecting means. Swing direction determining means, and distance measuring means for projecting a light beam in the swing direction of the reference point to receive the reflected light and measuring the distance between the observation point and the light reflecting means based on the received light signal. A position characterized by having and Detection device. 2. The position detecting device according to claim 1, further comprising a device for calculating the position of the light reflecting device based on the distance measured by the distance measuring device and the detected azimuth angle at that time. apparatus. 3. The rotary scanning means and the light receiving means are fixed to a common rocking member which rocks so that a rotation center axis of the rotary scanning means draws a substantially conical locus. The position detection device according to claim 1. 4. The position detecting device according to claim 1, wherein the observation point is a moving body and is mounted on the moving body.