JPH0816982A - Vehicle guiding device - Google Patents

Abstract

PURPOSE:To provide a vehicle guiding device which can guide a vehicle to a guided target position by only setting one reference point. CONSTITUTION:This device is provided with a distance measuring device 21 to output a distance signal correlated with distance between the reference point to be the standard of guidance and the vehicle, a distance calculating part 29A to calculate the distance between the reference point and the vehicle on the basis of the distance signal, an azimuth sensor 23 to calculate the advancing direction of the vehicle in regard to a geomagnetic axis, a path calculating part 29B to calculate a path from an arbitrary starting position to the guided target position on the basis of the calculated distance and the calculated advancing direction of the vehicle, and a guiding and controlling device 24 to guide the vehicle to the guided target position in conformity with the path calculated by the path calculating part 29B.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a vehicle guidance device for guiding a vehicle to a guidance target position such as a parking position.

[0002]

2. Description of the Related Art As a vehicle guidance device of this type, one disclosed in, for example, Japanese Patent Laid-Open No. 61-263851 is known. In the conventional vehicle guidance device, a guidance route for guiding the vehicle is stored in advance, and a fixed route guidance system that guides the vehicle according to the stored guidance route and a coordinate of the vehicle at the time of starting parking (guidance start) And a direction, and a coordinate system of the vehicle at the time of completion of parking (guidance completion) and a necessary steering angle are calculated, and the vehicle is guided based on the calculated steering angle. Also, a method of using both of these methods in combination is disclosed in Japanese Utility Model Application Laid-Open No. 60-16206 and JP-A-4-27
It is disclosed in Japanese Patent No. 7900. Among these fixed route guidance methods and point-to-point guidance methods, particularly in the point-to-point guidance method, it is necessary to accurately detect the relative position coordinates and direction of the vehicle with respect to the guidance target position.

In the vehicle guidance system disclosed in Japanese Patent Laid-Open No. 63-291200, two markers 1 and 2 are provided at the entrance of the parking position (guidance target position) Pe as shown in FIG.
22, the markers 1 and 2 are detected by the detectors 4 and 5 provided at two positions on the rear of the vehicle 3 as shown in FIG. 22, and the relative positions between the markers 1 and 2 and the vehicle 3, The relative position coordinates and direction of the vehicle 3 with respect to the parking position are detected.

However, if the detection ranges of the detectors 4 and 5 are limited, the two markers 1 and 2 cannot be accurately detected depending on the parking start position, and conversely the two markers 1 and 2 cannot be detected. The position at which 1 and 2 can be accurately detected must be the parking start position, and the parking start position is limited. As an example, in the example shown in FIG.
Since both of the two markers 1 and 2 are located within the detection ranges A4 and A5 of the detectors 4 and 5, these detectors 4 and 5 are
Although any of 5 can accurately detect the markers 1 and 2 at two locations, in the example shown in FIG. 23 (b), the marker 2 is out of the detection range A4 of the detector 4, and the detector 4
Cannot detect marker 2 accurately. Further, in the example shown in FIG. 23 (c), the marker 2 is the detector 4,
Since it is out of both the detection range A4 and A5 of 5,
Neither of the detectors 4 and 5 can detect the marker 2 accurately.

Therefore, in the device disclosed in Japanese Unexamined Patent Publication No. 4-277900, two cylindrical bar codes 6 and 7 are provided at the entrance of the parking position and a flat plate is provided at the back of the parking position as shown in FIG. The barcodes 8 and 9 are provided at two places, and the barcodes 6 to 9 at these four places are imaged by the CCDs 10 and 11 provided at two places on the rear portion of the vehicle 3 as shown in FIG. The relative position with respect to the vehicle 3, and eventually the relative position coordinates and direction of the vehicle 3 with respect to the parking position, are detected. Barcodes 6 to 9 were used instead of the markers because when there are more markers set because there is no restriction on the parking start position, it is necessary to identify the markers according to their positions when the number of markers increases. Is caused. In the example disclosed in Japanese Patent Laid-Open No. 4-277900, the widths of the cylindrical barcodes 6 and 7 are not constant in the images captured by the CCDs 10 and 11,
On the other hand, since the interval between the widths of the flat bar codes 8 and 9 is substantially constant, the bar codes 6 to 9 are identified by detecting the widths of the bar codes 6 to 9.

[0006]

In the above-mentioned conventional vehicle guidance system, a plurality of markers 1 and 2 or bar codes 6 to 9 which serve as reference points for vehicle guidance are provided between the reference point and the vehicle 3. Since the relative position coordinates of the reference points are calculated, if the number of reference points is small, the relative position coordinates may not be calculated accurately as described above, and if the number of reference points is increased, these reference points There was a need to identify the location. However, there is no guarantee that the reference points, especially the barcodes 6 to 9 can always be clearly and clearly identified in an environment exposed to the wind and rain outdoors such as a parking lot. Further, a detector for individually identifying the marker or the like is required on the vehicle 3 side, which is a factor that complicates the configuration of the vehicle guiding device. It is an object of the present invention to provide a vehicle guidance device that can guide a vehicle to a guidance target position by providing only one reference point.

[0007]

The present invention is applied to a vehicle guidance system for guiding a vehicle to a predetermined guidance target position, and will be described with reference to FIG. 1 showing an embodiment. Is a distance detecting means 21, 29A for detecting the distance between the vehicle and a reference point which is a reference for guidance, and a traveling direction detecting means 2 for detecting the traveling direction of the vehicle with respect to a reference axis on the ground surface.
3, and trajectory calculating means 29B for calculating a trajectory from an arbitrary starting position to a guidance target position based on the distance detected by the distance detecting means 21 and 29A and the vehicle traveling direction detected by the traveling direction detecting means 23. And the guidance control means 24 for guiding the vehicle to the guidance target position according to the trajectory calculated by the trajectory calculation means 29B. The distance detecting means of the vehicle guiding apparatus according to claim 2 outputs a distance signal correlating with a distance between a vehicle and a reference point serving as a guidance, and a distance signal outputting means along with the movement of the vehicle. A horizontal distance calculating unit 29A that calculates the height between the vehicle and the reference point based on the distance signal obtained from 21 and calculates the horizontal distance between the reference point and the vehicle based on the calculated height and the distance signal. With. The vehicle guiding apparatus according to claim 3 further includes a detection unit 22 that detects an obstacle around the vehicle, and the trajectory calculation unit 29B prevents the vehicle from colliding with the obstacle when the detection unit 22 detects the obstacle. To calculate the trajectory again, and guide control means 2
4 is for guiding the vehicle in accordance with the trajectory calculated again by the trajectory calculating means 29B. The trajectory calculating means 29B of the vehicle guiding device according to claim 4 calculates circles each passing through the starting position and the guidance target position and having two radii of the minimum turning radius of the vehicle, and the starting position and the guidance. It has a tangent line calculating means for calculating a tangent line connecting two circles passing through the target position and a selecting means for selecting a trajectory from a combination of the calculated circle and tangent line. According to a fifth aspect of the present invention, the selecting means of the vehicle guiding apparatus selects a track having a minimum distance in a combination of a circle and a tangent line. The track calculation means 29B of the vehicle guidance device according to claim 6 has weighting means for weighting the distance traveled by the vehicle in the backward direction, and the selection means, after the weighting, combines a circle and a tangent line. It selects the orbit that has the smallest distance in. When the reference point is composed of a radio wave reflector, the distance between the reference point and the vehicle is determined by the radio wave transmitter mounted on the vehicle, which emits radio waves toward the radio wave reflector, and the radio wave reflected from the radio wave reflector. By using a radio wave receiver for receiving the radio wave, the distance between the reference point and the vehicle can be calculated based on the time until the radio wave emitted from the radio wave transmitter is received by the receiver. It is also possible to mount a geomagnetic sensor on the vehicle and detect the traveling direction of the vehicle with respect to the geomagnetic axis measured by this sensor.

[0008]

The distance detecting means 21 and 29A detect the distance between the vehicle and a reference point which is a reference for guidance. The traveling direction detecting means 23 detects the traveling direction of the vehicle with respect to the reference axis on the ground surface. Based on the distance between the vehicle and the reference point that is the reference of the detected guidance, and the detected traveling direction described above,
The trajectory calculating means 29B calculates a trajectory from an arbitrary starting position to the guidance target position. The guidance control unit 24 guides the vehicle to the guidance target position according to the trajectory calculated by the trajectory calculation unit 29B. When the vehicle further includes a detection unit 22 that detects an obstacle around the vehicle, the trajectory calculation unit 2
When an obstacle is detected by the detection means 22, 9B recalculates the trajectory so that the vehicle does not collide with this obstacle,
The guidance control means 24 guides the vehicle in accordance with the trajectory calculated again by the trajectory calculation means 29B. When the trajectory calculating means 29B is composed of a circle calculating means, a tangent calculating means, and a selecting means, the circle calculating means passes through the starting position and the guidance target position, respectively, and sets the minimum turning radius of the vehicle as the radius. Each two circles are calculated, and the tangent calculation means is
A tangent line connecting two circles passing through the calculated start position and guide target position is calculated, and the selecting means selects a trajectory from the combination of the calculated circle and tangent line.

Incidentally, in the section of means and action for solving the above-mentioned problems for explaining the constitution of the present invention, the drawings of the embodiments are used to make the present invention easy to understand. It is not limited to.

[0010]

【Example】

First Embodiment FIG. 1 is a functional block diagram in which the configuration of the first embodiment of the vehicle guidance device according to the present invention is divided into functions, and FIG. 2 shows a first embodiment.
A perspective view showing a vehicle equipped with a vehicle guidance device of an embodiment,
FIG. 3 is a plan view of the vehicle. In these figures, 20
Is a vehicle, and a pair of distance measuring devices 21 is provided on the upper surface of the roof of the vehicle 20. The distance measuring device 21 is, for example, a radar device that radiates a radio wave in the range of 360 ° to detect the distance from the reflected radio wave to the object, or similarly a radar device.
When the radio wave reflector is present around the vehicle 20 and is provided with a set of radio wave transmitters that emit radio waves in the range of 0 ° and radio waves that receive reflected radio waves, the distance from the vehicle 20 to the radio wave reflector To detect. These pair of distance measuring devices 21
Is synchronized with each send / receive operation,
Radio waves do not interfere with each other. In this embodiment, FIG.
As shown in, the metal ball 30a provided on the top of the streetlight 30 near the parking position is used as a radio wave reflector, and the metal ball 30a is used as a marker for detecting the relative coordinates and the direction of the parking position with respect to the vehicle 20. In the following description, the radio wave reflector, the metal ball 30a and the marker are used without distinction.

Reference numeral 22 denotes an obstacle detection sensor, which is used for the vehicle 20.
One is provided at each of the front part and the rear part of the.
The obstacle detection sensor 22 is composed of, for example, an ultrasonic sensor, an optical sensor, or the like, and detects the presence of an ultrasonic reflector, a light reflector, or the like in the front-rear direction of the vehicle 20 to determine its size and the distance to the vehicle 20. To detect.

Reference numeral 23 denotes an azimuth sensor which detects the geomagnetic axis to detect the azimuth of the vehicle 20 with respect to the geomagnetic axis, that is, FIG.
As shown in FIG.
Detects the angle formed by. The azimuth sensor 23 is, for example, a combination of a geomagnetic sensor and an angular velocity sensor. The geomagnetic sensor detects the geomagnetic axis, and the angular velocity sensor detects the amount of angular deviation in the advancing direction with respect to the geomagnetic axis. Detects the angle between and the earth's magnetic axis.

Reference numeral 24 is a guiding device, which comprises an input device 25 shown in FIG. 4 and a display device 26 shown in FIG. The input device 25 has a target position button 25a that is operated to set the guide target position such as a parking position when the guide target position is reached, and a start command of a guide operation at an arbitrary guide start position. Guide button 25 operated by
b and a display 25c for displaying the two-dimensional position of the vehicle 20. In the illustrated example, the display 25
The initial position (guidance start position) MS of the vehicle 20, the target position (parking position) ME of the vehicle 20, the position Ob of the obstacle, and the position RF of the marker are displayed at c.

The display device 26 displays an instructed steering angle display portion 26a showing a steering instruction angle along a guide track, an actual steering angle display portion 26b showing an actual steering angle of the vehicle, and a current vehicle speed, which will be described later. A vehicle speed display unit 26c and a traveling amount display unit 26d that displays which part of the guideway the vehicle has traveled are provided.

In the present embodiment, since the guideway is composed of the circle having the minimum turning radius and its tangent line, as will be described later, there are only three kinds of steering instruction angles, the maximum to the right, the straight and the maximum to the left. . Therefore, in the conventional vehicle guidance device described above, the steering angle is automatically controlled during the guidance operation, but in the present embodiment, the vehicle can be guided to the guidance target position without performing such automatic control. The steering instruction angle along the guidance track is displayed to the occupant, and the occupant controls the steering angle according to the steering instruction angle to guide the vehicle to the guidance target position. Therefore, the instructed steering angle display unit 26a displays 3 types of these three types of steering angles.
It is composed of two point light source-like members. On the other hand, the actual steering angle display unit 26b is configured so that the dot-shaped member moves substantially continuously in an arc shape in order to display the continuously changed actual steering angle. In addition, the traveling amount display unit 26d represents the guide track from the guide start position to the parking position on a straight line, and indicates where the current vehicle 20 is at by a rod-shaped member that can move on the straight line. .

Returning to FIG. 1, reference numeral 27 is a storage unit, which is a guidance target position (parking position) input by the input device 25.
Relative coordinate values and directions for the markers are stored in advance. A vehicle speed sensor 28 detects the speed of the vehicle 20 based on the number of rotations of the tire and the like. 29 is the distance calculation unit 2
9A and a trajectory calculation unit 29B, which is a calculation unit, and includes a distance measuring device 21, an obstacle detection sensor 22, an orientation sensor 23,
Signals from the guidance device 24 and the vehicle speed sensor 28 are input, and based on these input signals, the distance between the reference point and the vehicle and the traveling direction of the vehicle with respect to the geomagnetic axis are calculated and the guidance trajectory is obtained, as described later. Is calculated and the obstacle avoidance is performed, and the result is output to the guidance device 24, and the calculation result when the guidance target position is set is stored in the storage unit 27.

Next, referring to the flowchart of FIG.
The operation of the vehicle guiding system of this embodiment will be described. (1) Own vehicle direction detection In step S100, the guidance button 25 of the input device 25
The operation waits until b is operated, and when the guide button 25b is operated, the process proceeds to step S101. In step S101,
Vehicle 20 and marker 3 detected by distance measuring device 21
The relative position coordinates and direction of the vehicle 20 with respect to the marker 30a are detected based on the distance between the vehicle 20 and the geomagnetic axis detected by the azimuth sensor 23 and the distance between the vehicle 0 and the vehicle 0.

The distance measuring device 21 measures the distance between the vehicle 20 and the marker 30a, more precisely, the distance between each distance measuring device 21 and the marker 30a as follows.
The distance measuring device 21 emits a pulsed radio wave, and measures the elapsed time from the emission of the radio wave pulse to the reflection and reception of the radio wave pulse.
Measure the linear distance to a. However, the distances measured here are three-dimensional distances L1 ′ and L2 ′ between the distance measuring device 21 and the marker 30a as shown in FIG. 7, and the relative position coordinates and direction of the vehicle 20 to be described later are calculated. In the operation, the three-dimensional distances L1 ′ and L2 ′ are shown in FIG. 7 in order to obtain the coordinates and the direction on the two-dimensional plane set at the height of the distance measuring device 21 in order to simplify the handling. (Horizontal distance) It is necessary to convert to L1 and L2. If the height h from the distance measuring device 21 to the marker 30a is known, the two-dimensional distances L1 and L2 are given by the following equation.

[Equation 1]

Since these L1 and L2 are the basis for detecting the relative position and direction of the vehicle 20, which will be described later,
In order to reliably detect the distances L1 and L2 regardless of the position of the vehicle 20, as shown in FIG.
It is desirable to install it at a position higher than the distance measuring device 21.

Next, the two-dimensional distance L between the distance measuring device 21 and the marker 30a detected by the distance measuring device 21.
Based on the angle between the vehicle 20, and the geomagnetic axis detected by 1, L2 and the direction sensor 23, the marker 30a
A procedure for obtaining the relative position coordinates and direction of the vehicle 20 with respect to will be described. According to the procedure described above, the distance measuring instrument 21 exists on two concentric circles with radii L1 and L2 centered on the marker 30a as shown in FIG. 8A, and the vehicle 20 also exists on this concentric circle. However, the orientation of the vehicle 20 with respect to the marker 30a, that is, the direction in which the vehicle 20 faces is not determined by this alone. In other words, the distance measuring instrument 21 outputs the same measurement result whether the vehicle 20 is at the point A or the point B in FIG. 8A. Therefore, the direction of the vehicle 20 with respect to the marker 30a is obtained using the angle θ formed by the traveling direction 31 of the vehicle 20 and the geomagnetic axis 32 measured by the azimuth sensor 23, and the relative position coordinates of the vehicle 20 with respect to the marker 30a and The direction of travel is determined as follows.

Assuming a line 33 connecting the marker 30a and the distance measuring device 21 on the side of the marker 30a, and letting the angle between this line 33 and the geomagnetic line 32 be φ, then between the angle θ and the angle φ. Has the following relationship.

## EQU2 ## ψ = φ-θ. ． ． (2) Here, ψ is an angle formed by a line parallel to the line 33 and the traveling direction 31 of the vehicle 20. As shown in FIG. 8B, the marker 30a and the pair of distance measuring devices 21 are respectively apexes. When the triangle is considered, the following formula is given from the cosine theorem that holds in this triangle.

(Equation 3)

It should be noted that there may be a magnetic substance structure around the vehicle 20 and a systematic error may be included in the measurement value of the azimuth sensor 23, but in this embodiment, the reference point (marker 30a) is used. Since only the relative positional relationship and direction with respect to is used and the guidance target position is also given by relative position coordinates and direction with respect to this reference point as will be described later, systematic errors are canceled. Alternatively, the vehicle 20 is provided with a steering angle sensor and a speed sensor, and the rate of change in the direction of the vehicle 20 given by the steering angle sensor and the speed sensor is compared with the rate of change in the direction given by the direction sensor 23. It is also possible to detect an abnormality of the orientation sensor 23 by the difference in

On the other hand, when the height h from the distance measuring device 21 to the marker 30a is not known when the distances L1 and L2 are calculated, the height h can be calculated as follows, for example. As shown in FIGS. 9A and 9B, the vehicle 20 is moved from the point A in the figure to the point B via the point C, and the differential value of the value of the equation (3) at that time is The vehicle 20 is once stopped at the point where it becomes zero. The fact that the differential value of the value of Expression (3) becomes 0 is, as can be seen from FIG. 9C, the value obtained by differentiating the left side of Expression (3) is −cos ψ = 0, so ψ = π / 2 and the angle formed by the traveling direction 31 of the vehicle 20 and the line 33 is a right angle. In the illustrated example, the point where the differential value of the value of Expression (3) becomes 0 corresponds to the point C shown in FIG. 9B.

Then, paying attention to the two right triangles formed by the distance measuring device 21 and the marker 30a at the above-mentioned point C and the point on the street lamp 30 which is the starting point of the measurement of the height h, The formula is derived.

[Equation 4] L1 ', L2': Three-dimensional distance obtained by the distance measuring device 21 at the point C d: Interval of the distance measuring device 21 As described above, even the marker 30a whose height h is unknown is used for vehicle guidance. Therefore, any object can be used as the marker 30a as long as it is a radio wave reflector.

(2) Calculation of trajectory to target point Returning to FIG. 6, in step S102, the marker 30a
Based on the relative position coordinates and direction of the current position (guidance start position) of the vehicle 20 with respect to, and the relative coordinate value and direction of the guidance target position (parking position) stored in the storage unit 27 with respect to the marker 30a, the guidance start The trajectory from the position to the guidance target position is calculated. In this example, FG
Method proposed by Pin et al. (HAVasseur, FGPi
n et al., The 1991 IEEE International Conference o
n Refer to Robitics and Automation, CONF-910451-9, referred to as “reference paper” below) to calculate trajectories.

According to the above-mentioned reference paper, the shortest trajectory for moving from the point A to the point B in FIG.
It is a track that rotates at a minimum turning radius at a point A, then proceeds straight toward a point B, and then rotates at a minimum turning radius at this B point to obtain a desired vehicle traveling direction. Therefore,
The orbits at this time are two circles of minimum radius gyrations CA1 and CA2 passing through the point A, and two circles of minimum radius gyration C passing through the point B.
B1 and CB2, and these minimum circles of gyration CA1, CA2, C
It is represented by a combination with tangents T1 to T4 connecting B1 and CB2. In the illustrated example, only the four tangent lines T1 to T4 connecting the circles CA1 and CB1 having the smallest radius of gyration are shown, but the circle CA
There are actually 16 combinations of tangents connecting 1, CA2 and the circles CB1, CB2.

In the following description, the turning at the point A (that is, the guidance start position) is the initial turning, and the point B is the turning.
The turning at (that is, the guidance target position) is referred to as a final turning, and the linear trajectory connecting the initial turning and the final turning is referred to as an intermediate linear trajectory.

In the present embodiment, the trajectory is calculated according to the above-mentioned reference paper, and the maximum steering angle turning and stopping operations are performed in the initial turning and the final turning, and only the straight running and stopping operations are performed in the intermediate straight path. There are only three steering angles during the guidance operation. For this reason, in the present embodiment, the occupant is instructed of three kinds of steering instruction angles, and the occupant controls the steering angle according to the steering instruction angles to guide the vehicle 20 on the guide track. Of course, the steering angle and the traveling speed may be automatically controlled by a steering actuator, a throttle actuator, a brake actuator, or the like.

As shown in FIG. 11, the trajectory consisting of two sets of circles CA1, CA2, CB1 and CB2 and the tangents T1 to T4 connecting them is the direction of rotation of the vehicle 20 along the circles CA1, CA2, CB1 and CB2. And tangents T1 to T4 and circles CA1 and CA
2, coordinates of contact points with CB1 and CB2 (x1, y1), (x2,
It is represented by y2). Contact coordinates (x1, y1), (x2, y
2) is represented by the mathematical formula in FIG.

In the above referenced paper, two sets of circles CA1 and C
Of the 16 tangents connecting A2, CB1, and CB2, select 8 tangents that do not cause the traveling direction of the vehicle 20 to reverse 180 degrees with respect to the desired traveling direction at the guidance target position,
An appropriate trajectory is selected from the trajectories including this tangent line, and there is no particular problem regarding the traveling direction of the vehicle during the guidance. However, in a manned vehicle such as a passenger car to which the present embodiment is applied, it is basically based on the fact that the vehicle travels in the forward direction. Can be easily grasped, and it is natural to guide the vehicle in the forward direction even from the riding posture. Therefore, in the case of guiding a manned vehicle as in the present embodiment, unlike the trajectory planning problem in an unmanned guided vehicle as in the above-mentioned reference paper, a trajectory traveling in the forward direction should be selected as much as possible. is there.
However, sticking to the forward direction as the traveling direction of the vehicle 20 more than necessary is not necessarily appropriate when the vehicle 20 is properly guided in a narrow area, and is selected if an appropriate trajectory is included in the trajectory including the reverse direction. Should.

Therefore, in the present embodiment, as shown in FIG. 12, both the initial turning and the final turning have a rotation angle> π, that is, a counterclockwise direction (this is because the traveling direction of the vehicle 20 is the reverse direction in the initial turning and the final turning). The total of 64 sets of trajectories are considered first. That is, as described above, the total number of tangent lines connecting the two sets of circles is 16, and as shown in FIG.
For the trajectory from Point A to Point C, the arrow α
There are two kinds of trajectories for the direction B and the direction β, and two trajectories for traveling from the point D to the point B, that is, two directions of the γ direction and the δ direction in total, so that 16 × 4 = 64 sets of trajectories are generated. Next, among these 64 sets of tracks, 32 sets of tracks in which the traveling direction of the vehicle 20 does not reverse to the desired traveling direction at the guidance target position are generated, and the travel distances of these 32 sets of tracks are calculated. Select the track with the shortest lever travel distance and use it as the guide track. At this time, for a portion where the traveling direction of the vehicle 20 is the reverse direction, for example, the distance is 1.5
Is used to facilitate the selection of a trajectory having a long moving distance in the forward direction, and guidance is performed on a trajectory that does not make the occupant feel uncomfortable.

Returning to FIG. 6, in step S103, the vehicle 20 is guided from the guidance start position to the guidance target position according to the guidance trajectory selected in step S102.
The guidance method is as described above, and the instruction steering angle display unit 26a of the display device 26 displays the steering instruction angle and the actual steering angle of the vehicle 20 is displayed by the actual steering angle display unit 26b. Instruct the occupant to follow the steering angle. Further, the display 25c of the input device 25 displays the initial position MS of the vehicle 20, the parking position ME, the position RF of the marker, and the relative positional relationship between them. Further, the travel distance of the vehicle 20 from the start of the guidance is calculated based on the vehicle speed signal obtained from the vehicle speed sensor 28, and the traveling amount display unit 26d of the display device 26 displays the position of the vehicle 20 on the guidance track.

(3) Obstacle Avoidance Operation In step S104, it is judged from the detection signal from the obstacle detection sensor 22 whether or not there is an obstacle around the vehicle 20, and if it is judged that there is an obstacle. Step S1
The process proceeds to 05 to perform the obstacle avoidance operation, and when it is determined that there is no obstacle, the process directly proceeds to step S106.

In the obstacle avoiding operation, first, as shown in FIG. 13, an obstacle 40 around the vehicle 20 is connected to form a free space convex polygon PF around the vehicle 20, and a free space convex polygon is formed. It is assumed that the area inside the polygon PF is an area where the vehicle 20 can move freely. The free space convex polygon PF is formed by connecting the sides of each obstacle 40.
Each of the interior angles of F is made within 180 °. Therefore, if the obstacles 40 are simply connected, the free space convex polygon PF has the side as shown by the dotted line in FIG. 13, but the convex polygon PF having the side as the solid line is set due to the above limitation. .

Next, as shown in FIG. 13, a vehicle convex polygon PV that surrounds the vehicle 20 in the immediate vicinity is created, and the vehicle convex polygon PV is generated on the 64 tracks generated in step S102 of FIG. 4 described above. Free space convex polygon PF when PV moves
It is determined whether to collide with. The determination is performed by the procedure shown below.

As shown in FIG. 14, the vertex of the convex polygon is represented by P
1 to P5, each vertex P from the point A in the convex polygon
Assuming vectors going from 1 to P5 and adding all angles (ω in the illustrated example) formed by adjacent vectors (for example, vectors AP1 and AP2), the result is 2π. However, when the same calculation is performed on the point B outside the convex polygon, the result becomes 0. That is, the angle ω at the point A inside the convex polygon has one of positive and negative signs, but the angle ω at the point B outside the convex polygon has both positive and negative signs. Therefore, the sign of the angle ω is obtained for each vertex of the convex polygon,
If the sign is reversed in the middle, it may be determined that the point to be detected is outside the convex polygon, and if the sign is constant, it may be determined that the point to be detected is inside the convex polygon.

FIG. 15 is a flow chart for explaining a concrete procedure of collision judgment. In FIG. 15, the point to be detected is described as the point B in FIG. 14, but the point B is not necessarily located outside the convex polygon as shown in FIG.

In step S200, a value serving as a reference for code determination is calculated. In the illustrated example, the outer product of the vector BP1 extending from the point B to the vertex P1 and the vector BP2 extending from the point B to the vertex P2 is used as the reference value SGN for code determination. A variable I for counting the number of vertices is initialized in step S201, and the variable I is incremented in step S202. In step S203, it is determined whether or not the value of the variable I is larger than the number of vertices of the convex polygon, and if the determination is affirmative, the process proceeds to step S205, and it is determined that the point to be detected is within the convex polygon. To do. On the other hand, if the determination is negative, the process proceeds to step S204. In step S204, the above-mentioned reference value SG is added to the outer product of the vectors respectively directed to the vertex of the variable I and the vertex of the variable I + 1 adjacent to the vertex.
It is determined whether the value multiplied by N is smaller than 0, and if the determination is affirmative, the process proceeds to step S206, and it is determined that the point to be detected is not within the convex polygon. On the other hand, if the determination is negative, the process returns to step S202 and the above-described work is repeated.

The step S204 is affirmed when the signs of the cross product of the vector BPI and the vector BPI + 1 and the reference value SGN are different. Thereby, step S20
If 4 is affirmed, the point to be detected is outside the convex polygon by the above-mentioned principle, and if step S204 is denied for all the vertices of the convex polygon, the point to be detected is similarly convex. It can be judged that it is inside the polygon.

It should be noted that the judgment in step S105 is a collision between the vehicle convex polygon PV and the free space convex polygon PF, but the collision between the convex polygons results in one convex polygon (in this case, the vehicle convex polygon). It is determined by whether or not an arbitrary point of the polygon PV) is inside the other convex polygon (in this case, the free space convex polygon PF). Then, as a result of the determination, the trajectory determined that the vehicle convex polygon PV collides with the free space convex polygon PF is rejected, and the trajectory giving the shortest distance among the remaining trajectories is selected as the guide trajectory. This trajectory may be different from the trajectory selected in step S102, and it may not be necessary to change the trajectory selected even if there is an obstacle.

It may be determined that the vehicle convex polygon PV and the free space convex polygon PF collide with each other in all of the 64 sets of trajectories generated in step S102. In this case, the above-mentioned references are used. As described above, in the initial turning, the vehicle 20 is moved to the point where it collides with the obstacle that blocks the initial turning circle, and at that point, 64 sets of trajectories are generated again in the same manner as in step S102, and the obstacle is not collided. ,
That is, a trajectory reaching the guidance target position can be generated by selecting the trajectory with the shortest distance in which the vehicle convex polygon PV and the free space convex polygon PF do not collide.

Depending on the current position of the vehicle 20, the guidance target position, and the positional relationship of obstacles, the guidance target position may not be included in the above-mentioned free space convex polygon PF. For example,
As shown in FIG. 16, when the guidance target position B is inside the crank-shaped dead end and the current position A of the vehicle 20 is at the crank entrance, it is set in step S105 from the condition of the inside angle of the convex polygon described above. The free space convex polygon PF1 is as shown in the figure, and the guide target position B is not included. If the guide target position B is not included in the free space convex polygon PF1, the trajectory is generated so that the vehicle 20 reaches the guide target position B outside the free space convex polygon PF1, Even if a simple trajectory is generated, the vehicle convex polygon collides with the free space convex polygon PF1. Therefore, the vehicle 20 cannot reach the guidance target position by the above procedure.

Therefore, as shown in the above-mentioned reference, a plurality of free-space convex polygons having mutually overlapping regions are generated and these free-space convex polygons are connected to each other, and the free-space convex polygons are virtually overlapped with each other. By setting a specific guidance target position and selecting a trajectory from the current position to the virtual guidance target position and a trajectory from the virtual guidance target position to the guidance target position, a trajectory that can reach the guidance target position as a whole can be generated.

Specifically, another free space convex polygon PF2 is generated as shown in FIG. 16, and as shown in FIG. 17, in the area where these two free space convex polygons PF1 and PF2 overlap. A virtual guide target position (hereinafter, referred to as a subgoal) C may be set to create a trajectory around A → C → B. As a subgoal setting method, for example, a joining line that divides the two areas PF1 and PF2 in the overlapping area so as not to overlap each other, that is, a position that is the shortest distance to the guidance target position B on the straight line EF in FIG. And so on. There is also a method of setting a sub-goal position C that minimizes an evaluation function consisting of the distance or the distance from the start position A to the guidance target position B or the number of times by linear or nonlinear programming. Here, if the evaluation function is linear, linear programming is used, and if the evaluation function is nonlinear, nonlinear programming is used. Also, as a method of generating a plurality of free space convex polygons,
As shown in FIG. 18, a method in which a contour line (side in plan view) 41 of the obstacle 40 is extended to connect a plurality of regions obtained by dividing the free space around the vehicle 20 to generate a free space convex polygon There is.

However, as shown in FIG. 19, when the sub-goal cannot be set because the overlapping area of the free space convex polygons PF1 and PF2 is narrow, that is, when the vehicle 20 cannot fit in the overlapping area, this overlapping area A virtual overlapping area 42 is set around the area. The virtual overlapping region 42 is a joining line of the overlapping regions, that is, a quadrangle having a side perpendicular to the straight line EF in FIG. 19, that is, a quadrangle GHIJ in FIG.
And In this quadrangle GHIJ, sides GJ and HI are straight lines EF.
The length of the sides GJ, HI may be any length as long as it is twice or more the length of the longest diagonal of the vehicle 20. The virtual overlap region 42, that is, the subgoal in the quadrangle GHIJ is in the PF1, is the shortest from the start position A, and the vehicle 20 is in contact with the straight line EF.
And PF2, which is the shortest from the guidance target position B, and two points D where the vehicle 20 contacts the straight line EF are set. Further, as described above, the positions of C and D where the evaluation function becomes the minimum may be set by linear or non-linear programming.

Further, in this embodiment, when the effective subgoal cannot be set even by the above-mentioned procedure, that is, when the movement from the start position to the guidance target position is physically impossible, the guidance device 24 or the above-described drawing is used. Notify the occupant to that effect by a notifying device. In the case of a configuration in which the movement of the vehicle 20 is automatically controlled by a steering actuator or the like, the guiding operation is stopped and the vehicle 20 is immediately stopped. However, a trajectory obtained by giving a specific constraint condition to a specific guidance target position is stored in advance in the storage unit 27 in the form of a subgoal, and this subgoal is used during the obstacle avoiding operation in step S105. You may If the storage unit 27 stores subgoals instead of trajectories, the storage capacity can be reduced.

Returning to FIG. 6, in step S106, it is determined whether or not the vehicle 20 has reached the guidance target position. If the determination is affirmative, the operation ends, and if the determination is negative, the process returns to step S104. The above operation is repeated.

Therefore, according to the present embodiment, only one marker 30a is used as the reference point for the guiding target position as the parking position which has been conventionally provided, and the distance between the reference point and the reference point is obtained and the reference axis is used. Since the relative position coordinate and the traveling direction of the vehicle 20 with respect to the reference point are detected by detecting the azimuth of the vehicle 20 with respect to the geomagnetic axis 32, it is not necessary to identify the reference point unlike the conventional vehicle guidance device. The configurations of both the reference point and the detection means on the vehicle 20 side are simple.

Further, in this embodiment, the guidance target position is set to an arbitrary position, and the vehicle 2 is set to the arbitrary guidance target position.
Since 0 is guided, unlike the conventional vehicle guidance device, which differs from the conventional guide system in which the guide trajectory is set to a reference point whose positional relationship with the guide target position is known in advance, the reference point and the guide target position are The degree of freedom of the relative position between the two is increased, and there is no restriction that the vehicle is installed near the parking position unlike the conventional vehicle guidance device.

-Second Embodiment- FIG. 20 (a) is a view showing a second embodiment of the vehicle guiding apparatus according to the present invention, and is a perspective view showing a vehicle to which the vehicle guiding apparatus is applied. In this embodiment, the laser radar device 50 is used as the distance measuring device, and the optical reflector 51 is used as the marker. Further, the optical fiber gyro 52 is used as a sensor that constitutes the azimuth sensor. Therefore, according to this embodiment as well, it is possible to obtain the same effects as those of the above-mentioned first embodiment.

-Third Embodiment- FIG. 20 (b) is a diagram showing a third embodiment of the vehicle guiding apparatus according to the present invention, and is a perspective view showing a vehicle to which the vehicle guiding apparatus is applied. In this embodiment, the CCD camera 61 provided on the rotary pedestal 60 is used as the distance measuring device, and the optical fiber gyro 52 is used as the sensor forming the azimuth sensor. An image serving as a reference point can be extracted from the image data obtained from the CCD camera 61, and the distance to this reference point can be obtained. Further, from the rotation angle of the rotary pedestal 60 at that time, the direction of the vehicle with respect to the reference point can be determined. Can be detected.

Therefore, according to this embodiment as well, it is possible to obtain the same effects as those of the above-mentioned first embodiment. In particular,
In this embodiment, the reference point is determined by image processing, and it is possible to use an arbitrary object as the reference point without using a preset reference point as in the above-described embodiments. Produce an effect.

In this embodiment, since the distance to the reference point and the azimuth with respect to the reference point are obtained by the CCD camera 61 and the rotary pedestal 60, when the height h of the reference point is not known, the procedure is similar to that described above. The vehicle 20 is moved from the point A to the point B in FIG. 9, and the distances L1A ′ and L1B ′ to the reference points at the points A and B and the moving distance dr from the point A to the point B (this is determined by the vehicle speed sensor 28). The height h of the joy point is given by the following equation.

(Equation 5)

In the above embodiment, the distance measuring device 21 and the distance calculating section 29A serve as the distance detecting means and the distance calculating means 29
A corresponds to the horizontal distance calculating means, the azimuth sensor 23 to the traveling direction detecting means, the trajectory calculating section 29B corresponds to the circle calculating means, the tangent calculating means, the selecting means, and the weighting means, and the detection sensor 22 corresponds to the detecting means. To do.

The details of the vehicle guiding device of the present invention are not limited to those of the above-described embodiments, and various modifications are possible. As an example, in each of the above-described embodiments, one guidance target position is stored in the storage unit and the vehicle is guided to this guidance target position. However, a plurality of target position buttons 25a and guidance buttons 25b are set, etc. If a means for selecting a plurality of guidance target positions is provided, the vehicle can be guided to a plurality of guidance target positions.

[0056]

As described in detail above, according to the present invention, the distance between the vehicle and the reference point, which is the reference for guidance, and the traveling direction of the vehicle with respect to the reference axis are calculated, and based on these calculation results. Since the trajectory from the arbitrary starting position to the guidance target position is calculated, the vehicle can be properly guided to the guidance target position by only obtaining the relative positional relationship between one reference point and the vehicle.

[Brief description of drawings]

FIG. 1 is a functional block diagram showing a vehicle guidance device that is a first embodiment of the present invention.

FIG. 2 is a perspective view showing a vehicle equipped with the vehicle guidance system of the first embodiment.

FIG. 3 is a plan view of the vehicle of the first embodiment.

FIG. 4 is a front view showing the input device according to the first embodiment.

FIG. 5 is a front view showing the display device of the first embodiment.

FIG. 6 is a flow chart for explaining the operation of the vehicle guidance system of the first embodiment.

FIG. 7 is a diagram showing a relationship between a three-dimensional distance and a two-dimensional distance between a vehicle and a reference point.

FIG. 8A and FIG. 8B are diagrams for explaining a method of determining the relative coordinates and direction of the vehicle with respect to the reference point.

9A to 9C are views for explaining a method for obtaining the height of a reference point.

10A is a diagram showing an example of a trajectory from a guidance start position to a guidance end position, and FIG. 10B is a diagram for explaining the number of trajectories.

FIG. 11 is a diagram for explaining a trajectory calculation method.

FIG. 12 is a diagram showing a trajectory with a rotation angle <π.

FIG. 13 is a plan view showing a free space convex polygon and a vehicle convex polygon.

FIG. 14 is a diagram for explaining the principle of collision determination of a convex polygon.

FIG. 15 is a flowchart for explaining the operation of collision determination for a convex polygon.

FIG. 16 is a plan view showing the connection of a free space convex polygon.

FIG. 17 is a diagram showing a setting position of a subgoal.

FIG. 18 is a diagram for explaining a method of dividing a free space convex polygon.

FIG. 19 is a plan view showing a setting position of a virtual overlapping area.

FIG. 20 (a) is a perspective view showing a vehicle equipped with a vehicle guidance system of a second embodiment of the present invention, and FIG. 20 (b) is a third view of the present invention.
It is a perspective view showing a vehicle in which a vehicle guidance device which is an example is carried.

FIG. 21 is a diagram for explaining an example of a conventional vehicle guidance device, and is a plan view showing a positional relationship between a parking position and a marker.

FIG. 22 is a perspective view showing an arrangement position of a sensor in an example of a conventional vehicle guidance device.

23A to 23C are schematic plan views showing markers detected by a sensor at different vehicle positions.

FIG. 24 is a diagram for explaining another example of the conventional vehicle guidance device, and is a perspective view showing a positional relationship between a parking position and a barcode.

FIG. 25 is a CCD in another example of a conventional vehicle guidance device.
It is a schematic plan view which shows the arrangement position of.

[Explanation of symbols]

 20 vehicle 21 distance measuring device 22 obstacle detection sensor 23 direction sensor 24 guidance control device 25 input device 26 output device 27 storage unit 28 vehicle speed sensor 29 calculation unit 29A distance calculation unit 29B trajectory calculation unit 30a marker

Claims (8)

[Claims] 1. A vehicle guidance device for guiding a vehicle to a predetermined guidance target position, distance detection means for detecting a distance between a reference point serving as a reference for guidance and the vehicle, and traveling of the vehicle with respect to a reference axis on the ground surface. A traveling direction detecting means for detecting a direction, and a trajectory from an arbitrary starting position to the guiding target position based on the distance detected by the distance detecting means and the vehicle traveling direction detected by the traveling direction detecting means And a guide control unit that guides the vehicle to the guide target position in accordance with the trajectory calculated by the trajectory calculation unit. 2. The vehicle guiding apparatus according to claim 1, wherein the distance detecting unit outputs a distance signal that correlates to the distance between the reference point and the vehicle, and a distance signal outputting unit that outputs the distance signal. The height between the vehicle and the reference point is calculated based on the distance signal obtained from the distance signal output means along with the movement, and the reference point and the distance are calculated based on the calculated height and the distance signal. A vehicle guidance device comprising: a horizontal distance calculating means for calculating a horizontal distance to a vehicle. 3. The vehicle guiding apparatus according to claim 1, further comprising a detection unit that detects an obstacle around the vehicle, wherein the trajectory calculation unit detects the obstacle by the detection unit. A vehicle guidance device, wherein the trajectory is recalculated so that the vehicle does not collide with the obstacle, and the guidance control unit guides the vehicle according to the trajectory recalculated by the trajectory calculation unit. . 4. The vehicle guidance device according to claim 1, wherein the trajectory calculation means passes through the starting position and the guidance target position, respectively.
A circle calculating means for calculating two circles each having a radius of the minimum turning radius of the vehicle, a tangential line calculating means for calculating a tangent line connecting two circles passing through the starting position and the guide target position, and the calculated circle and A vehicle guidance device comprising: a selecting unit that selects the track from a combination of tangent lines. 5. The vehicle guiding apparatus according to claim 4, wherein the selecting unit selects a track having a minimum distance in the combination of the circle and the tangent line. 6. The vehicle guiding apparatus according to claim 4, wherein the trajectory calculating means has weighting means for weighting a travel distance of a trajectory of the vehicle traveling in a reverse direction, and the selecting means. After the weighting, the vehicle guidance device is characterized in that the trajectory having the minimum distance is selected from the combination of the circle and the tangent line. 7. The vehicle guiding apparatus according to claim 1, wherein the reference point is a radio wave reflector, and the distance detecting unit emits a radio wave toward the radio wave reflector mounted on the vehicle. A radio wave transmitter for transmitting and a radio wave receiver for receiving reflected radio waves from the radio wave reflector are provided,
The distance between the reference point and the vehicle is calculated based on the time until the radio wave emitted from the radio wave transmitter is received by the receiver, and the traveling direction detecting means is the reference axis. A vehicle guidance device comprising: a geomagnetic sensor that outputs a signal that correlates to the direction of the geomagnetic axis; and a computing unit that calculates the traveling direction of the vehicle with respect to the geomagnetic axis measured by the geomagnetic sensor. 8. The vehicle guiding apparatus according to claim 2, wherein the reference point is a radio wave reflector, and the distance signal output means emits a radio wave to the radio wave reflector mounted on the vehicle. And a radio wave receiver for receiving the reflected radio wave from the radio wave reflector, wherein the horizontal distance computing means is provided for the time until the radio wave emitted from the radio wave transmitter is received by the receiver. The distance between the reference point and the vehicle is calculated based on the traveling direction detecting means, and a geomagnetic sensor that outputs a signal correlated with the direction of the geomagnetic axis that is the reference axis, and the geomagnetic sensor measures the geomagnetic sensor. A vehicle guidance device comprising: a calculation unit that calculates a traveling direction of the vehicle with respect to the geomagnetic axis.