Travel planning device, travel planning method, and travel planning program

Abstract

This travel planning device comprises: an initial processing unit that acquires map information and a target route; an acquisition unit that acquires an initial position and attitude of a vehicle; an update unit that updates a future position and attitude of the vehicle for each of update cycles previously set according to the initial position and attitude of the vehicle; a speed planning unit that sets a candidate position series of the vehicle within a prediction target time range longer than the future update cycle on the basis of the future position and attitude of the vehicle, and obtains, on the basis of the map information, the target route, and the candidate position series, a planned speed of the vehicle at which the vehicle can travel while preventing an obstacle from entering a safety area over the prediction target time range and while reducing a deviation from the target route when the planned speed is used for traveling of the vehicle; and a generation unit that generates a planned speed series for each update cycle.

Description

Travel planning device, travel planning method, and travel planning program

[0001] The present disclosure relates to a driving planning device, a driving planning method, and a driving planning program.

[0002] Patent Document 1 (JP 2023-154648 A) discloses a device that plans control inputs for driving a vehicle so that obstacles do not enter a safety zone, which increases in length as the vehicle speed increases from the front end toward the front. This device reduces the vehicle's driving speed when an obstacle is detected within the safety zone. Specifically, this device first acquires map information indicating the start point, finish point, a reference route from the start point to the finish point (e.g., a line connecting the center positions of the passage width), and the location of obstacles in the driving zone. It then plans a series of vehicle translational speeds and steering angles that will enable the vehicle to travel from the start point to the finish point in the shortest time while preventing the safety zone and the vehicle body from interfering with the obstacles.

[0003] Incidentally, when obtaining information on the start point, finish point, and location of obstacles in the driving area before driving, and then determining a target route that will allow the vehicle body to reach the finish point without interfering with obstacles, a method is required for planning control inputs that will allow the vehicle to drive while suppressing deviation from the target route and preventing interference between the safety area and the vehicle body with obstacles.

[0004] The present disclosure aims to provide a driving planning device, a driving planning method, and a driving planning program that create a driving plan that allows a vehicle to travel while suppressing deviation from a target route, while preventing obstacles from entering a safety area that is set according to the vehicle's speed.

[0005] In order to achieve the above object, a travel planning device according to the present disclosure is a travel planning device for a vehicle in which a safety region is set having a length that increases as the translational speed of the vehicle increases from the front end of the vehicle forward and a width that approximates the width of the vehicle, and includes: an initial processing unit that acquires map information indicating the placement of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while the vehicle is traveling; an acquisition unit that acquires an initial position and attitude of the vehicle; an update unit that updates a future position and attitude of the vehicle at each predetermined update period from the initial position and attitude of the vehicle; a speed planning unit that sets a candidate position series that is a series of candidate positions of the vehicle within a future target time range that is longer than the update period, based on the future position and attitude of the vehicle, and calculates a planned speed of the vehicle that, when used for traveling the vehicle, prevents the obstacle from entering the safety region throughout the target time range and allows the vehicle to travel with minimal deviation from the target route, based on the map information, the target route, and the candidate position series; and a generation unit that generates a planned speed series that is the series of planned speeds for each update period.

[0006] According to the present disclosure, it is possible to provide a driving planning device, a driving planning method, and a driving planning program that create a driving plan that allows a vehicle to travel while suppressing deviation from a target route, while preventing obstacles from entering a safety area that is set according to the vehicle's speed.

[0007] FIG. 1 is a schematic diagram illustrating an example of a safety area. FIG. 2 is a diagram illustrating the hardware configuration of a travel planning device. FIG. 3 is a configuration diagram of a travel planning device according to this embodiment. FIG. 4 is an example of a grid representing a set candidate position series. FIG. 5 is a diagram illustrating an example of setting a candidate position series. FIG. 6 is a diagram illustrating calculation of an obstacle term. FIG. 7 is a diagram illustrating calculation of an obstacle term taking a safety area into consideration. FIG. 8 is a flowchart illustrating the flow of travel planning processing by the travel planning device according to the first embodiment. FIG. 9 is a flowchart illustrating the flow of speed planning processing according to the first embodiment. FIG. 10 is a diagram illustrating an example of a vehicle travel image corresponding to the travel planning processing by the travel planning device. FIG. 11 is an example of an image when the shape of the vehicle is represented by a circle. FIG. 12 is an illustration of a candidate state based on an objective function and constraints. FIG. 13 is an illustration of obstacle determination. FIG. 14 is an illustration of obstacle determination taking a safety area into consideration. FIG. 15 is a flowchart illustrating the flow of travel planning processing by the travel planning device according to a second embodiment. FIG. 16 is a flowchart illustrating the flow of speed planning processing according to the second embodiment. Fig. 17 is a diagram for explaining a method for expressing the shape of a vehicle using multiple circles. Fig. 18 is a diagram for explaining a method for expressing the shape of a vehicle using multiple circles. Fig. 19 is a diagram for explaining a setting mode of a candidate position series in a modified example. Fig. 20 is an image diagram for determining v and ω using DWA. Fig. 21 is an image diagram for determining v and ω using MPC. Fig. 22 is an experimental example in which the shape of a vehicle is expressed using multiple circles and the method of this embodiment is applied.

[0008] An example of an embodiment of the present invention will be described below with reference to the drawings. The same reference numerals are used throughout the drawings to designate identical or equivalent components and parts. The dimensional proportions of the drawings are exaggerated for illustrative purposes and may differ from the actual proportions.

[0009] The present disclosure relates to a driving plan for a vehicle serving as a mobile robot. The "driving plan" here refers to a driving plan generated by simulation before the vehicle travels, and the actual vehicle travels according to the driving plan. The driving plan of the present disclosure plans the vehicle's travel by setting a safety region whose length increases as the vehicle's translational speed increases from the front end of the vehicle forward and whose width approximates the vehicle's width. "Forward" here refers to the direction toward which the vehicle is traveling. Generally, the direction is preferably the same as the vehicle's direction of travel. However, in the case of a vehicle with a steering mechanism, the direction toward which the vehicle is traveling based on the vehicle's posture may be used instead. The width of the safety region may be wider than the vehicle's width, as long as it is appropriate for the purpose of enabling travel without interfering with obstacles. For example, a safety region width several times the vehicle's width would make it difficult to travel without interfering with obstacles, and therefore such a size is not included. The shapes of the safety region and vehicle used in this embodiment may be circular, rectangular, elliptical, or polygonal.

[0010] In this embodiment, the map information indicating the location of obstacles, the safety area, and the vehicle shape are described as being projected onto a plane, but they are not limited to this and may be treated three-dimensionally, including the height direction. In this case, the shape of the safety area and the vehicle may be configured as a sphere, a rectangular parallelepiped, a cube, etc. Furthermore, the calculation to prevent the safety area and the vehicle from interfering with obstacles also includes the distance in the height direction.

[0011] The safety area will now be described. Fig. 1 is a schematic diagram illustrating an example of the safety area. The safety area may be determined in advance from information on safety standards and vehicle shape, or may be determined from the distance required for the target vehicle to decelerate and stop based on its traveling speed. The safety area is defined using the length of the upper safety area, the upper speed, the length of the lower safety area, and the lower speed.

[0012] For example, assume that the upper limit safety zone length is 1.6 m, the upper limit speed is 1.8 m / sec, the lower limit safety zone length is 0.35 m, and the lower limit speed is 0.3 m / sec. In this case, the length Y of the safety zone at an arbitrary translational speed can be calculated linearly based on the upper and lower limit values ​​using the following formula:

[0013] Y=(1.6-0.35) / (1.8-0.3)*(v-0.3)+0.35

[0014] where v is an arbitrary translational velocity. The vertical width of the safety area is calculated using the above formula, and the horizontal width is calculated from the horizontal width of the vehicle shape. The manner in which the safety area is calculated as a circle will be described later.

[0015] As typical examples of the processing of the speed command generation system related to the vehicle travel plan, DWA (Dynamic Window Approach), which is a search system method, and MPC (Model Predictive Control), which is an optimization system model prediction method, can be used. Therefore, an embodiment using DWA will be described as a first embodiment, and an embodiment using MPC will be described as a second embodiment. Furthermore, after describing the first and second embodiments, other modified examples will be described.

[0016] We will explain search-based methods and optimization-based methods. Search-based methods are methods that divide space into grids based on translational velocity and angular velocity, and calculate a series of translational angular velocities that approach the goal in the shortest time without interference. Optimization-based methods are methods that express objective functions and constraints as mathematical formulas and use optimization techniques to calculate a series of translational velocity and angular velocity. Note that translational velocity is a scalar that represents the change in position over time. Angular velocity represents the change in attitude over time.

[0017] First, the functional configuration and hardware configuration common to each embodiment will be described. Fig. 2 is a diagram showing the hardware configuration of a driving planner. Fig. 3 is a diagram showing the configuration of the driving planner according to this embodiment.

[0018] 2, the driving planner 30 includes a CPU (Central Processing Unit) 32, a memory 34, a storage device 36, a drive mechanism 38, a sensor 40, a storage medium reader 42, and a communication I / F (Interface) 44. Each component is connected to each other via a bus 46 so as to be able to communicate with each other.

[0019] The storage device 36 stores a travel planning program for executing travel planning processing. The CPU 32 is a central processing unit that executes various programs and controls each component. That is, the CPU 32 reads the program from the storage device 36 and executes the program using the memory 34 as a work area. The CPU 32 controls each component and performs various arithmetic processing in accordance with the program stored in the storage device 36.

[0020] The memory 34 is configured with a random access memory (RAM) and serves as a working area to temporarily store programs and data. The storage device 36 is configured with a read-only memory (ROM), a hard disk drive (HDD), a solid state drive (SSD), etc., and stores various programs including the operating system and various data.

[0021] The drive mechanism 38 includes mechanisms such as a motor, a power source such as a battery, a transmission, and tires for driving and operating the vehicle 20. The sensor 40 detects the position, attitude, speed, acceleration, and other conditions of the vehicle 20, and includes, for example, a gyro sensor, a speed sensor, a camera, and the like.

[0022] The storage medium reader 42 reads data stored in various storage media such as CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray Disc, USB (Universal Serial Bus) memory, etc., and writes data to the storage media. The communication I / F 44 is an interface for communicating with other devices, and uses standards such as Ethernet (registered trademark), FDDI, and Wi-Fi (registered trademark).

[0023] 3, the driving planner 30 is a device provided inside the vehicle 20. The driving planner 30 is not limited to being provided inside the vehicle 20, but may be provided outside the vehicle 20 and transmit driving instruction data to the vehicle 20.

[0024] The control device 10 is, for example, a personal computer, a tablet terminal, a controller, or the like, and transmits instructions necessary for controlling the driving planner 30. The vehicle 20 is, for example, an AMR (Autonomous Mobile Robot), an AGV (Automatic Guided Vehicle), or the like.

[0025] 3 , the driving planner 30 includes, as functional components, an execution control unit 100, an initial processing unit 102, an acquisition unit 104, an update unit 106, a speed planning unit 108, and a generation unit 110. Each functional component is realized by the CPU 32 reading out a driving plan program stored in the storage device 36, expanding it in the memory 34, and executing it. Furthermore, since the control contents of each unit of the driving planner 30 are different processing units in each embodiment, the reference numerals of each unit of the driving planner 30 in the first embodiment are suffixed with A, and the reference numerals of each unit of the driving planner 30 in the second embodiment are suffixed with B to distinguish them from each other.

[0026] The execution control unit 100 executes the initial processing unit 102 and the acquisition unit 104 before the vehicle 20 starts traveling, and then repeatedly executes the update unit 106 and the speed planning unit 108. Note that, hereinafter, the vehicle 20 may also be simply referred to as a vehicle. The traveling mechanism of the vehicle may be a two-wheel differential mechanism, an automobile mechanism (steering mechanism), or an omnidirectional movement mechanism.

[0027] The initial processing unit 102 acquires map information indicating the location of obstacles and a target route set so that the vehicle body does not interfere with the obstacles while traveling. The map information may be stored in advance in the storage device 36, or may be acquired from an external map database (not shown). The target route is given as a sequence of positions, for example, (Px1, Py1), (Px2, Py2), ... (Pxn, Pyn).

[0028] The acquisition unit 104 acquires the initial position and attitude of the vehicle. The initial position and attitude of the vehicle are detected by the sensor 40. The position and attitude of the vehicle are given, for example, by the vehicle position (x, y) and attitude θ (angle). The translational velocity is given by v, and the angular velocity is given by ω. Obstacles are given as a series of positions on the map, for example, (Ox1, Oy1), (Ox2, Oy2), ... (Oxn, Oyn).

[0029] The update unit 106 calculates the future vehicle position and attitude at every predetermined update period from the initial vehicle position and attitude acquired by the acquisition unit 104. This "calculation" is referred to as "update" here. Furthermore, "initial" refers to the first time point of the update period (t = 0), and "future" refers to a time point after the start of the update period (t > 0). Specifically, the update unit 106 determines the future vehicle position and attitude when the vehicle travels for the update period from the future vehicle position and attitude at a planned speed corresponding to the future vehicle position and attitude as the updated future vehicle position and attitude. The future vehicle position and attitude can be calculated using the following equation. Here, future time is t, update period is dt, translational velocity is v, and attitude is θ.

[0030] x(t)=x(t-1)+v(t-1)cos(θ(t-1))dt y(t)=y(t-1)+v(t-1)sin(θ(t-1))dt

[0031] The speed planning unit 108 sets a candidate position series, which is a series of candidate positions of the vehicle within a prediction target time range that is longer than the future update period, based on the future position and attitude of the vehicle.The speed planning unit 108 then calculates a predicted speed series, which is a series of predicted speeds that, when used for traveling the vehicle, will prevent obstacles from entering the safety area over the prediction target time range and allow the vehicle to travel with minimal deviation from the target route, based on the map information, the target route, and the candidate position series, and calculates a planned speed based on the predicted speed series.The predicted speed and planned speed are represented by translational speed and angular speed.

[0032] The generator 110 generates a planned speed series, which is a series of planned speeds for each update period.

[0033] Each embodiment will be described below. [First Embodiment] The first embodiment will be described. The initial processing unit 102A acquires the target translational speed at each position on the target route, along with the map information and the target route described above. The target translational speed may be, for example, a trapezoidal speed curve that accelerates from the start position, maintains a constant speed thereafter, and then decelerates before the finish line, or the maximum speed specified in the vehicle's specifications.

[0034] The speed planning unit 108A performs two processes: (1) a setting process and (2) a calculation process. The calculation process is a cost calculation process and a calculation process of predicted speeds (predicted translational speed and predicted angular speed).

[0035] (1) The setting process will be described. The speed planning unit 108A performs a process of setting a plurality of combinations of possible translational velocities and possible angular velocities that are discretely prepared. The possible translational velocities and possible angular velocities are constant throughout the prediction target time range. Furthermore, the speed planning unit 108A performs a process of setting a possible translational velocity series, which is a series of possible translational velocities, and a possible angular velocity series, which is a series of possible angular velocities, for each combination of possible translational velocities and possible angular velocities.

[0036] Here, an example of the aspect of the candidate position series set by the speed planning unit 108A will be described. FIG. 4 shows an example of a grid representing the set candidate position series. A near the center represents the future translational velocity Vc and future angular velocity ωc, which indicate the host vehicle speed at a certain point in the future. a represents the maximum value of acceleration, and α represents the maximum value of angular acceleration. The speed planning unit 108A calculates the possible translational velocity and possible angular velocity by calculating the range of the velocity and angular velocity that the vehicle 20 can assume at the next time (a time dt hours ahead of the future time) based on the acceleration and angular acceleration, with the host vehicle speed as the center. The speed planning unit 108A truncates the range of the candidate position series at the upper and lower limits when the upper or lower limits of v and ω are reached. The upper limit of V is Vc + a × dt or the maximum value of V, and the lower limit of V is Vc + - a × dt or the minimum value of V. The upper limit of ω is ωc + α × dt or the maximum value of ω, and the lower limit of ω is ωc + - α × dt or the minimum value of ω. The velocity planning unit 108A divides the range into grids at specific sampling intervals. The center of each grid represents one combination of possible translational velocity v and possible angular velocity ω at each point in Figure 4. Note that the maximum and minimum values ​​of V and ω, and the maximum and minimum acceleration and angular acceleration values ​​are determined, for example, from the specifications of the motor of the vehicle 20.

[0037] (2) The calculation process will be described. As a preprocessing step, the speed planning unit 108A sets a series of vehicle positions within a prediction target time range as a candidate position series based on the future vehicle positions and attitudes, the possible translational velocity series, and the possible angular velocity series for each set combination. Note that, if necessary for calculating the obstacle term described below, a series of attitudes may be set as the candidate position series in addition to the position series. Next, the speed planning unit 108A calculates a cost including a predetermined error using the set candidate position series. The error included in the cost is the error between the vehicle position at the end of the candidate position series and the target path, and the error between the possible translational velocity and the target translational velocity corresponding to the future vehicle position. Furthermore, the speed planning unit 108A calculates, from the possible translational velocity series and the possible angular velocity series, a predicted translational velocity series, which is a series of predicted translational velocities, and a predicted angular velocity series, which is a series of predicted angular velocities, that minimize the cost based on map information, under the condition that the vehicle body and safety area at all positions in the candidate position series do not interfere with obstacles. Then, the speed planning unit 108A sets the translational velocities and angular velocities constituting the calculated predicted translational velocity sequence and predicted angular velocity sequence as planned velocities.

[0038] Specifically, the speed planning unit 108A calculates the cost using a cost function, which is expressed by the following equation (1).

[0039] cost=α*path+β*vel+γ*obstacle...(1)

[0040] In the cost function, path represents the following term, vel represents the velocity term, obstacle represents the obstacle term, and α, β, and γ represent weights. FIG. 5 is a diagram illustrating an example of setting a candidate position sequence. The speed planning unit 108A calculates a predicted trajectory from each candidate position sequence after tn time, assuming a constant velocity based on the velocity vector (v, ω). The velocity vector (v, ω) is expressed as a vector representing each point of the combination of the possible translational velocity v and the possible angular velocity ω. In this way, the speed planning unit 108A sets a candidate position sequence, which is a sequence of vehicle positions within the prediction target time range. However, as described above, the prediction target time range is assumed to be longer than the future update period.

[0041] The speed planning unit 108A performs the following first to fifth calculation processes as a cost calculation process. As a first calculation process, the speed planning unit 108A calculates a following term. A perpendicular line between the end of the forecasted trajectory and the target route is found, and the distance is used as the value of the following term. The following term is a term that indicates whether the vehicle will be close to the target route after tn time. As a second calculation process, the speed planning unit 108A calculates a velocity term. The velocity term is the absolute value of the difference between the possible translational velocity and the target translational velocity. In other words, it is a term that indicates whether the target translational velocity is being achieved. The target translational velocity at this time is the target translational velocity at a position close to the future vehicle position to be updated by the update unit 106, among the target translational velocities at each position on the target route acquired by the initial processing unit 102. As a third calculation process, the speed planning unit 108A calculates an obstacle term. The obstacle term is a term that calculates whether there is interference with an obstacle at each time. As a fourth process, the speed planning unit 108A calculates the cost based on the first to third calculation processes. As a fifth process, the speed planning unit 108A performs the above calculation for all velocity vectors (v, ω) and determines the combination with the lowest cost among the calculated combinations as the combination of predicted translational velocities and predicted angular velocities that minimizes the cost.

[0042] The calculation of the obstacle term will now be described. FIG. 6 is a diagram for explaining the calculation of the obstacle term. FIG. 7 is a diagram for explaining the calculation of the obstacle term taking the safety area into consideration. R indicates the position of the candidate position series at each time, S indicates the safety area at each time, and each circled O indicates the detected point of the obstacle. 6A indicates the location of interference with the obstacle. In the calculation of the obstacle term, the first process calculates the position and orientation of the vehicle at each future time. The second process determines whether the obstacle interferes with the shape of the vehicle in the candidate position series based on the position and orientation of the vehicle at each future time. In the second process, in the determination taking the safety area into consideration, interference is determined if an obstacle point is found within the safety area and the shape of the vehicle. 7A indicates the location of interference between the obstacle and the vehicle and safety area in the candidate position series. In the third process, if interference occurs, a large fixed value is set as the value of the obstacle term. If there is no interference, the distance between the vehicle's origin and the obstacle at each time when it is closest to the obstacle is calculated, and the reciprocal of the calculated distance is set as the value of the obstacle term. For example, a fixed value in the case of interference is set to a value larger than the reciprocal value in the case of no interference. In addition, in equation (1), calculation may be performed using a cost function of only the following term and the velocity term without using an obstacle term, such that a velocity vector (v, ω) where the predicted trajectory interferes with an obstacle is not adopted as a combination of the predicted translational velocity and the predicted angular velocity.

[0043] As described above, the speed planning unit 108A obtains a predicted translational velocity series, which is a series of predicted translational velocities, and a predicted angular velocity series, which is a series of predicted angular velocities, that minimize the cost, and sets the translational velocities and angular velocities that constitute the predicted translational velocity series and the predicted angular velocity series as planned speeds.

[0044] The generating unit 110A generates a planned speed series, which is a series of planned speeds for each update period, based on the planned speeds set by the speed planning unit 108A.

[0045] (Processing Flow) Fig. 8 is a flowchart showing the flow of the trip planning process by the trip planning device 30 according to the first embodiment. Fig. 9 is a flowchart showing the flow of the speed planning process according to the first embodiment. Fig. 10 is a diagram showing an example of a traveling image of the vehicle 20 corresponding to the trip planning process by the trip planning device 30.

[0046] When the vehicle 20 is powered on, the CPU 32 reads out the trip planning program from the storage device 36, loads it into the memory 34, and executes it, causing the CPU 32 to function as each functional component of the trip planning device 30, and executes the trip planning process shown in Fig. 8. In executing the trip planning program, first, the execution control unit 100A instructs the initial processing unit 102A to start up.

[0047] In step S10, the initial processing unit 102A acquires map information indicating the location of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while traveling. An image of the vehicle 20 in step S10 is shown in the left diagram of FIG.

[0048] In step S12, the acquisition unit 104A acquires the initial position and attitude of the vehicle by sensing. An image of the vehicle 20 in step S12 is shown in the left diagram of FIG.

[0049] In step S14, the speed planning unit 108A sets candidate position sequences and calculates a planned speed that minimizes the cost. An image of the vehicle 20 and the safety area S in step S14 is shown in the right diagram of FIG.

[0050] In step S16, the update unit 106A sets the future position and attitude of the vehicle when it travels at the planned speed calculated by the speed planning unit 108A for the period of the update cycle as the updated future position and attitude of the vehicle. An image of the vehicle 20 and the safety area S in step S16 is shown in the right diagram of Figure 10.

[0051] In step S18, the speed planning unit 108A determines whether the updated future vehicle position is at the end of the route for which speed planning is desired (i.e., the target route). If it is determined that the vehicle position is not at the end of the route (if the determination is negative), the process returns to step S14 and repeats. On the other hand, if it is determined that the vehicle position is at the end of the route (if the determination is positive), the process proceeds to step S20. An image of the vehicle 20 and the safety area S in step S18 is shown in the right diagram of FIG.

[0052] In step S20, the generation unit 110A generates a planned speed series, which is a series of planned speeds for each update period, from the planned speeds repeatedly calculated by the speed planner 108A for each update period, and then ends the series of processes performed by the travel planning program. Here, Table 1 shows an example of a planned speed series of planned speeds (v, ω) used by the vehicle 20 until it reaches the destination point.

[0053]

[0054] Next, a subroutine of the speed planning process executed by the speed planning unit 108A in step S14 will be described with reference to FIG.

[0055] In step S30, the velocity planning unit 108A sets a plurality of discretely prepared combinations of possible translational velocities and possible angular velocities.

[0056] In step S32, the speed planning unit 108A sets a candidate position series, which is a series of vehicle positions within the prediction target time range, based on the future vehicle position and attitude, possible translational velocity, and possible angular velocity for each set combination.

[0057] In step S34, the speed planning unit 108A calculates the cost by calculating the following term, the speed term, and the obstacle term for each set combination using the cost function of the above equation (1).

[0058] In step S36, the speed planning unit 108A obtains a predicted translational velocity series, which is a series of predicted translational velocities, and a predicted angular velocity series, which is a series of predicted angular velocities, that minimize the cost, and sets the translational velocities and angular velocities that constitute the obtained predicted translational velocity series and predicted angular velocity series as planned speeds, and then returns to step S16 in FIG. 8.

[0059] As described above, according to the driving planning device of this embodiment, by performing a simulation using DWA, which is an example of speed command generation system processing, before the vehicle starts driving, it is possible to create a driving plan that prevents obstacles from entering a safety area set according to the vehicle speed, while suppressing deviation from the target route and allowing the vehicle to drive.

[0060] [Second Embodiment] A second embodiment will be described. The speed planning unit 108B performs the following processes: acquiring a target position series generated based on a target route for each update period; setting a command series that is a candidate translational velocity series and a candidate angular velocity series for each update period within a prediction target time range based on a future vehicle position and attitude; setting a vehicle position series based on the command series as a candidate position series; determining a predicted translational velocity series and a predicted angular velocity series from the command series that minimize an evaluation value including an error amount between the target position series and the candidate position series within the prediction target time range based on map information, on the condition that the vehicle body and safety areas at all positions in the candidate position series do not interfere with obstacles; and setting translational velocities and angular velocities included in the predicted translational velocity series and the predicted angular velocity series and corresponding to the future vehicle position and attitude as planned velocities. That is, the processes of the speed planning unit 108B include (1) acquiring a target position series, (2) calculating a candidate position series, and (3) calculating a planned velocity.

[0061] (1) The process of acquiring a target position series will be described. The speed planning unit 108B acquires a target position series, which is a series of positions generated based on a target route and updated every update period. The target position series is expressed as x ｒｅｆ , y ｒｅｆ The target attitude is θ ｒｅｆ Let's say.

[0062] (2) The process of calculating a candidate position series will be described. The speed planning unit 108B sets a command series based on the future position and attitude of the vehicle, and calculates a candidate position series, which is a series of candidate vehicle positions, based on the command series. The command series is a candidate translational velocity series, which is a series of candidate translational velocities for each update period over the prediction target time range, and a candidate angular velocity series, which is a series of candidate angular velocities.

[0063] The candidate translational velocity sequence and candidate angular velocity sequence are calculated, for example, as follows: The candidate translational velocity v is calculated using an equation such as future translational velocity + acceleration × dt (predicted times t1, t2, tn). The candidate angular velocity ω is calculated using θ ｒｅｆ (k+i)-θ ｒｅｆ It is calculated using a formula such as (k+i-1) / dt.

[0064] (3) The process of calculating the planned speed will be described. The speed planning unit 108B calculates a predicted translational speed and a predicted angular speed based on a command sequence that minimizes a predetermined evaluation value. The evaluation value is based on map information, and is determined based on the condition that the vehicle and the safety area at all positions in the candidate position sequence do not interfere with obstacles. The evaluation value also includes the amount of error between the target position sequence and the candidate position sequence over the prediction target time range. Note that at least the position must be considered as the target of the error amount, and the attitude may also be considered as the target of the error amount. The traveling direction and attitude can be uniquely determined from the position and vehicle kinematics.

[0065] The objective function and constraints for calculating the evaluation value will be described.

[0066] The objective function is expressed by the following equation (2). ...(2) The first term is a term for the error between the end points of the target position series and the end points of the candidate position series, the second term is a term for the error between the midpoint of the target position series and the midpoint of the candidate position series, and the third term is a term for the error between the target command series and the set command series. Also, the deviation e from the target position and attitude in the following equation (2-1) ｘ (k+i), the deviation e from the target control input in equation (2-1) ｕ It is expressed as (k+i-1). ... (2-1) ... (2-2)

[0067] where Np is the prediction horizon, Nc is the control horizon, Nc<Np, P∈R ３×３ , Q∈R ３×３ , R∈R ３×３ is.

[0068] The constraints are expressed by the following equations (3-1) to (3-4). ... (3-1) ... (3-2) ... (3-3) ...(3-4) The constraint in (3-1) is a kinematic constraint of the robot, such as the inability of the differential two-wheel mechanism to move sideways. The constraint in (3-2) is a constraint on translational velocity and angular velocity. The constraint in (3-3) is a constraint on translational acceleration and angular acceleration. The constraint in (3-4) is a constraint to avoid interference with obstacles. Note that in equation (2), the third term of the command series may not be used, and only the first and second terms may be used. Also, in equation (2-1), θ(k+i)-θ ｒｅｆ (k+i) is not used, x(k+i)-x ｒｅｆ (k+i) and y(k+i)-y ｒｅｆ Only (k+i) may be used.

[0069] 11 shows an example of an image in which the shape of a vehicle is represented by a circle. The use of a circumscribing circle as in 10A has the advantage of reducing the calculation load. The use of multiple circles as in 10B has the advantage of reducing waste relative to the shape of the vehicle.

[0070] 12 is an image diagram of a candidate position sequence based on the objective function and constraints. The acquired vehicle position is indicated by x and y, and the target route is the position x ｒ , y ｒ The locus of the reference path is shown by

[0071] FIG. 13 is an illustration of obstacle determination. In the determination, a circumscribing circle is set for the shape of the vehicle in the candidate position series. In the constraint calculation of equation (3-4), the distance between the circumscribing circle of each position in the candidate position series and the obstacle is calculated, and the variables are updated so that the distance is equal to or greater than the radius of the circumscribing circle. In this way, the constraint calculation is performed to prevent interference with obstacles, and is reflected in the calculation of the objective function.

[0072] FIG. 14 is an image diagram of obstacle determination that takes safety areas into consideration. Multiple circles are set for the safety area of ​​the candidate position series and the shape of the vehicle. In calculating the constraints in equation (3-4), the distances to all obstacles are calculated for the multiple circles at each position in the candidate position series, and the variables are updated so that the calculated distances are equal to or greater than the radius of each of the multiple circles. For example, if obstacle constraints are set using multiple circles, the number of inequality constraints in equation (3-4) increases by the number of circles.

[0073] (Processing Flow) Fig. 15 is a flowchart showing the flow of a driving planning process by the driving planning device 30 according to the second embodiment. Fig. 16 is a flowchart showing the flow of a speed planning process according to the second embodiment.

[0074] When the vehicle 20 is powered on, the CPU 32 reads out the trip planning program from the storage device 36, loads it into the memory 34, and executes it, causing the CPU 32 to function as each functional component of the trip planning device 30, and executes the trip planning process shown in Fig. 15. In executing the trip planning program, first, the execution control unit 100B instructs the initial processing unit 102B to start up.

[0075] In step S110, the initial processing unit 102B acquires map information indicating the location of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while traveling.

[0076] In step S112, the acquisition unit 104B acquires the initial position and attitude of the vehicle through sensing.

[0077] In step S114, the speed planning unit 108B calculates a planned speed based on the command sequence that minimizes the evaluation value.

[0078] In step S116, the update unit 106B sets the future position and attitude of the vehicle when it travels at the planned speed calculated by the speed planning unit 108B for the period of the update cycle as the updated future position and attitude of the vehicle.

[0079] In step S118, the speed planning unit 108B determines whether the updated future vehicle position is at the end of the route for which speed planning is desired (i.e., the target route). If it is determined that the vehicle position is not at the end of the route (if the determination is negative), the process returns to step S114 and repeats. On the other hand, if it is determined that the vehicle position is at the end of the route (if the determination is positive), the process proceeds to step S120.

[0080] In step S120, the generation unit 110B generates a planned speed series, which is a series of planned speeds for each update period, from the planned speeds repeatedly calculated for each update period by the speed planning unit 108B, and ends the series of processes by this driving planning program.

[0081] Next, a subroutine of the speed planning process executed by the speed planning unit 108B in step S114 will be described with reference to FIG.

[0082] In step S130, the speed planning unit 108B acquires a target position series, which is a series of positions generated based on the target route.

[0083] In step S132, the speed planning unit 108B sets a command sequence based on the future position and attitude of the vehicle, and obtains a candidate position sequence, which is a sequence of candidate vehicle positions, based on the command sequence.

[0084] In step S134, the speed planning unit 108B uses the objective function of the above equation (2) and the constraints of the equations (3-1) to (3-4) to calculate a predicted translational velocity series and a predicted angular velocity series based on the command series that minimizes the evaluation value, sets the translational velocities and angular velocities for the future vehicle position and attitude that are included in the calculated predicted translational velocity series and predicted angular velocity series as planned velocities, and returns to step S116 in FIG. 15.

[0085] As described above, according to the driving planning device of this embodiment, by performing a simulation using MPC, which is an example of speed command generation system processing, before the vehicle starts driving, it is possible to create a driving plan that prevents obstacles from entering a safety area set according to the vehicle speed, while suppressing deviation from the target route and allowing the vehicle to drive.

[0086] (Method for Representing a Vehicle Shape with Multiple Circles) A ​​specific method for representing a vehicle shape with multiple circles will now be described in more detail. FIGS. 17 and 18 are diagrams for explaining a method for representing a vehicle shape with multiple circles. Multiple circles are set using steps (A1) to (A5). In step (A1), a square is calculated whose side length is the same as the shorter side when the vehicle shape is approximated as a rectangle. In step (A2), the number of squares that can fill the vehicle shape is calculated. For example, squares are added starting from the left edge, and the number of squares is found until the total length exceeds the vehicle shape. This number of squares becomes the number of multiple circles. In step (A3), squares that extend beyond the vehicle shape are deleted, and new squares are added starting from the right edge. This is to reduce excess space when the vehicle shape is covered with squares. In step (A4), the distance between the squares at both ends is calculated and divided by the number of squares other than those at both ends plus one. Based on this value, the positions of the squares are corrected so that they are evenly spaced. In step (A5), the circumscribing circles of each square are calculated. In this way, circumscribing circles are set for each of the multiple squares arranged in the longitudinal direction of the vehicle shape. When controlling the attitude based on the multiple circles set, the center coordinates of each circumscribing circle are rotated according to the attitude of the vehicle.

[0087] [Modification 1] In Modification 1, an embodiment will be described in which only the translational velocity v of the first embodiment is controlled, and the angular velocity ω is calculated and set from the translational velocity v and the target path.

[0088] The speed planning unit 108A performs the following processes: setting a plurality of discretely prepared possible translational velocities that are constant over a prediction target time range; setting a possible translational velocity series that is a series of possible translational velocities for each possible translational velocity; setting a series of vehicle positions on a target route as a candidate position series based on future vehicle positions, the target route, and the possible translational velocity series for each possible translational velocity, and calculating a cost including an error between an end of the candidate position series and the target route and an error between the possible translational velocities and the target translational velocities corresponding to the future vehicle positions; obtaining a predicted translational velocity series that is a series of predicted translational velocities that minimizes the cost based on map information on the condition that the vehicle body and safety areas at all positions in the candidate position series do not interfere with obstacles; and setting the translational velocities that constitute the predicted translational velocity series and the angular velocities calculated based on the translational velocities and the target route as planned velocities. That is, the speed planning unit 108A performs (1) a process of setting a plurality of discretely prepared possible translational velocities, and (2) a process of setting a candidate position series. As described above, the possible translational speed is a constant value throughout the prediction target time range. The candidate position series is a series of vehicle positions on the target route within the prediction target time range, based on the future vehicle positions, the target route, and the possible translational speed series, for each possible translational speed. Next, the speed planning unit 108A performs (3) a process to calculate a predicted translational speed. The predicted translational speed is calculated based on map information so that the difference between the possible translational speeds and the target translational speed corresponding to the future vehicle position is minimized, on the condition that the vehicle and the safety area at all positions in the candidate position series do not interfere with obstacles. The speed planning unit 108A then performs (4) a process to calculate an angular velocity based on the predicted translational speed and the target route.

[0089] FIG. 19 is a diagram illustrating a setting mode of a candidate position sequence in a modified example. As a premise, the position and attitude (x, y, θ) of the vehicle are obtained using self-position estimation. The range of possible translational speeds that the vehicle 20 can take at the next time (the time dt time ahead from the current time) is calculated based on the acceleration, with the vehicle speed at the center. The possible translational speeds within that range are divided at specific sampling intervals. For each divided possible translational speed, a predicted trajectory for a time tn that follows the target route, assuming a constant speed, is calculated. The cost of the DWA cost function is calculated, and the lowest cost v (predicted translational speed) is determined, taking into account the safety area. The angular velocity ω (predicted angular velocity) is determined so that the vehicle will be positioned on the target route when traveling for a time dt at the obtained predicted translational speed.

[0090] The processing flow of Modification 1 can be executed by replacing the flowchart of FIG. 9 described in the first embodiment. In step S30, the speed planning unit 108A sets multiple discretely prepared possible translational velocities. In step S32, the speed planning unit 108A sets, for each possible translational speed, a candidate position series, which is a series of vehicle positions on the target route within the prediction target time range, based on the future vehicle positions, the target route, and the possible translational speed series. In step S34, the speed planning unit 108A calculates a cost using the cost function of equation (1) above, and determines a predicted translational speed so as to minimize the difference from a predetermined target translational speed. In step S36, the speed planning unit 108A sets, as planned speeds, the translational speeds constituting the predicted translational speed and the angular speed calculated based on the translational speed and the target route.

[0091] In the second modification, the velocity vector (v, ω) is calculated by reducing the error between the end position of the predicted trajectory and a position far ahead of the vehicle on the target path. The first and second modifications will be described below.

[0092] FIG. 20 is an illustration of how v and ω are calculated using DWA. In the example shown in FIG. 20, v and ω are selected to minimize the error between the intersection PA with the target route, which is a certain distance from the vehicle, and the end point of the predicted trajectory. The certain distance may be assumed to be a rectangle as shown in FIG. 20, or a circle. The position of the intersection PA may be set to a position much farther away than the position that can be reached after tn seconds. In this case, the vehicle travels toward that distant position, allowing it to smoothly return to the target route if it deviates from the target route due to slippage or the like. In this case, the velocity term does not need to be used in Equation (1). In this case, the maximum translational velocity v is selected from among the possible translational velocities within a range that does not interfere with the safety region.

[0093] 21 is an illustration of how v and ω are calculated using MPC. The objective function of equation (2) above, which selects vω that minimizes the error between the intersection point with the target path at a fixed distance from the vehicle and the end of the prediction horizon, uses only the first term, and the second and third terms are not required. This is because the positions and orientations of points along the trajectory can be uniquely calculated due to kinematic inequality constraints.

[0094] 22 shows an example of an experiment in which the method of this embodiment was applied to a vehicle whose shape was represented by multiple circles. In a simulator environment, the shape of the vehicle was represented by multiple circles and set as an obstacle constraint, and it was confirmed that the vehicle could pass through a passage containing an obstacle.

[0095] In each of the above embodiments, the information processing performed by the CPU after reading the software (program) may be performed by various processors other than the CPU. Examples of such processors include a PLD (Programmable Logic Device) whose circuit configuration can be changed after manufacture, such as an FPGA (Field-Programmable Gate Array), and a dedicated electrical circuit, such as an ASIC (Application Specific Integrated Circuit), which is a processor having a circuit configuration designed specifically to perform specific processing. Furthermore, the information processing may be performed by one of these various processors, or by a combination of two or more processors of the same or different types (e.g., multiple FPGAs, or a combination of a CPU and an FPGA). Furthermore, the hardware structure of these various processors is, more specifically, an electrical circuit that combines circuit elements such as semiconductor elements.

[0096] In addition, in each of the above embodiments, the information processing program is described as being pre-stored (installed) in a ROM or storage, but this is not limiting. The program may be provided in a form recorded on a non-transitory recording medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), or a USB (Universal Serial Bus) memory. The program may also be downloaded from an external device via a network.

[0097] The following are additional notes regarding this disclosure.

[0098] (Additional Item 1) A travel planning device (30) for a vehicle, in which a safety region is set having a length that increases as the translational speed of the vehicle increases from the front end of the vehicle forward and a width that approximates a width of the vehicle, comprising: an initial processing unit (102) that acquires map information indicating the location of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles when the vehicle is traveling; an acquisition unit (104) that acquires an initial position and attitude of the vehicle; an update unit (106) that updates a future position and attitude of the vehicle at a predetermined update period from the initial position and attitude of the vehicle; and a speed planning unit (108) that sets a candidate position series that is a series of candidate positions of the vehicle within a prediction target time range that is longer than the future update period based on the future position and attitude of the vehicle, and calculates a planned speed of the vehicle based on the map information, the target route, and the candidate position series that, when used for traveling the vehicle, prevents the obstacle from entering the safety region over the prediction target time range and allows the vehicle to travel with a small deviation from the target route. a generating unit (110) that generates a planned speed series that is a series of the planned speeds for each update period, (Supplementary Item 2) The travel planning device (30) according to Supplementary Item 1, wherein the updating unit (106) sets the future position and attitude of the vehicle when the vehicle travels for the update period at the planned speed corresponding to the future position and attitude of the vehicle from the future position and attitude of the vehicle as the updated future position and attitude of the vehicle.(Supplementary Item 3) The initial processing unit (102) further acquires or sets a target translational velocity at each position on the target route, and the velocity planning unit (108) performs a process of setting a plurality of combinations of possible translational velocities and possible angular velocities, each of which is prepared discretely and is constant over the prediction target time range, a process of setting, for each combination, a possible translational velocity series that is a series of the possible translational velocities and a possible angular velocity series that is a series of the possible angular velocities, and a process of setting, for each combination, a series of positions of the vehicle as the candidate position series based on a future position and attitude of the vehicle, the possible translational velocity series, and the possible angular velocity series, and calculating a cost including an error between an end of the candidate position series and the target route and an error between the possible translational velocity and the target translational velocity corresponding to a future position of the vehicle. 3. The driving planning device according to claim 1 or 2, further comprising: a process for determining a predicted translational velocity series that is a series of predicted translational velocities and a predicted angular velocity series that is a series of predicted angular velocities from the possible translational velocity series and the possible angular velocity series, which minimize the cost on the condition that the vehicle body and the safety area do not interfere with the obstacle at all positions in the candidate position series based on the map information; and a process for setting the translational velocities and the angular velocities that constitute the predicted translational velocity series and the predicted angular velocity series as the planned velocities.(Additional Item 4) The initial processing unit (102) further acquires or sets a target translational speed at each position on the target route, and the speed planning unit (108) performs the following steps: a process of setting a plurality of discretely prepared possible translational speeds that are constant over the prediction target time range; a process of setting a possible translational speed series that is a series of the possible translational speeds for each possible translational speed; a process of setting a series of the vehicle's positions on the target route as the candidate position series based on a future position of the vehicle, the target route, and the possible translational speed series for each possible translational speed, and calculating a cost including an error between an end of the candidate position series and the target route and an error between the possible translational speed and the target translational speed corresponding to the future position of the vehicle; and a process of obtaining a predicted translational speed series that is a series of predicted translational speeds that minimizes the cost based on the map information, on the condition that the vehicle body and the safety area do not interfere with the obstacle at all positions in the candidate position series. setting, as the planned speed, translational speeds constituting the predicted translational speed series and an angular speed calculated based on the translational speeds and the target route.(Supplementary Item 5) The travel planning device according to Supplementary Item 1 or Supplementary Item 2, wherein the speed planning unit (108) performs the following processes: acquiring a target position series that is a series of positions for each update period, generated based on the target route; setting a command series that is a candidate translational velocity series that is a series of candidate translational velocities and a candidate angular velocity series that is a series of candidate angular velocities for each update period over the prediction target time range, based on a future position and attitude of the vehicle, and setting a series of positions of the vehicle based on the command series as the candidate position series; obtaining, from the command series, a predicted translational velocity series that is a series of predicted translational velocities and a predicted angular velocity series that is a series of predicted angular velocities that minimize an evaluation value including an error amount between the target position series and the candidate position series over the prediction target time range, on the condition that the vehicle body and the safety area do not interfere with the obstacle at all positions in the candidate position series based on the map information; and setting a translational velocity and an angular velocity that are included in the predicted translational velocity series and the predicted angular velocity series and that correspond to a future position and attitude of the vehicle as the planned speed. (Supplementary Item 6) The travel planning device according to any one of Supplementary Items 1 to 5, wherein the safety area and the shape of the vehicle for setting the safety area are configured as any one of a circle, a rectangle, and a polygon. (Supplementary Item 7) The travel planning device according to Supplementary Item 6, wherein when the shape is set as a plurality of circles, the number of squares whose side lengths are the same as the length of the shorter side of a rectangular shape of the vehicle is calculated, and a circumscribing circle is set for each of the squares arranged in the longitudinal direction of the shape of the vehicle.(Supplementary Item 8) A travel planning method for a vehicle in which a safety region is set having a length that increases as the translational speed of the vehicle increases from the front end of the vehicle forward and a width that approximates a width of the vehicle, the method comprising: acquiring map information indicating the arrangement of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while the vehicle is traveling; acquiring an initial position and attitude of the vehicle; updating a future position and attitude of the vehicle from the initial position and attitude of the vehicle for each predetermined update period; setting a candidate position series that is a series of candidate positions of the vehicle within a prediction target time range that is longer than the future update period based on the future position and attitude of the vehicle; determining a planned speed of the vehicle that, when used for traveling of the vehicle, prevents the obstacle from entering the safety region throughout the prediction target time range and allows the vehicle to travel with little deviation from the target route, based on the map information, the target route, and the candidate position series; and generating a planned speed series that is the series of planned speeds for each update period. (Supplementary Item 9) A travel planning program for a vehicle in which a safety area is set having a length that increases as the translational speed of the vehicle increases from the front end of the vehicle forward and a width that approximates the width of the vehicle, the travel planning program causing a computer to execute the following processes: acquire map information indicating the placement of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while the vehicle is traveling; acquire an initial position and attitude of the vehicle; update a future position and attitude of the vehicle from the initial position and attitude of the vehicle for each predetermined update period; set a candidate position series that is a series of candidate positions of the vehicle within a prediction target time range that is longer than the future update period based on the future position and attitude of the vehicle; calculate a planned speed of the vehicle that, when used for traveling of the vehicle, prevents the obstacle from entering the safety area throughout the prediction target time range and allows the vehicle to travel with little deviation from the target route, based on the map information, the target route, and the candidate position series; and generate a planned speed series that is the series of planned speeds for each update period.

[0099] The disclosure of Japanese Patent Application No. 2024-151009, filed on September 2, 2024, is incorporated herein by reference in its entirety. All documents, patent applications, and technical standards mentioned herein are incorporated herein by reference to the same extent as if each individual document, patent application, and technical standard was specifically and individually indicated to be incorporated by reference.

Claims

1. A travel planning device for a vehicle in which a safety area is set, the length of which increases as the translational speed of the vehicle increases from the front end of the vehicle forward and the width of which approximates the width of the vehicle, the travel planning device comprising: an initial processing unit that acquires map information indicating the placement of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while traveling; an acquisition unit that acquires an initial position and attitude of the vehicle; an update unit that updates a future position and attitude of the vehicle from the initial position and attitude of the vehicle at each predetermined update period; a speed planning unit that sets a candidate position series that is a series of candidate positions of the vehicle within a future target time range that is longer than the future update period based on the future position and attitude of the vehicle, and calculates a planned speed of the vehicle based on the map information, the target route, and the candidate position series that, when used for traveling the vehicle, will prevent the obstacle from entering the safety area throughout the target time range and will allow the vehicle to travel with minimal deviation from the target route; and a generation unit that generates a planned speed series that is a series of planned speeds for each update period.

2. The driving planning device according to claim 1, wherein the update unit sets the future position and attitude of the vehicle when the vehicle travels for the update period at the planned speed corresponding to the future position and attitude of the vehicle from the future position and attitude of the vehicle as the updated future position and attitude of the vehicle.

3. The initial processing unit further acquires or sets a target translational velocity at each position on the target route, and the velocity planning unit performs a process of setting a plurality of combinations of possible translational velocities and possible angular velocities that are prepared discretely and are constant over the prediction target time range; a process of setting, for each combination, a possible translational velocity series that is a series of the possible translational velocities and a possible angular velocity series that is a series of the possible angular velocities; a process of setting, for each combination, a series of the vehicle positions as the candidate position series based on the future position and attitude of the vehicle, the possible translational velocity series, and the possible angular velocity series, and calculating a cost including an error between an end of the candidate position series and the target route and an error between the possible translational velocity and the target translational velocity corresponding to the future position of the vehicle; and a process of obtaining, from the possible translational velocity series and the possible angular velocity series, a predicted translational velocity series that is a series of predicted translational velocities and a predicted angular velocity series that is a series of predicted angular velocities that minimizes the cost on the condition that the vehicle body and the safety area do not interfere with the obstacle at all positions in the candidate position series based on the map information; 3. The driving planning device according to claim 1, further comprising: a process of setting, as the planned speed, translational velocities and angular velocities constituting the predicted translational speed series and the predicted angular speed series.

4. The initial processing unit further acquires or sets a target translational speed at each position on the target route, and the speed planning unit performs the following steps: a process of setting a plurality of discretely prepared possible translational speeds that are constant over the prediction target time range; a process of setting a possible translational speed series, which is a series of the possible translational speeds, for each possible translational speed; a process of setting, for each possible translational speed, a series of the vehicle's positions on the target route as the candidate position series based on the future position of the vehicle, the target route, and the possible translational speed series, and calculating a cost including an error between an end of the candidate position series and the target route and an error between the possible translational speed and the target translational speed corresponding to the future position of the vehicle; and a process of obtaining, from the possible translational speed series, a predicted translational speed series, which is a series of predicted translational speeds that minimizes the cost on the condition that the vehicle body and the safety area do not interfere with the obstacle at all positions in the candidate position series based on the map information.

3. The driving planning device according to claim 1, further comprising: a process of setting, as the planned speed, translational speeds constituting the predicted translational speed series and an angular speed calculated based on the translational speeds and the target route.

5. The driving planning device according to claim 1 or 2, wherein the speed planning unit performs the following processes: acquiring a target position series that is a series of positions for each update period, generated based on the target route; setting a command series that is a candidate translational velocity series that is a series of candidate translational velocities and a candidate angular velocity series that is a series of candidate angular velocities for each update period over the prediction target time range based on a future position and attitude of the vehicle, and setting a series of positions of the vehicle based on the command series as the candidate position series; obtaining, from the command series, a predicted translational velocity series that is a series of predicted translational velocities and a predicted angular velocity series that is a series of predicted angular velocities that minimize an evaluation value including an error amount between the target position series and the candidate position series over the prediction target time range on the condition that the vehicle body and the safety area do not interfere with the obstacle at all positions in the candidate position series based on the map information; and setting a translational velocity and an angular velocity that are included in the predicted translational velocity series and the predicted angular velocity series and that correspond to the future position and attitude of the vehicle as the planned speed.

6. The driving planning device according to claim 1 or 2, wherein the safety area and the shape of the vehicle for setting the safety area are configured as one of a circle, a rectangle, and a polygon.

7. The driving planning device according to claim 6, wherein when the shape is set as a plurality of circles, the number of squares whose side lengths are the same as the length of the shorter side of a rectangular shape of the vehicle is calculated, and a circumscribing circle is set for each of the plurality of squares arranged in the longitudinal direction of the shape of the vehicle.

8. A method for planning a journey for a vehicle in which a safety area is set having a length that increases as the translational speed of the vehicle increases from the front end of the vehicle forward and a width that approximates the width of the vehicle, wherein a computer executes the following processes: acquire map information indicating the location of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while traveling; acquire an initial position and attitude of the vehicle; update a future position and attitude of the vehicle from the initial position and attitude of the vehicle at each predetermined update period; set a candidate position series that is a series of candidate positions of the vehicle within a prediction target time range that is longer than the future update period based on the future position and attitude of the vehicle; calculate a planned speed of the vehicle that, when used for traveling the vehicle, will prevent the obstacle from entering the safety area throughout the prediction target time range and will allow the vehicle to travel with minimal deviation from the target route, based on the map information, the target route, and the candidate position series; and generate a planned speed series that is the series of planned speeds for each update period.

9. A travel planning program for a vehicle in which a safety area is set having a length that increases as the translational speed of the vehicle moves forward from the front end of the vehicle, and a width that approximates the width of the vehicle, the travel planning program causing a computer to perform the following processes: acquire map information indicating the location of obstacles and a target route that is set so that the vehicle body does not interfere with the obstacles while traveling; acquire an initial position and attitude of the vehicle; update a future position and attitude of the vehicle from the initial position and attitude of the vehicle at each predetermined update period; set a candidate position series that is a series of candidate positions of the vehicle within a prediction target time range that is longer than the future update period based on the future position and attitude of the vehicle; calculate a planned speed of the vehicle that, when used for traveling the vehicle, will prevent the obstacle from entering the safety area throughout the prediction target time range and will allow the vehicle to travel with minimal deviation from the target route, based on the map information, the target route, and the candidate position series; and generate a planned speed series that is the series of planned speeds for each update period.

Images