JP3291707B2 - Apparatus and method for guiding and guiding a mobile object - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a case where a load is loaded at a quarrying site, a material storage place, or the like, and is transported and discharged to a target object such as a dump truck at a discharge place. The present invention relates to a guided traveling control device for a traveling body such as a wheel loader for guiding a traveling body to a target object such as the dump truck.

[0002]

2. Description of the Related Art A loading transport vehicle 1 such as a wheel loader is
As shown in FIG. 4, after the load is scooped at a loading position (digging position) Q of a loading place (digging place) 30 such as a crushed stone site (face) and a material storage place (shed), a turning point 31 is set. (The position is indicated by R), returns to the dumping place 32, and the dump truck 50 stopped at the dumping place 32.
And the like, and discharges the cargo to the dump truck 50 by being positioned at the target position T of the target object. And it moves again from the earth removal place 32 to the loading place 30 via the turning point 31. In this way, the wheel loader 1 repeatedly performs the loading / transporting operation (so-called V-shape operation) in which the above-described operation is one cycle. here,
The traveling route of the wheel loader 1 is a scheduled traveling route 40.
The wheel loader 1 is guided by a so-called dead reckoning along a taught planned traveling route while measuring a current position and a current attitude angle of the wheel loader 1 and is a target object. The teaching target position T of the dump truck 50 is reached.

[0003] On the other hand, when the dump truck 50 is fully loaded (usually loaded in about three cycles), the dump truck 50 moves from the stop position to the load discharge place, and then returns to the discharge place 32 and stops again. . However, the dump truck 50
Does not always stop exactly at the same position, and the position varies every time the vehicle stops.

Therefore, the actual target position of the dump truck 50 is different from the teaching target position T taught first, and even if the wheel loader 1 is guided along the planned traveling route 40, the actual target position of the dump truck 50 is not changed. There has been a problem in that it may not be possible to reach an accurate target position corresponding to the actual stop position, and it may not be possible to accurately perform the earth discharging operation.

Therefore, as shown in Japanese Patent Application No. 8-243737 (title "Automated Excavation and Loading System"), the dump truck 50 is imaged by a camera mounted on the wheel loader 1, and the image is processed. In this way, an attempt is made to measure the actual stop position of the dump truck 50, and to make the wheel loader 1 reach an accurate target position corresponding to the actual stop position of the dump truck 50 according to the measurement result. It has been done.

[0006]

However, the method of measuring the position of the target object by such image processing is dependent on the environmental conditions (such as cloudy weather and direct sunlight) depending on the environmental conditions. May not be clear and the position measurement accuracy may be degraded due to this. Further, the imaging camera itself may be affected by environmental conditions (for example, when the camera receives direct sunlight), so that the position measurement accuracy may be deteriorated.

Further, such a position measuring system for a target object by image processing requires a camera and an image processing device, and thus has a problem that the configuration is complicated and the cost is increased.

Further, such equipment lacks reliability in a severe environment in which a construction machine is used. The present invention has been made in view of such circumstances, and improves the reliability by using equipment that has already been used even in a severe environment where construction machines are used, and achieves a simple system configuration. It is an object of the present invention to provide an apparatus and a method for accurately guiding a vehicle to an actual target position by always measuring the position of a target object accurately without being affected by environmental conditions. Is what you do.

[0009]

Therefore, according to a first aspect of the present invention, a planned traveling route to a target position of a target object is taught, and a plane on which the moving body moves is constituted by two coordinate axes. Indicated by a coordinate system, the two coordinate axes are set at the movement start position of the moving body, the current position of the moving body is measured as a coordinate position on the coordinate system, and one of the two coordinate axes is By guiding the moving body along the taught planned traveling path by measuring the current posture angle of the moving body with reference to the reference direction, guiding the moving body to reach the taught target position of the target object In the travel control device, a first light reflecting means and a second light reflecting means for reflecting light are disposed on the target object at a predetermined distance from each other, and the moving object is provided on the moving object. And light receiving means for receiving light emitted from the light emitting means and reflected by the first and second light reflecting means of the target object. A current position and a current attitude angle of the moving body at the time when the light reflected by the first light reflector is received by the light receiving means;
The current position and the current attitude angle of the moving body at the time when the light reflected by the light reflector is received, the direction in which light is projected from the moving body toward the target object, and the predetermined distance. And a target position measuring unit that measures an actual target position of the target object based on the posture angle of the target object with the one coordinate axis as a reference direction, and a target position measured by the target position measuring unit. Control means for correcting the teaching target position based on the actual target position of the target object, and further guiding the moving body to the corrected target position.

According to the structure of the first aspect of the invention, as shown in FIG. 5, the first light reflecting means 51R and the second light reflecting means 51F for reflecting light are separated from the target object 50 by a predetermined distance l1.
It is arranged only apart. On the other hand, a light projecting unit 29R for projecting light toward the target object 50 on the moving body 1, and a first and second light reflecting unit 51R of the target object 50, which are projected from the light projecting unit 29R, And light receiving means 19R for receiving the light reflected by 51F.

Then, the current position (X1, Y1) and the current attitude angle β1 of the moving body 1 at the time when the light L1 reflected by the first light reflecting means 51R is received by the light receiving means 19R and the light receiving means 19R The current position (X2, Y) of the moving body 1 at the time when the light L3 reflected by the second light reflecting means 51F is received.
2) and the current attitude angle β2, the direction α in which light is projected,
The actual target position T 'of the target object 50 is measured based on the predetermined distance l1 and the attitude angle γ of the target object 50.

The teaching target position T is corrected based on the measured actual target position T 'of the target object 50, and the moving body 1 is further guided to the corrected target position T'.

As described above, according to the first aspect of the present invention, even in a severe environment in which a construction machine is used, reliability is improved by using an optical device that has already been used, and a simple system configuration is realized. Therefore, it is possible to obtain the effect that the position of the target object is always accurately measured without being affected by the environmental conditions, and the vehicle is guided accurately to the actual target position.

According to a second aspect of the present invention, a planned traveling route to a target position of a target object is taught, and a plane on which the moving body moves is indicated by a coordinate system including two coordinate axes.
The two coordinate axes are set at the movement start position of the moving body, and the current position of the moving body is measured as a coordinate position on the coordinate system, and one of the two coordinate axes is set as a reference direction and By measuring the current posture angle of the moving body, the guided traveling control device of the moving body that guides the moving body along the taught scheduled traveling path and reaches the teaching target position of the target object, A first light reflecting means and a second light reflecting means for reflecting light are arranged on the target object at a predetermined distance from each other, and light is projected on the moving body toward the target object. Light emitting means for emitting light, light emitted from the first light emitting means,
A first light receiving unit that receives light of the target object reflected by the first light reflecting unit, and a direction different from a light emitting direction of the first light emitting unit toward the target object. A second light projecting means for projecting light to the first target, and a second light receiving means for receiving light emitted from the second light projecting means and reflected by the second light reflecting means of the target object Means, the current position and the current attitude angle of the moving body at the time when the light reflected by the first light reflecting means is received by the first light receiving means, and the second light receiving means A current position and a current attitude angle of the moving body at the time when the light reflected by the second light reflecting unit is received, and a direction in which light is projected from the moving body toward the target object, Based on the predetermined distance and a posture angle of the target object with the one coordinate axis as a reference direction. And correcting the teaching target position by a target position measuring means for measuring an actual target position of the target object and an actual target position of the target object measured by the target position measuring means. Control means for further guiding the moving body to the target position.

According to the configuration of the second aspect of the present invention, as shown in FIG. 7, a first light reflecting means 51R and a second light reflecting means 51F that reflect light are separated from a target object 50 by a predetermined distance l1.
It is arranged only apart. On the other hand, the first light projecting unit 29L that projects light toward the target object 50 on the moving body 1
And a first light receiving unit 19L that receives light emitted from the first light projecting unit 29L and reflected by the first light reflecting unit 51R of the target object 50, and toward the target object 50. A second light projecting means 29R for projecting light in a direction -α different from the light projecting direction α of the first light projecting means 29L, and a target object projected from the second light projecting means 29R. And a second light receiving means 19R for receiving the light reflected by the 50 second light reflecting means 51F.

The current position (X5, Y5) and the current attitude angle β3 of the moving body 1 at the time when the light L4 reflected by the first light reflecting means 51R is received by the first light receiving means 19L.
And the current position (X6, Y6) and current attitude angle β3 of the moving body 1 at the time when the light L6 reflected by the second light reflecting means 51F is received by the second light receiving means 19R, and the light is projected. The actual target position T ″ of the target object 50 is measured based on the directions α, −α, the predetermined distance l 1, and the attitude angle γ of the target object 50.

The teaching target position T is corrected based on the measured actual target position T 'of the target object 50, and the moving body 1 is further guided to the corrected target position T ".

As described above, according to the second aspect of the present invention, the reliability is improved by using an optical device that has already been used even in a severe environment in which a construction machine is used, similarly to the first aspect of the invention. With the simple system configuration, the cost can be reduced, and the position of the target object is always accurately measured without being affected by the environmental conditions, so that the vehicle can be guided accurately to the actual target position. Can be

According to a third aspect of the present invention, a predetermined traveling route to a target position of a target object is taught, and a plane on which the moving body moves is indicated by a coordinate system including two coordinate axes.
The two coordinate axes are set at the movement start position of the moving body, and the current position of the moving body is measured as a coordinate position on the coordinate system, and one of the two coordinate axes is set as a reference direction and By measuring the current posture angle of the moving body, the guided traveling control device of the moving body that guides the moving body along the taught scheduled traveling path and reaches the teaching target position of the target object, A first light reflecting means and a second light reflecting means for reflecting light are arranged on the target object at a predetermined distance from each other, and light is projected on the moving body toward the target object. Light emitting means for emitting light, light emitted from the first light emitting means,
A first light receiving unit that receives light of the target object reflected by the first light reflecting unit, and a direction different from a light emitting direction of the first light emitting unit toward the target object. A light projecting from the second light projecting means, and light reflected by the first light reflecting means or the second light reflecting means of the target object. And a second light receiving means for receiving the light reflected by one of the first and second light reflecting means by one of the first and second light receiving means. The current position and current attitude angle of the moving body at the time when the light is received, and the light reflected by the other light reflecting means different from the one light receiving means by the same light receiving means as the one light receiving means is received. The current position and current attitude angle of the moving body at the time of An actual target of the target object is determined based on a direction in which light is projected toward the target object, the predetermined distance, and an attitude angle of the target object with the one coordinate axis as a reference direction. A first target position measuring means for measuring a position, and the teaching target position is corrected by an actual target position of the target object measured by the first target position measuring means, and up to the corrected target position. First control means for further guiding the moving body to guide the vehicle,
After the moving body is further brought closer to the target object by the control means, one of the first and second light reflecting means is used by one of the first and second light receiving means. The current position and current attitude angle of the moving body at the time when the light reflected by the light reflecting means is received, and the other light receiving means different from the one light reflecting means in the other light receiving means different from the one light receiving means The current position and the current attitude angle of the moving body at the time when the light reflected by the light reflecting means is received, the direction in which light is projected from the moving body toward the target object, and the predetermined distance. Second target position measuring means for measuring an actual target position of the target object based on the attitude angle of the target object with the one coordinate axis as a reference direction; and the second target position measurement Said target object measured by means The teaching target position is corrected by the actual target position, so that comprises a second control means for further guided to travel the movable body until the corrected target position.

According to the third aspect of the present invention, as shown in FIGS. 5 and 7, the first light reflecting means 51R and the second light reflecting means 51F, which reflect light, are separated from the target object 50 by a predetermined distance. It is arranged at a distance of l1. On the other hand, the first light projecting unit 29 that projects light toward the target object 50 on the moving body 1
L and a first light receiving unit 19L that receives light emitted from the first light projecting unit 29L and reflected by the first light reflecting unit 51R or the second light reflecting unit 51L of the target object 50. A second light projecting means 29R for projecting light in a direction -α different from the light projecting direction α of the first light projecting means 29L toward the target object 50; and a second light projecting means. 29R
And the first light reflecting means 51 of the target object 50
R or second light receiving means 19R for receiving the light reflected by the second light reflecting means 51F.

At the time when the light L1 reflected by the one light reflecting means 51R of the first and second light reflecting means is received by one of the first and second light receiving means 19R. The current position (X1, Y1) and the current attitude angle β1 of the moving body 1 and the light L3 reflected by the other light reflecting means 51L different from the one light reflecting means 51R by the same light receiving means as the one light receiving means 19R. Is present position (X2, Y2) and the current attitude angle β2 of the moving body 1 at the time when the light is received, the direction α in which the light is projected, the predetermined distance l1, and the attitude angle γ of the target object 50. Based on this, the actual target position T 'of the target object 50 is measured.

The teaching target position T is corrected based on the measured actual target position T 'of the target object 50, and the mobile unit 1 is further guided to the corrected target position T'.

In this way, the moving body 1 is further moved to the target object 5.
After approaching 0, the light L4 reflected by one of the first and second light reflecting means 51R was received by one of the first and second light receiving means 19L. The current position (X5, Y5) and current attitude angle β3 of the moving body 1 at the time and the other light reflecting means 51F different from the one light reflecting means 51R in the other light receiving means 19R different from the one light receiving means 19L. The current position (X6, Y6) and the current attitude angle β3 of the moving body 1 at the time when the reflected light L6 is received, the directions α and -α at which the light is projected, the predetermined distance l1, the target object The actual target position T ″ of the target object 50 is measured based on the attitude angle γ of the target object 50.

Then, the teaching target position T is corrected based on the measured actual target position T "of the target object 50, and the mobile unit 1 is further guided to the corrected target position T".

According to the third invention, in addition to the effects of the first invention and the second invention, in the case where the moving body 1 turns toward the target object 50 from a distance, and when the moving body 1 further moves toward the target When the vehicle approaches the target object 50 and faces the target object 50 while facing the target object 50, appropriate position measurement is accurately performed in accordance with each case, and an effect that guidance traveling is performed accurately is obtained.

[0026]

Embodiments of the present invention will be described below with reference to the drawings.

FIG. 1 is a perspective perspective view of a wheel loader 1 which is a loading / transporting vehicle assumed in the present embodiment, and shows a side view of a dump truck 50 which is a target object.

As shown in FIG. 1, a wheel loader 1 (hereinafter referred to as a vehicle 1) is driven by an engine 25 and has a boom 23 and a bucket 22 as working machines.

The target engine speed is set by operating the accelerator pedal, and the fuel injection amount is controlled in accordance with an electric signal applied to the electronic control governor 11 to change the engine 25 speed. Engine rotation sensor 1
2 detects the actual rotation speed of the engine 25, and this detection signal is used as a feedback signal for controlling the engine rotation speed.

If the forward / reverse clutch is in a forward or reverse state (other than neutral), the power of the engine 25 is transmitted to the tire 26 via a torque converter, a transmission, a propeller shaft, and a differential gear.
The running speed of the vehicle 1 can be changed by changing the engine rotation.

The transmission solenoid valve 15 is a valve for changing the speed stage of the transmission according to the applied electric signal, and detects the value detected by the accelerator pedal angle sensor 13 for detecting the depression angle of the accelerator pedal and the operation of the brake pedal. Brake operation detection sensor 1 for detecting
Transmission solenoid valve 1 according to the detected value of
5 is changed.

The hydraulic pump is driven by the engine 25, and the discharge pressure oil of the hydraulic pump is supplied to the boom cylinder for driving the boom 23 and the bucket cylinder for driving the bucket 22 via the boom-bucket hydraulic proportional solenoid valve 6. , Boom 23 and bucket 22 are driven according to an electric signal applied to the electromagnetic proportional valve 6. A boom angle sensor 2 for detecting an angle of the boom 23 is provided on a boom foot pin portion of the boom 23,
A bucket angle sensor 3 that detects the angle of the bucket 22 is provided on a bucket mounting pin of the bucket 22.

The boom cylinder for driving the boom 23 is provided with a boom 2 by detecting the hydraulic pressure in the cylinder.
Boom cylinder oil pressure sensor 5 for detecting the load applied to 3
The bucket cylinder driving the bucket 22 is provided with a bucket cylinder oil pressure sensor 4 for detecting the load applied to the bucket 22 by detecting the oil pressure in the cylinder.

The hydraulic pressure discharged from the hydraulic pump is supplied to a steering cylinder for driving a steering 27 via a steering hydraulic electromagnetic proportional valve 8.
Are driven in accordance with an electric signal applied to the electromagnetic proportional valve 8.

The steering 27 is configured to be steered by bending the front and rear vehicle bodies of the articulated type vehicle 1.
A vehicle body refraction angle sensor 7 that detects a refraction angle of the front and rear vehicle bodies as a steering angle of the steering 27 is provided.

A target value of the brake pressure is set by operating the brake pedal, and the brake pressure is changed in accordance with an electric signal applied to the brake pneumatic proportional valve 10 to operate the brake. The brake is provided with a brake pressure sensor 9 for detecting a brake pressure, and this detection signal is used as a feedback signal for brake pressure control.

The tire 26 is provided with a tire rotation sensor 16 which is a vehicle running distance detecting unit.
Is detected. Further, an optical fiber gyro 17 as a vehicle azimuth detecting unit is provided in the vehicle body, and detects an angular velocity ω of the vehicle body posture angle.

The vehicle position is detected based on the outputs of the tire rotation sensor 16 and the optical fiber gyro 17 as will be described later. Since the vehicle position includes accumulated errors due to tire slip and the like, As will be described later, the accumulated error is intermittently corrected based on the relative positional relationship between the vehicle 1 and the reflection pole 18 intermittently arranged along the planned traveling path of the vehicle.

That is, the right and left sides of the vehicle body of the vehicle 1 are provided with laser light emitting / receiving sensors 19R and 19L, respectively, and the reflecting pole 18 is provided intermittently along the planned traveling path. As will be described later, the correction position of the vehicle 1 is obtained from the light projecting / receiving distance of the laser light between the laser light emitting / receiving sensors 19R and 19L and the reflector 18a of the reflection pole 18, and the above-described cumulative error is intermittently corrected.

The laser oscillators 29R, 29R for detecting the actual target position of the dump truck 50, which is the target object, and correcting the taught target position T are located at the same positions as the laser light emitting / receiving sensors 19R, 19L. 29L are provided. The laser oscillators 29R and 29L are disposed so that the optical axes L of the laser beams emitted from the laser oscillators cross each other and are projected toward the front of the vehicle 1, as shown in FIG. The right laser light emitting and receiving sensor 19R detects light emission and light reception of the right laser oscillator 29R, and the left laser light emitting and receiving sensor 19L detects light emission and light reception of the left laser oscillator 29L.

On the other hand, in front of and behind the side of the loading platform of the dump truck 50, the laser
The reflectors 51F and 51R that reflect the laser light L output from the R and 19L are arranged at a predetermined distance l1. The laser light L reflected by the reflectors 51F and 51R is detected by the light emitting and receiving sensors 19R and 19L of the vehicle 1.

Various switches and indicators, such as an automatic / manual changeover switch, are arranged in the operation switch / display unit 21. The automatic / manual changeover switch is a switch for selecting whether the traveling and the operation of the vehicle 1 are performed automatically or manually, and in the present embodiment, it is assumed that the vehicle 1 is switched to the automatic side.

The arithmetic processing unit 20 receives a detection signal from each sensor or the like and applies an electric signal as a control signal to each of the electromagnetic proportional valves and the like, and is composed mainly of a CPU and a memory. .

Hereinafter, the contents of the arithmetic processing executed by the arithmetic processing device 20 will be described.

When the detection signal of the tire rotation sensor 16 as the vehicle traveling distance detecting unit and the detection signal of the optical fiber gyro 17 as the vehicle azimuth detecting unit are input to the arithmetic processing unit 20, the following processing is sequentially performed. Be executed.

Calculation of the vehicle running distance S The tire rotation speed N is obtained based on the detection signal of the tire rotation sensor 16.

Next, the vehicle running distance S is calculated from the product of the tire rotational speed N and the known tire load radius r.

Calculation of the vehicle azimuth θ The vehicle azimuth change Δθ is calculated by integrating the angular velocity ω of the attitude angle of the vehicle body based on the detection signal of the optical fiber gyro 17.
Is calculated, and the azimuth change Δθ is added to the known initial azimuth to calculate the current vehicle azimuth θ with respect to the initial vehicle azimuth. Calculation of vehicle position (X, Y) Sine sin, cosine c of vehicle travel distance S and vehicle direction θ
The vehicle coordinate position (X, Y) on the XY coordinate system is obtained by integrating the product S · sin θ and S · cos θ with os.
Ask for.

That is, as shown in FIG. 2, the sequential vehicle position (X1, Y1) = (S1 · cos θ1, S1 · sin θ
1), (X2, Y2) = (X1 + S2 · cos θ2, Y1 + S2
· Sin θ2), ... are calculated, and the calculated trajectory 41 of the vehicle 1 is obtained.

However, since the vehicle position obtained by the integration includes the accumulated error due to the integration or the slip of the tire, the position is corrected as shown in FIG.

That is, as shown in FIG. 3, assuming that P1, P2, and P3 have been acquired as the successive calculation positions of the vehicle 1, a laser beam is emitted from the vehicle 1 at the position P3 and reflected. The reflected light is reflected by the reflector 18a of the pole 18-1 and the reflected light is received by the vehicle 1. From the output of the laser light emitting / receiving sensor 19 at this time, the vehicle 1 and the reflection pole 18-1
Is calculated, and when the vehicle 1 moves by a predetermined distance to reach the position P4, the vehicle 1
The relative distance between and the reflection pole 18-1 is determined. Therefore, the corrected position P'4 of the vehicle 1 is obtained based on the relative distance, the known arrangement position of the reflection pole 18-1 and the predetermined distance to which the vehicle 1 has moved. Then, the calculation position P4 including the accumulated error is replaced with the correction position P'4 to correct the accumulated error.

When the vehicle 1 arrives at the next reflection pole 18-2, the corrected position P'7 is similarly obtained, and the calculated position P7 including the accumulated error is replaced with the corrected position P'7. Correct the accumulated error. Thus, the actual trajectory 41 'of the vehicle 1 with the accumulated error corrected is obtained.

The arithmetic processing unit 20 compares the actual trajectory 41 ′ of the vehicle 1 calculated and corrected in this way with the planned traveling path 40 as the target path so that the vehicle 1 follows the planned traveling path 40. The vehicle 1 is controlled by so-called dead reckoning. That is, the arithmetic processing unit 20 outputs a required electric signal to the steering hydraulic solenoid proportional valve 8 so that the successive target vehicle position (X, Y) and the target vehicle direction θ on the scheduled traveling path 40 can be obtained. Then, the steering angle of the steering 27 is controlled. In addition, the arithmetic processing unit 20 includes a
A required electric signal is output to the electronic control governor 11, the transmission electromagnetic valve 15, and the brake pressure electromagnetic proportional valve 10 so that the above successive target vehicle position (X, Y) and target vehicle direction θ can be obtained. It controls the number of revolutions of the engine 25, the speed stage of the transmission, and the brake pressure. In this manner, the vehicle 1 is guided along the scheduled traveling path 40.

Here, the planned traveling route 40 assumed in this embodiment will be described.

The wheel loader 1, which is a loading and carrying vehicle,
As shown in FIG. 4, after digging at the loading (start) position Q of the loading site 30 to scoop up the load, the vehicle returns to the dumping site 32 via the turning point 31 and returns to the dumping site 32. After the cargo is discharged, the load carrying operation (so-called V-shape operation) of one cycle of moving from the unloading place 32 to the loading place 30 via the turning point 31 again is repeatedly performed. Here, the loading site 30 is assumed to be a face that is a crushed stone site, but may be a material storage site (shed).

A dump truck 50 from which the load of the wheel loader 1 is discharged to a carrier is stopped at a predetermined attitude angle γ at the discharge site 32.

The teaching data of the planned traveling path 40 for carrying out such loading / transporting operation is input to the arithmetic processing unit 20, and each point on the planned traveling path 40 (position Q to position R to position T) is The position P and the vehicle azimuth angle θ are taught.
At this time, the target position T of the earth removal site 32 is also taught.

The target position T is set at an intermediate position between the reflectors 51F and 51R on the side surface of the dump truck 50.

Further, teaching necessary for the automatic work is also provided.

For example, when the vehicle 1 is moved to the discharge place 32, the working machine posture is set to the posture (bucket height) at which the load is discharged, and the boom angle sensor 2 and the bucket The detection angle of the angle sensor 3 is input as teaching data.

As shown in FIG. 9, when the above-described teaching is completed by teaching the target position T of the dump truck 50 (step 101), the arithmetic processing unit 20
Based on the teaching data stored in the memory, the vehicle 1 is guided to travel along the scheduled travel path 40 by dead reckoning navigation, and one cycle (steps 102 to 111 to step 1) is performed.
02) V-shape operation (a series of operations such as loading (digging), transporting, and discharging) is repeatedly performed.

That is, the vehicle 1 has a loading site (excavation site) 3
0 to excavate and rake the cargo (Step 10
2) The vehicle travels on the scheduled traveling path 40 and reaches the turning point 31. The vehicle 1 is temporarily stopped at the turning point 31 and starts traveling along the path between the turning point 31 and the dumping place 32 toward the dump truck 50 in the dumping place 32 (Step 10).
3).

Hereinafter, a process of correcting the teaching target position T, which is the target point of the earth removal site 32, is executed, and the vehicle 1 is guided and controlled according to the correction result.

Here, the teaching target position T is corrected by
For the following reasons.

That is, the dump truck 50 is usually
In about three cycles, the cargo bed is fully loaded, the cargo is moved from a stop position to a cargo discharge place to discharge the loaded cargo, and then returned to the discharge place 32 and stopped again. However, the dump truck 50 is not always stopped exactly at the same position of the earth dumping place 32, and the position varies every time the vehicle stops, and a displacement occurs as shown by an arrow A in FIG. .

Therefore, the actual target position of the dump truck 50 is different from the initially taught target position T, and even if the vehicle 1 is guided along the scheduled traveling path 40, the actual target position of the dump truck 50 is not changed. May not be able to reach the exact target position according to the stop position of
This is because there is a problem that the discharging operation cannot be performed accurately.

When the vehicle 1 starts traveling toward the teaching target position T of the dump truck 50 with the turning point 31 as the start position R, as shown in FIG.
29R is operated, and laser light L is emitted forward of the vehicle.

Here, as shown in FIG. 5, the position R when the vehicle 1 is once stopped at the turning point 31, that is, the position 1a indicating the vehicle 1 is defined as the origin, and the center axis C of the vehicle body is defined as Y.
An XY coordinate system is defined in which the X-axis is the left-right axis of the vehicle body passing through the axis and the origin. The laser oscillator 29L on the left side of the vehicle body 1a (origin) is such that the emitted laser beam L is directed toward the inside of the vehicle body by −α (to the right) with respect to the center C of the vehicle body.
And -ΔX and ΔY, and
The laser oscillator 29R on the right side of the vehicle body emits the laser light L
Is located at a position shifted from the vehicle position 1a (origin) by ΔX and ΔY so as to face the vehicle center by + α (to the left) with respect to the vehicle center C.

First Position Measurement FIG. 5 shows the geometrical relationship immediately after the vehicle 1 starts running from the start position R.
It is taking a posture approaching while turning left from a distance.

As described above, when the vehicle 1 is taking a posture approaching while turning left from a distance toward the teaching target position T of the dump truck 50, one of the right and left laser oscillators 29L and 29R is turned right. The process of measuring the actual target position T 'of the dump truck 50 using the laser light emitted from the laser oscillator 29R is performed as follows.

Here, various symbols are defined as follows.

11: distance between the reflectors 51F and 51R before and after the dump truck 50 (known) γ: stopping direction of the dump truck 50 (direction of entry into the dumping place)
X1, Y1: vehicle coordinate position at the time when the right laser light emitting / receiving sensor 19R detects the reception of the reflected light by the reflector 51R behind the dump truck 50. β1: vehicle azimuth at the same detection time. θ (angle formed with the X axis) X′1, Y′1: right laser light emitting / receiving sensor 19 at the same detection time
R coordinate position X2, Y2: vehicle coordinate position at the time when the right laser light emitting / receiving sensor 19R detects the reception of the reflected light by the reflector 51F in front of the dump truck 50 β2: vehicle azimuth θ at the same detection time (with the X axis X'2, Y'2: right laser light emitting / receiving sensor 19 at the same detection point
R coordinate position X3, Y3: reflector 51F in front of dump truck 50
X4, Y4: reflectors 51 before and after the dump truck 50
F, the coordinate position of the midpoint of the line segment connecting 51R (the actual target position T 'of the dump truck 50) L1: the rear reflector 5 with the right laser light emitting / receiving sensor 19R
A straight line L2 indicating the laser beam axis at the time of detecting the reflected light by 1R. L2: A straight line obtained by translating the straight line L1 so as to pass through the coordinate position (X3, Y3) of the front reflector 51F. With front reflector 5
When a straight line indicating the laser optical axis at the time of detecting the reflected light by 1F is used, as is apparent from FIG. The reflector 51 emitted from the laser oscillator 29R by the sensor 19R and located behind the dump truck 50
The reception of the laser beam L1 reflected by R is performed by using the coordinate position (X'1, Y'1) of the laser light emitting / receiving sensor 19R and the attitude angle β1 +
Since it is detected by α, the equation of a straight line L1 indicating this laser optical axis is obtained as a straight line passing through this position and having this inclination.

Y = {tan (β1 + α)} X− {tan (β1 + α)} X1 ′ + Y1 ′ (1) The coordinate position of the front reflector 51F is obtained by subtracting the coordinate position of the rear reflector 51R in the X direction by − l 1 · cos γ Since it is at a position translated in the Y direction by −l 1 · sin γ (2), the rear reflector 51
A straight line L2 obtained by translating the straight line L1 of the above equation (1) passing through R so as to pass through the front reflector 51F is obtained as follows.

Y = {tan (β1 + α)} X− {tan (β1 + α)} (X1′−11 · cosγ) + Y1′−11 · sinγ (3) The vehicle 1 further turns left and the vehicle position 1a is coordinated. Position (X
2, Y2) (vehicle azimuth angle β2), the laser light is emitted from the laser oscillator 29R by the same laser light emitting / receiving sensor 19R on the right side as the sensor that first detected the reflected laser light L1, and is reflected by the reflector 51F in front of the dump truck 50. The reflected light beam L3 is received by the laser
Since the detection is performed at the coordinate position (X'2, Y'2) of R and the attitude angle β2 + α, a straight line L3 indicating the laser optical axis is obtained as a straight line passing through this position and having this inclination.

Y = {tan (β2 + α)} X− {tan (β2 + α)} X2 ′ + Y2 ′ (4) The coordinate position (X3, Y3) of the front reflector 51F is obtained as the intersection of the straight lines L2, L3. , The above equation (3),
From equation (4), Becomes

The coordinate position (X 4, Y 4) of the midpoint of the line connecting the front and rear reflectors 51 F, 51 R, that is, the actual target position T ′ (X 4, Y 4) of the dump truck 50 is obtained in the above (5).
The coordinate position (X3, Y3) obtained by the equation and (2)
X4 = X3 + (11 · cosγ) / 2 Y4 = Y3 + (11 · sinγ) / 2 (6)

Here, the relationship between the vehicle position 1a (X1, Y1) when the reflected laser beam L1 is first received and the coordinate position (X'1, Y'1) of the laser light emitting / receiving sensor 19R is shown in FIG. From the geometric relationship shown in FIG. 6, X'1 = X1 + .DELTA.Xcos (.beta.1-90.degree.)-. DELTA.Ysin (.beta.1-90.degree.) = X1 + .DELTA.Xsin.beta.1 + .DELTA.Ycos.beta. ΔXcosβ1 + ΔYsinβ1 (7)

Similarly, the relationship between the vehicle position 1a (X2, Y2) at the time when the reflected laser beam L3 is received next and the coordinate position (X'2, Y'2) of the laser light emitting / receiving sensor 19R is expressed by the following expression. '2 = X2 + ΔXcos (β2-90 °) -ΔYsin (β2-90 °) = X2 + ΔXsinβ2 + ΔYcosβ2 Y′2 = Y2 + ΔXsin (β2-90 °) + ΔYcos (β2-90 °) = Y2-ΔXcosβ2 + ΔYsin2 is obtained.

Therefore, by substituting the above equations (7) and (8) into the above equation (5), the coordinate position (X3, Y3) of the front reflector 51F is obtained, and further substituted into the equation (6). Thus, the actual target position T '(X
4, Y4) is obtained.

As described above, this first position measurement is performed by the right reflector 51R using the right laser projection / reception sensor 19R.
The current position (X1, Y1) and the current attitude angle β1 of the vehicle 1 when the reception of the laser beam L1 reflected by R is detected.
(The vehicle position and attitude angle are sequentially calculated based on the detection signal of the tire rotation sensor 16 and the detection signal of the optical fiber gyro 17 as described above), and are reflected by the front reflector 51F by the right laser light emitting / receiving sensor 19R. Current position (X2, Y2) and current attitude angle β2 of the vehicle 1 at the time when the reception of the received laser beam L3 is detected, a known laser beam projecting direction α, a known predetermined distance l1, a dump truck 50 Is performed as a process of calculating the actual target position T 'of the dump truck 50 based on the known attitude angle γ of the dump truck (determination YES in step 104).

Then, the measured dump truck 5
The teaching target position T is corrected by the actual target position T ′ of 0 (the taught planned traveling path 40 is corrected), and the vehicle 1 is further guided to the corrected target position T ′ by dead reckoning navigation (step). 105).

On the other hand, if the first position measurement cannot be performed because the reflected laser light is not received (NO in step 104), the teaching target The vehicle 1 is further guided to the position T by dead reckoning navigation (step 106).

Second Position Measurement The first position measurement is a position measurement at a position distant from the dump truck 50.
When the vehicle approaches and comes to a position facing the dump truck 50, a second position measurement is performed to finely adjust the stop position of the vehicle 1. FIG. 7 shows a geometric relationship when the vehicle 1 further approaches the dump truck 50 from the time of the first position measurement and comes to a position directly facing the dump truck 50.

At this time, the vehicle 1 is shifted to the left with respect to the planned traveling path 40 by the shift amount SD (41 'indicates the actual trajectory of the vehicle 1). The process of measuring the actual target position T ″ of the dump truck 50 using the laser beams emitted from both the left and right laser oscillators 29L and 29R is performed as follows.

Here, various symbols are defined as follows.

X5, Y5: Left laser light emitting / receiving sensor 19L
X′1, Y′1: the left laser light emitting / receiving sensor 19 at the time of detecting the reception of the reflected light by the reflector 51R behind the dump truck 50
L coordinate position X6, Y6: Vehicle coordinate position at the time when the right laser projection / reception sensor 19R detects the reception of reflected light by the reflector 51F in front of the dump truck 50. X'6, Y'6: Right laser at the same detection time. Light emitting / receiving sensor 19
R coordinate position β3: vehicle azimuth angle θ (angle formed with X axis) at the above positions (X5, Y5), (X6, Y6) X7, Y7: reflector 51F in front of dump truck 50
X8, Y8: reflectors 51 before and after the dump truck 50
F4, the coordinate position of the midpoint of the line segment connecting 51R (the actual target position T ″ of the dump truck 50) L4: the rear reflector 5 with the left laser light emitting / receiving sensor 19L
A straight line L5 indicating the laser beam axis at the time of detecting the reflected light by 1R. L5: A straight line obtained by translating the straight line L4 so as to pass through the coordinate position (X7, Y7) of the front reflector 51F. L6: Right laser beam sensor 19R With front reflector 5
A straight line indicating the laser optical axis at the time when the reflected light by 1F is detected SF: Forward distance from current vehicle position 1a (X6, Y6) to actual target position T ″ SD: Schedule of current vehicle position 1a (X6, Y6) As is apparent from FIG. 7, when the vehicle position 1a is at the coordinate position (X5, Y5) (vehicle azimuth angle β3), the laser beam is emitted from the laser oscillator 29L by the left laser light emitting / receiving sensor 19L. And the reflector 51 behind the dump truck 50
The reception of the laser beam L4 reflected by R is performed by using the coordinate position (X'5, Y'5) of the laser projection / reception sensor 19L and the attitude angle β3−
Since it is detected by α, the equation of a straight line L4 indicating this laser optical axis is obtained as a straight line passing through this position and having this inclination.

Y = {tan (β3−α)} X− {tan (β3−α)} X5 ′ + Y5 ′ (9) The coordinate position of the front reflector 51F is the coordinate position of the rear reflector 51R. As described above, the rear reflector 51 is located at a position translated in the X direction by −11 · cosγ in the Y direction by −11 · sinγ (2).
A straight line L5 obtained by translating the straight line L4 of the above equation (9) passing through R so as to pass through the front reflector 51F is obtained as follows.

Y = {tan (β3−α)} X− {tan (β3−α)} (X5′−11 · cosγ) + Y5′−11 · sinγ (10) The vehicle 1 proceeds further and the vehicle Position 1a is the coordinate position (X
6, Y6) (the same vehicle azimuth β3), the laser is emitted from the laser oscillator 29R by the right laser projection / reception sensor 19R different from the sensor that first detected the reflected laser beam L4, and the reflector in front of the dump truck 50 is provided. 51F
Receiving the laser beam L6 reflected at the coordinate position (X'6, Y'6) of the laser projection / reception sensor 19R and the attitude angle β3 + α
Therefore, the equation of a straight line L6 indicating the laser optical axis is obtained as a straight line passing through this position and having this inclination.

Y = {tan (β3 + α)} X− {tan (β3 + α)} X6 ′ + Y6 ′ (11) The coordinate position (X7, Y7) of the front reflector 51F is obtained as the intersection of the straight lines L5 and L6. Therefore, the above (1)
From equations (0) and (11), Becomes

The dump truck 5, which is the coordinate position (X8, Y8) of the midpoint of the line connecting the front and rear reflectors 51F and 51R.
The actual target position T ″ of 0 is obtained by using the coordinate position (X7, Y7) obtained by the above equation (12) and the translation amount of the above equation (2), X8 = X7 + (11 · cosγ) / 2 Y8 = Y7 + (l1 · sinγ) / 2 (13)

Here, the relationship between the vehicle position 1a (X5, Y5) at the time when the reflected laser beam L4 is first received and the coordinate position (X'5, Y'5) of the laser projection / reception sensor 19L is as described above. X'5 = X5-.DELTA.Xsin.beta.3-.DELTA.Ycos.beta.3 Y'5 = Y5-.DELTA.Xcos.beta.3 + .DELTA.Ysin.beta.3 (14) in the same manner as in the equations (7) and (8).

Similarly, the relationship between the vehicle position 1a (X6, Y6) at the time when the reflected laser beam L6 is received next and the coordinate position (X'6, Y'6) of the laser light emitting / receiving sensor 19R is expressed by the following expression. '6 = X6 + ΔXsinβ3 + ΔYcosβ3 Y'6 = Y6−ΔXcosβ3 + ΔYsinβ3 (15)

Therefore, by substituting the above equations (14) and (15) into the above equation (12), the coordinate position (X7, Y7) of the front reflector 51F is obtained, and further substituted into the equation (13). Thus, the actual target position T ″ (X8, Y8) of the dump truck 50 is obtained.

As described above, the second position measurement is performed by the left reflector 51 using the left laser light emitting / receiving sensor 19L.
The current position (X5, Y5) and the current attitude angle β3 of the vehicle 1 at the time when the reception of the laser beam L4 reflected by R is detected.
(The vehicle position and attitude angle are sequentially calculated based on the detection signal of the tire rotation sensor 16 and the detection signal of the optical fiber gyro 17 as described above), and the other right laser which is different from the left laser projection / reception sensor 19L. Light emitting / receiving sensor 19
The current position (X6, Y6) of the vehicle 1 when the reception of the laser beam L6 reflected by the front reflector 51F at R is detected.
And the current attitude angle β3 and the projection directions α and −α of the laser beam.
The actual target position T ″ of the dump truck 50 is measured based on the known predetermined distance l 1 and the known attitude angle γ of the dump truck 50.

Then, as shown in FIG.
a (X6, Y6) the lateral deviation amount SD with respect to the scheduled traveling path 40
Is the measured actual target position T ″ (X8, Y8)
And the current vehicle position (X6, Y6) and current attitude angle β3 sequentially calculated based on the detection signal of the tire rotation sensor 16 and the detection signal of the optical fiber gyro 17,
It is obtained as in equation (6).

[0096] Then, the forward distance SF from the current vehicle position 1a (X6, Y6) to the actual target position T "is determined by the measured actual target position T" (X8, Y8) and the sequentially calculated current vehicle position. (X6, Y6) and the lateral shift amount SD are obtained as in the following equation (17).

[0097] Then, the teaching target position T is corrected based on the measured actual target position T ″ of the dump truck 50, and the vehicle 1 is further guided to the corrected target position T ″.

That is, while the vehicle 1 is moving forward by the forward distance S1, the vehicle 1 is guided so that the lateral shift amount S2 becomes zero (step 109), and the vehicle 1 is stopped at the actual target position T ″ (step 109). Step 110).

On the other hand, if the second position measurement cannot be performed because the reflected laser light is not received (NO in step 107), the vehicle 1 is stopped without traveling. (Step 108).

Thus, the dump truck 50 of the dumping place 32
The vehicle 1 having arrived at the loading position T ″ performs an automatic unloading operation. In this case, the unloading operation is performed while confirming the position of the working machine based on the detection angles of the boom angle sensor 2 and the bucket angle sensor 3. It runs automatically.

That is, when the vehicle arrives at the discharge site 32,
Since the working machine of the vehicle 1 is in the working machine posture during automatic traveling taught during teaching, the working machine posture is changed from this posture to the working machine posture during unloading taught during teaching. When the bucket 22 is raised to the bucket height taught at the time of discharging as described above, the load (earth and sand) is discharged at that height, and then the posture of the work machine is changed to the position at the time of automatic traveling taught at the time of teaching. The vehicle is returned to the posture, travels to the loading site (excavation site) 30 in that posture, and the same processing is executed again (steps 102 to 102).

However, since the dump truck 50 remains stopped at the same position until the cargo bed is fully loaded by three loading operations in three cycles, the first position measurement in the first cycle and the second position measurement in the first cycle Position measurement (Step 10
4, the second cycle after step 107) is performed,
In the cycle, the first position measurement and the second position measurement (steps 104 and 107) are not performed in an overlapping manner, and the vehicle 1 is guided and driven based on the position measurement result in the first cycle (step 104). 105, step 10
9). As described above, according to the present embodiment, the vehicle 1
When the vehicle 1 turns toward the dump truck 50 from a distance (first position measurement), and when the vehicle 1 further approaches the dump truck 50 and heads directly facing the dump truck 50 (second time). The position measurement is appropriately performed in accordance with each of them, and the effect that the guided traveling is accurately performed can be obtained. Note that γ
Can use not the actual stop posture angle of the dump truck but the value (known) of the target stop posture angle. This is because even if the actual stop posture angle of the dump truck 50 slightly deviates from the preset value γ, the accuracy of the position measurement is not significantly affected.

Note that the actual target position T 'is obtained based on the first position measurement, and the actual target position T' obtained as a result of the first position measurement without performing the second position measurement. It is also possible to guide the vehicle 1 to reach '.

Further, the actual target position T ″ is obtained based on the second position measurement without performing the first position measurement, and the vehicle 1 is guided to travel to reach the actual target position T ″. Implementation is also possible.

In the present embodiment, it is assumed that the vehicle 1 approaches the dump truck 50 while turning left as shown in FIG. 5 for the first position measurement. Can also perform position measurement by the same method.

In this embodiment, it is assumed that the vehicle 1 is shifted to the left with respect to the scheduled traveling path 40 as shown in FIG. In this case, the position can be measured by the same method.

Further, in this embodiment, a loading / transporting vehicle such as a wheel loader is assumed as a moving object, and a dump truck is assumed as a target object. However, the present invention is not limited to this. The type of the object is arbitrary.

[Brief description of the drawings]

FIG. 1 is a perspective perspective view of a wheel loader, which is a loading transport vehicle assumed in an embodiment of the present invention, and a side view of a dump truck, which is a target object.

FIG. 2 is a diagram illustrating a sequential position and azimuth calculation process of a vehicle;

FIG. 3 is a diagram showing how a position of a vehicle guided and guided by dead reckoning navigation is intermittently corrected.

FIG. 4 is a diagram illustrating each route constituting a planned traveling route of the present embodiment.

FIG. 5 is a geometric relationship diagram for explaining a first position measurement of the embodiment.

FIG. 6 is a geometric relationship diagram showing a positional relationship between a laser light emitting / receiving sensor and a vehicle position.

FIG. 7 is a geometric relationship diagram illustrating a second position measurement according to the present embodiment.

FIG. 8 is a view for explaining guided traveling control based on a second position measurement.

FIG. 9 is a flowchart illustrating a procedure of a process executed by the arithmetic processing device illustrated in FIG. 1;

[Explanation of symbols]

 Reference Signs List 1 vehicle (wheel loader) 20 arithmetic processing unit 50 dump truck 19R, 19L laser light emitting / receiving sensor 29R, 29L laser oscillator 51F, 51R reflector

──────────────────────────────────────────────────続 き Continuation of front page (72) Inventor Shinji Takasugi 1144-1 Shinomiya, Hiratsuka-shi, Kanagawa Prefecture Komatsu Engineering Co., Ltd. (56) References JP-A-62-182616 (JP, A) JP-A-1-145705 (JP, a) JP flat 5-127739 (JP, a) JP flat 6-83443 (JP, a) JP flat 6-138939 (JP, a) (58 ) investigated the field (Int.Cl. ⁷ , DB name) G05D 1/02

Claims (6)

(57) [Claims] An instructor teaches a planned traveling route to a target position of a target object, shows a plane on which a moving body moves by a coordinate system including two coordinate axes, and indicates the two coordinate axes by a movement start position of the moving body. By setting and measuring the current position of the moving body as a coordinate position on the coordinate system, and measuring the current posture angle of the moving body with one of the two coordinate axes as a reference direction, A guided traveling control method for a moving body that guides the moving body along the taught planned traveling route and reaches a teaching target position of the target object, wherein light is directed from the moving body toward the target object. By projecting the light, the process of receiving the light reflected by the first light reflector of the target object with the moving object, and at the time when the moving object further approaches the target object,
Projecting light from the moving body toward the target object to receive light reflected by a second light reflector of the target object by the moving body; and the first light The current position and the current attitude angle of the moving body at the time when the light reflected by the reflector is received, and the current position and the current position of the moving body at the time when the light reflected by the second light reflector is received A current attitude angle, a direction in which light is projected from the moving body toward the target object, an arrangement distance between the first light reflector and the second light reflector,
A process of measuring an actual target position of the target object based on an attitude angle of the target object with two coordinate axes as reference directions, and the teaching by the measured actual target position of the target object. A step of correcting a target position and further guiding the moving body to the corrected target position. 2. A method for teaching a planned traveling route to a target position of a target object, indicating a plane on which the moving body moves by a coordinate system including two coordinate axes, and indicating the two coordinate axes by a movement start position of the moving body. By setting and measuring the current position of the moving body as a coordinate position on the coordinate system, and measuring the current posture angle of the moving body with one of the two coordinate axes as a reference direction, A guided traveling control device for a moving body that guides a moving body along the taught planned traveling route and reaches a teaching target position of the target object, wherein a first object that reflects light to the target object is provided. A light reflecting means and a second light reflecting means are arranged at a predetermined distance from each other, and a light projecting means for projecting light toward the target object; Be light Wherein the target object first
And light receiving means for receiving light reflected by the second light reflecting means, and the current position of the moving body at the time when the light reflected by the first light reflecting body is received by the light receiving means and A current attitude angle, a current position and a current attitude angle of the moving body at the time when the light reflected by the second light reflector is received by the light receiving means, and a direction from the moving body to the target object. Based on a direction in which light is projected, the predetermined distance, and a posture angle of the target object with the one coordinate axis as a reference direction,
A target position measuring means for measuring an actual target position of the target object; and correcting the teaching target position by an actual target position of the target object measured by the target position measuring means, wherein the corrected target And a control means for further guiding the moving body to a position. 3. A method for teaching a planned traveling route to a target position of a target object, indicating a plane on which a moving body moves by a coordinate system including two coordinate axes, and indicating the two coordinate axes by a movement start position of the moving body. By setting and measuring the current position of the moving body as a coordinate position on the coordinate system, and measuring the current posture angle of the moving body with one of the two coordinate axes as a reference direction, A guided traveling control device for a moving body that guides a moving body along the taught planned traveling route and reaches a teaching target position of the target object, wherein a first object that reflects light to the target object is provided. A light projecting unit that arranges the light reflecting unit and the second light reflecting unit at a predetermined distance from each other, and projects light toward the target object on the moving body; and the first light projecting unit. Throw from means And a first light receiving unit that receives light of the target object reflected by the first light reflecting unit, and a light emitting direction of the first light emitting unit toward the target object. A second light projecting means for projecting light in different directions; a second light projecting from the second light projecting means for receiving light reflected by the second light reflecting means of the target object A current position and a current attitude angle of the moving body at a time when the light reflected by the first light reflecting means is received by the first light receiving means, and a second light receiving means. A current position and a current attitude angle of the moving body at the time when the light reflected by the second light reflecting means is received by the means, and a direction in which light is projected from the moving body toward the target object. And the predetermined distance, and the attitude of the target object with the one coordinate axis as a reference direction. Based on the target position measuring means for measuring the actual target position of the target object, the teaching target position is corrected by the actual target position of the target object measured by the target position measuring means, And a control unit for further guiding the moving body to the corrected target position. 4. The method according to claim 1, wherein the control unit is configured to perform a lateral shift with respect to the planned traveling route based on the correction target position, a current position and a current attitude angle of the moving body, and advance from the current position of the moving body to the correction target position. The guided travel control device for a mobile body according to claim 3, wherein the distance is obtained, and the mobile body is guided to travel such that the lateral displacement becomes zero while the vehicle travels by the forward distance. 5. A method for teaching a planned traveling route to a target position of a target object, indicating a plane on which the moving body moves by a coordinate system including two coordinate axes, and indicating the two coordinate axes by a movement start position of the moving body. By setting and measuring the current position of the moving body as a coordinate position on the coordinate system, and measuring the current posture angle of the moving body with one of the two coordinate axes as a reference direction, A guided traveling control device for a moving body that guides a moving body along the taught planned traveling route and reaches a teaching target position of the target object, wherein a first object that reflects light to the target object is provided. A light projecting unit that arranges the light reflecting unit and the second light reflecting unit at a predetermined distance from each other, and projects light toward the target object on the moving body; and the first light projecting unit. Throw from means And a first light receiving unit that receives light of the target object reflected by the first light reflecting unit, and a light emitting direction of the first light emitting unit toward the target object. Second light projecting means for projecting light in different directions; light projected from the second light projecting means, and reflected by the first light reflecting means or second light reflecting means of the target object And second light receiving means for receiving the reflected light, wherein the light is reflected by one of the first and second light reflecting means at one of the first and second light receiving means. The current position and the current attitude angle of the moving body at the time the received light is received, and the light reflected by the other light reflecting means different from the one light receiving means by the same light receiving means as the one light receiving means The current position and the current attitude angle of the moving body at the time when The direction in which light is projected from the moving body toward the target object, and the predetermined distance,
First target position measuring means for measuring an actual target position of the target object based on the attitude angle of the target object with the one coordinate axis as a reference direction; and the first target position measuring means. A first control unit that corrects the teaching target position based on an actual target position of the target object measured in step 1, and further guides the moving body to the corrected target position; and the first control unit. Then, after the moving body is further approached to the target object, one of the first and second light reflecting means is one of the first and second light reflecting means by one of the first and second light receiving means. The current position and the current attitude angle of the moving body at the time when the light reflected at is received, and the other light reflecting means different from the one light reflecting means in the other light receiving means different from the one light receiving means Reflected by The current position and the current attitude angle of the moving body at the time when the light is received, the direction in which light is projected from the moving body toward the target object, the predetermined distance, and the one coordinate axis. A second target position measuring unit that measures an actual target position of the target object based on the orientation angle of the target object as a direction; and the target measured by the second target position measuring unit. A second control means for correcting the teaching target position based on the actual target position of the target object and further guiding the moving body to the corrected target position; 6. The lateral shift with respect to the planned traveling route based on a corrected target position based on the measurement by the second target position measuring unit, and a current position and a current attitude angle of the moving body. And a forward distance from the moving body current position to the correction target position is obtained,
The guided travel control device for a mobile body according to claim 5, wherein the guided travel of the mobile body is performed such that the lateral displacement becomes zero while the vehicle travels for the forward distance.