JP2000330632A - Guiding device for moving object - Google Patents

Abstract

PROBLEM TO BE SOLVED: To equalize the control characteristics of a moving vehicle in forward movement and backward movement to each other by making the moving object move to between a next couple of end points at the time of arrival moving body moving between a current couple of end points. SOLUTION: Points P1, P2, P3, P4, P5, P6 and P7 in an array on an expected travel path K are regarded as multiple targets and a couple of end points PR1 and PL2 are generated from the point P1 among the points P1, P2, P3, P4, P5, P6, and P7. Then the moving object S when reaching the point P1 is made to move to between the generated couple of end points PR2 and PL2. Consequently, the control characteristics of the moving vehicle in forward movement and backward movement are equalized to each other to facilitate control over the setting angle.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a moving body guiding apparatus, and more particularly to a moving body guiding apparatus suitable for a case where the moving body turns a sharp curve with a narrow road width or moves forward or backward.

[0002]

2. Description of the Related Art As a conventional technique for guiding a moving vehicle, there is a technique called a "point following method".

FIG. 37 is a diagram showing a technique for guiding a moving vehicle using a "point following method".

In the technique shown in FIG. 37, a curve, which is a planned traveling route of a moving vehicle S, is moved to a target point P of the moving vehicle S.
It is taught as a point sequence of 1, P2, P3, P4, P5, P6, P7. The area in which the moving vehicle S travels is represented by a coordinate system, and the target points P1, P2, P of the moving vehicle S traveling in this coordinate system.
3, P4, P5, P6, and P7 are represented by coordinate data.

Therefore, in the technique shown in FIG. 37, the coordinate data and the target points P1, P2, P3, P4, P5, P6, P7
Radii r1, r2, r3, r4, r5 of circles centered on
By using r6 and r7, the moving vehicle S is guided to sequentially follow the target points P1, P2, P3, P4, P5, P6, and P7.

[0006] The technique shown in Fig. 37 is suitable for guiding a large construction machine running on a vast uneven terrain outdoors.

On the other hand, in an automated factory, unmanned transport vehicles or unmanned forklifts are used, or ITS (Inte
In some cases, such as using an unmanned vehicle in an lligent transportation system, small mobile vehicles are guided indoors, on narrow premises, or on narrow roads.

This case will be described with reference to FIG.

For example, each target point P1, P2, P3, P4,
The moving vehicle S is guided to turn a curve while sequentially following P5, P6, and P7.

At this time, since the curve is steep, the moving vehicle S greatly deviates from the target points P1, P2, P3, P4, P5, P6, and P7, and a small turning phenomenon that passes through the inside K 'of the curve occurs.

It is considered that this is because the moving vehicle S is guided toward the next target when it enters the circle corresponding to the current target point.

That is, it is considered that the cause is that the moving vehicle S is guided to the next target in front of the target point.

For this reason, if there is an obstacle at the point Kp inside the curve, the obstacle may collide with the moving vehicle S and cause an accident.

The present invention has been made in view of such a situation, and a first solution is to guide a moving body without causing a small turning phenomenon even when a planned traveling route of the moving body is a sharp curve. Make it an issue.

In the field of automotive engineering, there are cases where a steering type four-wheeled vehicle is virtually replaced with a two-wheeled vehicle.
The motorcycle in this case is called an equivalent motorcycle.

FIG. 38 (a) is a diagram showing a case where the equivalent two-wheeled vehicle is moved forward and guided to the target point, and FIG. 38 (b) is a case where the equivalent two-wheeled vehicle is moved backward and guided to the target point. FIG.

In FIGS. 38 (a) and 38 (b), the equivalent 2
The wheel S0 has two wheels, a steered wheel S1 and a fixed wheel S2.

In FIGS. 38 (a) and 38 (b), the steered wheels S1
Is directed to the target point Pn (Xn, Yn) so that the equivalent two-wheeled vehicle reaches the target point Pn (Xn, Yn).

As shown in FIG. 38 (a), when the equivalent two-wheeled vehicle S0 is advanced to reach the target point Pn (Xn, Yn), the fixed wheel S2 advances according to the steering angle の of the steered wheel S1.

Therefore, when the equivalent two-wheeled vehicle S0 is advanced, the equivalent two-wheeled vehicle S0 can reach the target point Pn (Xn, Yn).

However, as shown in FIG. 38B, the equivalent two-wheeled vehicle S0 is moved backward to reach the target point Pn (Xn, Y
In the case of reaching n), if the steering angle ζ is directed to the target point Pn (Xn, Yn) as in the case of the forward movement, the traveling directions of the steered wheels S1 and the fixed wheels S2 will be different.

Accordingly, when the equivalent two-wheeled vehicle S0 is moved backward, the equivalent two-wheeled vehicle S0 cannot reach the target point Pn (Xn, Yn).

That is, in FIGS. 38 (a) and 38 (b), the steering angle 時 when the equivalent two-wheeled vehicle moves forward and the steering angle 後 when the equivalent two-wheeled vehicle moves backward cannot be the same.

For this reason, the moving vehicle S is moved to the target point Pn (X
n, Yn) is disclosed in, for example,
As disclosed in Japanese Patent Application Laid-Open No. 3-19011, the control algorithm is switched according to when the equivalent two-wheeled vehicle moves forward and backward.

As a result, according to the above-mentioned prior art, the moving vehicle S
This causes a problem that the control characteristics change when the vehicle moves forward and backward. In addition, there is a problem that the control of the steering angle ζ becomes complicated.

The present invention has been made in view of the above circumstances, and it is a second object of the present invention to make the control characteristics of a moving vehicle equal to when the vehicle is moving forward and when the vehicle is moving backward, and to simplify the control of the steering angle ζ. And

[0027]

Therefore, according to the first aspect of the present invention, in order to achieve the above-mentioned first object, the present invention is based on a current target among a plurality of targets on a planned traveling route of a moving body. When the vehicle reaches the target, in the guidance apparatus for a moving body that guides the moving body toward the next target, a pair of end points separated by a predetermined distance across the planned traveling path are set as the target. When the moving body reaches between the current pair of end points, the moving body is directed to the next pair of end points.

The first invention will be described with reference to FIG.

According to the first aspect, the pair of end points PRn and PLn separated by a predetermined distance with respect to the planned traveling route K are provided.
Is the target of the moving object S.

Therefore, for example, the current pair of end points PR1, P2
When the moving body S reaches between L1, the next pair of end points PR
2. Move the moving body S between PL2.

Therefore, according to the first aspect of the present invention, when the moving body S reaches between the current pair of end points PR1 and PL1, the moving body S is directed between the next pair of end points PR2 and PL2. Therefore, even if the scheduled traveling route K of the moving body S is a sharp curve, the moving body S can be guided without causing the small turning phenomenon.

Further, in the second invention of the present invention, in order to achieve the first object, when the vehicle reaches the current passage area among a plurality of passage areas on the scheduled traveling route of the moving body, the next A moving body guiding device for guiding the moving body toward the passing area, wherein the moving body travels a predetermined distance after first detecting the current passing area, and the passing area When the vehicle arrives at a predetermined point, the mobile object is directed to the next passage area.

The second invention will be described with reference to FIG.

According to the second aspect of the present invention, when the moving body S travels a predetermined distance after first detecting the current passage area M1, and reaches the predetermined point P1 in the passage area M1, The moving body S is directed to the next passage area M2.

Therefore, according to the second aspect, the moving object S
Travels a predetermined distance after the first detection of the current passage area M1 and reaches the predetermined point P1 in the passage area M1, so that the mobile object S is directed to the next passage area M2. Therefore, the moving body S can be guided without causing the small turning phenomenon even when the planned traveling route K of the moving body S is a sharp curve, similarly to the first invention. Further, according to the second invention, the effects of the first invention can be obtained even if the target of the moving body S is any geometrical shape such as a circle or a polygon.

According to the third aspect of the present invention, in order to achieve the second object, a present object is selected from among a plurality of targets on a scheduled traveling route of a moving body having a steered wheel and a fixed wheel. At the time of arrival, in a moving body guidance device for guiding the moving body toward a next target, a distance between a center axis of the steered wheels and a center axis of the fixed wheel,
The steering angle of the steered wheels is determined from the radius of the turning circle when the moving body turns to the next target, and the moving body is steered based on the determined steering angle, and the steering is performed toward the target. The moving object is directed to it.

The third invention will be described with reference to FIGS. 2, 6, and 8.

According to the third aspect of the present invention, the central axis A of the steered wheel S1 of the moving body S and the fixed wheel S2
The steering angle の of the steered wheel S1 is determined from the distance L between the steering wheel S1 and the center axis B and the turning radius rg when turning to the next target.

The moving body S is steered based on the obtained steering angle ζ, and the moving body S is guided toward the next pair of end points PRn and PLn.

Therefore, according to the third aspect of the present invention, the steering command calculating means 5 determines the distance L between the center axis A of the steered wheel S1 of the moving body S and the center axis B of the fixed wheel S2, and the turning radius rg. , The steering angle の of the steered wheel S1 is obtained, and the moving body S is steered based on the obtained steering angle ζ, so that the control characteristics when the moving vehicle moves forward and when it moves backward can be made the same. Further, the control of the steering angle ζ can be simplified.

According to a fourth aspect of the present invention, in order to achieve the first and second problems, a plurality of targets on a scheduled traveling route of a moving body having steered wheels and fixed wheels are provided. At the time when the vehicle reaches the current target, in a mobile object guiding device that guides the mobile object toward the next target, a pair of mobile terminals separated by a predetermined distance across the scheduled travel path With the end point as the target, the steering of the steered wheels is determined from the distance between the central axis of the steered wheels and the central axis of the fixed wheel, and the radius of the turning circle when the moving body turns to the next target. An angle is obtained, and at the time when the moving body passes between the current pair of end points, the moving body is steered based on the obtained steering angle, and the moving body is directed between the next pair of end points. I am trying to make it.

FIG. 1A, FIG. 2, FIG.
This will be described with reference to FIG.

According to the fourth aspect of the invention, the pair of end points PRn and PLn separated by a predetermined distance with respect to the planned traveling route K are provided.
Is the target of the moving object S.

The distance L between the central axis of the steered wheel S1 and the central axis of the fixed wheel S2 is calculated by the steering command calculating means 5;
The steering angle の of the steered wheel S1 is determined from the turning radius rg.

When the moving body S reaches between the current pair of end points PR1 and PL1, the moving body S is steered based on the steering angle ζ, and moves toward the next pair of end points PR2 and PL2. S is directed.

Therefore, according to the fourth aspect of the present invention, the steering command calculating means 5 controls the steering of the steered wheel S1 from the distance L between the central axis of the steered wheel S1 and the central axis of the fixed wheel S2 and the turning radius rg. The angle ζ is obtained, and when the moving body S reaches between the current pair of end points PR1 and PL1, the moving body S is steered based on the steering angle 、, and is moved between the next pair of end points PR2 and PL2. Since the moving body S is directed, the first invention and the third invention are described.
The same effect as the invention can be obtained.

According to the fifth aspect of the present invention, in order to achieve the first and second problems, a plurality of vehicles having a steered wheel and a fixed wheel pass through a plurality of passages on a predetermined traveling route. When the vehicle reaches the current passing area of the area, the moving body guiding device guides the moving body toward the next passing area, wherein the central axis of the steering wheel and the central axis of the fixed wheel are provided. And the steering angle of the steered wheels from the radius of the turning circle when the moving body turns to the next target, and the moving body first detects the current passage area. At a predetermined distance in the passing area, and at a time when the moving body is steered based on the obtained steering angle, and moves toward the next passing area. To make it head.

The fifth invention will be described with reference to FIGS. 2, 6, and 14.

According to the fifth aspect of the present invention, the steering command calculating means 5 determines the distance L between the center axis of the steered wheel S1 and the center axis of the fixed wheel S2, and the turning radius r for turning to the next target.
The steering angle の of the steered wheel S1 is obtained from g.

When the moving body S travels a predetermined distance after the current passage area M1 is first detected and reaches a predetermined point P1 in the passage area M1, the steering angle ζ
, The moving body S is steered, and the moving body S is directed toward the next passage area M2.

Therefore, according to the fifth aspect of the present invention, the steering command calculating means 5 controls the steering of the steered wheel S1 from the distance L between the central axis of the steered wheel S1 and the central axis of the fixed wheel S2 and the turning radius rg. The angle ζ is determined, and the moving body S is moved to the current passage area M1.
At a predetermined point P1 in the passing area M1, the mobile body S is steered based on the steering angle ζ, and moves toward the next passing area M2. Since the moving body S is directed to the vehicle, the same effects as those of the second and third aspects can be obtained.

According to a sixth aspect of the present invention, in the first aspect, the plurality of targets on the planned traveling route are a series of points, and a pair of end points of the series of points corresponding to the curve in the planned traveling path. Is generated, and when the mobile object reaches a current point in the point sequence, the moving object is caused to move between the pair of generated end points.

The sixth invention will be described with reference to FIG.

According to the sixth aspect of the present invention, a plurality of points P1, P1, P2, P3, P4, P5, P6, and P7 on the planned traveling route K are set.

Therefore, on the planned traveling route K of the curve, a pair of end points PR1 and PL1 are divided into a series of points P1, P1, P2, P3, P4,
It is generated from point P1 among P5, P6, and P7.

The moving body S is composed of a series of points P1, P1, P2,
When the current point P1 among the points P3, P4, P5, P6 and P7 is reached, the point is moved between the pair of generated end points PR2 and PL2.

That is, according to the sixth aspect, when the planned traveling route K is a curve, a pair of end points PR2 and PL2 are generated from the point P2.

Therefore, according to the sixth aspect of the present invention, when the planned traveling route K is a curve, a pair of end points PR2 and PL2 are generated. This is easier than performing an operation to generate all of the point sequence at a pair of end points. Further, according to the sixth aspect, the same effects as those of the first aspect can be obtained.

[0059]

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a perspective view of a moving object guiding apparatus according to the present invention. In addition,
The moving body of this embodiment is assumed to be a steering-type four-wheeled vehicle.

FIG. 2 is a block diagram showing the configuration of the guide device for a mobile vehicle according to the present invention.

The guidance apparatus for a moving vehicle shown in FIG. 2 includes a current motion measuring means 2, a planned traveling route storage means 1, a planned traveling route changing means 3, a traveling condition selecting means 4, and a steering command calculating means 5. And vehicle speed command calculating means 6, obstacle detecting means 8, and vehicle body 7 (moving vehicle S).

Next, the above components will be described.

The current movement measuring means 2 uses the GPS (Global Positi) to obtain information on the current movement state of the moving vehicle such as the current position P1, the traveling direction θt, the speed, and the turning angular velocity ω of the moving vehicle S.
oning System) and dead reckoning (Dead Reckoning).

The planned traveling route storage means 1 stores the CAD (Comp
uter Automation Design) or data converted from point data created by teaching playback into a pair of end points or an arbitrary geometrical figure is stored as a plurality of target data of the planned traveling route K. The planned traveling route storage unit 1 acquires information on the current motion state of the moving vehicle S from the current motion measurement unit 2. Further, the planned traveling route storage means 1 outputs a plurality of target data of the stored planned traveling route K to the traveling condition selecting means 4.

The obstacle detecting means 8 detects an obstacle existing on the planned traveling route K.

The planned traveling route changing means 3 stores data of a pair of end points different from target data of the planned traveling route K or data of an arbitrary geometric figure. Further, the scheduled traveling route changing unit 3 acquires information on an obstacle on the scheduled traveling route K from the obstacle detecting unit 8 and acquires information on a current motion state of the moving vehicle S from the current motion measuring unit 2. The planned traveling route changing means 3 outputs to the traveling condition selecting means 4 a pair of end points different from the target data of the planned traveling route K or data of an arbitrary geometric figure (planned traveling route change information).

The traveling condition selecting means 4 acquires a plurality of target data of the planned traveling route K from the planned traveling route storage means 1 in a normal case (when there is no obstacle on the planned traveling route K). The information of the current motion state of the moving vehicle S is acquired from the current motion measuring means 2. On the other hand, in the case of an emergency (when an obstacle exists on the planned traveling route K), the planned traveling route changing means 3 transmits data of a pair of end points different from the target data of the planned traveling route K or arbitrary geometric figures (planned). Travel route change information). The traveling condition selecting means 4 outputs the current relative position of the moving vehicle with respect to the target data, or a pair of end points different from the target data or data of an arbitrary geometrical figure to the steering command calculating means 5 and the vehicle speed command calculating means. The running conditions are output to 6.

The steering command calculating means 5 calculates the steering angle ζ and outputs a steering command to the vehicle body 7 (moving vehicle S) to control the steering angle の of the moving vehicle S. As a result, the steering command calculating means 5 causes the moving vehicle S to perform straight traveling, right turning, and left turning.

The vehicle speed command calculating means 6 calculates the speed of the vehicle body 7 and outputs a traveling command to the vehicle body 7 to control the speed of the vehicle body 7. As a result, the vehicle speed command calculating means 6 causes the vehicle body 7 to perform acceleration, deceleration, forward movement, and backward movement.

The vehicle body 7 (moving vehicle S) adjusts the vehicle speed in response to the steering command output from the steering command calculating means 5 and the traveling command output from the vehicle speed command calculating means 6.

The obstacle detecting means 8 uses an ultrasonic sensor, a laser range finder, or a technique using a plurality of video cameras, such as stereo vision, which measures the distance to an object from the parallax of an image captured by the video camera. Detecting the presence or absence of an obstacle in front of or around the moving vehicle S, recognizing the motion state of the obstacle, measuring the distance to the obstacle, and the like.

The planned traveling route changing means 3 receives the information on the obstacle from the obstacle detecting means 8 and acquires the information on the current motion state of the moving vehicle S from the current motion measuring means 2. Then, based on the information on these obstacles and the information on the current motion state of the moving vehicle S, the information about the change of the planned traveling route K is output to the traveling condition selecting means 4.

Next, the operation of the guidance device for a moving vehicle shown in FIG. 2 will be described.

Here, a case where a plurality of target data of the planned traveling route K is a plurality of paired end points will be described with reference to FIGS.

FIG. 1A is a diagram showing an embodiment in which a plurality of target data of the planned traveling route K is a plurality of paired end points PRn, PLn (n = 1 to 7), and FIG. 4) is a diagram showing an example in which target data is described as a width d, an inclination θa with respect to a reference direction, and a line segment Ga of a center point Pa.

Each of the pair of end points PRn and PLn is
Are separated from each other.

Each target point Pn (n = 1 to 7) is included between each pair of end points PRn and PLn.

The allowable amount of passing of the moving vehicle S is determined by the width d of the line segment Ga in FIG. 1B.

The pair of end points PRn and PLn and the line segment Ga are also called gates.

FIG. 3 is a flowchart for explaining the processing performed by the guidance apparatus for a moving vehicle shown in FIG.

In the flowchart shown in FIG. 3, first, at step 101, it is determined whether or not the moving vehicle S of FIG. 1 is traveling straight (turning).

In step 101, the current motion measuring means 2 shown in FIG.
The turning radius rt of the moving vehicle S is obtained by measuring t and the speed vt. As a result, if the turning radius rt is so large that the trajectory traveled by the moving vehicle S can be regarded as a straight line, it is determined that the moving vehicle S is traveling straight (YES in step 101). On the other hand, unless the turning radius rt is extremely large, it is determined that the moving vehicle S is turning (NO in step 101). Turning radius rt of the moving vehicle S
Is larger than the vehicle body size (for example, the turning radius rt is 10 times the vehicle body size), it can be considered that the moving vehicle S is traveling straight.

If YES is determined in step 101,
Thereafter, the processing is performed assuming that the moving vehicle S is traveling straight (step 102). On the other hand, if NO is determined in step 101, the process is performed assuming that the moving vehicle S is turning thereafter (step 103).

First, a description will be given of a process for moving the traveling vehicle S straight ahead between the end points PR2 and PL2 without steering.

At step 104, it is determined whether or not the moving vehicle S traveling straight ahead can pass between the next end points PR2 and PL2 after reaching the end point PR1 and PL1.

That is, in step 104, it is determined whether or not the traveling condition selecting means 4 in FIG.

If the moving vehicle S can pass between the end points PR2 and PL2 without steering (step 104).
Is YES), the traveling condition selecting means 4 outputs data for causing the moving vehicle S to proceed as it is to the steering command calculating means 5 (step 107).

The processing performed by the traveling condition selecting means 4 will be described with reference to FIGS.

First, a process for confirming passage between the end points PR2 and PL2 will be described with reference to FIG.

In FIG. 4, a pair of end points PR1 and PL1 are defined as a width d, an inclination θ1 with respect to a reference direction, and a center point P1 (x1, y
It is expressed as the line segment G1 of 1). The representative point of the moving vehicle S at time t is Pt (xt, yt), and at time t +
1 is represented by Pt + 1 (xt + 1, yt +
1) Therefore, the equation of the line segment G1 is given by the following (1)
It is expressed by an equation.

(X−x 1) · tan θ 1 − (y−y 1) = 0 (1) Here, the point Pt (xt, yt) exists below the line segment G 1 as shown in FIG. ing. Therefore, the point Pt (xt, y
Substituting t) into the above equation (1), the equation of a straight line passing through the point Pt (xt, yt) is expressed by the following equation (2).

Μt = (xt−x1) · tan θ1− (yt−y1)> 0 (2) On the other hand, the point Pt + 1 (xt + 1, yt + 1) is a line segment as shown in FIG. It is located above G1. Therefore, the point Pt + 1 (x
t + 1, yt + 1) into the above equation (1), the point Pt + 1
The equation of a straight line passing through (xt + 1, yt + 1) is expressed by the following equation (3).

Μt + 1 = (xt + 1−x1) · tan θ1− (yt + 1−y1) <0 (3) Then, the representative point Pt (xt, yt) or Pt + 1 ( If (xt + 1, yt + 1) exists on the line segment G1, the above equation (1) is satisfied, and the value of μt or μt + 1 becomes 0.

When the moving vehicle S passes through the line segment G1, the signs of the above μt and μt + 1 are opposite to each other, or the respective values are 0.

That is, the traveling condition selecting means 4 calculates the representative point Pt (xt, yt) of the moving vehicle S at the time t, which changes every moment as the moving vehicle S advances, using the equation of the line segment G1 as the current target. Μt is obtained by substituting into equation (1), and the representative point Pt + 1 (xt + 1, yt + 1) of the moving vehicle S at time t + 1, which is the next moment after time t, is calculated by the equation (1 ) To obtain μt + 1.

When the product of μt and μt + 1 obtained in this way becomes 0 or the sign of this product becomes negative (when μt · μt + 1 ≦ 0), the moving vehicle S is a line segment G
It turns out that one has passed.

Thus, the traveling condition selecting means 4 confirms whether or not the moving vehicle S has reached the line G1.

Here, the mobile vehicle S is connected to a pair of end points PR2, P
The process that proceeds toward L2 will be described with reference to FIG.

In FIG. 5, the representative point of the moving vehicle S (the point on the line G1) is P1 (x1, y1). Further, the traveling direction of the moving vehicle S is defined as θt. Therefore, the equation of a straight line that is a trajectory when the moving vehicle S goes straight can be expressed by the following equation (4).

(X−x 1) · tan θt− (y−y 1) = 0 (4) Here, when the straight line represented by the equation (4) is set as the boundary line, the endpoint shown in FIG. PR2 (xR2, yR2) is (4)
Since it exists below the straight line in the equation, it is included in the negative region. On the other hand, since the end point PL2 (xL2, yL2) exists above the straight line of the equation (4), it is included in the positive region.

By the way, the point P1 (x1, y1) and the end point PR2
The equation of a straight line passing through (xR2, yR2) is expressed as follows: λR = (xR2−x1) · tan θt− (yR2−y1) (5)

The point P1 (x1, y1) and the end point PL2 (xL
2, yL2) is represented by the following equation: λL = (xL2−x1) · tanθt− (yL2−y1) (6)

Here, since the straight line in equation (5) passes through the negative region, the sign of λR becomes negative. On the other hand, since the straight line in equation (6) passes through the positive region, the sign of λL is positive.

Therefore, when the product of the above equations (5) and (6), λ = λR · λL (7), is obtained, the sign of λ becomes negative.

Further, the straight line of the equation (4) is expressed by an end point PR2 (xR2,
λ is 0 if it passes through one of yR2) and PL2 (xL2, yL2).

Therefore, when λ ≦ 0, it can be seen that the straight line of the equation (4) passes between the end points PR2 (xR2, yR2) and PL2 (xL2, yL2).

As a result, the traveling condition selecting means 4 determines that the moving vehicle S can pass between the end points PR2 and PL2 without steering (determination YES in step 104).

On the basis of the result of the determination by the traveling condition selecting means 4, the steering command calculating means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).

Next, a description will be given of a process of steering the moving vehicle S that is traveling straight and traveling between the pair of end points PR2 and PL2.

If the moving vehicle S cannot pass between the end points PR2 and PL2 (NO in step 104), the traveling condition selecting means 4 outputs data for steering the moving vehicle S to the steering command calculating means 5. I do.

The processing performed by the traveling condition selecting means 4 will be described with reference to FIG.

As shown in FIG. 4, the traveling condition selecting means 4 determines that the moving vehicle S is located between the end points PR1 and PL1 (on the line segment G1).
Check if you have reached.

Here, the above equation (4) (shown below) is expressed as follows: (x−x1) · tanθt− (y−y1) = 0 (4) Above the end points PR2 and PL2 shown in FIG. If present, these end points PR2 and PL2 are included in the negative region.

That is, the signs of λR and λL are each negative.

Therefore, when λ is obtained using the above equation (7) (shown below), λ = λR · λL (7) The sign of λ is positive.

When the above equation (4) exists below the end points PR2 and PL2 shown in FIG. 5, these end points PR2 and PL2 are included in the positive region.

That is, the signs of λR and λL are each positive.

Therefore, when λ is obtained by using the above equation (7), the sign of λ is positive.

That is, when λ> 0, it can be seen that the straight line of the equation (4) does not pass between the end points PR2 and PL2.

Thus, the traveling condition selecting means 4 determines that the moving vehicle S cannot pass between the end points PR2 and PL2 in this state (NO in step 104).

Therefore, the driving condition selecting means 4 sets the target point P2 (x2, y2) acquired from the planned driving route storing means 1.
And the data of the current position P1 (x1, y1) and traveling direction θt of the moving vehicle S acquired from the current motion measuring means 2 are output to the steering command calculating means 5.

The steering command calculating means 5 performs an operation for steering the moving vehicle S (step 106).

The processing performed by the steering command calculating means 5 will be described with reference to FIGS.

The steering command calculating means 5 calculates the acquired target point P2 (x2, y2) and the current position P1 (x1, y) of the moving vehicle S.
1) From the data of the traveling direction θt, the target turning radius rg for moving the vehicle S to the target point P2 (x2, y2).
Is calculated using the following equation (8).

R g = {(x ² −x 1) ² + (y ² −y 1) ² } / {(x 2 −x 1) · sin θt − (y 2 −y 1) · cos θt} / 2 (8) where rg> 0 Turns right and turns left when rg <0.

The moving vehicle S described above is assumed to be a steering-type four-wheeled vehicle.

Here, the steering of the moving vehicle S will be described with reference to FIG. 6 by replacing the moving vehicle S with an equivalent two-wheeled vehicle S0.

In FIG. 6, this equivalent two-wheeled vehicle S0 has a steered wheel S1 and a fixed wheel S2. The distance (wheel base) between the center axis A of the steered wheel S1 and the center axis B of the fixed wheel S2 is L. The turning radius rt of the turning circle and the tangential direction of the turning circle when the equivalent two-wheeled vehicle S0 makes a turning run always have a relationship of a right angle R. The steering angle θt is the turning center point Pc of the turning circle.
It is equal to the surrounding APcB.

Accordingly, the current turning radius rt of the equivalent two-wheeled vehicle S0 is expressed by the following equation (9): rt = L / tan θt (9)

Therefore, from the above equation (9), the target turning radius r
The target steering angle と き at the time of g is obtained as follows: ζ = tan ⁻¹ (L / rg) (10)

The steering command calculating means 5 outputs the thus obtained target steering angle こ う to the moving vehicle S (vehicle body 7) as a steering command to control the steering angle of the equivalent two-wheeled vehicle S0.

As a result, the moving vehicle S is moved to the next end point PR2,
You can head between PL2.

However, the processing of steps 101 to 107 shown in FIG. 3 is repeated about 10 times per second.
As a result, the traveling direction of the moving vehicle S is constantly corrected, so that the moving vehicle S can be directed to the direction between the next end points PR2 and PL2.

Incidentally, the moving vehicle S can be steered by using the turning radius without using the steering system.

FIG. 7 is a diagram for explaining a case in which a skid steer type moving vehicle S is steered. The skid steer system is also called a "differential drive system".

The moving vehicle S0 'of the skid steer system shown in FIG. 7 is provided with two drive wheels SR and SL which can be different from each other in rotational speed. By making the rotation speeds vR, vL of the drive wheels SR, SL different from each other, it is possible to make a turning travel.

Here, the turning radius rc of the moving vehicle S0 'is expressed by the following equation (11) from the vehicle width W and the speeds vR and vL of the drive wheels SR and SL.

Rc = W (vL + vR) / 2 (vL−vR) (11) That is, when moving the vehicle S0 ′ to go straight in order to pass through the target point P2 (x2, y2), the steering command calculation is performed. The means 5 outputs vR = vL as a steering command to the moving vehicle S0 '(vehicle body 7).

Similarly, when the vehicle is turned around the moving vehicle S0 'in order to pass the target point P2 (x2, y2), the following equation (8) (shown below) is used. (X2−x1) ² + (y2−y1) ² ｝ / {(x2−x1) · sinθt− (y2−y1) · cosθt} / 2 (8) Assuming rg = rc, the steering command calculation means 5 turns. The radius rc is obtained, and the turning radius rc is output to the moving vehicle S0 '(vehicle body 7) as a steering command.

The vehicle speed command calculating means 6 calculates the following (1)
2) The vehicle center speed vc is calculated using the equation.

Vc = (vR + vL) / 2 (12) Then, the vehicle speed command calculating means 6 outputs the vehicle center speed vc as a running command to the moving vehicle S0 '(vehicle body 7).

The moving vehicle S0 '(vehicle body 7) receives the steering command and the traveling command, and obtains the speed v from the turning radius rc and the vehicle center speed vc obtained by the above equations (11) and (12).
R and vL are back calculated to adjust the speed.

Thus, even the skid steer type moving vehicle S0 'can be directed between the next end points PR2 and PL2.

When the mobile vehicle S0 'is to be steered from a state where it is traveling straight at the maximum speed vmax, the driving wheels S0'
Although it is necessary to make 1 or S2 higher than the maximum speed vmax, in practice, the driving wheels S1 and S2 cannot be made higher than the maximum speed vmax. At the maximum speed vmax. For the other drive wheel, the speed of the moving vehicle S0 'is inversely calculated by using the above equation (11) so as to maintain the desired turning radius rc, and the speed of the moving vehicle S0' is adjusted.

Next, a description will be given of a process for moving the turning traveling vehicle S directly between the end points PR2 and PL2 without steering.

In step 105, it is determined whether or not the traveling vehicle S can pass between the next end points PR2 and PL2 after reaching the end point PR1 and PL1.

That is, in step 105, it is determined whether or not the traveling condition selecting means 4 in FIG.

Here, if the moving vehicle S can pass between the end points PR2 and PL2 without steering (step 105).
Is YES), the traveling condition selecting means 4 outputs data for causing the moving vehicle S to proceed as it is to the steering command calculating means 5 (step 107).

The processing performed by the traveling condition selecting means 4 will be described with reference to FIG.

In FIG. 8, the moving vehicle S moves in the traveling direction θt,
The vehicle is turning with a turning radius rt.

As shown in FIG. 4 described above, the traveling condition selecting means 4 determines that the moving vehicle S is located between the end points PR1 and PL1 (line G1).
Check whether the above has been reached. And the moving vehicle S
From the turning center point Pc (xc, yc) to the end point PR2 (xR2, xc) after reaching the point between the end points PR1 and PL1 (on the line segment G1).
yR2) and the turning center point Pc (xc, y
The distance rL from c) to the end point PL2 (xL2, yL2) is obtained.

[0152] The distance rR is expressed by rR = √ {(xR2-xc ) 2 + (yR2-yc) 2} ... (13).

[0153] On the other hand, the distance rL is expressed by rL = √ {(xL2-xc ) 2 + (yL2-yc) 2} ... (14).

The speed vt and the turning angular speed ωt of the moving vehicle S are measured by the current motion measuring means 2 and the turning radius rt
Is required.

Here, if the turning radius rt is longer than the distance rR, the sign of the difference λrR between the distance rR and the turning radius rt shown in the following equation (15) becomes negative.

ΛrR = rR− | rt | (15) (The turning radius rt is an absolute value because the sign changes between right turning and left turning.) On the other hand, the turning radius rt is larger than the distance rL. If shorter, the sign of the difference λrL between the distance rL and the turning radius rt shown in the following equation (16) is positive.

ΛrL = rL− | rt | (16) Further, the turning radius rt is equal to the end points PR2 (xR2, yR2) and PL2.
If it passes through one of (xL2, yL2), λrR
Or one of λrL becomes 0.

Therefore, the above (1) shown in the following equation (17)
From the product of the expressions (5) and (16), λ = λrR · λrL (17), the traveling condition selecting means 4 determines that the turning radius rt is equal to the end points PR2 (xR2, yR2), PL2 ( xL2, yL2)
Judge that it passes.

As a result, the traveling condition selection means 4 determines that the moving vehicle S can pass between the end points PR2 and PL2 without steering (determination YES in step 105).

On the basis of the result of the determination by the traveling condition selecting means 4, the steering command calculating means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).

Next, a description will be given of a process for steering the traveling vehicle S to travel between the pair of end points PR2 and PL2.

If the moving vehicle S cannot pass between the end points PR2 and PL2 (NO in step 105), the traveling condition selecting means 4 outputs data for steering the moving vehicle S to the steering command calculating means 5. I do.

The processing performed by the traveling condition selecting means 4 will be described with reference to FIG.

In FIG. 8, as described above, the distance rR from the turning center point Pc (xc, yc) to the end point PR2 (xR2, yR2) and the turning center point Pc (xc, yc) to the end point PL2
The distance rL to (xL2, yL2) and the turning radius rt of the moving vehicle S are obtained.

Further, as shown in FIG. 4 described above, the traveling condition selecting means 4 determines that the moving vehicle S is located between the end points PR1 and PL1 (line segment G1).
1) Check to see if it has been reached.

Here, if the turning radius rt is shorter than the distance rR, the sign of λrR in the equation (15) (shown below) becomes positive.

ΛrR = rR− | rt | (15) On the other hand, when the turning radius rt is shorter than the distance rR, the turning radius rt is naturally shorter than the distance rL. Therefore, the sign of λrL in equation (16) (shown below) is positive.

ΛrL = rL− | rt | (16) Accordingly, when the turning radius rt is shorter than the distances rR and rL, λ in the above equation (17) is as follows: λ = λrR · λrL (17) Becomes positive. (Λ> 0) If the turning radius rt is longer than the distance rL, the above (1)
The sign of λrL in equation (6) is negative.

If the turning radius rt is longer than the distance rL, the turning radius rt is naturally longer than the distance rR. Therefore, the sign of λrR in equation (15) is negative.

Therefore, the turning radius rt is larger than the distances rR and rL.
Is long, λ in the above equation (17) is positive. (Λ>
0) With this, the traveling condition selecting means 4 determines that when λ> 0,
The turning radius rt is the end point PR2 (xR2, yR2), PL2 (xL2,
jL2) is judged to be impossible. (No in step 104).

Therefore, the driving condition selecting means 4 sets the target point P obtained from the planned driving route storing means 1 as described above.
2 (x2, y2) and the current position P1 (x1, y1) and traveling direction θt of the moving vehicle S acquired from the current motion measuring means 2 are output to the steering command calculating means 5.

The steering command calculating means 5 performs an operation for steering the moving vehicle S (step 106).

Here, as described above with reference to FIGS. 5 and 6, the steering command calculating means 5 obtains the acquired target point P2 (x
2, y2), the current position P1 (x1, y1) of the moving vehicle S, and the traveling direction θt, the moving vehicle S is set to the target point P2.
The target turning radius rg for moving to (x2, y2) is obtained by using the above equation (8) (shown below).

R g = {(x ² −x 1) ² + (y ² −y 1) ² } / {(x 2 −x 1) · sin θt − (y 2 −y 1) · cos θt} / 2 (8) ) The target steering angle と き at the target turning radius rg is obtained from the equation (shown below).

Ζ = tan ^-1 (L / rg) (10) The target steering angle ζ obtained in this way is calculated by the steering command calculating means 5.
Outputs a steering command to the moving vehicle S (the vehicle body 7) to control the steering angle of the moving vehicle S.

As a result, the traveling vehicle S
Can be directed between the next end points PR2 and PL2.

As described above, in the above-described embodiment, when the moving body S reaches between the pair of end points PR1 and PL1, the moving body S is directed between the next pair of end points PR2 and PL2. Therefore, even if the planned traveling route K of the moving body S is a sharp curve, the moving body S can be guided without causing the small turning phenomenon.

In the above embodiment, the moving vehicle S
The steering angle 0 at the time of forward movement of 0 and the steering angle 後 at the time of retreat can be made the same.

FIG. 9A is a diagram showing a case where the moving vehicle S0 moves forward toward the center point Pn of the target gate Gn.
FIG. 9B is a diagram showing a case where the moving vehicle S0 moves backward toward the center point Pn of the target gate Gn.

As shown in FIGS. 9A and 9B, the steering angle の of the moving vehicle S0 is the same when moving forward and when moving backward.

Therefore, in the above-described embodiment, the control characteristics when the moving vehicle moves forward and when the vehicle moves backward can be made the same. Further, the control of the steering angle ζ can be simplified.

By the way, because the road surface of the scheduled traveling route K of the moving vehicle S has irregularities or an obstacle exists on the road, it cannot pass between the pair of target end points PLn and PRn, and the moving vehicle S
May deviate from the planned traveling route K.

Therefore, when the moving vehicle S cannot pass between the target pair of end points PLn and PRn, the moving vehicle S is directed to the next pair of end points PLn and PRn. It will be described with reference to FIG. For the sake of explanation,
A pair of target end points PLn and PRn are represented by gates.

In FIG. 10, since the moving vehicle S has deviated from the current target gate Gn, it reaches an extension Gb of the current target gate Gn.

Therefore, the moving vehicle S moves to the current target gate G.
When the vehicle reaches the extension of n, the moving vehicle S is directed to the next target gate Gn + 1.

As a result, the planned traveling route K of the moving vehicle S
The mobile vehicle S can be returned to the planned traveling route K even when the road surface has irregularities or when an obstacle exists on the road and the vehicle cannot pass through the target gate Gn. However, when the moving vehicle S deviates from the current target gate Gn, it is desirable to reduce the speed of the moving vehicle S.

In FIG. 11, the representative points of the moving vehicle S change to Pt-1, Pt, Pt + 1, and these points Pt-1, Pt-1
The distance between t, Pt + 1 and the current end point of the target gate Gn is rt-
When the moving vehicle S moves so as to change to 1, rt, and rt + 1, the moving vehicle S is directed to the next target gate Gn + 1.

As a result, the planned traveling route K of the moving vehicle S
The mobile vehicle S can be returned to the planned traveling route K even when the road surface has irregularities or when an obstacle exists on the road and the vehicle cannot pass through the target gate Gn. However, when the moving vehicle S deviates from the current target gate Gn, it is desirable to reduce the speed of the moving vehicle S.

In the above-described embodiment, the case has been described where the target data on the planned travel route K is a pair of end points. However, the target data on the planned travel route K may be an arbitrary geometric figure.

The operation of the guidance apparatus for a moving vehicle shown in FIG. 2 when the target data on the planned traveling route K is an arbitrary geometric figure will be described with reference to FIGS.

FIG. 12 is a diagram showing an embodiment in which a plurality of target data of the planned traveling route K are a plurality of arbitrary geometric figures Mn (n = 1 to 3).

In FIG. 12, an arbitrary geometric figure Mn is set as a target on the planned traveling route K.

[0193] Step 1 in the flowchart of FIG.
The processing performed in 01 to 103 is the same as the processing in the case where the target data on the planned traveling route K is a pair of end points, and thus the description is omitted.

First, a description will be given of a process of moving a straight moving vehicle S toward an arbitrary geometrical figure M2 without steering.

In step 104, the moving vehicle S traveling straight ahead travels a predetermined distance after first detecting the current arbitrary geometrical figure M1, and at a predetermined point P1 in the arbitrary geometrical figure M1. Is reached, it is determined whether or not it can pass through the next arbitrary geometric figure M2.

At this time, in step 104, it is determined whether or not the traveling condition selecting means 4 shown in FIG.

Here, when the moving vehicle S can pass through any geometrical figure M2 without steering (step 1).
04, YES), the traveling condition selecting means 4 outputs data for causing the moving vehicle S to proceed as it is to the steering command calculating means 5 (step 107).

Referring to FIG. 13, a description will be given of the processing of the traveling condition selecting means 4 for causing the moving vehicle S traveling straight to proceed into an arbitrary geometrical figure M2 without steering.

This processing is similar to that of FIG. 5 except that a pair of end points PRn and PLn are placed at a position perpendicular to the traveling direction θt of the moving vehicle S in an arbitrary geometrical figure M2 with the target point P2 interposed therebetween. This is the same as the processing described with reference to FIG.

In FIG. 13 as well, the equation of the straight line which is the trajectory when the moving vehicle S goes straight is expressed by the above equation (4) (shown below).

(X−x 1) · tan θt− (y−y 1) = 0 (4) Therefore, when the straight line represented by the above equation (4) is used as the boundary line, an arbitrary value shown in FIG. Since the end point PR2 (xR2, yR2) in the geometric figure M2 exists below the straight line of the equation (4), it is included in the negative area. On the other hand, the end point PL2 (xL2,
Since yL2) exists above the straight line in equation (4), it is included in the positive region.

The point P1 (x1, y1) and the end point PR2 (xR
2, yR2) is expressed by the above equation (5) (shown below).

ΛR = (xR2−x1) · tan θt− (yR2−y1) (5) Further, the equation of the straight line passing through the point P1 (x1, y1) and the end point PL2 (xL2, yL2) is given by the above equation (6). ) Formula (shown below).

ΛL = (xL2−x1) · tan θt− (yL2−y1) (6) Here, since the straight line of the above equation (5) passes through a negative region, the sign of λR becomes negative. On the other hand, since the straight line in the above equation (6) passes through the positive region, the sign of λL is positive.

Therefore, when the above equation (7) (shown below) is obtained, λ = λR · λL (7) The sign of λ is negative.

Further, the straight line of the above equation (4) is defined by the end points PR2 (xR2, yR2) and PL2 (xL2, yL) in an arbitrary geometrical figure M2.
If it passes through one of 2), λ is 0.

Therefore, when λ ≦ 0, the straight line of the above equation (4) is converted to an end point PR2 (xR2, yR) in an arbitrary geometrical figure M2.
2), it passes through PL2 (xL2, yL2).

As a result, the traveling condition selecting means 4 determines that the moving vehicle S does not need to steer any geometrical figure M2 as it is.
It is determined that the vehicle can pass through the vehicle (determination YE in step 104).
S).

On the basis of the result of the determination by the traveling condition selecting means 4, the steering command calculating means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).

Next, a description will be given of a process of steering a moving vehicle S that is traveling straight and traveling between end points PR2 and PL2 in an arbitrary geometric figure M2.

When the moving vehicle S cannot pass between the end points PR2 and PL2 in the arbitrary geometrical figure M2 (NO in step 104), the traveling condition selecting means 4 steers data for steering the moving vehicle S. Output to the command calculation means 5.

The processing performed by the traveling condition selecting means 4 will be described with reference to FIG.

As shown in FIG. 12 described above, the traveling condition selecting means 4 determines that the traveling vehicle S has traveled a predetermined distance since the current arbitrary geometric figure M1 is first detected, and It is confirmed whether or not a predetermined point P1 in M1 has been reached.

Here, the above equation (4) (shown below) is expressed as follows: (x−x1) · tan θt− (y−y1) = 0 (4) Above the end points PR2 and PL2 shown in FIG. If present, the end points PR2 in these arbitrary geometrical figures M1,
PL2 is included in the negative region.

That is, the signs of λR and λL are each negative.

Therefore, when λ is obtained using the above equation (7) (shown below), λ = λR · λL (7) The sign of λ is positive.

When the equation (4) exists below the end points PR2 and PL2 shown in FIG. 13, these end points PR2 and PL2 are included in the positive region.

That is, the signs of λR and λL are each positive.

Therefore, when λ is obtained using the above equation (7), the sign of λ is positive.

That is, when λ> 0, it can be seen that the straight line of the equation (4) does not pass between the end points PR2 and PL2.

In this state, the traveling condition selecting means 4 determines that the moving vehicle S is in this state, the end point PR in the arbitrary geometrical figure M1.
2. It is determined that the vehicle cannot pass between PL2 (step 104).
NO).

Therefore, the driving condition selecting means 4 stores the target point P obtained from the planned driving route storing means 1 as described above.
2 (x2, y2) and the current position P1 (x1, y1) and traveling direction θt of the moving vehicle S acquired from the current motion measuring means 2 are output to the steering command calculating means 5.

The steering command calculating means 5 performs an operation for steering the moving vehicle S (step 106).

Here, as described above with reference to FIGS. 6 and 13, the steering command calculating means 5 sets the acquired target point P2
(X2, y2), the current position P1 (x1, y) of the moving vehicle S
1) From the data of the traveling direction θt, the target turning radius rg for moving the vehicle S to the target point P2 (x2, y2).
Is determined using the above equation (8) (shown below).

R g = {(x ² −x 1) ² + (y ² −y 1) ² } / {(x 2 −x 1) · sin θt − (y 2 −y 1) · cos θt} / 2 (8) ) The target steering angle と き at the target turning radius rg is obtained from the equation (shown below).

Ζ = tan ⁻¹ (L / rg) (10) The target steering angle ζ obtained in this way is calculated by the steering command calculating means 5.
Outputs a steering command to the moving vehicle S (the vehicle body 7) to control the steering angle of the moving vehicle S.

As a result, the moving vehicle S traveling straight can be directed to the next arbitrary geometric figure M2.

Next, a description will be given of a process of moving the turning traveling vehicle S toward an arbitrary geometrical figure M2 without steering.

At step 105, the turning traveling vehicle S travels a predetermined distance after the current arbitrary geometric figure M1 is first detected, and the arbitrary geometric figure M
It is determined whether or not a predetermined point P1 in 1 has been reached.

That is, at step 105, it is determined whether or not the traveling condition selecting means 4 of FIG.

If the moving vehicle S can be directed to an arbitrary geometric figure M2 without steering (judgment YES in step 105), the traveling condition selecting means 4
Outputs to the steering command calculating means 5 data for causing the moving vehicle S to proceed as it is (step 10).
7).

The processing performed by the traveling condition selecting means 4 will be described with reference to FIG.

In FIG. 14, the moving vehicle S moves in the traveling direction Θ.
t, the vehicle is turning with a turning radius rt.

In any geometrical figure M2,
It is assumed that a pair of end points PR2 (XR2, YL2) and PL2 (XL2, YL2) exist at a position perpendicular to the traveling direction θt of the moving vehicle S. Here, end points PR2 (XR2, YR2) and PL2 (X
L2, YL2) is d.

Further, the center point between these end points PR2 and PL2 is defined as P
2 (X2, Y2). Further, the moving vehicle S is located at the end point PR.
2. The distance from the center point P2 (X2, Y2) at the time of passing between PL2 is defined as a lateral shift η.

The lateral shift η is determined by the following equation: the traveling vehicle S travels a predetermined distance after the current arbitrary geometric figure M1 is first detected, and a predetermined point P1 in the arbitrary geometric figure M1. Is calculated using the following equations (18) and (19).

That is, based on the turning radius rt obtained by the current motion measuring means 2 and the traveling direction Δt of the moving vehicle S, if the turning radius rt is larger than 0, η = Y−rtcosΘt + √ ｛rt ² − (X + rtsinΘt)) ² … (18), and when the turning radius rt is smaller than 0, η = Y-rtcosΘt-√ ｛rt ^2- (X + rtsinΘt) ² … (19) .

Further, the direction vector of Δt in the above equations (18) and (19) indicates that the current traveling direction θt of the moving vehicle S is the target point P2 (X) in the next arbitrary geometric figure M2.
2, Y2) is coordinate-transformed according to the traveling direction, and is represented by the following equation (20).

[0239] Further, the coordinate axes X and Y are obtained by performing coordinate conversion on the coordinate axes x and y, and are represented by the following equation (21).

[0240] Further, the direction vector cos θ2 of θ2 in the above equation (21),
In order to obtain sin θ2, first, cosφt and sinφt are obtained from the current position P1 (x1, y1) of the moving vehicle S and the target point P2 (x2, y2) using the following equations (22) and (23).
Ask for.

[0241] cosφt = (x2-x1) / {(x2-x1) 2 + (y2-y1) 2} ... (22) sinφt = (y2-y1) / {(x2-x1) 2 + (y2-y1 ) ² ｝ (23) And, from these cosφt and sinφt, the following (24)
The direction vector cos θ2 of θ2 by using the equation (25),
sin θ2 is determined.

Cos θ2 = (cos ² φt−sin ² φt) · cos θt + ² sin φt · cos φt · sin θt (24) sin θ2 = + ² sin φt · cos φt · cos θt− (cos ² φt−sin ² φt) · sin θt (25) , The lateral shift η obtained using the above equations (18) and (19), and the end points PR2 (XR2, YR2) and PL2 (X
L2, YL2) is -d / 2 <η <d / 2
, The traveling condition selecting means 4 determines that the moving vehicle S has the end points PR2 (xR2, yR2) and PL2 (xL) in an arbitrary geometrical figure M2.
2, jL2) judge that it passes.

On the basis of the result of the determination by the traveling condition selecting means 4, the steering command calculating means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).

Next, a description will be given of a process of steering the moving vehicle S which is turning and traveling toward an arbitrary geometrical figure M2.

If the moving vehicle S cannot pass through the arbitrary geometrical figure M2 (NO in step 105), the traveling condition selecting means 4 sends data for steering the moving vehicle S to the steering command calculating means 5. Output.

The processing performed by the traveling condition selecting means 4 will be described with reference to FIG.

As described above, the lateral deviation η is determined by the fact that the moving vehicle S travels a predetermined distance after the current arbitrary geometrical figure M1 is first detected, and the predetermined deviation within the arbitrary geometrical figure M1. After the point P1 is reached, the above (18),
It is determined using the equation (19) (shown below).

Η = Y−rtcosΘt + √ ｛rt ² − (X + rtsinΘt)) ² … (18) η = Y−rtcosΘt−√ ｛rt ² − (X + rtsinΘt)) ² … (19) where (18) ) And (19) are used to determine the end points PR2 (XR2, YR2) and PL2 (XL
2, YL2), the relationship between the lateral shift η and the distance d between the end points PR2, PL2 is η <−d / 2, d / 2.
<Η.

As a result, the traveling condition selecting means 4 determines that the moving vehicle S is at an end point PR2 (xR2, yR) in an arbitrary geometrical figure M2.
2) It is determined that the vehicle cannot pass between PL2 (xL2, yL2). (No in step 104).

Therefore, the driving condition selecting means 4 sets the target point P obtained from the planned driving route storing means 1 as described above.
2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S acquired from the current motion measuring means 2, and the data of the traveling direction Δt are output to the steering command calculating means 5.

The steering command calculating means 5 performs an operation for steering the moving vehicle S (step 106).

Here, as described above with reference to FIGS. 6 and 13, the steering command calculating means 5 sets the acquired target point P2
(X2, y2), the current position P1 (x1, y) of the moving vehicle S
1) The target turning radius rg for moving the vehicle S to the target point P2 (x2, y2) from each data of the traveling direction Δt.
Is determined using the above equation (8) (shown below).

R g = {(x ² −x 1) ² + (y ² −y 1) ² } / {(x 2 −x 1) · sin {t− (y 2 −y 1) · cos {t} / 2 (8) ) The target steering angle と き at the target turning radius rg is obtained from the equation (shown below).

Ζ = tan ⁻¹ (L / rg) (10) The target steering angle ζ obtained in this way is calculated by the steering command calculating means 5.
Outputs a steering command to the moving vehicle S (the vehicle body 7) to control the steering angle of the moving vehicle S.

As a result, the traveling vehicle S
Can be directed to the next arbitrary geometric figure M2.

As described above, in the above-described embodiment, the moving body S travels a predetermined distance after the first detection of the arbitrary geometrical figure M1, and the predetermined point P1 in the arbitrary geometrical figure M1. At the time of reaching the next arbitrary geometric figure M2
The moving body S is directed to
Even if the target on the planned traveling route K is not a pair of endpoints but an arbitrary geometric figure, the moving body S can be guided without causing the small turning phenomenon as in the case of the pair of endpoints.

In this embodiment, the steering angle 時 when the moving vehicle S moves forward and the steering angle 時 when the moving vehicle S moves backward can be the same as in the case of the pair of end points.

FIG. 15A is a diagram showing a case where the moving vehicle S0 moves forward toward a target point Pn of an arbitrary geometrical figure Mn, and FIG. It is a figure showing the case where mobile vehicle S0 moves backward toward point Pn.

As shown in FIGS. 15A and 15B, the steering angle ζ of the moving vehicle S0 is the same when moving forward and when moving backward.

Therefore, in this embodiment, the control characteristics when the moving vehicle S moves forward and when it moves backward can be the same as in the case of the pair of end points. Further, the control of the steering angle ζ can be simplified.

In the above-described embodiment, the target data on the planned traveling route K has been described as an arbitrary geometric figure. However, the target data may of course be circular.

The operation of the guidance apparatus for a moving vehicle shown in FIG. 2 when the target data on the planned traveling route K is circular will be described with reference to FIGS.

FIG. 16 is a diagram showing an embodiment in which the target data of the planned traveling route K is a circle C1.

In FIG. 16, a circle C 1 is set as a target on the planned traveling route K.

Step 1 in the flowchart of FIG.
The processing performed in 01 to 103 is the same as the processing in the case where the target data on the planned traveling route K is a pair of end points or an arbitrary geometrical figure, and thus the description is omitted.

First, a description will be given of a process in which the moving vehicle S that is traveling straight ahead is advanced toward the circle C2 without steering.

In step 104, the moving vehicle S traveling straight ahead travels a predetermined distance after first detecting the current circle C1 and reaches a predetermined point P1 in the circle C1. It is determined whether the vehicle can pass through the circle C2.

At this time, in step 104, it is determined whether or not the traveling condition selecting means 4 shown in FIG.

Here, when the moving vehicle S can pass through the circle C2 without steering (judgment YE in step 104).
S), the traveling condition selecting means 4 outputs data for causing the moving vehicle S to proceed as it is to the steering command calculating means 5 (step 107).

Referring to FIG. 17, a description will be given of a process of the traveling condition selecting means 4 for causing the traveling vehicle S to proceed straight into the circle C2 without steering.

In FIG. 17, the equation of a straight line which is a trajectory when the moving vehicle S goes straight ahead is expressed by the above equation (4) (shown below).

(X−x1) · tanθt− (y−y1) = 0 (4) Further, the equation of the center point P2 (x2, y2) and the circle C2 with the radius d / 2 is given by the following equation (26). expressed.

(Xx−2) ² + (y−y2) ² = (d / 2) ² (26) Therefore, the above equations (4) and (26) are regarded as simultaneous equations, and A solution is required.

Therefore, if the solution of this simultaneous equation is a real number solution, it is understood that the straight line of the above equation (4) and the circle C2 intersect.

As a result, the traveling condition selecting means 4 determines that the moving vehicle S can pass through the circle C2 without steering (determination YES in step 104).

On the basis of the result of the determination by the traveling condition selecting means 4, the steering command calculating means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).

Next, a description will be given of a process of steering the moving vehicle S that is traveling straight and moving toward the circle C2.

If the moving vehicle S cannot pass through the circle C2 (NO in step 104), the traveling condition selecting means 4
Outputs data for steering the moving vehicle S to the steering command calculating means 5.

The processing performed by the driving condition selecting means 4 will be described with reference to FIG.

As shown in FIG. 17 described above, the traveling condition selecting means 4 moves a predetermined distance after the moving vehicle S first detects the current circle C1 and moves to a predetermined point P1 in the circle C1. Check if it has been reached.

When the moving vehicle S is steered to the circle C2 (NO in step 104), the traveling condition selecting means 4 calculates the current relative position of the moving vehicle S with respect to the circle C2 by the steering command calculating means 5. (Step 1
07).

The moving vehicle S traveling straight is steered to form a circle C2.
The process performed by the traveling condition selecting means 4 for inward traveling will be described with reference to FIG.

In FIG. 17, the above equation (4) and (26)
The solution of the simultaneous equation with the equation (shown below) is obtained.

(X−x1) · tan θt− (y−y1) = 0 (4) (xx−2) ² + (y−y2) ² = (d / 2) ² (26) where If the solution of the simultaneous equations is not a real number solution, it is understood that the straight line of the above equation (4) does not intersect with the circle C2 of the above equation (26).

Thus, the traveling condition selecting means 4 determines that the moving vehicle S cannot pass through the circle C2 in this state (NO in step 104).

Therefore, the driving condition selecting means 4 sets the target point P obtained from the planned driving route storing means 1 as described above.
2 (x2, y2) and the current position P1 (x1, y1) and traveling direction θt of the moving vehicle S acquired from the current motion measuring means 2 are output to the steering command calculating means 5.

The steering command calculating means 5 performs an operation for steering the moving vehicle S (step 106).

Here, as described above with reference to FIGS. 6 and 13, the steering command calculating means 5 sets the acquired target point P2
(X2, y2), the current position P1 (x1, y) of the moving vehicle S
1) From the data of the traveling direction θt, the target turning radius rg for moving the vehicle S to the target point P2 (x2, y2).
Is determined using the above equation (8) (shown below).

R g = {(x ² −x 1) ² + (y ² −y 1) ² } / {(x 2 −x 1) · sin θt − (y 2 −y 1) · cos θt} / 2 (8) ) The target steering angle と き at the target turning radius rg is obtained from the equation (shown below).

Ζ = tan ^-1 (L / rg) (10) The target steering angle ζ obtained in this way is calculated by the steering command calculating means 5.
Outputs a steering command to the moving vehicle S (the vehicle body 7) to control the steering angle of the moving vehicle S.

Thus, the moving vehicle S traveling straight can be directed to the next circle C2.

Next, a description will be given of a process of moving the turning traveling vehicle S toward the circle C2 without steering.

[0293] In step 105, it is determined whether or not the turning traveling vehicle S has traveled a predetermined distance since it first detected the current circle C1 and has reached a predetermined point P1 in the circle C1. judge.

That is, in step 105, it is determined whether or not the traveling condition selection means 4 in FIG.

Here, when the moving vehicle S can be directed to the circle C2 without steering (step 10).
5, the traveling condition selecting means 4 determines whether the traveling vehicle S
Is output to the steering command calculating means 5 (step 107).

The processing performed by the running condition selecting means 4 will be described with reference to FIG.

In FIG. 18, the moving vehicle S moves in the traveling direction Θ.
t, turning at the turning radius rt, and the center point P2 (X2, Y
2) It is assumed that it is directed to a circle C2 having a radius of d / 2. Further, the distance from the center point P2 when the moving vehicle S passes the diameter d of the circle C2 is defined as a lateral shift η.

[0298] The lateral shift η is determined by the following equation after the moving vehicle S travels a predetermined distance after the current circle C1 is first detected and reaches a predetermined target point P1 in the circle C1. It is obtained using the equations (18) and (19) (shown below).

That is, from the turning radius rt obtained by the current motion measuring means 2 and the traveling direction Δt of the moving vehicle S, if the turning radius rt is larger than 0, η = Y−rtcosΘt + √ ｛rt ² − (X + rtsinΘt)) ² … (18), and when the turning radius rt is smaller than 0, η = Y-rtcosΘt-√ ｛rt ^2- (X + rtsinΘt) ² … (19) .

Here, the relationship between the lateral shift η obtained by using the above equations (18) and (19) and the radius d / 2 of the circle C2 is -d / 2 <η <d / 2. , The traveling condition selecting means 4 determines that the moving vehicle S passes through the circle C2.

Based on the result of the determination by the traveling condition selecting means 4, the steering command calculating means 5 outputs a steering command to the moving vehicle S so as to maintain the steering as it is (step 107).

Next, a description will be given of a process of steering the moving vehicle S turning and traveling toward the circle C2.

When the moving vehicle S cannot pass through the circle M2 (NO in step 105), the traveling condition selecting means 4
Outputs data for steering the moving vehicle S to the steering command calculating means 5.

The processing performed by the traveling condition selecting means 4 will be described with reference to FIG.

As described above, the lateral deviation η is equal to or less than the time when the moving vehicle S travels a predetermined distance after the current detection of the current circle C1 and reaches the predetermined point P1 in the circle C1. , (18) and (19) (shown below).

Η = Y−rtcosΘt + √ ｛rt ² − (X + rtsinΘt)) ² … (18) η = Y−rtcosΘt-√ ｛rt ² − (X + rtsinΘt)) ² … (19) where (18) ) And (19), when the lateral shift η is not included in the diameter d of the circle C2, the relationship between the lateral shift η and the radius d / 2 of the circle C2 is η <−. d /
2, d / 2 <η.

[0307] Thus, the traveling condition selecting means 4 determines that the moving vehicle S cannot pass through the circle C2. (No in step 104).

[0308] Therefore, the driving condition selecting means 4 obtains the target point P obtained from the planned driving route storing means 1 as described above.
2 (x2, y2), the current position P1 (x1, y1) of the moving vehicle S acquired from the current motion measuring means 2, and the data of the traveling direction Δt are output to the steering command calculating means 5.

The steering command calculating means 5 performs an operation for steering the moving vehicle S (step 106).

Here, as described above with reference to FIGS. 6 and 17, the steering command calculating means 5 sets the acquired target point P2
(X2, y2), the current position P1 (x1, y) of the moving vehicle S
1) The target turning radius rg for moving the vehicle S to the target point P2 (x2, y2) from each data of the traveling direction Δt.
Is determined using the above equation (8) (shown below).

R g = {(x ² −x 1) ² + (y ² −y 1) ² } / {(x 2 −x 1) · sin {t− (y 2 −y 1) · cos {t} / 2 (8) ) The target steering angle と き at the target turning radius rg is obtained from the equation (shown below).

Ζ = tan ⁻¹ (L / rg) (10) The target steering angle ζ obtained in this way is calculated by the steering command calculating means 5.
Outputs a steering command to the moving vehicle S (the vehicle body 7) to control the steering angle of the moving vehicle S.

As a result, the moving vehicle S that is turning
Can be directed to the circle C2.

As described above, in the above-described embodiment, when the moving body S travels a predetermined distance after the circle C1 is first detected and reaches the predetermined point P1 in the circle C1, the next time. Since the moving object S is directed to the circle C2,
Even if the target on the scheduled traveling route K of the moving body S is a pair of end points, the moving body S can be guided without causing the small turning phenomenon as in the case of the pair of end points.

In this embodiment, the steering angle の when the moving vehicle S moves forward and the steering angle の when the moving vehicle S moves backward can be the same as in the case of the pair of end points.

FIG. 19A shows a target point (circle Cn) in the circle Cn.
FIG. 19B is a diagram showing a case where the moving vehicle S0 moves forward toward the target point Pn of the circle Cn.

As shown in FIGS. 19 (a) and 19 (b), the steering angle の of the moving vehicle S0 is the same when moving forward and when moving backward.

Therefore, in this embodiment, the control characteristics of the moving vehicle S0 when moving forward and when moving backward can be made the same as in the case of the pair of end points. Further, the control of the steering angle ζ can be simplified.

Next, generation of target data on the planned traveling route K will be described.

For example, target data of a pair of end points is generated from point Pn (xn, yn) and point sequence data composed of a circle having a radius dn / 2 centered on the point Pn (xn, yn). .

[0321] Here, a pair of end points will be described as gates.

Also, the point sequence data is generated by CAD as described above.
(Computer Automation Design) and teaching playback.

The data converted from the point sequence data to the gate is stored in the planned traveling route storage means 1 as target data.

FIG. 20A is a diagram showing a case where the density of point sequence data is sufficiently dense as compared with the curvature of the planned route K, and FIG. It is a figure showing the case where it becomes sparse compared with the curvature of K.

As shown in the traveling route Ka2 of FIG. 20B, if the density of the point sequence data is lower than the curvature of the planned route K, the curve of the planned traveling route K cannot be reproduced.

Therefore, in describing the generation of the target data on the planned travel route K, as shown in FIG. 20A, the density of the point sequence data at which the target data is generated depends on the curvature of the planned route K. It is assumed that the density is sufficiently high.

The density of the point sequence data at which the target data is generated is such that when the three adjacent target points are connected by an arc, the curve of the planned traveling route K can be sufficiently reproduced as shown in the traveling route Ka1. And

Therefore, assuming point sequence data in which three adjacent target points are aligned on a straight line, when the moving vehicle S passes around the first target point P1 and heads for the next target point P2, A case where the target gate G2 is generated from the target point P2 will be described with reference to FIG.

FIG. 21 is a diagram showing point sequence data in which three target points are aligned on a straight line.

As shown in FIG. 21, when three target points are aligned, the direction vector from the target point P1 to P2 and the direction vector from P1 to P3 are the same.

Therefore, the relationship between these direction vectors is expressed by the following equation (27).

(X 3 −x 1) / (y 3 −y 1) = (x 2 −x 1) / (y 2 −y 1) (27) By further transforming the above equation (27), the following equation (28) is obtained.

(X3−x1) · (y2−y1) − (x2−x1) · (y3−y1) = 0 (28) Here, when the above equation (28) is satisfied, the target point P1 , P2, and P3 are on a straight line.

The direction vector of the target gate G2 generated from the target point P2 is orthogonal to the direction vector from the target point P1 (x1, y1) to P2 (x2, y2). Therefore, the inclination θ2 of the target gate G2 is expressed by the following equation (29).

[0335] Therefore, when the planned traveling route K is a straight line, the target gate G2 generated from the target point P2 becomes the center point P2 (x2, y
2), the width d2 (diameter of a circle centered on the target point P2 (x2, y2)), and the direction vector (cos θ2, sin θ2) expressed by the above equation (29).

If the above equation (28) (shown below) does not hold, the target points P1, P2, P3
Since the points are not on a straight line, these target points P1, P2, P3
In order to pass through, the moving vehicle S must perform a turning operation while drawing, for example, an arc.

(X3−x1) · (y2−y1) − (x2−x1) · (y3−y1) = 0 (28) FIG. 22 shows that three target points P1, P2, and P3 correspond to the moving vehicle S.
FIG. 6 is a diagram showing point sequence data arranged on the circumference of the turning circle of FIG.

In this case, as shown in FIG. 22, the planned traveling route K is a curve.

Therefore, assuming point sequence data in which the next three adjacent target points are arranged on the circumference of the turning circle of the moving vehicle S, the moving vehicle S passes through the periphery of the first target point P1. A case where the target gate G2 is generated from the target point P2 when heading to the next target point P2 will be described with reference to FIGS.

FIG. 23 is a diagram showing a circle passing through the three target points P1, P2, and P3 and the inclination of the target gate.

Here, the direction vector (cos θ2, sin θ2) of the target gate G2 generated from the target point P2
Is equal to the vector directed from the center point P0 (x0, y0) of the circle in FIG. 23 to the center point P2 (x2, y2) (target point P2) of the target gate G2.

Therefore, the direction vector (cos θ2, sin θ2) of the target gate G2 is expressed by the following equation (30).

[0343] Also, it passes through the center point P2 (x2, y2) of the target gate G2,
The radius r0 of the circle at the center point P0 (x0, y0) is expressed by the following equation (31).

R 0 = {(x 2 −x 0) ² + (y ² −y 0) ² } (31) Here, the setting of the center point P 0 (x 0, y 0) of the circle will be described.

FIG. 24 is a view for explaining the setting of the center point P0 (x0, y0) of a circle passing through the three target points P1, P2, P3.

The center point P0 (x0, y0) is the target point P1,
A straight line T that bisects the line connecting P2 vertically and a target point P
2 is an intersection with a straight line S that bisects the line connecting P3 vertically.

Here, the straight line T is expressed by the following equation (32).
The straight line S is represented by the following equation (33).

[0348] Therefore, the center point P0 (x
The coordinates of (0, y0) are expressed by the following equation (34).

[0349] Where t0 = {(− xb + xd) · yc − (− yb + yd) · xc} / (xa · yc−ya · xc) (35) Therefore, when three adjacent target points P1, P2, and P3 exist on the circumference, the target gate G2 generated from the target point P2 has a center point P2 (x2, y2) and a width d2 (the target point P2 ( x2, y2)) and the direction vector (cos θ2, si
nθ2). Further, it passes through the center point P2 (x2, y2) of the target gate G2 and passes through the center point P0.
The radius r0 of the circle of (x0, y0) can be expressed by the above equation (31).

As described above, in the present embodiment, the target gate is generated from the point sequence data in the case where the planned traveling route K ahead of the moving vehicle S is a curve. Can be reduced as compared with a case where is generated and stored as a target gate. As a result, the operation of generating the target gate from the point sequence data becomes easier than the case of performing the operation of generating all of the point sequence data on the target gate.

In this embodiment, a target gate is generated from the point sequence data for a plurality of target point sequences that are to pass shortly in front of the moving vehicle S.
In the present invention, a target gate may be generated from the point sequence data for one target point that is scheduled to pass immediately before the moving vehicle S.

In the present embodiment, when the planned traveling route K in front of the moving vehicle S is a curve, the target gate is determined from the point sequence data for a plurality of point sequences scheduled to pass immediately before the moving vehicle S. However, even if the planned traveling route K ahead of the moving vehicle S is a straight line, one or a plurality of point sequences scheduled to pass shortly in front of the moving vehicle S are obtained from the point sequence data. A gate may be generated.

Further, even if the planned traveling route K of the moving body S is a sharp curve, the moving body S can be guided without causing the small turning phenomenon.

By the way, three adjacent target points P1, P
When the moving vehicle S travels on the circumference of a circle having a center point P0 (x0, y0) and a radius r0 passing through 2, P3, the following examples (a), (b), and (c) In this case, it is necessary for the vehicle speed command calculation means 6 to output a traveling command for the moving vehicle S to reduce the speed appropriately so as to be lower than the allowable speed.

Example (a): The moving vehicle S has a small turning radius r.
When traveling at a high speed on a curve with zero, a centrifugal force is generated, and the vehicle body may skid and be unable to turn the curve. In this case, it is necessary to automatically decelerate the moving vehicle to a turnable speed in advance.

Example (b): At the moment when the moving vehicle S enters a curve with a small turning radius r0 from straight traveling, passes through the target gate G1, and goes to the next target gate G2, the moving vehicle S may be a steering vehicle. If the steering changes from a straight line to a sharp turn, it must be done. At this time, if the moving vehicle S is running at a high speed, the curve may not be able to be completely turned even when the steering is turned sharply. In this case, it is necessary to automatically decelerate the moving vehicle to a steerable speed in advance.

Example (c): In the above examples (a) and (b),
Even if the moving vehicle S is greatly decelerated so as to move to the target gate G2 when passing through the target gate G1, the moving vehicle S
Is traveling at a very high speed, it may not be possible to turn a curve around the target gate G2.

Therefore, when the target gate is generated from the point sequence data, the target gate is generated from the point sequence data not at the time of passing through the target gate G1, but at a sufficiently early time at which the moving vehicle can safely decelerate or stop. There is a need.

If there is an unplanned obstacle on the planned traveling route, it is necessary to take measures such as avoiding the moving vehicle from the obstacle or decelerating and stopping the moving vehicle immediately before the obstacle. become.

Therefore, in the case where the moving vehicle is avoided from the obstacle or the moving vehicle is decelerated and stopped immediately before the obstacle, data different from the target data on the planned traveling route is generated in FIGS. 2 and 25. This will be described with reference to FIG.

FIG. 25 is a diagram showing an embodiment in which data different from target data on a planned traveling route is generated when a moving vehicle is avoided from an obstacle.

In this embodiment, the target data is point sequence data, and data different from the target data is point sequence data.

In FIG. 25, the moving vehicle S is
When the vehicle travels sequentially through the upper target points P1, P2, P3, P4, P5, P6, and P7, α in front of the moving vehicle S
It is assumed that an obstacle SB exists at a distance of m (between target points P3 and P4 on the planned traveling route K). In FIG. 25, it is assumed that an oncoming vehicle TK travels from the right front of the moving vehicle S.

Here, if an obstacle SB exists on the planned traveling route K, the obstacle detecting means 8 in FIG.
The information about the obstacle shown in (e), (f), and (g) is detected.

(D) The obstacle SB exists in a range of ± β degrees in the left-right direction with the distance αm ahead of the moving vehicle S and the center of the front of the course as the center.

(E) Since the oncoming vehicle runs from the right front of the moving vehicle S, it is dangerous to avoid it on the right side.

(F) There is no obstacle in front of the left of the moving vehicle S.

(G) No other vehicle is approaching from the rear left of the moving vehicle S.

[0369] The obstacle detecting means 8 is provided by the above (d),
The information about the obstacles shown in (e), (f), and (g) is output to the scheduled traveling route changing means 3.

[0370] The planned traveling route changing means 3 receives the information on the obstacle from the obstacle detecting means 8 and acquires the information on the current motion state of the moving vehicle S from the current motion measuring means 2. Then, based on the information on these obstacles and the information on the current motion state of the moving vehicle S, the traveling condition selecting means 4 informs the traveling condition selecting means 4 that “the obstacle SB is located at a distance α from the moving vehicle S on the planned traveling route K Exists, change the planned traveling route K to the left with respect to the traveling direction.A new obstacle is found in the route when the obstacle SB is returned to the original planned traveling route K. In this case, decelerate to a stop, and if it is possible to return to the original planned traveling route S, return to the original planned traveling route K and travel as it is. "

[0371] The traveling condition selecting means 4 receives the signal from the planned traveling route changing means 3 that "an obstacle SB exists at a point a distance α from the moving vehicle S on the planned traveling route K, Avoid the direction and prepare to stop. "

The driving condition selecting means 4 outputs driving condition data to the vehicle speed command calculating means 6 based on the information so as to decelerate as quickly as possible within a range where safety can be ensured. Then, the vehicle speed command calculation means 6 outputs a traveling command to the moving vehicle S to decelerate. Further, the traveling condition selecting means 4 starts from a point at a distance αm from the moving vehicle S,
The target points P2, P3, P4, P5, P6, and P7 on the planned traveling route K are arranged so that the moving vehicle S can travel in the direction of the left β degree with respect to the traveling direction and by the width of the moving vehicle S. Different points VP2, VP3, VP4 (failure avoidance point), VP5 (stoppable point), and VP6 (safe stop point) are generated.

VP4 (failure avoidance point) is a point obtained by moving the target point P4 in the direction of left β degrees.

VP5 (stoppable point) is a point at which the moving vehicle S can be stopped even when the road surface condition is poor.

VP6 (safe stop point) is the next point farther than VP5.

Points VP2, VP3, VP4, VP5, VP6
Is generated, the driving condition selecting means 4 determines whether the points VP2, V
Gates are further generated from P3, VP4, VP5, and VP6.

[0377] The running condition selecting means 4 gives the steering command calculating means 5 to the steering command calculating means 5 based on the information on the current motion state of the moving vehicle S and the above-mentioned information on the change of the planned running route K. Relative location information, while
The running conditions are output to the vehicle speed command calculating means 6.

Further, a steering command (a steering angle required to pass through the gate) is output from the steering command calculating means 5 to the moving vehicle S, and a traveling command (passing through the gate) is output from the vehicle speed command calculating means 6. The speed required for this is output.

Thus, the points VP2, VP3, VP4,
The moving vehicle S is guided to pass through the gate generated from VP5 and VP6.

However, the traveling command at the time of the avoiding operation of the moving vehicle S is a speed that can be stopped at the point VP5.

Next, a case where the moving vehicle S is returned from the obstacle SB to the planned traveling route K while avoiding the obstacle SB will be described with reference to FIG.

[0382] Fig. 26 is a diagram for explaining a case where the moving vehicle S is caused to avoid the obstacle SB and is returned to the planned traveling route K.

In FIG. 26, return gates TG6 and TG7 are provided on the return travel route K1 between the point VP4 and the target point P7.
Has been generated.

Here, first, the obstacle detection means 8 confirms that there is no obstacle at the place where the return travel route K1 is to be formed.

Then, the scheduled traveling route changing means 3 outputs information to the traveling condition selecting means 4 that there is no obstacle in the visual field range of the obstacle sensor.

[0386] Based on this information, the traveling condition selecting means 4 returns the returning gates TG6, T for returning the moving vehicle S from the current avoidance travel route K0 to the planned travel route K.
G7 is generated on the return travel route K1, and the speed of the moving vehicle S is returned to the original speed.

[0387] Thus, the moving vehicle S can move the return gate T
After passing through G6 and TG7, the vehicle returns to the planned traveling route K and runs away from the point P4 where the obstacle SB exists.

As described above, in the above-described embodiment, it is not necessary to stop the moving vehicle at a dangerous point where there is an obstacle, so that it is possible to prevent a rear-end collision between the moving vehicle and the following vehicle.

It should be noted that, as described above, it is necessary to decelerate the moving vehicle S when the obstacle SB is found on the planned traveling route K.

Here, during the period from the point P1, which is the current position of the moving vehicle S, to the point VP4 at which the avoidance is completed, it is necessary to perform an operation of rapidly turning the steering while further decelerating the moving vehicle S by applying a brake. .

Therefore, the points VP2 and VP3 are generated from the point P1, which is the current position of the moving vehicle S, to immediately before the avoidance.
The radius r0 of the turning circle of the moving vehicle S passing through these points VP2 and VP3 is a turning radius large enough to safely steer at the current speed.

In this embodiment, the current position of the moving vehicle S is the point P1, and the position of the obstacle SB is the target points P3 and P4.
However, ideally, the moving vehicle S always runs at a speed at which it can stop and steer, and if the speed is high, it can be decelerated before the point P1. desirable.

Next, in addition to the above-described embodiment, an embodiment in which the moving vehicle is avoided from an obstacle or the moving vehicle is decelerated and stopped immediately before the obstacle will be described.

FIG. 27 is a diagram showing an embodiment for generating data different from target data on a planned traveling route when a moving vehicle is put in a garage.

In FIG. 27, when the traveling vehicle S passing through the target gates G 1 and G 2 on the planned traveling route K reaches the target gate G 3, the driving condition selecting means 4
The gates TG1 and TG2 different from 1, G2 and G3 are generated.

Then, the moving vehicle S is moved from the target gate G3 to the gates TG1 and TG2.

When the vehicle reaches the gate TG2, the moving vehicle S is moved backward to generate the gate PG generated in the garage SK.
1. Go to PG2.

As described above, in this embodiment, the moving vehicle S traveling on the scheduled traveling route K can be moved backward and put into the garage SK.

FIG. 28 is a diagram showing an embodiment in which data different from target data on the planned traveling route is generated when a moving vehicle traveling on the planned traveling route is merged with a traveling route different from the planned traveling route.

In FIG. 28, when the traveling condition selecting means 4 reaches the target gate G1 on the planned traveling route K, it generates a gate TG1 different from the target gate G1.

Then, the moving vehicle S is directed to the gate TG1.

Then, when the vehicle reaches the gate TG1, the moving vehicle S is moved to the target gates G2 'and G2 on the planned traveling route Kb.
Go to 3 '.

As described above, in this embodiment, the moving vehicle S traveling on the planned traveling route K can join the traveling route Kb different from the planned traveling route K.

FIG. 29 is a diagram showing an embodiment in which, when an obstacle is present in front of the moving vehicle traveling on the planned traveling route, the moving vehicle is decelerated in front of the obstacle.

In FIG. 29, when the traveling condition selecting means 4 reaches the target gate G2 on the scheduled traveling route K, it generates gates TG1, TG2 different from the target gates G1, G2 just before the obstacle SB '.

Then, the moving vehicle S is moved to the gates TG1, TG2.
To decelerate sequentially.

As described above, in this embodiment, the moving vehicle S
, The moving vehicle S can be decelerated just before the obstacle.

[0408] Fig. 30 is a diagram showing an embodiment in which, when an obstacle is present on the planned traveling route of the moving vehicle, data different from target data on the planned traveling route is generated to avoid the obstacle.

In FIG. 30, when the traveling condition selecting means 4 reaches the target gate G1 on the planned traveling route K, the moving vehicle S avoids the obstacle SB 'through the gates TG1, TG2, TG3 different from the target gate G1. Generate it on the right side of the traveling direction so that you can.

Then, the moving vehicle S is moved to the gates TG1, TG
2. Go to TG3.

[0411] Then, when the vehicle reaches the gate TG3, the moving vehicle S is directed to the target gate G2 on the original planned traveling route K.

Thus, in this embodiment, the moving vehicle S
If the obstacle SB 'exists on the planned traveling route K, the obstacle SB' can be avoided.

FIG. 31 is a diagram showing an embodiment in which, when an obstacle is present in front of a moving vehicle traveling on a planned traveling route, the moving vehicle is stopped in front of the obstacle.

In FIG. 31, when the traveling condition selecting means 4 reaches the target gate G2 on the planned traveling route K, it generates gates TG1, TG2 different from the target gates G1, G2 just before the obstacle SB '.

[0415] Therefore, the moving vehicle S is rapidly decelerated at the gate TG1 and stopped at TG2 or TG3.

As described above, in this embodiment, the moving vehicle S
When the obstacle SB 'exists in front of the vehicle, the moving vehicle S can be stopped in front of the obstacle.

FIG. 32 is a diagram showing an embodiment in which data different from target data on the planned traveling route is generated when a moving vehicle traveling on the planned traveling route is stopped sideways at a stop point.

In FIG. 32, when the traveling condition selecting means 4 reaches the target gate G2 on the planned traveling route K, the traveling condition selecting means 4 moves the gates TG1, TG2 different from the target gates G1, G2 between the target gate G2 and the stop point Ps. At the stop point Ps and TG3
Generate

Then, the moving vehicle S is moved to the gates TG1, TG2.
And decelerate while stopping at the gate TG3.

As described above, in this embodiment, the moving vehicle S traveling on the planned traveling route K can be stopped sideways on the stop point Ps.

FIG. 33 shows an example in which a moving vehicle stopped sideways at a stop point is started and gradually accelerated to generate data different from target data on the planned traveling route when returning to the planned traveling route. It is a figure showing a form.

In FIG. 33, the driving condition selecting means 4 includes gates TG1, TG2, TG different from the target gates G1, G2 between the stop point Ps and the target gate G1 on the planned driving route K.
Generates 3.

Then, the moving vehicle S is moved to the gates TG1, TG
2. Gradually accelerate while moving toward TG3, and move to targets G1 and G2 on the planned traveling route K.

As described above, in this embodiment, the stop point Ps
The moving vehicle S, which is stopped sideways by the vehicle, can be started and gradually accelerated, and returned to the planned traveling route K.

FIG. 34 is a diagram showing an embodiment in which when a vehicle running next to a moving vehicle approaches the moving vehicle, the moving vehicle is avoided from the approaching vehicle.

In FIG. 34, when the traveling condition selecting means 4 reaches the target gate G1 on the planned traveling route K, the gates TG1, TG2, TG3 different from the target gates G1, G2.
Is generated on the right side in the traveling direction so that the moving vehicle S can avoid the vehicle Sx.

Then, the moving vehicle S is moved to the gates TG1, TG
2. Go to TG3.

Then, when reaching the gate TG3, the moving vehicle S is made to head to the target gate G2 on the original planned traveling route K.

As described above, in this embodiment, the moving vehicle S
When the vehicle Sx running next to the vehicle approaches the moving vehicle S, the moving vehicle S can be avoided from the moving vehicle Sx.

FIG. 35 is a diagram showing an embodiment in which, when the planned traveling route of the moving vehicle is a sharp curve, the distance between the target gates is reduced to reduce the speed of the moving vehicle at the sharp curve.

In FIG. 35, the interval WT between the target gates G3 and G4 and the interval WT between the target gates G3 and G4, which are generated in the sharp curve Kc, are different from the interval W between the target gates G1 and G2 and the target gate G6. It is narrower than the distance W from G7.

Here, at each target gate, when the moving vehicle S reaches each target gate, the steering command calculating means 5 performs the steering calculation. Therefore, in the sharp curve Kc, the calculation is performed by the narrower the interval between the target gates. Are performed continuously. As a result, the calculation time becomes longer in the sharp curve Kc, so that the speed v at which the moving vehicle S passes through the sharp curve Kc decreases. In this way, the passage time between the target gates can be adjusted to be constant.

Therefore, in this embodiment, the planned traveling route K
When the vehicle is in a sharp curve, the speed v of the moving vehicle S is reduced to prevent derailing, and the moving vehicle S can be driven safely.

FIG. 36 is a diagram showing an embodiment in which the speed is reduced in order to reduce the centrifugal force exerted on a moving vehicle traveling on a sharply curved scheduled traveling route.

In FIG. 36, the centrifugal force exerted on the moving vehicle S traveling on the scheduled traveling path K having a sharp curve is inversely proportional to the turning radius r 0 and proportional to the speed v of the moving vehicle S.

When the moving vehicle S runs on a sharp curve, the turning radius r0 of the moving vehicle S decreases.

[0437] For this reason, the centrifugal force exerted on the moving vehicle S traveling on the scheduled traveling path K having a sharp curve increases, and the moving vehicle S may cause a rollover accident or a slip accident.

Therefore, it is necessary to reduce the speed v of the moving vehicle S.

Then, the turning radius r0 of the moving vehicle S is obtained by the current motion measuring means 2 (the speed v is detected by the vehicle speed sensor and ω is detected by the angular speed sensor) (r0 = v / ω).
If the turning radius r0 of the (moving vehicle S) is small, the speed v is adjusted so as to decrease.

[0440] Thus, a rollover accident or a slip accident of the moving vehicle S traveling on the planned traveling route K of a sharp curve can be prevented.

In the above embodiment, the above (1)
Although tan θ is used for the slope of the equation of the straight line shown in the equation and the equation (4), when θ is an integral multiple of 90 degrees, the straight line is parallel to the X axis or the Y axis. In this case, in order to determine the positional relationship between the straight line and the coordinates of the point, only the X coordinate or the Y coordinate may be appropriately compared.

Further, in the above-described embodiment, the point sequence data stored in the planned traveling route storage means 1 is used as the target point sequence. If a marker such as is buried or a transmitter indicating the passing position of the moving vehicle S is installed along the road, the current position of the moving vehicle S can be accurately detected by the current motion measuring means 2.

[Brief description of the drawings]

FIG. 1 is a diagram showing an embodiment of a guidance device for a mobile vehicle according to the present invention.

FIG. 2 is a block diagram showing a configuration of a guidance device for a mobile vehicle according to the present invention.

FIG. 3 is a flowchart illustrating a process performed by the guide device of the moving vehicle according to the present invention.

FIG. 4 is a diagram illustrating a process of confirming that a moving vehicle has passed between a pair of end points;

FIG. 5 is a diagram illustrating a process of moving a moving vehicle that is traveling straight ahead between a pair of end points;

FIG. 6 is a diagram illustrating steering of the moving vehicle by replacing the moving vehicle with an equivalent two-wheeled vehicle.

FIG. 7 is a diagram illustrating the steering of a skid steer type moving vehicle.

FIG. 8 is a diagram illustrating a process of moving a turning traveling vehicle toward between a pair of end points;

9A is a diagram showing a case where the moving vehicle is moved forward toward the target gate, and FIG. 9B is a diagram showing a case where the moving vehicle is moved backward toward the target gate. .

FIG. 10 is a diagram showing a modification of the embodiment of the guidance device for a mobile vehicle according to the present invention.

FIG. 11 is a view showing a modification of the embodiment of the guide device for a mobile vehicle according to the present invention.

FIG. 12 is a diagram showing an embodiment of a guidance device for a mobile vehicle according to the present invention.

FIG. 13 is a diagram illustrating a process of causing a moving vehicle that is traveling straight ahead to proceed to an arbitrary geometric figure.

FIG. 14 is a diagram for explaining a process of moving a turning traveling vehicle toward an arbitrary geometric figure;

FIG. 15A is a diagram showing a case where a moving vehicle is moved forward to an arbitrary geometrical figure, and FIG.
(B) is a figure which shows the case where a moving vehicle retreats toward arbitrary geometric figures.

FIG. 16 is a diagram showing an embodiment of a guidance device for a mobile vehicle according to the present invention.

FIG. 17 is a diagram illustrating a process of moving a moving vehicle that is traveling straight toward a circle;

FIG. 18 is a diagram illustrating a process of moving a turning traveling vehicle toward a circle;

FIG. 19 (a) is a diagram showing a case where the moving vehicle is moved forward toward a circle, and FIG. 19 (b) is a diagram showing a case where the moving vehicle is moved backward toward a circle.

FIG. 20A is a diagram illustrating a case where the density of point sequence data is sufficiently dense compared to the curvature of the planned route, and FIG. 20B is a diagram illustrating the case where the density of point sequence data is It is a figure showing the case where it becomes sparse compared with the curvature of K.

FIG. 21 is a diagram showing point sequence data in which three target points are aligned on a straight line.

FIG. 22 is a diagram showing point sequence data in which three target points are arranged on the circumference of a turning circle of a moving vehicle.

FIG. 23 is a diagram illustrating a circle passing through three target points and an inclination of a target gate.

FIG. 24 is a diagram illustrating setting of a center point of a circle passing through three target points.

FIG. 25 is a diagram illustrating an embodiment in which data different from target data on a planned traveling route is generated when a moving vehicle is avoided from an obstacle.

FIG. 26 is a diagram illustrating a case where a moving vehicle is avoided from an obstacle and is returned to a planned traveling route.

FIG. 27 is a diagram showing a modification of the embodiment of the guide device for a mobile vehicle according to the present invention.

FIG. 28 is a view showing a modification of the embodiment of the guide device for a mobile vehicle according to the present invention.

FIG. 29 is a diagram showing a modification of the embodiment of the guidance device for a mobile vehicle according to the present invention.

FIG. 30 is a view showing a modification of the embodiment of the guidance device for a mobile vehicle according to the present invention.

FIG. 31 is a view showing a modification of the embodiment of the guide device for a mobile vehicle according to the present invention.

FIG. 32 is a view showing a modification of the embodiment of the guide device for a mobile vehicle according to the present invention.

FIG. 33 is a view showing a modification of the embodiment of the guide device for a mobile vehicle according to the present invention.

FIG. 34 is a view showing a modification of the embodiment of the guide device for a mobile vehicle according to the present invention.

FIG. 35 is a view showing a modification of the embodiment of the guidance device for a mobile vehicle according to the present invention.

FIG. 36 is a view showing a modification of the embodiment of the guidance device for a mobile vehicle according to the present invention.

FIG. 37 is a diagram showing a conventional technique.

FIG. 38 (a) is a diagram showing a case where the equivalent two-wheeled vehicle is advanced and guided to a target point, and FIG. 38 (b) is a case where the equivalent two-wheeled vehicle is retracted and guided to a target point. FIG.

[Explanation of symbols]

 S: moving vehicle K: planned traveling route PRn, PLn: a pair of end points

──────────────────────────────────────────────────の Continued on the front page (51) Int.Cl. ⁷ Identification FI FI Theme Court ゛ (Reference) B62D 117: 00 137: 00 (72) Inventor Tomoo Matsuda 2612 Shinomiya, Hiratsuka-shi, Kanagawa Prefecture Komatsu Manufacturing Co., Ltd. F-term (reference) 3D032 CC20 DA02 DA08 DA22 DA23 DA78 DA87 DA88 DA90 EA01 EB01 EB04 EB15 5H301 AA10 BB02 BB05 CC03 CC06 DD02 FF08 FF11 GG07 GG11 GG14 GG16 HH02 HH03 HH10 HH15 JJ01 LL01 LL01LL06LL

Claims (6)

[Claims] 1. A guidance apparatus for a moving body for guiding a moving body toward a next target when a current target among a plurality of targets on a planned traveling route of the moving body is reached, A pair of end points separated by a predetermined distance with respect to the planned traveling route is set as the target, and when the moving body reaches between the current pair of end points, the moving body is positioned between the next pair of end points. A guidance device for a mobile object, characterized by causing 2. Guidance of a moving body that guides the moving body toward a next passing area when the vehicle reaches a current passing area among a plurality of passing areas on a planned traveling route of the moving body. In the apparatus, when the moving object travels a predetermined distance after first detecting the current passing area, and reaches a predetermined point in the passing area, the moving object moves to the next passing area. A guidance device for a mobile object, characterized by causing 3. When a current target among a plurality of targets on a planned traveling route of a moving body having a steered wheel and a fixed wheel is reached, the moving body is guided to the next target. In the guidance device for a moving body to be moved, the distance between the center axis of the steered wheel and the center axis of the fixed wheel and the radius of a turning circle when the moving body turns to the next target are determined by the steering wheel. A steering angle of the moving body, and the moving body is steered based on the obtained steering angle, and the moving body is directed toward the target. 4. When a current target among a plurality of targets on a planned traveling route of a moving body having a steered wheel and a fixed wheel is reached, the moving body is guided to the next target. In the guidance device for a moving body to be set, a pair of end points separated by a predetermined distance with respect to the planned traveling route is set as the target, a distance between a center axis of the steered wheels and a center axis of the fixed wheel, The steering angle of the steered wheel is determined from the radius of the turning circle when the moving body turns to the next target, and at the time when the moving body passes between the current pair of end points, the determined steering is performed. A moving body guidance device, wherein the moving body is steered based on an angle, and the moving body is directed between the next pair of end points. 5. A moving body having a steered wheel and a fixed wheel, when reaching a current passing area of a plurality of passing areas on a planned traveling route of the moving body, moving the moving body toward a next passing area. In the guidance device for a moving body to be guided as described above, the distance between the center axis of the steering wheel and the center axis of the fixed wheel, and the radius of a turning circle when the moving body turns to the next target The steering angle of the steered wheels is determined, and when the moving body travels a predetermined distance after first detecting the current passage area, and reaches a predetermined point in the passage area, the calculated angle is calculated. A moving body steering device that steers the moving body based on the steering angle and directs the moving body toward the next passage area. 6. A plurality of targets on the planned traveling route are a sequence of points, and a pair of end points are generated for a sequence of points corresponding to a curve in the planned traveling route. The mobile object guidance device according to claim 1, wherein the mobile device is directed between the pair of generated end points when the current point is reached.