JP3191957B2 - Guided travel control device for unmanned vehicles - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for guiding an unmanned vehicle along a guide line.

[0002]

2. Description of the Related Art Conventionally, there has been known a method of automatically guiding an unmanned vehicle along a guidance line indicating a planned traveling path (in this case, "guidance" is used to mean following a line). For example, in an automatic guided vehicle used in factories, etc., an induction cable (in this case, "induction" is used to mean electromagnetic induction) that generates an induction magnetic field is laid on the floor surface. 1 arranged on the left and right sides of the body of the carrier
A magnetic field generated from the induction cable is detected by a pair of pickup coils, and a lateral displacement of the carrier with respect to the induction cable is determined from a deviation between detection values (induced voltages) of the two coils. The vehicle is steered and guided along the planned traveling path.

Regarding this type of technology, the present applicant has proposed a configuration in which two sets of the above-mentioned pair of pickup coils are provided, one set each before and after the body of an unmanned traveling vehicle. Japanese Unexamined Patent Publication No. 54-44180) calculates the attitude angle of the unmanned vehicle with respect to the guide cable from the difference between the deviation of the detection values of both coils at the front of the vehicle body and the deviation of the detection values of both coils at the rear of the vehicle body. The steering of the unmanned traveling vehicle is controlled based on this.

[0004]

However, the target vehicles to which the above-mentioned guidance traveling technology is applied are limited to vehicles having a body integrally formed, which is a so-called articulated (joint) type vehicle, That is, there is no example in which the present invention is applied to a vehicle in which a front vehicle body is rotatably connected to a rear vehicle body in a horizontal direction.

[0005] Even if the above-described prior art is applied to an articulated vehicle as it is, accurate guided driving cannot be expected due to the characteristic that the front and rear sides of the vehicle body are bent.

[0006] Moreover, as articulated vehicles, construction machines such as wheel loaders having a digging machine on the front vehicle body are mainly used. Unlike a construction machine guided by an unmanned guided vehicle traveling on a flat floor, the construction machine rides on earth and sand to excavate earth and sand and travels in a state where the vehicle body is inclined. For this reason, the relative vertical distance between the induction cable and the pickup coil changes according to the degree of inclination due to the earth and sand, thereby causing an error in the detected value of the coil. Therefore, it cannot be expected that the guided traveling is performed accurately.

SUMMARY OF THE INVENTION The present invention has been made in view of the above circumstances, and a first object of the present invention is to enable accurate guidance when an articulated type unmanned vehicle is guided along a guide line. It is a second object of the present invention to enable accurate guidance when an unmanned vehicle travels on a road surface that is rich in inclination.

[0008]

Therefore, according to a first aspect of the present invention, a guide line is laid along a planned traveling path, and a lateral displacement with respect to the guide line is detected on the left and right sides of an unmanned vehicle body. A pair of displacement detecting means is provided, and the unmanned vehicle is steered based on a deviation between the two detection values of the pair of displacement detecting means to guide the unmanned vehicle along the planned traveling path. In the guided traveling control device for an unmanned vehicle, the unmanned vehicle is a steered vehicle in which a front body and a rear body are connected to each other so as to be rotatable in a horizontal direction, and the pair of displacement detection means Angle detecting means disposed at a front portion and a rear portion of the front vehicle body and at a front portion and a rear portion of the rear vehicle body, respectively, and detecting an angle formed by a longitudinal direction of the front vehicle body with respect to a longitudinal direction of the rear vehicle body. And the angle detection hand The two pairs of displacement detecting means are selected from the pair of displacement detecting means in accordance with the detected value of the pair, and based on the respective deviations of the selected two pairs of displacement detecting means, Calculating means for calculating a shift of the unmanned vehicle with respect to the guidance line, and steering control of the unmanned vehicle is performed so that the shift calculated by the calculating means is eliminated.

According to a second aspect of the present invention, there is provided a similar unmanned vehicle guidance / running control device, wherein the unmanned vehicle detects front-rear inclination angle detecting means for detecting a front-rear inclination angle of the vehicle or a left-right inclination angle. When the unmanned vehicle travels on a slope, the pair of left and right inclination angles is detected based on the detection value of the front / rear inclination angle detection unit and the position of the pair of displacement detection units in the front-rear direction of the vehicle. The two detection values of the pair of detection means are corrected or calculated based on the detection value of the left and right inclination angle detection means and the position of the pair of displacement detection means in the lateral direction of the vehicle body. The detection value is corrected or calculated, and the unmanned vehicle is subjected to steering control based on the difference between the corrected and calculated detection values.

[0010]

According to the structure of the first aspect of the invention, one set is provided for each of the front and rear portions of the front vehicle body and the front and rear portions of the rear vehicle body in accordance with the magnitude of the rotational displacement with respect to the rear vehicle body. Optimum 2 for calculating the displacement of the unmanned vehicle with respect to the guide line from among the four pairs of displacement detection means provided.
A pair of displacement detection means is selected. Thus, the deviation of the unmanned vehicle with respect to the guide line is accurately calculated based on the respective deviations of the selected two pairs of displacement detection means,
The steering of the unmanned vehicle is accurately controlled based on the calculated shift, and the unmanned vehicle is guided with high precision along the guide line indicating the planned traveling path.

Further, according to the second aspect of the present invention, even if the unmanned vehicle is traveling on a slope inclined forward and backward, the detection value of the front-rear inclination angle detecting means and the pair of displacement detecting means are provided. The two detected values of the pair of displacement detecting means are corrected and calculated to values when the vehicle is traveling on a flat ground based on the arrangement position in the vehicle longitudinal direction. Alternatively, even if the unmanned vehicle is traveling on a slope that is inclined left and right, the unmanned vehicle is based on the detection value of the left and right inclination angle detection means and the position of the pair of displacement detection means in the lateral direction of the vehicle body. The two detection values of the pair of displacement detection means are corrected and calculated to values obtained when the vehicle is traveling on a flat ground. By controlling the steering of the unmanned vehicle based on the difference between the two detected values corrected in this way, the vehicle can be guided with high accuracy regardless of the inclination.

[0012]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of an unmanned vehicle guidance and travel control device according to the present invention will be described below with reference to the drawings.

In the embodiment, a wheel loader is assumed as an applicable vehicle, and FIGS. 2A and 2B show an upper surface and a side surface of the wheel loader.

As shown in FIG. 1, on a road surface GD on which the wheel loader 10 travels, an induction cable CL is laid along a planned traveling path on which the wheel loader 10 should travel.
An induction current of a predetermined frequency is conducted through the induction cable CL.

The wheel loader 10 is a so-called articulated type loader, and has a center pivot steering type steering mechanism. That is, the front vehicle body 1
Between the vehicle body 1 and the rear vehicle body 12, a pivot pin 17 for connecting the vehicle bodies 11 and 12 and rotating the front vehicle body 11 in the horizontal direction with respect to the rear vehicle body 12 is provided. In the figure, a line segment CT1 is a line segment indicating a longitudinal central axis of the front vehicle body 11, and a line segment CT2 is a rear vehicle body 12
3 is a line segment showing the central axis in the longitudinal direction of FIG. The line segment CT is a line segment indicating the longitudinal central axis of the entire wheel loader 10 when the two line segments CT1 and CT2 coincide with each other. Therefore, by steering the steering such that the line segment CT1 is inclined by an angle θ with respect to the line segment CT2 (indicated by CT′1), the wheel loader 1 is moved in a direction corresponding to the angle θ.
0 will run. The pin 17 has the rotation angle θ
Is attached. The control of the steering mechanism will be described later with reference to FIG.

The front vehicle body 11 includes front left and right wheels 13L, 1L.
3R, boom 15 and bucket 16 which are working machines for excavation
Are arranged. On the other hand, rear left and right wheels 14L and 14R are arranged on the rear vehicle body 12.

Wheel loader (hereinafter referred to as vehicle 10)
At the front of the bottom surface of the front body 11 of the
A pair of pickup coils FL and FR, which detect the magnetic field generated in step (a) and output induced voltages V (FL) and V (FR) having magnitudes corresponding to the strength of the magnetic flux density, are symmetrical by a distance WF. At the rear of the bottom surface of the front vehicle body 11, the induced voltages V (ML1), V
A pair of pickup coils ML1 and MR1 that respectively output (MR1) are symmetrically spaced apart by the same distance WF.

On the other hand, a similar pair of pickup coils ML2 and MR2 are disposed symmetrically and spaced apart by a distance WR at the front of the bottom of the rear vehicle body 12, and the rear vehicle body 1
A pair of similar pickup coils R are also provided at the rear of the bottom surface of No. 2.
L and RR are disposed symmetrically and separated by the same distance WR. From the coils ML2 and MR2, the induced voltage V (M
L2) and V (MR2) are output, and the coil R
L and RR output induced voltages V (RL) and V (RR), respectively.

Here, the pickup coils ML1, MR
1 is disposed at a position away from the pin 17 by a distance Lb in front of the vehicle body, and the pickup coil FL,
The FR is disposed at a position further away from the coils ML1 and MR1 by a distance Lf toward the front of the vehicle body. On the other hand, the pickup coils ML2 and MR2 are disposed at a position away from the pin 18 by the distance La to the rear of the vehicle body, and the pickup coils RL and RR are further away from the coils ML2 and MR2 by the distance Lr to the rear of the vehicle body. It is arranged in.

These four pairs of coils, that is, a total of eight coils, are separated from the induction cable CL by the height H in the vertical direction on the assumption that the road surface GD is horizontal and the road surface GD is horizontal. It is located at the location. In FIG. 2B, a straight line 1 is a line segment connecting the centers of the coils, and the straight line 1 is parallel to the road surface GD which is a horizontal plane.

FIG. 1 is a block diagram showing the configuration of a steering control device for steering the vehicle 10 by rotating the front vehicle body 11 by an angle θ as shown in FIG. 2A. Here, as the vehicle 10, vehicle traveling control is performed by driving control of drive wheels in addition to steering control for controlling the rotation angle θ of the pin 17, and the vehicle traveling control and the steering control are performed along the planned traveling path. Although the guided traveling control for traveling is realized, the traveling control itself is not directly related to the gist of the present invention, and thus the components thereof are omitted. As shown in FIG. 1, the steering control device receives the detection signals V (FR) to V (RL) of the pickup coils FR to RL and the detection signal θ of the potentiometer 18, The controller 1 mainly outputs a steering angle command value Δθ to a proportional solenoid valve 7 which is an actuator for driving the steering based on these input detection signals.

That is, in the input circuit 4 of the controller 1, the detection signal of each pickup coil is converted into a required digital signal via an amplifier, a filter, a rectifier circuit, an A / D converter, etc. , The detection signal of the potentiometer 18 is applied to the CPU 2 via the buffer. The memory 3 stores a program for operating the CPU 2 and the like.
In U2, the processes of FIGS. 3 to 5 described later are executed based on the contents stored in the memory 3. A steering angle command value Δθ, which will be described later, calculated by the execution of the processing of the CPU 2 is added to the output circuit 5 and is converted into a required analog signal via a D / A converter, an amplifier, and the like. Added to solenoid. As a result, the spool of the proportional solenoid valve 7 is displaced in accordance with the steering angle command value Δθ to vary the flow rate flowing into the oil chamber of the hydraulic cylinder 8 that drives the steering. For this reason, the hydraulic cylinder 8 sets the steering angle command value Δθ
And the vehicle 10 is steered so that the steering angle θ is displaced by the angle Δθ.

When the vehicle 10 travels, it travels forward or backward. The command signal for making the vehicle travel forward or backward is given to the CPU 2 by changing the frequency of the current flowing through the induction cable CL. That is, the frequency is detected, and the detected content is
It is determined whether the vehicle is traveling forward or backward. Then, the forward shift clutch or the reverse shift clutch of the transmission of the vehicle 10 is selected according to the determination result, and the forward traveling or the reverse traveling is performed.

The inclinometers 19 and 20 shown in FIG. 1 are used in an embodiment described later.

Hereinafter, the flowcharts shown in FIGS. 3 to 5 will be described with reference to schematic posture diagrams of the vehicle 10 as viewed from above as shown in FIGS. 6 to 10.

Relative attitude angle of front vehicle body 11 with respect to rear vehicle body 12 車体 Calculation processing First, as shown in FIG.
Based on the R and FL detection signals V (FR) and V (FL), a deviation Fε of these detection signals is calculated as follows: Fε = V (FR) −V (FL) (1) The fact that the deviation Fε is zero means that the vehicle 10 is not displaced at all in the lateral direction with respect to the front part of the front vehicle body 11 with respect to the guide cable CL, and if the deviation Fε is a predetermined value. The vehicle 10 is displaced with respect to the front part of the front vehicle body 11 in a direction (left, right) corresponding to the polarity (+,-) of the predetermined value by a deviation amount corresponding to the absolute value of the predetermined value. Means

Similarly, the coil MR at the rear of the front body 11
1. Based on the detection signals V (MR1) and V (ML1) of ML1, a deviation M1ε of these detection signals is calculated as follows: M1ε = V (MR1) −V (ML1) (2) And these deviations Fε, M1ε
By dividing the distance Lf between the pair of coils in the longitudinal direction of the vehicle body from the difference between the pair of coils, the attitude angle Δf of the front vehicle body 11 with respect to the guide cable CL is expressed as follows: Δf = (Fε−M1ε) / Lf (3) Is calculated as follows. Note that the attitude angle Ψf is accurately expressed as Ψf = tan -1 (KＫ (Fε-M1ε) / Lf) (3) ', where K is a constant, but approximately (3) It is calculated by the formula.

Hereinafter, the attitude angles Ψr, Ψfr, and 演算 m calculated by the equations (6), (10), and (19) are also represented by similar approximations (step 101, see FIG. 6A). .

In the rear body 12, similarly to the front body 11, the deviation M2ε of these detection signals is M2ε based on the detection signals V (MR2) and V (ML2) of the front coils MR2 and ML2. = V (MR2) -V (ML2) (4)

Similarly, the coil R at the rear of the rear vehicle body 12
Based on the detection signals V (RR) and V (RL) of R and RL, the deviation Rε of these detection signals is calculated as follows: Rε = V (RR1) −V (RL) (5) And these deviations M2ε, Rε
By dividing the distance Lr between the pair of coils in the longitudinal direction of the vehicle body from the difference between the coils, the posture angle Δr of the rear vehicle body 11 with respect to the induction cable CL is represented by the following equation: Δr = (M2ε−Rε) / Lr (6) (Step 102, see FIG. 6A).

Then, from the front vehicle body posture angle Ψf calculated in steps 101 and 102, the rear vehicle body posture angle Ψr
By subtracting, the front body 1 with respect to the rear body 12
The relative attitude angle １ of 1 is obtained as Ψ = Ψf−Ψr (7) (step 103). Here, the relative posture angle Ψ is, as shown in FIG. 6B, a front-side vehicle body longitudinal direction central axis CT with respect to the rear-side vehicle body longitudinal direction central axis CT2.
Since 1 is an angle (bending angle), it means the rotation angle θ of the front vehicle body 11 that rotates about the pin 17. Therefore, steps 101 to 1
As shown in FIG. 03, it is also possible to obtain the detected value θ of the potentiometer 18 as the relative attitude angle Ψ without performing the process of calculating the relative attitude angle 基 づ い based on the output of the pickup coil.

The magnitude of the relative attitude angle Ψ and the forward / reverse determination processing Once the relative attitude angle Ψ has been calculated as described above, the magnitude of the relative attitude angle お よ び, the forward state of the vehicle 10, and the reverse The two pairs of pickup coils for calculating the deviation of the vehicle 10 from the induction coil CL according to the state are replaced with the four pairs of pickup coils ((FR, FL),
(MR1, ML1), (MR2, ML2), (RR, R
L)) is determined.

First, a threshold value Ψ for the relative attitude angle Ψ
1, Ψ2 (Ψ1 <Ψ2) is set in advance, and it is determined whether the relative attitude angle に is in the range of Ψ ≦ Ψ1, Ψ1 <Ψ ≦ Ψ2, Ψ2 <Ψ (8) ( Step 10
4).

When the vehicle 10 is traveling at low speed and with high accuracy along the straight guide cable CL.

In such a case, the determination in step 104 is Ψ ≦ Ψ1 (when the relative attitude angle に is in the smallest range), and the procedure shifts to step 201 in FIG. 4A. In this case, as shown in FIG. 7 (a), the front body longitudinal center axis CT1 is not so inclined with respect to the rear body longitudinal center axis CT2 (for example, the relative attitude angle Ψ = 0). In this case, there is no need to consider the front vehicle body 11 and the rear vehicle body 12 as separate bodies, and they can be considered as an integrated vehicle body 10 '(shown by a broken line). Therefore, by selecting the pair of coils FR and FL at the forefront and the pair of coils RR and RL at the end of the integrated vehicle body 10 ′, the displacement (deviation amount) of the vehicle 10 with respect to the induction cable CL is selected. And the posture angle) are preferably obtained.

Therefore, the lateral shift amount Xfr of the vehicle 10 with respect to the guide cable CL is determined by the deviation Fε of the pair of pickup coils FR and FL at the front portion of the front vehicle body 11 (Equation (1) above) as described below. Xfr = Fε + Rε (9) is calculated by adding the deviation of the detection values of the pair of pickup coils RR and RL at the rear part of the rear vehicle body 12 (formula (5)) (step 201, FIG. 7B).

Next, the attitude angle Ψ of the vehicle 10 with respect to the guide cable CL is obtained as follows by dividing the value obtained by subtracting the difference Rε from the difference Fε by the distance L between the two pairs of coils.

.DELTA.fr = (F.epsilon.-R.epsilon.) / L (10) Note that the distance L is clearly shown in FIG.
a, Lb, and Lr are obtained as a sum (Step 202, see FIG. 7C).

Next, the vehicle 10 with respect to the induction coil CL
Is calculated as follows: Δθ = K1 · Xfr + K2 · Ψfr (11), which is output as a steering angle command value. Here, the above equation (11) means that the larger the lateral displacement Xfr, the greater the displacement, and the steering must be turned by a steering angle proportional to the magnitude of the displacement (broken line in FIG. 7B). In addition, the greater the attitude angle Ψfr, the greater the deviation, which means that the steering must be turned by a steering angle proportional to the magnitude of the deviation (FIG. 7 (c)). Dashed arrow G). Note that K1 and K2 are predetermined constants, and the values of these constants are changed depending on whether the vehicle is traveling forward or backward (step 203).

When the vehicle 10 is traveling forward at high speed along the straight guide cable CL, or
Is traveling forward along the guidance cable CL having a large radius of curvature.

In such a case, the determination in step 104 is Ψ1 <Ψ ≦ Ψ2 (when the relative attitude angle に is in the middle range in the equation (8)).
At 5, it is determined whether the vehicle 10 is moving forward or backward. As a result, if it is determined that the vehicle is moving forward,
The procedure proceeds to step 301 in FIG.

In this case, as shown in FIG. 8A, the front body longitudinal center axis CT1 is not so inclined with respect to the rear body longitudinal center axis CT2, and the rear body 12 is taken into consideration. The rear vehicle body 12 may follow if the steering is performed in consideration of the deviation of the front vehicle body 11. Therefore, it can be considered that the vehicle 10 is the front vehicle body 11. Conversely, only the front vehicle body 11 is connected to the guide cable C as shown in FIG.
If the vehicle body 12 is not displaced from L, it means that the vehicle follows as shown by a dashed arrow G 'without steering even if the rear vehicle body 12 is displaced.

Therefore, by selecting the pair of coils FR and FL at the front and the pair of coils MR1 and ML1 at the rear of the front vehicle body 11, the vehicle 1
A deviation (a deviation amount and a posture angle) from the induction coil CL of 0 is suitably obtained.

Therefore, the lateral shift amount Xf of the vehicle 10 with respect to the guide cable CL is determined by the deviation Fε of the pair of pickup coils FR and FL detected at the front part of the front vehicle body 11 (formula (1) above) as described below. Xf = Fε + M1ε (12) is calculated by adding the deviation M1ε of the detection values of the pair of pickup coils MR1 and ML1 in the rear part of the front vehicle body 11 (formula (2)) (step 301). .

Next, the attitude angle Ψf of the vehicle 10 with respect to the guide cable CL is obtained as follows by dividing the value obtained by subtracting the difference M1ε from the difference Fε by the distance Lf between the two pairs of coils.

Ψf = (Fε−M1ε) / Lf (13) (Step 302, see FIG. 8A). Next, the deviation Δθ of the vehicle 10 with respect to the guidance cable CL
Is calculated as Δθ = K1 · Xf + K2 · Ψf (14), which is output as the steering angle command value. Here, the above equation (14) is the same idea as the above equation (11), and means that the larger the lateral shift amount Xf, the more the steering must be turned, and in addition, the attitude angle Ψf becomes larger. The larger the value, the greater the steering must be turned (step 303, see broken line arrow G in FIG. 8A). -When the vehicle 10 is traveling backward at high speed along the straight guide cable CL, or when the vehicle 10 is traveling backward along the guide cable CL having a large radius of curvature.

In such a case, the determination in step 104 is Ψ1 <Ψ ≦ Ψ2 (when the relative attitude angle 中 is in the middle range in the equation (8)), and then in step 10
At 5, it is determined whether the vehicle 10 is moving forward or backward. As a result, if it is determined that the vehicle is moving backward,
The procedure moves to step 401 of FIG.

In this case, as shown in FIG. 9A, the rear body longitudinal center axis CT2 is not so much inclined with respect to the front vehicle body longitudinal center axis CT1, and the front body 12 is taken into consideration. This is a case where the front vehicle body 11 follows when steering is performed in consideration of only the displacement of the rear vehicle body 12. Therefore, it can be considered that the vehicle 10 is the front vehicle body 11. This is, conversely,
If only the rear vehicle body 12 is not displaced with respect to the guide cable CL as shown in FIG. 4B, even if the front vehicle body 11 is displaced, even if the front vehicle body 11 is not steered, the broken arrow G
'Means that the front vehicle body 11 follows.

Therefore, by selecting a pair of coils MR2 and ML2 at the front and a pair of coils RR and RL at the rear of the rear body 12, the vehicle 1
0 displacement from the induction cable CL (displacement amount and attitude angle)
Is preferably required.

Therefore, the lateral displacement Xr of the vehicle 10 with respect to the guide cable CL is equal to the deviation M2ε of the detection values of the pair of pickup coils MR2 and ML2 at the front part of the rear vehicle body 12 (Equation (4) above). Xr = M2ε + Rε (15) is calculated by adding the deviation Rε of the detection values of the pair of pickup coils RR and RL at the rear part of the rear vehicle body 12 (Equation (5) above) (step 15). 401).

Next, the attitude angle Ψr of the vehicle 10 with respect to the guide cable CL is determined as follows by dividing the value obtained by subtracting the difference Rε from the difference M2ε by the distance Lr between the two pairs of coils.

Ψr = (M2ε−Rε) / Lr (16) (Step 402, see FIG. 9A). Next, the deviation Δθ of the vehicle 10 with respect to the guidance cable CL
Is calculated as Δθ = K1 · Xr + K2 · Ψr (17), and this is output as the steering angle command value. Here, the above equation (17) is the same concept as the above equation (11), and means that the larger the left / right deviation amount Xr, the larger the steering must be turned, and in addition, the attitude angle Ψr The larger the value, the more the steering must be turned (step 403, see broken arrow G in FIG. 9A).

When the vehicle 10 is traveling along the guide cable CL having a small radius of curvature.

In such a case, the determination in step 104 is Ψ2 <Ψ (when the relative attitude angle に is in the maximum range in equation (8)), and the procedure shifts to step 501 in FIG. 5B. Is done.

In this case, as shown in FIG. 10A, the longitudinal axis CT1 in the longitudinal direction of the front vehicle body is greatly inclined with respect to the longitudinal axis CT2 in the longitudinal direction of the rear vehicle body. This is a case where it cannot be considered that only the rear vehicle body 12 is shifted or only the rear vehicle body 12 is shifted, and the front and rear vehicle bodies 11 and 12 cannot be considered as one body.
The displacement of the vehicle 10 in this case is best represented by the broken line 1
This is a shift at the center of the vehicle indicated by 0 '.

Therefore, the 1 at the rear of the front vehicle body 11
By selecting the pair of coils MR1 and ML1 and the pair of coils MR2 and ML2 at the front of the rear vehicle body 12, the deviation (the deviation amount and the attitude angle) of the vehicle 10 with respect to the induction cable CL is suitably obtained. That is, as shown in FIG. 10 (a), when the longitudinal center axis CT 'of the vehicle central portion 10' coincides with the tangent line CL 'of the induction coil CL and there is no deviation, the vehicle 10 is connected to the induction cable CL. If the longitudinal center axis CT ′ is displaced from the tangent line CL ′ as shown in FIG. 10 (b), the steering is carried out by an amount corresponding to the displacement as shown by a broken arrow G. Must be cut.

Therefore, the lateral displacement Xm of the vehicle 10 with respect to the guide cable CL is determined by the difference M1ε between the detected values of the pair of pickup coils MR1 and ML1 at the rear part of the front vehicle body 11 (formula (2) above). Xm = M1ε + M2ε (18) is calculated by adding the deviation M2ε of the detection values of the pair of pickup coils MR2 and ML2 at the front part of the side body 12 (the above equation (4)) (step 501). ).

Next, the attitude angle Ψm of the vehicle 10 with respect to the guide cable CL is obtained as follows by dividing the value obtained by subtracting the difference M2ε from the difference M1ε by the distance Lm between the pair of coils.

Ψm = (M1ε−M2ε) / Lm (19) It is clear that the distance Lm can be obtained from FIG. 2A by adding the distances La and Lb (step 502, FIG. 10 (b)).

Next, the vehicle 1 with respect to the guidance cable CL
The deviation Δθ of 0 is calculated as Δθ = K1 · Xm + K2 · Ψm (20), and this is output as the steering angle command value. Here, the expression (20) is based on the same concept as the expression (11), and the steering must be steered more as the lateral displacement Xm, that is, the distance between the tangent line CL ′ and the central portion 10 ′ is larger. Means, in addition to this,
The greater the attitude angle Ψf, that is, the greater the angle formed by the longitudinal central axis CT 'of the central portion 10' with respect to the tangent line CL ', the greater the steering must be turned. The values of K1 and K2 are changed according to the forward running and the backward running (step 503, see broken arrow G in FIG. 10B).

In the steering angle control, the detected value θ of the potentiometer 18 is used as a feedback amount for changing the steering angle by Δθ.

Incidentally, since the vehicle 10 is a vehicle for excavation work, there is a case where the vehicle 10 runs on an inclined land SD formed by earth and sand as shown in FIG.

For this reason, the vertical distance between the guide cable CL and each pickup coil changes depending on the degree of inclination of the front and rear of the vehicle body, and the detection value of each pickup coil changes.

Here, when only one pair of pickup coils is provided on the vehicle body and the deviation of the vehicle is determined from the deviation of the detected values of these coils, it is assumed that the vertical distance changes according to the inclination. Although the vertical distance between the left and right coils is the same, there is no particular problem. However, when two pairs of pickup coils arranged at different positions in the front and rear of the vehicle body are selected as in the embodiment, Since the two sets selected have different vertical distances from the cable CL, the detected values cannot be used as they are. If the detected value is used as it is, the steering control will not be performed with high accuracy.

Therefore, when the vehicle body tilts back and forth as described above, the detected value of each coil due to the inclination is determined by the road surface GD.
In the embodiment described below, the detected value is corrected assuming that it is a horizontal plane having no inclination, and an accurate steering control is performed using the corrected value.

The vehicle 10 may not only be inclined forward and backward (pitch) but also inclined left and right (rolling). In this case, 1
The detected value differs between the left and right sides of the pair of pickup coils in accordance with the inclination, and similarly, the steering control is not accurately performed. Therefore, the correction by the rolling will be described below together with the correction by the pitching.

As shown in FIG. 11, a front-to-rear direction inclination angle (pitching angle) φ
An inclinometer 19 for detecting p is provided, and the vehicle 1
An inclinometer 20 for detecting a left-right inclination angle (rolling angle) φr of 0 is provided. Note that these inclinometers 1
Any of sensors 9 and 20 can be used, such as a gyro.

The CPU 2 has an input circuit 4 as shown in FIG.
, Detection signals φp and φr of the inclinometers 19 and 20 are added.

Therefore, in the CPU 2, the detection signal φ
The following correction calculation processing is executed based on p and φr. Hereinafter, φp and φr will be described separately for convenience of explanation.

When the vehicle 10 is pitched by the angle φp.

In such a case, the state shown in FIG. 11 is obtained, and the line segment l 'connecting the coils is inclined by an angle φp from the horizontal state l.

It is apparent from the geometrical relationship that the vertical distance H from the cable CL does not change even at the inclination at the point n. This n point is the center position of the rear wheel 14, and the distance between the center position n of the rear wheel 14 and the pin 17 is shown in FIG.
As shown in FIG.

For this reason, the distance from the point n to the pickup coils FR and FL at the front part of the front vehicle body 11 is Ls + Lb + Lf, and the coils FR and FL are located above the line segment l by (Ls + Lb + Lf) sinφp. Will be. Therefore, the vertical distance H′f from the cable CL to the coils FR and FL in the line segment l ′ is the distance from the cable CL to the coils FR and FL when there is no inclination and the road surface GD is a horizontal plane.
Assuming that f, H′f = Hf + (Ls + Lb + Lf) sinφp (21) FIG. 12 is a view of the vehicle 10 in FIG. 11 as viewed from the front, and shows a state where the vehicle 10 is inclined by an angle φp as indicated by an arrow PT and the coil FL moves to an upper position FL ′.

The current flowing through the induction cable CL is represented by I, and the horizontal distance between the cable CL and the coil FL is represented by X.
When f is given, the induced voltage V (FL) detected by the coil FL when there is no inclination is as follows: V (FL) = Hf · I / (2π · (Xf2 + Hf2)) (22) The induced voltage V '(FL) detected when tilted only by the following formula is as follows: V' (FL) = H'f.I / (2.pi. (Xf2 + H'f2)) (23) (see FIG. 12) .

Therefore, from the above equations (22) and (23), the induced voltage V (FL) when there is no slope is obtained by using the induced voltage V '(FL) detected when the slope is obtained, V (FL) = V '(FL) ・ Hf ・ (Xf2 + Hf2) / (H'f ・ (Xf2 + Hf2)) (24) Here, Xf can be approximately regarded as zero, and the above equation (24) is expressed as follows: V (FL) = V ′ (FL) · Hf ′ / Hf (25)

Therefore, by substituting the above equation (21) into the above (25), the induced voltage V 'detected when the inclination is detected.
(FL) is the inclination angle φp and the known distances Hf, Ls, L
b, Lf, the induced voltage V (F
L), V (FL) = V ′ (FL) · (Hf + (Ls + Lb + Lf) sinφp) / Hf (26)

Similarly, the induced voltage V (FR) of the coil FR when there is no inclination is given by: V (FR) = V ′ (FR) · (Hf + (Ls + Lb + Lf) sinφp) / Hf (27) Will be corrected.

Similarly, the distance between the coils MR1 and ML1 at the rear of the front vehicle body 11 and the point n is Ls + Lb.
Assuming that the vertical distance from R1 and ML1 is Hm1, the detection value V
'(MR1) and V' (ML1) are respectively V (MR1) = V '(MR1)） (Hm1 + (Ls + Lb) sinφp) / Hm1 ... (28) V (ML1) = V' (ML1) ・ (Hm1 + (Ls + Lb) sinφp) / Hm1 (29) Induced voltages V (MR1) and V when there is no slope as shown in (29)
(ML1).

Similarly, the distance between the coils MR2 and ML2 at the front of the rear vehicle body 12 and the point n is Ls-La, so that the cable CL and the coil M
Assuming that the vertical distance from R2 and ML2 is Hm1, the detected value V
'(MR2) and V' (ML2) are respectively V (MR2) = V '(MR2)） (Hm2 + (Ls-La) sinφp) / Hm2 ... (30) V (ML2) = V' (ML2)） (Hm2 + (Ls-La) sinφp) / Hm2 (31) Induced voltage V (MR2), V when there is no slope as shown
(ML2).

Similarly, the distance between the coils RR, RL at the rear of the rear body 12 and the point n is La + Lr-Ls. Since the coils RR, RL are below the line segment l, there is no inclination. Cable CL and coil MR2, M
Assuming that the vertical distance from L2 is Hr, the detection value V ′ (R
R) and V ′ (RL) are respectively V (RR) = V ′ (RR) · (Hr− (La + Lr−Ls) sinφp) / Hr (32) V (RL) = V ′ (RL) · ( Hr- (La + Lr-Ls) sin φp) / Hr (33) Induced voltages V (RR) and V (R
L).

By performing the calculations in steps 101 to 503 using the induced voltage V corrected as described above, an accurate steering angle command value Δθ is always obtained irrespective of the influence of pitching, and the vehicle 10 The vehicle is guided accurately along the guidance cable CL.

When the vehicle 10 is rolling by the angle φr.

In such a case, as shown in FIG. 12, the coil FL moves upward by the distance (WF / 2) · sin φr by using the distance WF between the coils FR and FL with the rolling RL. Therefore, the detection value V '(FL)
Is corrected as follows: V (FL) = V ′ (FL) · (Hf + (WF / 2) sinφr) / Hf (34)

On the other hand, since the coil FR moves downward, it is corrected as follows: V (FR) = V ′ (FR) · (Hf− (WF / 2) sinφr) / Hf (35)

The coils MR1 and ML1 are also
This is the same as the above equations (34) and (35). Rear body 12
The same applies to the coils MR2, ML2, RR, RL of the above, and the distance WR may be used instead of the distance WF.

By using the induced voltage V obtained and corrected as described above, the calculations in steps 101 to 503 are performed, whereby
Always accurate steering angle command value に 関 regardless of the effect of rolling
is obtained, and the vehicle 10 is guided with high precision along the guidance cable CL.

In the embodiment, the case where the deviation from the expected traveling path is detected by using the magnetic field from the induction cable CL as a medium has been described as an example of the guidance method, but the present invention is not limited to this. It is also possible to use a light as a detection medium to detect a shift. For example, a white line is laid on the floor along the planned travel path, the vehicle travels while illuminating the white line with a light source, and the line sensor detects reflected light of the white line to detect a deviation from the planned travel path indicated by the white line. can do.

[0088]

As described above, according to the present invention, when an unmanned articulated vehicle is run, two pairs of optimal displacements are set in accordance with the relative posture angles of the front and rear vehicle bodies. A detecting means is selected, and an accurate deviation of the unmanned vehicle with respect to the guide line is obtained based on the detected values of the two optimal pairs of displacement detecting means, so that the vehicle is guided accurately along the planned traveling path. be able to. In addition, even if the unmanned vehicle is inclined in the front-back direction or in the left-right direction, the detection value of the displacement detection unit that detects the displacement in the lateral direction with respect to the guide line is accurately corrected, Based on this detection value, the unmanned vehicle is guided accurately along the planned traveling path.

[Brief description of the drawings]

FIG. 1 is a block diagram showing the configuration of an embodiment of an unmanned vehicle guidance traveling control device according to the present invention.

FIGS. 2A and 2B are diagrams showing a wheel loader which is an unmanned vehicle according to the embodiment. FIG. 2A is a diagram showing an upper surface thereof, and FIG. 2B is a diagram showing a side surface thereof.

FIG. 3 is a flowchart showing a processing procedure executed by a CPU shown in FIG. 1;

FIG. 4 is a flowchart showing a processing procedure executed by a CPU shown in FIG. 1;

FIG. 5 is a flowchart showing a processing procedure executed by a CPU shown in FIG. 1;

FIG. 6 is a top view schematically showing the attitude of the unmanned vehicle used to explain the flowcharts shown in FIGS. 3 to 5;

FIG. 7 is a top view schematically showing the attitude of the unmanned vehicle used for explaining the flowcharts shown in FIGS. 3 to 5;

FIG. 8 is a top view schematically showing the attitude of the unmanned vehicle used to explain the flowcharts shown in FIGS.

FIG. 9 is a top view schematically showing the attitude of the unmanned vehicle used to explain the flowcharts shown in FIGS. 3 to 5;

FIG. 10 is a top view schematically showing the attitude of the unmanned vehicle used for explaining the flowcharts shown in FIGS. 3 to 5;

FIG. 11 is a side view showing an unmanned vehicle running on a slope.

FIG. 12 is a diagram schematically showing the unmanned vehicle shown in FIG. 11 as viewed from the front.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 Controller 2 CPU 7 Proportional solenoid valve 8 Hydraulic cylinder for steering 10 Unmanned vehicle (wheel loader) 11 Front body 12 Rear body 17 Pivot pin 19 Inclinometer 20 Inclinometer

Continuation of the front page (72) Inventor Katsuhiro Kamikawa 1200 Manda, Hiratsuka-shi, Kanagawa Prefecture, Komatsu Ltd. In-laboratory (56) References JP-A-54-44180 (JP, A) JP-A-63-255186 (JP, A ) Actually open sho 59-111759 (JP, U) Actually open hei 3-127918 (JP, U) (58) Field surveyed (Int. Cl. ⁷ , DB name) G05D 1/00-1/12

Claims (7)

(57) [Claims] 1. A guide line is laid along a planned traveling path, and a pair of displacement detecting means for detecting a lateral displacement with respect to the guide line is provided on the left and right sides of the body of the unmanned vehicle. An unmanned vehicle guidance traveling control device configured to perform steering control of the unmanned vehicle based on a deviation between the two detection values of the pair of displacement detection means and to guide the unmanned vehicle along the scheduled traveling path. Is a vehicle in which a front vehicle body and a rear vehicle body are rotatably connected to each other in a horizontal direction, and the pair of displacement detecting means includes a front part and a rear part of the front vehicle body and a front part of the rear vehicle body. An angle detecting unit disposed at a rear portion for detecting an angle formed by a longitudinal direction of the front vehicle body with respect to a longitudinal direction of the rear vehicle body; 2 out of displacement detection means Select the displacement detecting means of a pair of,
Calculating means for calculating the displacement of the unmanned vehicle with respect to the guide line based on the respective deviations of the selected two sets of displacement detecting means; An unmanned vehicle guidance and travel control device for steering control of a vehicle. 2. The angle detecting means subtracts the other deviation from one deviation of each of a pair of displacement detecting means disposed at a front portion and a rear portion of the front vehicle body, and calculates the subtracted value and the two values. An attitude angle of the front vehicle body with respect to the guide line is calculated based on a distance between the pair of displacement detection means, and one of a pair of displacement detection means disposed at a front part and a rear part of the rear body. Is subtracted from the deviation of the vehicle body, and the posture angle of the rear vehicle body with respect to the guide line is calculated based on the subtracted value and the distance between the pair of displacement detection means. The guided travel control device for an unmanned vehicle according to claim 1, wherein a deviation of the attitude angle is detected as the angle. 3. The unmanned vehicle according to claim 1, wherein the calculation means selects the two pairs of displacement detection means according to whether the unmanned vehicle is traveling forward or traveling backward. Guidance traveling control device. 4. The arithmetic unit determines whether a value detected by the angle detecting unit is equal to or less than a first threshold, or greater than the first threshold and equal to or less than a second threshold. Or, if it is determined that the detected value is equal to or less than the first threshold, the front portion of the front vehicle body and the rear Selecting two pairs of displacement detecting means disposed at the rear part of the vehicle body, wherein the detected value is larger than the first threshold value and the second
Is determined to be equal to or less than the threshold value, and when the unmanned vehicle is traveling forward, two pairs of displacement detecting means disposed at the front and rear of the front vehicle body are selected. The second detection value is larger than the first threshold value;
And if the unmanned vehicle is traveling in reverse, two pairs of displacement detecting means disposed at the front and rear of the rear vehicle body are selected. If it is determined that the detected value is larger than the second threshold value, two pairs of displacement detections provided at a rear portion of the front vehicle body and a front portion of the rear vehicle body are detected. 4. An unmanned vehicle guidance and travel control device according to claim 3, wherein said means is selected. 5. The calculation means calculates a shift amount of the unmanned vehicle with respect to the guide line by adding respective deviations of the selected two sets of displacement detection means, and calculates the shift amount of the unmanned vehicle with respect to the guide line. Subtracting the other deviation from one of the two pairs of displacement detecting means, and based on the subtraction value and the distance between the two pairs of displacement detecting means, the unmanned vehicle with respect to the guide line. And calculating the deviation of the unmanned vehicle with respect to the guide line based on the calculated deviation amount and the posture angle. Steering control of the unmanned vehicle so that the calculated deviation is eliminated. An unmanned vehicle guidance and travel control device according to claim 1. 6. The unmanned vehicle guidance and travel control device according to claim 1, wherein the deviation calculated by the calculation means is corrected and calculated according to whether the unmanned vehicle is traveling forward or backward. 7. A guide line is laid along a planned traveling path, and a pair of displacement detecting means for detecting a lateral displacement with respect to the guide line is provided on the left and right sides of the body of the unmanned vehicle. An unmanned vehicle guidance traveling control device configured to perform steering control of the unmanned vehicle based on a deviation between the two detection values of the pair of displacement detection means and to guide the unmanned vehicle along the scheduled traveling path. And a front-rear inclination angle detecting means for detecting a front-rear inclination angle of the vehicle, wherein when the unmanned vehicle runs on a slope, the detection value of the front-rear inclination angle detecting means and the vehicle front-rear direction of the pair of displacement detecting means are provided. Guidance for an unmanned vehicle that corrects and calculates both detection values of the pair of detection means based on the arrangement position in the direction, and controls the steering of the unmanned vehicle based on a deviation between the corrected and calculated detection values. Travel control device.