JP2012153324A - Track calculation device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To calculate the track with a small calculation amount considering obstacles at low cost.SOLUTION: The position of each obstacle recognized by an obstacle recognition means of a track planning unit 8 of a vehicle 1 is referred to. If any obstacle is detected on a track connecting an arc of the vehicle turning radius at the designated starting position to an arc of the vehicle turning radius at the target position by a line in contact with both arcs, an extraction means of the track planning unit 8 extracts the distance between obstacles having the length allowing the vehicle 1 to pass through out of various kinds of distances between the detected obstacle and each peripheral obstacle, and a setting means of the track planning unit 8 sets an obstacle avoiding circle for avoiding any collision for each opposing obstacle of each distance between obstacles extracted by the extraction means. If both obstacle avoiding circles do not overlap, a track calculation means of the track planning unit 8 calculates the track reaching the target position from the starting position via a relay point while the relay point is an intersection between any one of the obstacle avoiding circles with the distance line between the obstacles.

Description

  The present invention relates to a trajectory calculation device that calculates a trajectory of a vehicle at the time of parking assistance, automatic parking, and the like, and more particularly, to a trajectory calculation in consideration of an obstacle.

  Conventionally, a parking assistance device or an automatic parking device as one of vehicle driving assistance devices, for example, moves a vehicle from a start position (current position) designated by a driver in a parking lot to a predetermined parking position according to a calculated trajectory. Guidance or automatic driving. In calculating the trajectory, it has been proposed to calculate the trajectory by calculating a two-circle orbit model having a higher degree of freedom of movement than the one-circle orbit model (for example, Patent Document 1 (paragraphs [0002] to [0003]). ], [0045]-[0050], FIG. 2 etc.)).

  In both the 1-circle and 2-circle models, the trajectory of the vehicle that reaches the target position from the specified start position (current position) is defined by the arc of the vehicle turning radius (for example, the minimum turning radius) and the tangent line. The one-circle trajectory model calculates a trajectory by combining, for example, one arc of the minimum turning radius drawn between the start position and the target position and a tangential straight line. The two-circle orbit model calculates a smoother orbit by a combination of two arcs of the minimum turning radius of each of the start position and the target position and a tangential line.

JP 2008-296638 A

  In the conventional device such as the parking assist device of Patent Document 1, even in the case of a two-circle trajectory model, it only calculates a trajectory in which the arc at the start position and the arc at the target position are connected by a tangential line. It is not possible to calculate a trajectory that avoids an obstacle (for example, a pillar, a wall, a parked vehicle, etc.) existing between the start position and the target position. Therefore, especially when there are a plurality of obstacles on the calculated trajectory, the driver can only drive by considering a detour trajectory that avoids the obstacles.

  By the way, as a method for avoiding a plurality of obstacles, a so-called “potential method” has been proposed in the field of robots. This method considers a potential field in which an attractive force is generated according to the distance from the target position (destination), and a repulsive force is generated from each obstacle according to the distance. This is a method of creating a trajectory path along the path.

  In the above-described parking assist device for vehicles, it is conceivable to calculate the trajectory using the “potential method” and avoid a plurality of obstacles. In this case, it is extremely difficult to reach the target position from the start position. It is necessary to calculate a large number of trajectories, the calculation amount becomes enormous, and a high-performance arithmetic unit or the like is required, resulting in an increase in cost.

  An object of the present invention is to calculate a trajectory considering an obstacle with a small amount of calculation at low cost.

In order to achieve the above object, the trajectory calculation apparatus of the present invention uses a trajectory of a vehicle that reaches a target position from a specified start position as a trajectory of a combination of an arc of a vehicle turning radius and a straight line that touches the arc. A trajectory calculation device for calculating, an obstacle recognition means for recognizing the positions of obstacles around the start position and the target position, an arc of a vehicle turning radius at the start position, and a vehicle turning radius at the target position If an obstacle is detected on a track that is connected to a straight line that touches both arcs, the obstacle between the detected obstacle and the obstacles around it is long enough for the vehicle to pass. An extraction means for extracting between objects; a setting means for setting an obstacle avoidance circle for avoiding a collision with respect to each of obstacles facing each other between the obstacles extracted by the extraction means;
If the obstacle avoidance circles do not overlap, an arc of the vehicle turning radius at the start position with the intersection of either one of the obstacle avoidance circles and the distance line between the obstacles as the relay point, and A trajectory that connects a circular arc of a vehicle turning radius passing through a relay point and a circular arc of a vehicle turning radius at the target position by a straight line that touches each of the circular arcs, from the start position to the target position via the relay point And a trajectory calculating means for calculating a trajectory to reach (claim 1).

  The trajectory calculation means of the trajectory calculation apparatus according to the present invention further includes a line connecting intersections of the obstacle avoidance circles if the trajectory calculation means partially overlaps the obstacle avoidance circles. A trajectory that reaches the target position from the start position via the relay point is calculated using the intersection of the minute and the distance line between the obstacles as a relay point (claim 2).

  According to the trajectory calculation apparatus of the present invention as set forth in claim 1, referring to the position of each obstacle recognized by the obstacle recognition means, the vehicle turning radius arc at the specified start position and the vehicle turning at the target position. If an obstacle is detected on a trajectory that connects a circular arc of a radius with a straight line that touches both arcs, the extraction means will calculate the trajectory that bypasses the obstacle and avoids the obstacle. Among the obstacles, the distance between the obstacles through which the vehicle can pass is extracted. Furthermore, in order to calculate the trajectory that passes between obstacles so that they do not collide with obstacles, the setting means sets obstacle avoidance circles that avoid collisions between the obstacles facing each other between the obstacles extracted by the extraction means To do.

  If the obstacle avoidance circles do not overlap between the extracted obstacles, both obstacles are calculated by the trajectory calculation means so that the trajectory is shortened as much as possible and the fuel consumption is improved by minimizing the turning steering. A vehicle that passes through the arc of the turning radius of the vehicle at the start position and the vehicle passing through the intersection point between one of the avoidance circles, specifically the intersection of the obstacle avoidance circle on the side closer to the vehicle and the distance line between the obstacles A trajectory that connects the arc of the turning radius and the arc of the turning radius of the vehicle at the target position with a straight line that touches each arc, and reaches the target position from the start position via the relay point and avoiding multiple obstacles One or more trajectories can be calculated.

  In this case, the obstacles that the vehicle cannot pass are removed in advance, a relay point is set between the obstacles that the vehicle can pass, and the trajectory passing through each relay point between the start point and the target position may be calculated. Therefore, the calculation amount is small compared with the calculation of the “potential method” and the like, and a high-performance arithmetic unit or the like is unnecessary, and the cost can be reduced.

  According to the trajectory calculation apparatus of the present invention as set forth in claim 2, even if the distance between the obstacles through which the vehicle can pass is, a part of the obstacle avoidance circle set for each of the obstacles facing each other is mutually For overlapping narrow obstacles, if the intersection of one of the obstacle avoidance circles and the distance line between the obstacles is used as a relay point as described above, there is a possibility that the obstacle will be too close. A trajectory that is not too close to the obstacle can be calculated by using the intersection of the line segment connecting the intersections of both obstacle avoidance circles and the distance line between the obstacles (that is, the intermediate point between the obstacles) as a relay point.

It is a block diagram of one embodiment of a trajectory calculation device of the present invention. It is explanatory drawing of an example of the recognition map of the data storage part of FIG. It is explanatory drawing of the example of a locus | trajectory calculated by calculation of the 2-circle orbit model of the locus | trajectory plan part of FIG. It is explanatory drawing of the example of extraction between the obstructions of the trajectory plan part of FIG. It is explanatory drawing of the example of the extraction result between the obstructions which a vehicle can pass. It is explanatory drawing of the example of a setting of an obstacle avoidance circle. (A) is explanatory drawing between the obstacles where the obstacle avoidance circle of both ends does not overlap, (b) is explanatory drawing between the obstacles where the obstacle avoidance circles of both ends overlap. (A) is an explanatory diagram of a part of a procedure for determining a trajectory combining a start position, a target position and each relay point, and (b) is an example of a procedure for determining a trajectory combining a start position, a target position and each relay point. FIG. It is explanatory drawing of the example of calculation of the determined orbit. It is a flowchart for operation | movement description of the track | orbit plan part of FIG.

  An embodiment of the present invention will be described with reference to FIGS.

  FIG. 1 shows a trajectory calculation apparatus according to this embodiment provided in a vehicle 1. The trajectory calculation apparatus is a parking assist or automatic parking trajectory when the vehicle 1 is parked in a predetermined direction at a predetermined parking position of a parking lot, for example. Is calculated.

  And information on at least the position of each obstacle such as a pillar or wall of a parking lot, a fire hydrant, a parked vehicle (this information may be information of an absolute position on a map obtained by GPS reception, for example, (It may be information on the relative distance from the appropriate reference point such as the entrance of the parking lot, or the position of the direction, etc.), but it can be rewritten as an obstacle map in some way on a hard disk or solid memory, etc. The data storage unit 2 stores and holds the data. Specifically, for example, every time the vehicle 1 travels in a parking lot, the radar data of the radar unit 3 such as laser radar or ultrasonic radar and the monocular formed by the camera unit 4 such as a CCD image sensor or a CMOS image sensor are used. Based on the captured image of the camera, the obstacle detection unit 5 detects each obstacle by a well-known sensor fusion object recognition process, and the position information thereof is updated and written in the data storage unit 2 and accumulated and held. . Alternatively, the position information of each obstacle is wirelessly transmitted from the external device into the parking lot, and the position information is received by, for example, the car navigation device of the vehicle 1 and written in the data storage unit 2 so as to be updated and stored. The

  Next, for example, when the driving of the vehicle 1 is stopped at an appropriate point in the parking lot and the driver presses a parking assistance or automatic parking switch (not shown), the vehicle 1 is set to the parking assistance or automatic parking mode. The At this time, based on the obstacle map in the data storage unit 2 and the captured image of the camera unit 4, the current position of the vehicle 1 is set in the car navigation device or the like set in the parking assistance or automatic parking display mode of the vehicle 1, A map screen of parking positions and obstacles around them is displayed.

  Then, for example, the driver operates the operation unit 6 formed by the map screen having a touch panel configuration such as the car navigation device, and touches the start position (for example, the current position) and the parking position where the map screen trajectory calculation is started. When these positions are designated, the designation is given to the target position determination unit 7.

  The target position determination unit 7 determines a start position and a target position designated with reference to the obstacle map in the data storage unit 2 and gives the position information to the trajectory planning unit 8.

  The trajectory planning unit 8 is formed by a microcomputer, similar to the obstacle detection unit 5, the target position determination unit 7, and an operation control unit 9, a movement amount management unit 11, a vehicle stability control unit 12, and an EPS control unit 13, which will be described later. By executing the set program, the obstacle recognition means, extraction means, setting means, and trajectory calculation means of the present invention are formed.

  The obstacle recognition means recognizes the position of each obstacle around the start position and the target position based on the position information from the target position determination unit 7 and the position information of each obstacle in the obstacle map of the data storage unit 2. Then, a recognition map is created in which the start position, the target position, and the obstacles around them are arranged.

  FIG. 2 shows an example of the recognition map MP, where S is a designated start position (current position), G is a designated target position (parking position), and α1 to α4 are obstacles. The isosceles triangular symbol mark of the start position S and the target position G indicates the position and direction (direction) of the vehicle 1, the vertex indicates the front side of the vehicle 1, and the bottom side indicates the rear side of the vehicle. And if it is a human, the locus | trajectory (path | route) like the white arrow line of FIG. In the present embodiment, as will be described later, such an obstacle avoidance trajectory is calculated by reducing the amount of calculation as much as possible.

  The extraction means, for example, by calculation of a well-known two-circle trajectory model, on the recognition map MP, an arc of a vehicle turning radius at the start position S (for example, the minimum turning radius of the vehicle 1) and a vehicle turning radius at the target position G ( For example, if a trajectory connecting the arc of the minimum turning radius of the vehicle 1 with a straight line that touches both arcs is drawn and any one of the obstacles α1 to α4 is detected on the trajectory, the detected obstacle (for example, an obstacle) Of the obstacles between the obstacle α1) and the surrounding obstacles α2 to α4, the distance between the obstacles through which the vehicle 1 can pass is extracted. If the obstacles α1 to α4 are not detected on the track, the track reaches the target position G from the start position S without colliding with the obstacles α1 to α4. Decide on a guide track for automatic parking.

  FIG. 3 is an explanatory diagram of obstacle detection when the obstacle α1 is detected on the trajectory calculated by the calculation of the two-circle trajectory model. The two broken circles C1 and C2 in contact with each other are drawn, and the two broken circles C3 and C4 in contact with each other on the left and right of the target position G are also drawn. Further, arcs ARC2 and ARC3 formed by a part of the circles C2 and C3 selected from the two circles C1, C2, C3, and C4 in consideration of the turning direction of the vehicle 1 and the like are represented by a straight line SL23 tangent to the circles C2 and C3. The trajectory T23 connected with is calculated. Then, since the calculated trajectory T23 passes through the obstacle α1, the obstacle α1 existing on the trajectory T23 is detected. In this case, the track T23 is not determined as the guide track. Note that the method of calculating the trajectory T23 connecting the arc ARC2 of the start position S and the arc ARC3 of the target position G is not limited to the above-described calculation method of the two-circle trajectory model.

  FIG. 4 is an explanatory diagram of an example of extraction between obstacles when the obstacle α1 is detected on the trajectory T23. In this case, the extraction means searches for the shortest distance point between the detected obstacle α1 and each of the obstacles α2 to α4 in the vicinity thereof, and distance lines L12, L13 between the obstacles connecting the two shortest distance points. The distance between the obstacles with a length greater than or equal to the predetermined reference length is extracted except for the distance between the obstacles whose length L14 is shorter than the predetermined reference length set longer than the lateral width of the vehicle 1.

  FIG. 5 shows an example of the result of extraction between obstacles. Here, the distance line L14 between obstacles is shorter than a predetermined reference length, and the vehicle 1 cannot pass through the distance line L14 (see FIG. Between the obstacles on the distance lines L12 and L13 is extracted.

  The setting means sets an obstacle avoidance circle Cc for avoiding a collision for each of the obstacles α1 and α2 and the obstacles α1 and α3 facing each other between the obstacles extracted by the extraction means.

  FIG. 6 shows a setting example of the obstacle avoidance circle Cc. Each obstacle avoidance circle Cc indicated by a broken line is, for example, the minimum turning radius of the vehicle 1 and between the obstacles closest to each other by the obstacle α1 and the obstacle α2. Both ends of the distance line L12, and both ends of the distance line L13 between the obstacles where the obstacle α1 and the obstacle α3 are closest to each other are drawn and set. In order to reduce the deterioration of fuel consumption due to the turning steering of the vehicle 1, it is preferable to reduce the turning operation of the vehicle 1 so that the track is not bent as much as possible. It is drawn and set with a point closer to the obstacle than the both ends of the distance lines L12 and L13 between.

  The trajectory calculation means is configured so that the obstacle avoiding circles Cc of the obstacle α1 and the obstacle α2 and the obstacle α1 and the obstacle α3 facing each other do not overlap each other between the obstacles of the extracted distance lines L12 and L13. , With each intersection of one of the obstacle avoidance circles Cc and the distance lines L12 and L13 between the obstacles as a relay point J, the arc of the vehicle turning radius at the start position S and the turning radius at the relay point J A trajectory that connects a circular arc and an arc of the vehicle turning radius at the target position G with a straight line that touches each of the circular arcs and that reaches the target position G from the start position S via at least one relay point J is calculated. To do. If a part of both obstacle avoidance circles Cc overlap each other, the intersection of the line segment connecting the intersections of both obstacle avoidance circles Cc and the distance line between the obstacles is set as a relay point J from the start position S. A trajectory that reaches the target position G via the relay point J is calculated.

  FIG. 7A shows an enlarged portion between the obstacles α1 and α2 where the obstacle avoidance circles Cc at both ends do not overlap, and FIG. 7B shows an obstacle where a part of the obstacle avoidance circles Cc at both ends overlap. The part between α1 and α3 is shown enlarged. When both obstacle avoidance circles Cc do not overlap, the vehicle is close to the vehicle 1 at the start position S, as shown in FIG. The intersection point between the obstacle avoidance circle Cc on the obstacle α1 side and the distance line L12 between the obstacles is set as a relay point J, and the vehicle 1 indicated by a triangle in the figure calculates a trajectory passing through the relay point J. When both obstacle avoidance circles Cc overlap, as described above, the intersection of the obstacle avoidance circle Cc on the obstacle α1 side near the vehicle 1 at the start position S and the distance line L12 between the obstacles is determined. If the relay point J is used, there is a possibility that the vehicle 1 is too close to the obstacle α1, and therefore, as shown in FIG. 7B, the line segment LC connecting the intersections P1 and P2 of the obstacle avoidance circle Cc and the obstacle The intersection point of the distance line L13 between the objects, that is, a substantially intermediate point between the obstacles α1 and α3, is set as the relay point J, and a trajectory in which the vehicle 1 is not too close to the obstacles α1 and α3 is calculated.

  FIGS. 8A and 8B show an example of a procedure for determining a trajectory combining the start position S, the target position G, and each relay point J set as described above. First, FIG. 8A shows the procedure. As described above, the start position S, the target position G, and each relay point J are used as nodes, and they are connected by each link LK. Next, as shown in FIG. 5B, a link LK that passes through any of the obstacles α1 to α4, here, a link that extends from the start position S through the obstacle α1 to the relay point J of the distance line L13. LK is deleted, and a trajectory candidate Lx in which the start position S, the relay point J of the distance line L12, the relay point J of the distance line L13, and the target position G are connected by the remaining link LK is obtained. When there are a large number of relay points J, a plurality of trajectory candidates Lx are obtained.

  FIG. 9 shows an example of calculation of the determined trajectory. For each trajectory candidate Lx such as the trajectory candidate Lx in FIG. 8B, the minimum turning radius drawn so as to be in contact with the start position S on the left and right in the vehicle traveling direction. The arc ARC1 of the two broken circles C1 and C2 (specifically, the circle C1 on the left side of the paper close to the relay point J of the distance line L12 so as to connect as smoothly as possible) and the relay point J of the distance line L12 One of the two broken circles Cj with the minimum turning radius drawn so as to be in contact with each other on the left and right in the vehicle traveling direction (specifically, a circle on the right side of the page close to the vehicle 1 at the start position S so as to be connected as smoothly as possible) For example, one of the two circles Cj of a broken line with a minimum turning radius drawn to be in contact with the left and right of the vehicle traveling direction at the relay point J of the distance line L13 (specifically, connect as smoothly as possible) Next An arc ARCj on the upper side of the paper C3 near the target position G), for example, one of two circles C3 and c4 with a minimum turning radius drawn in contact with the left and right in the vehicle traveling direction at the target position G (specifically Specifically, the trajectory Ly is formed by connecting the arcs ARC3 of the circle C3) on the left side of the paper close to the previous relay point J with the respective straight lines SL1j, SLjj, SLj3 so as to connect as smoothly as possible, from the start position S. The trajectory Ly that reaches the target position G while avoiding the plurality of obstacles α1 to α4 via the two relay points J is calculated as a guide trajectory. When a plurality of tracks can be calculated by the same method, all the tracks are calculated as guide tracks.

  FIG. 10 shows an example of the calculation procedure of the trajectory Ly by the above means of the trajectory planning unit 8. First, the trajectory connecting the start position S and the target position G is calculated by, for example, normal trajectory calculation of a two-circle trajectory model ( Step S1). At this time, if none of the obstacles α1 to α4 exists on the track, this track is determined as the guide track and the process is terminated (NO in step S2). On the other hand, if any of the obstacles α1 to α4 is present on the trajectory (YES in step S2), distance lines L12 to L14 between the obstacle and the obstacles in the vicinity are calculated (step S3).

  Of the distance lines L12 to L14 between the obstacles, distance lines between obstacles (for example, distance line L14) that the vehicle 1 cannot pass are deleted, and obstacles that the vehicle 1 can pass through are extracted (step S4). .

  Next, an obstacle avoidance circle Cc is drawn from both ends of the distance lines L12 and L13 between the extracted obstacles (for the obstacles facing each other) (step S5).

  If the obstacle avoidance circles Cc at both ends do not overlap (NO in step S6), the intersection point between one of the obstacle avoidance circles Cc and the distance line L13 is set as the relay point J (step S7). The direction of the relay point J is the tangential direction of the obstacle avoidance circle Cc. Furthermore, the start position S, each relay point J, and the target position G are used as nodes to connect with each link LK (step S8), and the link LK that passes through (collises with) any of the obstacles α1 to α4 is deleted (step S9). .

  Then, a trajectory candidate Lx is calculated from the path connected by the remaining link LK (step S10), and a trajectory from the start position connected by the link LK to the target position G via the relay point J for each trajectory candidate Lx. Ly is calculated as a guide track (step S11), and the process ends.

  In step S5, if part of the obstacle avoidance circle Cc drawn from both ends of the extracted distance lines L12 and L13 between the obstacles overlaps (YES in step S6), the obstacle avoidance circle Cc at both ends is overlapped. The intersection point of the line segment LC connecting the intersection points P1 and P2 and the distance lines L12 and L13 is set as a relay point J (step S12), and the relay station J is connected from the start position connected by the link LK by the processing after step S8. The trajectory Ly reaching the target position G is calculated as a guide trajectory. In this case, the direction of the relay point J is the direction of the inclination of the line segment LC.

  Then, the trajectory planning unit 8 avoids the plurality of obstacles α1 to α4 by passing through the relay point J as described above, and calculates a position from the start position S to the target position G or a plurality of guide trajectories. These guide tracks are displayed on the map screen of the car navigation device or the like, for example, and one of the tracks is set as a parking assistance or automatic parking track by a driver's selection operation. Instead of selecting by the driver, for example, the shortest guide track that consumes less fuel may be automatically selected.

  Furthermore, when the driver commands the start of parking assistance or automatic parking by a button operation or the like, information on the set guide track is input to the driving control unit 9 of FIG. 1, and the driving control unit 9 causes the vehicle 1 to be described later. Is controlled.

  Further, the movement amount management unit 11 starts the vehicle 1 based on the wheel speed of the wheel speed sensor of the vehicle sensor unit 10, the rudder angle of the rudder angle sensor, the yaw rate of the yaw rate sensor, the depression amount of the accelerator pedal and the brake pedal, and the like. Monitor the amount of movement from the position and the direction of movement.

  Then, the feedback control of the operation control unit 9 based on the monitoring of the movement amount management unit 11 causes the vehicle 1 to move from the start position S to the target position G via the relay point J along the guide track. Further, acceleration / braking of the vehicle 1 by a vehicle stability control unit (VSC (Vehicle Stability Control) ECU, etc.) 12 is controlled, and steering of the vehicle 1 is performed via an electric power steering control unit (EPS ECU, etc.) 13. Be controlled.

  And based on said parking assistance or control of automatic parking, the vehicle 1 moves to a predetermined parking position (target position G) along the guide track in consideration of energy saving.

  As described above, in the case of the present embodiment, one or a plurality of trajectories that reach the target position from the designated start position S by avoiding the plurality of obstacles α1 to α4 are calculated, and the calculated trajectory is obtained. The vehicle 1 can be moved from the start position S to the target position G by the parking assistance based on the control or the automatic parking control.

  Then, a path passing through each relay point J between the start position S and the target position G is set in advance between the obstacles through which the vehicle 1 can pass except for the obstacles through which the vehicle 1 cannot pass. Therefore, the calculation amount is small compared with the calculation of the “potential method” and the like, and a high-performance arithmetic unit or the like is unnecessary, and the cost can be reduced. In addition, since a trajectory with less turning by steering and a short moving distance is calculated, there is an advantage that fuel consumption is improved.

  In addition, between narrow obstacles where a part of the obstacle avoidance circles Cc set for the obstacles α1 and α3 facing each other overlap each other, between the line segment LC connecting the intersections of both obstacle avoidance circles Cc and the obstacles A trajectory that is not too close to the obstacles α1 and α3 can be calculated using the intersection point of the distance line L13 (that is, a substantially intermediate point between the obstacles α1 and α3) as the relay point J.

  The present invention is not limited to the above-described embodiment, and various modifications other than those described above can be made without departing from the spirit of the present invention. Or the like, and the calculation procedure of the trajectory planning unit 8 may be different from that of the embodiment.

  Next, it is needless to say that the present invention can be similarly applied when the start position S and the target position G are set at a place other than the parking lot. At this time, the number, direction, and the like of the relay point J between the start position S and the target position G vary depending on the number of obstacles α1 to α4, the arrangement shape, and the like.

  The present invention can be applied to the calculation of various vehicle trajectories.

1 Vehicle 8 Trajectory Planning Department G Target Position J Relay Point S Start Position α1 to α4 Obstacle

Claims (2)

A trajectory calculation device that calculates a trajectory of a vehicle that reaches a target position from a specified start position as a trajectory of a combination of an arc of a vehicle turning radius and a straight line that touches the arc,
Obstacle recognition means for recognizing the position of each obstacle around the start position and the target position;
If an obstacle is detected on a track connecting the arc of the vehicle turning radius at the start position and the arc of the vehicle turning radius at the target position by a straight line contacting both arcs, the detected obstacle and the obstacles around it are detected. Extracting means for extracting between the obstacles of a length between which the vehicle can pass,
A setting means for setting an obstacle avoidance circle for avoiding a collision with respect to each obstacle facing each other between the obstacles extracted by the extraction means;
If the obstacle avoidance circles do not overlap, an arc of the vehicle turning radius at the start position with the intersection of either one of the obstacle avoidance circles and the distance line between the obstacles as the relay point, and A trajectory that connects a circular arc of a vehicle turning radius passing through a relay point and a circular arc of a vehicle turning radius at the target position by a straight line that touches each of the circular arcs, from the start position to the target position via the relay point A trajectory calculation device comprising trajectory calculation means for calculating a trajectory to reach. The trajectory calculation apparatus according to claim 1,
The trajectory calculation means, if a part of the obstacle avoidance circles overlap each other, the intersection of the line segment connecting the intersections of the obstacle avoidance circles and the distance line between the obstacles as a relay point, A trajectory calculation apparatus that calculates a trajectory that reaches the target position from the start position via the relay point.

Images