JPH02109105A - Steering position detector for self-traveling vehicle - Google Patents

Abstract

PURPOSE:To use the latest reference point position information to the computing operation for the position and the traveling direction of a self-traveling vehicle and to reduce the error between the computed value and the present position of the vehicle by performing the computing operations based on the position information and synchronously at the time point of the position information on each reference point is updated. CONSTITUTION:The light beam sent from a light emitter 2 is scanned in the revolving direction of a turntable 4, and the beam reflected from a reflector 6 is made incident on a light receiver 3. A counter 9 counts the number of pulses received from a rotary encoder 7 in accordance with the revolution of the turntable 4. The count value of the counter 9 is processed by an identification process part 11, and the azimuth of the reflector 6 is calculated from the count value of the pulses received for each reception of the reflected light against the traveling direction of a self-traveling vehicle 1. While the azimuth angle detected by the part 11 is inputted to an opening angle arithmetic part 10. Thus the opening angle of each reflector viewed from the vehicle 1 is calculated. Then the position and the traveling direction of the vehicle 1 are calculated based on the position information and synchronously with the time point when the position information on each reference point is updated.

Description

[Detailed description of the invention]

(Industrial Application Field) The present invention relates to a steering position detection device for a self-propelled vehicle, and particularly relates to a steering position detection device for a self-propelled vehicle such as an automobile, an unmanned mobile transport device in a factory, agricultural and civil engineering machinery, etc. . (Prior Art) Conventionally, as a device for detecting the current position of a moving object such as a bipedal self-propelled vehicle, a device is used that scans a light beam generated by the moving object in a circumferential direction with the moving object as the center. , a device has been proposed that is fixed at at least three locations apart from a moving body and includes a light reflecting means that reflects light in the incident direction, and a light receiving means that receives reflected light from the light reflecting means. (Japanese Unexamined Patent Publication No. 5967476). This technology detects the opening angle between three light reflecting means centered on the moving body based on the light receiving output of the light receiving means, and combines the detected opening angle with preset position information of the light reflecting means. The position of the moving object is calculated based on the following. By the way, in this case, the reflected light from the light reflecting means is horizontally
If the light-receiving sensor section is configured so that the light-receiving sensor that rotates in the direction receives the light in order, the light-receiving sensor part can be simplified.
Since each of the light reflecting means fixed at the three locations is not observed at the same time, each light reflecting means is sequentially detected as the moving object moves, so when each light reflecting means is detected. The positions of the moving bodies will be different from each other. For example, as shown in FIG. 7, the reflected light from the light reflecting means placed at reference point A is received when the moving object is at the position of PSIa, and the reflected light from the light reflecting means placed at reference point B is received. The reflected light is received when the moving body is at the position of PSlb. Then, the reflected light from the light reflecting means arranged at the reference point C is received when the moving body is at the position PSlc. In this way, in the conventional technology, the position of the moving object is calculated based on the opening angles α and β between the respective light reflecting means detected around the moving object whose position changes every moment. . (Problems to be Solved by the Invention) Therefore, depending on the timing of calculation, the error in the position of the moving body may become large. However, in the prior art, the calculation timing of the position of the moving object is irregular, and as a result, there is a problem that the error becomes large. For example, in FIG. 7, if the light receiving means is rotated counterclockwise and the above calculation is performed when the moving body is at position PSK, the opening angle α obtained up to position PSIc,
Since the position of the moving body is calculated based on β, the error between the calculation result and the actual position PSK increases. Furthermore, even when the above calculation is performed when the moving object is at position PSH, reference point A is detected at position PS2a, but reference point B is not detected, so the opening angle is at position PS2a.
Since the opening angle obtained at the time of lc remains unupdated, the position of the moving object is still calculated as the position coordinate of the current position PSH based on the opening angles α and β obtained up to the time of position PSlc. I end up being exposed to it. In addition, if the above calculation is performed when the moving body is at position PSN, the opening angle α of reference points A and B is set at position PSN.
Although new data has been detected and updated in S2b, the opening angles β of reference points B and C have not been updated up to position PS2c, so the error between the actual position and the calculation result is large in this case as well. Note that in order to solve the problems of the conventional method, one light receiving means is rotated to sequentially receive the reflected light. It is also conceivable to provide the same number of light receiving means as the number of reference points to be detected, and configure the apparatus to detect the reflected light from all the reference points in as short a time as possible, thereby minimizing the detection error. However, in this case, it is necessary to prepare a plurality of expensive light-receiving means, and the mechanism for rotating these light-receiving means also becomes complicated. An object of the present invention is to solve the above-mentioned problems of the prior art, and to calculate the position of a moving object by sequentially detecting optical signals from a reference point while rotating a light receiving means. Another object of the present invention is to provide a steering position detection device for a self-propelled vehicle that can suppress the error between the calculation result and the actual position of the moving object. (Means and effects for solving the problem) In order to solve the above-mentioned problems and achieve the objects, the present invention provides a method for receiving optical signals from at least three reference points provided at positions distant from the self-propelled vehicle. mounted on a self-propelled vehicle,
A light-receiving means that rotates in the horizontal direction and receives the respective optical signals in order, and a means for detecting the azimuth of the reference point with respect to the traveling direction of the self-propelled vehicle based on the light-receiving output of the light-receiving means. a means for calculating an opening angle between the respective reference points as seen from the self-propelled vehicle; and a means for calculating the position of the self-propelled vehicle based on two scheduled consecutive opening angles among the opening angles; means for calculating the traveling direction of the self-propelled vehicle based on the azimuth angle; and means for calculating the traveling direction of the self-propelled vehicle based on the azimuth angle; The first feature is that it is configured to perform direction calculations. In addition to the first feature, the present invention also includes a light emitting means that rotates together with the light receiving means, and the means for emitting a light beam from the reference point directs the irradiated light from the light emitting means in the direction of incidence thereof. It is characterized by being a light reflecting means. In the present invention having the configuration described in -1-, the self-propelled vehicle is Since the position and heading of the vehicle are calculated, the error between the actual values of the position and heading of the self-propelled vehicle and the calculation results can be kept small. As a result, the structure is configured such that one light receiving means is rotated to sequentially detect the azimuth angles of a plurality of reference points, the opening angle of each is determined, and the steering control of the self-propelled vehicle is performed based on the opening angle. The practicality of the position detection device can be greatly improved in such cases. (Example) An example of the present invention will be described below with reference to the drawings. FIG. 10 is a perspective view showing the arrangement of a self-propelled vehicle equipped with the control device of the present invention and a light reflector disposed in a work area in which the G-propelled vehicle travels. FIG. 8 is a diagram showing the arrangement of the self-propelled vehicle and the light reflector in a coordinate system. In FIG. 10, a self-propelled vehicle 1 is, for example, a self-propelled vehicle for agricultural work such as a lawn mower. A rotary table 4 driven by a motor 5 is provided on the top of the self-propelled vehicle 1. The rotary table 4 has a light emitter 2 that generates a light beam.
A light receiver 3 is mounted to receive the reflected light of the light beam. The light emitter 2 includes a light emitting diode that generates light, and the light receiver 3 includes a photodiode that receives incident light and converts it into an electrical signal (both not shown). Further, the rotary encoder 7 is provided so as to be interlocked with the drive shaft of the rotary table 4.
The rotation angle of the rotary table 4 can be detected by calculating the gt number of pulses output from the rotary table 4. A reflector 6 is arranged around the working area of the self-propelled vehicle 1. The reflector 6 has a reflecting surface that reflects incident light in the direction of incidence, and a so-called corner cube prism or the like which is conventionally available on the market can be used. In FIG. 8, the placement positions of the reflectors 6 are A and A, respectively.
B and C (hereinafter referred to as reference points A, B, and C). In the figure, the positions of the reflectors 6 arranged at three locations are represented by an xy coordinate system in which the reference point B is the origin and the line connecting the reference points B and C is the X axis. The position of the self-propelled vehicle 1 is indicated by T (x, y), and the traveling direction with respect to the X axis is indicated by of. By scanning the light beam in the circumferential direction with the self-propelled vehicle 1 as the center, each reference point A, B with respect to the traveling direction of the self-propelled vehicle 1 is determined.
, C are detected. Based on the azimuth angle, the opening angles α and β between each reference point are calculated. Then, the traveling direction θf of the self-propelled vehicle 1 is calculated based on the azimuth angles θa, θb, and θC, and the coordinates (X, Y) of the position T of the self-propelled vehicle 1 are calculated based on the opening angles α and β. In the arrangement shown in FIG. 8, the actual position and traveling direction of the self-propelled vehicle 1, the opening angles α, β, and the azimuth angle
, θb and θC, when the light receiver 3 mounted on the self-propelled vehicle 1 is rotating counterclockwise, the reference point C It is preferable that the position and heading of the self-propelled vehicle 1 be calculated immediately after the opening angle β is detected and the opening angle β is calculated. The procedure and formula for calculating the coordinates of the position T and the 11th position of the self-propelled vehicle 1 are disclosed in Japanese Patent Application No. 63-11668.
9 and Japanese Patent Application No. 63149619, the details are omitted here. Next, the configuration of the control device of this embodiment will be explained according to the block diagrams shown in FIGS. 1 and 2. The control device is configured to calculate the position and traveling direction of the self-propelled vehicle 1 in synchronization with the detection timing of the opening angle β, and perform steering control of the self-propelled vehicle 1 based on the calculation results. There is. In FIG. 1, the light beam emitted from the light emitter 2 is
The light is scanned in the rotating direction of the rotary table 4 and reflected by the reflector 6. The light beam reflected by the reflector 6 is incident on the light receiver 3. The counter 9 counts the number of pulses output from the row encoder 7 as the rotary table 4 rotates. Then, each time the light receiver 3 receives reflected light, the pulse count value is transferred to the identification processing section 11. The identification processing unit 11 calculates the azimuth angle of each reflector 6 with respect to the traveling direction of the self-propelled vehicle 1 based on the count value of the pulses transferred each time the reflected light is received. The azimuth detected by the identification processing unit 11 is processed by the aperture color calculation unit 10.
The opening angle of each reflector 6 as seen from the self-propelled vehicle 1 is calculated. The calculation timing determination unit 36 determines the calculation timing of the position and traveling direction of the self-propelled vehicle 1 from the Nobubetsu signal of the reflector 6 output from the identification processing unit 11, and if it is determined that the timing is the scheduled timing, the gate 37 is activated. open. When the gate 37 is opened, the opening angle and the azimuth are input to the position/heading calculation section. The calculation timing is determined by reading the count value of the pole counter 27 (FIG. 2) of the identification processing section 11 and determining whether or not the count value matches a scheduled value. The position/heading calculation unit 13 calculates the current position coordinates of the self-propelled vehicle 1 based on the input opening angle, and calculates the traveling direction of the self-propelled vehicle 1 based on the manually input azimuth. The calculation result is input to the comparison section 25. In the comparison section 25,
The data representing the travel course set in the travel course setting section 16 and the coordinates and traveling direction of the self-propelled vehicle 1 obtained by the position/direction calculating section 13 are compared. Of the components shown in FIG. 1, the portions surrounded by chain lines in the figure can be constructed by a microcomputer. The comparison result is input to the steering section 14, and a steering motor (not shown) connected to the front wheels 17 of the self-propelled vehicle is driven based on the comparison result. The steering angle of the front wheels 17 by the steering motor is detected by a steering angle sensor 15 provided on the front wheels of the self-propelled vehicle 1, and is fed back to the steering unit 14. The drive unit 18 controls the starting and stopping of the engine 19 and the operation of the clutch 20 that transmits the power of the engine 19 to the rear wheels 21. Next, the detailed configuration of the identification processing section 11 will be explained. In the identification processing section 11, the azimuth is determined, and based on the azimuth, identification of the light reflector 6 and processing for losing sight of the light reflector 6 are performed. In FIG. 2, the azimuth calculation section 23 uses the reflector 6 in the traveling direction of the self-propelled vehicle 1 based on the count value of the counter 9.
The azimuth angle of is calculated. The azimuth storage unit 35 stores the azimuth when the same reflector 6 was detected last time and the azimuth when the same reflector 6 was detected two times before. The storage unit 35 can store the previous and two previous values of the azimuth angles θa, θb, and θC of each reflector 6 with respect to the traveling direction of the self-propelled vehicle. Then, based on the previous azimuth, the pre-AWj azimuth calculation unit 26
Then, the negative r-measurement direction in which the reflector 6 should be detected is calculated. The predicted azimuth has a predetermined range. The identification unit 24 compares the predicted azimuth and the actual azimuth calculated by the azimuth calculation unit. The identification unit 24 also includes a count value of a pole counter 27 that is configured to update the count value to a count value corresponding to each reflector 6 every time each reflector 6 is detected. The count value is input and the scheduled value is compared. According to the comparison result of the r-measured azimuth and the actual azimuth in the identification section 24 and the comparison result of the count value of the pole counter 27 and the scheduled numerical value, the signal detected by the light receiver 3 is detected by the scheduled reflector 6. A determination is made as to whether or not the detection signal is of reflected light from. When the reflected light from the reflector 6 is detected as planned, the gate 12 is opened by the signal a, and the actual azimuth is inputted to the opening angle calculation unit 10 and the position/heading calculation unit 13. . If the detection signal from the light receiver 3 is determined to be a signal from a reflector 6 other than the intended reflector 6, it is assumed that the intended reflector 6 has been lost, and the same number counter 30 Update count value. If the detection signal from the light receiver 3 is a signal from a reflector 6 other than the P constant reflector 6, and it is determined that two scheduled reflectors 6 have been lost in succession, the signal C indicates the number of times of multiple lost sights. The count value of the counter 31 is updated. Further, if the detection signal by the light receiver 3 is a signal from a signal source other than the reflector 6, it is processed as noise, and the position of the self-propelled vehicle 1 at the time of noise reception and the location of the self-propelled vehicle 1 at the time of noise reception are stored in the noise storage section 29. The azimuth angle of the noise source seen is memorized. The count value of the counter 30 is compared with a threshold value TI in a first comparison section 32, and the count value of the counter 31 is compared with a threshold value T2 in a second comparison section 33. Then, according to the comparison results in the comparison sections 32 and 33, the click!・If the value (number of lost sights) is less than the threshold,
The lost sight processing unit 28 performs a process of estimating the actual azimuth based on the previous azimuth and the previous azimuth stored in the azimuth storage unit 25. The previous azimuth and the previous azimuth are updated by the process of the lost sight processing section. Further, when the count value (number of times of losing sight) is equal to or greater than the threshold value, signals d and e are output to the drive unit 18, and the traveling of the self-propelled vehicle 1 is stopped. Since the loss of two reference points is more serious than the loss of one reference point, the threshold TI is set to the threshold T2.
A larger number is set. That is, when two reference points are lost, the self-propelled vehicle 1 is stopped early before the number of times the reference points are lost becomes too large. Note that when the identification unit 24 determines that the scheduled reflected light from the reflector 6 has been received, the number of lost sights counter 30 and the multiple times of lost sights counter 31 are set to "0" by the signal a. The value of the pole counter 27 is updated when the intended reflector 6 is normally detected by the identification unit 24 and when the lost sight process is completed. Further, if the scheduled reflector 6 is normally detected, the gate 34 is opened,
Among the values in the azimuth storage unit 35, the previous azimuth is updated with the latest azimuth, and the azimuth before the previous one is updated with the previous azimuth. Next, a description will be given of steering control for controlling the running direction of the self-propelled heavy equipment based on the position information of the self-propelled vehicle 1 calculated by the above procedure. FIG. 9 is a diagram showing the positional relationship between the traveling course of the self-propelled vehicle 1 and a reference point, and FIG. 3 is a flowchart of steering control. FIG. 9 shows the position of the self-propelled vehicle 1 and the work area 22 by the self-propelled vehicle 1 in a coordinate system with reference point B as the origin and a line passing through reference points B and C as the y-axis. There is. Point R (Xret, Yret) indicates the return position of the self-propelled vehicle 1, and the work area 22 has the coordinates (Xst. Yst), (Xs t, Ye), (Xe, Yst),
This is an area connecting the points indicated by (Xe, Ye). Here, the position T of the self-propelled vehicle 1 is indicated by (Xp, Yp). In addition, in FIG. 9, in order to simplify the explanation, an example is shown in which the four sides of the work area 22 are y-axis or parallel to the y-axis, but there are reference points A, B, and C around the work area 22. , the shape of the work area 22 and the work area 2
The orientation of the four sides of 2 is arbitrary. The control procedure will be explained according to the flowchart in FIG. First, in step S1, the self-propelled vehicle 1 is moved from point R to the work start position by radio control. In step S2, while the self-propelled vehicle 1 is stopped, the light emitting device 2. The light receiver 3 is rotated to detect each reference point, and the azimuth angles θa, θb, and θC of each reference point as seen from the self-propelled vehicle 1 are stored in the storage unit 35. In step S3, Xs is set as the X coordinate Xn of the driving course.
Set t and determine the driving course. In step S4, the self-propelled vehicle 1 starts running. In step S5, it is determined whether the light receiver 3 has received the reflected light from the reference point. Step S5 is repeated until reflected light is detected. If reflected light is detected, the process advances to step S6, and a reference point identification process shown in a subroutine to be described later is executed. In step S7, the opening angle between the reference points is calculated based on the azimuth angle of the reference points detected in the reference point identification process. In step S8, the count value of the pole counter 27 is read into the calculation timing determination unit 36, and it is determined whether the count value matches the count value scheduled to be set in the calculation timing determination unit 36. In this example,
The count value of the pole counter 27 corresponding to the reference point C"
3'' is set as the planned count value. If the count value of the pole counter 27 matches the planned count value, it is determined that it is the timing to calculate the position and heading of the self-propelled vehicle 1. Then, the process proceeds to step S9. If the count value of the pole counter 27 does not match the scheduled count value, the process proceeds to step S5. In step S9, the self-propelled vehicle 1 The position (Xp, Yp) and traveling direction of are calculated, and the straight-line running control of the self-propelled vehicle 1 is performed according to the calculation results. In step SIO, the self-propelled vehicle 1 is determined in the y-axis direction in the direction ( It is determined whether the vehicle is traveling in the direction (going direction) or in the direction approaching the origin (returning direction). If it is the traveling direction, in step S ], 1, -
It is determined whether the stroke has ended (Yp>Ye), and if it is in the return direction, in step 512 - stroke end (Yp>Ye) is determined.
It is determined whether Yp<Yst). Step S1
1 or S12, if it is determined that the -stroke has not been completed, steps S5 to S10 are performed. If it is determined in step S11 or S12 that the -stroke has been completed, then it is determined in step S1.3 whether or not all the strokes have been completed (Xp>Xe). If the entire stroke has not been completed, the process moves from step 81.3 to step S14, and the U-turn control of the self-propelled vehicle 1 is performed. The U-turn control is performed in a different manner from the straight-line stroke steering control in step S9, in which the position information of the self-propelled vehicle 1 calculated by the position/heading calculation part 13 is fed back to the steering part 14. That is, during the turning stroke, the self-propelled turret 1 is driven with its steering angle fixed at a preset angle. Then, when at least one of the azimuth angles of each of the reference points A, B, and C with respect to the self-propelled vehicle 1 matches within a predetermined angle range, the turning is stopped and the steering control is returned to straight-line travel. . In step S15, Xn+L is set to Xn,
The next running course is set. Once the next travel course is set, the process returns to step S5 and the above process is repeated. When the entire stroke is completed, the vehicle returns to the return position R (Xret, Yret) (step 516) and stops traveling (step 517). Next, the reference point identification process in step S6 will be explained. In this embodiment, as a method for identifying reference points, each 1. (Focusing A, B, C1!: By associating the count values of the pole counter 27 with the reference point A with the count value 1, the reference point B with the count value 2, and the reference point C with the count value 3, the six standardizations can be made. I try to identify the jiao, i.e.
Each time the light receiver 3 detects reflected light, the pole counter 27 is configured to sequentially output count values 1, 2.3, and by monitoring the count values, it is possible to identify whether it is a reference point. In this method, if the reference point is lost or light is received from a reflective object other than the reference point, the reference point and the count value will no longer correspond, resulting in incorrect position IN detection. Therefore, in the reference point identification process shown in flowchart 1 in FIG. Below, it is determined whether the count value output from the pole counter 27 corresponds to the reference point, and as a result, it is determined that the scheduled reference point has been normally detected. As a result of the above judgment, if it is determined that the reference point has been lost, or if it is determined that light has been received from an anti-JIJ object other than the reference point, position detection is continued according to the reference point loss processing described later. , if the number of times the reference point is lost exceeds the scheduled number of times, self-propelled

[I am trying to stop the PL from running. Next, a method for predicting the azimuth of the reference point will be explained. 1st
FIG. 1 is a diagram showing changes in the azimuth of a reference point A as seen from the self-propelled vehicle 1 as the self-propelled vehicle 1 moves. In the same figure, when the self-propelled vehicle 1 is at point m1, the azimuth angle is θ
an-1, the azimuth angle when self-propelled vehicle 1 is at point m2 is θan
It is. Then, when the self-propelled vehicle 1 is at point m2, the azimuth angle when the self-propelled vehicle 1 is at point m3 is predicted to be θan±θ. Here, the fixed value θ is an experimentally determined angle (θ
It is set based on an −(θan-1) l . According to experiments conducted by the inventors, except when the self-propelled vehicle 1 is turning (θan (θan-1) l is approximately within the range of 3 degrees, in this example, the fixed value θK is However, when the self-propelled vehicle 1 is turning, the azimuth changes rapidly, and the fixed value θ is changed to a larger setting value than when traveling in a straight line. θ was set to 30°. Note that θ may be set to a fixed value based on the results obtained through experiments as described above, or may be set to a fixed value based on the results obtained by experiment as described above.
The difference may be calculated based on the azimuth angles detected last time and the time before the time stored in , and used as θ. Although the azimuth prediction method for the reference point A has been described in FIG. 11, the azimuth angle prediction for the other reference points B and C is performed in the same way. The reference point identification process explained above will be explained according to the flowchart of FIG. 4. First, in step S61, the azimuth angle of the reflective object with respect to the traveling direction of the self-propelled vehicle 1 calculated based on the latest light reception signal is read as θX. In step S62, it is determined whether θX is within the range θan±θ. If the judgment in step S62 is affirmative, it is assumed that the light reception signal is a reflected signal from the reference point A, and the process proceeds to step S63 in FIG. 4 (part 2). In step S63, it is determined whether the count value of the pole counter 27 is "3", which corresponds to the reference point C that should be detected before focusing A. If the count value is "3", the judgment that "the received light signal is a reflected signal from the reference point" assumed in step S62 is determined to be correct, and the processes of steps S64 and S65 are performed. Said step S7 (Fig. 3)
Return to In step S64, since it is determined that the light reception signal is a reflected signal from the reference point A, a process for determining the azimuth θa of the reference point A as seen from the self-propelled vehicle 1 is performed. This azimuth angle θ
Details of the a determination process will be described later with reference to FIG. In step S65, the count value of the pole counter 27 is set to "1". On the other hand, if it is determined in step S63 that the count value of the pole counter 27 is not "3", the process proceeds to step S66, where the count value of the pole counter 27 is set to "1". is "2". If the determination in step 556 is affirmative, it is determined that the reference point C has been lost, and the process proceeds to step S67, where processing for losing the reference point C is performed. Details of the loss of sight process will be described later with reference to FIG. 6. When the loss of sight process for the reference point C is completed, the process proceeds to step 568. In step 868, the number of times the reference point C is lost I Loss
A determination is made whether tC exceeds a threshold T1. Number of times I lost sight of reference point C 1. ostC is the threshold'
If rl is exceeded, the process proceeds to step S69, where the clutch 20 is disengaged, the engine 19 is stopped, and the fail lamp is blinked. In the judgment in step S68, if the number of times I LostC of losing sight of the reference point C does not exceed the threshold T1, the process advances to step S65. Further, if the determination at step 566 is negative, it is determined that reference points B and C have been lost, and the process advances to step S70, where processing for losing reference point B is performed. When the process for losing sight of the reference point B is completed, the process advances to step S71. In step S71, the number of times I L loses sight of the reference no λB
A determination is made whether or not ost13 exceeds the threshold T1. If the number of times I LostB loses sight of the reference point B exceeds the threshold value T1, the process advances to step S69. If it is determined in step S71 that the number of times the reference point B is lost, 11.05t13, does not exceed the threshold T], the process advances to step S72, and processing for losing the reference point C is performed. In step S73, the number of times the reference point C is lost I Loss
A determination is made whether tC exceeds a threshold TI. Number of times I lose sight of reference point C LostC is threshold value TI
If it exceeds, the process advances to step S69. In the judgment in step S73, if the number of times II, ostC of losing sight of the reference point C does not exceed the threshold value TI, the process advances to step S74. In step S74, the multiple lost sight count counter 31,
Number of times reference points B and C were lost in succession: 1 Add "1" to LostBC. In step S75, it is determined whether the number of times It and ostBC that the reference points B and C have been lost in succession exceeds the threshold value T2. Number of times I lost sight of reference points B and C consecutively I LostB
If C exceeds the threshold T2, the process advances to step S69. If it is determined in step S75 that the number of times I LostBC of consecutively losing sight of the reference points B and C does not exceed the threshold value T2, the process proceeds to step S65. In step S62, the light reception signal is θan±θ
If it is determined that the received light signal is not detected within the range Obn±θ, the process proceeds to step 376, where it is determined whether the received light signal is detected within the range Obn±θ. When it is determined that the light reception signal is detected within the range of Obn±θ, that is, it is a light reception signal of reflected light from the reference point B, the process proceeds to step S78 (Fig. 4, Part 3), and the following steps are performed. Processing of steps 379 to 89 is performed. Further, if the determination at step 376 is negative, the process proceeds to step 5.77, and the light reception signal is set at θel.
A determination is made as to whether or not it has been detected within the range of +±θ. If it is determined that the light reception signal is detected within the range of Ocn±θ, that is, it is a light reception signal of reflected light from the reference point C, step 590 (Fig. 4, Part 4)
The process proceeds to Steps 391 to 5101, and the processing of Steps 391 to 5101 is performed. Step S62. If the judgments in S76 and S77 are all negative, it means that the received light signal is not due to reflected light from any of the reference points A, B, or C, and the received light signal is due to reflection from a reflective object other than the reference point. It is determined that the signal is received light or light from another light source, and is processed as noise. When the received light signal is determined to be noise,
The process advances from step 577 to 8102, where the noise warning light is temporarily turned on. In step 5103, the coordinates of the self-propelled vehicle 1 at the time the noise was detected and the 11-degree angle of the noise source as seen from the self-propelled vehicle 1 are read, and the values are stored in the storage unit 29. Note that the processing in steps 378 to S89 and step 3
The processes in steps 90 to 5101 are performed in the same way as the processes in steps 863 to 75, so the description thereof will be omitted. Next, the azimuth θa of the reference point as seen from the self-propelled vehicle 411. θb and θC determination processing and reference point loss processing S2a
Figures 5 and 6 regarding the operation of the reference point loss processing in
This will be explained with reference to flowchart 1 in the figure. FIG. 5 is a flowchart showing the process of determining the azimuth angle θa of the reference point A. In the same figure, in step 811G, the number of lost sights I LostA of the reference point A of the lost sight number counter 30, the number of times of lost sight I LostAB of the multiple lost sight number counter 31 that stores the number of times the reference points A and B are successively lost. The same number of lost sights T i, ost of the multiple lost sight count counter 31 that stores the number of consecutive lost sights of the reference points C and A.
In step 5111, which sets "0" to cA, θa
θan is read into n-1, θX is read into θan, and the data is updated. The process of determining the azimuth θb of the reference point B and the azimuth θC of the reference point C is the same as the process of determining the azimuth θa of the reference point A, so a description thereof will be omitted. FIG. 6 is a flowchart 1 of the process of losing sight of the reference point C. In the same figure, in step 5120, "1' is added to the number of times I LostC of losing sight of the reference point C. In step 5121, the azimuth of the reference point C that the self-propelled vehicle 1 detected the previous time and the reference point detected the day before last are added. The difference (θcn - (θcr+-1)) from the azimuth angle of C is read as Δθ, the previous detected azimuth θcn is updated by the previously detected azimuth θen, and the previously detected azimuth θcn is changed to θcn+Δθ
Update with. If a reference point cannot be detected within the predetermined range due to such loss of sight processing, the azimuth data of the undetected reference point is estimated and updated based on the previous and previous data, and the updated data is updated. The position and azimuth of the self-propelled vehicle 1 are calculated based on the following. The process of losing sight of reference points A and B is performed in the same way as the process of losing sight of reference point C. Note that the difference Δθ in the azimuth angle is (θcn-(θcn-1,
) 1, and Δθ is a fixed value set in advance based on not only the result but also the value obtained by experiment.
May be used as As explained above, in this embodiment, the position I of the self-propelled vehicle 1 is
The calculation timing of N and the heading direction is synchronized with the calculation end time of the opening angle β. Therefore, the azimuth angles θa, θb and θC, and the opening angle α
, β are updated to the latest data, the position and heading of the self-propelled vehicle 1 can be calculated, and as a result, the error between the calculated value and the actual value is reduced. Note that this embodiment has been described for the case where the reference points are placed at three locations, but if the reference points are installed at four or more locations and three of these are selected, 5, The same means as in this embodiment can be used to calculate the position and traveling direction of the self-propelled vehicle based on the azimuth and opening angle of the reference point. Furthermore, in this embodiment, an example is shown in which the calculation timing is synchronized with the time when the reference point C is detected, but the rotation direction of the light receiver 3 determines which reference point the calculation timing is synchronized with. , is determined depending on which opening angle between the reference points is used to calculate the position and heading of the self-propelled vehicle 1. In short, it is sufficient that the calculation timing is synchronized with the time when two adjacent opening angles, which are determined by r, used for calculating the position of the self-propelled vehicle 1 are successively detected. In this embodiment, an example was shown in which both the light emitter and the light receiver are mounted on a self-propelled vehicle, but the light emitting means is placed at the reference point,
The present invention can also be implemented in a system in which a self-propelled vehicle is equipped with only a light receiving means. (Effects of the Invention) As is clear from the above description, according to the present invention, the following effects can be achieved. (1) The same IJ1 at the time the position information of each reference point is updated.
Since the position and traveling direction of the self-propelled vehicle are calculated based on the position information, the latest reference point position information can be used for the calculation. As a result, the error between the calculated value and the current position of the self-propelled vehicle can be reduced. (2) Immediately after the position information of the reference point is updated, the position and traveling direction of the self-propelled vehicle are calculated, and before the position information is updated, the position and traveling direction of the self-propelled vehicle are calculated again based on the same position information. There is no need to waste time calculating the direction.

[Brief explanation of drawings]

1 and 2 are block diagrams showing one embodiment of the present invention,
Figure 3 is a flowchart of steering control, Figure 4 is a flowchart of reference point identification processing, Figure 5 is a flowchart of azimuth determination processing, Figure 6 is a flowchart of reference point loss processing, and Figure 7 is reference point detection. Figure 8 is a diagram showing the principle of detecting the traveling direction of a self-propelled vehicle. Figure 9 is a diagram showing the traveling course of a 1'J vehicle and the arrangement of reflectors. Figure 10 is a diagram showing the traveling direction of a self-propelled vehicle. FIG. 11 is a perspective view showing the arrangement of the reflectors, and FIG. 11 is an azimuth explanatory diagram of the reference point. 1... Self-propelled vehicle, 2... Emitter, 3... Light receiver, 6
- Reflector, 7... Rotary encoder, 9... Counter, 10... Opening angle calculation section, 11... Identification processing section, 13... Position/direction calculation section, 14... Steering Part, 23... Planned angle setting part, 24... Identification part, 25
32.33... Comparison unit, 26... T-side azimuth angle calculation unit, 27... Pole counter, 28... Loss of sight processing unit, 29... Noise storage unit, 30... Number of times of loss of sight Counter, 31... Counter for the number of times you lose track of multiple items, 36...
・Calculation Timing Judgment Department Representative Patent Attorney Michito Hiraki and 1 other person

Claims (4)

[Claims] (1) Receive optical signals from light generation means provided at at least three reference points apart from the self-propelled vehicle, and detect the position and traveling direction of the self-propelled vehicle based on the received light signals. A steering position detection device for a self-propelled vehicle, comprising: a light receiving means mounted on the self-propelled vehicle and configured to sequentially receive each of the optical signals while rotating in a horizontal direction; and a light receiving means based on a light reception interval of the light receiving means. means for detecting the azimuth of the reference point with respect to the traveling direction of the self-propelled vehicle; means for calculating the angle of difference between the respective reference points as seen from the self-propelled vehicle; means for calculating the position of the self-propelled vehicle based on the two aperture angles; means for calculating the traveling direction of the self-propelled vehicle based on the azimuth; and a reference point that is the end of the two consecutive aperture angles. A steering position detection device for a self-propelled vehicle, characterized in that the device is configured to calculate the position and heading of the self-propelled vehicle in synchronization with the timing at which the azimuth is detected. (2) A light emitting means is provided which is rotatably mounted on the self-propelled vehicle together with the light receiving means, and the light generating means is arranged at the reference point and reflects the irradiated light from the light emitting means in the direction of incidence thereof. The steering position detecting device for a self-propelled vehicle according to claim 1, wherein the device is a light reflecting means for detecting a steering position of a self-propelled vehicle. (3) The position of the self-propelled vehicle is based on the light reception signal from the light generating means placed at any three reference points among the light generating means placed at three or more reference points. The steering position detection device for a self-propelled vehicle according to claim 1 or 2, wherein the device detects a steering position of a self-propelled vehicle. (4) A pole counter is provided that is updated with a count value corresponding to each reference point each time the reference point is detected, and when the scheduled count value of the counter is detected, the position of the self-propelled vehicle and 4. The steering position detection device for a self-propelled vehicle according to claim 1, wherein the device is configured to calculate a traveling direction.