JPH04350514A - Position detector - Google Patents

Abstract

PURPOSE:To achieve the detection operation even if levels of an installation surface of an observation point and a reflector do not match by allowing a light beam which is emitted from an observation point to be applied to a reflector and then the position of the reflector is detected by its reflected light. CONSTITUTION:A light beam which is emitted from a light-scanning device 2 is rotated and scanned in a direction of an arrow R and at the same time its rotary center axis 8 is rotated as shown by an arrow 17a while it is inclined by an angle of phi. According to scanning of this light beam, an orientation angle for receiving a reflection light 2R from light reflectors 6a-6d and a rocking direction of light beam are detected. A light beam is directed toward the orientation angle and rocking direction which are detected for receiving a reflection light and the orientation angle of each reflector and a distance to the reflector observed from an observation point are detected based on the light-reception signal.

Description

[Detailed description of the invention]

0001

[Industrial Application Field] The present invention relates to a position detection device.
In particular, self-propelled machines used in agricultural and civil engineering work;
It relates to position detection devices such as automatic conveyance devices used in factories.

0002

2. Description of the Related Art Conventionally, a device for detecting the current position of a moving object includes means for scanning a light beam generated by the moving object in a circumferential direction centering on the moving object, and at least three locations distant from the moving object. An apparatus has been proposed that is equipped with a light reflecting means that is fixed at a reference point and reflects light in the incident direction, and a light receiving means that receives the light reflected by the light reflecting means (Japanese Patent Laid-Open No. 59-67476). Publication No.).

[0003] In this device, the opening angle between the three light reflecting means as seen from the moving object, that is, the observation point, is detected based on the output signal of the light receiving means. The position of the moving object is calculated from the detected opening angle and preset information (position information) representing the position of each light reflecting means.

In the above-mentioned apparatus, a slight error in the positional information of the reference point, that is, the light reflecting means, affects the accuracy of control of the entire system. For this purpose, for example, if the mobile object is a working machine used in farmland, the position information of the light reflecting means installed in each work area, that is, the spacing and relative angle of each light reflecting means, can be collected prior to the work using the working machine. It had to be measured accurately. In this way, the spacing and relative angle of each light reflecting means installed in a large work area such as farmland can be adjusted.
It was extremely difficult to take accurate measurements and input them every time the work area changed.

In response to this problem, the present applicant has proposed a device that can simplify the work of measuring and inputting the position information (Japanese Patent Laid-Open No. 1-287415). This device scans a light beam projected from a self-propelled vehicle, that is, a moving object, and detects the reflected light of this light beam from a light reflecting means. Then, the distance between the moving object and the light reflecting means calculated based on this reflected light detection signal and the azimuth angle of the light reflecting means as seen from the moving object are used to accurately measure the spacing and relative angle of each light reflecting means. and the work of inputting it can be done automatically.

[0006] Incidentally, in the case where the distance or relative angle between the light reflecting means is measured, for example, a position detection device is mounted on a moving body, the moving area of this moving body is not necessarily flat. Therefore, depending on the topography of the location where the moving object is placed to measure the distance and azimuth, the light beam may be scanned while the moving object is tilted.
Even if the light beam is rotated and scanned only in one direction, that is, in the horizontal direction, the light beam may not be able to irradiate the light reflecting means. In this case, the light receiving means cannot detect the reflected light of the light beam that should be reflected by the light reflecting means and returned. Moreover, on the contrary, there have been cases where unnecessary light from a reflecting object other than the intended light reflecting means is detected.

[0007] In this way, if the reflected light from the light reflecting means cannot be detected, or if light from another source is mistakenly recognized as reflected light from the intended light reflecting means, the position of the moving object cannot be accurately calculated. For example, the mobile object may not be able to run along the planned travel course.

[0008] As a countermeasure for solving this problem, it is conceivable to oscillate the light beam in the vertical direction in addition to the rotational scanning in the horizontal direction. For example, a beam light scanning device has been proposed in which a generated light beam is scanned in the horizontal direction while being vibrated in the vertical direction at high speed using a galvanometer mirror (Japanese Patent Application Laid-Open No. 60-242313).

FIG. 22 is a diagram showing a scanning locus (light trail) of a light beam by such a conventional device. Figure (a)
shows a portion of the light trail when the light beam is rotated and scanned in the horizontal direction and also oscillated and scanned in the vertical direction using a galvanometer mirror. FIG. 5B shows a portion of the light trail when the vertical scanning of the light beam is performed using a polygon mirror.

0010

SUMMARY OF THE INVENTION The above beam light scanning device has the following problems. In a method that uses a galvano mirror to perform swing scanning with a predetermined amplitude in the vertical direction, as the distance between the moving object and the light reflecting means increases, the amplitude of the light trail at the position of the light reflecting means increases. , the wavelength becomes longer. Therefore, for example, as shown in FIG. 22(a), the light reflecting means 6 may not be able to intersect with the light beam.

[0011] Also, in the system using a polygon mirror, as the distance between the self-propelled vehicle and the light reflecting means 6 increases, the distance between the light traces at the position of the light reflecting means becomes wider. For this purpose, for example, as shown in FIG. 22(b), the light reflecting means 6
It is possible that the light beam and the light beam cannot intersect with each other.

In order to make it easier for the light beam to cross the light reflecting means 6 by reducing the wavelength or interval of the light trace, it is necessary to increase the speed ratio of the vertically swinging scan to the rotational scan. In other words, increase the driving speed for vertical swing scanning, or
It is necessary to reduce the rotational scanning speed.

However, it is very difficult to increase the driving speed of the galvano mirror or polygon mirror due to mechanical structural limitations. In addition, if the horizontal scanning speed is reduced, the number of received light data per certain period of time will decrease, resulting in a decrease in position detection accuracy, especially when used to detect the position of a moving object such as a self-propelled vehicle. is remarkable.

[0014] The present invention solves the problems of the prior art described above.
By configuring the light beam projected from the moving body to irradiate each light reflecting means with high probability, accurate positional information of the light reflecting means can be easily detected, and the position of the observation point can be determined based on that positional information. An object of the present invention is to provide a position detection device that can accurately detect a position.

[0015]

[Means for solving the problem] Solving the above problems,
To achieve the object, the present invention comprises a light beam generating means and a light receiving means for detecting an optical signal, which are arranged at an observation point;
scanning means for scanning in the circumferential direction while vertically swinging the light beam generated by the light beam generating means; and means for detecting the azimuth of the incident light when the light signal is received by the light receiving means. , means for storing substantially the same azimuth angles among the detected azimuth angles as one azimuth angle data;
a reference point recognition means for recognizing that the azimuth angle data is the same number as the number of light reflecting means arranged in advance at a plurality of reference points distant from the observation point; When a received light signal is detected from the direction indicated by one of the azimuth angle data, the rotation scanning of the light beam is stopped and the projection direction of the light beam is fixed in that direction. means for receiving the reflected light of the light beam while the projection direction of the light beam is fixed, and measuring the distance between the observation point and the light reflecting means based on the received light signal; and measuring the distance. means for releasing the fixation of the projection direction of the light beam on condition of termination;
The first feature is that the distance measurement is repeated as many times as there are light reflecting means, and the position of the observation point with respect to the reference point is calculated based on the measured distance and azimuth data at this time.

The present invention also provides a method in which the light beam generating means and the light receiving means are fixed to a common table, and this table is swung so that the central axis of rotation for rotating and scanning the light beam draws a conical locus, and A second feature is that the light beam is configured to perform rotational scanning of the light beam a plurality of times during this unit swing cycle, thereby causing the light beam to swing in the vertical direction.

Further, the present invention includes means for detecting a swinging angle and an azimuth of the table with respect to the incident light detected by the light receiving means, and stopping the swinging of the table at the swinging angle, and detecting the stopped state. A point configured to perform rotational scanning at , and in this rotational scanning state, when an optical signal is detected again at the detected azimuth, the rotational scanning is stopped, the light beam projection direction is fixed, and the distance measuring operation is performed. has a third characteristic.

[0018]

[Operation] In the present invention having the first feature described above, the azimuth angle of each light reflecting means arranged at each reference point is first detected and memorized, and the azimuth angle of each light reflecting means arranged at each reference point is first detected and stored. After the angle has been detected, the reflected light can be detected by reliably irradiating the light beam to the light reflecting means in sequence based on the stored data. Therefore, the distance between the light reflecting means and the observation point can be accurately measured, and the position of the reference point and further the position of the observation point can be accurately calculated.

Furthermore, in the present invention having the second feature, rotational scanning is performed a plurality of times during one cycle of vertical scanning of the light beam. Therefore, assuming a cylindrical surface centered at the observation point, a mesh-like light trail is drawn by the light beam on the cylindrical surface. That is, in the vicinity of the vertically erected light reflecting means, rotational scanning is performed several times or more at different heights in the vertical direction during one cycle of rocking. As a result, the light beam crosses the light reflecting means with a high probability, and the probability of receiving reflected light by the light reflecting means increases accordingly.

Since the light beam projection direction for distance measurement can be determined based on the reflected light from the light reflecting means detected in this manner, the possibility of losing sight of the light reflecting means due to unevenness of the terrain or the like is reduced.

Furthermore, in the present invention having a third feature,
Rotational scanning can be performed with the vertical swinging of the light beam stopped at the swinging angle detected by the light reflecting means at least once in the previous scanning, and the rotating scanning can be performed at a low speed. Therefore, since it is possible to project the light beam in a direction that is effective for receiving the reflected light from the light reflecting means based on the stored data, in that state, the reflected light from the light reflecting means in a good receiving state can be projected. The distance between the observation point and the reference point can be measured by continuously receiving light.

[0022]

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, light reflectors (hereinafter simply referred to as reflectors) 6a to 6a having reflective surfaces that reflect incident light in the direction of incidence are located around the area where the self-propelled vehicle 1 as a moving body is traveling. 6d
is installed. Known light reflecting means such as a corner cube prism is used for the reflecting surfaces of the reflectors 6a to 6d. The self-propelled vehicle 1 is, for example, a lawn mower having a cutter blade (not shown) for mowing the lawn 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 includes a light emitter that generates a light beam 2E, and the reflectors 6a to 6.
It has a light receiver that receives the reflected light 2R of the light beam 2E reflected at d. The light emitter has a light emitting diode, and the light receiver has a photodiode that converts incident light into an electrical signal. A light emitter and a light receiver are connected to the inner ring member 1.
4 is housed in a casing 3 which is attached by known fastening means such as bolting.

The light beam emitted from the light emitter is refracted and reflected in a right angle direction by a rotating mirror (hereinafter simply referred to as a mirror) 4, thereby changing its direction and being projected from the scanning device 2 to the outside. The mirror 4 is rotated by the motor 5 around the central axis of rotation 8 in the direction of an arrow 17, and the rotation of the mirror 4 causes the light beam 2E to rotate and scan in the direction of the arrow R around the central axis of rotation 8. The projection direction of the light beam 2E determined by the rotational position of the mirror 4, that is, the rotational angle of the motor 5, is detected by an encoder 7.

The scanning device 2 has a gimbal swing mechanism for continuously changing (swing scanning) the angle of the rotating scanning surface drawn by the light trail of the light beam 2E. This swing mechanism is
An outer ring member 11 is swingably supported on a shaft 12 of the bracket 9 and a shaft (not shown) of the bracket 10.
and an inner ring member 14 provided inside this outer ring member 11. This inner ring member 14 is
It is oscillated by a shaft 13 provided on the outer ring member 11 on a line perpendicular to the extension line of the support shaft of the outer ring member 11 and another shaft 20 (shown in FIG. 1) provided at a position facing this shaft 13. It is pivoted for free movement.

[0025] The gimbal swing mechanism includes a swing drive motor 1.
5. Due to this gimbal swing mechanism, the rotation center axis 8 of the mirror 4 is tilted by an angle φ from the vertical, and the direction of the tilt (hereinafter referred to as the swing direction) is continuously changed and rotated in the direction of the arrow 17a. . Due to such rotation of the rotation center shaft 8, the angle of the scanning surface caused by the rotational scanning of the light beam 2E changes continuously. That is, the projection direction of the light beam 2E changes continuously in the vertical direction, and swing scanning is performed.

Next, the scanning device and the rocking drive device of the gimbal rocking mechanism will be described in detail. FIG. 1 is a sectional view of a main part of a scanning device 2 mounted on a self-propelled vehicle 1, and the same reference numerals as in FIG. 2 indicate the same or equivalent parts. First, the scanning device 2 will be explained. The mirror 4 is attached to one end 5a of the shaft of the motor 5 via a pedestal 4a. On the other hand, 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 pulses of the encoder 7 are sent to a control device (not shown) and are used to calculate the rotation angle and number of rotations of the mirror 4.

A suction plate 34 is provided on the pedestal 4a of the mirror 4. This attraction plate 34 is made of a magnetic material, for example iron, and is attracted to the electromagnet 16 when the electromagnet 16 is energized. By this attraction operation, the stop position of the mirror 4 is fixed at any timing when the electromagnet 16 is energized.

Next, the swing drive device of the gimbal swing mechanism will be explained. The swing drive device is provided on the top surface of the self-propelled vehicle 1. Bearing 21 attached to the top surface of self-propelled vehicle 1
A shaft 22 is inserted through the shaft 22, and a small disk 23 is fixed to one end of the shaft 22, and a large disk 24 is fixed to the other end. The small disk 23 has an eccentric shaft 23a protruding from a position eccentric to the shaft 22, and the large disk 24 similarly has an eccentric shaft 23a.
4a is provided protrudingly. Eccentric shaft 23a and eccentric shaft 24
The eccentric directions of a are shifted by 90 degrees from each other.

The shaft 15a of the swing motor 15 is connected to the shaft 22.
An L-shaped block 32 is fixed to the shaft 15a. In other words, the eccentric shaft 23
a, 24a are eccentric with respect to the shaft 15a by the same amount as the eccentricity with respect 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 swing motor 15
When is rotated, this rotation is transmitted to the eccentric shaft 23a by the block 32, causing the shaft 22 to rotate. As a result, the eccentric shaft 24a also rotates around the shaft 22.

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

Since the small disk 23 and the inner ring member 14 are connected in this way, the rotational movement of the eccentric shaft 23a with respect to the small disk 23 is caused by the swinging of the inner ring member 14 about the shafts 13 and 20. converted into movement.

On the other hand, an eccentric shaft 24 protruding from the large disk 24
a is supported by a spherical bearing 28. Outer ring member 1
1 is provided with a protruding shaft 29, and a spherical bearing 30 is supported by this 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 also has the same structure as the inner ring member 1.
4, the shaft 12 and the shaft (
(not shown).

When the swings of the outer ring member 11 and the inner ring member 14 are combined, the central axis of rotation 8 of the mirror 4 of the scanning device 2 attached to the inner ring member 14
The ring members 11 and 14 pivot at a predetermined angle of inclination about the intersection of their respective pivot axes. In other words, the locus of the rotation center axis 8 due to this turning becomes a side surface of a cone (hereinafter simply referred to as a cone) with the apex at the intersection. Since the casing 3 housing the light emitter and the light receiver is also attached to the lower surface of the inner ring member 14, it swings together 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,
This connecting bolt 26 connects the block 25 and the spherical bearing 2.
7, the connection length between the spherical bearing 27 and the block 25 can be adjusted. Like the connecting bolt 26, the connecting bolt 31 is also used to adjust the length of connection with the spherical bearings 28, 30 into which the connecting bolt 31 is screwed.

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

An encoder 35 for detecting the rotational position of the motor 15 is attached behind the motor 15. The inclination φ of the rotation center axis 8 of the mirror 4 can be detected by the output signal of the encoder 35 and the output signal of the sensor 33. 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 amount of rotation of the thin disk 24b in addition to the slit for detecting the reference position, and two types of slits may be detected by two sensors. . Further, the encoder 35 may be configured to output both signals indicating the amount of rotation of the motor 15 and the reference rotation position.

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

In this embodiment, the swing locus is conical, that is, the outer ring member 11 and the inner ring member 14
Eccentric shaft 2 so that the maximum inclination angle of each is the same.
The amount of eccentricity of 3a and 24a is set.

In this embodiment, the outer ring member 11 and the inner ring member 14 are driven by one motor, but each ring member may be driven by a separate motor. In that case, it goes without saying that each motor is rotated synchronously so that the central axis of rotation 8 draws a desired conical shape.

When the above-described swing mechanism is driven to project a light beam, a swing scan is performed in which the central axis of rotation 8 of the mirror 4 rotates in a conical manner, and the rotation of the mirror 4 creates a light trail. The surface drawn by (rotational scanning surface) is not fixed to one plane and constantly changes during one swing cycle.

By making the period of one rotation of the mirror 4 sufficiently shorter than the period of one rotation of the central axis of rotation 8 drawing a cone, it is possible to obtain a scanning locus at a fine pitch as described later. You can draw it. In this embodiment, the mirror 4 was rotated at 2700 rpm, and the shaft 22 for swinging the rotation center shaft 8 was rotated at 90 rpm.

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

As shown in the figure, the light beam 2E projected from the scanning device 2 creates 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 number of rotations of the mirror 4 is set to 27.
00 rpm, and the number of swings of the rotation center shaft 8, that is, the rotation speed of the shaft 22, was set to 90 rpm, so that the mirror 4 itself rotates 30 times while the rotation center shaft 8 makes one conical rotation. That is, 30 light trails cross any vertical line 18 on the cylindrical surface while the central axis of rotation 8 makes one rotation in a conical shape.

Next, when the reflector is disposed on the vertical line 18, how easily the light beam is irradiated to the reflector during one swing cycle will be explained. FIG. 4 shows an enlarged view of a portion of the light trace. In the figure, as shown by reference numeral 6H, when the self-propelled vehicle 1 and the reflector 6 are close and the height dimension of the reflector 6 is sufficiently long with respect to the swing width BB of the light trail, 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 shown by the symbol 6L, the dimension in the height direction of the reflector 6 is relative to the swing width BB of the light trail. becomes shorter. However, as described above, even if the height dimension of the reflector 6 is relatively short, if the maximum distance H between the light trails in the vertical direction is relatively smaller than the height direction dimension of the reflector 6. , the light trail crosses the reflector 6 at least once during one conical movement of the rotation center shaft 8. In addition, FIGS. 3 and 4 are
The number of light trails is less than the actual number because it is shown as a model to avoid complexity and facilitate drawing.

Next, the basic principle for detecting where the self-propelled vehicle 1 equipped with the scanning device 2 having the above configuration is located within its travel area and in which direction it is traveling. Explain. 5 and 6 show the self-propelled vehicle 1 and the reflectors 6a to 6a in the coordinate system showing the travel area of the self-propelled vehicle 1.
It is a figure which shows the position of 6d. In the figure, reflector 6a
~6d, that is, the reference points A, B, C, D, and the position T (Xp, Yp) of the self-propelled vehicle 1, the origin is the reference point B, and the x-axis is the straight line connecting the reference points B and C. x-y
Represented in a coordinate system.

As shown, the position T of the self-propelled vehicle 1 is on the circumcircle of the triangle ATB and at the same time on the circumcircle of the triangle BTC.
Exists on the circumcircle of Therefore, the position of the self-propelled vehicle 1 is determined by calculating the intersection of the circumscribed circles Q and P of these two triangles. Since one of the two intersections of the circumscribed circles Q and P is the reference point B, that is, the origin, the other intersection is the position of the self-propelled vehicle 1. A calculation formula for determining the position of the self-propelled vehicle 1 according to this principle is disclosed in Japanese Patent Application Laid-Open No. 1-2874, which the applicant has already filed.
Details are shown in No. 15 and Japanese Unexamined Patent Publication No. 1-316808.

Further, the traveling direction of the self-propelled vehicle 1 is calculated using the following equation. In FIG. 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 with respect to the traveling direction is θc, the x-coordinate of the reference point C is xc, y
When the coordinate is Yp, θf=360°-tan-1{Yp/(xc-x)
}-θc (1).

Next, steering control for controlling the running direction of the self-propelled vehicle 1 based on the position information obtained by the calculation formula described in the above publication and the calculation formula (1) above will be explained. FIG. 7 is a diagram showing the positional relationship between the self-propelled vehicle 1 and reference points A to D.

It is assumed that the self-propelled vehicle 1 starts traveling from a starting position near the reference point B, travels along the planned traveling course 36, and returns to the home position 63. The running course consists of straight strokes that are set in parallel with an interval L, and a turning stroke that connects the straight strokes. After the self-propelled vehicle 1 has traveled straight ahead, at the position where the y-coordinate reaches Ytn or Ytf,
The vehicle travels through a turning stroke with the steering angle fixed at a constant value, and then moves on to the next straight straight stroke. If the x-coordinate of the straight-line travel exceeds the final x-coordinate Xend, the vehicle returns to the home position 63 after the straight-line travel through the final turning stroke.

In FIG. 7, in order to simplify the explanation, the reference points A, B, C, and D are arranged so that straight lines connecting them form a rectangle, and the straight line distance is Although the straight line connecting points A and B is parallel to the y-axis, 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 explained with reference to a flowchart. The meanings of various parameters (symbols) used in the flowcharts referred to for this explanation are as follows. θ(n)...Azimuth determined based on the light reception signal, θq(n)...Predicted azimuth, Cg(i)...Number of light receptions for each detection block, Am(i)...Detection azimuth for each detection block, Cp( n)...Number of light reception at reference point n, Ap[n,
I]... Light receiving azimuth angle of reference point n, As [n, I]... Rocking direction when reference point n is detected, Cm [n, I]... Mirror rotation number counter value when reference point n is detected, Aps (k )...Azimuth representing the detection block whose number of light receptions is equal to or greater than the threshold value, Aps
(n)...Azimuth where Aps(k) is set to n=1 to 4 in descending order, i...Detection block number, j...Number of detection blocks whose number of light receptions is greater than or equal to the first threshold, k...Light reception The number of detection blocks whose number of times is equal to or greater than the second threshold, I... A number indicating the storage order of the rocking direction when the mirror 4 is rotated a predetermined number of times and the reference point n is detected, J... The mirror 4 is rotated a predetermined number of times. Number of consecutive detections of reference point n when rotating, K... Maximum number of consecutive detections of reference point n when mirror 4 is rotated a predetermined number of times, e
...The last number of the numbers indicating the memory order of the rocking direction when the maximum number of consecutive detections occurs, Asc(n)...Reference point n
The swinging direction that can be captured with a high probability, Ac(n)...The azimuth angle determined based on the light reception signal in straight-ahead processing, θt
(n) Turn release angle of reference point n for turn release First, reflected light reception processing, which is the basis of steering control, will be explained. The light beam emitted from the scanning device 2 and reflected by the reflectors 6a to 6d, that is, the reflected light, is received and processed as follows.

FIG. 8 is a flowchart showing the control procedure for the reflected light reception process. In step S100, it is determined whether an optical signal is detected by the light receiver. If an optical signal is detected, the process advances to step S101. however,
At this point, it is not possible to determine whether the detected optical signal is reflected light from the reflectors 6a to 6d.

In step S101, it is determined whether the signal detection is due to chattering, depending on whether the angle by which the mirror 4 was rotated after the previous processing is very small. That is, if a plurality of optical signals are detected before the mirror 4 rotates by only a small angle, it is determined that chattering is occurring, and optical signals detected later are ignored. If there is no chattering, the process advances to step S102.

In step S102, a variable i indicating the detection block number is set to "0". In this embodiment, the mirror 4 rotates 30 times while the rotation center axis 8 of the mirror makes one rotation while drawing a conical locus. In other words, rotational scanning is performed 30 times while the rotation center shaft 8 makes one rotation while drawing a conical locus. Through these 30 rotational scans, there is a possibility that reflected light from the same reflector will be received many times. Detection data regarding a plurality of optical signals incident on the light receiver from substantially the same direction are stored together in one group as data for the same light emitting source or reflector. This group is called a detection block. Therefore, if only the light from the scheduled reflectors 6a to 6d is detected, the number of detection blocks is four, which corresponds to the number of installed reflectors.

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

In the first process, this determination is affirmative, and the process proceeds to step S106, where the mirror angle, that is, the azimuth angle at which the light was detected, is stored. The currently detected azimuth is stored as the azimuth Am(i) representing the detection block (i), and the number of times the optical signal is received by the detection block (i) is calculated as Cg(i).
Increment the value of .

In step S107, the value n of the counter for identifying the reference point is cleared. In this example, the counter value "1" corresponds to the reference point A, the counter value "2" corresponds to the reference point B, the counter value "3" corresponds to the reference point C, and the counter value "4" corresponds to the reference point D. I've let it happen. Step S108
Then, the value n of the counter is incremented.

In step S109, it is determined whether or not the currently detected azimuth is substantially the same as the predicted azimuth θq(n) set in the initial pole identification process or outbound straight-on process, 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 of the reference point A corresponding to this counter value "1" substantially matches the detected azimuth. be judged. Predicted azimuth angle θ
For example, q(n) is a constant value α for the azimuth at the time of current detection.
However, since the reception interval of the reflected light is short relative to the amount of movement of the self-propelled vehicle 1, there is no practical problem in using the same value as the current value as the predicted azimuth, and the processing is simple.

[0059] If the determination in step S109 is negative,
In step S110, it is determined whether the counter value n is "4". The processes in steps S108 and S109 are repeated until the determination in step S110 becomes affirmative, and the reference points A to
For all predicted azimuths θq(n) of D, it is determined whether these and detected azimuths substantially match.

If the predicted azimuth θq(n) substantially matches the detected azimuth, steps S109 to S11
Go to 1. In step S111, assuming that the scheduled reference point has been detected, the number of times Cp(n) of light reception at the reference point indicated by the counter value n is incremented. Further, the light receiving azimuth 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[n,
Cp(n)]. This rotation counter value is a value indicating the number of rotations of the mirror 4 counted from the time when the rocking direction is in the predetermined direction based on the output signal of the sensor 33 as a reference.

Note that if it is determined in step S103 that the number of light receptions Cg(i) in the detection block (i) is not "0", that is, it is not the first time that light is received, step S1
Proceed to 04. In step S104, the detection azimuth is the azimuth Am(
It is determined whether or not it substantially matches i). If the two match, the process advances to step S106, and the azimuth Am(i) of the detection block (i) is updated with the current detection azimuth.

Further, if the determination in step S104 is negative, that is, if the azimuth Am(i) of the optical signal previously received by the detection block (i) does not match the azimuth detected this time, It is determined that the light is from another detection block, and the process proceeds to step S105, where the detection block number (i)
Increment. After incrementing the detection block number (i), the process proceeds to step S103, and it is determined whether or not the detection block corresponding to the incremented detection block number (i) is receiving light for the first time.

Based on the azimuth of the light reception signal stored by the reflected light reception process, that is, the azimuth of the reference point, the position and traveling direction of the self-propelled vehicle 1 are calculated as described later, and steering control is performed. . 9 and 10 are general flowcharts showing the entire steering control.

In FIG. 9, in step S1, the motor 5
and 15 to rotate the mirror 4 about the rotation center axis 8, and operate the gimbal swing mechanism so that the rotation center axis 8 draws a conical trajectory. 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 are reliably irradiated with the light beam.

In step S2, initial pole identification processing is performed to determine the initial azimuths of the reference points A to D, that is, the reflectors 6a to 6d. Details of this processing will be described later with reference to FIGS. 11 and 12. In step S3, a pole position measurement process is performed to measure each distance from the self-propelled vehicle 1 to the reference points A to D and calculate the position of each reference point, that is, the reference coordinate value in the xy coordinate system. The details of this process are shown in Figure 1.
3, as will be described later with reference to FIGS. 14 and 15. Step S
4, the current position coordinates (Xp, Yp) of the self-propelled vehicle 1 are calculated based on the azimuth and coordinate values of the reference point calculated in steps S2 and S3. In step S5,
The current x-coordinate Xp of self-propelled vehicle 1 is
Set the coordinates as Xref. 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.

Subsequently, in step S8 of FIG. 10, an outbound straight-line process is performed in which the self-propelled vehicle 1 is caused to travel straight ahead in a direction in which the y-coordinate value increases. In this straight forward processing, the self-position (X
p, Yp) and the traveling direction θf. Then, the difference between these values and the set travel 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 process is not directly related to the present invention, a detailed flowchart will not be illustrated.

In step S9, the y-coordinate Yp of the self-propelled vehicle 1 is
It is determined whether or not the first straight-line travel has been completed based on whether or not the y-coordinate Ytf has become larger than the scheduled y-coordinate Ytf. When it is determined that the self-propelled vehicle 1 has finished traveling in a straight line, the process proceeds to step S10. In step S10, the next straight travel is set by adding the distance L to the adjacent straight travel to the x-coordinate Xref of the straight travel. In step S11, right turn release angle setting processing is performed to set the azimuth at which the turning stroke is ended. In the right turn release angle setting process, the azimuth angle for each reference point, that is, the right turn release angle, is calculated for each reference point to complete the U-turn process to be performed in the next step.

In step S12, the steering angle is fixed at a predetermined value and the self-propelled vehicle 1 is turned clockwise with a constant turning radius.
A U-turn process is performed in which the self-propelled vehicle 1 travels along a scheduled turning stroke. Since the processes in steps S11 and S12 are also not directly related to the present invention, detailed flowcharts are omitted. In step S13, the value of a release counter (counted during the U-turn processing) that counts the number of reference points at which the azimuth seen from the self-propelled vehicle 1 has reached the scheduled right turn release angle exceeds "1". determine whether or not. If this determination is affirmative, it is determined that the turning stroke has ended, and the process proceeds to step S14.

[0070] In step S14, a return trip straight process is performed in which the self-propelled vehicle 1 is caused to travel straight in a direction in which the y-coordinate value thereof becomes smaller. This straight-on return process is similar to the straight-on outward process in step S8. In step S15, it is determined whether or not the second straight travel has been completed based on whether or not the y-coordinate Yp of the self-propelled vehicle 1 is smaller than the scheduled y-coordinate Ytn. In step S16, the x-coordinate Xref of the straight travel
It is determined whether or not exceeds the x-coordinate Xend of the scheduled end point of the run. If the determination in step S16 is negative, the process advances to step S17 to set the next straight stroke. In step S18, left turn release angle setting processing is performed to set the azimuth angle at which the leftward turning stroke is ended. This process is similar to the right turn release angle setting process except that the value of the release angle to be set is different. In step S19,
Perform U-turn processing. In step S20, it is determined whether the value of the release counter exceeds "1". If this determination is affirmative, it is determined that the turning stroke has ended and the process returns to step S8.

Further, if the determination in step S16 is affirmative, the process advances to step S21. If the determination in step S16 is affirmative, this means that all straight-line travel has been completed, and in step S21, processing is performed to set the release angle for the final turning stroke. This process is performed in the same way as the right turn release angle set and the left turn release angle set.

[0072] In step S22, a U-turn process is performed,
In step S23, it is determined whether the value of the release counter exceeds 1 or not. In step S24, a process is performed in which the vehicle travels straight back to the home position 63. This process is similar to the straight forward process.

[0073] In step S25, the x-coordinate of the self-propelled vehicle 1
It is determined whether p has become 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 process ends.

Next, the initial pole identification process in step S2 will be described in detail. FIG. 11 is a flowchart of the initial pole identification process, and FIG. 12 is a flowchart of the pole selection process performed in the initial pole identification process.

In this initial pole identification process, the reflected light reception process is repeated until the number of detection blocks that have received light more than a predetermined number of times matches the number of all installed reflectors, and the reflector or reference point is determined. This is the process of In other words, the detection azimuth Am(i) representing the detection block that matches the number of reflectors is determined as the azimuth of the reference point.

In FIG. 11, in step S120,
It is determined whether the swing direction has reached "0°", that is, whether or not the predetermined reference position has been detected by the swing reference detection sensor 33. When the scheduled reference position is detected and it is determined that the swing direction is "0°", step S12
Go to 1. In step S121, the data in the memory area that stores the data obtained by the reflected light reception process is cleared.

In step S122, the reflected light reception process shown in FIG. 8 is performed. In the reflected light reception process in this initial pole identification process, since the predicted azimuth has not been determined before this process, processes corresponding to steps S100 to S106 of the reflected light reception process described with reference to FIG. 8 are performed. .

In step S123, the rocking direction is changed again to "
0°". In other words, it is determined whether one cycle of oscillating scanning in which the rotation center axis 8 draws a cone has been completed.The reflected light reception process is not performed until one cycle of scanning is completed. Continuing, when one cycle is completed, the process advances to step S124.

In step S124, reference point selection processing (pole selection processing) is performed. In this selection process, four detection blocks with a large number of light receptions Cg(i) are selected from among the detection blocks detected in the reflected light reception process, and the detection azimuth Am(i) representing the detection block is set as the azimuth angle. Set Aps(n) in descending order. In this embodiment, the reference points are set at four locations A, B, C, and D, so n=1 to 4. As shown in the flowchart of the reflected light reception process in FIG. 8, the detected azimuth Am(i) representing a detection block is the latest data of the azimuth detected by that detection block. By using the latest data in this way, the storage capacity of the memory for storing this azimuth angle can be saved.

Furthermore, in step S124, the pole selection modes "1" to "3" are determined, which will be the basis for the determination in the next step S125. The pole selection process is
Further details will be given with respect to FIG.

In step S125, it is determined whether the pole selection mode determined in the pole selection process is one of "1" to "3". If the pole selection mode is "1", it is assumed that all four reference points have been identified and their azimuths have been detected, and the process advances to step S126. Step S1
26, the azimuth angle θ(n) of the reference point is set as step S
The azimuth Aps(n) obtained in step 124 is set. If the pole selection mode is "2", the four reference points cannot be selected, that is, the number of light receptions Cg(i) has not reached the predetermined value, and the process returns to step S122 to continue the reflected light reception process. Furthermore, when the pole selection mode is "3", there are five or more detection blocks in which the number of light receptions Cg(i) is equal to or greater than a predetermined value due to an unexpected reflective object. In this case, it is assumed that the reference point could not be specified from among the detection blocks, and step S1
Returning to step 21, this initial pole identification process is repeated from the beginning.

Next, an example of data obtained in the reflected light reception process in this initial pole identification process will be shown. In the initial pole identification process, a reference point identification process is performed based on the light reception data stored while the rotation center shaft 8 swings once, that is, while the mirror 4 rotates 30 times. FIG. 18 is a diagram showing an example of received light data accumulated while the mirror 4 rotates 30 times.

In the figure, the vertical axis represents the detection block (i)
The horizontal axis is the number of light receptions Cg(i) of each detection block (
i) is the azimuth angle Am(i). As shown in the figure, if there are seven detection blocks (i=0 to 6), this means that optical signals were received from seven directions while the mirror 4 rotated 30 times. In a detection block in which the number of light receptions Cg(i) is plural, the azimuth Am(i) is the latest detection data as described above. Note that the detection block numbers i are not necessarily arranged in order of decreasing azimuth angle because the numbers are assigned in the order of light reception.

The details of the pole selection process will be explained with reference to this light reception data. FIG. 12 is a flowchart of the pole selection process. In this pole selection process, detection blocks whose number of light receptions Cg(i) has reached a predetermined threshold value are extracted, and whether the number of extracted detection blocks matches the predetermined number of reference points, that is, "4"? determine whether If they match, the light reception data of that detection block is determined to be data related to the scheduled reference points A to D. Furthermore, if the number of extracted detection blocks is large, it is determined that the reference point cannot be specified and data is collected again, and if the number of extracted detection blocks is small, data collection is continued.

In this embodiment, the threshold values are set to "3" and "5".
Two stages were set. Then, the number of light receptions Cg(i) is the first
The number of detected blocks that have reached the second threshold value "3" is stored as a parameter j, and the number of detected blocks that have reached the second threshold value "5" is stored as a parameter k. Based on these parameters j and k, it is determined whether or not the reference point can be identified.

First, in step S130, data obtained in the reflected light reception process, that is, the light detection azimuth Am(i) and the number of times Cg(i) of the reflected light is received are read.

In step S131, parameters i, j
, k is cleared. In step S132, it is determined whether the detection block (i) is a detection block whose number of light receptions Cg(i) is greater than three. Number of light reception Cg
If (i) is a detection block that has been detected more than three times, the process proceeds to step S133, and a parameter j indicating the number of detection blocks for which the number of light receptions Cg(i) is three or more times is incremented.

In step S134, the number of light receptions Cg(i
) is detected more than 5 times. If the detection block receives light more than 5 times,
Proceeding to step S135, a parameter k indicating the number of detection blocks for which light has been received five or more times is incremented.

In step S136, it is determined whether the value of parameter k is "4" or more. It is determined whether there are four detection blocks in which the number of light receptions Cg(i) exceeds five times, that is, whether or not there are more than the total number of scheduled reference points. If the number of detection blocks for which the number of light receptions Cg(i) exceeds 5 is four or less, in step S137, the number of light receptions Cg(i)
) is the detection azimuth Am(i) representing the detection block that has occurred five or more times.
(k).

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

In step S140, parameters j, k
It is determined whether or not both are "4", that is, whether the number of detection blocks that received light three times or more and the number of detection blocks that received light five or more times were both four. If this judgment is affirmative, it is judged that both detection blocks are the same, and the azimuth Am(i) stored corresponding to these four detection blocks is the azimuth of the reference points A to D. It is determined that

If step S140 is affirmative, the process advances to step S141, where the detected azimuths Aps(k) stored in step S137 are sorted in ascending order of azimuths Aps(1).
~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, where it is determined whether the parameter j is "4" or more, that is, whether there are four or more detection blocks that have received light three or more times. do. Step S14
If 3 is negative, the pole selection mode is set to "2" in step S144, and if affirmative, the pole selection mode is set to "3" in step S145. According to these determined pole selection modes, the determination in step S125 in the initial pole identification process of FIG. 11 is performed.

For example, in the example of the light reception data shown in FIG. 18, the azimuth of the detection block (number i = 0, 2, 3, 5) for which the number of light receptions Cg(i) exceeds 5 is the scheduled standard. is identified as the azimuth of the point.

Next, a process for determining the coordinates of a reference point necessary for the pole position measurement process shown in step S3 in FIG. 9 will be described. First, a pole capturing swing direction determination process for determining the inclination (swing direction) of the rotation center axis 8 with a high probability of irradiating each reference point with a light beam will be explained. The outline of this pole capture swing direction determination process is as follows.

FIG. 19 shows the reference point A (n=1) detected in the reflected light reception process in the pole capture swing direction determination process.
An example of light reception data related to is shown below. The figure shows light reception data detected during one cycle of rocking, that is, when the mirror 4 rotates 30 times. In this embodiment, as will be described later, light reception data is captured while the rotation center axis 8 makes one rotation, that is, while the mirror 4 makes 30 rotations. Note that the swing direction is the amount of rotation of the encoder 35.

As shown in the flowchart of reflected light reception processing in FIG. 8, the parameter Cm indicates the rotation counter value when counting starts from the position where the swing direction is "0°" and the reference point n is detected. , and here mirror 4 is 3
Since the light reception data is collected during zero rotation, the maximum parameter Cm is "30". Parameter I is a number indicating the storage order of the rocking directions when the reference point n is detected. In other words, the maximum value of number I indicating this storage order is the reference point n
The number of times of light reception is Cp(n).

In this pole capture swing direction determination process, a group in which the reference point n is successively detected is searched for every cycle of rotational scanning of the mirror 4. For example, in FIG. 19, the rocking directions indicated by numbers I "1" and "2" and "3" to "6" are each treated as one group of data. Among such groups, the range of the swing direction in the group with the largest number of consecutive detections is detected. Then, calculate the median value of the range. Then, the obtained median value of the swing direction is determined to be the swing direction that allows the light beam to be irradiated to the reference point n with a high probability. This median value may be the average value of all rocking directions of the same group, or may be the average value of the maximum value and minimum value. In this embodiment, the average value of the maximum value and the minimum value is calculated.

For example, in FIG. 19, when the reference point n is detected, the counter values Cm are consecutive when the number I is "1" to "1".
When the number I is "2" and when it is "3" to "6", and among these, when the number I is "3" to "6", the number of consecutive times is large. Therefore, the reference point (pole) capture swing direction is determined by averaging the swing direction values for numbers I of "3" and "6". The average value is (108.6° + 144.4°)
/2=126.5°.

FIGS. 13 and 14 are flowcharts of the pole capture swing direction determination process. In FIG. 13, in step S150, it is determined whether the swing direction has reached "0°". If it is determined that the rocking direction has become "0°", the process advances to step S151. In step S151, data in a memory area that stores data obtained by the reflected light reception process is cleared. In step S152, reflected light reception processing is performed. This process is as described with respect to FIG.

[0101] In step S153, the swing direction is "0°".
” is the second time, that is, whether one cycle of rocking has been completed. If it is determined that the central axis of rotation 8 has completed one cycle of rocking in a conical shape, step The process proceeds to S154.That is, the reflected light reception processing is performed while the rotation center shaft 8 swings once in a conical manner, and the light reception data during that time is accumulated.

In step S154, the data obtained in step S111 of the reflected light reception process in FIG. 8 is read. In step S155, the reference point identification counter n is cleared. In step S156, the counter n is incremented. In step S157, the counter n
It is determined whether the parameter Cp(n) indicating the number of times the reference point (reference point n) indicated by is detected is "0" or not, and it is determined whether the reference point n has been detected. If this judgment is affirmative, it is assumed that the reference point n has not been detected and step S
The process returns to step 150 and the received light data is collected again.

If step S157 is negative, the process advances to step S158, and the parameter I is set to "1". This parameter I is a number for identifying the order in which the swing directions in which the reference point n was detected are stored, as described above.

[0104] In step S159, parameters K and e are cleared. This parameter K is the maximum value of the number of consecutive detections, that is, the maximum continuous detection when a light reception signal from the same direction is detected every time the mirror 4 rotates, that is, when a light signal from the same direction is detected continuously every time. Indicates the number of times. Further, the parameter e particularly indicates the storage number of the last oscillation direction when optical signals are continuously detected among the oscillation direction storage numbers I.

In step S160 of FIG. 14, a value J of a counter that counts the number of consecutive light reception signals is set to "1". In step S161, by determining whether the parameter Cp(n) indicating the number of times the reference point n has been detected is the same as the parameter I indicating the memorized number of the rocking direction,
It is determined whether all of the stored light reception data has been checked. Normally, there is a plurality of received light data, and since 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 of the received light data has not been checked. The process then proceeds to step S162.

In step S162, regarding the rotation counter value Cm of the mirror 4, the counter value Cm at the time of current detection is determined.
(n, I+1) is the counter value Cm(n, I
) is the value obtained by adding “1” to the value. 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 determination is affirmative, the process advances to step S163, where counter J and parameter I
Increment the value of .

On the other hand, if step S162 is negative,
Proceeding to step S164, the counter value J is the parameter K.
In other words, it is determined whether the current number of consecutive detections is greater than the previously stored number of consecutive detections. If this judgment is affirmative, the process advances to step S165, where the maximum number of continuous detections K is updated with the current number of consecutive detections J, and the parameter e indicating the memorized number of the last swing direction when detecting the continuous light reception signal is set to the current number of swings. Update with storage number I of moving direction. In step S166, the memory number I for the swing direction is incremented.

On the other hand, if step S161 is affirmative, that is, if it is determined that all received light data has been checked, the process advances to step S167. Step S16
7 and S168 are steps S164 and S1
Since this is the same process as 65, the explanation will be omitted.

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

[0110] In step S171, the maximum value max
and the minimum value min is calculated and stored as the reference point n capture swing direction Asc(n). This reference point n capturing swing direction Asc(n) is a swing direction in which the reference point n can be detected with a high probability.

[0111] In step S172, the counter value n is "
4” for all reference points depending on whether the reference point n
It is determined whether the process of determining the capture swing direction has been completed. If step S172 is negative, the process advances to step S156 in FIG.

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

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

[0114] 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 Set the azimuth θ(n) as the reference point n capture azimuth TG-ML.

In step S184, swing position control is performed to align the swing direction with the reference point n capturing swing direction TG-SW. This swing position control is performed based on the value of an encoder 35 that detects the rotation of the swing motor 15, and the value of this encoder 35 is set to the reference point n capture swing direction TG-S.
Rotate the shaft 22 until it substantially coincides with W. encoder 3
When the value of 5 and the reference point n acquisition swing direction substantially match, the motor 15 is stopped and the inclination of the rotation center axis 8 is fixed. 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 each time as a result of fixing the rotating scanning plane. If this determination is negative, it is determined that there is an abnormality such as a sensor failure, a display indicating the abnormality is displayed 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 each time and the determination in step S185 is affirmative, the process advances to step S187. In step S187, mirror position control processing is performed to adjust the reference point azimuth to the reference point n capture azimuth TG-ML. This mirror position control process is performed based on the value of the encoder 7 that detects the rotation of the mirror rotation motor 5, and the motor 5 is rotated until the value of the encoder 7 substantially matches the reference point n capture azimuth TG-ML. . Then, the value of the encoder 7 is the reference point n capture azimuth TG
- Stop the motor 5 when it substantially matches ML. Further, the electromagnet 16 is energized so that the motor 5 can remain stopped at the stopped position. As a result, the suction plate 34 is attracted to the electromagnet 16, and the stop position of the motor 5, that is, the stop position of the mirror 4 is fixed.

In step S188, the reflected light from the reference point n is detected for 3 seconds or more. Of course, this time is not limited to 3 seconds, and may be any time that allows confirmation of whether the mirror orientation is reliably fixed.

In step S189, the distance between the self-propelled vehicle 1 and the reference point n is measured based on the detection result of the reflected light from the reference point n. This is calculated based on the phase difference between the light beam emitted from the light emitter and the reflected light detected by the light receiver. In step S190, the counter value n for identifying the reference point is
It is determined whether or not is "4", that is, whether the distances from the self-propelled vehicle 1 have been measured for all reference points. If step S190 is negative, the process returns to step S182, the counter value n is incremented, and the same process is performed for other reference points. In step S191, based on the distance between each detected reference point and the self-propelled vehicle 1, and the azimuth obtained by the initial pole identification process, an arbitrary coordinate system having the self-propelled vehicle 1 as the origin, for example, a self-propelled The coordinates [X(n), Y(n)] of each of the reference points A to D on a coordinate system with the car 1 as the origin and the direction of the reference point A as the positive direction of the x-axis are calculated.

In step S192, the coordinates are expressed as coordinates [x(n), y(n)] on the coordinate system with reference point B as the origin.
] Convert to

Next, the control function for performing the above operations will be explained. First, the function of reflected light reception processing will be explained. FIG. 20 is a block diagram showing the main functions of the reflected light reception processing section. The functions of the reflected light reception processing section shown in the figure correspond to the contents of the reflected light reception processing, initial pole identification processing, and pole selection processing shown in the flowcharts of FIGS. 8, 11, and 12.

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

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

[0124] The block-by-block light reception count storage section 40 stores the light reception count for each detection block. The light reception frequency determination unit 41 determines which detection block has received the most number of light receptions, and extracts four detection blocks in descending order of the number of light receptions. Then, the azimuths representing those detection blocks are read out from the block-by-block azimuth storage section 39 and stored in the azimuth storage section 42.
Store in. At this time, storage areas Ap
Stored in s(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. This predicted azimuth may be the same value as the currently detected azimuth,
As mentioned above, a value obtained by adding the planned value may be used.

The azimuth comparing section 44 compares the predicted azimuth and the currently detected azimuth, and outputs a coincidence signal a if the two angles almost match. In response to this coincidence signal, the normally open switches SW1 and SW2 are closed, and various data are input to and stored in the data storage section 45 for each reference point. These various data are based on the azimuth Ap, the swing direction As, and the output of the sensor 33, and are the number of rotations Cm of the mirror 4 from the time of output, and the number of light receptions Cp(n) for each reference point. Here, the values Ap, As, Cm
is stored as a function of the reference point n and the number of light receptions for each reference point Cp(n).

Next, the function of the pole (reference point) position measurement processing section will be explained. FIG. 21 is a block diagram showing the main functions of the pole position measurement processing section. In the figure, the continuous light reception detection section 46 determines whether the optical signal is continuously detected every rotation of the mirror 4, based on the mirror rotation number Cm when the reflected light is received. Then, all the values of the swing direction in the continuous period when the largest number of optical signals are continuously received are read out from the reference point data storage section 45 to the swing direction range calculation section 47. The swing direction range calculating section 47 calculates the range of the swing direction based on the maximum value and minimum value of the supplied swing direction data.

[0127] The reference point-by-reference point acquisition swing direction calculating section 48 calculates the midpoint of the range of swing directions based on data indicating the range of swing directions. This midpoint value Asc(n) is supplied to the mirror direction fixing section 49. On the other hand, the azimuth angle θ(n) is supplied from the azimuth storage section 42 to the mirror fixing section 49. In the mirror direction fixing section 49, these data θ(n) and As
Using c(n) as a target value, the direction of the mirror 4, that is, the swing direction and azimuth, are adjusted to fix the mirror 4 in the direction indicated by these target values.

When the mirror 4 is fixed, the distance measuring section 50
The distance between the self-propelled vehicle 1 and each reference point is measured. The distance measurement is calculated, for example, based on the difference between the phase of the optical signal emitted from the light emitting section 51 and the phase of the optical signal detected by the light receiving section 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 azimuth θ(n).

Next, a modification of the rotation scanning device, that is, the device for rotating the mirror 4 about the rotation center axis 8 will be described. This rotary scanning device includes means for finely adjusting the stopping position of the mirror 4 after the attracting plate 34 is attracted by the electromagnet 16, and is also configured to reduce the size of the scanning device 2 as a whole.

FIG. 16 is a front sectional view of the main part of the scanning device 2, and FIG. 17 is a side sectional view of the same, and the same reference numerals as in FIG. 1 indicate the same or equivalent parts. 16 and 17, the motor 5 attached to the upper part of the base 64 has a shaft protruding from both ends thereof, and one end 5a of this shaft is attached to the pedestal 4 of the mirror 4.
A is fixed, and a balancer 65 is attached to the other end 5b so that the motor 5 can be smoothly rotated at high speed. On the upper part of the base 64, a plurality of protrusions 66 are arranged at appropriate intervals on the same circumference centered on the axis of the motor 5. A guide groove is formed. The inner circumference of a ring-shaped movable bracket 67 is fitted into this guide groove, and this movable bracket 67 can rotate around the motor 5 while being guided by the guide groove.

Electromagnet 1 fixed to movable bracket 67
The magnetic pole 16a of No. 6 is connected to the movable bracket 67 and the base 6.
It penetrates the hole provided in 4 and protrudes downward. The electromagnet 16 is fixed to the movable bracket 67 with the position adjusted so that the magnetic pole 16a maintains a predetermined gap with respect to the adsorption plate 34 attached to the upper part of the pedestal 4a.

Further, a holder member 68 is attached to the base 64, and the feed screw device 6 is connected by this holder member 68.
9 is being held. The feed screw device 69 has a knob 69
By rotating a, the spindle 69b is screw-fed to advance and retreat back and forth.

A tension coil spring 70 is provided between the base 64 and the movable bracket 67 in order to rotate the movable bracket 67 relative to the base 64 using the guide groove of the protrusion 66 as a guide. That is, the coil spring 70
One end is hooked to a rod 71 erected on the base 64, and the other end is hooked to a hole provided in the movable bracket 67 or the electromagnet 16 as appropriate.

On the other hand, the rod 7 is attached to the movable bracket 67.
2 are erected. This rod 72 is a coil spring 70
As the movable bracket 67 rotates under the action of the expansion and contraction force, it is placed in a position where it comes into contact with the end surface of the spindle 69b. Since the movable bracket 67 is pressed against the spindle 69b by the coil spring 70, the knob 6
When 9a is rotated, the spindle 69b moves back and forth depending on the direction of rotation, and the movable bracket 67 is connected to the motor 5.
It rotates by a small angle around .

[0135] Further, the suction plate 34 has a large number of slits formed at equal intervals around the entire circumference near its periphery. One of these slits extends longer toward the center of the suction plate 34 than the other slits. A pair of light transmission type sensors 73 and 74 are arranged with the peripheral edge of the suction plate 34 in between. One sensor 73 is provided to detect the plurality of slits, and the other sensor 74 is provided to detect the slit extending toward the center of the suction plate 34. These sensors 73,
The rotation angle of the mirror 4 can be detected based on the output of the mirror 74.

The base 64 is supported by a cylinder 2a made of a transparent material such as glass or acrylic resin, and the cylinder 2a is attached to the inner ring member 14 of the gimbal swing mechanism. Furthermore base 64
The cylinder 2b and its lid 75 are attached to the upper part of the cylinder 2b. Unlike the cylinder 2a, the cylinder 2b does not need to transmit light, so its material is not particularly limited.

With the above configuration, after fixing the position of the mirror 4 by energizing the electromagnet 16 and attracting the adsorption plate 34 to the magnetic pole 16a, if further fine adjustment is required, adjustment is performed by the feed screw device 69. be able to. That is, by displacing the movable bracket 67 by a small angle around the motor 5, the stopping position of the suction plate 34, that is, the mirror 4 attracted to the electromagnet 16 fixed to the movable bracket 67 can be finely adjusted.

Whether or not it is necessary to adjust the stopping position of the mirror 4 using the feed screw device 69 may be determined by, for example, a pilot lamp that is turned on and off depending on the presence or absence of a light reception signal.

[0139] In this embodiment, the feed screw device 69 is
Although an easy-to-understand configuration is adopted in which the step motor is operated manually, it is of course possible to automatically adjust the drive control of the step motor in response to an instruction signal for the pilot lamp.

[0140]

Effects of the Invention As is clear from the above description, according to the present invention, the light beam is rotated and scanned in the horizontal direction, and the light beam is scanned up and down, so that the reference point distant from the observation point can be fixed. Even when setting the projection direction of the light beam to ensure that the light beam crosses the vertically installed light reflecting means, the position information of the light reflecting means (reference point) must be accurately determined before starting work. Can be easily and easily measured. As a result, even if the installation location of the position detection device is not flat, just by appropriately placing the light reflecting means that serves as a reference point around the observation point, the position can be accurately surveyed in advance and the results can be transmitted to the control device. This saves you the trouble of manually entering information.

[Brief explanation of the drawing]

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

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

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

FIG. 4 is a diagram showing the relationship between light trails and light reflectors.

FIG. 5 is a diagram explaining the principle of self-propelled vehicle position calculation.

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

FIG. 7 is a diagram showing the driving course of a self-propelled vehicle and the arrangement of reflectors.

FIG. 8 is a flowchart of reflected light reception processing.

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 initial pole identification processing.

FIG. 12 is a flowchart of pole selection processing.

FIG. 13 is a flowchart of pole capture swing direction determination processing.

FIG. 14 is a flowchart of pole capture swing direction determination processing.

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

FIG. 16 is a front cross-sectional view of main parts of the light beam scanning device.

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

FIG. 18 is a diagram showing the 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 mirror rotations and the rocking direction when a reference point is detected.

FIG. 20 is a block diagram showing main functions of reflected light reception processing.

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

FIG. 22 is a diagram showing the relationship between a light beam track and a reflector according to the prior art.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1...Self-propelled vehicle, 2...Light beam scanning device, 4...Rotating mirror, 5...Mirror drive motor, 6, 6a-6
d...Light reflector, 7...Encoder, 8...Rotation center axis, 9, 10...Bracket, 11...Outer ring member, 14...Inner ring member, 15...Swing motor,
16... Electromagnet, 19... Connecting metal fittings, 23... Large disc, 24... Small disc, 26, 31... Connecting bolt,
33... Swing reference detection sensor, 34... Adsorption plate, 35
... Encoder, 36... Driving course, 37... Azimuth detection section, 38... Rocking direction detection section, 39... Azimuth angle storage section for each block, 40... Light reception number storage section for each block, 41... Light reception number determination section, 42... Azimuth storage section, 43... Predicted azimuth storage section, 44... Azimuth comparison section, 45... Data storage section by reference point, 46... Continuous light reception detection section, 47... Swinging direction range calculation section, 48... By reference point Capture swing direction calculation section, 49...Mirror direction fixing section, 50...Distance measurement section, 51...Light emitting section, 52...
Light receiving section, 64...Base, 67...Movable bracket, 69...Feed screw device, 70...Tension coil spring, 73, 74...Light transmission type sensor

Claims (5)

[Claims] 1. A light beam generating means and a light receiving means for receiving the light beam reflected by a light reflecting means installed at a position distant from the light beam generating means are arranged at an observation point, and the light receiving means In a position detection device that detects the position of an observation point with respect to the light reflection means based on a received light reception signal, the light beam generated by the light beam generation means is rotated and scanned in a circumferential direction while swinging in the vertical direction. a scanning means, a means for detecting an azimuth of the incident light when the optical signal is received by the light receiving means, and a means for storing substantially the same azimuth among the detected azimuths as one azimuth data. , a reference point recognition means for recognizing that the azimuth angle data has become the same number as the number of the light reflecting means installed in advance; and a reference point recognition means for recognizing that the azimuth angle data has become the same number as the number of the light reflecting means installed in advance. means for stopping rotational scanning of the light beam and fixing the projection direction of the light beam in that direction when a light reception signal from a direction indicated by one of the azimuth angle data is detected; a means for receiving the reflected light of this light beam while fixing the projection direction of the light beam, and measuring the distance between the observation point and the light reflecting means based on the received light signal; and a means for releasing the fixation of the projection direction of the light reflecting means, and calculating the position of the observation point with respect to the light reflecting means based on the measured distance and azimuth data at this time by repeating the distance measurement for the number of the light reflecting means. A position detection device characterized by comprising means. 2. A common table for mounting the light beam generating means and the light receiving means, the table being swung so that the central axis of rotation of the light beam rotation scanning means draws a conical locus, and 2. The position detection device according to claim 1, wherein the light beam is rotated and scanned a plurality of times during this unit swing cycle to cause the light beam to swing in the vertical direction. Device. 3. Means for detecting a rocking angle and an azimuth of the table relative to the incident light detected by the light receiving means, the rocking of the table is stopped at the rocking angle, and rotation scanning is performed in this stopped state. and when an optical signal is detected again at the detected azimuth in this rotational scanning state, the rotational scanning is stopped, the light beam projection direction is fixed, and the distance measuring operation is performed. The position detection device according to claim 2. 4. The means for fixing the projection direction of the light beam includes a magnetic disk that rotates together with the light beam rotation and scanning means, and an electromagnet that attracts the disk. 4. The position detection device according to any one of 3 to 3. 5. The position detection device according to claim 4, further comprising means for displacing the electromagnet by a minute angle about the rotation scanning means while the disk is attracted.