JPH08211934A - Steering controller for traveling object - Google Patents

Abstract

PURPOSE: To continuously exactly detect the position of a traveling object. CONSTITUTION: A first position detector 23 detects the position of the traveling object from the light receiving direction of light scanned by a light beam scanner 2 and reflected at a reference point. A second position detector 16 detects the position corresponding to a yaw angle change and a moving distance due to signals from a gyro 11 and a car speed pulse generator 12. The second position detector 16 can continuously detect the position but since its cumulative error is big, the result is successively corrected by the detected data of the first position detector 23. Thus, two position detectors continuously and exactly detect the position of the traveling object while being complemented each other. Especially, at a turning part where the advancing direction is rapidly changed or when the reference point gets lost, the effect due to the second position detector 16 is big.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a steering control device for controlling the steering direction while detecting the position of a moving body, and more particularly to an unmanned mobile transfer device in a factory, agriculture and civil engineering construction fields. The present invention relates to a steering control device for various moving bodies such as a self-propelled vehicle used.

[0002]

2. Description of the Related Art As a position detecting device for a moving body in a traveling area, a means for scanning a light beam in the circumferential direction around the moving body and a means for fixing the moving body are fixed at at least three locations apart from the moving body. An apparatus has been proposed which includes a light reflecting means for reflecting light in the incident direction and a light receiving means for receiving the reflected light from the light reflecting means (for example, Japanese Patent Publication No. 2-57843).
In this device, the azimuth angle of the light reflecting means viewed from the moving body is calculated from the light receiving direction of the reflected light. Then, using this azimuth, the position of the moving body is detected by the principle of triangulation.

In this type of position detecting device, the light beam is rotated in the horizontal direction so that the reflected light from each of the light reflecting means is received in order, so that it comprises a light emitting device and a light receiving device. There is no need to provide an expensive light beam scanning means for each light reflecting means, and there is an advantage that one set can be used.

[0004]

The conventional position detecting device has the advantages as described above, but has the following problems. That is, in the position detecting device, the light beam is scanned once, that is, 360 °, and the light receiving signals from all the planned light reflecting means are detected so that the position calculation can be performed once. For this,
The time interval required for sampling the received light data for one time is long, and particularly when the moving speed of the moving body equipped with the position detecting device is high, the position detection accuracy is lowered and the deviation from the predetermined traveling course is likely to be large. There is a problem. Regarding the problem caused by the long sampling interval, there is a gazette of the applicant's application (Japanese Patent Laid-Open No. 2-10).
No. 9105).

Further, even when the moving body changes its traveling method in a short time such as turning, the position detection accuracy is significantly lowered, and the light reflecting means is blocked by an obstacle or the like during traveling. Is detected, or the reflected light from the light reflection means in another adjacent traveling area is detected, and the light reflection means is lost or misdetected. There is also a problem that the deviation easily becomes large.

The object of the present invention is to solve the above problems,
It is an object of the present invention to provide a steering control device for a moving body, which is capable of performing accurate steering control even when the moving speed of the moving body is high or when there is a sudden direction change.

[0007]

SUMMARY OF THE INVENTION The present invention for solving the above problems and achieving the object is a light beam scanning means for circumferentially scanning a light beam with respect to a reference point around a moving body. Azimuth detecting means for sequentially detecting reflected light of the light beam reflected from the reference point to detect its azimuth angle,
First position detection means for detecting the position of the moving body based on the azimuth angle and position information of at least three reference points, and second position detection for continuously detecting the inertial position of the moving body by integrating displacement. Means, steering means for steering the moving body based on the position of the moving body detected by the second position detecting means, and position data of the moving body detected by the second position detecting means, The first feature is that the correction means is provided to sequentially correct the position data of the moving body detected by the one-position detection means.

Further, according to the present invention, every time the position of the moving body is detected by the first position detecting means, the position data of the moving body detected by the second position detecting means is converted into the detected position data. The second feature is that the correction means is configured to be updated.

Further, according to the present invention, when it is determined that the traveling direction of the moving body has changed more than a predetermined value, the first direction
A third characteristic is that the correcting means is configured so as to prohibit the position data updating operation by the position detecting means.

The present invention further comprises a turning detection means for detecting that the moving body has reached a predetermined turning start position and a turning end position, and the turning detection means causes the moving body to reach the turning start position. A fourth feature is that the correction means is configured to prohibit the position data updating operation by the first position detection means when it is determined that the movement has been reached, and release the prohibition when the turning end position is reached. .

Further, the azimuth angle detected by the azimuth angle detecting means and the absolute azimuth angle based on the traveling azimuth data used for position detection by the second position detecting means are compared with previously detected values to identify the reference point. The fifth feature lies in that the reference point identifying means is provided.

[0012]

According to the first feature, the moving body is steered based on the position data obtained by the second position detecting means. The second
The position detecting means has an advantage that position data can be continuously obtained, but has a drawback that an integration error occurs because position data is obtained by integrating displacement. Therefore, the defect of the second position detecting means is compensated by sequentially correcting with the position data obtained by the first position detecting means which does not cause the accumulated error.

According to the second feature, the position data by the second position detecting means is corrected by the latest position data obtained by the first position detecting means. According to the third and fourth characteristics, the correction operation based on the position data detected by the first position detecting means is prohibited when the traveling azimuth changes greatly.

According to the fifth feature, each reference point is identified by an absolute azimuth angle which has a small influence even when the traveling direction of the moving body is largely changed.

[0015]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS The present invention will be described in detail below with reference to the drawings. First, the configuration of the moving body according to the embodiment of the present invention will be described with reference to FIG. In FIG. 1, a moving body 1 includes a light beam scanning device 2, a drive device 3 for traveling the moving body 1, a steering device 4 for determining a traveling direction of the moving body 1, and a control device 5 for controlling each of the devices. Consists of. The control device 5
Can be composed of a microcomputer.

The light beam scanning device (hereinafter referred to as "scanning device")
2 is a light emitter 6, a light receiver 7, a motor 8 that rotates the light emitter 6 and the light receiver 7, and a rotary encoder that outputs a pulse signal in accordance with the rotation of the light emitter 6 and the light receiver 7. It consists of 9. The light emitter 6 includes, for example, a light emitting diode, and outputs a light beam 2E, which is scanned in the rotating direction of the light emitter 6. At least three light reflectors 10 are arranged around the traveling area of the moving body 1 as reference points for setting the traveling area, and the light beam reflected by the surface of the retro-reflector of the light reflector 10 is arranged. 2R is incident on the light receiver 7. The light receiver 7 is provided with, for example, a photodiode, and the incident light is converted into an electric signal by the photodiode and input to the control device 5.

On the other hand, the pulse signal from the rotary encoder 9 is also input to the control device 5 and counted. The count value of the pulse signal is read every time the detection signal is input from the light receiver 7. The control device 5 detects the azimuth angle of each reference point and the opening angle between the reference points based on the traveling direction of the moving body 1 based on the counted number of the pulse signals, and detects the opening angle and each reference point. The position of the moving body 1 is calculated based on the position information of the point 10. The above-mentioned constituent element necessary for this position calculation is the first position detecting means.

Further, a gyro 11 and a vehicle speed pulse generator 12 are provided to detect the self-position of the moving body 1 by the inertial position detecting method. As the gyro 11, for example, a piezoelectric vibration gyro can be used. Further, the vehicle speed pulse generator 12 is preferably provided in pairs corresponding to both the left and right wheels of the moving body 1, and for example, the average value of the both is taken, or the output signal on the side that is extremely different from the planned reference value is adopted. It is better not to do so to improve accuracy. Output signals from the gyro 11 and the vehicle speed pulse generator 12 are supplied to the controller 5. Obtaining the traveling direction information of the moving body 1 based on the yaw angle supplied from the gyro 11, and obtaining the rotation amount information of the wheels of the moving body 1, that is, the moving distance information, by the vehicle speed pulse supplied from the vehicle speed pulse generator 12. You can The position of the mobile unit 1 is calculated based on these pieces of information. The above-mentioned component necessary for this position calculation is the second position detecting means.

The control device 5 receives the position data corrected by one or the other of the position data of the moving body 1 detected by the two position detecting means as described above.
It compares with the traveling course set by the traveling course information stored in the RAM or the like in the control device 5, and outputs a steering instruction according to the comparison result to the steering device 4.

The drive unit 3 includes an engine, and the control unit 5
Gives an instruction to the drive unit 3 to start and stop the engine. The method of calculating the self-position of the moving body 1 by the first position detecting means and the steering control based on the calculation result are disclosed in detail in Japanese Patent Application Laid-Open No. 1-287415 or Japanese Patent Application Laid-Open No. 1-316808. .
The description of the above publication is incorporated by reference in the description of the present invention.

Next, the position control of the moving body will be described. First, the absolute azimuth angle used in the following position control will be described with reference to FIG. In FIG. 3, when the moving body 1 moving in the traveling area around which the reference points A to D are arranged changes its direction from the traveling direction d0 to the traveling direction d1 to the right, the reference point A for the traveling body 1 is changed. The azimuth angle of changes greatly from θa0 to θa1. However, even in such a case, the straight line connecting the reference points B and C and the moving body 1
And the angles θA0 and θA1 formed by the straight line connecting the reference points A
Does not change much. From this, in this embodiment,
Angles θA0, θA1 (hereinafter referred to as “absolute azimuth θA”)
Based on the above, the reference points A to D are identified. For example, the absolute azimuth angle when the reference point A was detected in the previous light beam scan is stored, and when the reflected light is detected in the azimuth value close to the absolute azimuth angle in the next scan, the reflected light is reflected. The light is determined to be the light from the light reflecting means at the reference point A.
The absolute azimuth angles of the reference points B to D are indicated by symbols θB to θD.

On the other hand, as shown in FIG. 4, the absolute azimuth θA is obtained by adding the traveling azimuth θf and the azimuth θa, and it is necessary to know the traveling azimuth θf in order to know the absolute azimuth θA. is there. Here, the traveling azimuth θf is the traveling direction of the moving body 1 based on the straight line connecting the reference points B and C, and the azimuth θa is the azimuth angle of the reference point A with respect to the traveling direction of the moving body 1. In the present embodiment, the traveling direction θf is set to the gyro 1
The detection is performed based on the change in the yaw angle obtained from 1.
The advancing direction θf is updated by integrating the change in the yaw angle with respect to the initial setting value.

Next, the control operation of this embodiment will be described with reference to the flow chart. In FIG. 5, step S1
Then, the initial values of the absolute azimuth angles θA to θD are set. This initial value may be a value stored in advance, or may be input by the operator before starting the work. Further, the azimuth angles θa to θd and the traveling azimuth θ calculated from the light receiving signals of the reference points by the scanning device 2.
It may be calculated based on f.

In step S2, it is judged from the output of the scanning device 2 whether or not the reflected light is received. When the reflected light is received, the process proceeds to step S3, the azimuth angle is calculated based on the received light signal, and the azimuth angle is stored in the buffer. At this point in time, which azimuth of the reference point has not been determined, but when the optical signal from which reference point is identified by the pole identification processing described later, this azimuth is the azimuth of the identified reference point. Is determined as the azimuth angle of. In step S4, yaw angle data is read from the gyro 11. In step S5, the deviation of the current value of the yaw angle from the previous value, that is, the yaw angular velocity is calculated. In step S6, the absolute azimuth angle is calculated from the yaw angle and the azimuth angle, and is set as a variable θ. Step S
At 7, pole identification processing is performed to identify which reference point the absolute azimuth angle is. This poll identification process will be described with reference to FIG. In step S8, it is determined whether all four reference points have been identified. If this determination is affirmative, the routine proceeds to step S9, where it is determined whether or not the yaw angular velocity is large. The threshold value for this determination is set to a value corresponding to the angular velocity detected when the traveling direction of the moving body 1 is drastically changed, that is, a value where the position calculation error exceeds the allowable range.

If the determination in step S9 is negative, that is, if the change in the traveling direction of the moving body is small, step S1
Proceeding to 0, the position calculation of the moving body 1 by triangulation, that is, the position calculation based on the azimuth angle detected in step S3 and determined in step S7 and the position information of the reference point registered in advance is performed. In step S11, the traveling direction θf is
It is updated with the traveling direction calculated in the process of position calculation by the triangulation. In step S12, the old data is updated with the position data calculated in step S10.

On the other hand, when the reflected light is not received, the determination in step S2 is negative and the process proceeds to step S13. In step S13, the yaw angle is read from the gyro 11, and in step S14, the yaw angular velocity is obtained, and then the process proceeds to step 15. The determination in step S8 is negative until all four reference points are identified, and the process proceeds to step S15. Further, when the angular velocity of the yaw angle is larger than the threshold value, the determination in step S9 becomes affirmative and the process proceeds to step S15. In step S15, the traveling direction θf
Is updated with a value calculated based on the yaw angular velocity. In step S16, the traveling distance of the moving body 1 from the previous measurement is calculated based on the output signal of the vehicle speed pulse generator 12. In step S17, position calculation by inertial position calculation is performed based on the traveling direction θf and the traveling distance. In step S18, the old data is updated with the new position data calculated in step S17.

Next, the pole identification will be described with reference to a flow chart. In FIG. 6, step S2
At 0, it is determined whether or not the absolute azimuth angle θ calculated in step S6 (FIG. 5) is substantially equal to the absolute azimuth angle θA regarding the reference point A. If this determination is affirmative, it is determined that the reference point A has been detected, and the process advances to step S21 to update the absolute azimuth angle θA with θ. At the same time, the azimuth angle is the reference point A
Is determined as the azimuth angle of. If the determination in step S20 is negative, the process proceeds to step S22, and it is determined whether or not the absolute azimuth angle θ calculated in step S6 (FIG. 5) is substantially equal to the absolute azimuth angle θB regarding the reference point B. If this determination is affirmative, it is determined that the reference point B has been detected, and step S23 is performed.
To update the absolute azimuth angle θB with θ. At the same time, the azimuth angle is determined as the azimuth angle of the reference point B.
Hereinafter, steps S24 to S27 are steps S20 to S2.
Since it is the same as that of 3, the description thereof will be omitted. Note that step S
If the determination in 26 is negative, it is determined that the received light signal is not due to the reflected light from the predetermined reference point, noise processing (not shown) is performed, and the processing according to this flowchart ends.

Based on the position data of the moving body 1 calculated as described above, the steering control is performed as described with reference to FIG. 2, and the reference points A to D are set in the surrounding area. Scheduled work is carried out by moving the traveling course.

Next, the main functions of the control device 5 for performing the above-described processing will be described with reference to the block diagram.
In FIG. 1, the yaw angle detection unit 13 detects the output signal of the gyro 11, that is, the yaw angle. The yaw angle storage unit 14 stores the previously detected yaw angle. Angular velocity detector 1
In No. 5, the yaw angular velocity is detected based on the amount of change between the yaw angle detected this time and the yaw angle detected last time, and a signal is output when the yaw angular velocity is larger than a predetermined value. The second position detector 16 reads the vehicle speed pulse and the yaw angle generated by the vehicle speed pulse generator 12, and based on the traveling distance and the yaw angle obtained based on the vehicle speed pulse, the moving body 1
Calculate the position and heading of the.

On the other hand, the azimuth angle detector 17 detects the azimuth angle of the reference point based on the light receiving signal from the scanning device 2. The detected azimuth angle is input to the absolute azimuth angle calculation unit 18. The absolute azimuth calculation unit 18 includes an azimuth and yaw angle detection unit 1.
The absolute azimuth angle is calculated based on the yaw angle read from 3. The pole identification unit 19 compares the detected absolute azimuth angle with the absolute azimuth angle of each previously detected reference point stored in the absolute azimuth angle storage unit 20. If the two match within a predetermined range, the pole identifying section 19 outputs an identification signal. The identification signal is supplied to the switch means SW1 and SW2. This identification signal causes the switches SW1 and SW2 to
Is closed, and as a result, the azimuth angle detected by the azimuth angle detection unit 17 is stored in the azimuth angle storage unit 21 in correspondence with the identified reference point. Further, the storage data of the absolute azimuth storage unit 20 is updated with the newly detected absolute azimuth.
The all-pole detection determination unit 22 determines whether or not all the reference points are identified by the pole identification unit 19 by associating with the absolute azimuth, and outputs a detection signal when all the reference points are identified. To do. When all the reference points have been identified, the first position detection unit 23 stores the azimuth angles stored in the azimuth angle storage unit 21 in association with the respective reference points, and the reference point position information registered in advance. The position and traveling direction of the moving body 1 are calculated based on

The switch SW3 is normally switched to the contact b side and supplies the position and traveling direction calculated by the second position detecting section 16 to the position / traveling direction storage section 24. Then, only when the signal indicating that the angular velocity is large is not output from the angular velocity detection unit 15 and when the detection signal is output from the all-pole detection determination unit 22, the contact is switched to the contact a side. This switch SW3
The data stored in the position / traveling direction storage unit 24 is updated with the position and the traveling direction calculated by the first position detecting unit 23 by switching. The steering device 6 controls the steering of the moving body 1 based on the data stored in the position / travel direction storage unit 24.

As described above, in this embodiment, the gyro 1
The position calculated by the second position detector 16 when the traveling direction of the moving body 1 is largely changed by the output signal of 1 or until the azimuths of all the reference points are fixed, that is, during the sampling of the received light signal. And steering control is performed according to the traveling direction. On the other hand, when the change in the traveling direction of the moving body 1 is small and the azimuth angles of all the reference points are fixed, the steering is performed based on the position and the traveling azimuth calculated by the first position detection unit 23. Control is performed. At the same time, the position and heading calculated by the second position detector 16 are updated with the position and heading calculated by the first position detector 23.

The switch SW3 is normally switched to the contact a side, and is switched to the contact b side only when the angular velocity detecting section 15 outputs a detection signal indicating that the angular velocity is "high". You may Further, instead of the angular velocity detection unit 15, the moving body 1 is at a predetermined position,
For example, a means for detecting that the vehicle has reached the turning start position at the end of the traveling course may be provided. Then, during this turning, the switch SW3 is switched to the contact b side. With this configuration, even when the first position detecting unit 23 is in a turning position where the position cannot be accurately detected, the inertial position detecting unit such as the gyro 11, that is, the second position detecting unit can accurately steer the moving body 1. You can The determination as to whether or not the moving body 1 has reached the turning start position can be made by comparing the position data detected by the first position detection unit 23 with the planned traveling course data.

Further, the position where the turning is completed can be detected depending on whether or not the azimuth angle of the specific reference point has reached the predetermined value. By the way, in the turning part where the traveling direction of the moving body changes rapidly, it is easy to lose sight of the reference point, and it may be difficult to specify the reference point. In the present embodiment, even in such a case, the reference point can be identified by the absolute azimuth angle, so that there is an advantage that the reference point can be easily identified.

[0035]

As is apparent from the above description, according to the first to fifth aspects of the invention, the moving body is steered based on the position data obtained by the second position detecting means. The second position detecting means has an advantage that position data can be continuously obtained, but has a drawback that an integration error occurs because position data is obtained by integrating displacement. Therefore,
The defect of the second position detecting means is compensated by sequentially correcting with the position data obtained by the first position detecting means which does not cause the accumulated error.

Particularly, according to the invention of claim 2, the position can be continuously detected by the second position detecting means even in the position detecting interval of the first position detecting means, and the detection result is the first position detecting means. It is corrected by the latest position data obtained in. As a result, the position can be accurately and continuously detected, and the guiding control of the moving body can be accurately performed even when the reference point is lost.

According to the inventions of claims 3 and 4,
When the traveling direction is largely changed, the correction operation based on the position data detected by the first position detecting means is prohibited, so that the position data is corrected only under the condition that high accuracy is obtained.

Further, according to the invention of claim 5, the identification of each reference point is performed by the absolute azimuth angle which has a small influence even when the traveling direction of the moving body is largely changed. Therefore, it is possible to accurately perform steering control along a predetermined course.

[Brief description of drawings]

FIG. 1 is a block diagram showing a main function of a control device according to an embodiment of the present invention.

FIG. 2 is a block diagram showing a configuration of a moving body according to an embodiment of the present invention.

FIG. 3 is a diagram showing a positional relationship with a reference point that accompanies turning of a moving body.

FIG. 4 is a diagram showing a relationship between an absolute azimuth angle, an azimuth angle, and a traveling azimuth.

FIG. 5 is a flowchart showing a position detection operation.

FIG. 6 is a flowchart showing a reference point identifying operation.

[Explanation of symbols]

2 ... Light beam scanning device, 11 ... Gyro, 12 ... Vehicle speed pulse generation device, 13 ... Yaw angle detection unit, 14 ... Yaw angle storage unit, 15 ... Angular velocity detection unit, 16 ... Second position detection device, 17 ... Azimuth angle Detection unit, 18 ... Absolute azimuth calculation unit, 19 ... Pole identification unit, 20 ... Absolute azimuth storage unit, 21 ... Azimuth storage unit, 23 ... First position detection device

Claims (5)

[Claims] 1. A light beam scanning means for circumferentially scanning a light beam with respect to at least three reference points around a moving body, and sequentially detecting reflected light of the light beam reflected from the reference points. And an azimuth angle detecting means for detecting the azimuth angle, a first position detecting means for detecting the position of the moving body based on the azimuth angle and position information of at least three reference points, and the movement by integrating displacements. Second position detecting means for continuously detecting the position of the body, steering means for steering the moving body based on the position of the moving body detected by the second position detecting means, and the second position detecting means A steering control device for a mobile body, comprising: a correction unit that sequentially corrects the position data of the mobile body detected in step 1 by the position data of the mobile body detected by the first position detection means. 2. Each time the correction means detects the position of the moving body by the first position detection means,
The steering control device for a moving body according to claim 1, wherein the position data of the moving body detected by the second position detecting means is updated with the detected position data. 3. The correcting means is configured to prohibit the position data updating operation by the first position detecting means when it is determined that the traveling azimuth of the moving body has changed more than a predetermined value. The steering control device for a moving body according to claim 1 or 2. 4. A turning detection means for detecting that the moving body has reached a predetermined turning start position and a turning end position, wherein the correcting means causes the turning detection means to turn the moving body to a turning start position. The position data updating operation by the first position detecting means is prohibited when it is determined that the position is reached, and the prohibition is released when it is determined that the turning end position is reached. Item 3. A steering control device for a moving body according to items 1 and 2. 5. The turning detection means compares an azimuth angle detected by the azimuth angle detection means and an absolute azimuth angle based on traveling azimuth data used for position detection by the second position detection means with a previously detected value. 5. The steering control apparatus for a moving body according to claim 1, further comprising a reference point identification means for identifying the reference point.