JPH0545166A - Position detector - Google Patents

Abstract

PURPOSE:To detect the position of a reflector and make the distance measurement for determining the position of the reflector on a coordinate easy by using a light beam emitted from a measuring point and reflected by the reflector even in the case the placed positions of the measurement and the reflector disagree. CONSTITUTION:A scanning device 2 scans a light beam 2E circumferentially around an axis 8. The axis 8 of the scanning device 2 swings at jimbal in the state inclined for a predetermined angle from the vertical. As the result, the light beam 2E is scanned on the locus combined with rotation and swing up and down. The distance between the scanning device 2 and the reflector 6 is obtained from the maximum and minimum inclination angles Asmax and Asmin for irradiating the light beam to the top and bottom of the reflector 6 and the hight of the reflector. Based on the distance and the azimuth of the reflector from a predetermined direction, the position of the reflector 6 on the coordinate is obtained. This position on the coordinate is used for detection and control of the position of the reflector 2 (=measuring point).

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 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 position detecting device for a moving body, a means for scanning a light beam in a circumferential direction around the moving body, and a fixed position at at least three reference positions distant from the moving body are incident. An apparatus has been proposed which includes a light reflecting means for reflecting light in a direction and a light receiving means for receiving the light reflected by the light reflecting means (Japanese Patent Laid-Open No. 59-67).
476).

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 position information of the reference point to which each light reflecting means is fixed.

In the above apparatus, even a slight error in the position information of the light reflecting means, which is 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 input work 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,
Even if the light beam is projected toward the azimuth where the light reflecting means is supposed to exist due to the relationship between the period of the vertical swinging scan and the horizontal rotating scan and the setting of the vertical scan width, the vertical angle There is a case where the light reflecting means cannot be irradiated with the light beam due to the deviation of. For this reason, there is a problem in that it takes a lot of time and effort to detect the position of the reference point, which is performed especially before starting the operation.

The object of the present invention is to solve the above problems,
It is an object of the present invention to provide a position detection device capable of easily detecting the position of a reference point, which is performed at the preparation stage for starting operation.

[0009]

[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 rotationally scanning a light beam in a circumferential direction around an observation point, and the light reflected by the light reflection means installed at a position distant from the observation point. By rotating the light receiving means for receiving the light beam and the rotation center axis of the rotation scanning means so as to draw a substantially conical locus, the light beam is rotated once every plural times of the rotation scanning. An oscillating scanning means configured to oscillate up and down around an observation point, an azimuth angle in a rotational scanning direction when an optical signal is detected by the light receiving means, and the rotational center by the oscillating scanning means. A storage means for storing the tilt direction angle of the shaft; and a means for detecting the maximum value and the minimum value of the tilt direction angle when a plurality of optical signals are detected in substantially the same rotational scanning direction,
Characterized in that the distance between the observation point and the light reflecting means is calculated based on the maximum value and the minimum value of the detected tilt direction angle and the known height dimension of the light reflecting means corresponding to this azimuth. There is.

[0010]

In the present invention having the above characteristics, the rotary scanning is performed a plurality of times during one cycle of swinging the light beam in the vertical direction so that the light reflecting means can be easily irradiated with the light beam. By doing so, a mesh-like light trace by the light beam is drawn on the virtual cylindrical surface centered on the axis standing perpendicular to the observation point. That is, during one cycle of rocking, rotational scanning is performed several times or more with different vertical heights.

As a result, the light beam crosses the light reflecting means set up perpendicularly to the reference point with a high probability, and the light beam reflected by the light reflecting means is more likely to be received by the light receiving means. That is, it becomes easy to receive light.

Further, in calculating the position of the reference point (= calculating the position of the light reflecting means) before starting the operation,
When a plurality of optical signals are detected in substantially the same rotational scanning direction, it is determined that the optical signals are reflected light from the scheduled light reflecting means. Then, it is recognized that the upper and lower ends of the light reflecting means are irradiated with the light beam by the maximum value and the minimum value of the tilt direction angle when the reflected light is detected, and this tilt direction angle and the known light reflection The distance between the observation point and the light reflecting means is calculated based on the height dimension of the means.

[0013]

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. A well-known light reflecting means such as a corner cube prism is used for the reflecting surface. Self-propelled vehicle 1
Is a lawn mower having a cutter blade for lawn mowing work (not shown) on its lower surface, for example. At the top of self-propelled vehicle 1,
A light beam scanning device (hereinafter, simply referred to as a scanning device) 2 is mounted. The scanning device 2 has a light emitter for generating a light beam 2E and a light receiver for receiving the reflected light 2R of the light beam 2E reflected by the reflectors 6a to 6d. The light emitter and the light receiver are housed in the casing 3. As the light emitter and the light receiver, for example, a light emitting diode and a phototransistor can be used, respectively, and the casing 3 for accommodating them is fixed to the inner ring member 14.

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 a scanning device 2 to the outside. The mirror 4 is rotated about the central axis of rotation 8 by the motor 5 by the arrow 1
The light beam 2E is rotated in the direction of arrow 7 by the rotation of the mirror 4 by the rotation of the mirror 4. The rotation position of the mirror 4, that is, the projection direction of the light beam 2E is detected by the encoder 7 detecting the rotation angle of the motor 5.

The scanning device 2 uses a swing mechanism to drive the vehicle 1.
Supported by. This swing mechanism is known as a gimbal. This swinging mechanism is used for the outer ring member 11
And an inner ring member 14 provided inside the outer ring member 11. The outer ring member 11 is swingably supported on the brackets 9 and 10, and the inner ring member 14 is swingably supported on the outer ring member 11.

That is, the outer ring member 11 is supported by the brackets 9 and 10 by the shaft 12 and a shaft (not shown) provided at a position facing the shaft 12, and the inner ring member 14 is provided at a position facing the shaft 13. It is supported by the outer ring member 11 by a shaft (not shown). The swing center axes of the outer ring member 11 and the inner ring member 14 are provided so as to be orthogonal to each other.

The gimbal swing mechanism is driven by a swing driving motor 15. The scanning device 2 is mounted on the gimbal swing mechanism with the rotation center axis 8 of the mirror 4 tilted from the vertical by an angle φ, and the tilt direction (tilt direction angle) of the scanning device 2 is set by the motor 15 by the gimbal swing mechanism. It changes continuously as it is driven. That is, the scanning device 2 rotates in the direction of the arrow 17a. In this way, when the light beam 2E is rotationally scanned while rotating the central axis of rotation 8 conically, the inclination of the rotational scanning surface drawn by the light trace continuously changes. As a result, the light beam 2E is rotatively scanned about the rotation center axis 8 by the mirror, and is also oscillated and scanned in the vertical direction continuously due to the continuous change of the inclination of the rotary scanning surface.

The drive mechanism for swinging the outer ring member 11 and the inner ring member 14 around the swing central axes orthogonal to each other is as follows. First, a crank formed by an eccentric shaft whose eccentric direction is deviated by 90 degrees is connected to the motor 15. Then, the rotational movements of the respective eccentric shafts are converted into reciprocating movements by the rod 26 and a rod (not shown), and are transmitted to the outer ring member 11 and the inner ring member 14, respectively. By appropriately setting the amount of eccentricity of the eccentric shaft, the swing widths of the outer ring member 11 and the inner ring member 14 are selected, and the swing locus of the scanning device 2 fixed to the inner ring member 14, that is, the scanning device 2. The swing locus of the included rotation center axis 8 is determined.

For example, by setting the amount of eccentricity of the eccentric shaft,
If the maximum swing widths of the outer ring member 11 and the inner ring member 14 are made different, the swing locus of the rotation center axis 8 becomes an elliptical cone.

In this embodiment, the eccentricity amounts of the eccentric shafts are set so that the swing locus becomes substantially conical, that is, the maximum swing widths of the outer ring member 11 and the inner ring member 14 are the same. However, the present invention is not limited to this.

The motor 15 is also provided with an encoder 15a for detecting the rotational position of the motor 15. The tilt direction angle of the rotation center axis 8 of the mirror 4 is detected based on the output signal of the encoder 15a.

Although the outer ring member 11 and the inner ring member 14 are driven by one motor 15 in this embodiment, the ring members 11 and 14 may be driven by separate motors. In that case, it goes without saying that the respective motors rotate in synchronization so that the rotation center shaft 8 draws a desired conical shape. The scanning device 2 including the above drive mechanism is disclosed in Japanese Patent Application No.
No. 126511, which is described in more detail.

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 draws a cone to rotate, and the light trace due to the rotation of the mirror 4 is performed. 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, which 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 rotation center axis 8 is rotated at 90 rpm while drawing a cone.

Next, the light trace of the light beam by the scanning device of the present embodiment will be described with reference to FIG. This figure shows a model of light traces 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 draws a mesh-like light trace on the virtual cylindrical surface due to the conical movement of the rotation center axis 8 of the mirror 4. .. In this embodiment, the rotation speed of the mirror 4 is set to 2700.
Since the rpm and the rotation speed of the rotation center shaft 8 are 90 rpm, the mirror 4 itself makes 30 rotations while the rotation center shaft 8 makes one conical rotation. That is, while the rotation center axis 8 draws a cone and makes one rotation, the arbitrary vertical line 18 on the cylindrical surface is moved three times.
Zero light trails cross.

Next, it will be described with reference to the drawings how much a light beam is easily irradiated to the reflector in one oscillation cycle when the reflector is arranged on the vertical line 18.
FIG. 4 is an enlarged view of a part of the light trace. In the figure, as indicated by reference numeral 6H, when the self-propelled vehicle 1 and the reflector 6 are close to each other 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 the light trails of the book cross this reflector 6. On the other hand, as indicated by reference numeral 6L, when the distance between the vehicle 1 and the reflector 6 is very long, the size of the reflector 6 in the height direction is relative to the swing width BB of the light trace. It becomes relatively short. However, even if the dimension 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 heightwise dimension of the reflector 6, the light traces on the reflector 6 at least once while the central axis of rotation 8 makes one conical movement. Cross. In FIGS. 3 and 4, the number of light traces is smaller than the actual number in order to avoid complexity and to facilitate understanding of the drawings.

Next, an example of the steering control of the self-propelled vehicle 1 equipped with the scanning device 2 having the above configuration will be described. FIG. 5 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 travels along a traveling course 36 set in an area defined by the reference points A to D. And perform planned work such as lawn mowing. Prior to the work, the coordinate positions of the reference points A to D are first detected and input to the control device of the self-propelled vehicle 1.

The self-propelled vehicle 1 scans the light beam with the scanning device 2 and detects the incident angle (azimuth angle) of the reflected light of the light beam reflected by the reflectors at the reference points A to D. The self-propelled vehicle 1 calculates its own position based on the azimuth angle and the position information (coordinates) of the reference points A to D. Then, the traveling course 36 is compared with the self-position, and the steering angle is controlled so as to eliminate the difference, and the vehicle travels.

First, it is assumed that 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 the straight travel distance, the self-propelled vehicle 1 travels the turning stroke with the steering angle fixed at a constant value at the position where the y coordinate reaches Ytn or Ytf, and shifts to the next 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. 5, for simplification of explanation, the reference points A, B, C and D are arranged so as to be located at the respective vertices of the rectangle, and the straight travel distance is set to the reference points A and B. Although the straight line connecting the lines with each other, 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.

The principle and calculation formula for obtaining the position of the self-propelled vehicle 1 in the above steering control are described in JP-A-1-287415 and JP-A-1-316 of the applicant's earlier application.
Since it is described in detail in Japanese Patent No. 808, the description thereof will be omitted.

Next, steering control for causing the vehicle 1 to travel as described above will be described with reference to a flowchart. First, the reflected light receiving process will be described with reference to FIG. In this reflected light receiving process, the rotation angle of the mirror 4 at the time of receiving light, that is, the incident direction of the light, the inclination direction angle of the rotation center axis 8 of the mirror 4, and the like are detected and stored.

In FIG. 6, in step S100, the presence or absence of a light reception signal from the light receiver is determined. If it is determined that an optical signal is detected, the process proceeds to step S101. In step S101, it is determined based on the detection interval of the received light signal whether or not the received light is detected by chattering. That is, when a plurality of optical signals are detected while the mirror 4 is only rotating by a very small angle, it is determined that chattering occurs, and the signal detected later is ignored. If not chattering, the process proceeds to step S102.

In step S102, "0" is set to the variable i indicating the detected block number. The detection block is defined as follows. In this embodiment, the mirror 4 is rotated 30 times around the rotation center axis 8 while the rotation center axis 8 of the mirror 4 draws a conical locus and makes one rotation, and the light beam is rotated and scanned 30 times. There is. There is a possibility that reflected light from the same reflector may be received many times by the 30 times of rotational scanning. Therefore, the detection data relating to a plurality of optical signals incident from substantially the same direction are collectively stored as one group as reflected light from the same reflector. Here, this group is called a detection block. If only the reflected light from the planned reflectors 6a to 6d is detected, there are four detection blocks, which corresponds to the number of installed reflectors 6a to 6d.

In step S103, it is determined whether 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, 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 detection block "0" is first detected. It is determined whether or not the received optical signal.

In the first processing, this judgment is affirmative,
The process proceeds from step S103 to step S106, and the mirror angle, that is, the azimuth angle is stored. The azimuth angle detected this time is stored as the azimuth angle Am (i) representing the detection block i, and the value of the number of times of light reception Cg (i) in the detection block i is incremented. The azimuth angle is a value based on the traveling direction of the vehicle 1.

In step S107, the value n of the counter for identifying the reference point is cleared. In the present embodiment, the counter values "1" to "4" correspond to the reference points A to D, respectively. In step S108, 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). That is, since the counter value n is set to "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" matches the detected azimuth angle. It

The predicted azimuth angle θq (n) may be, for example, a value obtained by adding the predicted change amount α to the azimuth angle at this time detection,
Considering that the light receiving interval of the reflected light is short with respect to the moving amount of the self-propelled vehicle 1, even if the same value as the detected value this time 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". Until this determination becomes affirmative, step S108,
S109 is repeated, and it is determined whether or not the predicted azimuths of all the reference points A to D substantially match the detected azimuths.

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 n indicated by the counter value n is incremented. Further, the detected azimuth angle Ap [n, Cp (n)] of the reference point n, the mirror 4
Angle of the rotation center axis 8 of the
[N, Cp (n)] and the rotation counter value Cm [n, Cp (n)] of the mirror 4 are stored. This rotation counter value indicates the number of rotations of the mirror 4 counted from when the swing direction of the rotation center shaft 8 is in the predetermined reference direction. In addition,
The swing direction As [n, Cp (n)] is a value based on the traveling direction of the self-propelled vehicle 1 as with the azimuth angle, and is different from the later-described tilt direction angles Asmax and Asmin.

On the other hand, if it is determined in step S103 that the light reception in the detection block i is not the first time, the process proceeds to step S104, and the azimuth Am (i) of the previous light reception signal in the detection block i and the current light reception signal are detected. It is determined whether or not the azimuths of the two coincide with each other. If they are substantially the same, the process proceeds to step S106, and the azimuth angle Am (i) of the detection block i is updated with the current detected azimuth angle.

On the other hand, if the azimuth angle Am (i) of the previous received light signal in the detection block i and the azimuth angle of the received light signal of this time do not match, step S104 becomes negative,
In step S105, the detected block number (i) is incremented. Thereafter, the process returns to step S103, and it is determined whether or not the light reception detected this time is the first light reception for the incremented detection block (i).

Next, steering control using the data obtained by the reflected light receiving process will be described. Figure 7,
FIG. 8 is a general flowchart of steering control.

In FIG. 7, in step S1, the motor 5 is activated to rotate the mirror 4 about the rotation center axis 8 for rotational scanning, and the motor 15 is activated to move the rotation center axis 8 into a conical shape. Draw the locus of and rotate. 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 angle of the reflectors 6a to 6d is performed. In the initial pole identification processing, the received light data stored in the reflected light reception processing is analyzed while the rotation center axis 8 swings once, and the reference point identification processing is performed.

That is, the number of detection blocks during one rotation of the rotation center axis 8 is detected, and it is determined whether or not the number matches the number of planned reference points (= number of reflectors). If they match each other, it is determined that the received light signal is the detection signal of the reflected light from the reflector arranged at the predetermined reference point. On the other hand, if the two do not match, it is determined whether the block with a large number of times of light reception is a detection signal of light from the reference point or not, depending on whether or not the number of times of light reception in each detection block is larger than a predetermined threshold value. Whether to perform reflected light reception processing. This initial pole identification processing is described in more detail in Japanese Patent Application No. 3-126511 filed by the applicant earlier.

In step S3, the reference point A is calculated from the self-propelled vehicle 1.
Position of each reference point by measuring each distance from to D (coordinate value)
A pole position measurement process for calculating is performed. Details of this processing will be described later with reference to FIGS. 9 and 10.

In step S4, based on the azimuth angle of the reference point calculated in step S2 and the coordinate value calculated in step S3, the current position coordinates (Xp, Y) of the vehicle 1 are calculated.
p) is calculated. In step S5, the traveling course of the self-propelled vehicle 1 is set. That is, the current x coordinate X of the self-propelled vehicle 1
Set p as the x coordinate Xref of the first straight travel. The setting operation of setting the present position as Xref is performed when the vehicle 1 is at the traveling start position.

In step S6, the motors 5 and 15 are rotated at a high speed at a high 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 (FIG. 8), forward traveling straight traveling processing is performed in which the self-propelled vehicle 1 travels in a straight traveling direction in a direction in which its y coordinate value increases. In this process, the self-position (Xp, Yp) is calculated based on the azimuth angle obtained by the reflected light receiving process described later.
And the traveling direction θf is 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 flowchart showing the detailed operation is omitted.

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

In step S11, the azimuth angle at which the traveling of the turning stroke following the straight travel distance is ended, that is, the right turn release angle is set. In step S12, a U-turn process of fixing the steering angle of the self-propelled vehicle 1 to a predetermined value and making a right turn with a constant turning radius is performed. Since the processes of steps S11 and S12 are not directly related to the present invention, the flowchart showing the detailed operation is omitted.

In step S13, the release counter value (counted during U-turn processing) for counting the number of reference points at which the azimuth angle viewed from the vehicle 1 reaches the right turn release angle exceeds "1". Judge whether or not. If this judgment is affirmative, it is judged that the traveling of the turning stroke has ended, and the routine proceeds to step S14. In step S14, the self-propelled vehicle 1 is set to y
A return straight travel process is performed in which the travel travels straight in the direction in which the coordinate values decrease.

In step S15, it is determined whether or not the second straight travel has been completed depending on whether or not the y-coordinate of the vehicle 1 is smaller than the planned y-coordinate Ytn. 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 S17
Go to and set the next straight travel distance.

In step S18, the left turn release angle for ending the left turn is set. 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, and in step S20, the value of the release counter is "1".
Judgment is made. If this determination is affirmative, it is determined that the traveling with a positive turn is completed, and step S8 is performed.
Return to.

If the determination in step S16 is affirmative, it means that all straight travels have been completed, and the program proceeds to step S21 to set the release angle in the final turning stroke. The process of step S21 is similar to the right turn release angle set and the left turn release angle set.

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, processing for traveling in the straight travel distance for returning 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 or not p has become smaller than the x coordinate Xhome of the home position 63, and if this determination is affirmative, it can be determined that the vehicle 1 has returned to the home position 63, so all processing is terminated.

Next, the pole position measuring process in the steering control will be described. In the pole position measuring process, first, the tilt direction angle of the rotation center axis 8 that can detect the reference point is obtained. The tilt direction angle here is represented by an angle based on the azimuth angle of the reference point. On the other hand, the swinging direction is the tilting direction of the rotation center shaft 8 which is expressed with reference to the traveling direction of the self-propelled vehicle 1. Then, if the tilt direction angle is detected, the maximum value and the minimum value thereof are obtained.

The maximum value and the minimum value of the tilt direction angle are defined as shown in FIG. In addition, in FIG.
The same symbols as are used to indicate the same or equivalent parts. In FIG.
Reference symbol Z indicates the tilt direction of the rotation center axis 8 at that time. The tilt direction when the light beam 2E can be irradiated near the upper end of the reflector 6 is defined as the maximum tilt direction angle Asmax, and the tilt direction when the light beam 2E can be irradiated near the lower end of the reflector 6 is the minimum. It is defined as the tilt direction angle Asmin.

When the maximum value and the minimum value of the tilt direction angle are obtained, the distance between the observation point and each reference point is calculated based on the values, and the reference is calculated based on the distance data and the detected azimuth angle. The point position is determined.

The pole position measuring process will be described with reference to the flowcharts of FIGS. 9, 10 and 11. In FIG. 9, in step S150, an inclination direction angle determination process is performed. Here, the maximum tilt direction angle Asmax and the minimum tilt direction angle As that can receive the reflected light from the reference point.
Find min. This inclination direction angle is the relative inclination direction of the rotation center axis 8 based on the azimuth angle of the reference point as described above. Details of this process will be described later with reference to FIG.

In step S151, the counter value n for specifying the reference point is cleared. In step S152, the counter value n is incremented. Step S15
At 3, the distance r (n) from the observation point, that is, the self-propelled vehicle 1 to the reference point n is calculated. The formula for this calculation is described below with respect to FIG. In step S154, it is determined whether or not the counter value n is "4", that is, whether or not the number of reference points matches the planned number of reference points, and the distances from the self-propelled vehicle 1 are measured for all reference points. Determine whether or not

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

Next, the details of the inclination direction angle determination processing will be described with reference to FIGS. In the figure, in step S160, the motor 5 is rotated to rotate the mirror 4 around the rotation center axis 8, and the rotation center axis 8 is rotated in a conical locus. here,
The motor 15 is rotated at an extremely low speed so that the upper and lower ends of the reflectors 6a to 6d can be reliably irradiated with the light beam. The ultra-low speed rotation here means that the rotation speed is lower than the rotation speed in step S1 (FIG. 7).

In step S161, the counter value n for identifying the reference points A to D is cleared. In step S162, the counter value n is incremented.

In step S163, it is determined whether or not the swing direction of the rotation center axis 8 is equal to the azimuth angle θ (n) of the reference point n. That is, it is determined whether or not the rotation center axis 8 is inclined in the direction of the reference point n. If this determination is positive, the process proceeds to step S164, and the data in the reflected light receiving process is cleared.

In step S165, "0" is set to the parameter Cr indicating the counter value for counting the number of reflecting objects by the received light signal, that is, the number of detection blocks. In step S166, the swing direction is the azimuth θ (n) + 180 °
It is determined whether or not the rotation of the rotation center shaft 8 along a conical locus has been performed for half a cycle.

Until the rotation is performed for half a cycle, the process proceeds to step S167 (FIG. 11). In step S167, reflected light reception processing is performed. When the reflected light receiving process is completed, the process proceeds to step S168, and it is determined whether the mirror 4 has made one rotation. If it is determined that the mirror 4 has rotated once, the process proceeds to step S169 to read the data obtained by the reflected light receiving process during one rotation of the mirror 4.

In step S170, it is determined whether or not the reference point n is detected based on the number of times Cp (n) of light reception at the reference point n in the reflected light receiving process. When it is determined that the reference point n is detected, the process proceeds to step S171, and the tilt direction angle at the time of detecting the reference point n, that is, the swing direction based on the azimuth of the reference point n [As [n, Cp (n)]. −θ
(N)} is set to the variable a.

In step S173, it is determined whether or not this reference point n is continuously detected following the previous processing. If a discontinuous detection signal is obtained, the process proceeds to step S174, the counter value Cr is incremented, and the process proceeds to step S175, where the values of the variables are the maximum angle max (Cr) and the minimum angle min (Cr). Remember.

If the reference point n is continuously detected, the process proceeds to step S176, and the tilt direction angle detected this time, that is, the value of the variable a, is the maximum angle max stored in the processing up to the previous time. It is determined whether it is larger than (Cr). When this determination is affirmative, the process proceeds to step S177, and the maximum angle max (Cr) is updated with the value a.

Further, in step S178, the value of the variable a is set to the minimum angle min stored in the previous processing.
It is determined whether it is larger than (Cr). If this determination is positive, the process proceeds to step S179 and the minimum angle min
(Cr) is updated.

When the rotational scanning direction of the light beam is limited to the counterclockwise direction as in this embodiment, the light beam moves from the lowermost part to the uppermost part with respect to the light reflector at the reference point. Since the position changes, the first detection becomes the minimum angle, and the maximum angle is updated as long as it is continuously detected. Therefore, as indicated by the broken line in FIG. 11, when the determination in step S173 is affirmative, step S17
It is also possible to move to 7 '.

When the rotation center shaft 8 draws a conical locus for half a cycle, and if step S166 is affirmative, the process proceeds to step S180 (FIG. 10) and the counter value Cr is "1". Determine whether or not.

When the counter value Cr is "1", it is determined that the light from the reflecting object other than the reference point n is not received, and the process proceeds to step S181. In step S181, the maximum angle max (Cr) is set to the maximum inclination direction angle Asmax.
(N), and the minimum angle min (Cr) is stored as the minimum tilt direction angle Asmin (n).

When light from a reflecting object other than the reference point n is received, the determination at step S180 is negative and the process proceeds to step S182. In step S182, the difference between the maximum angle max (Cr) and the minimum angle min (Cr) is calculated, and the counter value Cr that maximizes the difference is detected. Then, in step S183, the maximum angle (Cr) and the minimum angle (Cr) corresponding to the calculated counter value Cr are respectively set to the maximum inclination direction angle Asmax.
(N) and the minimum inclination direction angle Asmin (n) are stored.

In step S184, it is determined whether the counter value n is "4". When this determination is affirmative, that is, with respect to all the reference points, the maximum tilt direction angle Asmax (n) and the minimum tilt direction angle Asmin that can irradiate the light beam to the reflector arranged at the reference point n.
When (n) has been stored, this inclination direction angle determination processing ends.

Next, the calculation formula in step S153 will be described. In FIG. 12, it is assumed that a circle O (radius r) on the horizontal plane and an inclined surface f intersecting the circle O at a predetermined angle φ. Let Q be a point where a vertical line standing at a point P on the circumference of the circle O intersects the inclined surface f, and pass through the center OC of the circle O,
Let E be a point where a straight line drawn perpendicular to the intersection line of the circle O and the inclined surface f intersects the circumference of the circle O. Further, a point where a straight line drawn from the point E perpendicular to the plane of the circle O and the inclined surface f intersect is defined as F, and a foot of a perpendicular line dropped from the point P to the intersection line of the circle O and the inclined surface f. Be G. Furthermore, the center O of the circle O
The opening angle of the points E and P viewed from C is θk.

The above-mentioned settings are as follows in correspondence with the position detecting device of the present embodiment. Point OC ... Position of the self-propelled vehicle 1, Angle φ ... Inclination of the rotation center axis 8, Inclined surface f ... Scanning surface of light beam that is rotationally scanned about the self-propelled vehicle 1,
P ... Reference point where the reflector 6 is installed, Q ... Intersection of the reflector 6 installed at the reference point P and the light beam, direction of the point E viewed from the center OC ... Swing direction Z, θk ... Inclination direction angle,
r: Distance from the self-propelled vehicle 1 to the reference point P.

The length of the straight line PQ, that is, the height from the point P to the point Q on the reflector 6 irradiated with the light beam can be calculated by the following equation. PG = r × sin (θk−90 °) = − r × cos θk (1) PQ = PG × tan φ (2) From equations (1) and (2), PQ = −r × cos θk × tan φ (3) ) Further, substituting the maximum inclination direction angle Asmax and the minimum inclination direction angle Asmin into the angle θk, the height of the lower end of the reflector 6 = -r x cos (Asmin) x tanφ ... (4) The upper end of the reflector 6 Height = −r × cos (Asmax) × tan φ ... (5) From equations (4) and (5), the height Hp of the reflector 6 is expressed by the following equation. Hp = −r × cos (Asmax) × tanφ − {− r × cos (Asmin) × tanφ} (6) Since the height of the reflector 6 is determined in advance, the self-propelled vehicle 1 and the reference point n are set. The distance r (n) of is expressed by the following equation.

R (n) = Hp / {tanφ × {cos (Asmin (n))-cos (Asmax (n))}} (7) Next, the control function of the present embodiment will be described. First, the function of the reflected light receiving process will be described. FIG. 13 is a block diagram showing the main functions of the reflected light receiving process.

In the figure, the received light signal is the azimuth angle detection unit 3.
7 and the swing direction detector 38. The azimuth angle detecting unit 37 has a counter that counts the number of pulse signals input from the encoder 7, and the incidence of the optical signal viewed from the vehicle 1 based on the pulse count value when the optical signal is input. Azimuth (= 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 each 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. 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 a when the both angles are substantially coincident with each other. In response to the coincidence signal a, 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, 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 reception times Cp (n) for each reference point.

Next, the main function of the reference point position measuring process will be described with reference to the block diagram of FIG. In the figure,
In the reference point detection determination unit 16, each time the mirror 4 makes one rotation, the number of received light data Cp from the reference point data storage unit 45 is received.
(N) is taken in and it is determined whether or not the reference point n is detected based on the received light count data Cp (n).

When the reference point n is detected, the reference point-specific data storage unit 45 responds to the detection signal supplied from the reference point detection determination unit 16 and causes the tilt direction angle calculation unit 19 to transmit the swing direction data As [ n, Cp (n)] and azimuth angle data Ap
And outputs [n, Cp (n)]. The tilt direction angle calculation unit 19 calculates the tilt direction angle based on these data, and outputs the calculation result to the buffer 20.

The reference point continuous detection determination unit 21 determines whether or not the reference point n is detected even during the previous one rotation of the mirror 4. For this determination, the number of rotations Cm [n, n of the mirror 4 when the reference point n is detected by the reference point detection determination unit 16
Cp (n)] is stored. The number of rotations Cm of the mirror 4 at the time of detecting the reference point n at this time and the previous time Cm
It is determined whether [n, Cp (n)] are continuous,
It is determined whether or not the reference point n is continuously detected.

When it is determined that the reference point n is continuously detected, the maximum angle max (Cr) and the minimum angle min (Cr) stored in the maximum / minimum angle storage unit 22 are read out to the comparison unit 23, and the buffer 20 is read. The magnitude is compared with the tilt direction angle stored in. When the tilt direction angle in the buffer 20 is larger than the maximum angle, and the buffer 2
When the tilt direction angle within 0 is smaller than the minimum angle, the maximum angle and the minimum angle of the maximum / minimum angle storage unit 22 are updated with the tilt direction angle within the buffer 20, respectively.

On the other hand, when the reference point n is not continuously detected, the value of the counter 24 is incremented and the tilt direction angle of the buffer 20 is stored in the maximum / minimum angle storage unit 22 in association with the counter value. ..

In the reference point identifying section 25, the value of the counter 24 is "1" depending on whether the value of the counter 24 is "1" or more.
If it is above, the reference point identification processing is performed. If the value of the counter 24 is "1" or more, the maximum angle and the minimum angle with respect to the reference point n are identified from the maximum angle and the minimum angle stored in the maximum / minimum angle storage unit 22, and the maximum angle is identified. Angle and minimum angle to maximum tilt direction angle Asmax
(N) and the minimum inclination direction angle Asmin (n) are stored in the inclination direction angle storage unit 26.

If the value of the counter 24 is "1", there is one maximum angle and one minimum angle stored in the maximum / minimum angle storage unit 22, and the angle value is used as it is for the maximum inclination direction angle. The tilt direction angle storage unit 26 stores Asmax (n) and the minimum tilt direction angle Asmin (n).

In the distance calculation unit 27, the maximum tilt direction angle A
smax (n) and minimum tilt direction angle Asmin
Using (n), the distance from the vehicle 1 to the reference point n is calculated according to the calculation formula (7).

The reference point position calculation unit 28 calculates the distance and the azimuth angle θ supplied from the azimuth angle storage unit 42.
Based on (n) and the position coordinates of the reference point [X (n), Y
(N)] is calculated.

In the flowchart shown in FIG. 10, the maximum angle max (Cr) and the minimum angle min (Cr)
The data that maximizes the difference between and is determined to be correct data for the reference point n (steps S182 and S18).
3) shows the maximum angle max (Cr) and the minimum angle min (Cr) corresponding to the counter value Cr when the number of detection signals of the reference point n is largest, respectively, as the maximum inclination direction angle Asm.
It may be ax (n) and the minimum inclination direction angle Asmin (n). This is based on the assumption that it is extremely unlikely that a reflecting object larger than this reflector is present at a position closer to the vehicle 1 than the planned reflector.

[0099]

As is apparent from the above description, according to the present invention, it is possible to provide a dedicated and expensive distance measuring sensor.
It is possible to perform distance measurement for position measurement processing of the reference point by utilizing the azimuth and swing direction detection functions.

Further, according to the present invention, even if the topography of the installation location of the position detecting device, that is, the moving area of the moving body is not flat, it is possible to irradiate the reference point with the light beam with high probability.
While rotating and scanning the light beam in the horizontal direction, for example, the light beam is vertically scanned at a slower speed than the scanning speed.

In such a structure, the position can be calculated by performing the distance measurement based on the projection azimuth (azimuth angle) and the swinging direction that are most likely to irradiate the light beam. As a result, it is possible to easily detect the position of the light reflecting means, that is, the reference point, which is performed at the preparation stage for starting the operation.

[Brief description of drawings]

FIG. 1 is a principle diagram of the present invention showing a definition of a tilt direction angle.

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 a diagram showing a traveling course of a self-propelled vehicle and an arrangement state of reflectors.

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

FIG. 7 is a flowchart (No. 1) showing steering control of the vehicle.

FIG. 8 is a flowchart (part 2) showing the steering control of the vehicle.

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

FIG. 10 is a flowchart (No. 1) of inclination direction angle determination processing.

FIG. 11 is a flowchart (No. 2) of inclination direction angle determination processing.

FIG. 12 is an explanatory diagram of a formula for calculating a distance.

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

FIG. 14 is a block diagram showing a main function of a reference point position measurement process.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 ... Self-propelled vehicle, 2 ... Light beam scanning device, 4 ... Rotating mirror, 5 ... Mirror drive motor, 6,6a-6d ... Optical reflector, 7 ... Encoder, 8 ... Rotation center axis, 11 ... Outer ring member, 14 ... Inner ring member, 15 ... Oscillation drive motor, 16 ... Reference point detection determination unit, 19 ... Inclination direction angle calculation unit, 21 ... Reference point continuous detection determination unit, 2
2 ... maximum / minimum angle storage unit, 25 ... reference point identification unit,
26 ... Inclination direction angle storage unit, 27 ... Distance calculation unit, 2
8 ... Reference point position calculation unit, 38 ... Azimuth angle detection unit, 42
... azimuth storage unit, 45 ... data storage unit for each reference point

Claims (5)

[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 receiving signal of the reflected light, a rotation center axis of the rotary scanning means is a substantially conical locus. By swinging so as to draw
A swing scanning unit configured to swing the light beam up and down about an observation point at a rate of rotation; an azimuth angle in a rotary scanning direction when the light signal is detected by the light receiving unit and the swing. A storage means for storing the tilt direction angle of the rotation center axis by the dynamic scanning means, and a maximum value and a minimum value of the tilt direction angle when a plurality of optical signals are detected in substantially the same rotation scanning direction. Means and the observation point and the light based on the maximum value and the minimum value of the tilt direction angle detected in the substantially same rotational scanning direction and the known height dimension of the light reflecting means corresponding to this azimuth direction. A position detecting device configured to calculate a distance between reflecting means. 2. The maximum value and the minimum value of the tilt direction angle are the rotation center axis when the received light signals whose stored azimuth angles are substantially the same are continuously detected for each rotation scan. 2. The position detection device according to claim 1, wherein the position detection device has a maximum value and a minimum value of the inclination direction angle of. 3. The maximum value and the minimum value of the tilt direction angle of the rotation center axis corresponding to the light receiving signal having the largest number of consecutive times among the light receiving signals continuously detected at each rotational scanning. The position detection device according to claim 1, wherein the position detection device has a maximum value and a minimum value of the inclination direction angle. 4. The rotary scanning means and the light receiving means are fixed to a common plate which rotates so that the rotation center axis draws a substantially conical locus.
The position detection device according to any one of 1. 5. The observation point is a moving body, and the position of the light reflecting means is calculated based on the measured distance from the moving body to the light reflecting means and the azimuth angle of the light reflecting means. A calculation means is provided, Claims 1 to 1 characterized by the above-mentioned.
The position detection device according to any one of 4 above.